diff --git a/artwork/stranded/robot_holding_satellite_dish.png b/artwork/stranded/robot_holding_satellite_dish.png
new file mode 100644
index 0000000000000000000000000000000000000000..f388e05be4417d774d653dbeff9a839cdcb847f9
Binary files /dev/null and b/artwork/stranded/robot_holding_satellite_dish.png differ
diff --git a/artwork/stranded/satelite_dish.png b/artwork/stranded/satelite_dish.png
new file mode 100644
index 0000000000000000000000000000000000000000..8603c3991e16090403fd5f5f8aed3146b6fee64e
Binary files /dev/null and b/artwork/stranded/satelite_dish.png differ
diff --git a/assignments/Assignment06_Trajectories.ipynb b/assignments/Assignment06_Trajectories.ipynb
new file mode 100644
index 0000000000000000000000000000000000000000..f0c3e7dee1a1e9f1239dcc339761776f3d88537b
--- /dev/null
+++ b/assignments/Assignment06_Trajectories.ipynb
@@ -0,0 +1,120 @@
+{
+ "cells": [
+ {
+ "cell_type": "markdown",
+ "id": "1e74f470-c7e0-4579-abcb-236de17e3b02",
+ "metadata": {},
+ "source": [
+ "# Robots do it again!\n",
+ "\n",
+ "dr. Vasilescu calls her MARS-O-HELP robot to do its magic once more: The robot is capable of controlling its kinematics pretty reliably, so what if she makes it hold the antenna towards a communication satellite and follow its trajectory?\n",
+ "\n",
+ "<img src=\"../artwork/stranded/robot_holding_satellite_dish.png\" width=60% />\n",
+ "\n",
+ "## First task\n",
+ "Using the code you constructed for the simulation in the [theory notebook for trajectories](../theory/lab06_Trajectories.ipynb), implement a line trajectory for the AL5D robot for the following starting and ending poses:\n",
+ " \n",
+ "|Coord|Initial|Final|\n",
+ "|-|-|-|\n",
+ "|<td colspan=3>Position<td colspan=2>\n",
+ "|X|0.16|0.18|\n",
+ "|Y|0.22|-0.18|\n",
+ "|Z|0.09|0.13|\n",
+ "|<td colspan=3>Orientation (Euler 'zyx' angles) <td colspan=2>\n",
+ "|roll|0|0|\n",
+ "|pitch|20|-90|\n",
+ "|yaw|0|10|\n",
+ " \n",
+ "Then, execute the trajectory using the robot on your desk"
+ ]
+ },
+ {
+ "cell_type": "code",
+ "execution_count": null,
+ "id": "b6b290fc-fe3c-4644-a57f-2b9424ff8629",
+ "metadata": {},
+ "outputs": [],
+ "source": [
+ "# You should only execute this cell once!\n",
+ "# If you execute it again, you need to restart the kernel and the robot\n",
+ "from al5d_control import *\n",
+ "\n",
+ "# for sending the commands\n",
+ "rrob = AL5DControl()"
+ ]
+ },
+ {
+ "cell_type": "code",
+ "execution_count": null,
+ "id": "67e26e69-22f4-4ca3-a763-a6c3bbe07f5c",
+ "metadata": {},
+ "outputs": [],
+ "source": [
+ "import time\n",
+ "# Solve the ikine for the respective interpolated poses\n",
+ "# The solution will be in radians!\n",
+ "\n",
+ "\n",
+ "# Take the robot in the configuration for the starting pose\n",
+ "rrob.robot_control(np.degrees(qs[0,0]),np.degrees(qs[0,1]),np.degrees(qs[0,2]),np.degrees(qs[0,3]),np.degrees(qs[0,4]),0)\n",
+ "time.sleep(3)\n",
+ "\n",
+ "# Give degrees as input for the joint values\n",
+ "for i in range(steps):\n",
+ " rrob.robot_control(np.degrees(qs[i,0]),np.degrees(qs[i,1]),np.degrees(qs[i,2]),np.degrees(qs[i,3]),np.degrees(qs[i,4]),0)\n",
+ " ti.sleep(0.1)"
+ ]
+ },
+ {
+ "cell_type": "markdown",
+ "id": "4444c263-c5db-4154-bcbc-6dcf3efadd10",
+ "metadata": {},
+ "source": [
+ "## Second task\n",
+ "\n",
+ "Calculate a series of poses so that the robot follows a trajectory of a circle on the XY plane, with radius $r=6cm$, and centre at $ X = 20cm$, $Y = 0cm$, and $Z = 4cm$. The end-effector should not be rotated w.r.t. the base frame (0 rotations). Then solve the inverse kinematics for those poses and send the resulting joint coordinates on the robot."
+ ]
+ },
+ {
+ "cell_type": "code",
+ "execution_count": null,
+ "id": "ee6c7d68-27ab-49d7-9bd3-6184cc8f913e",
+ "metadata": {},
+ "outputs": [],
+ "source": [
+ "from lab_functions import *\n",
+ "import swift\n",
+ "\n",
+ "env = swift.Swift()\n",
+ "env.launch(realtime=True, browser='notebook')\n",
+ "\n",
+ "al5d = rtb.models.URDF.AL5D_mdw()\n",
+ "al5d.q = [0,0,0,0,0]\n",
+ "\n",
+ "arrived = False\n",
+ "env.add(al5d)"
+ ]
+ }
+ ],
+ "metadata": {
+ "kernelspec": {
+ "display_name": "pythonRSC",
+ "language": "python",
+ "name": "pythonrsc"
+ },
+ "language_info": {
+ "codemirror_mode": {
+ "name": "ipython",
+ "version": 3
+ },
+ "file_extension": ".py",
+ "mimetype": "text/x-python",
+ "name": "python",
+ "nbconvert_exporter": "python",
+ "pygments_lexer": "ipython3",
+ "version": "3.11.5"
+ }
+ },
+ "nbformat": 4,
+ "nbformat_minor": 5
+}
diff --git a/theory/lab06_Trajectories.ipynb b/theory/lab06_Trajectories.ipynb
new file mode 100644
index 0000000000000000000000000000000000000000..cd13105b4559b4c00be4c74c87984cd69ed5e159
--- /dev/null
+++ b/theory/lab06_Trajectories.ipynb
@@ -0,0 +1,653 @@
+{
+ "cells": [
+ {
+ "cell_type": "markdown",
+ "metadata": {},
+ "source": [
+ "# A dish that can hold more than food\n",
+ "\n",
+ "One day, while performing an EVA (Extra Vehicular Activity), dr. Elena Vasilescu came across something that was bound to improve her life on the red planet significantly: A satellite dish! It was displaced during the failed landing, and the mechanism to control its motion was totally destroyed. But she figured, if she had a way to make it point towards one of the [Mars Relay Network](https://mars.nasa.gov/news/8861/the-mars-relay-network-connects-us-to-nasas-martian-explorers/) satellites, she might be able to establish some kind of communication with the Earth. After all, she did start getting lonely after all this time.\n",
+ "\n",
+ "<img src=\"../artwork/stranded/satelite_dish.png\" width=60% />\n",
+ "\n",
+ "The dish was rather big, and she couldn't possibly position it by her bare hands. Besides, she would need to move it constantly so that it follows the trajectories of the satellites, otherwise communication would be broken.\n",
+ "\n",
+ "What kind of mechanism would she need to make this happen?"
+ ]
+ },
+ {
+ "cell_type": "markdown",
+ "metadata": {
+ "jp-MarkdownHeadingCollapsed": true,
+ "tags": []
+ },
+ "source": [
+ "## End-effector trajectories\n",
+ "\n",
+ "Until now, we have seen how we describe a robotic system in a static configuration, either when it is about the relationship between joint position and end-effector position, or when it is about joint velocity and end-effector velocity (but solved for a specific joint configuration). However, one of the most relevant tasks for a robotic system is to be able to plan its trajectory over time. This is important for two reasons: a) it gives us a way to control the velocity of the robot along a specific path b) it allows us to develop obstacle avoidance algorithms.\n",
+ "\n",
+ "There are several methods for executing end-effector trajectories. In this laboratory we will explore a couple of them, starting from the most naive to more advanced ones."
+ ]
+ },
+ {
+ "cell_type": "markdown",
+ "metadata": {},
+ "source": [
+ "## Single joint trajectories\n",
+ "\n",
+ "A trajectory is a set of poses (path) with a time component. Remember that a pose defines both the position and the orientation of a body in space. For now, we will deal with position and orientation together, but later on we will see that we need to do different calculations for each component of pose.\n",
+ "\n",
+ "Since we want to control the position of the end-effector, we need to first have calculated the inverse kinematics model of the robot. If we know the inverse kinematics model, then we can control the joint coordinates so that the end-effector moves along the points of the trajectory.\n",
+ "\n",
+ "\\begin{equation}\n",
+ " f(P) \\rightarrow q\n",
+ "\\end{equation}\n",
+ "\n",
+ "Suppose that we want to move the end-effector of a robot to move from pose S to point E in space. We would need to firstly calculate the joint coordinates for the start and end position, using the inverse kinematics\n",
+ "\n",
+ "\\begin{equation}\n",
+ " f(P_{s}) \\rightarrow q_{s}\\\\\n",
+ " f(P_{e}) \\rightarrow q_{e}\n",
+ "\\end{equation}\n",
+ "\n",
+ "Remember that $q_s$ and $q_e$ are vectors, having one element for each joint. Therefore, if we want to move the from pose S to pose E, we need to change the joint coordinates from values $q_s$ to the values of $q_e$. There are two main ways of doing this, either using the maximum velocity of each joint, or using the mimimum velocity of all the joints.\n",
+ "\n",
+ "### Interpolation basics\n",
+ "\n",
+ "In this case, each joint moves individually with its maximum velocity. This will ensure the fastest possible transition from pose S to pose E, as each joint will move to its target coordinate the fastest possible. This means that some joints might finish executing their motion faster than others, based on their maximum velocity and how much distance they need to cover.\n",
+ "\n",
+ "\\begin{equation}\n",
+ " q_{1s} \\rightarrow q_{1e}\\\\\n",
+ " q_{2s} \\rightarrow q_{2e}\\\\\n",
+ " \\vdots \\\\\n",
+ " q_{ns} \\rightarrow q_{ne}\n",
+ "\\end{equation}\n",
+ "\n",
+ "The process of changing smoothly a variable from an initial value to a final value over time, is called _interpolation_ and it can be achieved using the following equation:\n",
+ "\n",
+ "\\begin{equation}\n",
+ " q(t) = (1-s(t))q_s+s(t)q_e\n",
+ "\\end{equation}\n",
+ "\n",
+ "Where $q(t)$ is the interpolated value of the joint coordinate over time, $q_s$ and $q_e$ are the starting and ending joint coordinates, and $s(t)$ is a function that varies from 0 to 1 monotonously over time. This means that $s(0) = 0$, $s(T) = 1$, and it has values between 0 and 1 for the time in between. Based on the form of $s(t)$ we can achieve different types of interpolations."
+ ]
+ },
+ {
+ "cell_type": "markdown",
+ "metadata": {},
+ "source": [
+ "### Linear interpolation\n",
+ "\n",
+ "The most basic form of interpolation is _linear interpolation_ and can be achieved if $s(t)$ is a linear function. For example, if want to achieve a motion from $q_s = 1$ to $q_e = 5$ in 8 seconds, then $s(t) = \\frac{1}{8}t$, since $s(0) = 0$ and $s(8) = 1$. This means that:\n",
+ "\n",
+ "\\begin{equation}\n",
+ " q(0) = (1 - 0)q_s + 0q_e = q_s\\\\\n",
+ " q(8) = (1 - 1)q_s + 1q_e = q_e\n",
+ "\\end{equation}\n",
+ "\n",
+ "The general equation for defining $s(t)$ would be:\n",
+ "\n",
+ "\\begin{equation}\n",
+ " s(t) = \\dfrac{t-t_s}{t_e-t_s}\n",
+ "\\end{equation}\n",
+ "\n",
+ "Where $t_s$ and $t_e$ are the starting and ending time respectively. You can see a graphic representation of how the variable s and q look like over time, for various values of time duration, and starting and ending joint coordinates."
+ ]
+ },
+ {
+ "cell_type": "code",
+ "execution_count": null,
+ "metadata": {},
+ "outputs": [],
+ "source": [
+ "from lab_functions import *\n",
+ "import swift\n",
+ "\n",
+ "env = swift.Swift()\n",
+ "env.launch(realtime=True)\n",
+ "\n",
+ "ur5 = rtb.models.URDF.UR5()\n",
+ "ur5.q = ur5.qr\n",
+ "\n",
+ "arrived = False\n",
+ "env.add(ur5)"
+ ]
+ },
+ {
+ "cell_type": "code",
+ "execution_count": null,
+ "metadata": {
+ "tags": []
+ },
+ "outputs": [],
+ "source": [
+ "def linear_interpolation(qs=-90, qe=0, T=8):\n",
+ " steps = 20*T\n",
+ " time = np.linspace(0.01,T,steps)\n",
+ " s = time/T\n",
+ " ds = s/time\n",
+ " dds = ds-ds\n",
+ " q = (1-s)*qs+s*qe\n",
+ " dq = (1-ds)*qs+ds*qe\n",
+ " ddq = (1-dds)*qs+dds*qe\n",
+ " s.shape = (steps,1)\n",
+ " q.shape = (steps,1)\n",
+ " dq.shape = (steps,1)\n",
+ " ddq.shape = (steps,1)\n",
+ " plot_interpolation(time, s, q, dq, ddq, 'Linear interpolation', False)\n",
+ " \n",
+ " zers = np.asmatrix(np.zeros((time.shape[0], 1)))\n",
+ " qs = np.concatenate((zers, q, zers, zers, zers, zers), axis=1)\n",
+ "\n",
+ " for i in range(steps):\n",
+ " ur5.q = np.radians(qs[i,:])\n",
+ " env.step(0.05)\n",
+ " \n",
+ "interact_manual(linear_interpolation, qs=(-90,90), qe=(-90,90), T=(1,10))"
+ ]
+ },
+ {
+ "cell_type": "markdown",
+ "metadata": {},
+ "source": [
+ "The main disadvantage of linear interpolation is that it introduces discontiniouities in the first and second derivatives of the variable. This means that velocity and acceleration will need to change instanteounsly, which will introduce trouble for the controller."
+ ]
+ },
+ {
+ "cell_type": "markdown",
+ "metadata": {},
+ "source": [
+ "### Polynomial interpolation\n",
+ "\n",
+ "To alleviate the problems of discontiniouities in the velocity and acceleration, we can perform more complex forms of interpolation, namely with a polynomial instead of a linear function. We can choose the order of the polynomial to our needs, but if we want to ensure continiouity in the first and second derivaties at the beginning and the end of the trajectory, then we need at least a 5th order polynomial. This polynomial would look like this:\n",
+ "\n",
+ "\\begin{equation}\n",
+ " s(t) = At^5 + Bt^4 + Ct^3 + Dt^2 + Et + F, t \\in [0,T]\n",
+ "\\end{equation}\n",
+ "\n",
+ "To ensure continiouity, we can define the following boundary conditions, that define the six coefficients of the polynomial.\n",
+ "\n",
+ "\\begin{equation}\n",
+ " s(0) = q_s \\\\\n",
+ " s(T) = q_e \\\\ \n",
+ " \\dot{s}(0) = 0 \\\\\n",
+ " \\dot{s}(T) = 0 \\\\\n",
+ " \\ddot{s}(0) = 0 \\\\\n",
+ " \\ddot{s}(T) = 0 \\\\\n",
+ "\\end{equation}\n",
+ "\n",
+ "Knowning that:\n",
+ "\n",
+ "\\begin{equation}\n",
+ " \\dot{s} = 5As^4 + 4Bs^3 + 3Cs^2 + 2Ds + E \\\\\n",
+ " \\ddot{s} = 20As^3 + 12Bs^2 + 6Cs + 2D\n",
+ "\\end{equation}\n",
+ "\n",
+ "We can of course define different starting and ending velocities and accelerations. Using these boundary conditions, we end up with a system of six equations with six unknowns, that can be expressed in algebraic form like this:\n",
+ "\n",
+ "\\begin{equation}\n",
+ "\\left[ {\\begin{array}{*{20}c}\n",
+ " s(0) \\\\\n",
+ " s(T) \\\\\n",
+ " \\dot{s}(0)\\\\\n",
+ " \\dot{s}(T)\\\\\n",
+ " \\ddot{s}(0)\\\\\n",
+ " \\ddot{s}(T)\\\\\n",
+ "\\end{array}}\n",
+ "\\right] = \\left[ {\\begin{array}{*{20}c}\n",
+ " 0 & 0 & 0 & 0 & 0 & 1 \\\\\n",
+ " T^5 & T^4 & T^3 & T^2 & T & 1 \\\\\n",
+ " 0 & 0 & 0 & 0 & 1 & 0 \\\\\n",
+ " 5T^4& 4T^3& 3T^2& 2T & 1 & 0 \\\\\n",
+ " 0 & 0 & 0 & 2 & 0 & 0 \\\\\n",
+ " 20T^3&12T^2&6T& 2 & 0 & 0 \\\\\n",
+ "\\end{array}} \\right] \\left[ {\\begin{array}{*{20}c}\n",
+ " A \\\\\n",
+ " B \\\\\n",
+ " C \\\\\n",
+ " D \\\\\n",
+ " E \\\\\n",
+ " F \\\\\n",
+ "\\end{array}}\n",
+ "\\right]\n",
+ "\\end{equation}\n",
+ "\n",
+ "This is a linear system of equations that can be easily solved by inverting the 6x6 matrix. Remember that we are looking for the coefficiencts A, B, C, D, E, and F for desired boundary conditions (velocity and acceleration at the beginning and the end of the trajectory). You can see a graphic representation of how the variable s and q look like over time, for various values of time duration, starting and ending joint coordinates, and boundary conditions."
+ ]
+ },
+ {
+ "cell_type": "code",
+ "execution_count": null,
+ "metadata": {
+ "tags": []
+ },
+ "outputs": [],
+ "source": [
+ "from lab_functions import *\n",
+ "\n",
+ "def polynomial_interpolation(qs=-90, qe=-20, dss=0, dse=0, ddss=0, ddse=0, T=8):\n",
+ " steps = 20*T\n",
+ " time = np.linspace(0,T,steps)\n",
+ " A = np.array([[0,0,0,0,0,1],[T**5, T**4, T**3,T**2, T, 1],[0,0,0,0,1,0],[5*T**4,4*T**3,3*T**2,2*T,1,0],[0,0,0,2,0,0],[20*T**3,12*T**2,6*T,2,0,0]])\n",
+ " inpt = np.array([0,1,dss,dse,ddss,ddse])\n",
+ " inpt.shape = (6,1)\n",
+ " sol = np.linalg.inv(A).dot(inpt)\n",
+ " timed = np.array((np.power(time,5),np.power(time,4),np.power(time,3),np.power(time,2),time,np.ones((steps,))))\n",
+ " s = sol[0]*timed[0]+sol[1]*timed[1]+sol[2]*timed[2]+sol[3]*timed[3]+sol[4]*timed[4]+sol[5]*timed[5]\n",
+ " ds = 5*sol[0]*timed[1]+4*sol[1]*timed[2]+3*sol[2]*timed[3]+2*sol[3]*timed[4]+sol[4]\n",
+ " dds = 20*sol[0]*timed[2]+12*sol[1]*timed[3]+6*sol[2]*timed[4]+2*sol[3]\n",
+ " q = (1-s)*qs+s*qe\n",
+ " dq = (1-ds)*qs+ds*qe\n",
+ " ddq = (1-dds)*qs+dds*qe\n",
+ " s.shape = (steps,1)\n",
+ " q.shape = (steps,1)\n",
+ " dq.shape = (steps,1)\n",
+ " ddq.shape = (steps,1)\n",
+ " plot_interpolation(time, s, q, dq, ddq, 'Polynomial interpolation', True)\n",
+ " \n",
+ " zers = np.asmatrix(np.zeros((time.shape[0], 1)))\n",
+ " qs = np.concatenate((zers, q, zers, zers, zers, zers), axis=1)\n",
+ " \n",
+ " for i in range(steps):\n",
+ " ur5.q = np.radians(qs[i,:])\n",
+ " env.step(0.05)\n",
+ " \n",
+ "interact_manual(polynomial_interpolation, qs=(-90,90), qe=(-90,90), dss=(-2,2,0.1), dse=(-2,2,0.1), ddss=(-2,2,0.01), ddse=(-2,2,0.01), T=(1,10))"
+ ]
+ },
+ {
+ "cell_type": "markdown",
+ "metadata": {},
+ "source": [
+ "Obviously, we can do the same thing for two or more joints. We just need to decide on the boundary conditions of each joint and calculate its own trajectory"
+ ]
+ },
+ {
+ "cell_type": "code",
+ "execution_count": null,
+ "metadata": {
+ "scrolled": true,
+ "tags": []
+ },
+ "outputs": [],
+ "source": [
+ "from lab_functions import *\n",
+ "\n",
+ "def polynomial_interpolation(qs=0, qe=70, dss=0, dse=0, ddss=0, ddse=0, T=8):\n",
+ " steps = 20*T\n",
+ " time = np.linspace(0,T,steps)\n",
+ " timed = np.array((np.power(time,5),np.power(time,4),np.power(time,3),np.power(time,2),time,np.ones((steps,))))\n",
+ " A = np.array([[0,0,0,0,0,1],[T**5, T**4, T**3,T**2, T, 1],[0,0,0,0,1,0],[5*T**4,4*T**3,3*T**2,2*T,1,0],[0,0,0,2,0,0],[20*T**3,12*T**2,6*T,2,0,0]])\n",
+ " inpt1 = np.array([0,1,0,0,0,0])\n",
+ " inpt1.shape = (6,1)\n",
+ " sol1 = np.linalg.inv(A).dot(inpt1)\n",
+ " s1 = sol1[0]*timed[0]+sol1[1]*timed[1]+sol1[2]*timed[2]+sol1[3]*timed[3]+sol1[4]*timed[4]+sol1[5]*timed[5]\n",
+ " q1 = (1-s1)*-90-s1*20\n",
+ " \n",
+ " inpt = np.array([0,1,dss,dse,ddss,ddse])\n",
+ " inpt.shape = (6,1)\n",
+ " sol = np.linalg.inv(A).dot(inpt)\n",
+ " s = sol[0]*timed[0]+sol[1]*timed[1]+sol[2]*timed[2]+sol[3]*timed[3]+sol[4]*timed[4]+sol[5]*timed[5]\n",
+ " q = (1-s)*qs+s*qe\n",
+ " \n",
+ " q.shape = (steps,1)\n",
+ " q1.shape = (steps,1)\n",
+ " \n",
+ " zers = np.asmatrix(np.zeros((time.shape[0], 1)))\n",
+ " qs = np.concatenate((zers, q1, -q, zers, zers, zers), axis=1)\n",
+ "\n",
+ " for i in range(steps):\n",
+ " ur5.q = np.radians(qs[i,:])\n",
+ " env.step(0.05)\n",
+ " \n",
+ "interact_manual(polynomial_interpolation, qs=(-90,90), qe=(-90,90), dss=(-2,2,0.1), dse=(-2,2,0.1), ddss=(-2,2,0.01), ddse=(-2,2,0.01), T=(1,10))"
+ ]
+ },
+ {
+ "cell_type": "markdown",
+ "metadata": {},
+ "source": [
+ "## End effector trajectories\n",
+ "\n",
+ "Controlling each individual joint is not by itself very useful, as we are usually interested to control the end-effector pose. As we know from inverse kinematics we can calculate the joint coordinates required to achieve a certain pose, depending always on the degrees of freedom of our robotic arm. Therefore, if we can calculate a trajectory for the end-effector, then we can calculate the required joint coordinates for achieving the trajectory using inverse kinematics. In order to produce once again smooth transitions in pose, we will need to interpolate from a starting to an ending position. We will decompose the interpolation of pose into two parts: position and orientation\n",
+ "\n",
+ "### Interpolating position\n",
+ "\n",
+ "Interpolating the position is rather straight-forward, as we can interpolate each one of the components of the position vector ($X$, $Y$, and $Z$ coordinates) from the beginning to the end-position. If we want to ensure smooth motion without jerk, we should once again apply a polynomial interpolation to each coordinate individually.\n",
+ "\n",
+ "\\begin{equation}\n",
+ " P_x(0) \\rightarrow P_x(T)\\\\\n",
+ " P_y(0) \\rightarrow P_y(T)\\\\\n",
+ " P_z(0) \\rightarrow P_z(T)\\\\\n",
+ "\\end{equation}"
+ ]
+ },
+ {
+ "cell_type": "code",
+ "execution_count": null,
+ "metadata": {
+ "tags": []
+ },
+ "outputs": [],
+ "source": [
+ "from lab_functions import *\n",
+ "\n",
+ "def threed_interpolation(xs=0, xe=20, ys=0, ye=30, zs=0, ze=10, T=8):\n",
+ " dss, dse, ddss, ddse = 0,0,0,0\n",
+ " steps = 20*T\n",
+ " time = np.linspace(0,T,steps)\n",
+ " timed = np.array((np.power(time,5),np.power(time,4),np.power(time,3),np.power(time,2),time,np.ones((steps,))))\n",
+ " A = np.array([[0,0,0,0,0,1],[T**5, T**4, T**3,T**2, T, 1],[0,0,0,0,1,0],[5*T**4,4*T**3,3*T**2,2*T,1,0],[0,0,0,2,0,0],[20*T**3,12*T**2,6*T,2,0,0]]) \n",
+ " inpt = np.array([0,1,dss,dse,ddss,ddse])\n",
+ " inpt.shape = (6,1)\n",
+ " sol = np.linalg.inv(A).dot(inpt)\n",
+ " s = sol[0]*timed[0]+sol[1]*timed[1]+sol[2]*timed[2]+sol[3]*timed[3]+sol[4]*timed[4]+sol[5]*timed[5]\n",
+ " x = (1-s)*xs+s*xe\n",
+ " y = (1-s)*ys+s*ye\n",
+ " z = (1-s)*zs+s*ze\n",
+ " \n",
+ " x.shape = (steps,1)\n",
+ " y.shape = (steps,1)\n",
+ " z.shape = (steps,1)\n",
+ " \n",
+ " plot_car_interpolation(time, x, y, z, '3D interpolation')\n",
+ " \n",
+ "interact_manual(threed_interpolation, xs=(0,90), xe=(-90,90), ys=(-2,2,0.1), ye=(-2,2,0.1), zs=(-2,2,0.01), ze=(-2,2,0.01), T=(1,10))"
+ ]
+ },
+ {
+ "cell_type": "markdown",
+ "metadata": {},
+ "source": [
+ "Now that we have a cartesian trajectory, we can generate the joint trajectories by solving the inverse kinematics model for each step of the trajectory."
+ ]
+ },
+ {
+ "cell_type": "code",
+ "execution_count": null,
+ "metadata": {
+ "tags": []
+ },
+ "outputs": [],
+ "source": [
+ "from lab_functions import *\n",
+ "\n",
+ "def eef_translation(xs=0.6, xe=0.5, ys=0.2, ye=0.4, zs=0.3, ze=0.4, T=8):\n",
+ " dss, dse, ddss, ddse = 0,0,0,0\n",
+ " steps = 20*T\n",
+ " time = np.linspace(0,T,steps)\n",
+ " timed = np.array((np.power(time,5),np.power(time,4),np.power(time,3),np.power(time,2),time,np.ones((steps,))))\n",
+ " A = np.array([[0,0,0,0,0,1],[T**5, T**4, T**3,T**2, T, 1],[0,0,0,0,1,0],[5*T**4,4*T**3,3*T**2,2*T,1,0],[0,0,0,2,0,0],[20*T**3,12*T**2,6*T,2,0,0]]) \n",
+ " inpt = np.array([0,1,dss,dse,ddss,ddse])\n",
+ " inpt.shape = (6,1)\n",
+ " sol = np.linalg.inv(A).dot(inpt)\n",
+ " s = sol[0]*timed[0]+sol[1]*timed[1]+sol[2]*timed[2]+sol[3]*timed[3]+sol[4]*timed[4]+sol[5]*timed[5]\n",
+ " x = (1-s)*xs+s*xe\n",
+ " y = (1-s)*ys+s*ye\n",
+ " z = (1-s)*zs+s*ze\n",
+ " \n",
+ " x.shape = (steps,1)\n",
+ " y.shape = (steps,1)\n",
+ " z.shape = (steps,1)\n",
+ " \n",
+ " qs = np.zeros((steps,6))\n",
+ " for i in range(steps):\n",
+ " setpoint = SE3(x[i,0],y[i,0],z[i,0])*SE3.Rx(180,'deg')*SE3.Rz(-90,'deg')\n",
+ " if i == 0:\n",
+ " qs[i,:] = ur5.ikine_LM(setpoint).q\n",
+ " else:\n",
+ " qs[i,:] = ur5.ikine_LM(setpoint, q0=qs[max(i-1,0),:]).q\n",
+ "\n",
+ " for i in range(steps):\n",
+ " ur5.q = qs[i,:]\n",
+ " env.step(0.05)\n",
+ " \n",
+ " plot_car_interpolation(time, x, y, z, 'Cartesian space interpolation')\n",
+ " plot_joint_interpolation(time, qs[:,0], qs[:,1], qs[:,2], qs[:,3], qs[:,4], qs[:,5], 'Joint space interpolation')\n",
+ "\n",
+ "interact_manual(eef_translation, xs=(-0.9,0.9), xe=(-0.9,0.9), ys=(-0.9,0.9), ye=(-0.9,0.9), zs=(-0.9,0.9), ze=(-0.9,0.9), T=(1,10))"
+ ]
+ },
+ {
+ "cell_type": "markdown",
+ "metadata": {},
+ "source": [
+ "### Interpolating orientation\n",
+ "\n",
+ "Orientation is more complicated to interpolate, as its components cannot be interpolated independently of each other when written in a matrix form. This is because the numbers of the transformation matrix are not all independent from each other, and each column represents a unit vector. As such, we cannot interpolate the numbers from our starting orientation toward the ending orientation, since the interpolated values are not guaranteed to form unit vectors. Consider the following simple example of interpolating from one unit vector to another:\n",
+ "\n",
+ "\\begin{equation}\n",
+ " \\left[ {\\begin{array}{*{20}c}\n",
+ " 0 \\\\\n",
+ " 0 \\\\\n",
+ " 1\n",
+ "\\end{array}}\n",
+ "\\right] \\rightarrow\n",
+ "\\left[ {\\begin{array}{*{20}c}\n",
+ " 1 \\\\\n",
+ " 0 \\\\\n",
+ " 0\n",
+ "\\end{array}}\n",
+ "\\right]\n",
+ "\\end{equation}\n",
+ "\n",
+ "if we interpolate each component individually, at some point we will end up with vector:\n",
+ "\n",
+ "\\begin{equation}\n",
+ "\\left[ {\\begin{array}{*{20}c}\n",
+ " 0.5 \\\\\n",
+ " 0 \\\\\n",
+ " 0.5\n",
+ "\\end{array}}\n",
+ "\\right]\n",
+ "\\end{equation}\n",
+ "\n",
+ "which is not a unit vector!\n",
+ "\n",
+ "Therefore, for interpolating orientation, we need to break the matrix down to its components. As we have seen in the transformations laboratory, there are different ways to express orientation. If we use the Euler angle representation, then we can interpolate the components individually and end up with a smooth transition from one orientation to another one."
+ ]
+ },
+ {
+ "cell_type": "code",
+ "execution_count": null,
+ "metadata": {
+ "tags": []
+ },
+ "outputs": [],
+ "source": [
+ "from lab_functions import *\n",
+ "\n",
+ "def eef_pose(rs=145, re=180, ps=110, pe=40, yas=50, yae=90, xs=0.2, xe=0.4, ys=0.2, ye=0.4, zs=0.2, ze=0.4, T=8):\n",
+ " dss, dse, ddss, ddse = 0,0,0,0\n",
+ " steps = 20*T\n",
+ " time = np.linspace(0,T,steps)\n",
+ " timed = np.array((np.power(time,5),np.power(time,4),np.power(time,3),np.power(time,2),time,np.ones((steps,))))\n",
+ " A = np.array([[0,0,0,0,0,1],[T**5, T**4, T**3,T**2, T, 1],[0,0,0,0,1,0],[5*T**4,4*T**3,3*T**2,2*T,1,0],[0,0,0,2,0,0],[20*T**3,12*T**2,6*T,2,0,0]]) \n",
+ " inpt = np.array([0,1,dss,dse,ddss,ddse])\n",
+ " inpt.shape = (6,1)\n",
+ " sol = np.linalg.inv(A).dot(inpt)\n",
+ " s = sol[0]*timed[0]+sol[1]*timed[1]+sol[2]*timed[2]+sol[3]*timed[3]+sol[4]*timed[4]+sol[5]*timed[5]\n",
+ " x = (1-s)*xs+s*xe\n",
+ " y = (1-s)*ys+s*ye\n",
+ " z = (1-s)*zs+s*ze\n",
+ " r = ((1-s)*rs+s*re)\n",
+ " p = ((1-s)*ps+s*pe)\n",
+ " ya = ((1-s)*yas+s*yae)\n",
+ " \n",
+ " x.shape = (steps,1)\n",
+ " y.shape = (steps,1)\n",
+ " z.shape = (steps,1)\n",
+ " r.shape = (steps,1)\n",
+ " p.shape = (steps,1)\n",
+ " ya.shape = (steps,1)\n",
+ " \n",
+ " qs = np.zeros((steps,6))\n",
+ " for i in range(steps):\n",
+ " setpoint = SE3(x[i,0],y[i,0],z[i,0])*SE3.RPY([r[i,0],p[i,0],ya[i,0]], unit='deg')\n",
+ " qs[i,:] = ur5.ikine_LM(setpoint, q0=qs[max(i-1,0),:]).q\n",
+ "\n",
+ " for i in range(steps):\n",
+ " ur5.q = qs[i,:]\n",
+ " env.step(0.05)\n",
+ " \n",
+ " plot_car_interpolation(time, x, y, z, 'Cartesian space interpolation')\n",
+ " plot_joint_interpolation(time, qs[:,0], qs[:,1], qs[:,2], qs[:,3], qs[:,4], qs[:,5], 'Joint space interpolation')\n",
+ "\n",
+ "interact_manual(eef_pose, rs=(-180,180), re=(-180,180), ps=(-180,180), pe=(-180,180), yas=(-180,180), yae=(-180,180), xs=(-0.9,0.9), xe=(-0.9,0.9), ys=(-0.9,0.9), ye=(-0.9,0.9), zs=(-0.9,0.9), ze=(-0.9,0.9), T=(1,10))"
+ ]
+ },
+ {
+ "cell_type": "markdown",
+ "metadata": {},
+ "source": [
+ "Similarly, we can use quaternions, which can be interpolated as well. Using quaternions avoids problems with 'gimbal lock' (loss of one or more degrees of freedom), and produce a shorter path interpolation than Euler angles.\n",
+ "\n",
+ "Quaternions can be interpolated using the [SLERP](https://en.wikipedia.org/wiki/Slerp) method (**S**pherical **L**inear int**ERP**olation), which is implemented in the [SciPy](https://docs.scipy.org/doc/scipy/reference/generated/scipy.spatial.transform.Slerp.html) library of Python. The procedure for performing the interpolation is the same as with the previous cases: Choose initial and final orientation, calculate the interpolation polynomial, interpolate the value of the angles at the timesteps of the polynomial (using SLERP).\n",
+ "\n",
+ "Below, you can see a basic example of how to use SLERP for interpolating between two orientations using SLERP."
+ ]
+ },
+ {
+ "cell_type": "code",
+ "execution_count": null,
+ "metadata": {
+ "tags": []
+ },
+ "outputs": [],
+ "source": [
+ "from scipy.spatial.transform import Slerp\n",
+ "from lab_functions import *\n",
+ "\n",
+ "def quat_vs_eul(rs=180, re=-20, ys=20, ye=180, T=8, quaternions=True):\n",
+ " # It is easier if we create a quaternion from a known transformation matrix (or euler angles).\n",
+ " init_q = r2q(rotz(ys, 'deg')@rotx(rs, 'deg'))\n",
+ " final_q = r2q(rotz(ye, 'deg')@rotx(re, 'deg'))\n",
+ "\n",
+ " # Constructing interpolator s using polynomial\n",
+ " dss, dse, ddss, ddse = 0,0,0,0 # Starting and ending velocity and acceleration\n",
+ " steps = 20*T\n",
+ " time = np.linspace(0,T,steps)\n",
+ " timed = np.array((np.power(time,5),np.power(time,4),np.power(time,3),np.power(time,2),time,np.ones((steps,))))\n",
+ " A = np.array([[0,0,0,0,0,1],[T**5, T**4, T**3,T**2, T, 1],[0,0,0,0,1,0],[5*T**4,4*T**3,3*T**2,2*T,1,0],[0,0,0,2,0,0],[20*T**3,12*T**2,6*T,2,0,0]]) \n",
+ " inpt = np.array([0,1,dss,dse,ddss,ddse]).reshape(6,1)\n",
+ " sol = np.linalg.inv(A).dot(inpt)\n",
+ " s = sol[0]*timed[0]+sol[1]*timed[1]+sol[2]*timed[2]+sol[3]*timed[3]+sol[4]*timed[4]+sol[5]*timed[5]\n",
+ "\n",
+ " quat = np.zeros((4,steps))\n",
+ " # Interpolating in Quaternion space\n",
+ " for i in range(steps):\n",
+ " quat[:,i] = qslerp(init_q, final_q, s=s[i], shortest=True)\n",
+ " \n",
+ " # Interpolating in Euler space\n",
+ " r = ((1-s)*rs+s*re)\n",
+ " y = ((1-s)*ys+s*ye)\n",
+ " \n",
+ " qs = np.zeros((steps,6))\n",
+ " for i in range(steps):\n",
+ " if quaternions:\n",
+ " setpoint = SE3(0.1,0.2,0.5)*SE3.Rt(q2r(quat[:,i]))\n",
+ " else:\n",
+ " setpoint = SE3(0.1,0.2,0.5)*SE3.RPY([r[i],0,y[i]], unit='deg', order='zyx')\n",
+ " qs[i,:] = ur5.ikine_LM(setpoint, q0=qs[max(i-1,0),:]).q\n",
+ " \n",
+ " for i in range(steps):\n",
+ " ur5.q = qs[i,:]\n",
+ " env.step(0.05)\n",
+ " \n",
+ "interact_manual(quat_vs_eul, rs=(-180,180), re=(-180,180), ys=(-180,180), ye=(-180,180), T=(1,10))"
+ ]
+ },
+ {
+ "cell_type": "markdown",
+ "metadata": {},
+ "source": [
+ "## Robotics Toolbox\n",
+ "\n",
+ "The toolbox allows us to construct trajectories between two poses, using the __ctraj__ method. Simply give as input the initial and final pose, and the number of steps in between, and the toolbox will calculate the poses in between. If we then need to move our robot along the desired trajectory, we need to solve the inverse kinematics for each individual pose calculated. In order to minimize the time required, for each call of ikine, we can give the solution of the previous step as an initial guess for the numerical optimization. This is what is demonstrated in the example below"
+ ]
+ },
+ {
+ "cell_type": "code",
+ "execution_count": null,
+ "metadata": {
+ "tags": []
+ },
+ "outputs": [],
+ "source": [
+ "from lab_functions import *\n",
+ "\n",
+ "# We define starting and ending poses\n",
+ "T1 = SE3(0.3,0.2,0.3)@SE3.RPY(180,0,90, unit='deg')\n",
+ "T2 = SE3(0.5,0.3,0.5)@SE3.RPY(110,20,70, unit='deg')\n",
+ "steps = 100\n",
+ "tr = rtb.ctraj(T1,T2,steps) # tr is a collection of cartesian poses of size 'steps'\n",
+ "\n",
+ "# We solve the inverse kinematics for the target poses\n",
+ "qs = ur5.ikine_LM(tr).q\n",
+ "\n",
+ "for i in range(steps):\n",
+ " ur5.q = qs[i,:]\n",
+ " env.step(0.05)"
+ ]
+ },
+ {
+ "cell_type": "markdown",
+ "metadata": {},
+ "source": [
+ "## Helping dr. Vasilescu\n",
+ "\n",
+ "Now that you know everything about trajectories, help dr. Vasilescu mount the satelite dish on top of the MARS-O-HELP and [implement a trajectory](../assignments/Assignment06_Trajectories.ipynb) for communicating with the Mars Relay Network satelites."
+ ]
+ }
+ ],
+ "metadata": {
+ "kernelspec": {
+ "display_name": "pythonRSC",
+ "language": "python",
+ "name": "pythonrsc"
+ },
+ "language_info": {
+ "codemirror_mode": {
+ "name": "ipython",
+ "version": 3
+ },
+ "file_extension": ".py",
+ "mimetype": "text/x-python",
+ "name": "python",
+ "nbconvert_exporter": "python",
+ "pygments_lexer": "ipython3",
+ "version": "3.11.5"
+ },
+ "varInspector": {
+ "cols": {
+ "lenName": 16,
+ "lenType": 16,
+ "lenVar": 40
+ },
+ "kernels_config": {
+ "python": {
+ "delete_cmd_postfix": "",
+ "delete_cmd_prefix": "del ",
+ "library": "var_list.py",
+ "varRefreshCmd": "print(var_dic_list())"
+ },
+ "r": {
+ "delete_cmd_postfix": ") ",
+ "delete_cmd_prefix": "rm(",
+ "library": "var_list.r",
+ "varRefreshCmd": "cat(var_dic_list()) "
+ }
+ },
+ "types_to_exclude": [
+ "module",
+ "function",
+ "builtin_function_or_method",
+ "instance",
+ "_Feature"
+ ],
+ "window_display": false
+ }
+ },
+ "nbformat": 4,
+ "nbformat_minor": 4
+}