diff --git a/assignments/Assignment01_Transformations.ipynb b/assignments/Assignment01_Transformations.ipynb
new file mode 100644
index 0000000000000000000000000000000000000000..2a654740ba8ebf29404a260425c6fb7c9df1ef66
--- /dev/null
+++ b/assignments/Assignment01_Transformations.ipynb
@@ -0,0 +1,88 @@
+{
+ "cells": [
+ {
+ "cell_type": "markdown",
+ "id": "9bc27c03-54a3-4a9c-b4bc-a07d3a9385b5",
+ "metadata": {},
+ "source": [
+ "# Finding necessary resources\n",
+ "\n",
+ "With the help of the camera system, dr. Elena Vasilescu has managed to identify a very important package that dropped on the surface during the failed landing. If her calculations are correct, the package contains enough food supplies to last for a year, by which time she hopes to be either out of Mars, or have received help from Earth. The camera system reports that the package is located in the following coordinates: $X = -40 m$, $Y = -90 m$, and $Z = 200 m$. The coordinate frame of the box is rotated around the X axis of the camera by -90 degrees. These coordinates are relative to the camera. \n",
+ "\n",
+ "<img src=\"../artwork/stranded/mars_base_isometric_annotated.png\" width=90%/>\n",
+ "\n",
+ "She can only perform a short trip around the base, using one of the rovers that is equipped with a crane. That is why she needs to identify the location of the package accurately, so she can save time and reduce the risk of getting stranded out of the base.\n",
+ "\n",
+ "After checking the operations manual from the mission, dr. Vasilescu has determined that the camera system is located at $X = 54m$, $ Y = 220 m$, and $Z= 90 m$ away from her, and is rotated around her Z axis by 180 degrees, and around her X axis by 90 degrees.\n",
+ "\n",
+ "Can you help her find the location of the package relative to her base?\n",
+ "\n",
+ "What maneuvers does she need to perform with her crane to go from her base location to the box. The crane must be aligned with the coordinate system of the box in order to pick it up."
+ ]
+ },
+ {
+ "cell_type": "markdown",
+ "id": "e948d8d8-cccf-4078-8df9-6199633b97e6",
+ "metadata": {
+ "jupyter": {
+ "source_hidden": true
+ }
+ },
+ "source": [
+ "## Expand for help\n",
+ "\n",
+ "* Calculate the transformation from frame $V$ to frame $C$ as $R_V^C$\n",
+ "* Transform the coordinates of the box $B$ from the frame of the camera $C$ ($B_C$) to the coordinates of the box on the frame of dr. Vasilescu ($P_V$) using equation 1.11"
+ ]
+ },
+ {
+ "cell_type": "markdown",
+ "id": "b9379d8f-49ff-45da-b5e6-6dda28eb0c98",
+ "metadata": {},
+ "source": [
+ "# Back at home\n",
+ "\n",
+ "With the help of your instructor, connect to the robot arm on your desk and execute the cell below. Move around the sliders to get familiar with how the robot moves. Make sure you read the instructions on the desk, and keep them in mind. Robots can be dangerous, so let's be friendly with them ;)"
+ ]
+ },
+ {
+ "cell_type": "code",
+ "execution_count": null,
+ "id": "31115015-d965-48af-b3ca-fbe9d553d51d",
+ "metadata": {
+ "code_folding": []
+ },
+ "outputs": [],
+ "source": [
+ "### Cell for sending commands to the AL5D robot ###\n",
+ "# Importing necessary modules\n",
+ "from al5d_control import *\n",
+ "from ipywidgets import interact, fixed\n",
+ "\n",
+ "rrob = AL5DControl()\n",
+ "interact(rrob.robot_control, q0=(-90,90), q1=(-90,90), q2=(-90,90), q3=(-90,90), q4=(-90,90), gripper=(-90,90), dq0=fixed(15),dq1=fixed(15),dq2=fixed(15),dq3=fixed(15), dq4=fixed(15), vel=fixed(False)) "
+ ]
+ }
+ ],
+ "metadata": {
+ "kernelspec": {
+ "display_name": "Python3",
+ "language": "python",
+ "name": "python3"
+ },
+ "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/lab01_Transformations.ipynb b/theory/lab01_Transformations.ipynb
new file mode 100644
index 0000000000000000000000000000000000000000..e2907e82376d81dbdd323c84ab07ea55d77a518a
--- /dev/null
+++ b/theory/lab01_Transformations.ipynb
@@ -0,0 +1,541 @@
+{
+ "cells": [
+ {
+ "cell_type": "markdown",
+ "metadata": {},
+ "source": [
+ "# Looking for food in the blind\n",
+ "\n",
+ "Day after day, Dr. Vasilescu dedicated herself to constructing a sustainable habitat out of the remnants of the damaged Artemis spacecraft. She also tried to set up her robotic companions so that they work tirelessly alongside her, so that their mechanical precision compensates for the absence of human interaction. The first system that she managed to bring back online is a camera placed on the initial landing site, which is away from her current habitat and may give her indications on where to find useful resources for her survival.\n",
+ "\n",
+ "<img src=\"../artwork/stranded/mars_landing_site_with_base.png\" width=60%/>\n",
+ "\n",
+ "She could get readings from the camera about the position of a very important package with food supplies, however she needed to understand where exactly is the package relative to her base. She had to understand the basics of coordinate frames and transformations."
+ ]
+ },
+ {
+ "cell_type": "markdown",
+ "metadata": {},
+ "source": [
+ "## Coordinate frames\n",
+ "Kinematics is the study of motions of objects in space. This is very relevant when working with robots, since we are usually interested in interacting with objects in space, or with positioning sensors in specific poses (position and orientation) so that they can gather useful information (e.g., getting images). How do we describe kinematics?\n",
+ "\n",
+ "To describe pose (position and orientation) of objects, we use the concept of _coordinate frames_ (or as you might have learned it in the past, _coordinate systems_). When dealing with moving objects for which we need to describe their _kinematics_, we always need a reference on which distances should be reported on. This is essential for any type of robot, whether it is a robot arm, mobile robot, drone, or underwater. If, besides position, we also care about orientation, then we need to use two coordinate frames: a _reference_ frame, and a _moving_ or _target_ frame.\n",
+ "\n",
+ "A coordinate frame consists of axes, on top of which we are measuring distances. The number of axes depends on the dimensions of the motion we are studying. For instance, if we are studying the motion of a body moving in one dimension, then one single axis would be enough. If we study motions of objects on flat surfaces (e.g., mobile robots), we use coordinate frames with two axes. If we study motions of objects in three-dimensional (3D) space (e.g., robot arms, drones, underwater robots), then we use coordinate frames with three axes. A necessary condition for defining an orthogonal coordinate frame, is that its axes must be all perpendicular to each other. When the units of distance on each of the axes are equal as well, then we say that this coordinate frame is orthonormal.\n",
+ "\n",
+ "When defining coordinate frames in three (or more dimensions), the position of the axes is important. For three dimensions, coordinate frames must respect the rule of the right hand, which is demonstrated in the figure below. To make things more visual and simple, we are also usually coloring each axis. The usual convention in robotics is that $X$ is red, $Y$ is green, and $Z$ is blue. We will use this convention for all coordinate frames in this course.\n",
+ "\n",
+ "<img src=\"../artwork/transformations/R3-right_hand_rule.png\" width=30%/>\n",
+ "\n",
+ "A coordinate frame is essentially a collection of three vectors. The way we represent vectors in mathematical terms is using the vector notation of a point in space. The vector has three elements, each representing the coordinates of the end-point of the vector in a reference coordinate frame:\n",
+ "\n",
+ "$$\n",
+ "V_1 = \\begin{bmatrix}\n",
+ " P_x \\\\\n",
+ " P_y \\\\\n",
+ " P_z \\\\\n",
+ "\\end{bmatrix}\n",
+ "$$\n",
+ "\n",
+ "To represent a coordinate frame, we group three vectors together in order to form a matrix:\n",
+ "\n",
+ "$$\n",
+ "V = \\begin{bmatrix}\n",
+ " P_x^X & P_y^Y & P_x^Z \\\\\n",
+ " P_y^X & P_y^Y & P_y^Z \\\\\n",
+ " P_z^X & P_z^Y & P_z^Z \\\\\n",
+ "\\end{bmatrix}\n",
+ "$$\n",
+ "\n",
+ "or simpler:\n",
+ "\n",
+ "$$\n",
+ "V = \\begin{bmatrix}\n",
+ " X_x & Y_y & Z_x \\\\\n",
+ " X_y & Y_y & Z_y \\\\\n",
+ " X_z & Y_z & Z_z \\\\\n",
+ "\\end{bmatrix}\n",
+ "$$\n",
+ "\n",
+ "where $P_x^Y$ is the $x$ coordinate of the $Y$ axis. This is called the __transformation matrix__ and it tells us how is a _target_ coordinate frame __oriented__ relative to a _reference_ frame. In order to calculate it, we assume that the origin of the vectors are at the origin of the reference frame, and that they all have length 1. Since we always need to express a _target_ coordinate frame w.r.t (with respect to) a _reference_ frame, we need to use appropriate notation. We denote $T_r^t$ the transformation matrix of frame $t$ w.r.t frame $r$. This is called a [_3D rotation group_ and is often denoted as __SO(3)__](https://en.wikipedia.org/wiki/3D_rotation_group).\n",
+ "\n",
+ "You can visualize how the numbers in this matrix are changing when changing the orientation of a target frame $t$ w.r.t. a reference frame $r$, in the interactive widget below."
+ ]
+ },
+ {
+ "cell_type": "code",
+ "execution_count": null,
+ "metadata": {},
+ "outputs": [],
+ "source": [
+ "from ipywidgets import interact\n",
+ "from spatialmath.base import *\n",
+ "\n",
+ "def showAxes(x=0, y=0, z=0):\n",
+ " t = rotx(x*3.14/180)@roty(y*3.14/180)@rotz(z*3.14/180)\n",
+ " trplot(transl(0,0,0), color='gray', width=1, frame='r', length=1.3)\n",
+ " trplot(t, frame='t', style='rgb', width=2, dims=[-1, 1, -1, 1, -1, 1])\n",
+ " print(t)\n",
+ "\n",
+ "interact(showAxes, x=(-90,90), y=(-90,90), z=(-90,90))"
+ ]
+ },
+ {
+ "cell_type": "markdown",
+ "metadata": {},
+ "source": [
+ "## Transformations\n",
+ "\n",
+ "As demonstrated by the previous widget, a _transformation matrix_ represents a rotated coordinate frame with respect to a reference frame. The three columns of the matrix represent the coordinates of the axes of the _transformed_ frame w.r.t. the reference frame. Each of the three axes has three coordinates, and that is how we end up with a $3 \\times 3$ matrix. At the same time, _transformation matrices_ can be thought of as __actions__ for transforming an existing frame. You can imagine that you have a _target frame_ that is perfectly aligned with the reference frame. In that case, the _transformation matrix_ that represents the _target frame_ is:\n",
+ "\n",
+ "$$\n",
+ "V = \\begin{bmatrix}\n",
+ " X_x & Y_y & Z_x \\\\\n",
+ " X_y & Y_y & Z_y \\\\\n",
+ " X_z & Y_z & Z_z \\\\\n",
+ "\\end{bmatrix} = \\begin{bmatrix}\n",
+ " 1 & 0 & 0 \\\\\n",
+ " 0 & 1 & 0 \\\\\n",
+ " 0 & 0 & 1 \\\\\n",
+ "\\end{bmatrix} \n",
+ "$$\n",
+ "\n",
+ "which is the $3 \\times 3$ identity matrix. If we want to _transform_ this frame, we can multiply with another _transformation matrix_. Since the initial frame is the identity matrix, the result of this multiplication will be equal to the _transformation matrix_. Therefore, a transformation matrix represents __both a transformed frame__ w.r.t. to a reference frame, but also __the action of transforming__ a frame w.r.t. a reference frame.\n",
+ "\n",
+ "### Rotation\n",
+ "\n",
+ "When dealing with coordinate frames represented by $3 \\times 3$ matrices, we have only information about the orientation of the frames. Therefore, the only kind of transformations that we can perform (preserving orthogonality) are rotations. Since we have 3 axes, we can perform 3 types of rotations, each around one of the axes. You can visualize these three rotations using the widget below"
+ ]
+ },
+ {
+ "cell_type": "code",
+ "execution_count": null,
+ "metadata": {},
+ "outputs": [],
+ "source": [
+ "from ipywidgets import interact, widgets\n",
+ "from spatialmath.base import *\n",
+ "import numpy as np\n",
+ "\n",
+ "def RotateOnX(r):\n",
+ " t = rotx(r)\n",
+ " trplot(transl(0,0,0), color='gray', width=1, frame='r', length=1.3)\n",
+ " trplot(t, frame='t', style='rgb', width=2, dims=[-1, 1, -1, 1, -1, 1])\n",
+ " print(t)\n",
+ "\n",
+ "def RotateOnY(p):\n",
+ " t = roty(p)\n",
+ " trplot(transl(0,0,0), color='gray', width=1, frame='r', length=1.3)\n",
+ " trplot(t, frame='t', style='rgb', width=2, dims=[-1, 1, -1, 1, -1, 1])\n",
+ " print(t)\n",
+ "\n",
+ "def RotateOnZ(y):\n",
+ " t = rotz(y)\n",
+ " trplot(transl(0,0,0), color='gray', width=1, frame='r', length=1.3)\n",
+ " trplot(t, frame='t', style='rgb', width=2, dims=[-1, 1, -1, 1, -1, 1])\n",
+ " print(t)\n",
+ "\n",
+ "outx = widgets.interactive(RotateOnX, r=(-3.14,3.14))\n",
+ "outy = widgets.interactive(RotateOnY, p=(-3.14,3.14))\n",
+ "outz = widgets.interactive(RotateOnZ, y=(-3.14,3.14))\n",
+ "\n",
+ "tab = widgets.Tab(children = [outx, outy, outz])\n",
+ "tab.set_title(0, 'Rot X')\n",
+ "tab.set_title(1, 'Rot Y')\n",
+ "tab.set_title(2, 'Rot Z')\n",
+ "\n",
+ "display(tab)"
+ ]
+ },
+ {
+ "cell_type": "markdown",
+ "metadata": {},
+ "source": [
+ "What is the mathematical expression of these transformations? If you observe closely the numbers in the matrices, you will notice a certain pattern: some numbers don't change, and it seems to be those relating to the axis that the rotation is happening about. This makes sense, since e.g. when we rotate around the $X$ axis, the coordinates of that axis are not changing w.r.t. the reference frame.\n",
+ "\n",
+ "The general form of the transformation matrices relating to rotations around each of the three axes is the following:\n",
+ "\n",
+ "Rotation around $x$ axis:\n",
+ "$Rotx(\\varphi) = \\begin{bmatrix}\n",
+ " 1 & 0 & 0 \\\\\n",
+ " 0 & {\\cos \\varphi} & { - \\sin \\varphi } \\\\\n",
+ " 0 & {\\sin \\varphi } & {\\cos \\varphi} \\\\\n",
+ "\\end{bmatrix}$\n",
+ "\n",
+ "Rotation around $y$ axis:\n",
+ "$Roty(\\vartheta) = \\begin{bmatrix}\n",
+ " {\\cos \\vartheta } & 0 & {\\sin \\vartheta } \\\\\n",
+ " 0 & 1 & 0 \\\\\n",
+ " { - \\sin \\vartheta } & 0 & {\\cos \\vartheta } \\\\\n",
+ "\\end{bmatrix}$\n",
+ "\n",
+ "Rotation around $z$ axis:\n",
+ "$Rotz(\\psi) = \\begin{bmatrix}\n",
+ " {\\cos \\psi } & { - \\sin \\psi } & 0 \\\\\n",
+ " {\\sin \\psi } & {\\cos \\psi } & 0 \\\\\n",
+ " 0 & 0 & 1 \\\\\n",
+ "\\end{bmatrix}$"
+ ]
+ },
+ {
+ "cell_type": "markdown",
+ "metadata": {},
+ "source": [
+ "### Translation\n",
+ "\n",
+ "As you observed, the above transformation matrices affect only the orientation of the target coordinate frame. However, coordinate frames can also be displaced/translated w.r.t. a reference frame. To take translations into consideration, we need to use [__Homogeneous coordinates__](https://en.wikipedia.org/wiki/Homogeneous_coordinates). Using homogeneous coordinates, we can represent any vector in 3D space using 3 coordinates and a scaling factor. When we need to keep the same scale when applying a transformation, the scaling factor is one. Therefore, a point in space is represented as following:\n",
+ "\n",
+ "$$ P = \\begin{bmatrix} P_x \\\\ P_y \\\\ P_z \\\\ 1 \\end{bmatrix} $$\n",
+ "\n",
+ "In this case, transformations are defined as $4 \\times 4$ matrices of the following structure:\n",
+ "\n",
+ "$$\n",
+ "T = \\begin{bmatrix}\n",
+ " {} & {} & {} & {} \\\\\n",
+ " 3 & \\times & 3 & {3 \\times 1} \\\\\n",
+ " {} & {} & {} & {} \\\\ \\hline\n",
+ " 1 & \\times & 3 & {1 \\times 1} \\\\\n",
+ "\\end{bmatrix} =\n",
+ "\\begin{bmatrix}\n",
+ " {} & {} & {} & {trans - } \\\\\n",
+ " {} & {rotation} & {} & {la - } \\\\\n",
+ " {} & {} & {} & {tion} \\\\ \\hline\n",
+ " 0 & 0 & 0 & 1 \\\\\n",
+ "\\end{bmatrix} =\n",
+ "\\begin{bmatrix}\n",
+ " X_x & Y_x & Z_x & P_x \\\\\n",
+ " X_y & Y_y & Z_y & P_y \\\\\n",
+ " X_z & Y_z & Z_z & P_z \\\\\n",
+ " 0 & 0 & 0 & 1\n",
+ "\\end{bmatrix}\n",
+ "$$\n",
+ "\n",
+ "The rotation $ 3 \\times 3 $ part of the matrix, represents as before the orientation of the axes of the _target_ frame w.r.t. the reference frame. The translation $ 3 \\times 1 $ vector, represents the position of the origin of the _target_ frame w.r.t. the reference frame. The last row has always fixed values of 0s and 1. Using homogeneous transformation matrices, we can define the following elementary transformations:\n",
+ "\n",
+ "Translation on $X$ axis: $TransX(a) = \\begin{bmatrix}\n",
+ " 1 & 0 & 0 & a \\\\\n",
+ " 0 & 1 & 0 & 0 \\\\\n",
+ " 0 & 0 & 1 & 0 \\\\\n",
+ " 0 & 0 & 0 & 1 \\\\\n",
+ "\\end{bmatrix} $\n",
+ "\n",
+ "Translation on $Y$ axis: $TransY(b) = \\begin{bmatrix}\n",
+ " 1 & 0 & 0 & 0 \\\\\n",
+ " 0 & 1 & 0 & b \\\\\n",
+ " 0 & 0 & 1 & 0 \\\\\n",
+ " 0 & 0 & 0 & 1 \\\\\n",
+ "\\end{bmatrix} $\n",
+ "\n",
+ "Translation on $Z$ axis: $TransZ(c) = \\begin{bmatrix}\n",
+ " 1 & 0 & 0 & 0 \\\\\n",
+ " 0 & 1 & 0 & 0 \\\\\n",
+ " 0 & 0 & 1 & c \\\\\n",
+ " 0 & 0 & 0 & 1 \\\\\n",
+ "\\end{bmatrix} $\n",
+ "\n",
+ "Rotation around $X$ axis: $RotX(\\vartheta) = \\begin{bmatrix}\n",
+ " 1 & 0 & 0 & 0 \\\\\n",
+ " 0 & {\\cos \\vartheta } & { - \\sin \\vartheta } & 0 \\\\\n",
+ " 0 & {\\sin \\vartheta } & {\\cos \\vartheta } & 0 \\\\\n",
+ " 0 & 0 & 0 & 1 \\\\\n",
+ "\\end{bmatrix}$\n",
+ "\n",
+ "Rotation around $Y$ axis: $RotY(\\varphi) = \\begin{bmatrix}\n",
+ " {\\cos \\varphi } & 0 & {\\sin \\varphi } & 0 \\\\\n",
+ " 0 & 1 & 0 & 0 \\\\\n",
+ " { - \\sin \\varphi } & 0 & {\\cos \\varphi } & 0 \\\\\n",
+ " 0 & 0 & 0 & 1 \\\\\n",
+ "\\end{bmatrix} $\n",
+ "\n",
+ "Rotation around $Z$ axis: $ RotZ(\\psi ) = \\begin{bmatrix}\n",
+ " {\\cos \\psi } & { - \\sin \\psi } & 0 & 0 \\\\\n",
+ " {\\sin \\psi } & {\\cos \\psi } & 0 & 0 \\\\\n",
+ " 0 & 0 & 1 & 0 \\\\\n",
+ " 0 & 0 & 0 & 1 \\\\\n",
+ "\\end{bmatrix} $\n",
+ "\n",
+ "As before, a transformation matrix can be understood in different ways:\n",
+ "\n",
+ "- The action of transforming a coordinate frame into a new pose (position and orientation)\n",
+ "- A description of the pose (position and orientation) of a _target_ coordinate system w.r.t a _reference_ coordinate system\n",
+ "\n",
+ "## Combination of transformations\n",
+ "\n",
+ "The transformations we saw above are all elementary, in the sense that they describe either a translation or a rotation on a single axis. However, frames can be arbitrarily oriented relative to each other, resulting in more complex transformations? To describe such complex transformations, we need to combine elementary transformations through multiplication.\n",
+ "\n",
+ "> Any arbitrary transformation between two frames can be described with a finite sequence of elementary transformations\n",
+ "\n",
+ "Since transformation matrices are $4 \\times 4$, a multiplication between two such matrices is possible and will result into another $4 \\times 4$ matrix. This new matrix will also fulfil the conditions for being a transformation matrix. You can verify that through matrix multiplication, the last row of the resulting matrix will still have the same form of a homogeneous transformation matrix.\n",
+ "\n",
+ "$$\n",
+ "AB = \n",
+ "\\begin{bmatrix}\n",
+ " AX_x & AY_x & AZ_x & AP_x \\\\\n",
+ " AX_y & AY_y & AZ_y & AP_y \\\\\n",
+ " AX_z & AY_z & AZ_z & AP_z \\\\\n",
+ " 0 & 0 & 0 & 1\n",
+ "\\end{bmatrix} \\begin{bmatrix}\n",
+ " BX_x & BY_x & BZ_x & BP_x \\\\\n",
+ " BX_y & BY_y & BZ_y & BP_y \\\\\n",
+ " BX_z & BY_z & BZ_z & BP_z \\\\\n",
+ " 0 & 0 & 0 & 1\n",
+ "\\end{bmatrix} = \n",
+ "\\begin{bmatrix}\n",
+ " CX_x & CY_x & CZ_x & CP_x \\\\\n",
+ " CX_y & CY_y & CZ_y & CP_y \\\\\n",
+ " CX_z & CY_z & CZ_z & CP_z \\\\\n",
+ " 0 & 0 & 0 & 1\n",
+ "\\end{bmatrix}\n",
+ "= C\n",
+ "$$\n",
+ "\n",
+ "We can therefore combine several elementary transformations through multiplication to generate more complex transformations. A subtle detail comes in the order of multiplication, since as we know from linear algebra:\n",
+ "\n",
+ "$$ AB \\neq BA $$\n",
+ "\n",
+ "Which way do we need to multiply the transformations then when we are combining them?\n",
+ "\n",
+ "### Absolute transformation (Left multiplication)\n",
+ "\n",
+ "Considering an initial transformation matrix $T_B^A$ describing the pose of frame A w.r.t. frame B, we can further __transform frame A about the axes of frame B__, by multiplying from the __left__ with a transformation matrix $C$. You can see a visualization of an _absolute_ transformation in the following widget. Notice how frame A, which is already transformed w.r.t. frame B, is further transformed about the $X$ axis coordinate frame B, both in the translation and rotation tab."
+ ]
+ },
+ {
+ "cell_type": "code",
+ "execution_count": null,
+ "metadata": {},
+ "outputs": [],
+ "source": [
+ "from ipywidgets import interact, widgets\n",
+ "from spatialmath.base import *\n",
+ "import numpy as np\n",
+ "\n",
+ "TbA = transl(0.5,0.5,1)@trotz(0.807)\n",
+ "\n",
+ "def TranslateOnXLeft(a):\n",
+ " t = transl(a,0,0)@TbA\n",
+ " trplot(trotx(0), color='gray', width=1, frame='B', length=1.3)\n",
+ " trplot(t, frame='A', style='rgb', width=2, dims=[-2, 2, -2, 2, -2, 2])\n",
+ " print('C*T = \\n',t)\n",
+ " \n",
+ "def RotateOnXLeft(a):\n",
+ " t = trotx(a)@TbA\n",
+ " trplot(trotx(0), color='gray', width=1, frame='B', length=1.3)\n",
+ " trplot(t, frame='A', style='rgb', width=2, dims=[-2, 2, -2, 2, -2, 2])\n",
+ " print('C*T = \\n',t)\n",
+ "\n",
+ "rotx = widgets.interactive(RotateOnXLeft, a=(-3.14,3.14))\n",
+ "transx = widgets.interactive(TranslateOnXLeft, a=(-2.0,2.0))\n",
+ "\n",
+ "tab = widgets.Tab(children = [rotx, transx])\n",
+ "tab.set_title(0, 'rotation')\n",
+ "tab.set_title(1, 'translation')\n",
+ "\n",
+ "display(tab)"
+ ]
+ },
+ {
+ "cell_type": "markdown",
+ "metadata": {},
+ "source": [
+ "### Relative transformation (Right multiplication)\n",
+ "\n",
+ "Considering an initial transformation matrix $T_B^A$ describing the pose of frame A w.r.t. frame B, we can further __transform frame A about the axes of frame A__, by multiplying from the __right__ with a transformation matrix $C$. You can see a visualization of a _relative_ transformation in the following widget. Notice how frame A, which is already transformed w.r.t. frame B, is further transformed about its own $X$ axis, both in the translation and rotation tab."
+ ]
+ },
+ {
+ "cell_type": "code",
+ "execution_count": null,
+ "metadata": {},
+ "outputs": [],
+ "source": [
+ "from ipywidgets import interact, widgets\n",
+ "from spatialmath.base import *\n",
+ "import numpy as np\n",
+ "\n",
+ "TbA = transl(0.5,0.5,1)@trotz(0.807)\n",
+ "\n",
+ "def TranslateOnXRight(a):\n",
+ " t = TbA@transl(a,0,0)\n",
+ " trplot(trotx(0), color='gray', width=1, frame='B', length=1.3)\n",
+ " trplot(t, frame='A', style='rgb', width=2, dims=[-2, 2, -2, 2, -2, 2])\n",
+ " print('T*C = \\n',t)\n",
+ " \n",
+ "def RotateOnXRight(a):\n",
+ " t = TbA@trotx(a)\n",
+ " trplot(trotx(0), color='gray', width=1, frame='B', length=1.3)\n",
+ " trplot(t, frame='A', style='rgb', width=2, dims=[-2, 2, -2, 2, -2, 2])\n",
+ " print('C*T = \\n',t)\n",
+ "\n",
+ "rotx = widgets.interactive(RotateOnXRight, a=(-3.14,3.14))\n",
+ "transx = widgets.interactive(TranslateOnXRight, a=(-2.0,2.0))\n",
+ "\n",
+ "tab = widgets.Tab(children = [rotx, transx])\n",
+ "tab.set_title(0, 'rotation')\n",
+ "tab.set_title(1, 'translation')\n",
+ "\n",
+ "display(tab)"
+ ]
+ },
+ {
+ "cell_type": "markdown",
+ "metadata": {},
+ "source": [
+ "## Switching frames\n",
+ "\n",
+ "Quite often in robotic applications, we need to transform a known pose from one frame into another. Let's say we have pose data for an object from a camera, which is reporting poses in its own intrinsic coordinate frame. However, we want to interact with this object using a tool placed at the tip of a robot, which is calculated in the coordinate frame of the robot. Using transformation matrices, we can transform a known pose $T_r^A$ w.r.t. to _reference_ frame $r$, into a pose $T_t^A$ w.r.t. a _target_ frame, as long as we know the transformation $T_t^r$ between the _target_ and _reference_ frame. This is possible through the following operation:\n",
+ "\n",
+ "$$ T_t^A = T_t^r T_r^A $$\n",
+ "\n",
+ "If we don't know transformation $T_t^r$, and instead we know the transformation $T_r^t$ from _reference_ to _target_, we can __invert__ the transformation by inverting the matrix:\n",
+ "\n",
+ "$$ T_r^t = (T_t^r)^{-1} $$\n",
+ "\n",
+ "it can be shown that for homogeneous transformation matrices, the inverse of matrix T has the following form:\n",
+ "\n",
+ "$$\n",
+ "T^{ - 1} = \\begin{bmatrix}\n",
+ " {X_X } & {X_Y } & {X_Z } & { - P_X\\cdot X_X - P_Y \\cdot X_Y - P_Z\\cdot X_Z } \\\\\n",
+ " {Y_X } & {Y_Y } & {Y_Z } & { - P_X \\cdot Y_X - P_Y \\cdot Y_Y - P_Z\\cdot Y_Z } \\\\\n",
+ " {Z_X } & {Z_Y } & {Z_Z } & { - P_X\\cdot Z_X - P_Y \\cdot Z_Y - P_Z\\cdot Z_Z } \\\\\n",
+ " 0 & 0 & 0 & 1\n",
+ "\\end{bmatrix}\n",
+ "$$"
+ ]
+ },
+ {
+ "attachments": {},
+ "cell_type": "markdown",
+ "metadata": {},
+ "source": [
+ "## Spatial math commands\n",
+ "\n",
+ "There are several toolboxes available online for helping us with various robotics operations. The one we will use extensively during the laboratories is [__spatialmath__](https://bdaiinstitute.github.io/spatialmath-python/) together with its __spatialmath.base__. This toolbox offers a very easy way to calculate various transformation matrices, using the __trotx, troty, trotz, and transl__ commands. You can see an example of how are these used below:"
+ ]
+ },
+ {
+ "cell_type": "code",
+ "execution_count": null,
+ "metadata": {
+ "tags": []
+ },
+ "outputs": [],
+ "source": [
+ "from spatialmath.base import *\n",
+ "from spatialmath import *\n",
+ "\n",
+ "t1 = trotx(45, 'deg')\n",
+ "t2 = trotz(45, 'deg')\n",
+ "t3 = transl(2, 0 ,-1)\n",
+ "\n",
+ "# print is a function in python 3, it needs brackets\n",
+ "print(t2)\n",
+ "\n",
+ "# The matrices obtained using this method can be combined to calculate more complex transformations by multiplication:\n",
+ "\n",
+ "# Calculate coordinate '3' frame after three consecutive transformations\n",
+ "t13 = t1@t2@t3\n",
+ "print(t13)"
+ ]
+ },
+ {
+ "cell_type": "code",
+ "execution_count": null,
+ "metadata": {},
+ "outputs": [],
+ "source": [
+ "from spatialmath.base import *\n",
+ "from spatialmath import *\n",
+ "\n",
+ "# You can also visualise the coordinate frame if you use the 'SE3' class\n",
+ "p1 = SE3.Rx(theta=45, unit='deg')\n",
+ "p3 = SE3.Ry(theta=45, unit='deg')\n",
+ "p2 = SE3(x=2, y=0, z=-1)\n",
+ "\n",
+ "(p1@p2@p3).plot()"
+ ]
+ },
+ {
+ "cell_type": "markdown",
+ "metadata": {},
+ "source": [
+ "We can also define a point by its coordinates on a coordinate frame, and find out what are its coordinates on another frame if we know the transformation between the two frames. For defining the point we use the [__numpy__](https://numpy.org/) library. By default, numpy creates row vectors, that's why we need to transpose the vector into a column before we do the multiplication. We do that using the __T__ method:"
+ ]
+ },
+ {
+ "cell_type": "code",
+ "execution_count": null,
+ "metadata": {},
+ "outputs": [],
+ "source": [
+ "import numpy as np\n",
+ "\n",
+ "# Define point P with coordinates 1,2,3\n",
+ "P3 = np.array([[1,2,3,1]]).T\n",
+ "\n",
+ "# Calculating coordinates of point P w.r.t. frame '1', if we know its coordinates w.r.t. frame 3\n",
+ "t13@P3"
+ ]
+ },
+ {
+ "cell_type": "markdown",
+ "metadata": {},
+ "source": [
+ "## Helping dr. Vasilescu\n",
+ "\n",
+ "Now that you know enough about coordinate frames and transformations, go ahead and help dr. Vasilescu [find necessary resources for survival](../assignments/Assignment01_Transformations.ipynb)"
+ ]
+ }
+ ],
+ "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
+}