diff --git a/artwork/DynMod/fig7-2.png b/artwork/DynMod/fig7-2.png
new file mode 100644
index 0000000000000000000000000000000000000000..a2125d34adba5b0b83d518f864c6288d8ad6055b
Binary files /dev/null and b/artwork/DynMod/fig7-2.png differ
diff --git a/artwork/Lab1/.gitkeep b/artwork/Lab1/.gitkeep
new file mode 100644
index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391
diff --git a/artwork/Lab1/fig1-0.png b/artwork/Lab1/fig1-0.png
new file mode 100644
index 0000000000000000000000000000000000000000..97ed2dec0683448a35dd1d496660b568c6d5fdb6
Binary files /dev/null and b/artwork/Lab1/fig1-0.png differ
diff --git a/artwork/stranded/a_new_big_robot.png b/artwork/stranded/a_new_big_robot.png
new file mode 100644
index 0000000000000000000000000000000000000000..b6d285183222f7e3b6698e7c8c6b0a00dc35dbb6
Binary files /dev/null and b/artwork/stranded/a_new_big_robot.png differ
diff --git a/Python3_micro_tutorial.ipynb b/assignments/Assignment00_Python_micro_tutorial.ipynb
similarity index 81%
rename from Python3_micro_tutorial.ipynb
rename to assignments/Assignment00_Python_micro_tutorial.ipynb
index 0ced01cb3a9c06aab688c8c520f116aa37d48e01..bd025bed87aed01272a104cd3c85e6363cddb270 100644
--- a/Python3_micro_tutorial.ipynb
+++ b/assignments/Assignment00_Python_micro_tutorial.ipynb
@@ -1,35 +1,5 @@
{
"cells": [
- {
- "cell_type": "markdown",
- "metadata": {},
- "source": [
- "# Chapter 0: Alone in the Echoing Martian Chorus\n",
- "\n",
- "The year is 2023. An international cooperation between [NASA](https://www.nasa.gov/), [ESA](https://www.esa.int/), [JAXA](https://global.jaxa.jp/), and [CSA](https://www.asc-csa.gc.ca/eng/) has managed to send the first crewed mission to Mars, our neighboring planet. Their aim was to establish a permanent human settlement on the red planet, furthering mankind's quest to explore and inhabit extraterrestrial frontiers.\n",
- "\n",
- "As their spacecraft, the Artemis, touched down on Mars' rugged surface, the team discovered an unforeseen obstacle in the form of a powerful dust storm. Unrelenting and ferocious, the tempest consumed their vessel, leaving only one member of the crew and a handful of robots intact amidst the inhospitable Martian wasteland.\n",
- "\n",
- "<center>\n",
- " <figure>\n",
- " <img src=\"artwork/stranded/stranded.png\" width=60%/>\n",
- " <figcaption><center>Figure 0.1: I wish I had studied robotics at school...</center></figcaption>\n",
- "</figure>\n",
- "</center>\n",
- "\n",
- "Her name was... dr. Elena Vasilescu.\n",
- "\n",
- "Stranded and separated from humanity by millions of kilometers, Dr. Vasilescu fought to maintain her sanity in the face of her desolate surroundings. The relentless solitude stretched before her, every breath an arduous reminder of her isolation. However, her spirit remained unyielding, fueled by the belief that she was the only hope for survival. Luckily, previous preparatory missions had already deployed a fully functional base that could support life for a few weeks.\n",
- "\n",
- "Dr. Vasilescu was also not completely alone. She was accompanied by the best robots humans had ever made, each designed to assist the survival and scientific operations of the mission. The CREW (Cognitive Robotic Exploration Workforce) robots were assigned to collect samples, analyze atmospheric conditions, and scout for potential resources. However, the roboticist of the mission had vanished together with the rest of the crew, and it was now her task to operate those robots. Oh, how she wished she would have paid more attention to those robotics laboratories when she was a student...\n",
- "\n",
- "Now, she has to learn robotics again from scratch.\n",
- "\n",
- "Her life depends on it.\n",
- "\n",
- "But first, she needs to learn a bit of python."
- ]
- },
{
"cell_type": "markdown",
"metadata": {},
@@ -110,7 +80,7 @@
"\n",
"<center>\n",
" <figure>\n",
- " <img src=\"artwork/tutorial/all.JPG\" width=90%/>\n",
+ " <img src=\"../artwork/tutorial/all.JPG\" width=90%/>\n",
" <figcaption><center>Figure 1: Libraries trees</center></figcaption>\n",
"</figure>\n",
"</center>\n",
@@ -344,13 +314,13 @@
"\n",
"You can always ask us questions during the semester, during evaluation this option won't be available.\n",
"\n",
- " $!$ When working with the lab robots, restart the kernel when you want to run cells that send data to it. Make sure the port is the correct one as well."
+ "$!$ When working with the lab robots, restart the kernel when you want to run cells that send data to it. Make sure the port is the correct one as well."
]
}
],
"metadata": {
"kernelspec": {
- "display_name": "Python 3 (ipykernel)",
+ "display_name": "Python3",
"language": "python",
"name": "python3"
},
diff --git a/lab01_CoordinateSystems.ipynb b/lab01_CoordinateSystems.ipynb
deleted file mode 100644
index 3db3f730e45ec2bcb7a9e886f9fceed207d5ca70..0000000000000000000000000000000000000000
--- a/lab01_CoordinateSystems.ipynb
+++ /dev/null
@@ -1,960 +0,0 @@
-{
- "cells": [
- {
- "cell_type": "markdown",
- "metadata": {},
- "source": [
- "# Chapter 1: 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 systems and transformations."
- ]
- },
- {
- "attachments": {},
- "cell_type": "markdown",
- "metadata": {},
- "source": [
- "## Coordinate systems. Geometrical transformations\n",
- "\n",
- "One of the fundamental necessities in robotics, is the localisation of objects in three\n",
- "dimensional space. These objects can be elements that compose the physical structure of\n",
- "a robot, parts and components that the robots manipulate, tools or in general any body\n",
- "that exists in the space of operation of the robot. (See figure 1.1).\n",
- "These objects can be described fundamentally with roughly two attributes: their position and orientation. An immediate aspect of interest would be how to represent these\n",
- "and, furthermore, how can we manipulate mathematically these properties\n",
- "\n",
- "<center>\n",
- " <figure>\n",
- " <img src=\"artwork/transformations/fig1-0.png\" width=30%/>\n",
- " <figcaption><center>Figure 1.1: Coordinate systems in the space of operation of a robot</center></figcaption>\n",
- "</figure>\n",
- "</center>\n",
- "\n",
- "For the description of position and orientation of a solid in space, we will attach on it a rigid Cartesian coordinate system. Since any coordinate system can serve as a reference system on which we can express the position and orientation of a body, there is the question of how to change or transform these attributes of a body from a Cartesian system of reference to another one. This exercise presents conventions and methodology for the description of positions and orientations, as well as the mathematical formalisation that is used for the manipulation of these quantities in several coordinate systems.\n",
- "\n",
- "The manipulation using a robot assumes the fact that objects and tools will be displaced in space with the use of a mechanism. This fact determines the need to represent the position and orientation of such objects that we want to manipulate as well as the position of the mechanism of manipulation. To define and manipulate mathematically the quantities that represent the position and orientation it is necessary to define a coordinate system and the corresponding conventions used for their representation.\n",
- "\n",
- "### 1.1 Cartesian coordinate systems\n",
- "\n",
- "\n",
- "In robotics, the _standard_ Cartesian system is obtained through the application of the\n",
- "rule of the right hand, that is presented in figure 1.2. The thumb of the right hand indicates\n",
- "the positive direction of the Z axis, and the extended fingers indicate the positive direction\n",
- "for the X axis. By flexing the fingers by 90◦\n",
- ", we are obtaining the positive direction for\n",
- "axis Y.\n",
- "For the definition of positive directions of rotations, we use again the rule of the right\n",
- "hand. When the thumb is pointing the positive direction of an axis, the rest of the fingers\n",
- "indicate the positive rotation around that axis. \n",
- "\n",
- "<center>\n",
- " <figure class=\"image\">\n",
- " <img src=\"artwork/transformations/R3-right_hand_rule.png\" width=30% />\n",
- " <figcaption>1.2 Rule of the right hand</figcaption>\n",
- " </figure>\n",
- "</center>\n",
- "\n",
- "\n",
- "Try changing the Z coordinates from the sliders to see how the other axes change. The sliders dictate how the Z axis behaves. The 3 directions are the 3 points in space that describe vector Z."
- ]
- },
- {
- "cell_type": "code",
- "execution_count": 1,
- "metadata": {
- "code_folding": [
- 4
- ]
- },
- "outputs": [
- {
- "ename": "ModuleNotFoundError",
- "evalue": "No module named 'spatialmath'",
- "output_type": "error",
- "traceback": [
- "\u001b[0;31m---------------------------------------------------------------------------\u001b[0m",
- "\u001b[0;31mModuleNotFoundError\u001b[0m Traceback (most recent call last)",
- "Cell \u001b[0;32mIn[1], line 2\u001b[0m\n\u001b[1;32m 1\u001b[0m \u001b[38;5;28;01mfrom\u001b[39;00m \u001b[38;5;21;01mipywidgets\u001b[39;00m \u001b[38;5;28;01mimport\u001b[39;00m interact\n\u001b[0;32m----> 2\u001b[0m \u001b[38;5;28;01mfrom\u001b[39;00m \u001b[38;5;21;01mspatialmath\u001b[39;00m\u001b[38;5;21;01m.\u001b[39;00m\u001b[38;5;21;01mbase\u001b[39;00m \u001b[38;5;28;01mimport\u001b[39;00m \u001b[38;5;241m*\u001b[39m\n\u001b[1;32m 3\u001b[0m \u001b[38;5;28;01mfrom\u001b[39;00m \u001b[38;5;21;01mlab_functions\u001b[39;00m \u001b[38;5;28;01mimport\u001b[39;00m plotAxes, setPlot\n\u001b[1;32m 5\u001b[0m \u001b[38;5;28;01mdef\u001b[39;00m \u001b[38;5;21mshowAxes\u001b[39m(x\u001b[38;5;241m=\u001b[39m\u001b[38;5;241m0\u001b[39m, y\u001b[38;5;241m=\u001b[39m\u001b[38;5;241m0\u001b[39m, z\u001b[38;5;241m=\u001b[39m\u001b[38;5;241m0\u001b[39m):\n",
- "\u001b[0;31mModuleNotFoundError\u001b[0m: No module named 'spatialmath'"
- ]
- }
- ],
- "source": [
- "from ipywidgets import interact\n",
- "from spatialmath.base import *\n",
- "from lab_functions import plotAxes, setPlot\n",
- "\n",
- "def showAxes(x=0, y=0, z=0):\n",
- " m = trotx(x*3.14/180)@troty(y*3.14/180)@trotz(z*3.14/180)\n",
- " ax = setPlot()\n",
- " plotAxes(m, ax, 3)\n",
- "\n",
- "interact(showAxes, x=(-90,90), y=(-90,90), z=(-90,90))"
- ]
- },
- {
- "cell_type": "markdown",
- "metadata": {},
- "source": [
- "The position of a point can be described using the Cartesian coordinates of an object expressed relative to the origin of a coordinate system $(x_1,y_1,z_1)$ or through a vector of position $p_1$. For example, the position of a parallelepiped\n",
- "in a coordinate system can be described using 8 position vectors, one for each one of the vertices. In the case that the object is moving, the calculation of the new position assumes the calculation of a new set of 8 vectors of position. This situation can become even more complicated if we are talking about irregular objects (a particular case, when for each position of the object it might be necessary to calculate more than 8 vector of position), or in the case in which we have more objects that move independently in relation to each other. An alternative and more efficient solution is to attach a different coordinate system on each object, which will displace together with the object itself. If the object is rigid (i.e. it does not deform), the position of every point belonging to the object remains the same in respect to the coordinate system attached on the object, independent of its the displacement. In this way, the problem of calculating the motion of an object is reduced to the calculation of the relation between two coordinate systems (reference system and the coordinate system attached on the object). Moreover, this relationship allows the calculation of the new position of any point belonging to the object of interest. The position of point $P$ is described by its Cartesian coordinates: $P(x,y,z)$."
- ]
- },
- {
- "cell_type": "markdown",
- "metadata": {},
- "source": [
- "### 1.2 Elementary Transformations. Homogeneous Transformations"
- ]
- },
- {
- "cell_type": "markdown",
- "metadata": {},
- "source": [
- "A rigid solid, and its inertial coordinate system, have 6 degrees of freedom (DOF), or 6 independent ways in which the object can move. These elementary transformations are:\n"
- ]
- },
- {
- "cell_type": "markdown",
- "metadata": {},
- "source": [
- "##### 1. Translation on X axis. If the translation takes place with a distance ’d’, this is denoted as:Trans(X,d). \n",
- "Try moving the slider to see how translation on X takes place."
- ]
- },
- {
- "cell_type": "code",
- "execution_count": null,
- "metadata": {
- "code_folding": [
- 4
- ]
- },
- "outputs": [],
- "source": [
- "from ipywidgets import interact\n",
- "from spatialmath.base import *\n",
- "from lab_functions import setPlot, plotAxes\n",
- "\n",
- "def TransX(d):\n",
- " ax = setPlot()\n",
- " m = transl(d,0,0)\n",
- " plotAxes(m, ax, 4)\n",
- "\n",
- "interact(TransX, d=(-0.5,1.0))"
- ]
- },
- {
- "cell_type": "markdown",
- "metadata": {},
- "source": [
- "##### 2. Translation on Y axis. If the translation takes place with a distance ’d’, this is denoted as:Trans(Y,d). \n",
- "Try moving the slider to see how translation on Y takes place."
- ]
- },
- {
- "cell_type": "code",
- "execution_count": null,
- "metadata": {
- "code_folding": [
- 4
- ]
- },
- "outputs": [],
- "source": [
- "from ipywidgets import interact\n",
- "from spatialmath.base import *\n",
- "from lab_functions import setPlot, plotAxes\n",
- "\n",
- "def TransY(d):\n",
- " ax = setPlot()\n",
- " m = transl(0,d,0)\n",
- " plotAxes(m, ax, 4)\n",
- "\n",
- "interact(TransY, d=(-0.5,1.0))"
- ]
- },
- {
- "cell_type": "markdown",
- "metadata": {},
- "source": [
- "##### 3. Translation on Z axis. If the translation takes place with a distance ’d’, this is denoted as:Trans(Z,d). \n",
- "Try moving the slider to see how translation on Z takes place."
- ]
- },
- {
- "cell_type": "code",
- "execution_count": null,
- "metadata": {
- "code_folding": [
- 4
- ]
- },
- "outputs": [],
- "source": [
- "from ipywidgets import interact\n",
- "from spatialmath.base import *\n",
- "from lab_functions import setPlot, plotAxes\n",
- "\n",
- "def TransZ(d):\n",
- " ax = setPlot()\n",
- " m = transl(0,0,d)\n",
- " plotAxes(m, ax, 4)\n",
- "\n",
- "interact(TransZ, d=(-0.5,1.0))"
- ]
- },
- {
- "cell_type": "markdown",
- "metadata": {},
- "source": [
- "##### 4. Rotation on X axis. If the rotation takes place with an angle '$\\vartheta$', this is denoted as:Rot(X,$\\vartheta$). \n",
- "Try moving the slider to see how rotation on X takes place."
- ]
- },
- {
- "cell_type": "code",
- "execution_count": null,
- "metadata": {
- "code_folding": [
- 4
- ]
- },
- "outputs": [],
- "source": [
- "from ipywidgets import interact\n",
- "from spatialmath.base import *\n",
- "from lab_functions import setPlot, plotAxes\n",
- "\n",
- "def RotX(theta):\n",
- " ax = setPlot()\n",
- " m = trotx(theta)\n",
- " plotAxes(m, ax, 4)\n",
- "\n",
- "interact(RotX, theta=(-3.14,3.14))"
- ]
- },
- {
- "cell_type": "markdown",
- "metadata": {},
- "source": [
- "##### 5. Rotation on Y axis. If the rotation takes place with an angle '$\\vartheta$', this is denoted as:Rot(Y,$\\vartheta$). \n",
- "Try moving the slider to see how rotation on Y takes place."
- ]
- },
- {
- "cell_type": "code",
- "execution_count": null,
- "metadata": {
- "code_folding": [
- 4
- ]
- },
- "outputs": [],
- "source": [
- "from ipywidgets import interact\n",
- "from spatialmath.base import *\n",
- "from lab_functions import setPlot, plotAxes\n",
- "\n",
- "def RotY(theta):\n",
- " ax = setPlot()\n",
- " m = troty(theta)\n",
- " plotAxes(m, ax, 4)\n",
- "\n",
- "interact(RotY, theta=(-3.14,3.14))"
- ]
- },
- {
- "cell_type": "markdown",
- "metadata": {},
- "source": [
- "##### 6. Rotation on Z axis. If the rotation takes place with an angle ’$\\vartheta$’, this is denoted as:Rot(Z,$\\vartheta$). \n",
- "Try moving the slider to see how rotation on Z takes place."
- ]
- },
- {
- "cell_type": "code",
- "execution_count": null,
- "metadata": {
- "code_folding": [
- 4
- ]
- },
- "outputs": [],
- "source": [
- "from ipywidgets import interact\n",
- "from spatialmath.base import *\n",
- "from lab_functions import setPlot, plotAxes\n",
- "\n",
- "def RotZ(theta):\n",
- " ax = setPlot()\n",
- " m = trotz(theta)\n",
- " plotAxes(m, ax, 4)\n",
- "\n",
- "interact(RotZ, theta=(-3.14,3.14))"
- ]
- },
- {
- "cell_type": "markdown",
- "metadata": {},
- "source": [
- "The motion of a rigid object in respect to a reference coordinate system can be described as a succession of elementary rotations and translations applied on the coordinate system that is attached on the object. For the description of any translation in the three dimensional space, we need only three parameters. Therefore the matrix representation of a translation can be made using a vector with three elements $w = [w_{x}, w_{y}, w_z ]^T $. This transformation is finite, but it is not linear."
- ]
- },
- {
- "cell_type": "markdown",
- "metadata": {},
- "source": [
- "\n",
- "For the rotation, there are more than one methods of representation. One of the most popular methods is by using rotation matrices. Considering a Cartesian coordinate system on which we apply an arbitrary rotation, we express the orientation of the set of axes (represented by the vectors $u$, $v$, $w$) with respect to the original coordinate system (represented by the vectors $x$, $z$, $y$). This representation describes completely the rotation. The three vectors $u$,$v$,$w$, that form the expression of the new coordinate system, are each represented of the three components, resulting therefore in a total of 9 parameters that represent a rotation."
- ]
- },
- {
- "cell_type": "markdown",
- "metadata": {},
- "source": [
- "\\begin{equation}\\label{eq.1.1}\n",
- "A = \\left[ {\\begin{array}{*{20}c}\n",
- " {\\hat u_x } & {\\hat v_x } & {\\hat w_x } \\\\\n",
- " {\\hat u_y } & {\\hat v_y } & {\\hat w_y } \\\\\n",
- " {\\hat u_z } & {\\hat v_z } & {\\hat w_z } \\\\\n",
- "\\end{array}} \\right]\n",
- "\\end{equation}"
- ]
- },
- {
- "cell_type": "markdown",
- "metadata": {},
- "source": [
- "Each one of the elements of the matrix represents the cosine of the angle between an axis of the new and one of the reference coordinate system ($x$, $z$ or $y$), that is why the matrix above is called as well *direction cosine matrix*. The rotation matrix is an orthogonal matrix, with real elements and a determinant of +1. The eigenvalues of the matrix are $\\left\\{ {1,e^{ \\pm i \\cdot \\theta } } \\right\\}$ where $ e$ is the unit vector corresponding to the direction around which the rotation is made and $\\theta $ is the angle of the rotation.\n"
- ]
- },
- {
- "cell_type": "markdown",
- "metadata": {},
- "source": [
- "In the case that the rotation is happening with an angle $\\varphi$ around one of the elementary axes ($x$, $y$ or $z$), the direction cosine matrix has the following form:\n"
- ]
- },
- {
- "cell_type": "markdown",
- "metadata": {},
- "source": [
- "Rotation around $x$ axis:\n",
- "$A_x = \\left( {\\begin{array}{*{20}c}\n",
- " 1 & 0 & 0 \\\\\n",
- " 0 & {\\cos \\varphi } & { - \\sin \\varphi } \\\\\n",
- " 0 & {\\sin \\varphi } & {\\cos \\varphi } \\\\\n",
- "\\end{array}} \\right)$\n",
- "\n",
- "Rotation around $y$ axis:\n",
- "$A_y = \\left( {\\begin{array}{*{20}c}\n",
- " {\\cos \\varphi } & 0 & {\\sin \\varphi } \\\\\n",
- " 0 & 1 & 0 \\\\\n",
- " { - \\sin \\varphi } & 0 & {\\cos \\varphi } \\\\\n",
- "\\end{array}} \\right)$\n",
- "\n",
- "Rotation around $z$ axis:\n",
- "$A_z = \\left( {\\begin{array}{*{20}c}\n",
- " {\\cos \\varphi } & { - \\sin \\varphi } & 0 \\\\\n",
- " {\\sin \\varphi } & {\\cos \\varphi } & 0 \\\\\n",
- " 0 & 0 & 1 \\\\\n",
- "\\end{array}} \\right)$"
- ]
- },
- {
- "cell_type": "markdown",
- "metadata": {},
- "source": [
- "Therefore, a $3 \\times 3$ matrix can be used for describing a rotation, but not for a translation.\n"
- ]
- },
- {
- "cell_type": "markdown",
- "metadata": {},
- "source": [
- "\n",
- "The *homogeneous coordinates* were introduced by Möbius to allow the description of finite transformations using matrices. Furthermore, homogeneous coordinates allow to work the same way for rotational transformations as well as translational transformations."
- ]
- },
- {
- "cell_type": "markdown",
- "metadata": {},
- "source": [
- "In principle, the coordinates of a point in $n$-dimensional space can be represented through a vector of $n+1$ dimensions, by adding a non-zero scaling factor. The homogeneous coordinates of a point in three dimensional space are obtained through its Cartesian coordinates by adding a scaling factor, which for the robotics applications equals 1. Therefore, a point $P(x, y, z)$ has homogeneous coordinates:"
- ]
- },
- {
- "cell_type": "markdown",
- "metadata": {},
- "source": [
- "\\begin{equation}\n",
- "\\left[ {\\begin{array}{*{20}c}\n",
- " x \\\\\n",
- " y \\\\\n",
- " z \\\\\n",
- " 1 \\\\\n",
- "\\end{array}}\n",
- "\\right]\n",
- "\\end{equation}"
- ]
- },
- {
- "cell_type": "markdown",
- "metadata": {},
- "source": [
- "Geometrical transformations are represented in this case through a $4 \\times 4$ matrix:\n"
- ]
- },
- {
- "cell_type": "markdown",
- "metadata": {},
- "source": [
- "$\n",
- "\\begin{equation}\n",
- "T = \\left[ {\\begin{array}{ccc|c}\n",
- " {} & {} & {} & {} \\\\\n",
- " 3 & \\times & 3 & {3 \\times 1} \\\\\n",
- " {} & {} & {} & {} \\\\ \\hline\n",
- " 1 & \\times & 3 & {1 \\times 1} \\\\\n",
- "\\end{array}} \\right] = \\left[ {\\begin{array}{ccc|c}\n",
- " {} & {} & {} & {trans - } \\\\\n",
- " {} & {rotation} & {} & {la - } \\\\\n",
- " {} & {} & {} & {tion} \\\\ \\hline\n",
- " 0 & 0 & 0 & 1 \\\\\n",
- "\\end{array}} \\right] = \\left[ {\\begin{array}{*{20}c}\n",
- " X & Y & Z & P \\\\\n",
- " 0 & 0 & 0 & 1 \\\\\n",
- "\\end{array}} \\right]\\,\n",
- "\\end{equation}\n",
- "$"
- ]
- },
- {
- "cell_type": "markdown",
- "metadata": {},
- "source": [
- "where $\n",
- "X = \\left[ {\\begin{array}{*{20}c}\n",
- " {X_X } \\\\\n",
- " {X_Y } \\\\\n",
- " {X_Z } \\\\\n",
- "\\end{array}} \\right]\n",
- "$ represents the direction of $X$ axis of the new coordinate system,\n"
- ]
- },
- {
- "cell_type": "markdown",
- "metadata": {},
- "source": [
- "where $\n",
- "Y = \\left[ {\\begin{array}{*{20}c}\n",
- " {Y_X } \\\\\n",
- " {Y_Y } \\\\\n",
- " {Y_Z } \\\\\n",
- "\\end{array}} \\right]\n",
- "$ represents the direction of $Y$ axis of the new coordinate system,\n"
- ]
- },
- {
- "cell_type": "markdown",
- "metadata": {},
- "source": [
- "where $\n",
- "Z = \\left[ {\\begin{array}{*{20}c}\n",
- " {Z_X } \\\\\n",
- " {Z_Y } \\\\\n",
- " {Z_Z } \\\\\n",
- "\\end{array}} \\right]\n",
- "$ represents the direction of $Z$ axis of the new coordinate system,\n"
- ]
- },
- {
- "cell_type": "markdown",
- "metadata": {},
- "source": [
- "$\n",
- "P = \\left[ {\\begin{array}{*{20}c}\n",
- " {P_X } \\\\\n",
- " {P_Y } \\\\\n",
- " {P_Z } \\\\\n",
- "\\end{array}} \\right]\n",
- "$\n",
- " represents the position of the origin of the new coordinate system."
- ]
- },
- {
- "cell_type": "markdown",
- "metadata": {},
- "source": [
- "The matrix $T$ therefore describes the position (through vector $P$) and the orientation (through vectors $X$, $Y$ and $Z$) of the new coordinate system with respect to the reference system. Through the multiplication of two homogeneous transformation matrices, the result is still a homogeneous transformation matrix."
- ]
- },
- {
- "cell_type": "markdown",
- "metadata": {},
- "source": [
- "\n",
- "The following homogeneous transformation matrices are associated to elementary geometrical transformations:\n",
- "\\begin{equation}\n",
- " Trans(X,a) = \\left[ {\\begin{array}{*{20}c}\n",
- " 1 & 0 & 0 & a \\\\\n",
- " 0 & 1 & 0 & 0 \\\\\n",
- " 0 & 0 & 1 & 0 \\\\\n",
- " 0 & 0 & 0 & 1 \\\\\n",
- "\\end{array}} \\right]\n",
- "\\end{equation}\n",
- "\n",
- "\\begin{equation}\n",
- "Trans(Y,b) = \\left[ {\\begin{array}{*{20}c}\n",
- " 1 & 0 & 0 & 0 \\\\\n",
- " 0 & 1 & 0 & b \\\\\n",
- " 0 & 0 & 1 & 0 \\\\\n",
- " 0 & 0 & 0 & 1 \\\\\n",
- "\\end{array}} \\right]\n",
- "\\end{equation}\n",
- "\n",
- "\\begin{equation}\n",
- "Trans(Z,c) = \\left[ {\\begin{array}{*{20}c}\n",
- " 1 & 0 & 0 & 0 \\\\\n",
- " 0 & 1 & 0 & 0 \\\\\n",
- " 0 & 0 & 1 & c \\\\\n",
- " 0 & 0 & 0 & 1 \\\\\n",
- "\\end{array}} \\right]\n",
- "\\end{equation}\n",
- "\n",
- "\n",
- "\\begin{equation}\n",
- "Rot(X,\\vartheta ) = \\left[ {\\begin{array}{*{20}c}\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{array}} \\right]\n",
- "\\end{equation}\n",
- "\n",
- "\\begin{equation}\n",
- "Rot(Y,\\varphi ) = \\left[ {\\begin{array}{*{20}c}\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{array}} \\right]\n",
- "\\end{equation}\n",
- "\n",
- "\n",
- "\n",
- "\\begin{equation}\n",
- "Rot(Z,\\omega ) = \\left[ {\\begin{array}{*{20}c}\n",
- " {\\cos \\omega } & { - \\sin \\omega } & 0 & 0 \\\\\n",
- " {\\sin \\omega } & {\\cos \\omega } & 0 & 0 \\\\\n",
- " 0 & 0 & 1 & 0 \\\\\n",
- " 0 & 0 & 0 & 1 \\\\\n",
- "\\end{array}} \\right]\n",
- "\\end{equation}"
- ]
- },
- {
- "cell_type": "markdown",
- "metadata": {},
- "source": [
- "A matrix of a transformation can be understood in different ways:\n",
- "\n",
- "- as a transformation from one coordinate system to another one\n",
- "- as a description of the origin and orientation of the new coordinate system with respect to the reference coordinate system\n",
- "- as a description of the displacement of an object from a position (reference system) to another one (new coordinate system)\n",
- "- as a method that allows the calculation of the position of a point, that is part of an object, with respect to a system of reference, knowing its position with respect to the new coordinate system:\n",
- "$\n",
- " \\begin{equation}\\label{eq.1.9}\n",
- "^R q = ^R T_N \\cdot ^N q\n",
- " \\end{equation}$\n",
- "<br>\n",
- "where:\n",
- "\n",
- "<br>$^R q$ is the vector of position of a point in the system $R$ (reference system);\n",
- "<br>$^R T_N$ is the transformation of the new coordinate system in respect to the reference system, known as \\emph{direct transformation}. In other words, it is the transformation that is applied on system $R$ so that it arrives at the position of system $N$.\n",
- "<br>$^N q$ is the vector of position of a point in the coordinate system $N$(new coordinate system).\n",
- " \n",
- "\n"
- ]
- },
- {
- "cell_type": "markdown",
- "metadata": {},
- "source": [
- "<center>\n",
- " <figure>\n",
- " <img src=\"artwork/transformations/swap_coordinates.png\" width=50% />\n",
- " <figcaption>Figure 1.3: Calculation of the position of a point in the reference coordinate system, using direct transformation and the coordinates of the point in its own coordinate system</figcaption>\n",
- " </figure>\n",
- "</center>"
- ]
- },
- {
- "cell_type": "markdown",
- "metadata": {},
- "source": [
- "### 1.3 Combination of transformations\n"
- ]
- },
- {
- "cell_type": "markdown",
- "metadata": {},
- "source": [
- "As it was shown, a geometrical transformation can be decomposed in a succession of elementary rotations and translations. The combination of a succession of elementary geometrical transformations in a general transformation, can be therefore done:"
- ]
- },
- {
- "cell_type": "markdown",
- "metadata": {},
- "source": [
- "- Using <b>LEFT MULTIPLICATION</b> of homogeneous transformation matrices if the transformations are made in respect to a reference coordinate system.\n",
- " (<b>ABSOLUTE TRANSFORMATIONS</b>). In these conditions, if we have a succession of $n$ absolute transformations$(T_i, \\;\\;i=\\overline{1\\ldots n})$, the transformation matrix can be written as:\n",
- " \n",
- "\\begin{equation}\n",
- "T_A=\\prod \\limits_{i=n}^1 T_i=T_n\\cdot T_{n-1}\\cdot\\ldots\\cdot T_1\n",
- "\\end{equation}\n",
- "\n",
- "\n",
- "You can see a visualisation of an 'absolute' transformation in the following snippet. Run it and notice how the 'small' coordinate frame is transformed around the red axis of the 'big' coordinate frame, both in the translation and rotation tab.\n",
- "\n"
- ]
- },
- {
- "cell_type": "code",
- "execution_count": null,
- "metadata": {
- "code_folding": [
- 5,
- 11
- ]
- },
- "outputs": [],
- "source": [
- "from ipywidgets import interact, widgets\n",
- "from lab_functions import setPlot, plotAxes\n",
- "from spatialmath.base import *\n",
- "import numpy as np\n",
- "\n",
- "def TranslateOnXLeft(a):\n",
- " ax = setPlot()\n",
- " matrix = np.matmul(transl(0.5,0.5,1),trotz(0.407))\n",
- " tf = np.matmul(transl(a,0,0),matrix);\n",
- " plotAxes(tf, ax, lw=6, ln=1)\n",
- " \n",
- "def RotateOnXLeft(a):\n",
- " ax = setPlot()\n",
- " matrix = np.matmul(transl(0.5,0.5,1),trotz(0.407))\n",
- " tf = np.matmul(trotx(a),matrix);\n",
- " plotAxes(tf, ax, lw=6, ln=1)\n",
- "\n",
- "out1 = widgets.interactive(RotateOnXLeft, a=(-3.14,3.14))\n",
- "out2 = widgets.interactive(TranslateOnXLeft, a=(-1.0,1.0))\n",
- "\n",
- "tab = widgets.Tab(children = [out1, out2])\n",
- "tab.set_title(0, 'rotation')\n",
- "tab.set_title(1, 'translation')\n",
- "\n",
- "display(tab)"
- ]
- },
- {
- "cell_type": "markdown",
- "metadata": {},
- "source": [
- "- Using <b>RIGHT MULTIPLICATION</b> of homogeneous transformation matrices if transformations are made in respect to a new coordinate system (the system that is obtained as a result of the last transformation). (<b>RELATIVE TRANSFORMATIONS</b>). In these conditions, if we have a succession of $n$ relative transformations $(T_i, \\;\\;i=\\overline{1\\ldots n})$, the transformation matrix can be written as:\n",
- "\n",
- "\\begin{equation}\n",
- "T_r=\\prod \\limits_{i=1}^n T_i=T_1\\cdot T_2\\cdot\\ldots\\cdot T_n\n",
- "\\end{equation}\n",
- "\n",
- "You can see a visualisation of an 'relative' transformation in the following snippets. Run it and notice how the 'small' coordinate frame is transformed around the red axis of its own coordinate frame, both on the translation and the rotation tab:"
- ]
- },
- {
- "cell_type": "code",
- "execution_count": null,
- "metadata": {
- "code_folding": [
- 5,
- 11
- ]
- },
- "outputs": [],
- "source": [
- "from ipywidgets import interact\n",
- "from lab_functions import setPlot, plotAxes\n",
- "from spatialmath.base import *\n",
- "import numpy as np\n",
- "\n",
- "def TranslateOnXRight(a):\n",
- " ax = setPlot()\n",
- " matrix = np.matmul(transl(0.5,0.5,1),trotz(0.407))\n",
- " tf = np.matmul(matrix,transl(a,0,0));\n",
- " plotAxes(tf, ax, lw=6, ln=1)\n",
- " \n",
- "def RotateOnXRight(a):\n",
- " ax = setPlot()\n",
- " matrix = np.matmul(transl(0.5,0.5,1),trotz(0.407))\n",
- " tf = np.matmul(matrix,trotx(a));\n",
- " plotAxes(tf, ax, lw=6, ln=1)\n",
- "\n",
- "out1 = widgets.interactive(RotateOnXRight, a=(-3.14,3.14))\n",
- "out2 = widgets.interactive(TranslateOnXRight, a=(-1.0,1.0))\n",
- "\n",
- "tab = widgets.Tab(children = [out1, out2])\n",
- "tab.set_title(0, 'rotation')\n",
- "tab.set_title(1, 'translation')\n",
- "\n",
- "display(tab)"
- ]
- },
- {
- "cell_type": "markdown",
- "metadata": {},
- "source": [
- "### Swapping coordinate frames"
- ]
- },
- {
- "cell_type": "markdown",
- "metadata": {},
- "source": [
- "With the help of direct geometrical transformations it is possible to determine the position of a point in the reference system ${ }^Rq$ if we know its position in another coordinate system ${}^Nq$ with direct geometrical transformation $^R T_N$. If we want to find ${}^Nq$, with given ${}^Rq$ and $^R T_N$, we must proceed as following:"
- ]
- },
- {
- "cell_type": "markdown",
- "metadata": {},
- "source": [
- "\n",
- "\\begin{equation}\n",
- "{ }^Rq = T \\cdot { }^Nq \\to T^{ - 1} \\cdot { }^Rq = T^{ - 1} \\cdot T \\cdot {\n",
- "}^Nq \\to { }^Nq = T^{ - 1} \\cdot { }^Rq\n",
- "\\end{equation}\n",
- "\n",
- "\n",
- "<br>\n",
- "Therefore, the inverse geometric transformation is described with the inverse matrix of the direct transformation.\n",
- "<br>\n",
- "\n",
- "Denoted\n",
- "\n",
- "\\begin{equation}\\label{eq.1.11}\n",
- "T = \\left[ \\begin{array}{cccc}\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{array} \\right]\n",
- "\\end{equation}\n",
- "\n",
- "\n",
- "<br>\n",
- "it can be demonstrated that the inverse of a homogeneous transformation matrix, is:\n",
- "<br>\n",
- "\n",
- "\\begin{equation}\n",
- "T^{ - 1} = \\left[\n",
- "\\begin{array}{cccc}\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{array} \\right]\n",
- "\\end{equation}\n"
- ]
- },
- {
- "cell_type": "markdown",
- "metadata": {},
- "source": [
- "### 1.4 Python commands"
- ]
- },
- {
- "cell_type": "markdown",
- "metadata": {},
- "source": [
- "There are several toolboxes available online for helping us with various robotics operations. Ones we will use extensively during the laboratories is [__spatialmath__](https://petercorke.github.io/spatialmath-python/intro.html) 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)\n"
- ]
- },
- {
- "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",
- "P = np.array([[1,2,3,1]]).T\n",
- "\n",
- "# Calculating coordinates of point P in frame '3'\n",
- "t13@P"
- ]
- },
- {
- "cell_type": "markdown",
- "metadata": {},
- "source": [
- "# Assignments\n",
- "\n",
- "## 1.Transforming a point\n",
- "\n",
- "A point with coordinates $P(1, 2, 3)$ is given. We apply the following transformations on the point, with the specified order: $Trans(X,4),\\;\\; Trans(Y,-4),\\;\\; Rot(X, 90^\\circ), \\;\\;Trans(Z,4),\\;\\; Rot(Y, 90^\\circ)$. Determine the new coordinates of the point if we consider Absolute transformations.\n",
- "\n",
- "\n",
- "## 2.Transforming a coordinate frame\n",
- "\n",
- "In robotics, coordinate frames can be attached to different parts of a robot.\n",
- "Consider the model of the AL5D robot, shown below. \n",
- "- Identify the homogeneous transformation matrix that can express the frame attached to the gripper of the robot in the base frame. The origin of the gripper frame with respect to the base frame lies at $(0, 3, 0.5)$\n",
- "- Connect the robot to your computer and run the code below. Try to move your robot to the pose shown in the figure using the sliders. What are the values of each joint $(q_0, q_1, q_2, q_3, q_4)$ in this the pose?\n",
- "- A camera attached to the gripper of the robot would see the purple ball at the position $(-1, -0.5, 2)$ expressed in the gripper's frame. Find the coordinates of the ball expressed in the base frame.\n",
- "\n",
- "<center>\n",
- " <figure class=\"image\">\n",
- " <img src=\"artwork/transformations/transformations_al5d_withball.png\" width=50% />\n",
- " <figcaption>Model of the AL5D robot with one coordinate frame attached to the base of the robot and another one to the gripper.</figcaption>\n",
- " </figure>\n",
- "</center>\n"
- ]
- },
- {
- "cell_type": "code",
- "execution_count": null,
- "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)) "
- ]
- },
- {
- "cell_type": "markdown",
- "metadata": {},
- "source": [
- "## Help dr. Vasilescu\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 = 100 m$, $Y = 450 m$, and $Z = -60 m$. These coordinates are relative to the camera. After checking the operations manual from the mission, dr. Vasilescu has determined that the camera system is located $X = 54m$, $ Y = 125 m$, and $Z=60m$ away from her, and is rotated around the Z axis by 90 degrees, relative to her.\n",
- "\n",
- "<img src=\"artwork/stranded/mars_base_isometric.png\" width=60%/>\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",
- "Can you help her find the location of the package relative to her base?"
- ]
- }
- ],
- "metadata": {
- "kernelspec": {
- "display_name": "Python 3 (ipykernel)",
- "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"
- },
- "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
-}
diff --git a/lab02_DirectGeometricModel.ipynb b/lab02_DirectGeometricModel.ipynb
deleted file mode 100644
index 3f722fc2990b961c6a5a581b649808dd6a038460..0000000000000000000000000000000000000000
--- a/lab02_DirectGeometricModel.ipynb
+++ /dev/null
@@ -1,514 +0,0 @@
-{
- "cells": [
- {
- "cell_type": "markdown",
- "metadata": {},
- "source": [
- "# Chapter 2: Taking initiative\n",
- "\n",
- "Having solved her basic needs for food, dr. Elena Vasilescu started worrying about other primitive survival needs of her body: heat! The base was relying on solar panels for generating heat. However, she would be freezing to death whenever the first sandstorm would hit the region. Sandstorms are pretty often on that corner of the solar system. The initial plan was for the team to set up additional support systems that would store excessive energy in batteries, but those modules have been lost during the landing failure.\n",
- "\n",
- "She had to get her hands on a reliable heat source to keep her self warm during the cold days. Luckily, the engineers back at home had prepared the mission with such a source, albeit to be used only in extreme emergency situations.\n",
- "\n",
- "<img src=\"artwork/stranded/elena_teleoperating_robot.png\" width=60%/>\n",
- "\n",
- "Dr. Vasilescu took the initiative to consider herself in such a situation."
- ]
- },
- {
- "cell_type": "markdown",
- "metadata": {},
- "source": [
- "# Laboratory 2\n",
- "\n",
- "## Direct Geometric Model\n",
- "\n",
- "This laboratory presents an algorithm for the determination of the direct geometric model for an 'open chain' manipulation structure. We will:\n",
- "\n",
- "- Define the homogeneous transformation matrices for the joints of a manipulation structure with \"n\" degrees of freedom.\n",
- "- Define and calculate the position vector $Z_{p}=[x\\;\\;y\\;\\;z]^{T}$ for the specified manipulation structure."
- ]
- },
- {
- "cell_type": "markdown",
- "metadata": {
- "tags": []
- },
- "source": [
- "### Anatomy of a robot\n",
- "The arm of a robot is composed of a succession of rigid segments, called *links*. The links are connected with each other through articulations, called *joints*. The links and joints are forming a kinematic chain. The kinematic chain can be open or closed. Every link in a closed kinematic chain is connected at least with two joints. In the case of an open kinematic chain, the first link (base) and the last link (end effector), have only a single joint. There are two types of joints available when constructing a robot: joints that allow a rotation (R) and joints that allow a translation (T). If a joint is actuated with the help of a motor, it is called motor joint.\n",
- "\n",
- "<figure>\n",
- " <img src=\"artwork/DGM/fig2-1.png\" width=50%/>\n",
- " <figcaption><center>Figure 2.1: The KR 60 P2 robot</center></figcaption>\n",
- "</figure>\n",
- "\n",
- "In figure 2.1 the KR 60 P2 robot of KUKA Roboter GmbH is presented. All the joints of this robot are rotational joints (robot RRR/RRR), denoted $A1 \\;- \\;A6$.\n",
- "\n",
- "The purpose of the direct geometric model is to provide a connection between the movement of a joint (described with a variable for each one of the joints) and the movement of the end effector. With other words, we intend to establish a link between the generalised coordinates of each joint and the Cartesian coordinates of the end effector."
- ]
- },
- {
- "cell_type": "markdown",
- "metadata": {},
- "source": [
- "### Direct geometric model of an open chain manipulation structure\n",
- "\n",
- "For a sequence of bodies $i-j-k$, within a kinematic chain of a industrial robot, the iterative calculation of its position uses the relationship:\n",
- "\n",
- "\\begin{equation}\n",
- " r_{ik} ^{(i)} = r_{ij} ^{(i)} + R_{ij} \\cdot r_{jk}^{(j)}\n",
- "\\end{equation}"
- ]
- },
- {
- "cell_type": "markdown",
- "metadata": {},
- "source": [
- "$$\n",
- " R_{ik}=R_{ij}\\cdot R_{jk}\n",
- "$$\n",
- "\n",
- "where $i$ is the index for a reference link towards which the position is referenced. Vector $r_{ij}^{(i)}$ defines the position of body $(j)$ in respect to body $(i)$, while $R_{ij}$ defines the orientation of body $(j)$ in respect to the body $(i)$ (Direction cosine matrix, of dimension $3 \\times 3$).\n",
- "\n",
- "Considering a sequence that includes the base body (denoted \"1\"):\n",
- "\n",
- "$1-j-k$, (see figure $2.2$) we can write:\n",
- "\n",
- "\n",
- "$$\n",
- "r_{1k} ^{(1)} = r_{1j} ^{(1)} + R_{1j} \\cdot r_{jk}\n",
- "^{(j)}\n",
- "$$\n",
- "$$\n",
- " R_{1k}=R_{1j}\\cdot R_{jk}\n",
- "$$\n",
- "\n",
- "<figure>\n",
- " <img src=\"artwork/DGM/fig2-2.png\" width=50%/>\n",
- " <figcaption><center>Figure 2.2: Sequence of Cartesian systems and the relationship between them</center></figcaption>\n",
- "</figure>\n",
- "\n",
- "We can write the relationships \\ref{eq2.1} and \\ref{eq2.2} as:\n",
- "\n",
- "\n",
- "\\begin{equation}\n",
- "\\label{eq2.5}\n",
- "\\left[ {\\begin{array}{c}\n",
- " \\mathop r\\nolimits_{ik}^{(i)} \\\\\n",
- " \\\\\n",
- " 1 \\\\\n",
- " \\end{array}} \\right] = \\left[ {{\\begin{array}{*{20}c}\n",
- " {\\mathop R\\nolimits_{ij} } & {\\mathop r\\nolimits_{ij}^{(i)} } \\\\\n",
- " \\\\\n",
- " 0 & 1 \\\\\n",
- "\\end{array} }} \\right]\\cdot\\left[ {\\begin{array}{c}\n",
- " \\mathop r\\nolimits_{jk}^{(j)} \\\\\n",
- " \\\\\n",
- " 1 \\\\\n",
- " \\end{array}} \\right]\n",
- "\\end{equation}\n",
- "\n",
- "\n",
- "Matrix\n",
- "\n",
- "\\begin{equation}\n",
- "\\label{eq2.6}\n",
- "\\mathop H\\nolimits_{ij} = \\left[ {{\\begin{array}{*{20}c}\n",
- " {\\mathop R\\nolimits_{ij} } & {\\mathop r\\nolimits_{ij}^{(i)} } \\\\\n",
- " \\\\\n",
- " 0 & 1 \\\\\n",
- "\\end{array} }} \\right]\n",
- "\\end{equation}\n",
- "\n",
- "is a homogeneous transformation matrix, with ${\\mathop R\\nolimits_{ij} }$ orientation matrix (Direction Cosine Matrix, with dimension $3 \\times 3$) and ${\\mathop r\\nolimits_{ij}^{(i)} }$ position vector (with dimension $3\\times 1$).\n",
- "\n",
- "The equation \\ref{eq2.3} can be written in the following form:\n",
- "\n",
- "\n",
- "\\begin{equation}\\label{eq2.7}\n",
- "\\left[ {{\\begin{array}{*{20}c}\n",
- " {r_{1k}^{(1)} } \\hfill \\\\\n",
- " \\\\\n",
- " 1 \\hfill \\\\\n",
- "\\end{array} }} \\right] = \\left[ {{\\begin{array}{*{20}c}\n",
- " {R_{1i} } \\hfill & {r_{1i}^{(1)} } \\hfill \\\\\n",
- " \\\\\n",
- " 0 \\hfill & 1 \\hfill \\\\\n",
- "\\end{array} }} \\right]\\cdot\\left[ {{\\begin{array}{*{20}c}\n",
- " {r_{ik}^{(i)} } \\hfill \\\\\n",
- " \\\\\n",
- " 1 \\hfill \\\\\n",
- "\\end{array} }} \\right]\n",
- "\\end{equation}\n",
- "\n",
- "The equation \\ref{eq2.7} together with relationship \\ref{eq2.5} leads to: \n",
- "\\begin{equation}\n",
- "\\left[ {\\begin{array}{c}\n",
- " \\mathop r\\nolimits_{1k}^{(1)} \\\\\n",
- " \\\\\n",
- " 1 \\\\\n",
- " \\end{array}} \\right] = \\left[ {{\\begin{array}{*{20}c}\n",
- " {\\mathop R\\nolimits_{1i} } & {\\mathop r\\nolimits_{1i}^{(1)} } \\\\\n",
- " \\\\\n",
- " 0 & 1 \\\\\n",
- "\\end{array} }} \\right]\\cdot\\left[ {{\\begin{array}{*{20}c}\n",
- " {\\mathop R\\nolimits_{ij} } & {\\mathop r\\nolimits_{ij}^{(i)} } \\\\\n",
- " \\\\\n",
- " 0 & 1 \\\\\n",
- "\\end{array} }} \\right]\\cdot\\left[ {\\begin{array}{c}\n",
- " \\mathop r\\nolimits_{jk}^{(i)} \\\\\n",
- " \\\\\n",
- " 1 \\\\\n",
- " \\end{array}} \\right]\n",
- "\\end{equation}\n",
- "\n",
- "Keeping the notation from relationship \\ref{eq2.6} we obtain:\n",
- "\n",
- "\\begin{equation}\n",
- "\\left[ {\\begin{array}{c}\n",
- " \\mathop r\\nolimits_{1k}^{(1)} \\\\\n",
- " \\\\\n",
- " 1 \\\\\n",
- " \\end{array}} \\right] = \\mathop H\\nolimits_{1i} \\cdot \\mathop\n",
- "H\\nolimits_{ij} \\cdot \\left[ {\\begin{array}{c}\n",
- " \\mathop r\\nolimits_{jk}^{(j)} \\\\\n",
- " \\\\\n",
- " 1 \\\\\n",
- " \\end{array}} \\right] = \\mathop H\\nolimits_{1j} \\cdot \\left[\n",
- "{\\begin{array}{c}\n",
- " \\mathop r\\nolimits_{jk}^{(j)} \\\\\n",
- " \\\\\n",
- " 1 \\\\\n",
- " \\end{array}} \\right]\n",
- " \\end{equation}\n",
- "\n",
- "where\n",
- "\n",
- "\\begin{equation}\n",
- "H_{1j} = H_{1i} \\cdot H_{ij}\n",
- "\\end{equation}\n",
- "\n",
- "For a structure with \"n\" bodies, the position and orientation of body \"n\" (end-effector), in respect to the base body 1 (inertial reference), are given by:\n",
- "\n",
- "\\begin{equation}\\label{eq2.11}\n",
- "H_{1n} = H_{12} \\cdot H_{23} \\cdot .... \\cdot H_{n - 1,n}\n",
- "\\end{equation}\n",
- "\n",
- "The homogeneous transformation matrices $H_{i - 1,i}$ are referring to the kinematic joints of the structure (links $i-1,i$) which are rotational or translational. \n",
- "The definition the matrices for these situation, is found in relationships $1.3 - 1.8$.\n",
- "\n",
- "### 2.3 Algorithm for DGM\n",
- "\n",
- "The algorithm for the determination of the direct geometric model, with the example of the RTT robot in figure $2.3$ could be the following: \n",
- "\n",
- "For efficiency reasons, we will use the following notation \n",
- "<br>\n",
- "\n",
- "\\begin{equation}\n",
- "\\left\\{ {{\\begin{array}{rcl}\n",
- " \\cos(q_i)\\stackrel{\\mbox{not}}{=} & c_i & \\\\\n",
- " \\\\\n",
- " \\sin(q_i)\\stackrel{\\mbox{not}}{=} & s_i & \\\\\n",
- " \\end{array} }} \\right. for\\;any\\; generalised\\;coordinate\\; q_i,\\; i=\\overline{1..n}\n",
- "\\end{equation}\n",
- "\n",
- "\n",
- "<figure>\n",
- " <img src=\"artwork/DGM/fig2-3.png\" width=50%/>\n",
- " <figcaption><center>Figure 2.3: Robot RRT</center></figcaption>\n",
- "</figure>\n",
- "\n",
- "#### 1.We attach a variable on each motor joint (generalised coordinate) $q_1...q_n$.\n",
- "\n",
- "<figure>\n",
- " <img align=\"left\" src=\"artwork/DGM/step1.png\" width=50%/>\n",
- "</figure>"
- ]
- },
- {
- "cell_type": "markdown",
- "metadata": {},
- "source": [
- "#### 2. We attach a coordinate system on each link, starting with the base (1) until the end effector (n).\n",
- "\n",
- "<figure>\n",
- " <img align=\"left\" src=\"artwork/DGM/step2.png\" width=50%/>\n",
- "</figure>"
- ]
- },
- {
- "cell_type": "markdown",
- "metadata": {},
- "source": [
- "#### 3. We calculate iteratively, the position and orientation of each body in respect to the base body, using relationships \\ref{eq2.3} and \\ref{eq2.4}. Alternatively we can express this with homogeneous matrices, from relationship \\ref{eq2.11}\n",
- "\n",
- "<video controls src=\"artwork/DGM/1to2.mp4\" width = 50%>animation</video>\n",
- "\n",
- "\n",
- " \\begin{equation}\n",
- "\\label{mgd-eq4}\n",
- " T_{12} = \\left[ {{\\begin{array}{*{20}c}\n",
- " {\\cos (q_1 )} & { - \\sin (q_1 )} & 0 & 0 \\\\\n",
- " {\\sin (q_1 )} & {\\cos (q_1 )} & 0 & 0 \\\\\n",
- " 0 & 0 & 1 & 0 \\\\\n",
- " 0 & 0 & 0 & 1 \\\\\n",
- "\\end{array} }} \\right]\n",
- "\\end{equation}\n",
- "\n",
- "<video controls src=\"artwork/DGM/2to3.mp4\" width = 50%>animation</video>\n",
- "\n",
- "\n",
- "\\begin{equation}\n",
- "T_{23} = \\left[ {{\\begin{array}{*{20}c}\n",
- " 1 & 0 & 0 & 0 \\\\\n",
- " 0 & 1 & 0 & 0 \\\\\n",
- " 0 & 0 & 1 & {q_2 } \\\\\n",
- " 0 & 0 & 0 & 1 \\\\\n",
- "\\end{array} }}\n",
- "\\right]\n",
- "\\end{equation}\n",
- "\n",
- "<video controls src=\"artwork/DGM/3to4.mp4\" width = 50%>animation</video>\n",
- "\n",
- "\n",
- "\\begin{equation}\n",
- "T_{34} = \\left[ {{\\begin{array}{*{20}c}\n",
- " 1 & 0 & 0 & 0 \\\\\n",
- " 0 & 1 & 0 & {q_3 + L_1} \\\\\n",
- " 0 & 0 & 1 & 0 \\\\\n",
- " 0 & 0 & 0 & 1 \\\\\n",
- "\\end{array} }} \\right]\n",
- "\\end{equation}\n",
- "\n",
- "\n",
- "We determine the direct geometric model using post-multiplication of the elementary matrices that characterise the structure, starting from the base body until the end-effector:\n",
- "\\begin{equation}\n",
- "T=T_{14}=T_{12}\\cdot T_{23}\\cdot T_{34}\n",
- "\\end{equation}\n",
- "\n",
- "\\begin{equation}\n",
- "T_{14}=\\left[ {{\\begin{array}{*{20}c}\n",
- " c_1 & -s_1 & 0 & 0 \\\\\n",
- " s_1 & c_1 & 0 & 0 \\\\\n",
- " 0 & 0 & 1 & 0 \\\\\n",
- " 0 & 0 & 0 & 1 \\\\\n",
- "\\end{array} }} \\right]\\cdot\n",
- "\\left[ {{\\begin{array}{*{20}c}\n",
- " 1 & 0 & 0 & 0 \\\\\n",
- " 0 & 1 & 0 & 0 \\\\\n",
- " 0 & 0 & 1 & {q_2 } \\\\\n",
- " 0 & 0 & 0 & 1 \\\\\n",
- "\\end{array} }}\n",
- "\\right]\\cdot\n",
- "\\left[ {{\\begin{array}{*{20}c}\n",
- " 1 & 0 & 0 & 0 \\\\\n",
- " 0 & 1 & 0 & {q_3 + L_1} \\\\\n",
- " 0 & 0 & 1 & 0 \\\\\n",
- " 0 & 0 & 0 & 1 \\\\\n",
- "\\end{array} }} \\right]\n",
- "\\end{equation}\n",
- "\n",
- "resulting in direct geometric model in the following form:\n",
- "\\begin{equation}\n",
- "T_{14}=\\left[ {{\\begin{array}{*{20}c}\n",
- " c_1 & -s_1 & 0 & -(q_3+L_1)\\cdot s_1 \\\\\n",
- " s_1 & c_1 & 0 & (q_3+L_1)\\cdot c_1 \\\\\n",
- " 0 & 0 & 1 & q_2 \\\\\n",
- " 0 & 0 & 0 & 1 \\\\\n",
- "\\end{array} }} \\right]\n",
- "\\end{equation}\n",
- "\n",
- "The position parameters of the end-effector are given in the final column of matrix $T_{14}$:\n",
- "\n",
- "\\begin{equation}\n",
- "Z_P=\\left[ {{\\begin{array}{rcc}\n",
- " x_{ef}& =& -(q_3+L_1)\\cdot {\\sin(q_1)} \\\\\n",
- " y_{ef} &= &(q_3+L_1)\\cdot {\\cos(q_1)}\\\\\n",
- " z_{ef} &=& q_2 \\\\\n",
- " \\end{array} }} \\right]\n",
- "\\end{equation}"
- ]
- },
- {
- "cell_type": "markdown",
- "metadata": {},
- "source": [
- "### 2.4 Python examples for symbolic manipulation\n",
- "\n",
- "Using the robotics toolbox, you can calculate transformation matrices with symbolic expressions, e.g. for the joint coordinate representing a rotation. You can also combine several symbolic transformations to calculate the overal symbolic transformation (e.g. for the direct geometric model)."
- ]
- },
- {
- "cell_type": "code",
- "execution_count": 8,
- "metadata": {
- "tags": []
- },
- "outputs": [
- {
- "name": "stdout",
- "output_type": "stream",
- "text": [
- "[[((-sin(q1)*sin(q3) + cos(q1)*cos(q2)*cos(q3))*cos(q4) - sin(q2)*sin(q4)*cos(q1))*cos(q5) + (-sin(q1)*cos(q3) - sin(q3)*cos(q1)*cos(q2))*sin(q5)\n",
- " -((-sin(q1)*sin(q3) + cos(q1)*cos(q2)*cos(q3))*cos(q4) - sin(q2)*sin(q4)*cos(q1))*sin(q5) + (-sin(q1)*cos(q3) - sin(q3)*cos(q1)*cos(q2))*cos(q5)\n",
- " -(-sin(q1)*sin(q3) + cos(q1)*cos(q2)*cos(q3))*sin(q4) - sin(q2)*cos(q1)*cos(q4)\n",
- " l1*(sin(q1)*cos(q3) + sin(q3)*cos(q1)*cos(q2)) + l2*(((-sin(q1)*sin(q3) + cos(q1)*cos(q2)*cos(q3))*cos(q4) - sin(q2)*sin(q4)*cos(q1))*cos(q5) + (-sin(q1)*cos(q3) - sin(q3)*cos(q1)*cos(q2))*sin(q5))]\n",
- " [(sin(q2)*cos(q3)*cos(q4) + sin(q4)*cos(q2))*cos(q5) - sin(q2)*sin(q3)*sin(q5)\n",
- " -(sin(q2)*cos(q3)*cos(q4) + sin(q4)*cos(q2))*sin(q5) - sin(q2)*sin(q3)*cos(q5)\n",
- " -sin(q2)*sin(q4)*cos(q3) + cos(q2)*cos(q4)\n",
- " l1*sin(q2)*sin(q3) + l2*((sin(q2)*cos(q3)*cos(q4) + sin(q4)*cos(q2))*cos(q5) - sin(q2)*sin(q3)*sin(q5))]\n",
- " [((-sin(q1)*cos(q2)*cos(q3) - sin(q3)*cos(q1))*cos(q4) + sin(q1)*sin(q2)*sin(q4))*cos(q5) + (sin(q1)*sin(q3)*cos(q2) - cos(q1)*cos(q3))*sin(q5)\n",
- " -((-sin(q1)*cos(q2)*cos(q3) - sin(q3)*cos(q1))*cos(q4) + sin(q1)*sin(q2)*sin(q4))*sin(q5) + (sin(q1)*sin(q3)*cos(q2) - cos(q1)*cos(q3))*cos(q5)\n",
- " -(-sin(q1)*cos(q2)*cos(q3) - sin(q3)*cos(q1))*sin(q4) + sin(q1)*sin(q2)*cos(q4)\n",
- " l1*(-sin(q1)*sin(q3)*cos(q2) + cos(q1)*cos(q3)) + l2*(((-sin(q1)*cos(q2)*cos(q3) - sin(q3)*cos(q1))*cos(q4) + sin(q1)*sin(q2)*sin(q4))*cos(q5) + (sin(q1)*sin(q3)*cos(q2) - cos(q1)*cos(q3))*sin(q5))]\n",
- " [0 0 0 1]]\n"
- ]
- }
- ],
- "source": [
- "from spatialmath.base import *\n",
- "from spatialmath import *\n",
- "import spatialmath.base.symbolic as sym\n",
- "from sympy import *\n",
- "\n",
- "# Defining the joint coordinates symbols\n",
- "q0, q1, q2, q3, q4, q5, l1, l2 = sym.symbol('q0, q1, q2, q3, q4, q5, l1, l2')\n",
- "\n",
- "# Calculating the symbolic direct geometric model\n",
- "dgm = trotx(-pi/2)@trotz(q1)@trotx(pi/2)@trotz(q2)@trotx(-pi/2)@trotz(q3)@trotx(pi/2)@trotz(q4)@transl(0,0,l1)@trotx(-pi/2)@trotz(q5)@transl(l2,0,0)\n",
- "print(dgm)\n",
- "\n",
- "# Calculating the direct geometric model for specific joint coordinates\n",
- "#Matrix(dgm).subs({q1:pi, q2:pi/2, q3:2})"
- ]
- },
- {
- "cell_type": "markdown",
- "metadata": {},
- "source": [
- "## Help dr. Vasilescu\n",
- "\n",
- "In the compartment adjacent to dr. Vasilescu is a storage module, which contains a [RTG (Radioisotope Thermoelectric Generator)](https://en.wikipedia.org/wiki/Radioisotope_thermoelectric_generator) in a box, and a three robot arms that can handle the boxes.\n",
- "\n",
- "<img src=\"artwork/stranded/robot_crane.png\" width=30%/>\n",
- "\n",
- "She has access to a control panel for the robots, which only reports on the values of their joint coordinates. However, she does not have visual information on which of the robots is currently in the position to pick the box. She needs to be very careful which robot to activate as not to damage the RTG unit or any of the other boxes. In the first case, it would mean a slow and painful radioactive death. In the second case, it might mean a really fast and equally painful death due to explosion.\n",
- "\n",
- "Luckily, she happens to have a model of the robotic cranes on her desk, and can therefore calculate the forward kinematics model. The robot model is identical to the AL5D robot you have on your own desk. \n",
- "\n",
- "<figure>\n",
- " <img src=\"artwork/DGM/al5d.png\" width=30%/>\n",
- " <figcaption><center>Figure 2.4: AL5D robot</center></figcaption>\n",
- "</figure>\n",
- "\n",
- "She has calculated that the equivalent position of the RTG for the model would be at $X = 24 cm$, $Y = 18 cm$, and $ Z = 2 cm$.\n",
- "\n",
- "The three robots are reporting the following set of joint coordinates:\n",
- "\n",
- "|Robot|Robot 1| Robot 2| Robot 3|\n",
- "|---|---|---|---|\n",
- "|$q_0$|10|20|30|\n",
- "|$q_1$|10|20|30|\n",
- "|$q_2$|10|20|30|\n",
- "|$q_3$|10|20|30|\n",
- "|$q_4$|10|20|30|\n",
- "\n",
- "Which of these robots is the one she should activate for manipulating the RTG unit? Calculate the forward kinematics of the AL5D and use the coordinates for each robot to find the correct answer.\n",
- "\n",
- "Verify the result by moving the AL5D of your desk to the target coordinates."
- ]
- },
- {
- "cell_type": "markdown",
- "metadata": {
- "jupyter": {
- "source_hidden": true
- }
- },
- "source": [
- "# Expand for help\n",
- "\n",
- "Consider the AL5D robot from figure 2.4.\n",
- "\n",
- "- Measure the lengths A, B, C, D and E on your robot.\n",
- "- Draw the schematic structure of the robot.\n",
- "- Determine the direct geometric model using homogeneous transformation matrices.\n",
- "- Calculate the position obtained by imposing the following position of joints:"
- ]
- },
- {
- "cell_type": "code",
- "execution_count": null,
- "metadata": {
- "code_folding": []
- },
- "outputs": [],
- "source": [
- "### Cell for sending command to the AL5D ###\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": "Python 3 (ipykernel)",
- "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"
- },
- "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()) "
- }
- },
- "position": {
- "height": "567.727px",
- "left": "1160.45px",
- "right": "20px",
- "top": "72.9943px",
- "width": "567.003px"
- },
- "types_to_exclude": [
- "module",
- "function",
- "builtin_function_or_method",
- "instance",
- "_Feature"
- ],
- "window_display": false
- }
- },
- "nbformat": 4,
- "nbformat_minor": 4
-}
diff --git a/lab03_DHconvention.ipynb b/lab03_DHconvention.ipynb
deleted file mode 100644
index 195ec50bd39e3da9e516cfd72be21fbf6feb1486..0000000000000000000000000000000000000000
--- a/lab03_DHconvention.ipynb
+++ /dev/null
@@ -1,546 +0,0 @@
-{
- "cells": [
- {
- "cell_type": "markdown",
- "metadata": {},
- "source": [
- "# MARS-O-HELP\n",
- "\n",
- "With each hurdle she would overtake, there were a couple of new ones appearing. Dr. Elena Vasilescu felt like a modern Hercules, fighting a [Lernean Hydra](https://en.wikipedia.org/wiki/Lernaean_Hydra), where for every problem she solved, two new ones appeared to make her [sols](https://en.wikipedia.org/wiki/Mars_sol) less boring. After having obtained a good portion of food supplies and a reliable heat source for the bad days, she realised she will soon be out of oxygen inside her base. So much for all the food she managed to source.\n",
- "\n",
- "Dr. Vasilescu discovered an unrepairable fault in her carbon dioxide converter, the main life support unit of her base. Luckily, there was a spare converter, but it was way too heavy to life with her portable crane attached on her rover, and it was too far away from the robots in the adjacent container she used previously.\n",
- "\n",
- "She had to rely on the MARS-O-HELP (Mars Assistance Rendering Strong Operational Help for Evacuation and Lifting of Payloads) robot.\n",
- "\n",
- "<img src=\"artwork/stranded/a_new_big_robot.png\" width=40%/>\n",
- "\n",
- "Uf... yet another robot to learn how to use. How could she calculate the forward kinematics of such a BIG robot? She couldn't measure all the distances and angles… Luckily, NASA had shipped the robot together with a manual, but the only mention to forward kinematics was in a table caption saying: _To calculate the forward kinematics of the MARSOHELP, use the following DH parameters_. \n",
- "\n",
- "What were DH parameters?"
- ]
- },
- {
- "cell_type": "markdown",
- "metadata": {},
- "source": [
- "## DH convention for determining the direct geometric model\n",
- "\n",
- "*Kinematics* is the domain that studies motion without taking into consideration forces that generate the respective motion. The study of kinematics analyses the positions, velocities, accelerations and derivatives of higher order of the position (derivatives in respect to time or other variables). The study of kinematics of a robotic arm implies studying the geometric and temporal properties of a motion.\n",
- "\t\n",
- "The study of the geometry of a robotic arm, implies the attachment of a coordinate system on the different parts that constitute the arm. Once this is done, we can study the relationship between these coordinate systems to define the position and orientation of the end effector. However, when the geometry of the robot arm is not a trivial one, the position and orientation of these coordinate systems is difficult to determine in a repetitive way. This laboratory exercise presents a method to determine the position and orientation of the end-effector in respect to the reference frame, as a function of variables that are related to each segment of the arm. This model is an alternative method for calculating the direct geometric model of a robot."
- ]
- },
- {
- "cell_type": "markdown",
- "metadata": {},
- "source": [
- "### 3.2 Description of a link. DH parameters\n",
- "\n",
- "A robotic arm can be seen as a series of bodies connected together through joints, forming a kinematic chain. The bodies are called **links** and a **joint** connects two subsequent links together. From a mechanical design perspective, the majority of the joints implement a single degree of freedom, whether we are referring to translational or rotational joints. In the rare cases that a mechanism constitutes a joint with *n degrees of freedom*, this can be modelled as a series of *n joints with one degree of freedom, connected on n-1 links of length zero*.\n",
- "\n",
- "A link can have several attributes that can be important from a mechanical design point of view (form, material, rigidity, mass, etc.). For the purpose of obtaining the geometric model, a link can be considered as a rigid solid that defines a relationship between two adjacent joints (distal and proximal). The axis of a joint is defined as axis around which link $i$ rotates in relation to link $i-1$. For each two axes in space, the distance between them can be measured based on the common perpendicular. This common perpendicular exists always, and it is unique, except in the case of two parallel axes. In this case, there are infinite common perpendiculars but their length is always the same.\n",
- "\n",
- "<center>\n",
- " <figure>\n",
- " <img src=\"artwork/DH/fig3-1.png\" width=60% />\n",
- " <figcaption>Figure 3.1: DH parameters definition</figcaption>\n",
- " </figure>\n",
- "</center>\n",
- "\n",
- "Figure 3.1 presents a link $i-1$ for which we have identified the axis of the joints $i-1$ and $i$, and their common perpendicular. The first parameter that characterises a link is the **the length of the link**, denoted as $r_{i-1}$ and identified as the length of the common perpendicular between the axis of joints $i-1$ and $i$. \n",
- "\n",
- "The second parameter that is specific to a link is the angle between the axes of the joints. To obtain this parameters, we work as following:\n",
- "\n",
- "- We identify a plane that is normal to the common perpendicular of the two joint axes.\n",
- "- We project the axes of the joints on this plane.\n",
- "- We obtain the angle between these two projection.\n",
- "\n",
- "This parameter is denoted $\\alpha_{i-1}$, and is called **crossing angle** and represents the angle formed from the axes of the two joints, measured from the projection of axis $i-1$ until the axis $i$.\n",
- "\n",
- "Adjacent joints share a common axis. The third parameter refers to the distance measured on top of this common axis between link $i-1$ and link $i$. This parameter is called **link offset**, is denoted as $d_i$ and represents the distance measured on axis $i$ between the common perpendicular of axes $i-1$ and $i$ (for link $i-1$) and the common perpendicular of axes $i$ and $i+1$ (for link $i$).\n",
- "\n",
- "The fourth parameters specifies the angle of rotation between the direction of link $i-1$ and the direction of link $i$. This parameter is called **joint angle** and is denoted as $\\theta_i$.\n",
- "\n",
- "Figure 3.1 presents the ratio between link $i-1$ and link $i$. The parameter $r_{i-1}$ is the length of the perpendicular between the axes of link $i-1$ (axes $i-1$ and $i$). In a similar way, the parameter $r_i$ is the length of the perpendicular between the axes $i$ and $i+1$, specific to link $i$. The first parameter that realises the interconnection between segment $i-1$ and $i$ is the offset $d_i$, which represents the length (with sign) measured on the length of axis $i$ between the point which segment $r_{i-1}$ intersects axis $i$ and the point which segment $r_i$ intersects axis $i$. The parameter $d_i$ is variable if join $i$ is a translational joint.\n",
- "\n",
- "The second parameter that realises the interconnection is the angle measured around axis $i$ between the extension of segment $r_{i-1}$ and segment $r_i$. This parameter $\\theta_i$ is variable if the join is a rotational joint.\n",
- "\n",
- "The next video shows how we can get from a frame to another using the 4 transformations with the 4 DH parameters.\n",
- "\n",
- "<video controls src=\"artwork/DH/dh_parameters.mp4\" width = 50%>animation</video>\n",
- "\n",
- "Thus, by determining the values of these four parameters for each link, it is possible to determine the geometric model of any kinematic chain. Two of these parameters describe the link itself, while the other two parameters describe the connection with the neighbouring link. The definition of a mechanism through these four quantity is a convention called **Denavit-Hartenberg convention**, or simply **DH** introduced by J. Denavit and R.S. Hartenberg in \"A kinematic notation for lower pair mechanisms based on matrices\", Journal of Applied Mechanics, No. 22, 1959: pp. 215-221."
- ]
- },
- {
- "cell_type": "markdown",
- "metadata": {},
- "source": [
- "### 3.2 Association of coordinate systems according to the DH convention\n",
- "\n",
- "For the description of the position and orientation of any of the joints in relation to adjacent joints, we attach a coordinate system on each one of the links. These are numbered accordingly to the link to which they are attached. Therefore, system $i$ is attached on link $i$."
- ]
- },
- {
- "cell_type": "markdown",
- "metadata": {},
- "source": [
- "#### 3.2.1 Intermediate links\n",
- "\n",
- "We will use the following convention for attaching the coordinate system of a link: We denote as $Z_i$ the axis $Z$ of the coordinate system $i$. This axis is superimposed with the axis of joint $i$, and the positive direction of $Z_i$ should coincide with the positive rotation of axis $i$. The origin of system $i$, $O_i$, is located in the point that the perpendicular $r_i$ intersects the axis of joint $i$. The axis $X_i$ has the direction of segment $r_i$ and its direction heading from joint $i$ towards joint $i+1$.\n",
- "\n",
- "If $r_i=0$, the axis $X_i$ is the perpendicular on the plane formed by axes $Z_i$ and $Z_{i+1}$. Parameter $\\alpha_i$ is measured according to the positive direction of rotation along axis $X_i$. The axis $Y_i$ completes the coordinate system so that we can apply the rule of the right hand."
- ]
- },
- {
- "cell_type": "markdown",
- "metadata": {},
- "source": [
- "#### 3.2.2 First and last link\n",
- "\n",
- "We attach on the base of the robot (also known as link $\\{0\\}$) a Cartesian system $O_0X_0Y_0Z_0$. This system is static and can be seen as the reference system, when thinking of the determination of the direct geometric model. It is possible to describe the position and orientation of all the other coordinate systems in relation to the reference system. Even though the system $\\{0\\}$ can be chosen arbitrarily, it is easier if we chose the system to coincides with system $\\{1\\}$ when the variable associated with axis 1 is equal to 0. Under this conditions, we will always have $r_0=0$ and $\\alpha_0=0$. Furthermore, if the first axis is a revolute axis, parameter $d_1$ is 0 or if the first joint is a prismatic one, $\\theta_1$ will be 0. We must also choose system $\\{0\\}$ so that it respects the DH convention, namely that $X_0$ should be perpendicular to $Z_0$ and $Z_1$.\n",
- "\n",
- "If the last joint (joint $n$) is a revolute joint, we chose its direction $X_N$ so that it aligns with that of $X_{N-1}$ when $\\theta_N=0$. We further position the origin of system $\\{N\\}$ so that $d_N$ is 0. If the last joint (joint $n$) is a prismatic joint, we chose its direction $X_N$ so that $\\theta_N=0$, while we position the origin of the system $\\{N\\}$ on the intersection of axis $X_{N-1}$ and the axis of joint $n$ when $d_N$ is 0."
- ]
- },
- {
- "cell_type": "markdown",
- "metadata": {},
- "source": [
- "#### 3.2.3 DH parameters expressed in function of coordinate systems\n",
- "\n",
- "If all the links have a coordinate system attached according to the convention that was just presented, we can use the following definition for the DH parameters:\n",
- "\n",
- "<l>\n",
- " <ul>$r_{i}$ - distance between axes $Z_{i}$ and $Z_{i+1}$, measured on axis $X_{i}$;</ul>\n",
- " <ul>$\\alpha_{i}$ - angle between axes $Z_{i}$ and $Z_{i+1}$, measured around axis $X_{i}$;</ul>\n",
- " <ul>$d_i$ - distance between axes $X_{i}$ and $X_{i+1}$, measured on axis $Z_{i+1}$;</ul>\n",
- " <ul>$\\theta_i$ - angle between axes $X_{i}$ and $X_{i+1}$, measured around axis $Z_{i+1}$.</ul>\n",
- "</l>\n",
- "\n",
- "Usually, parameters $r_i$ are positive, as they describe a distance. Parameters $\\alpha_i$, $d_i$ and $\\theta_i$ *correspond to signed sizes*."
- ]
- },
- {
- "cell_type": "markdown",
- "metadata": {},
- "source": [
- "#### 3.2.4 Procedure for associating a Cartesian system on each link"
- ]
- },
- {
- "cell_type": "markdown",
- "metadata": {},
- "source": [
- "1. We identify the joints of the mechanism and we associate each one of the joints with a variable $q_i$, beginning from 1 until the number of degrees of freedom.\n",
- " "
- ]
- },
- {
- "cell_type": "markdown",
- "metadata": {},
- "source": [
- "<figure>\n",
- " <img align=\"left\" src=\"artwork/DH/step0_joints.png\" width=50%/>\n",
- "</figure>"
- ]
- },
- {
- "cell_type": "markdown",
- "metadata": {},
- "source": [
- "We draw the axes of all the joints. For the following steps (from 2 to 4), we will always consider two adjacent of these axes (corresponding to joints $i$ and $i+1$).\n",
- "\n",
- " "
- ]
- },
- {
- "cell_type": "markdown",
- "metadata": {},
- "source": [
- "2. We attach $Z_i$ along the direction of movement of joint $i$.\n",
- " "
- ]
- },
- {
- "cell_type": "markdown",
- "metadata": {},
- "source": [
- "<figure>\n",
- " <img align=\"left\" src=\"artwork/DH/step1_addZ.png\" width=50%/>\n",
- "</figure>\n"
- ]
- },
- {
- "cell_type": "markdown",
- "metadata": {},
- "source": [
- "3. We attach $X_i$ along the common perpendicular of $Z_i$ and $Z_{i+1}$\n",
- " "
- ]
- },
- {
- "cell_type": "markdown",
- "metadata": {},
- "source": [
- "<figure>\n",
- " <img align=\"left\" src=\"artwork/DH/step2_addX.png\" width=50%/>\n",
- "</figure>"
- ]
- },
- {
- "cell_type": "markdown",
- "metadata": {},
- "source": [
- "4. We attach $Y_i$ in a way so that we form a Cartesian system according to the right-hand rule.\n",
- " "
- ]
- },
- {
- "cell_type": "markdown",
- "metadata": {},
- "source": [
- "<figure>\n",
- " <img align=\"left\" src=\"artwork/DH/step3_addY.png\" width=50%/>\n",
- "</figure>"
- ]
- },
- {
- "cell_type": "markdown",
- "metadata": {},
- "source": [
- "5. We attach system $\\{0\\}$ in a way to coincide with system $\\{1\\}$ when the first joint is in position 0. System $\\{0\\}$ can also be chosen as a first step and proceed from there on. For the end-effector (system $N$) we choose the origin and the axis $X_N$ so that we cancel as many parameters as possible.\n"
- ]
- },
- {
- "cell_type": "markdown",
- "metadata": {},
- "source": [
- "<figure>\n",
- " <img align=\"left\" src=\"artwork/DH/step4_fin.png\" width=50%/>\n",
- "</figure>"
- ]
- },
- {
- "cell_type": "markdown",
- "metadata": {},
- "source": [
- "### 3.3 Calculation of the transformation matrices of a link\n",
- "\n",
- "The determination of the transformations that define system $\\{i\\}$ relatively to system $\\{i-1\\}$ are presented. In general, such a transformation will be obtained using the four parameters of the DH convention. Furthermore, for any robot, this transformation will have a single variable (associated with the joint), while the rest of the parameters will be constant and determined only by the structure of the link.\n",
- "\n",
- "For solving the direct geometric model problem, we decomposed it in sub-problems, one for each link, represented by the matrices $^nT_{n-1}$. For solving these sub-problems, they will also be decomposed into a set of four sub-problems. Each one of these transformations will be a function of just a variable, and we can therefore write each matrix with a simple inspection of the structure of the robot.\n",
- "\n",
- "For a link between joints $i-1$ and $i$, we define a set of intermediate transformation: $\\{P\\}$, $\\{Q\\}$ and $\\{R\\}$, as can be seen in figure 3.2.\n",
- "\n",
- "<center>\n",
- " <figure>\n",
- " <img src=\"artwork/DH/fig3-2.png\" width=60% />\n",
- " <figcaption>Figure 3.2: Elementary transformations within a link</figcaption>\n",
- " </figure>\n",
- "</center>\n",
- "\n",
- "\n",
- "The system $\\{R\\}$ differs from system $\\{i-1\\}$ by a rotation of $\\alpha_{i-1}$. System $\\{Q\\}$ differs from system $\\{R\\}$ by a translation of $r_{i-1}$. System $\\{P\\}$ differs from system $\\{Q\\}$ by a rotation of $\\theta_i$, while system $\\{i\\}$ differs from system $\\{P\\}$ by a translation of $d_i$. Therefore, the transformation that expresses system $\\{i\\}$ in $\\{i-1\\}$ is:\n",
- "\n",
- "\\begin{equation}\\label{eq3.1}\n",
- " T_{i-1}^i = T_{i-1}^R \\cdot T_R^Q \\cdot T_Q^P \\cdot T_P^i\n",
- "\\end{equation}\n",
- "\n",
- "\n",
- "\n",
- "which can be written in the following form too:\n",
- "\n",
- "\n",
- "\n",
- "\\begin{equation}\n",
- " T_i^{i+1} = RotX(\\alpha_{i}) \\cdot TransX(r_{i}) \\cdot RotZ(\\theta_{i}) \\cdot TransZ(d_{i})\n",
- "\\end{equation}\n",
- "\n",
- "\n",
- "\n",
- "Equation \\ref{eq3.1} in matrix form is:\n",
- "\n",
- "\n",
- "\\begin{equation}\n",
- "T_{i-1}^{i}=\\left[ {\\begin{array}{*{20}c}\n",
- " 1 & 0 & 0 & 0 \\\\\n",
- " 0 & {c\\alpha _{i} } & { - s\\alpha _{i} } & 0 \\\\\n",
- " 0 & {s\\alpha _{i} } & {c\\alpha _{i} } & 0 \\\\\n",
- " 0 & 0 & 0 & 1 \\\\\n",
- "\\end{array}} \\right]\\left[ {\\begin{array}{*{20}c}\n",
- " 1 & 0 & 0 & {r_{i} } \\\\\n",
- " 0 & 1 & 0 & 0 \\\\\n",
- " 0 & 0 & 1 & 0 \\\\\n",
- " 0 & 0 & 0 & 1 \\\\\n",
- "\\end{array}} \\right]\\left[ {\\begin{array}{*{20}c}\n",
- " {c\\theta _i } & { - s\\theta _i } & 0 & 0 \\\\\n",
- " {s\\theta _i } & {c\\theta _i } & 0 & 0 \\\\\n",
- " 0 & 0 & 1 & 0 \\\\\n",
- " 0 & 0 & 0 & 1 \\\\\n",
- "\\end{array}} \\right]\\left[ {\\begin{array}{*{20}c}\n",
- " 1 & 0 & 0 & 0 \\\\\n",
- " 0 & 1 & 0 & 0 \\\\\n",
- " 0 & 0 & 1 & {d_i } \\\\\n",
- " 0 & 0 & 0 & 1 \\\\\n",
- "\\end{array}} \\right]\n",
- "\\end{equation}\n",
- "\n",
- "\n",
- "resulting in:\n",
- "\n",
- "\\begin{equation}\\label{eq3.3}\n",
- "T_{i-1}^{i} = \\left[ {\\begin{array}{*{20}c}\n",
- " {c\\theta _i } & { - s\\theta _i } & 0 & {r_{i} } \\\\\n",
- " {s\\theta _i c\\alpha _{i} } & {c\\theta _i c\\alpha _{i} } & { - s\\alpha _{i} } & { - s\\alpha _{i} d_i } \\\\\n",
- " {s\\theta _i s\\alpha _{i} } & {c\\theta _i s\\alpha _{i} } & {c\\alpha _{i} } & {c\\alpha _{i} d_i } \\\\\n",
- " 0 & 0 & 0 & 1 \\\\\n",
- "\\end{array}} \\right]\n",
- "\\end{equation}\n",
- "\n",
- "After we have identified the Cartesian systems that are attached on the links, and the corresponding DH parameters (usually in the form of a table), we can proceed to the determination of a kinematic model. This work assumes the customisation of transformation matrices $^{n-1}T_n$ with parameters corresponding to each link. Once this is done, we can multiply the calculated matrices to obtain a single transformation matrix which provides the position and orientation of system $\\{N\\}$ in respect to system $\\{0\\}$:\n",
- "\n",
- "\n",
- "\n",
- "\\begin{equation}\\label{eq3.4}\n",
- "T_0^N = T_0^1 \\cdot T_1^2 \\cdot T_2^3 \\cdot T_3^4 \\cdot ... \\cdot T_{N - 1}^N\n",
- "\\end{equation}\n",
- "\n",
- "\n",
- "This matrix allows to identify the position and orientation of the end-effector in the coordinate system attached to the base."
- ]
- },
- {
- "cell_type": "markdown",
- "metadata": {},
- "source": [
- "### 3.4. Robotics toolbox for python\n",
- "\n",
- "The DH parameters are commonly used to describe kinematic chains (i.e. robotic structures) throughout the robotics world. Manufacturers usually provide the parameters for each link, and using only these parameters we can calculate the direct geometric model of a robot.\n",
- "\n",
- "The robotics toolbox has some very useful functions for simulating robotic arms just by using the DH parameters. We do that by creating a __DHRobot__ object and pass a list of __DHLink__ objects. If desired, you can only pass the non-zero DH parameters for each link. We can add, if necessary, a __tool__ (end effector) for a robot, by mentioning where the tool is with reference to the last joint. We can visualise the DH table of the robot and we can calculate the position of the end-effector of the robot for a specific set of joint coordinates using the __fkine__ command. Finally, we can visualise the robot for a specific set of joint coordinates by __plot__. "
- ]
- },
- {
- "cell_type": "code",
- "execution_count": null,
- "metadata": {},
- "outputs": [],
- "source": [
- "%matplotlib qt\n",
- "# matplotlib qt is for interactive rotation of figures\n",
- "from roboticstoolbox import *\n",
- "import numpy as np\n",
- "from math import *\n",
- "from spatialmath import *\n",
- "from spatialmath.base import *\n",
- "\n",
- "# a link may be Revolute or Prismatic\n",
- "\n",
- "# DHLink object creates a link with a joint attached to it, with arguments:\n",
- "# theta, \n",
- "# d, \n",
- "# a(r), \n",
- "# alpha, \n",
- "# offset, \n",
- "# sigma(0 for revolute, 1 for prismatic), \n",
- "# mdh (argument for choosing if the DH is standard or modified convention, the latter being the one we use, 1 for true) \n",
- "\n",
- "# or directly use one of: RevoluteDH, RevoluteMDH, PrismaticDH, PrismaticMDH which correspond to \n",
- "# modified or standard DH convention.\n",
- "\n",
- "# you can use only the non-zero arguments when creating the Links\n",
- "\n",
- "Link1 = DHLink(alpha = 0, d = 0.3, a = 0, sigma = 0, mdh = 1, offset = pi/2);\n",
- "Link2 = RevoluteMDH(alpha = pi/2) # this will default d,a,offset to 0\n",
- "Link3 = PrismaticMDH(alpha = pi/2)\n",
- "\n",
- "# we combine the links using DHRobot\n",
- "rob = DHRobot([\n",
- " Link1,\n",
- " Link2,\n",
- " Link3,\n",
- " RevoluteMDH(d=2)], name = \"my_4links_robot\")\n",
- "\n",
- "# adding a tool that is -0.4 units away from the last joint, on x:\n",
- "rob_tool = DHRobot([\n",
- " Link1,\n",
- " Link2,\n",
- " Link3,\n",
- " RevoluteMDH(d=2)], name = \"my_4links_robot_with_tool\", tool = transl(-0.4, 0, 0)@trotz(pi) )\n",
- "\n",
- "print(Link1)\n",
- "print(\"Tool transform\\n\",rob_tool.tool)\n",
- "\n",
- "# see the DH table of the robot\n",
- "print(rob)\n",
- "\n",
- "# find the pose (forward kinematics) of the robot with the following joint coordinates:\n",
- "q = np.array([0, pi/2, 0.4, -pi/6])\n",
- "T = rob.fkine(q)\n",
- "print(\"Pose of rob at q=\" , q , \"\\n\",T)\n",
- "\n",
- "# visualise the pose \n",
- "rob.plot(q)"
- ]
- },
- {
- "cell_type": "markdown",
- "metadata": {},
- "source": [
- "As another example, we shall see how the already modelled UR5 looks like. The UR5 is a 6 DOF robot with 6 revolute joints. We can use DHRobot methods, such as __plot__ or __fkine__ etc since the model is a DHRobot object. This example features __symbolic__ computations as well. In this case, it is recommended to use the __simplify__ method in order to have a smaller output (may take a few seconds)."
- ]
- },
- {
- "cell_type": "code",
- "execution_count": null,
- "metadata": {},
- "outputs": [],
- "source": [
- "%matplotlib qt\n",
- "# matplotlib qt is for interactive rotation of figures\n",
- "import roboticstoolbox as rtb\n",
- "from spatialmath.base import *\n",
- "import spatialmath.base.symbolic as sym\n",
- "from sympy import *\n",
- "\n",
- "ur5 = rtb.models.DH.UR5(symbolic=True)\n",
- "q = sym.symbol('q_:6')\n",
- "\n",
- "T = ur5.fkine(q)\n",
- "Ts = T.simplify()\n",
- "M = Matrix(Ts.A)\n",
- "pprint(M[:3,3])\n",
- "\n",
- "# Calculating the DGM for specific joint coordinates by substitution\n",
- "Tsub = M.subs({q[0]:0, q[1]:pi/4, q[2]:-pi/4, q[3]:0, q[4]:pi, q[5]:0})\n",
- "pprint(Tsub)\n",
- "\n",
- "# Calculating the DGM for specific joint coordinates directly\n",
- "Tfkine = ur5.fkine([pi,-pi/4,-pi/2,0,pi/4,0])\n",
- "pprint(Tfkine)\n",
- "\n",
- "# Plotting the robot\n",
- "ur5.plot([pi,-pi/4,-pi/2,0,pi/4,0])"
- ]
- },
- {
- "cell_type": "markdown",
- "metadata": {},
- "source": [
- "### 3.5. Proposed problems"
- ]
- },
- {
- "cell_type": "markdown",
- "metadata": {},
- "source": [
- "<center>\n",
- " <figure>\n",
- " <img src=\"artwork/DGM/al5d.png\" width=40%/>\n",
- " </figure>\n",
- " <figure>\n",
- " <img src=\"artwork/DH/AL5D_mdw.png\" width=50%/>\n",
- " <figcaption>Figure 3.3: AL5D robot and its schematic </figcaption>\n",
- " </figure>\n",
- " \n",
- "</center>"
- ]
- },
- {
- "cell_type": "markdown",
- "metadata": {},
- "source": [
- "1. Consider the AL5D robot from figure 3.3:\n",
- "- Calculate the direct geometric model of the robot using the DH convention. Measure A, B, C, D, E .\n",
- "- Define the robot model using the robotics toolbox.\n",
- "- Draw the robot for a specific set of coordinates, using the robotics toolbox.\n",
- "- Choose two joint coordinate sets to input to real the AL5D robot.\n",
- "- Check if the calculated poses of the end-effector using the robot model correspond to the real coordinates.\n",
- "- Write down any intermediary and final results and their intepretation.\n",
- "\n",
- "2. __Extra exercise__: Consider the two robots from figure 3.4\n",
- "- Determine their direct geometric model of the robots using the DH convention."
- ]
- },
- {
- "cell_type": "markdown",
- "metadata": {},
- "source": [
- "<center>\n",
- " <figure>\n",
- " <img src=\"artwork/DH/lego_robots-1.png\" width=60% />\n",
- " <figcaption>Figure 3.4: 5 joint (RRRTR) robots</figcaption>\n",
- " </figure>\n",
- "</center>"
- ]
- },
- {
- "cell_type": "code",
- "execution_count": null,
- "metadata": {
- "code_folding": []
- },
- "outputs": [],
- "source": [
- "### Cell for sending commands to the AL5D robot ###\n",
- "\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": "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
-}
diff --git a/lab04_Jacobian.ipynb b/lab04_Jacobian.ipynb
deleted file mode 100644
index 2f9e04dd66a217c5141fd9333b601f5f95838d01..0000000000000000000000000000000000000000
--- a/lab04_Jacobian.ipynb
+++ /dev/null
@@ -1,566 +0,0 @@
-{
- "cells": [
- {
- "cell_type": "markdown",
- "metadata": {},
- "source": [
- "## Chapter 4: \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 satelite dish! "
- ]
- },
- {
- "cell_type": "markdown",
- "metadata": {},
- "source": [
- "## The Jacobian\n",
- "\n",
- "Up until now, we've been discussing about the position and orientation of the\n",
- "robot as described by transformations. However, it is of special interest to\n",
- "consider the derivatives of the positions, meaning the velocities and\n",
- "accelerations. This is especially important since robots can be used in\n",
- "conditions where speed plays an important role: We usually want to perform a task\n",
- "as fast as possible, while still making it safe for the environment around the\n",
- "robot. The objective of this exercise is to define a way for calculating the\n",
- "end-effector velocity as a function of the joint velocities.\n",
- "\n",
- "### 4.1. Theoretical considerations\n",
- "\n",
- "Velocity is defined as the first derivative of the position. When discussing the\n",
- "velocity of a point, we can only have a linear velocity, since a point does not\n",
- "have any specific orientation. When discussing about objects though, there is a\n",
- "distinction between linear and angular velocities. The linear velocity shows us\n",
- "how fast an object translates relative to a fixed frame, while the angular\n",
- "velocity shows us how fast does one object rotate relative to a fixed frame.\n",
- "\n",
- "Each of these velocities has three components, one about each of the axes of the\n",
- "fixed coordinate frame. We denote the linear velocities as $ \\dot{x}, \\dot{y}$,\n",
- "and $ \\dot{z} $ and the angular velocities as $ \\omega_x, \\omega_y$, and $ \\omega_z $.\n",
- "\n",
- "\n",
- "If we define a vector containing all the end-effector velocities defined as \n",
- "\n",
- "\\begin{equation}\n",
- "\\xi = \\begin{bmatrix} \\dot{x} & \\dot{y} & \\dot{z} & \\omega_x & \\omega_y &\n",
- " \\omega_z \\end{bmatrix}^T \\text{ (1)}\\\\\n",
- "\\end{equation}\n",
- "\n",
- "we can define the relationship between end-effector and joint velocities as:\n",
- "\n",
- "\\begin{equation}\n",
- " \\xi = J \\dot{q} \\text{ (2)}\n",
- "\\end{equation}\n",
- "\n",
- "where $ \\dot{q} $ is a vector containing the velocities of the $ n $ robot\n",
- "joints. For the multiplication to be feasible, $ J $ should be a matrix and it\n",
- "should have dimensions $ 6 \\times n $. This matrix is called the\n",
- "_Jacobian_ and we can split it in two $ 3 \\times n $ submatrices, one\n",
- "corresponding to the angular and one to the linear velocities.\n",
- "\n",
- "#### 4.1.1 Angular velocities\n",
- "\n",
- "Depending on the design of the robot and the types of joints that it has, it\n",
- "might be able to change its orientation relative to the fixed frame. \n",
- "\n",
- "In the videos below you can see how a 2D robot would move to get to a specific rotation of the end-effector (considered as the end of the red link). \n",
- "\n",
- "\n",
- "|The movement of the whole robot|The movement of the end-effector |\n",
- "|--|--|\n",
- "|<video controls src=\"artwork/jacobian/angular_velocity_02.mp4\" width=400 />|<video controls src=\"artwork/jacobian/angular_velocity_01.mp4\" width=400 />|\n",
- "\n",
- "The part of the Jacobian matrix that describes the relationship between joint velocities and\n",
- "the angular velocity of the end effector is symbolised as $ J_{\\omega} $ and it\n",
- "is a $ 3 \\times n $ matrix, where $ n $ is the number of joints.\n",
- "\n",
- "Since prismatic joints do not produce any rotations, their contribution to the\n",
- "end-effector angular velocity is zero. Therefore, the columns corresponding to\n",
- "prismatic joints will be zero.\n",
- "\n",
- "For revolute joints, the contribution of each joint to each of the three angular\n",
- "velocities is related to the orientation of the joint. If a joint e.g. is\n",
- "aligned with the X-axis of the fixed frame, then it will naturally contribute\n",
- "only to the angular velocity of the end-effector around the X-axis, namely the\n",
- "$ \\omega_x $. Therefore, we define the column of the Jacobian corresponding to\n",
- "revolute joints equal to the orientation of the joint axis.\n",
- "\n",
- "\n",
- "If we have used the DH convention for calculating the transformation between two\n",
- "subsequent joints, then we have defined a coordinate frame whose z-axis is\n",
- "aligned with the joint axis of rotation. Therefore, the orientation of the\n",
- "revolute joint $ i $ is equal to the last column of the transformation matrix\n",
- "$ R_o^i $.\n",
- "\n",
- "\n",
- "Eventually we have:\n",
- "\n",
- "\\begin{equation}\n",
- " J_{\\omega} = \\begin{bmatrix} J_{\\omega 1} & J_{\\omega 2} & \\dots & J_{\\omega\n",
- " n} \\end{bmatrix} \\text{(3) } \\\\ \n",
- "\\end{equation}\n",
- " where:\n",
- "\n",
- "\\begin{equation}\n",
- " J_{\\omega i} = \\begin{bmatrix} 0 \\\\ 0 \\\\ 0 \\end{bmatrix} \\text{ (4)}\\\\\n",
- "\\end{equation}\n",
- " when $ i $ is a prismatic joint, and:\n",
- "\n",
- "\\begin{equation}\n",
- " J_{\\omega i} = \\begin{bmatrix} Z_x \\\\ Z_y \\\\ Z_z \\end{bmatrix} \\text{ (5)}\\\\\n",
- "\\end{equation}\n",
- " when $ i $ is a revolute joint.\n",
- " \n",
- " \n",
- "#### 4.1.2. Linear velocities\n",
- "\n",
- "The linear velocity of the end-effector is affected by both the revolute and the\n",
- "prismatic joints. \n",
- "\n",
- "In the videos below you can see how a 2D robot would move to get to a specific translation of the end-effector (considered as the end of the green link). \n",
- "\n",
- "|The movement of the whole robot|The movement of the end-effector |\n",
- "|--|--|\n",
- "|<video controls src=\"artwork/jacobian/linear_velocity_01.mp4\" width=400 />|<video controls src=\"artwork/jacobian/linear_velocity_02.mp4\" width=400 />|\n",
- "\n",
- "The prismatic joints contribute to the linear velocity of the\n",
- "end effector depending on the orientation of the joint, similar to the way that\n",
- "revolute joints contribute to the angular velocities. If e.g. a prismatic joint\n",
- "is oriented with the Z-axis of the fixed frame, then the velocity of the joint\n",
- "results in a velocity of the end-effector along the Z-axis of the fixed frame,\n",
- "and no velocity in the other two axes.\n",
- "\n",
- "Therefore, the column of the linear Jacobian corresponding to a prismatic joint\n",
- "has the form:\n",
- "\n",
- "\\begin{equation}\n",
- " J_{u i} = \\begin{bmatrix} Z_x \\\\ Z_y \\\\ Z_z \\end{bmatrix} \\text{ (6)}\n",
- "\\end{equation}\n",
- "\n",
- "\n",
- "However, contrary to the angular velocities, the linear velocity of the\n",
- "end-effector does not depend only on the prismatic joints. The revolute joints\n",
- "have a contribution to the linear velocity of the end-effector as well. This is\n",
- "similar to the way that the wheels of the car, even though they are just\n",
- "rotating and not translating, they still cause a linear motion of the car.\n",
- "\n",
- "The contribution of the revolute joints on the linear velocity of the\n",
- "end-effector are related to the orientation of the joint, but also to the\n",
- "distance between the joint and the end effector. This is because a revolute\n",
- "joint does not only change the orientation of the end-effector, but also it's position.\n",
- "\n",
- "Finally, the linear velocity resulting from an revolute joint is expressed as a\n",
- "vector, and more specifically as the cross product between the vectors of the\n",
- "axis of rotation and the vector connecting the joint origin and the\n",
- "end-effector. Therefore, for revolute joints the linear Jacobian has the form:\n",
- "\n",
- "\\begin{equation}\n",
- " J_{u i} = \\begin{bmatrix} z_{i} \\times (o_{n+1} - o_{i}) \\end{bmatrix}\n",
- "\\end{equation}\n",
- "\n",
- "The cross product is an operation between two 3D vectors that results in a third 3D vector that is perpedicular to the first two\n",
- "\n",
- "<center>\n",
- " <figure class=\"image\">\n",
- " <img src=\"artwork/jacobian/cross_product.png\" width=20% />\n",
- " <figcaption>Figure 4.1: Cross product between two vectors </figcaption>\n",
- " </figure>\n",
- "</center>\n",
- "\n",
- "We can visualise this for a 2D robot as in the figure 4.2:\n",
- "\n",
- "<center>\n",
- " <figure class=\"image\">\n",
- " <img src=\"artwork/jacobian/linear_velocity_revolute.png\" width=40% />\n",
- " <figcaption>Figure 4.2: The contribution of a revolute joint (q1) on the linear velocity of the end-effector. The cross product of the axis of rotation (blue arrow) and the radius of rotation (magenta arrow) produces the linear velocity vector (green arrow) </figcaption>\n",
- " </figure>\n",
- "</center>\n",
- "\n",
- "We've seen therefore how a revolute or prismatic joint can contribute to the jacobian of the robot and therefore to the end effector's velocity. In the example below, you can vary the initial pose of the joints (q1, q2) of a 2D robot and then see how the end-effector changes its velocity components depending on the selected joint and type."
- ]
- },
- {
- "cell_type": "code",
- "execution_count": null,
- "metadata": {},
- "outputs": [],
- "source": [
- "%matplotlib qt\n",
- "from ipywidgets import interact, interact_manual, widgets\n",
- "from lab_functions import velocity\n",
- "\n",
- "interact_manual(velocity, q1 = (0, 3.14), q2 = (0, 3.14), joint = ['q1', 'q2'], j_type = ['revolute', 'prismatic'])"
- ]
- },
- {
- "cell_type": "markdown",
- "metadata": {},
- "source": [
- "For the same 2DOF robot as in the previous example, we shall see the variation of the velocities, by changing the q1 and q2 angles. The ellipse represents the vectors of the possible end-effector velocities for a specific configuration of the robot. "
- ]
- },
- {
- "cell_type": "code",
- "execution_count": null,
- "metadata": {},
- "outputs": [],
- "source": [
- "%matplotlib qt\n",
- "from ipywidgets import interact, interact_manual, widgets\n",
- "from lab_functions import velocity_ell\n",
- "\n",
- "interact_manual(velocity_ell, q1 = (0, 3.14), q2 = (0, 3.14))"
- ]
- },
- {
- "cell_type": "markdown",
- "metadata": {
- "tags": []
- },
- "source": [
- "### 4.2 Inverse velocities\n",
- "\n",
- "As with the kinematics, it is usually more useful to be able to solve the\n",
- "inverse velocities problem i.e. to calculate the joint coordinates that will\n",
- "result in the desired end-effector velocities. This is accomplished by\n",
- "multiplying from the left with the inverse of the Jacobian both sides of our\n",
- "principal equation:\n",
- "\n",
- "\\begin{equation}\n",
- " \\xi = J \\dot{q} \\equiv J^{-1}\\xi = \\dot{q} \\text{ (8)}\n",
- "\\end{equation}\n",
- "\n",
- "\n",
- "However, to be able to do this, the Jacobian must be invertible. For a matrix to\n",
- "be invertible, it must satisfy two conditions:\n",
- "\n",
- "- It must be square\n",
- "- Its rank should be equal to its dimension\n",
- "\n",
- "\n",
- "\n",
- "As we noticed above, the size of the Jacobian is $ 6 \\times n $, where n is the\n",
- "number of joints. Therefore, for the Jacobian to be invertible we need to have\n",
- "six joints. The second condition is related to the degrees of freedom as the rank\n",
- "of the Jacobian is equal to the degrees of freedom of the robot. Therefore for the\n",
- "Jacobian to be invertible, the robot must not only have six joints, but also they\n",
- "should be arranged in such a configuration so that they result in six degrees of\n",
- "freedom.\n",
- "Since the Jacobian depends on the joint coordinates, we can see that for some\n",
- "set of joint coordinates, the Jacobian might lose rank and become\n",
- "non-invertible. When this occurs, we say that the robot is in a\n",
- "_singular_ configuration. This usually happens when two subsequent links get\n",
- "aligned, and therefore the influence of one of the two joints on the\n",
- "end-effector position becomes almost zero (see figure 4.3)\n",
- "\n",
- "<center>\n",
- " <figure class=\"image\">\n",
- " <img src=\"artwork/jacobian/jac1.png\" width=40% />\n",
- " <figcaption>Figure 4.3: A robot in a singular configuration </figcaption>\n",
- " </figure>\n",
- "</center>\n",
- "\n",
- "#### 4.2.1. The pseudoinverse\n",
- "\n",
- "\n",
- "Some times, we have robots with more than 6 joints, still resulting in a robot\n",
- "with six degrees of freedom. We should be able to calculate the inverse\n",
- "velocities problem since the robot has the dexterity to impose velocities in and\n",
- "around all axes. However, our Jacobian is $ 6 \\times 7 $ and therefore non\n",
- "invertible.\n",
- "\n",
- "To go around this problem, we calculate the pseudoinverse of the Jacobian. The\n",
- "pseudoinverse is a matrix that if multiplied with the original matrix gives the\n",
- "identity, just like the real inverse. We calculate it using the following\n",
- "methodology:\n",
- "\n",
- "\\begin{equation}\n",
- " (JJ^T)(JJ^T)^{-1}= I \\\\\n",
- " J[J^T(JJ^T)^{-1}]= I \\\\ \n",
- " JJ^+ = I \\\\ \n",
- "\\end{equation} \n",
- "\n",
- "where: \n",
- "\\begin{equation}\n",
- " J^+ = J^T(JJ^T)^{-1} \n",
- "\\end{equation}"
- ]
- },
- {
- "cell_type": "markdown",
- "metadata": {
- "tags": []
- },
- "source": [
- "### 4.3 Robotic toolbox for python\n",
- "\n",
- "The robotics toolbox methods to calculate the jacobian of a robot, for specific configurations. As we saw above,\n",
- "the Jacobian depends on the joint orientations, which depend on the joint angles. Therefore, the Jacobian is not a fixed quantity. In the following examples, we will work with some predefined robot models, namely the __AL5D__ robot.\n",
- "\n",
- "We can calculate the Jacobian for a specific configuration running the following command: __jacob0(q)__ where q is a list of the joints' angles. Jacobians can also be computed for symbolic joint variables as for forward kinematics."
- ]
- },
- {
- "cell_type": "code",
- "execution_count": null,
- "metadata": {},
- "outputs": [],
- "source": [
- "import roboticstoolbox as rtb\n",
- "from math import *\n",
- "import numpy as np\n",
- "np.set_printoptions(formatter={'float': '{: 0.3f}'.format})\n",
- "\n",
- "al5d = rtb.models.DH.AL5D_mdw()\n",
- "home = [0, 0, 0, 0, 0]\n",
- "J = al5d.jacob0(home)\n",
- "print(\"Jacobian in config home: \\n\", J)\n",
- "print(\"matrix rank in this config: \", np.linalg.matrix_rank(J))\n",
- "\n",
- "# You can calculate the pseudoinverse of the Jacobian like this\n",
- "Jpinv = np.linalg.pinv(J)\n",
- "print(\"Pseudojacobian in config home: \\n\", Jpinv)\n",
- "print(\"matrix rank in this config: \", np.linalg.matrix_rank(Jpinv))"
- ]
- },
- {
- "cell_type": "code",
- "execution_count": null,
- "metadata": {},
- "outputs": [],
- "source": [
- "import roboticstoolbox as rtb\n",
- "from spatialmath.base import *\n",
- "import spatialmath.base.symbolic as sym\n",
- "from sympy import *\n",
- "\n",
- "# Example of calculating the symbolic Jacobian of a robot using the toolbox\n",
- "rob = rtb.models.DH.TwoLink(symbolic=True)\n",
- "q = sym.symbol(\"q_:2\") \n",
- "Jsym = rob.jacob0(q)\n",
- "Js = Matrix(Jsym)\n",
- "Jsimple = simplify(Js)\n",
- "pprint(Jsimple)\n",
- "\n",
- "Jsub = Jsimple.subs({q[0]:0, q[1]:0})\n",
- "# pprint (from sympy) for pretty printing of the Matrix type variable\n",
- "pprint(Jsub)"
- ]
- },
- {
- "cell_type": "markdown",
- "metadata": {},
- "source": [
- "### 5.4. Proposed problems\n",
- "\n",
- "Considering the robotic manipulator AL5D fig 4.4.\n",
- " \n",
- " <center>\n",
- " <figure>\n",
- " <img src=\"artwork/DGM/al5d.png\" width=40%/>\n",
- " </figure>\n",
- " <figure>\n",
- " <img src=\"artwork/DH/AL5D_mdw.png\" width=50%/>\n",
- " <figcaption>Figure 4.4: AL5D robot and its schematic </figcaption>\n",
- " </figure>\n",
- " \n",
- " </center>\n",
- " \n",
- " a. Calculate the Jacobian of the AL5D model for the following set of joint coordinates, given the transformation matrices for each link\n",
- "\n",
- "$ q= \\begin{bmatrix} \\pi/6 & -\\pi/6 & \\pi/8 & 0 & 0 \\end{bmatrix} $ [rad]\n",
- "\n",
- " b. Create the AL5D model from the robotic toolbox\n",
- " c. Calculate the Jacobian for the same sets of joint coordinates below using the robotics toolbox.\n",
- " d. Compare the results with your Jacobian for the same joint coordinates.\n",
- " e. Calculate the resulting end-effector velocities (linear and angular) for this set of joint coordinates and joint velocities equal to:\n",
- "\n",
- "$ \\dot{q} = \\begin{bmatrix} 0.1 & 0.14 & 0.1. & 0.02 & 0.1 \\end{bmatrix} $ [rad/s]\n",
- "\n",
- " f. Calculate the joint velocities that are required for end-effector linear velocity along the Y-axis of 0.2 m/s (and 0 for the rest)"
- ]
- },
- {
- "cell_type": "markdown",
- "metadata": {},
- "source": [
- "The transformation matrices for the AL5D robot stored in list `R`. The first element represents $R_0^1$, the second $R_0^2$, and the nth element $R_0^n$. You need to update variable `q` for the desired joint coordinates."
- ]
- },
- {
- "cell_type": "code",
- "execution_count": null,
- "metadata": {
- "code_folding": []
- },
- "outputs": [],
- "source": [
- "import roboticstoolbox as rtb\n",
- "import spatialmath.base.symbolic as sym\n",
- "from spatialmath.base import trotx, trotz, transl\n",
- "from spatialmath import SE3\n",
- "import numpy as np\n",
- "from sympy import *\n",
- "init_printing(num_columns=500)\n",
- "\n",
- "\n",
- "# creating the model of the robot\n",
- "rob = rtb.models.DH.AL5D_mdw(symbolic=True)\n",
- "\n",
- "# initialisations\n",
- "pR = sym.symbol(\"R_0^1:7\")\n",
- "T = np.eye(4)\n",
- "R = []\n",
- "\n",
- "# loop for saving and printing the transformation matrices from the base to joint i in variable R\n",
- "q = np.array([0,0,0,0,0])\n",
- "\n",
- "for i in [0,1,2,3,4]: \n",
- " display(pR[i])\n",
- " T = T@rob.links[i].A(q[i]).A\n",
- " R.append(T)\n",
- " pprint(Matrix(R[i]))\n",
- " \n",
- "display(pR[5])\n",
- "R.append(T*rob.tool)\n",
- "pprint(Matrix(T*rob.tool))"
- ]
- },
- {
- "cell_type": "code",
- "execution_count": null,
- "metadata": {},
- "outputs": [],
- "source": [
- "###### PYTHON INDEXING #######\n",
- "import numpy as np\n",
- "\n",
- "# x = np.array([[0, 1, 2, 3, 4],[5, 6, 7, 8, 9], [10,11,12,13,14]])\n",
- "\n",
- "# Element indexing is 0-based\n",
- "print(\"\\n x matrix is \\n\", x)\n",
- "print(\"\\n element at line 1, column 3, is \", x[1, 3])\n",
- "\n",
- "# Note that if one indexes a multidimensional array with fewer indices than dimensions,\n",
- "# one gets a subdimensional array. For example:\n",
- "print(\"\\n element 0 of matrix x is \\n \",x[0])\n",
- "\n",
- "# The basic slice syntax is i:j where i is the starting index, j is the stopping index.\n",
- "# Assume n is the number of elements in the dimension being sliced. \n",
- "# Then, if i is not given it defaults to 0 for k > 0 and n - 1 for k < 0 . \n",
- "\n",
- "print(\"\\n Line 0, elements from column 3 onwards are \\n \",x[0:1,3:])\n",
- "\n",
- "print(\"\\n Elements from lines 0 to 1, columns 2 to 3 \\n\", x[0:2,2:4])\n",
- "\n",
- "###### USEFUL METHODS #######\n",
- "# Calculating a cross product\n",
- "x_arr = np.array([[1,2,3]])\n",
- "y_arr = np.array([[4,5,6]])\n",
- "\n",
- "c1 = np.cross(x_arr, y_arr)\n",
- "\n",
- "print(\"\\n c1: \\n\", c1, \"with shape \", c1.shape)\n",
- "\n",
- "# reshaping\n",
- "c2 = c1.reshape((3, 1))\n",
- "print(\"\\n c2 \\n\", c2, \" with shape \", c2.shape)\n",
- "\n",
- "# transposing\n",
- "c3 = c1.T\n",
- "\n",
- "# concatenating vectors\n",
- "print(\"\\n c3 and c3 concatenated by line \\n\", np.concatenate((c3, c3), axis=0))\n",
- "print(\"\\n c3 and c3 concatenated by column \\n\", np.concatenate((c3, c3), axis=1))"
- ]
- },
- {
- "cell_type": "markdown",
- "metadata": {},
- "source": [
- "### Extra exercises\n",
- " a. Using the cell below, give different velocity arguments in different scenarios and observe the robot's behaviour.\n",
- " b. Using the cell at the end, place the robot in a specific configuration (using new pose option) and, using the interact, visually check for each of the axes the possible instantaneous linear and angular velocities (choosing one joint at a time)."
- ]
- },
- {
- "cell_type": "code",
- "execution_count": null,
- "metadata": {},
- "outputs": [],
- "source": [
- "## extra exercise point a ##\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=(9,90),dq1=(9,90),dq2=(9,90),dq3=(9,90), dq4=(9,90), vel=fixed(True)) "
- ]
- },
- {
- "cell_type": "code",
- "execution_count": null,
- "metadata": {
- "code_folding": []
- },
- "outputs": [],
- "source": [
- "## extra exercise point b ##\n",
- "from al5d_control import *\n",
- "from ipywidgets import interact_manual\n",
- "\n",
- "rrob = AL5DControl()\n",
- "interact_manual(rrob.inst_AL5D_velocities, q0=(-90,90),q1=(-90,90),q2=(-90,90),q3=(-90,90),q4=(-90,90), joint = ['new pose','q0', 'q1', 'q2', 'q3','q4']) "
- ]
- }
- ],
- "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"
- },
- "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
-}
diff --git a/lab05_IKM.ipynb b/lab05_IKM.ipynb
deleted file mode 100644
index 6596fc404c6ae9b960133e5ed794065c91342c7f..0000000000000000000000000000000000000000
--- a/lab05_IKM.ipynb
+++ /dev/null
@@ -1,474 +0,0 @@
-{
- "cells": [
- {
- "cell_type": "markdown",
- "metadata": {},
- "source": [
- "# Laboratory 5\n",
- "## Determination of an inverse kinematics model\n",
- "\n",
- "In the previous works we analysed the problem of calculating the pose of a Cartesian system attached on the end-effector, when the joint coordinates are known. This exercise proposes an approach a bit more difficult: we want to determine the joint coordinates for a specific pose of the end-effector. In other words, determine a set of coordinates of the robot $q_1,q_2,...,q_n$, that ensure a specific pose of the end-effector. This problem is known as the __inverse kinematics model__ of a kinematic chain. The main objective of this exercise is the description of a heuristic method for determining this inverse kinematics model of a robotic arm and to apply this method in two robotic kinematic structures."
- ]
- },
- {
- "cell_type": "markdown",
- "metadata": {},
- "source": [
- "### 5.1 Theoretical considerations\n",
- "\n",
- "The problem of determining the inverse kinematics model of a robot is a non-linear problem. For a robotic structure, we are starting with the numerical values of the matrix that describes the direct geometric model ($T_0^n$) and we aim at determining the set of joint coordinates of the robot ($q_1,q_2,...q_n$). For a robot with 6 degrees of freedom (6 motor joints), we obtain a system with 12 equations and 6 unknowns. However, from the 9 equations that result from the correspondence of the orientation from matrix $T_0^6$, only three of them are independent and the other 6 are redundant. This limits us to the determination of maximum 3 independent variables. If, on these three equations, we add the three equations resulting from the correspondence of the position vectors from matrix $T_0^6$, we obtain a set of 6 independent equations that allow us to determine a maximum of 6 independent variables. These variables correspond to the 6 degrees of freedom of the robot. This system of equations is non-linear and transcendent, often proving difficult to solve. Like all non-linear systems of equations, the main problems are raised by the existence of solutions, existence of multiple solutions and the method to use for solving the system.\n",
- "\n",
- "Considering a robot for which we denote as $\\overline{q}=[q_1,\\; q_2,\\;...\\;q_n]^T$ the vector of coordinates of the robot, and as $\\overline{Z}=[P_{X},\\; P_{Y},\\; P_{Z},\\; \\phi,\\; \\theta,\\; \\psi]^T$ the vector of Cartesian coordinates of the end-effector, the inverse kinematics model (IKM) is represented by a system of equations that construct the dependence $\\overline{q}=f(\\overline{Z})$.\n",
- "\n",
- "We need to determine the inverse kinematics model since the trajectories of the robotic arm are described in Cartesian coordinates, while the robot is driven by the motor joints and is therefore described in the joint coordinates. The inverse kinematics model is determined based either on the direct geometric model, on heuristic methods (e.g. for simple structures), on iterative methods (e.g. for more complicated structures), or on geometric methods. It is possible to determine either the particular solution of a model or a general (analytic) solution thereof. Applying heuristic methods is convenient but does not guarantee a unique solution.\n",
- "\n",
- "Another problem with the determination of the IKM, is the redundancy of the manipulator. A manipulator is said to be redundant when it is able to reach a specific pose under two or more different configurations of its kinematic chain. This translates mathematically in two or more solutions of the IKM and is linked with the appearance of specific mathematical functions, such as the square root or the cosine, which give usually two solutions: a negative and a positive one.\n",
- "\n",
- "For example, in figure 5.1 we present a redundant manipulator with two degrees of freedom, and two configurations of its kinematic chain that generate the same pose of its end-effector.\n",
- "\n",
- "<center>\n",
- " <figure class=\"image\">\n",
- " <img src=\"artwork/IKM/redundant.png\" width=40% />\n",
- " <figcaption>Figure 5.1: Redundant manipulator</figcaption>\n",
- " </figure>\n",
- "</center>\n",
- "\n",
- "The choice of one over the other solution depends on external restrictions and on the construction elements of the robot. However, once a solution has been chosen, it should be used with continued consistency to avoid unnecessary displacement of the robot between the two configurations."
- ]
- },
- {
- "cell_type": "markdown",
- "metadata": {},
- "source": [
- "### 5.2 *Heuristic method for determining the IKM*\n",
- "\n",
- "__I__. We equate the homogeneous transformation matrices of the manipulator (direct geometric model) with the general homogeneous transformation matrix (\\ref{eq4.6}). If we are seeking a specific pose, the homogeneous transformation matrix of the manipulator is equated with a matrix that describes this specific pose.\n",
- "\n",
- "__II__. We inspect both matrices, observing if:\n",
- " \n",
- " a. there are elements that contain only one variable\n",
- " b. there are pairs of elements that can produce expressions with a single variable after a division. We especially \n",
- " use divisions that produce an arctan function.\n",
- " c. there are combinations of elements that can be simplified using trigonometric identities.\n",
- "\n",
- "__III__. Once we select such an element, we equate it with its correspondent from the second matrix, and we solve the obtained equation.\n",
- "\n",
- "__IV__. We repeat step __III__ until we solve all equations that contain elements identified in step __II__.\n",
- "\n",
- "__V__. If we obtain redundant or unsatisfactory results, we try again starting from a different equation, keeping the solution aside for later use. We prefer to obtain solutions in function of the vector $P=[P_{X},\\; P_{Y},\\; P_{Z}]^T$ and not in function of the vectors $X$, $Y$ or $Z$, since this imposes in general position only and not orientation.\n",
- "\n",
- "__VI__. If we cannot identify all the robot joint coordinates, we pre-multiply both parts of the equation with the inverse transformation matrix of the first kinematic joint, obtaining a new set of equations. Alternatively we can try post-multiplication of both parts of the equation with the transformation matrix of the last kinematic joint.\n",
- "\n",
- "__VII__. We repeat steps __II - VI__ until we obtain all the solutions or until we exhaust all the pre- or post-multiplication matrices.\n",
- "\n",
- "__VIII__. If we cannot obtain a suitable solution for a variable, we can consider one of the solutions obtained in step __V__.\n",
- "\n",
- "__IX__. If we cannot obtain a solution for one of the variables, it means that the manipulator cannot reach the respective pose (it is outside of the space of operation of the robot)\n",
- "\n",
- "For example, for the manipulator with two degrees of freedom from fig4.2 we can write the direct geometric model as:\n",
- "\n",
- "\n",
- "<center>\n",
- " <figure class=\"image\">\n",
- " <img src=\"artwork/IKM/mgi2.png\" width=40% />\n",
- " <figcaption>Figure 5.2: Robt RR</figcaption>\n",
- " </figure>\n",
- "</center>\n",
- "\n",
- "\\begin{equation}\n",
- "T_1=Rot(Z,q_1) = \\left[ {{\\begin{array}{*{20}c}\n",
- " {c_1 } & { - s_1 } & 0 & 0 \\\\\n",
- " {s_1 } & {c_1 } & 0 & 0 \\\\\n",
- " 0 & 0 & 1 & 0 \\\\\n",
- " 0 & 0 & 0 & 1 \\\\\n",
- "\\end{array} }} \\right]\n",
- "\\end{equation}\n",
- "\\begin{equation}\n",
- "T_2=Trans(Z,l1) = \\left[ {{\\begin{array}{*{20}c}\n",
- " 1 & 0 & 0 & 0 \\\\\n",
- " 0 & 1 & 0 & 0 \\\\\n",
- " 0 & 0 & 1 & l_1 \\\\\n",
- " 0 & 0 & 0 & 1 \\\\\n",
- "\\end{array} }} \\right]\n",
- "\\end{equation}\n",
- "\n",
- "\\begin{equation}\n",
- "T_3=Rot(Y,q_2) = \\left[ {{\\begin{array}{*{20}c}\n",
- " {c_2 } & 0 & s_2 & 0 \\\\\n",
- " 0 & 1 & 0 & 0 \\\\\n",
- " -s_2 & 0 & c_2 & 0 \\\\\n",
- " 0 & 0 & 0 & 1 \\\\\n",
- "\\end{array} }} \\right]\n",
- "\\end{equation}\n",
- "\n",
- "\\begin{equation}\n",
- "T_4=Trans(X,l_2) = \\left[ {{\\begin{array}{*{20}c}\n",
- " 1 & 0 & 0 & {l_2 }\\\\\n",
- " 0 & 1 & 0 & 0 \\\\\n",
- " 0 & 0 & 1 & 0 \\\\\n",
- " 0 & 0 & 0 & 1 \\\\\n",
- "\\end{array} }} \\right]\n",
- "\\end{equation}\n",
- "\n",
- "resulting in a direct geometric model in form:\n",
- "\\begin{equation}\n",
- "\\label{eq4.1}\n",
- "DGM = T = T_1 \\cdot T_2 \\cdot T_3 \\cdot T_4 = \\left[ {{\\begin{array}{*{20}c}\n",
- " {c_1\\cdot c_2 } & { - s_1 } & {c_1\\cdot s_2 } & { c_1\\cdot c_2\\cdot l_2} \\\\\n",
- " {s_1\\cdot c_2 } & {c_1 } & {s_1\\cdot s_2 } & { s_1\\cdot c_2\\cdot l_2} \\\\\n",
- " { - s_2 } & 0 & {c_2 } & {l_1 - l_2\\cdot s_2 } \\\\\n",
- " 0 & 0 & 0 & 1 \\\\\n",
- "\\end{array} }} \\right]\n",
- "\\end{equation}\n",
- "\n",
- "The determination of the inverse kinematics model, assumes the equation of the direct transformation matrix ($T$) with the general matrix of a homogeneous transformation ($T_g$) (see also relation eq.1.12):\n",
- "\n",
- "\n",
- "\\begin{equation}\\label{eq4.6}\n",
- "T = T_{g}\n",
- "\\end{equation}\n",
- "\n",
- "\\begin{equation}\\label{eq4.7}\n",
- "\\left[ {{\\begin{array}{*{20}c}\n",
- " {c_1\\cdot c_2 } & { - s_1 } & {c_1\\cdot s_2 } & { c_1\\cdot c_2\\cdot l_2} \\\\\n",
- " {s_1\\cdot c_2 } & {c_1 } & {s_1\\cdot s_2 } & { s_1\\cdot c_2\\cdot l_2} \\\\\n",
- " { - s_2 } & 0 & {c_2 } & {l_1 - l_2\\cdot s_2 } \\\\\n",
- " 0 & 0 & 0 & 1 \\\\\n",
- "\\end{array} }} \\right]=\n",
- "\\left[ \\begin{array}{cccc}\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{array} \\right]\n",
- "\\end{equation}\n",
- "\n",
- "\n",
- "For the determination of the joint coordinates vector of the robot $q=[q_1,\\; q_2]^T$ we construct a system of equations:\n",
- "\\begin{equation}\\label{mgi-eq3}\n",
- "\\left\\{ {{\\begin{array}{lll}\n",
- "{ c_1\\cdot c_2\\cdot l_2} & = & P_X\\\\\n",
- "{ s_1\\cdot c_2\\cdot l_2} & = & P_Y \\\\\n",
- "{l_1 - l_2\\cdot s_2 } & = & P_Z \\\\\n",
- "\\end{array} }} \\right.\n",
- "\\end{equation}\n",
- "\n",
- "\\begin{equation}\n",
- "\\frac{P_Y }{P_X } = \\frac{s_1 }{c_1 } \\;\\Longrightarrow \\;q_1 = \\arctan\\left({\\frac{P_Y }{P_X }} \\right)\n",
- "\\end{equation}\n",
- "\n",
- "For the determination of $q_2$ we resort to the pre-multiplication of both parts of the matrix equation (\\ref{eq4.7}) with the inverse of the transformation matrix of the first kinematic joint (step __VI__, obtaining a new set of equations:\n",
- "\n",
- "\\begin{equation}\n",
- "T_1^{ - 1} \\cdot T_1 \\cdot T_2 \\cdot T_3 \\cdot T_4= T_1^{ - 1} \\cdot T_g\n",
- "\\end{equation}\n",
- "\n",
- "\n",
- "\\begin{equation}\n",
- "\\left[ {{\\begin{array}{*{20}c}\n",
- " {c_2 } & 0 & {s_2 } & { c_2\\cdot l_2 } \\\\\n",
- " 0 & 1 & 0 & 0 \\\\\n",
- " { - s_2 } & 0 & {c_2 } & {l_1 - l_2\\cdot s_2 } \\\\\n",
- " 0 & 0 & 0 & 1 \\\\\n",
- "\\end{array} }} \\right] =\n",
- "\\\n",
- "\\\n",
- " \\left[ {{\\begin{array}{*{20}c}\n",
- " {X_X\\cdot c_1 + X_Y\\cdot s_1 } & {Y_X\\cdot c_1 + Y_Y\\cdot s_1 } & {Z_X\\cdot c_1 + Z_Y\\cdot s_1 } & {P_X\\cdot c_1 + P_Y\\cdot s_1 } \\\\\n",
- " {X_Y\\cdot c_1 - X_X\\cdot s_1 } & {Y_Y\\cdot c_1 - Y_X\\cdot s_1 } & {Z_Y\\cdot c_1 - Z_X\\cdot s_1 } & {P_Y \\cdot c_1 - P_X\\cdot s_1 } \\\\\n",
- " {X_Z } & {Y_Z } & {Z_Z } & {P_Z } \\\\\n",
- " 0 & 0 & 0 & 1 \\\\\n",
- "\\end{array} }} \\right]\n",
- "\\end{equation}\n",
- "\n",
- "\n",
- "resulting in the system\n",
- "\n",
- "\\begin{equation}\n",
- " \\left\\{ {{\\begin{array}{lll}\n",
- " c_2 \\cdot l_2 & = & P_X\\cdot c_1 + P_Y\\cdot s_1 \\\\\n",
- " s_2\\cdot l_2& = & l_1 - P_Z \\\\\n",
- "\\end{array} }} \\right.\n",
- "\\end{equation}\n",
- "\n",
- "\\begin{equation}\n",
- "\\frac{l_1 - P_Z }{P_X\\cdot c_1 + P_Y\\cdot s_1 } = \\frac{s_2 }{c_2 } \\;\\Longrightarrow\\;\n",
- " q_2 = \\arctan\\left( {\\frac{l_1 - P_Z }{P_X \\cdot c_1 + P_Y\\cdot s_1 }} \\right)\n",
- "\\end{equation}"
- ]
- },
- {
- "cell_type": "markdown",
- "metadata": {
- "tags": []
- },
- "source": [
- "### 5.3. Robotics toolbox\n",
- "In a previous laboratory, we saw how to define links and how to combine them into a robotic structure, using the __Revolute__, __Prismatic__ and __DHRobot__ methods. We also saw how to calculate the end effector pose using the __fkine__, the jacobian of the robot using the __jacob0__, and how to visualise the robot using the __plot__ methods.\n",
- "\n",
- "In the examples above, it is 'easy' to calculate the inverse kinematics models by hand, but for more complex robots, we need to solve it numerically. The robotic toolbox has methods for solving the inverse kinematics model, using numerical methods.\n",
- "\n",
- "The toolbox can do this using the __ikine__ method (from inverse kinematics). The method works by providing a desired end-effector pose (position and orientation) and gives back the joint coordinates the result in the desired pose. As we know, the inverse kinematics model might have more than just one solution for a specific pose. The way numerical methods work, they start looking for a solution around a initial guess (which we can provide), and slowly converge to the joint coordinates that result in the desired pose. The convergence point might depend on the initial guess we are providing.\n",
- "\n",
- "The __ikine_LM__ method provides a vector of joint coordinates that results in the end-effector pose to be the one desired. To do so, we need to provide a pose as an input to the method. The pose is provided in the form of a 4x4 homogeneous transformation matrix. This transformation matrix provides information about the 6 degrees of freedom available (3 positions and 3 orientations).\n",
- "\n",
- "The __ikine_LM__ method can solve the inverse kinematics problem even for kinematic chains with less than 6 degrees of freedom. In that case, we need to specify for which DoFs we want the inverse kinematics problem solved. We do this by providing a __mask__ as an input, which is a vector with six logical elements (0 or 1), specifying with 1 those DoFs that we want to solve the inverse kinematics for. The number of DoFs that we can solve the inverse kinematics for __must__ be equal to the DoFs of the robot itself."
- ]
- },
- {
- "cell_type": "markdown",
- "metadata": {
- "tags": []
- },
- "source": [
- "### 5.4 Example\n",
- "\n",
- "Below, we see an example of how the __ikine_LM__ method works. \n",
- "\n",
- "* We first create a DHRobot which represents a robot with two degrees of freedom. \n",
- "* Then we create a valid pose (__T_dummy__) by setting the joint coordinates of the robot to a specific value and calculating the forward kinematics.\n",
- "* Then we try to solve the inverse kinematics for that specific pose, and we obtain the joint coordinates (__q_comp__) that would result in that specific pose.\n",
- "* We verify that the inverse kinematics give the same solution as the input to the forward kinematics\n",
- "* We can also try to solve the inverse kinematics for any input pose (example 5.2.2)\n",
- "\n",
- "For the input pose to be valid, we need to use the __SE3__ constructors from the spatial math module of the robotics toolbox. The corresponding methods are:\n",
- "\n",
- "* Translation: SE3(x,y,z)\n",
- "* Rotation on X: SE3.Rx(theta)\n",
- "* Rotation on Y: SE3.Ry(theta)\n",
- "* Rotation on Z: SE3.Rz(theta)"
- ]
- },
- {
- "cell_type": "code",
- "execution_count": null,
- "metadata": {
- "tags": []
- },
- "outputs": [],
- "source": [
- "# 5.2.1\n",
- "# example of use-case\n",
- "from roboticstoolbox import *\n",
- "from spatialmath import *\n",
- "import numpy as np\n",
- "\n",
- "# create the robot\n",
- "# theta, d, r , alpha, offset, qlim, mdh\n",
- "Link1 = RevoluteMDH(d = 0.3);\n",
- "Link2 = RevoluteMDH(alpha = np.pi/2)\n",
- "Link3 = RevoluteMDH(a = 0.3)\n",
- "\n",
- "# we combine the links using DHRobot\n",
- "rob = DHRobot([\n",
- " Link1,\n",
- " Link2,\n",
- " Link3], name = \"my_robot\", tool=SE3(2,0,0))\n",
- "q_dummy = np.array([0, np.pi/2, -np.pi/4])\n",
- "\n",
- "# test the ikine \n",
- "T = rob.fkine(q_dummy)\n",
- "print(\"Desired pose: \\n\", T)\n",
- "sol_comp = rob.ikine_LM(T, mask=np.array([1,1,1,0,0,0]))\n",
- "print('Joint coordinates that we used for generating the input pose:', q_dummy)\n",
- "print('Joint coordinates computed by inverse kinematics for the input pose:', sol_comp.q)\n",
- "\n",
- "#you can also input an initial guess q0\n",
- "sol_comp2 = rob.ikine_LM(T, q0=q_dummy+0.2, mask=np.array([1,1,1,0,0,0]))\n",
- "print('Joint coordinates computed by inverse kinematics for the input pose, with initial guess:', sol_comp2.q)"
- ]
- },
- {
- "cell_type": "code",
- "execution_count": null,
- "metadata": {
- "tags": []
- },
- "outputs": [],
- "source": [
- "# 5.2.2\n",
- "# example of use-case\n",
- "from spatialmath import *\n",
- "\n",
- "# We can also try to solve the inverse kinematics for any arbitrary pose\n",
- "# Try to play with the mask, to see that each time the solution is different\n",
- "\n",
- "T = SE3(0.2, 0.35, 2.3)\n",
- "sol = rob.ikine_LM(T, mask=np.array([1,1,1,0,0,0]))\n",
- "print('The joint coordinates that take us to pose T are: ', sol.q)\n",
- "print('The desired pose was: \\n', T, 'while the actual pose is: \\n', rob.fkine(sol.q))"
- ]
- },
- {
- "cell_type": "markdown",
- "metadata": {},
- "source": [
- "### 5.5. Proposed problems\n",
- "\n",
- "#### 1. Analytical inverse kinematics\n",
- "We consider the robotic structure with 3 degrees of freedom from figure fig 5.3, for which $l_1=0.5\\;m$.\n",
- "\n",
- "<center>\n",
- " <figure class=\"image\">\n",
- " <img src=\"artwork/IKM/mgi3.png\" width=50% />\n",
- " <figcaption>Figure 5.3: Robot RRT</figcaption>\n",
- " </figure>\n",
- "</center>\n",
- "\n",
- "* Given the direct geometric model, calculate the inverse kinematics model for X, Y, and Z position of the end-effector\n",
- "\n",
- "\\begin{equation}\n",
- "\\left[ {{\\begin{array}{*{20}c}\n",
- " c_1 & -s_1c_2 & s_1s_2 & -q_3s_1c_2 \\\\\n",
- " s_1 & c_1c_2 & -c_1s_2 & q_3c_1c_2 \\\\\n",
- " 0 & s_2 & c_2 & l_1+q_3s_2 \\\\\n",
- " 0 & 0 & 0 & 1 \\\\\n",
- "\\end{array} }} \\right]\n",
- "\\\n",
- "\\end{equation}\n",
- "\n",
- "* Calculate the joint coordinates that result in the following position for the end-effector: [2,4,0.5]"
- ]
- },
- {
- "cell_type": "markdown",
- "metadata": {},
- "source": [
- "#### 2. Numerical inverse kinematics\n",
- "\n",
- "* Check what values does the gripper need for open/close positions\n",
- "* Notice that the robot accepts positions in meters\n",
- "\n",
- "Using the AL5D robot and the robotic toolbox, implement a program that:\n",
- "* Picks a box from coordinates X=20cm, Y=-15cm, Z=2cm, and places it at coordinates X=24cm, Y=12cm, and Z=2cm.\n",
- "* Consider a rotation around Y by -80 degrees for the picking pose.\n",
- "* Consider a rotation around Z by +45 degrees for the placing pose.\n",
- "* In both cases, the rotations are relative and are being multiplied from the right (Transl * Rot).\n",
- "* Keep in mind that the box is lying flat on the table.\n",
- "* Use the ikine_LM method of the robotics toolbox module for solving the inverse kinematics for the specific poses.\n",
- "* Specify that you are interested in the 3 positions and in the Y, Z orientations when solving the inverse kinematics.\n",
- "* Then use the provided functions for controlling the joint positions so that the desired poses are reached. Keep in mind to keep a delay between any two poses."
- ]
- },
- {
- "cell_type": "code",
- "execution_count": 77,
- "metadata": {},
- "outputs": [
- {
- "data": {
- "text/plain": [
- "Swift backend, t = 0.05, scene:\n",
- " AL5D_mdw"
- ]
- },
- "execution_count": 77,
- "metadata": {},
- "output_type": "execute_result"
- }
- ],
- "source": [
- "# Importing necessary modules\n",
- "import roboticstoolbox as rtb\n",
- "import numpy as np\n",
- "from spatialmath import *\n",
- "from spatialmath.base import *\n",
- "from math import *\n",
- "\n",
- "rob = rtb.models.URDF.AL5D_mdw()\n",
- "\n",
- "q = np.array([0,0,0,0,0])\n",
- "rob.plot(q)"
- ]
- },
- {
- "cell_type": "markdown",
- "metadata": {},
- "source": [
- "When implementing the motion with the actual robot, consider the sequence described in the following figure:\n",
- " \n",
- "<center>\n",
- " <figure class=\"image\">\n",
- " <img src=\"artwork/IKM/al5d_gripping_sequence.png\" width=50% />\n",
- " <figcaption>Figure 5.4: Pick and place sequence for a robot. With green are the steps when the gripper is closed</figcaption>\n",
- " </figure>\n",
- "</center>"
- ]
- },
- {
- "cell_type": "code",
- "execution_count": 9,
- "metadata": {},
- "outputs": [],
- "source": [
- "from al5d_control import *\n",
- "from time import sleep\n",
- "\n",
- "# for sending the commands\n",
- "rrob = AL5DControl()\n",
- "\n",
- "# Solve the ikine for the respective poses\n",
- "# The solution will be in radians\n",
- "\n",
- "# Control the robot to go to each pose using the robot_control function\n",
- "# Give degrees as input for the joint values\n",
- "\n",
- "# example of using the robot_control\n",
- "# !!! USE THESE FUNCTIONS WITH YOUR OWN SOLUTION TO START THE ROBOT ONLY AFTER SIMULATION\n",
- "rrob.robot_control(0,0,0,0,0,0)\n",
- "time.sleep(4)\n",
- "rrob.robot_control(10,0,0,0,0,0)"
- ]
- }
- ],
- "metadata": {
- "kernelspec": {
- "display_name": "Python 3 (ipykernel)",
- "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.10.8"
- },
- "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
-}
diff --git a/lab06_Trajectories.ipynb b/lab06_Trajectories.ipynb
deleted file mode 100644
index 771cb8bd4a9cdcbdefb1683633f59c8234810806..0000000000000000000000000000000000000000
--- a/lab06_Trajectories.ipynb
+++ /dev/null
@@ -1,660 +0,0 @@
-{
- "cells": [
- {
- "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": {
- "tags": []
- },
- "outputs": [],
- "source": [
- "from lab_functions import *\n",
- "\n",
- "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",
- " robot = rtb.models.URDF.UR5()\n",
- " robot.plot(qs*3.131/180, \"swift\")\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",
- " robot = rtb.models.URDF.UR5()\n",
- " robot.plot(qs*3.131/180, \"swift\")\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",
- " print(qs)\n",
- " robot = rtb.models.URDF.UR5()\n",
- " robot.plot(qs*3.131/180, \"swift\")\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",
- " robot = rtb.models.URDF.UR5()\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,:] = robot.ikine_LM(setpoint).q\n",
- " else:\n",
- " qs[i,:] = robot.ikine_LM(setpoint, q0=qs[max(i-1,0),:]).q\n",
- "\n",
- " robot.plot(qs, \"swift\")\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",
- " robot = rtb.models.URDF.UR5()\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,:] = robot.ikine_LM(setpoint, q0=qs[max(i-1,0),:]).q\n",
- "\n",
- " robot.plot(qs, \"swift\")\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 as well, 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",
- " T = 8 # Total time\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",
- " robot = rtb.models.URDF.UR5()\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,:] = robot.ikine_LM(setpoint, q0=qs[max(i-1,0),:]).q\n",
- " \n",
- " robot.plot(qs, \"swift\")\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 load our robot from the toolbox\n",
- "robot = rtb.models.URDF.UR5()\n",
- "\n",
- "# We solve the inverse kinematics for the target poses\n",
- "sol = robot.ikine_LM(tr)\n",
- "\n",
- "# Animating the trajectory\n",
- "robot.plot(sol.q, backend=\"swift\")"
- ]
- },
- {
- "cell_type": "markdown",
- "metadata": {},
- "source": [
- "# Assignment\n",
- "\n",
- "## AL5D robot\n",
- "\n",
- "Using the code you constructed for the simulation above, implement a trajectory following 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",
- "Compare the results between Euler and Quaternion interpolation"
- ]
- },
- {
- "cell_type": "code",
- "execution_count": null,
- "metadata": {},
- "outputs": [],
- "source": [
- "from al5d_control import *\n",
- "import time as ti\n",
- "\n",
- "# for sending the commands\n",
- "rrob = AL5DControl()"
- ]
- },
- {
- "cell_type": "code",
- "execution_count": null,
- "metadata": {},
- "outputs": [],
- "source": [
- "# Solve the ikine for the respective interpolated poses\n",
- "# The solution will be in radians!\n",
- "\n",
- "# Control the robot to go to each pose using the robot_control function\n",
- "# Give degrees as input for the joint values\n",
- "for i in range(steps):\n",
- " rrob.robot_control(qs[i,0],qs[i,1],qs[i,2],qs[i,3],qs[i,4],0)\n",
- " ti.sleep(0.1)"
- ]
- }
- ],
- "metadata": {
- "kernelspec": {
- "display_name": "Python 3",
- "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.8.8"
- },
- "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
-}
diff --git a/lab07_DynamicModel.ipynb b/lab07_DynamicModel.ipynb
deleted file mode 100644
index 7e488eb795895945469fa7fea4c98e3d57149c9e..0000000000000000000000000000000000000000
--- a/lab07_DynamicModel.ipynb
+++ /dev/null
@@ -1,571 +0,0 @@
-{
- "cells": [
- {
- "cell_type": "markdown",
- "metadata": {},
- "source": [
- "## Laboratories 7-8-9\n",
- "\n",
- "# Robot arm process dynamic modeling \n",
- "\n",
- "### 7.1 Euler-Lagrange equations\n",
- "\n",
- "The Lagrangian is defined as\n",
- "\n",
- "\\begin{equation}\n",
- "L=K-P \\text{ (1)}\n",
- "\\end{equation}\n",
- "where $K$ represents the total kinetic energy of the system and $P$ represents the total potential energy of the system. \n",
- "The Euler-Lagrange equations that describe the dynamics of a $n-DOF$ mechanical system are:\n",
- "\n",
- "\\begin{equation}\n",
- "\\frac{d}{dt}\\frac{\\partial L}{\\partial \\dot{q}_i}-\\frac{\\partial L}{\\partial q_i}=\\tau_i, \\qquad i=1,...,n, \\text{ (2)}\n",
- "\\end{equation}\n",
- "\n",
- "where $q_i$ represent generalized coordinates (in our case the joint angles) and $\\tau_i$ generalized forces (in our case motor torques)\n",
- "\n",
- "\n",
- "The matrix form of the Euler-Lagrange equations is:\n",
- "\n",
- "\\begin{equation}\\label{robmodel}\n",
- "D(q)\\ddot{q}+C(q,\\dot{q})\\dot{q}+G(q)=\\tau \\text{ (3)}\n",
- "\\end{equation}\n",
- "\n",
- "where $q=[q_1,...,q_n]^T, \\tau=[\\tau_1,...,\\tau_n]^T$. \n",
- "\n",
- "\n",
- "The matrix $D(q)$ is called inertia matrix, it is symmetric and positive definite, and can be expressed in terms of the kinetic energy:\n",
- "\\begin{equation}\n",
- "K=\\frac{1}{2} \\dot{q}^T D(q) \\dot{q}=\\frac{1}{2}\\sum_{i,j}^{n} d_{i,j}(q)\\dot{q}_i\\dot{q}_j. \\text{ (4)}\n",
- "\\end{equation}\n",
- "\n",
- "The matrix $C(q)$ takes into account centrifugal and Coriolis terms, and each $k,j-th$ matrix element can be calculated as:\n",
- "\n",
- "\\begin{equation}\\label{cterms}\n",
- "c_{kj}=\\frac{1}{2}\\sum_{i=1}^{n} \n",
- "\\bigg \\{\n",
- "\\underbrace{\n",
- "\\frac{\\partial d_{kj}}{\\partial q_i}+\\frac{\\partial d_{ki}}{\\partial q_j}-\\frac{\\partial d_{ij}}{\\partial q_k}\n",
- "}_{c_{ijk}}\n",
- "\\bigg \\}\n",
- "\\dot{q}_i. \\text{ (5)}\n",
- "\\end{equation}\n",
- "\n",
- "The last term $G(q)$, sometimes called gravity term, is a column vector $G=[g_1...g_n]^T$, where each $k-th$ term is derived from the potential energy:\n",
- "\n",
- "\\begin{equation}\\label{grav}\n",
- "g_k(q)=\\frac{\\partial P}{\\partial q_k }, \\qquad k=1,...,n. \\text{ (6)}\n",
- "\\end{equation}"
- ]
- },
- {
- "cell_type": "markdown",
- "metadata": {},
- "source": [
- "### 7.2 A 2DOF robot arm with spatial movement\n",
- "\n",
- "Consider a 2DOF robot arm with two revolute joints, that can move in a 3D Cartesian space, with the schematic representation from Figure 7.1. Because the first rotation axis is on the X axis, and the second on the Y axis, that robot can move in a 3D space.\n",
- "\n",
- "<center>\n",
- " <figure class=\"image\">\n",
- " <img src=\"artwork/DynMod/2DOF.png\" width=40% />\n",
- " <figcaption>Figure 7.1: Schematic representation of a 2DOF robot arm </figcaption>\n",
- " </figure>\n",
- "</center>\n",
- "\n",
- "#### 7.2.1 Geometric Model\n",
- "\n",
- "The geometric model can be derived through transformation matrices from the base frame to the end effector frame. The base frame coincides with the first frame (that is the frame of joint 1, with origin $O_1$ in the center of the joint). Thus the transformation matrix $T_{01}$ is simply a rotation around X: \n",
- "\n",
- "\\begin{equation}\n",
- "T_{01}=Rot(x,q_1)=\\begin{bmatrix}\n",
- " 1 & 0 & 0 & 0\\\\\n",
- " 0 & cos(q_1) & -sin(q_1) & 0\\\\\n",
- " 0 & sin(q_1) & cos(q_1) & 0\\\\\n",
- " 0 & 0 & 0 & 1\n",
- "\\end{bmatrix}\\ \\text{ (7)}\n",
- "\\end{equation}\n",
- "\n",
- "From Frame 1 we arrive at Frame 2 (corresponding to the joint 2) through a translation on Z and a rotation around Y ($T_{12}$):\n",
- "\n",
- "\\begin{equation}\n",
- "T_{12}=Transl(z,L_1) \\cdot Rot(y,q_2)=\\begin{bmatrix}\n",
- " cos(q2) & 0 & sin(q2) & 0\\\\\n",
- " 0 & 1 & 0 & 0\\\\\n",
- " -sin(q2) & 0 & cos(q2) & L_1\\\\\n",
- " 0 & 0 & 0 & 1\n",
- "\\end{bmatrix}\\ \\text{ (8)}\n",
- "\\end{equation}\n",
- "\n",
- "Finally, the end effector frame is obtained through a translation on Z ($T_{23}$): \n",
- " \n",
- "\\begin{equation}\n",
- "T_{23}=Transl(z,L_2)=\\begin{bmatrix}\n",
- " 1 & 0 & 0 & 0\\\\\n",
- " 0 & 1 & 0 & 0\\\\\n",
- " 0 & 0 & 1\t & L_2\\\\\n",
- " 0 & 0 & 0 & 1\n",
- "\\end{bmatrix}\\ \\text{ (9)}\n",
- "\\end{equation}\n",
- "\n",
- "The transformation matrix from the based frame to the end effector, that is the geometric model is obtained through multiplication:\n",
- "\n",
- "\\begin{equation}\\label{geom}\n",
- "T=T_{03}=T_{01} \\cdot T_{12} \\cdot T_{23}= \\begin{bmatrix}\n",
- " cos(q_2)& 0& sin(q_2)& L_2sin(q_2)\\\\\n",
- " sin(q_1)sin(q_2)& cos(q_1)& -cos(q_2)sin(q_1)& -sin(q_1)(L_1 + L_2cos(q_2))\\\\\n",
- " -cos(q_1)sin(q_2)& sin(q_1)& cos(q_1)cos(q_2)& cos(q_1)(L_1 + L_2cos(q_2))\\\\\n",
- " 0& 0& 0& 1\n",
- "\\end{bmatrix} \\text{ (10)}\n",
- "\\end{equation}\n",
- "\n",
- "The position of the end effector with respect to the joint angles $q_1$ and $q_2$ is given by the first three elements of the 4th column:\n",
- "\n",
- "\\begin{equation}\n",
- "x=L_2 sin(q_1),\n",
- "y=-L_1 sin(q_1)-L_2 sin(q_1) cos(q_2),\n",
- "z= L_1 cos(q_1)+L_2 cos(q_1) cos(q_2) \\text{ (11)}\n",
- "\\end{equation}\n",
- "\n",
- "The orientation of the end effector is given by the submatrix R (lines 1-3 and columns 1-3 of T):\n",
- "\n",
- "\\begin{equation}\n",
- "R=\n",
- "\\begin{bmatrix}\n",
- " cos(q_2)& 0& sin(q_2)\\\\\n",
- " sin(q_1)sin(q_2)& cos(q_1)& -cos(q_2)sin(q_1)\\\\\n",
- " -cos(q_1)sin(q_2)& sin(q_1)& cos(q_1)cos(q_2)\n",
- "\\end{bmatrix}. \\text{ (12)}\n",
- "\\end{equation}\n",
- "\n",
- "#### 7.2.2 Jacobian\n",
- "\n",
- "The Jacobian relates the joint velocities to the linear and angular velocities of the end effector. For the 2DOF robot arm from Figure 7.1, with the geometric model from equation (10), the Jacobian is:\n",
- "\n",
- "\\begin{equation}\n",
- "J=\n",
- "\\begin{bmatrix}\n",
- " 0 & \\frac{L_2}{2}cos(q_2) \\\\\n",
- "-\\frac{L_2}{2}cos(q_1)cos(q_2)-L_1cos(q_1) & \\frac{L_2}{2}sin(q_1)sin(q_2) \\\\\n",
- "-\\frac{L_2}{2}sin(q_1)cos(q_2)-L_1sin(q_1) & -\\frac{L_2}{2}cos(q_1)sin(q_2) \\\\\n",
- " 1 & 0 \\\\ \n",
- " 0 & cos(q_1) \\\\ \n",
- " 0 & sin(q_1) \n",
- "\\end{bmatrix} \\text{ (13)}\n",
- "\\end{equation}\n",
- "\n",
- "Thus, if we refer to link 2, the angular and linear Jacobians are:\n",
- "\n",
- "\\begin{equation}\n",
- "J_{vc2}=J_{vc}=\n",
- "\\begin{bmatrix}\n",
- " 0 & \\frac{L_2}{2}cos(q_2) \\\\\n",
- "-\\frac{L_2}{2}cos(q_1)cos(q_2)-L_1cos(q_1) & \\frac{L_2}{2}sin(q_1)sin(q_2) \\\\\n",
- "-\\frac{L_2}{2}sin(q_1)cos(q_2)-L_1sin(q_1) & -\\frac{L_2}{2}cos(q_1)sin(q_2)\n",
- "\\end{bmatrix} \\text{ (14)}\n",
- "\\end{equation}\n",
- "\n",
- "\\begin{equation}\n",
- "J_{\\omega2}=J_{\\omega}=\n",
- "\\begin{bmatrix}\n",
- " 1 & 0 \\\\ \n",
- " 0 & cos(q_1) \\\\ \n",
- " 0 & sin(q_1) \n",
- "\\end{bmatrix} \\text{ (15)}\n",
- "\\end{equation} \n",
- "\n",
- "Further on, the angular and linear Jacobians for link 1 can be determined as:\n",
- "\n",
- "\\begin{equation}\n",
- "J_{vc1}=\n",
- "\\begin{bmatrix}\n",
- " 0 & 0 \\\\\n",
- "-\\frac{L_1}{2}cos(q_1) & 0 \\\\\n",
- "-\\frac{L_1}{2}sin(q_1) & 0\n",
- "\\end{bmatrix}, \\text{ (16)}\n",
- "\\end{equation}\n",
- "\n",
- "\\begin{equation}\n",
- "J_{\\omega1}=\n",
- "\\begin{bmatrix}\n",
- " 1 & 0 \\\\ \n",
- " 0 & 0 \\\\ \n",
- " 0 & 0 \n",
- "\\end{bmatrix} \\text{ (17)}\n",
- "\\end{equation}\n",
- "\n",
- "#### 7.2.3 Lagrangian\n",
- "\n",
- "The Lagrangian is composed out of kinetic energy and potential energy.\n",
- "The kinetic energy has a translational and a rotational component\n",
- "\n",
- "\\begin{equation}\n",
- "K=K_{transl}+K_{rot} \\text{ (18)}\n",
- "\\end{equation}\n",
- "\n",
- "given by the expressions:\n",
- "\\begin{equation}\n",
- "K_{transl}=\\frac{1}{2}m_1 v_{c1}^T v_{c1}+\\frac{1}{2}m_2 v_{c2}^T v_{c2}=\\frac{1}{2} \\dot{q}^T (m_1 J_{vc1}^T J_{vc1}+m_2 J_{vc2}^T J_{vc2}) \\dot{q} \\text{ (19)}\n",
- "\\end{equation}\n",
- "\n",
- "and\n",
- "\\begin{equation}\n",
- "K_{rot}=\\frac{1}{2} \\dot{q}^T(J_{\\omega2}^T R_0^2 I_2 R_0^{2_T} J_{\\omega2}+J_{\\omega1}^T R_0^1 I_1 R_0^{1_T} J_{\\omega1})\\dot{q} \\text{ (20)}\n",
- "\\end{equation}\n",
- "\n",
- "with \\begin{equation}\n",
- "R_0^2=R, \\quad R_0^1=T_0^1(1:3,1:3), I_2=diag\\{0,I_{2y},0\\},\\quad I_1=diag\\{I_{1x},0,0\\}.\n",
- "\\end{equation}\n",
- "\n",
- "After calculating the expressions for both components of the kinetic energy, we obtain the inertia matrix $D(q)$ as\n",
- "\n",
- "\\begin{equation}\n",
- "D(q)=\\begin{bmatrix}\n",
- " d_{11}& d_{12}\\\\\n",
- " d_{21}& d_{22}\n",
- "\\end{bmatrix}=\\begin{bmatrix}\n",
- " I_{1x}+\\frac{L_1^2 m_1}{4}+L_1^2m_2+\\frac{L_2^2 m_2}{4}cos^2(q_2)+L_1L_2m_2cos(q_2) & 0\\\\\n",
- " 0\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t& \\frac{m_2 L_2^2}{4}+I_{2y}\n",
- "\\end{bmatrix} \\text{ (21)}\n",
- "\\end{equation}\n",
- "\n",
- "In deriving matrix $C(q,\\dot{q})$, we first calculate each $c_{ijk}$ term from equation (5):\n",
- "\n",
- "\\begin{equation}\n",
- "\\begin{matrix}\n",
- " c_{111}=\\frac{\\partial d_{11}}{\\partial q_1}+\\frac{\\partial d_{11}}{\\partial q_1}-\\frac{\\partial d_{11}}{\\partial q_1}=0 \\\\\n",
- " c_{112}=\\frac{\\partial d_{21}}{\\partial q_1}+\\frac{\\partial d_{21}}{\\partial q_1}-\\frac{\\partial d_{11}}{\\partial q_2}=\\frac{L_2^2 m_2}{4}sin(2q_2)+L_1L_2m_2sin(q_2)\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\\\\\n",
- " c_{121}=\\frac{\\partial d_{12}}{\\partial q_1}+\\frac{\\partial d_{11}}{\\partial q_2}-\\frac{\\partial d_{12}}{\\partial q_1}=-\\frac{L_2^2 m_2}{4}sin(2q_2)-L_1L_2m_2sin(q_2) \\\\\n",
- " c_{122}=\\frac{\\partial d_{22}}{\\partial q_1}+\\frac{\\partial d_{21}}{\\partial q_2}-\\frac{\\partial d_{12}}{\\partial q_2}=0 \\\\\n",
- " c_{211}=\\frac{\\partial d_{11}}{\\partial q_2}+\\frac{\\partial d_{12}}{\\partial q_1}-\\frac{\\partial d_{21}}{\\partial q_1}=c_{121} \\\\\n",
- " c_{212}=\\frac{\\partial d_{21}}{\\partial q_2}+\\frac{\\partial d_{22}}{\\partial q_1}-\\frac{\\partial d_{21}}{\\partial q_2}=0 \\\\\n",
- " c_{221}=\\frac{\\partial d_{12}}{\\partial q_2}+\\frac{\\partial d_{12}}{\\partial q_2}-\\frac{\\partial d_{22}}{\\partial q_1}=0 \\\\\n",
- " c_{222}=\\frac{\\partial d_{22}}{\\partial q_2}+\\frac{\\partial d_{22}}{\\partial q_2}-\\frac{\\partial d_{22}}{\\partial q_2}=0\n",
- "\\end{matrix} \\text{ (22)}\n",
- "\\end{equation}\n",
- "\n",
- "In the end we obtain the matrix:\n",
- "\\begin{equation}\n",
- "C(q,\\dot{q})= \\begin{bmatrix} -\\frac{L_2^2 m_2}{8}sin(2q_2)\\dot{q}_2-\\frac{1}{2} L_1L_2m_2sin(q_2)\\dot{q}_2 & -\\frac{L_2^2 m_2}{8}sin(2q_2)\\dot{q}_1-\\frac{1}{2}L_1L_2m_2sin(q_2)\\dot{q}_1\\\\\n",
- " \\frac{L_2^2 m_2}{8}sin(2q_2)\\dot{q}_1+\\frac{1}{2}L_1L_2m_2sin(q_2)\\dot{q}_1 & 0\n",
- "\\end{bmatrix} \\text{ (23)}\n",
- "\\end{equation}\n",
- "\n",
- "\n",
- "The potential energy is determined by multiplying the mass by the gravitational acceleration and the height at the center of mass:\n",
- "\\begin{equation}\n",
- "P_1=m_1g\\frac{L_1}{2}cos(q_1), \\text{ (24)}\\\\\n",
- "P_2=m_2g \\Big (L_1cos(q_1)+\\frac{L_2}{2}cos(q_1)cos(q_2) \\Big), \\text{ (25)}\\\\ \n",
- "P=P_1+P_2. \n",
- "\\end{equation}\n",
- "\n",
- "Based on equation (6), the gravity term is determined as:\n",
- "\\begin{equation}\n",
- "G(q)=\\begin{bmatrix}\n",
- "-\\frac{m_1gL_1+2m_2gL_1}{2}sin(q_1)-\\frac{m_2gL_2}{2}sin(q_1)cos(q_2)\\\\\n",
- "-\\frac{m_2gL_2}{2}cos(q_1)sin(q_2)\n",
- "\\end{bmatrix} \\text{ (26)}\n",
- "\\end{equation}\n",
- "\n",
- "This completes the dynamic model for our robot arm."
- ]
- },
- {
- "cell_type": "markdown",
- "metadata": {},
- "source": [
- "## 7.3 Proposed problems\n",
- "\n",
- " 1. Consider the robotic structure from Fig 7.2 with only 1 degree of freedom, for which $l=1\\;m$, $m=1\\;kg$, and $I= \\begin{bmatrix} \\frac{1}{12} &0 &0 \\\\ 0 &0 &0\\\\ 0 &0 &\\frac{1}{12}\\end{bmatrix}$.\n",
- " \n",
- " \n",
- " a. Compute the equation of motion for the robot based on the Euler-Lagrange formulation.\n",
- " b. Implement the robot model in Python using the state space representation and odeint for solving the differential equation. Give the input torque as a sine wave. Interpret the results.\n",
- "<center>\n",
- " <figure class=\"image\">\n",
- " <img src=\"artwork/DynMod/1DOF.png\" width=50% />\n",
- " <figcaption>Figure 7.2: 1DOF robot </figcaption>\n",
- " </figure>\n",
- "</center>\n",
- " "
- ]
- },
- {
- "cell_type": "code",
- "execution_count": null,
- "metadata": {},
- "outputs": [],
- "source": [
- "%reset -f\n",
- "import numpy as np\n",
- "from scipy.integrate import odeint\n",
- "import matplotlib.pyplot as plt\n",
- "import roboticstoolbox as rtb\n",
- "from roboticstoolbox import *\n",
- "from spatialmath import *\n",
- "from spatialmath.base import * \n",
- "import math as m\n",
- "\n",
- "####### 1B ########\n",
- "\n",
- "# the robot model\n",
- "def model(x,t,u):\n",
- " # gravitational acceleration\n",
- " g = 9.98\n",
- " \n",
- " # states\n",
- " q = x[0]\n",
- " dq = x[1]\n",
- " \n",
- " xdot1 = \n",
- " xdot2 = \n",
- " xdot = [xdot1,xdot2]\n",
- " \n",
- " return xdot\n",
- "\n",
- "# initial condition of the state variable\n",
- "x0 = [0,0]\n",
- "\n",
- "# number of time points\n",
- "n = 101\n",
- "\n",
- "# initial and final time (seconds)\n",
- "t0 = 0\n",
- "tf = 10\n",
- "\n",
- "# time points\n",
- "t = np.linspace(t0,tf,n)\n",
- "\n",
- "# input\n",
- "u = \n",
- "\n",
- "# store solution in an array like t\n",
- "q = np.empty_like(t)\n",
- "dq = np.empty_like(t)\n",
- "\n",
- "# record initial conditions\n",
- "q[0] = x0[0]\n",
- "dq[0] = x0[1]\n",
- "\n",
- "# solve ODE for each step\n",
- "for i in range(1,n):\n",
- " # span/period for next time step\n",
- " tspan = [t[i-1],t[i]]\n",
- " # solve for next step\n",
- " x = odeint(model,x0,tspan,args=(u[i],))\n",
- " # store solution for plotting\n",
- " q[i] = x[1][0]\n",
- " dq[i] = x[1][1]\n",
- " # next initial condition\n",
- " x0 = x[1]\n",
- "\n",
- "################ plot results\n",
- "# plt.plot(t,u,'g:',label='u(t)')\n",
- "# plt.plot(t,q,'b-',label='q(t)')\n",
- "# plt.plot(t,dq,'r--',label='dq(t)')\n",
- "# plt.ylabel('values')\n",
- "# plt.xlabel('time')\n",
- "# plt.legend(loc='best')\n",
- "# plt.show()"
- ]
- },
- {
- "cell_type": "code",
- "execution_count": null,
- "metadata": {},
- "outputs": [],
- "source": [
- "############### animation \n",
- "q = q.reshape(n,1)\n",
- "l = 1\n",
- "robot = DHRobot([ RevoluteMDH(alpha=m.pi/2, offset=m.pi/2), RevoluteMDH(d=l, alpha=m.pi/2), ], base=SE3(0.6, 0, 0), name = '1DOF robot')\n",
- "zers = np.zeros_like(q)\n",
- "coord = np.concatenate((q,zers),axis=1)\n",
- "robot.plot(coord, movie='1dof.gif', dt=tf/n, shadow=False)"
- ]
- },
- {
- "cell_type": "markdown",
- "metadata": {
- "jupyter": {
- "source_hidden": true
- },
- "tags": []
- },
- "source": [
- " 2. Consider the 2DOF robotic structure from Figure 7.3, for which $L1=L2=1 \\;m$, $m1=m2=1\\;kg$. The q1 and q2 initial conditions are $\\pi$/4 and 1.\n",
- " \n",
- "\n",
- " a. Find the DGM using the D-H convention and create the robot using the robotics toolbox library DHRobot.\n",
- " b. Compute the C, D, and G matrices. \n",
- " c. Implement in Python the robot model using the state space representation, with odeintw (wrapper of odeint for working with matrices) for solving the MIMO diferential equations. Give two sine waves as the input joint torques. \n",
- " d. Plot the positions and velocities compared with the inputs. Interpret the results.\n",
- " \n",
- "<center>\n",
- " <figure class=\"image\">\n",
- " <img src=\"artwork/DynMod/2DOFstud.png\" width=50% />\n",
- " <figcaption>Figure 7.3: 2DOF robot </figcaption>\n",
- " </figure>\n",
- "</center>\n",
- "\n",
- " 3. Take a look at the third cell, containing code on how to do the dynamics with the toolbox applied on the AL5D_mdw. Run it and see its results. "
- ]
- },
- {
- "cell_type": "code",
- "execution_count": null,
- "metadata": {
- "code_folding": [],
- "jupyter": {
- "source_hidden": true
- },
- "tags": []
- },
- "outputs": [],
- "source": [
- "%reset -f\n",
- "import numpy as np\n",
- "import matplotlib.pyplot as plt\n",
- "import roboticstoolbox as rtb\n",
- "import math as m\n",
- "from roboticstoolbox import *\n",
- "from spatialmath import *\n",
- "from spatialmath.base import * \n",
- "from lab_functions import lim_prismatic\n",
- "from odeintw import *\n",
- "\n",
- "########## 2C, 2D ############\n",
- "\n",
- "pr_lim = 10\n",
- "\n",
- "def model(x,t,u):\n",
- " \n",
- " \n",
- " u = u.reshape(2,1) # reshaping for mathematical operations\n",
- " \n",
- " q = x[:,0] #first column of states x, the positions\n",
- " dq = x[:,1] #second column of states x, the velocities\n",
- " lim_prismatic(q,dq,pr_lim)\n",
- " \n",
- " \n",
- " D = \n",
- "\n",
- " C =\n",
- " \n",
- " G = \n",
- " \n",
- " xdot1 = \n",
- " xdot2 = \n",
- " \n",
- " xdot = np.concatenate((xdot1, xdot2), axis=1)\n",
- "\n",
- " return xdot\n",
- "\n"
- ]
- },
- {
- "cell_type": "code",
- "execution_count": null,
- "metadata": {
- "jupyter": {
- "source_hidden": true
- },
- "tags": []
- },
- "outputs": [],
- "source": [
- "# 3. AL5D toolbox example\n",
- "\n",
- "import roboticstoolbox as rtb\n",
- "import numpy as np\n",
- "import matplotlib.pyplot as plt\n",
- "from math import pi \n",
- "\n",
- "# import the dh model of the Al5d_mdw\n",
- "rob = rtb.models.DH.AL5D_mdw()\n",
- "q = np.array([0,0,0,0,0])\n",
- "qd = q\n",
- "\n",
- "print(rob.dynamics())\n",
- "\n",
- "# G, M, C matrices at position and velocity q and qd\n",
- "G = rob.gravload(q)\n",
- "M = rob.inertia(q)\n",
- "C = rob.coriolis(q, qd)\n",
- "\n",
- "# inputs sent to the model defined in this function\n",
- "def no_torque_func(rob, t, q, qd):\n",
- " return np.zeros((rob.n, ))\n",
- "\n",
- "tg = rob.fdyn(T=5, q0=q, dt=0.01, torque=no_torque_func)\n",
- " \n",
- "# plotting the joint coordinates \n",
- "plt.figure(2)\n",
- "plt.plot(tg.t, tg.q[:,0], 'b', label='q1')\n",
- "plt.plot(tg.t, tg.q[:,1], 'r', label='q2')\n",
- "plt.plot(tg.t, tg.q[:,2], 'g', label='q3')\n",
- "plt.plot(tg.t, tg.q[:,3], 'm', label='q4')\n",
- "plt.plot(tg.t, tg.q[:,4], 'y', label='q5')\n",
- "plt.legend(loc='best')\n",
- "plt.ylabel('values')\n",
- "plt.xlabel('time')\n",
- "plt.grid(True)\n",
- "\n",
- "\n",
- "# # 3D visualisation of the al5d_mdw\n",
- "# rob = rtb.models.URDF.AL5D_mdw()\n",
- "# rob.plot(tg.q)"
- ]
- }
- ],
- "metadata": {
- "kernelspec": {
- "display_name": "Python 3 (ipykernel)",
- "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.10.8"
- },
- "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
-}
diff --git a/lab10_ControlDesign.ipynb b/lab10_ControlDesign.ipynb
deleted file mode 100644
index 4188ab376d3672b9b3a92e90065353766ba22e0c..0000000000000000000000000000000000000000
--- a/lab10_ControlDesign.ipynb
+++ /dev/null
@@ -1,382 +0,0 @@
-{
- "cells": [
- {
- "cell_type": "markdown",
- "metadata": {},
- "source": [
- "## Laboratory 10\n",
- "\n",
- "# Control design of a robot arm process\n",
- "\n",
- "## 10.1 Independent Joint Control\n",
- "\n",
- "Independent joint control is the classical control approach, where a controller is designed for each individual joint (fig 10.1}). The effect of other joints is considered as a disturbance that the controller has to reject.\n",
- "\n",
- "<br />\n",
- "<br />\n",
- "\n",
- "<center>\n",
- " <figure class=\"image\">\n",
- " <img src=\"artwork/control/Control_1.png\" width=40% />\n",
- " <figcaption>Figure 10.1: Independent joint control - PD controllers</figcaption>\n",
- " </figure>\n",
- "</center>\n",
- "\n",
- "<br />\n",
- "<br />\n",
- "\n",
- "Let us first rewrite a 2DOF model in terms of each individual joint $i$ :\n",
- "\n",
- "\\begin{equation}\\label{decoupledmodel}\n",
- "d_{ii}\\ddot{q}_i+c_{ii}\\dot{q}_i=\\tau_i-w_i , \\quad i=1,2,..,n \\text{ (1)}\n",
- "\\end{equation}\n",
- "\n",
- "where $d_{ii}$ includes only the constant diagonal terms of $D'(q)$, while the disturbance term $w_i$ includes all other $i^{th}$ terms of $D'(q)$, and $i^{th}$ components of $C(q,\\dot{q})$ and $G(q)$. It is important to notice that if the the Coriolis and centripetal terms are not very large, then the process dynamics can be approximated well by _n decoupled linear second-order systems_."
- ]
- },
- {
- "cell_type": "markdown",
- "metadata": {},
- "source": [
- "### 10.1.1 Control problem\n",
- "\n",
- "Design a linear controller for each joint that ensure tracking ($q$ tracks a reference signal $q_r$) and disturbance \n",
- "rejection ($w_{p}$).\n",
- "\n",
- "Consider standard PD controllers\n",
- "\\begin{equation}\\label{pd}\n",
- "\\tau_i=K(K_{di}\\dot{e}_i+K_{pi}e_i) \\text{ (2)}\n",
- "\\end{equation}\n",
- "where $e_i$ represents the tracking error, defined as $e_i=q_{ri}-q_{i}$, and\n",
- "$\\dot{e}_i=\\dot{q}_{di}-\\dot{q}_{i}$ is the error of the derivatives.\n",
- "\n",
- "If we consider the case of set-point tracking, that is $\\dot{q}_{ri}=0$, and replace eq(2) in eq(1) we obtain:\n",
- "\n",
- "\\begin{equation}\\label{simpleprocess}\n",
- "d_{ii}\\ddot{q}_i(t)+(c_{ii}+KK_{di})\\dot{q}_i(t)+KK_{pi}q_i(t)=KK_{pi}q_{ri}(t)-w_{pi}(t) , \\quad i=1,2,..,n \\text{ (3)}\n",
- "\\end{equation}\n",
- "\n",
- "By applying the Laplace transform we get the transfer function relating the outputs ($q_i$) to the reference and disturbance inputs ($q_{ri}$ and $w_{pi}$):\n",
- "\\begin{equation}\\label{tf}\n",
- "q_i(s)=\\frac{KK_{pi}}{d_{ii}s^2+(c_{ii}+KK_{di})s+KK_{pi}}q_{ri}(s)-\\frac{1}{d_{ii}s^2+(c_{ii}+KK_{di})s+KK_{pi}}w_{pi}(s), \\quad i=1,2,..,n \\text{ (4)}\n",
- "\\end{equation}\n",
- "\n",
- "The characteristic equations are \n",
- "\\begin{equation}\n",
- "{d_{ii}s^2+(c_{ii}+KK_{di})s+KK_{pi}}=0, \\text{ (5)}\n",
- "\\end{equation}\n",
- "which can also be written as\n",
- "\\begin{equation}\n",
- "s^2+\\frac{c_{ii}+KK_{di}}{d_{ii}}s+\\frac{ KK_{pi}}{d_{ii}}=0. \\text{ (6)}\n",
- "\\end{equation}\n",
- "\n",
- "Because the standard second order equation is given by\n",
- "\\begin{equation}\n",
- "s^2+2 \\zeta_i \\omega_{ni}s+\\omega^2_{ni}=0, \\text{ (7)}\n",
- "\\end{equation}\n",
- "\n",
- "the controller parameters can be expressed in terms of damping ratio $\\zeta$ and natural frequency $\\omega_n$:\n",
- "\n",
- "\\begin{equation}\\label{pdgain}\n",
- "K_{pi}=\\frac{d_{ii}\\omega^2_{ni}}{K} , \\quad K_{di}=\\frac{2\\zeta_i \\omega_{ni} d_{ii}-c_{ii}}{K} \\text{ (8)}\n",
- "\\end{equation}\n",
- "\n",
- "Usually $\\zeta$ is set to 1 (critical damping), and $\\omega_n$ is chosen as high\n",
- "as possible. One possible limitation in the value adopted for $\\omega_n$ is the\n",
- "input torque $\\tau_i$ saturation.\n",
- "The control strategy proves very efficient in practice. If we further want to force a very small or null steady state error $e_{ss}$, then we can either adopt PID type controllers\n",
- "\n",
- "\\begin{equation}\\label{pid}\n",
- "\\tau_i=K_{di}\\dot{e}_i+K_{pi}e_i+K_{ii}\\int_{0}^{t} e\\,dt, \\text{ (9)}\n",
- "\\end{equation}\n",
- "\n",
- "or PD controllers with an additional gravity term. Note that in this case the control signals are not decoupled anymore.\n",
- "\n",
- "\\begin{equation}\\label{pdgrav}\n",
- "\\tau_i=K_{di}\\dot{e}_i+K_{pi}e_i+G'_i(q) \\text{ (10)}\n",
- "\\end{equation}"
- ]
- },
- {
- "cell_type": "markdown",
- "metadata": {},
- "source": [
- "## 10.3 Numerical results\n",
- "\n",
- "### 10.3.1 Robot arm example\n",
- "\n",
- "As an example of a 2DOF robot with the structure as in Figure 7.1 (from Dynamic Model laboratory), consider the robot from Figure 10.4., that we model using eq(10.1).\n",
- "\n",
- "The parameters are (either measured or estimated) :$L_1=0.095 \\ m$, $L_2=0.1 \\ m$, $m_1=0.095 \\ kg$, $m_2=0.37\\ kg$, $g=9.81 \\ m/s^2$, $I_{1x}=2.27 \\ 10^{-2} \\ kg \\ m^2$, $I_{2y}=2.27 \\ 10{-2} \\ kg \\ m^2$, $b_1=0.24$, $b_2=0.16$, $r=1$. The torque control signal is limited to the range $[-1.18,1.18] \\, Nm$.\n",
- "\n",
- "<center>\n",
- " <figure class=\"image\">\n",
- " <img src=\"artwork/control/Control_4.png\" width=40% />\n",
- " <figcaption>Figure 10.4: Real robot arm process </figcaption>\n",
- " </figure>\n",
- "</center>"
- ]
- },
- {
- "cell_type": "markdown",
- "metadata": {},
- "source": [
- "### 10.3.2 Independent Joint Control\n",
- "\n",
- "Here we will design PD controller for independent joint controller\n",
- "\n",
- "\\begin{equation}\n",
- "\\tau_1=K_{d1}\\dot{e}_1+K_{p1}e_1,\n",
- "\\tau_2=K_{d2}\\dot{e}_2+K_{p2}e_2, \\text{ (21)}\n",
- "\\end{equation}\n",
- "\n",
- "for the robot process eq(10.1), with the parameters given in previous section. We will use the formula eq(7.5), with $B=0, J_{p1}=0.0263$,$J_{p2}=0.0236$, $\\zeta=1$, $K=1$. We started from a value for $\\omega_n=\\omega_{n1}=\\omega_{n2}$ of 0.1, an increased it until the step response of the closed loop system is fast enough, and the control torques reach saturation for a small time interval. In the end, a value of $12$ provided good enough results.\n",
- "\n",
- "Figure 10.5 shows simulations results of the closed loop system with step reference signals, while Figure 10.6 shows the control torques. The results show that a small steady state is present. This can be eliminated by adding a gravity term as in eq(10.10). The results with PD+gravity control are shown in Figure 10.7. It can be noticed that now the steady state error is zero. Note that in the case of PD+gravity controller, the controller is no longer joint independent, due to the gravity terms that contain expressions in both joint variables $q_1$ and $q_2$.\n",
- "\n",
- "A more interesting and demanding tracking scenario is that when the reference signals are sinusoidal ($q_r(t)=sin(t)$), which is shown Figure 10.8 for the PD+gravity controllers. Although the steady state error is now exactly zero, because the reference continuously changes, the error is kept into very small limits. \n",
- "\n",
- "<center>\n",
- " <figure class=\"image\">\n",
- " <img src=\"artwork/control/Control_5.png\" width=70% />\n",
- " <figcaption>Figure 10.5: Simulations with PD independent joint control - step response </figcaption>\n",
- " </figure>\n",
- "</center>\n",
- "\n",
- "<center>\n",
- " <figure class=\"image\">\n",
- " <img src=\"artwork/control/Control_6.png\" width=70% />\n",
- " <figcaption>Figure 10.6: Control torques for PD independent joint control </figcaption>\n",
- " </figure>\n",
- "</center>\n",
- "\n",
- "<center>\n",
- " <figure class=\"image\">\n",
- " <img src=\"artwork/control/Control_7.png\" width=70% />\n",
- " <figcaption>Figure 10.7:Simulations with PD+Gravity joint control - step response </figcaption>\n",
- " </figure>\n",
- "</center>\n",
- "\n",
- "<center>\n",
- " <figure class=\"image\">\n",
- " <img src=\"artwork/control/Control_8.png\" width=70% />\n",
- " <figcaption>Figure 10.8: Simulations with PD+Gravity joint control - sinusoidal response </figcaption>\n",
- " </figure>\n",
- "</center>"
- ]
- },
- {
- "cell_type": "markdown",
- "metadata": {},
- "source": [
- "## 10.4 Proposed problems\n",
- "\n",
- " 1. Consider the AL5D_mdw robot with known matrices D,C,G given in the code. Implement an Independent Joint Control with PD controllers.\n",
- " \n",
- " a. Use step of amplitude 0.1 and interpret the plots.\n",
- " b. Adapt the natural frequencies and observe their influence on the movement and position of the robot.\n",
- " c. Add an integrator and tune its gain empirically, for a PID; intepret the plots.\n",
- " d. Use a sine wave of amplitude 0.5 as input joint trajectories interpret the plots."
- ]
- },
- {
- "cell_type": "code",
- "execution_count": null,
- "metadata": {
- "code_folding": []
- },
- "outputs": [],
- "source": [
- "%reset -f\n",
- "import numpy as np\n",
- "import matplotlib.pyplot as plt\n",
- "from odeintw import *\n",
- "import math as m\n",
- "from math import pi\n",
- "import roboticstoolbox as rtb\n",
- "from roboticstoolbox import *\n",
- "from lab_functions import *\n",
- "plt.style.use('rcs.mplstyle')\n",
- "\n",
- "# loading robot model\n",
- "rob = rtb.models.DH.AL5D_mdw()\n",
- "\n",
- "# real system of al5d\n",
- "def model(x,t,tau): \n",
- " tau = tau.reshape(5,1) # reshaping for mathematical operations\n",
- " \n",
- " q = x[:,0] #first column of x, the states\n",
- " dq = x[:,1] #second column of x, the states derivated\n",
- "\n",
- " G = rob.gravload(q).reshape(5,1)\n",
- " D = rob.inertia(q)\n",
- " C = rob.coriolis(q, dq)\n",
- " \n",
- " xdot1 = dq.reshape(5,1)\n",
- " xdot2 = np.matmul(np.linalg.inv(D), (tau - C.dot(dq).reshape(5,1) - G))\n",
- " \n",
- " xdot = np.concatenate((xdot1, xdot2), axis=1)\n",
- "\n",
- " return xdot\n",
- "\n",
- "# time step\n",
- "dt = 0.01\n",
- "\n",
- "# final time\n",
- "tf = 5\n",
- "\n",
- "# nr of samples\n",
- "n = int(np.round(tf/dt))\n",
- "\n",
- "# Desired time samples for the solution. \n",
- "# np.arrange - returns evenly spaced values within a given interval.\n",
- "t = np.arange(0, tf, dt)\n",
- "\n",
- "# step reference\n",
- "sp = 0.1*np.heaviside(t, 0)\n",
- "\n",
- "# initialisation\n",
- "q = np.zeros((n,5))\n",
- "dq = np.zeros((n,5))\n",
- "tau = np.zeros((n,5))\n",
- "\n",
- "# x0 is the initial condition of the state space\n",
- "x0 = np.zeros((5,2))\n",
- "\n",
- "# natural frequencies and damping coefficient\n",
- "wn1, wn2, wn3, wn4, wn5 = 10, 10, 10, 10, 10\n",
- "zeta = 1\n",
- "\n",
- "# solve ODE for each step\n",
- "for i in range(2,n):\n",
- " # progress bar for visualisation of elapsed time\n",
- " printProgressBar((i+1)/n, prefix=\"Progress:\", suffix=\"complete\", length=60)\n",
- "\n",
- " # span for next time step\n",
- " tspan = [t[i-1],t[i]] \n",
- "\n",
- " # compute torques from the control law\n",
- " tau[i,:] = \n",
- "\n",
- " # solve for next step\n",
- " x = odeintw(model,x0,tspan,args=(tau[i,:],))\n",
- "\n",
- " # store solution for plotting\n",
- " q[i,:] = x[1][:,0]\n",
- " dq[i,:] = x[1][:,1]\n",
- "\n",
- " # next initial condition \n",
- " x0 = x[1]"
- ]
- },
- {
- "cell_type": "code",
- "execution_count": null,
- "metadata": {},
- "outputs": [],
- "source": [
- "# plots \n",
- "fig = plt.figure()\n",
- "fig.set_size_inches(18.5, 18.5)\n",
- "\n",
- "plt.subplot(3, 1, 1)\n",
- "plt.plot(t, q[:,0], label='q1')\n",
- "plt.plot(t, q[:,1], label='q2')\n",
- "plt.plot(t, q[:,2], label='q3')\n",
- "plt.plot(t, q[:,3], label='q4')\n",
- "plt.plot(t, q[:,4], label='q5')\n",
- "plt.plot(t, sp, label='ref')\n",
- "plt.legend(loc='best')\n",
- "plt.ylabel('values')\n",
- "plt.xlabel('time')\n",
- "plt.grid(True)\n",
- "\n",
- "plt.subplot(3, 1, 2)\n",
- "plt.plot(t, tau[:,0], label='tau1')\n",
- "plt.plot(t, tau[:,1], label='tau2')\n",
- "plt.plot(t, tau[:,2], label='tau3')\n",
- "plt.plot(t, tau[:,3], label='tau4')\n",
- "plt.plot(t, tau[:,4], label='tau5')\n",
- "plt.legend(loc='best')\n",
- "plt.ylabel('commands')\n",
- "plt.xlabel('time')\n",
- "plt.grid(True)\n",
- "\n",
- "plt.subplot(3, 1, 3)\n",
- "plt.plot(t, sp-q[:,0], label='err1')\n",
- "plt.plot(t, sp-q[:,1], label='err2')\n",
- "plt.plot(t, sp-q[:,2], label='err3')\n",
- "plt.plot(t, sp-q[:,3], label='err4')\n",
- "plt.plot(t, sp-q[:,4], label='err5')\n",
- "plt.legend(loc='best')\n",
- "plt.ylabel('errors')\n",
- "plt.xlabel('time')\n",
- "plt.grid(True)\n",
- "\n",
- "plt.show() "
- ]
- },
- {
- "cell_type": "code",
- "execution_count": null,
- "metadata": {},
- "outputs": [],
- "source": [
- "# animation\n",
- "\n",
- "rob = rtb.models.URDF.AL5D_mdw()\n",
- "rob.plot(q, backend=\"swift\")"
- ]
- }
- ],
- "metadata": {
- "kernelspec": {
- "display_name": "Python 3",
- "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.8.8"
- },
- "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
-}
diff --git a/lab11_ControlDesign-CTC.ipynb b/lab11_ControlDesign-CTC.ipynb
deleted file mode 100644
index e63a7858b868b2194ded5bbba3b58456caa051c1..0000000000000000000000000000000000000000
--- a/lab11_ControlDesign-CTC.ipynb
+++ /dev/null
@@ -1,498 +0,0 @@
-{
- "cells": [
- {
- "cell_type": "markdown",
- "metadata": {},
- "source": [
- "## Laboratory 11\n",
- "\n",
- "# Control design of a robot arm process\n",
- "\n",
- "## 11.1 Computed Torque Control\n",
- "\n",
- "Computed torque control (in some places called feedback linearization control) is a more sophisticated and modern control strategy that can be used to increase the control performances. The control consists out of an inner feedback loop and an outer feedback loop: fig.11.1. Although it is a nonlinear control approach, because the inner feedback loop achieves dynamic linearization, the outer feedback loop resume to a classical linear control design. \n",
- "\n",
- "<br>\n",
- "<br>\n",
- "\n",
- "<center>\n",
- " <figure class=\"image\">\n",
- " <img src=\"artwork/control/Control_2.png\" width=60% />\n",
- " <figcaption>Figure 11.1: Computed torque control - feedback linearization - principal control structure </figcaption>\n",
- " </figure>\n",
- "</center>\n",
- "\n",
- "<br>\n",
- "<br>\n",
- "\n",
- "Consider the robot arm process model:\n",
- "\n",
- "\\begin{equation}\\label{lastmodel}\n",
- "D(q)\\ddot{q}+C(q,\\dot{q})\\dot{q}+G(q)=\\tau \\text{ (1)}\n",
- "\\end{equation}\n",
- "\n",
- "By the change of notation $V(q,\\dot{q})=C(q,\\dot{q})\\dot{q}+G(q)$ we can rewrite the model more compactly as\n",
- "\n",
- "\\begin{equation}\n",
- "D(q)\\ddot{q}+V(q,\\dot{q})=\\tau \\text{ (2)}\n",
- "\\end{equation}\n",
- "\n",
- "Consider the following control law for the inner control loop:\n",
- "\\begin{equation}\\label{controlaw}\n",
- "\\tau=D(q)(\\ddot{q}_r-u)+V(q,\\dot{q}) \\text{ (3)}\n",
- "\\end{equation}\n",
- "\n",
- "We define the tracking error as\n",
- "\\begin{equation}\\label{error}\n",
- "e=q_r-q. \\text{ (4)}\n",
- "\\end{equation}\n",
- "Then $\\dot{e}=\\dot{q}_r-\\dot{q} \\quad and \\quad \\ddot{e}=\\ddot{q}_r-\\ddot{q}$.\n",
- "\n",
- "By replacing the torque from eq(3) in model eq(2), and using the definition eq(4), we obtain:\n",
- "\n",
- "\\begin{equation}\\label{doubleintegrator}\n",
- "\\ddot{e}=u \\text{ (5)}\n",
- "\\end{equation}\n",
- "\n",
- "Thus the inner feedback loop achieves dynamic linearization, in other words the outer loop \"sees\" a double integrator process.\n",
- "The double integrator model eq(5) can be written in state space form:\n",
- "\n",
- "\\begin{equation}\n",
- "\\dot{x_e}=Ax_e+Bu \\text{ (6)}\n",
- "\\end{equation}\n",
- "\n",
- "with \n",
- "\n",
- "\\begin{equation}\n",
- "x_e=\\begin{bmatrix}\n",
- "e\\\\\n",
- "\\dot{e}\n",
- "\\end{bmatrix},\n",
- "A=\\begin{bmatrix}\n",
- "0_n & I_n \\\\\n",
- "0_n & 0_n \\\\\n",
- "\\end{bmatrix},\n",
- "B=\\begin{bmatrix}\n",
- "0_n \\\\\n",
- "I_n\n",
- "\\end{bmatrix} \\text{ (7)}\n",
- "\\end{equation}\n",
- "\n",
- "where $x$ is a $m\\times1$ matrix, $n$ being the number of inputs, $m = 2n$ being the number of states .\n",
- "\n",
- "Now the outer loop can be designed by any classical linear control technique. We will consider here a state feedback control law with integrator component:\n",
- "\n",
- "\\begin{equation}\\label{statefeedback}\n",
- "u=-Kx+K_i\\epsilon \\text{ (8)}\n",
- "\\end{equation}\n",
- "\n",
- "where $\\epsilon$ is the output of the integrator ($\\dot{\\epsilon}=e$) with dimension $n\\times1$ - see Fig:11.2. The multiplication of the integrator term is done element by element, $K_i$ being a $n\\times1$ column vector as well. The multiplication of the state feedback term is a matrix multiplication, with K of dimension $n\\times m$.\n",
- "\n",
- "\n",
- "We will first design the state feedback gain K using Ackerman's formula for a $m^{th}$ order system:\n",
- "\n",
- "$$K=[\\underbrace{0_n \\, ... 0_n \\, }_{\\substack{\\textrm{m-1 times}}}I_n]M^{-1}_c \\Delta(A)\\text{ (9)}$$\n",
- "\n",
- "where $\\Delta$ is the desired characteristic polynomial of the closed loop system\n",
- "\n",
- "\\begin{equation}\n",
- "\\Delta(s)=s^{m}+a_{m-1}s^{m-1}+...+a_1s^1+a_0, \\text{ (10)}\n",
- "\\end{equation}\n",
- "\n",
- "and $M_c$ is the controllability matrix of dimension $m\\times nm$:\n",
- "\n",
- "\\begin{equation}\n",
- "M_c=[B|AB|A^2B|... |A^{m-1}B]. \\text{ (11)}\n",
- "\\end{equation}\n",
- "\n",
- "After the state feedback gain K is designed through pole placement, we will consider that the integrator gain can be designed through trial and error in simulations, in order the achieve the best tracking performances possible, while taking into account nonlinear effects like saturation, dead-zones or backlash.\n",
- "\n",
- "Finally, the detailed control structure is shown in Figure 11.2.\n",
- "\n",
- "<br>\n",
- "<center>\n",
- " <figure class=\"image\">\n",
- " <img src=\"artwork/control/Control_3.png\" width=60% />\n",
- " <figcaption>Figure 11.2: Computed torque control - feedback linearization - detailed control structure </figcaption>\n",
- " </figure>\n",
- "</center>"
- ]
- },
- {
- "cell_type": "markdown",
- "metadata": {},
- "source": [
- "## 11.2 Numerical results\n",
- "\n",
- "### Robot arm example\n",
- "\n",
- "As an example of a 2DOF robot with the structure as in Figure 7.1 (from Dynamic Model laboratory).\n",
- "\n",
- "The parameters are (either measured or estimated) :$L_1=0.095 \\ m$, $L_2=0.1 \\ m$, $m_1=0.095 \\ kg$, $m_2=0.37\\ kg$, $g=9.81 \\ m/s^2$, $I_{1x}=2.27 \\ 10^{-2} \\ kg \\ m^2$, $I_{2y}=2.27 \\ 10{-2} \\ kg \\ m^2$, $b_1=0.24$, $b_2=0.16$, $r=1$. The torque control signal is limited to the range $[-1.18,1.18] \\, Nm$.\n",
- "\n",
- "<center>\n",
- " <figure class=\"image\">\n",
- " <img src=\"artwork/control/Control_4.png\" width=40% />\n",
- " <figcaption>Figure 11.3: Real robot arm process </figcaption>\n",
- " </figure>\n",
- "</center>"
- ]
- },
- {
- "cell_type": "markdown",
- "metadata": {},
- "source": [
- "### Control simulation\n",
- "\n",
- "Consider the inner feedback loop with control law eq(3), and the outer feedback loop with control law eq(8) (Figure 11.1)\n",
- "\n",
- "We will design the state feedback gain K through pole placement. Consider the following closed loop pole configuration [-4 -4 -9 -9] (desired eigenvalues for $A-BK$). The poles where chosen such that we get an overdamped and fast enough response, while avoiding saturation as much as possible. As the poles are moved farther to the left in the complex plane, the response becomes faster, but the control effort increases. \n",
- "\n",
- "\\begin{equation}\n",
- "K=\\begin{bmatrix}\n",
- "36 & 0 & 13 & 0\\\\\n",
- "0 & 36 & 0 & 13\n",
- "\\end{bmatrix} \\text{ (22)}\n",
- "\\end{equation}\n",
- "\n",
- "\n",
- "The integrator gain $K_i$ is found through trial and error in simulations:\n",
- "we increase the gain starting from an initial value of 0.1 - until the maximum steady state error starts to increase. We keep the previous iterated value. Thus, we finally arrive at the value $0.6$ (that is $K_i=[0.6 \\quad 0.6]$). \n",
- "\n",
- "For a better comparison between the performances of the PD+gravity control approach versus the computed torque control, we reconsider the sinusoidal reference input from the previous section, but now we will look directly at the tracking errors defined as $e_1=q_{r1}-q_1, \\, e_2=q_{r2}-q_2$. Figure 11.4 shows the tracking errors for the two joint positions in the case of PD+gravity control. The maximum error is about 0.05 rad (2.8 deg). For the Computed Torque control the results are shown in Figure 11.15. Notice that the maximum error is now much smaller 0.01 rad (0.5 deg). Of course that the cost is an increase complexity for the controller - the implementation requires a considerable increase in computational power.\n",
- "\n",
- "<center>\n",
- " <figure class=\"image\">\n",
- " <img src=\"artwork/control/Control_9.png\" width=70% />\n",
- " <figcaption>Figure 11.4:Tracking error for PD$+$grav control </figcaption>\n",
- " </figure>\n",
- "</center>\n",
- "\n",
- "<center>\n",
- " <figure class=\"image\">\n",
- " <img src=\"artwork/control/Control_10.png\" width=70% />\n",
- " <figcaption>Figure 11.5: Tracking error for Computer Torque Control </figcaption>\n",
- " </figure>\n",
- "</center>"
- ]
- },
- {
- "cell_type": "markdown",
- "metadata": {},
- "source": [
- "## 11.3 Proposed problems\n",
- "\n",
- " 1. Consider the same AL5D_mdw robot. Implement a Computed Torque Control with state feedback controllers and an outer loop integrator.\n",
- " 2. Make some small changes in the model estimation (so not the real model) to check if the controller is still able to keep good performances. \n",
- "\n",
- "For the first exercise, there are some steps to be taken into consideration. \n",
- "\n",
- "We first have to eliminate the nonlinear element from the model. In order to do that, we remodel the equation of the torques using the D and V matrices (figure 11.6 and 11.7), which will later linearize the system. For that, we can just define 2 functions that would take the positions and velocities as input and return their respective terms (the following code cell).\n",
- " \n",
- "<br>\n",
- "\n",
- "<center>\n",
- " <figure class=\"image\">\n",
- " <img src=\"artwork/control/Control_3_2.png\" width=50% />\n",
- " <figcaption>Figure 11.6: D term </figcaption>\n",
- " </figure>\n",
- "</center>\n",
- "\n",
- "<center>\n",
- " <figure class=\"image\">\n",
- " <img src=\"artwork/control/Control_3_1.png\" width=50% />\n",
- " <figcaption>Figure 11.7: V term </figcaption>\n",
- " </figure>\n",
- "</center>"
- ]
- },
- {
- "cell_type": "code",
- "execution_count": null,
- "metadata": {},
- "outputs": [],
- "source": [
- "# model estimation\n",
- "# run this cell in order to load the functions in the workspace\n",
- "\n",
- "def Dterm(rob, x, a):\n",
- " \"\"\" function computing the D term = D*acceleration vector\"\"\"\n",
- " \n",
- " q = x[:,0] #first column of x, the states\n",
- " dq = x[:,1] #second column of x, the states derivated\n",
- "\n",
- " D = rob.inertia(q)\n",
- " \n",
- " return D@a\n",
- "\n",
- "def Vterm(rob, x):\n",
- " \"\"\" function computing the V term = C*velocity vector + G\"\"\"\n",
- "\n",
- " q = x[:,0] #first column of x, the states\n",
- " dq = x[:,1] #second column of x, the states derivated\n",
- "\n",
- " C = rob.coriolis(q, qd)\n",
- " G = rob.gravload(q).reshape(5,1)\n",
- " \n",
- " return (C@dq).reshape(5,1) + G"
- ]
- },
- {
- "cell_type": "markdown",
- "metadata": {},
- "source": [
- "After that, since we have a linearised system, we can try to just use a state feedback controller (with the error and error derivatives as states) for the inner control loop (fig. 11.8) and an integrator term for the outer control loop(fig. 11.9).\n",
- "\n",
- "\n",
- "<center>\n",
- " <figure class=\"image\">\n",
- " <img src=\"artwork/control/Control_3_3.png\" width=50% />\n",
- " <figcaption>Figure 11.8: Inner loop controller </figcaption>\n",
- " </figure>\n",
- "</center>\n",
- "\n",
- "<center>\n",
- " <figure class=\"image\">\n",
- " <img src=\"artwork/control/Control_3_4.png\" width=50% />\n",
- " <figcaption>Figure 11.9: Outer loop controller </figcaption>\n",
- " </figure>\n",
- "</center>"
- ]
- },
- {
- "cell_type": "code",
- "execution_count": null,
- "metadata": {
- "code_folding": []
- },
- "outputs": [],
- "source": [
- "import roboticstoolbox as rtb\n",
- "from roboticstoolbox import *\n",
- "import numpy as np\n",
- "import matplotlib.pyplot as plt\n",
- "from math import pi \n",
- "from scipy.integrate import odeint\n",
- "import math as m\n",
- "from odeintw import *\n",
- "from spatialmath import *\n",
- "from spatialmath.base import *\n",
- "from scipy.signal import place_poles \n",
- "from lab_functions import *\n",
- "plt.style.use('rcs.mplstyle')\n",
- "\n",
- "# load the robot in workspace\n",
- "rob = rtb.models.DH.AL5D_mdw()\n",
- "\n",
- "# real model\n",
- "def model(x,t,tau):\n",
- " g = 9.8\n",
- " \n",
- " tau = tau.reshape(5,1) # reshaping for mathematical operations\n",
- "\n",
- " q = x[:,0] #first column of x, the states\n",
- " dq = x[:,1] #second column of x, the states derivated\n",
- "\n",
- " G = rob.gravload(q).reshape(5,1)\n",
- " D = rob.inertia(q)\n",
- " C = rob.coriolis(q, dq)\n",
- "\n",
- " xdot1 = dq.reshape(5,1)\n",
- " xdot2 = np.matmul(np.linalg.inv(D), (tau - C.dot(dq).reshape(5,1) - G))\n",
- " \n",
- " xdot = np.concatenate((xdot1, xdot2), axis=1)\n",
- "\n",
- " return xdot\n",
- "\n",
- "# time interval/sampling time\n",
- "dt = 0.01\n",
- "\n",
- "# final time\n",
- "tf = 10\n",
- "\n",
- "# nr of samples\n",
- "n = int(np.round(tf/dt))\n",
- "\n",
- "# Desired time samples for the solution.\n",
- "t = np.arange(0, tf, dt)\n",
- "\n",
- "# trajectory planning\n",
- "sp_q = 0.5*np.sin(t)\n",
- "dsp_q = 0.5*np.cos(t)\n",
- "ddsp_q = -sp_q\n",
- "\n",
- "# x0 is the initial condition of the states\n",
- "x0 = np.zeros((5,2))\n",
- "\n",
- "# initialisations\n",
- "q = np.zeros((n,5))\n",
- "dq = np.zeros((n,5))\n",
- "ddq = np.zeros((n,5))\n",
- "tau = np.zeros((n,5))\n",
- "u = np.zeros((n,5))\n",
- "\n",
- "# pole placement for inner loop state feedback\n",
- "# syntax for place_poles from scipy: place_poles(A,B,p)\n",
- "# where A and B are 2 dimensional arrays taken from the state space representation, p is an array of desired poles\n",
- "A = # from equation 7\n",
- "B = # from equation 7\n",
- "p = # from system theory I, II\n",
- "K = \n",
- "\n",
- "# Define and tune by hand integrator gain\n",
- "Ki = \n",
- "\n",
- "# solve ODE for each step\n",
- "for i in range(1,n):\n",
- " printProgressBar((i+1)/n, prefix=\"Progress:\", suffix=\"complete\", length=60)\n",
- "\n",
- " # span for next time step\n",
- " tspan = [t[i-1],t[i]]\n",
- " \n",
- " # calculate total error (integral) for each joint\n",
- " terror = \n",
- " \n",
- " # calculate integrator term as in equation 8\n",
- " integrator_term = \n",
- " \n",
- " # calculate state feedback from error and derivative of error as in equation 8\n",
- " state_feedback = \n",
- "\n",
- " # calculate state feedback with integration as in equation 8\n",
- " u[i,:] = \n",
- "\n",
- " # linearising inner controller as in equation 3\n",
- " tau[i,:] = \n",
- " \n",
- " # solve for next step\n",
- " x = odeintw(model,x0,tspan,args=(tau[i,:],))\n",
- "\n",
- " # store solution for plotting\n",
- " q[i,:] = x[1][:,0]\n",
- " dq[i,:] = x[1][:,1]\n",
- " \n",
- " # next initial condition \n",
- " x0 = x[1]"
- ]
- },
- {
- "cell_type": "code",
- "execution_count": null,
- "metadata": {},
- "outputs": [],
- "source": [
- "plt.style.use('rcs.mplstyle')\n",
- "fig = plt.figure()\n",
- "fig.set_size_inches(18.5, 18.5)\n",
- "\n",
- "plt.subplot(4, 1, 1)\n",
- "plt.plot(t, q[:,0], label='q1')\n",
- "plt.plot(t, q[:,1], label='q2')\n",
- "plt.plot(t, q[:,2], label='q3')\n",
- "plt.plot(t, q[:,3], label='q4')\n",
- "plt.plot(t, q[:,4], label='q5')\n",
- "plt.plot(t, sp_q, label='ref')\n",
- "plt.legend(loc='best')\n",
- "plt.ylabel('values')\n",
- "plt.xlabel('time')\n",
- "plt.grid(True)\n",
- "\n",
- "plt.subplot(4, 1, 2)\n",
- "plt.plot(t, tau[:,0], label='tau1')\n",
- "plt.plot(t, tau[:,1], label='tau2')\n",
- "plt.plot(t, tau[:,2], label='tau3')\n",
- "plt.plot(t, tau[:,3], label='tau4')\n",
- "plt.plot(t, tau[:,4], label='tau5')\n",
- "plt.legend(loc='best')\n",
- "plt.ylabel('computed torques sent to process')\n",
- "plt.xlabel('time')\n",
- "plt.grid(True)\n",
- "\n",
- "plt.subplot(4, 1, 3)\n",
- "plt.plot(t, sp_q-q[:,0], label='err1')\n",
- "plt.plot(t, sp_q-q[:,1], label='err2')\n",
- "plt.plot(t, sp_q-q[:,2], label='err3')\n",
- "plt.plot(t, sp_q-q[:,3], label='err4')\n",
- "plt.plot(t, sp_q-q[:,4], label='err5')\n",
- "plt.legend(loc='best')\n",
- "plt.ylabel('errors')\n",
- "plt.xlabel('time')\n",
- "plt.grid(True)\n",
- "\n",
- "plt.subplot(4, 1, 4)\n",
- "plt.plot(t, u[:,0], label='cmd1')\n",
- "plt.plot(t, u[:,1], label='cmd2')\n",
- "plt.plot(t, u[:,2], label='cmd3')\n",
- "plt.plot(t, u[:,3], label='cmd4')\n",
- "plt.plot(t, u[:,4], label='cmd5')\n",
- "plt.legend(loc='best')\n",
- "plt.ylabel('commands calculated sent to nonlinear controller')\n",
- "plt.xlabel('time')\n",
- "plt.grid(True)\n",
- "plt.show()"
- ]
- },
- {
- "cell_type": "code",
- "execution_count": null,
- "metadata": {},
- "outputs": [],
- "source": [
- "# animation\n",
- "\n",
- "rob = rtb.models.URDF.AL5D_mdw()\n",
- "rob.plot(q, backend=\"swift\")"
- ]
- }
- ],
- "metadata": {
- "kernelspec": {
- "display_name": "Python 3 (ipykernel)",
- "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.10.8"
- },
- "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
-}
diff --git a/lab12_ControlDesign-Drones.ipynb b/lab12_ControlDesign-Drones.ipynb
deleted file mode 100644
index 820d8b25bf6748bfe733116035dbc339b46c2e6f..0000000000000000000000000000000000000000
--- a/lab12_ControlDesign-Drones.ipynb
+++ /dev/null
@@ -1,832 +0,0 @@
-{
- "cells": [
- {
- "cell_type": "markdown",
- "metadata": {},
- "source": [
- "## Laboratory 12\n",
- "\n",
- "# Nonlinear Dynamical Model of a Quadrotor\n",
- "\n",
- "## 12.1 Introduction\n",
- "\n",
- "Flight dynamics can be modelled using the following two approaches: using the equation of energy conservation and the Euler-Lagrange formalism, or describing the movement (rotation and translation) of a rigid body in an inertial space, based on the Newton-Euler approach. The former procedure determines the translational dynamics in a simple way, however the rotational dynamics remains at a higher level of description. On the other hand, the Newton-Euler approach\n",
- "provides a complete relationship between rotor speed inputs and the translational and rotational dynamics\n",
- "of the vehicle.\n",
- "\n",
- "Modelling dynamics commonly starts from the assumption of a single-rigid-body representation. However, when focusing e.g. only on the kinematics of the vehicle, a point mass model is also enough. On the other hand, more complex architectures (e.g. flapping-wing) may require non-rigid-body models. Dealing with single-rigid-body models, it is also common to account for additional dynamics separately. Specific to multirotors and helicopters, propeller blade flapping and induced drag are taken further into consideration.\n",
- "\n",
- "Consider a Qadcopter Drone from Figure 12.1. The goal is to model and control the drone and test the results in simulations.\n",
- "\n",
- "<center>\n",
- " <figure class=\"image\">\n",
- " <img src=\"artwork/drone/0.PNG\" width=50% />\n",
- " <figcaption>Figure 12.1: Parrot AR 2.0 Drone </figcaption>\n",
- " </figure>\n",
- "</center>"
- ]
- },
- {
- "cell_type": "markdown",
- "metadata": {},
- "source": [
- "## 12.2 Basic notations and transformations\n",
- "\n",
- "Concerning remotely controlled vehicles, two basic coordinate systems are relevant: a reference frame $\\mathcal{E}$ , called Earth frame and the body frame $\\mathcal{B}$, the frame attached to the quadrotor. Earth frame $\\mathcal{E}$ is an inertial frame of reference with its origin fixed to the home location. The body frame $\\mathcal{B}$ is the coordinate system attached to the center of mass of the quadrotor, as shown in Fig.11.2: the axes indicate the ahead (x axis), to the left (y axis), and upwards (z axis) directions. The kinematics of the vehicle are presented in the Earth frame, whereas the dynamics are studied with respect to the body frame. Whenever the frame is relevant in case of a variable, it will be marked by superscripts like $(\\cdot)^E$ or $(\\cdot)^B$.\n",
- "\n",
- "<center>\n",
- " <figure class=\"image\">\n",
- " <img src=\"artwork/drone/1.png\" width=60% />\n",
- " <figcaption>Figure 12.2: Quadrotor model </figcaption>\n",
- " </figure>\n",
- "</center>\n",
- "\n",
- "The pose of the vehicle in the Earth frame $\\mathcal{E}$ is defined as:\n",
- "\n",
- "\\begin{equation}\\label{pose}\n",
- "P^E = [\\xi^E, \\eta^E]^T = [x,y,z,\\phi, \\theta, \\psi]^T \\text{ (1)}\n",
- "\\end{equation}\n",
- "\n",
- "where $\\xi^E = [x, y, z]^T $ marks the position and $\\eta^E = [\\phi, \\theta, \\psi]^T$ the orientation. The orientation is expressed using the Euler angles roll ($\\phi$), pitch ($\\theta$), yaw ($\\psi$).\n",
- "\n",
- "The velocities of the quadrotor are defined in the body frame $\\mathcal{B}$:\n",
- "\n",
- "\\begin{equation}\\label{velocities}\n",
- "V^B = [\\rho^B, \\pi^B]^T = [u,v,w,p,q,r]^T \\text{ (2)}\n",
- "\\end{equation}\n",
- "\n",
- "To transform a vector defined in a frame into the another frame, the rotational matrix R can be used:\n",
- "\n",
- "\\begin{equation}\n",
- "R_B^E(\\eta)=\\begin{bmatrix}\n",
- "c(\\theta)c(\\psi) & c(\\psi)s(\\theta)s(\\phi) - c(\\phi)s(\\psi) & s(\\phi)s(\\psi) + c(\\phi)c(\\psi)s(\\theta) \\\\\n",
- "c(\\theta)s(\\psi) & c(\\phi)c(\\psi) + s(\\theta)s(\\phi)s(\\psi) & c(\\phi)s(\\theta)s(\\psi) - c(\\psi)s(\\phi) \\\\\n",
- "-s(\\theta) & c(\\theta)s(\\phi) & c(\\theta)c(\\phi)\n",
- "\\end{bmatrix} \\text{ (3)}\n",
- "\\end{equation}\n",
- "\n",
- "where $s(\\cdot)$ and $c(\\cdot)$ are shorthand notations for the sine and cosine operators, and $R_B^E$ marks that the rotation is performed from body frame to Earth frame. However, since R is an orthogonal matrix, we have the property that $R^{-1} = R^T$ , due to which the transpose of $R_B^E$ is $R_E^B$.\n",
- "\n",
- "The relationship between velocities $\\rho^B$ and $\\dot{\\xi}^E$ can be written using $R_B^E$:\n",
- "\n",
- "\\begin{equation}\\label{rhoandxi}\n",
- "\\dot{\\xi}^E = R_B^E \\cdot \\rho^B \\text{ (4)}\n",
- "\\end{equation}\n",
- "\n",
- "To relate angular velocities $\\pi^B$ and $\\dot{\\eta}^E$, one can use the inverse of the translational matrix T:\n",
- "\n",
- "\\begin{equation}\n",
- "T_E^B(\\eta)=\\begin{bmatrix}\n",
- "1 & 0 & -s(\\theta) \\\\\n",
- "0 & c(\\phi) & c(\\theta)s(\\phi) \\\\\n",
- "0 & -s(\\phi) & c(\\theta)c(\\phi)\n",
- "\\end{bmatrix} \\text{ (5)}\n",
- "\\end{equation}\n",
- "\n",
- "Since $T_E^B$ is not orthogonal, calculating its inverse one obtains\n",
- "\n",
- "\\begin{equation}\n",
- "T_B^E(\\eta)=\\begin{bmatrix}\n",
- "1 & s(\\phi)t(\\theta) & c(\\phi)t(\\theta) \\\\\n",
- "0 & c(\\phi) & -s(\\phi) \\\\\n",
- "0 & -s(\\phi)c(\\theta) & c(\\phi)/c(\\theta)\n",
- "\\end{bmatrix} \\text{ (6)}\n",
- "\\end{equation}\n",
- "\n",
- "where $t(\\cdot)$ denotes the tangent operator. Then, we have:\n",
- "\n",
- "\\begin{equation}\\\n",
- "\\dot{\\eta}^E = T_B^E \\cdot \\pi^B \\text{ (7)}\n",
- "\\end{equation}\n",
- "\n",
- "Note that if working with small angles, i.e. in near-hovering mode of the quadrotor, matrix T can be simplified to the identity matrix, which makes $\\dot{\\eta}^E = \\pi^B$"
- ]
- },
- {
- "cell_type": "markdown",
- "metadata": {},
- "source": [
- "# 12.3 Quadrotor dynamics. Euler-Lagrange approach\n",
- "\n",
- "The dynamical model that we present builds upon the near-hovering behaviour of a quadrotor. Also, the quadrotor is modelled as a single rigid body, having a symmetrical structure, with the origin of the body frame in the center of gravity. Furthermore, the propellers are assumed to be rigid objects.\n",
- "\n",
- "The Euler-Lagrange approach starts with defining the Lagrangian of the quadrotor. Using vectors $P^E$, $\\xi^E$ and $\\eta^E$ from (1), the Lagrangian of a quadrotor can be written as\n",
- "\n",
- "\\begin{equation}\\\n",
- "L(P^E,\\dot{P^E}) = E_{transl} + E_{rot} - E_{pot} = 0.5m\\dot{\\xi}^T\\dot{\\xi} + 0.5\\dot{\\eta}^TJ(\\eta)\\dot{\\eta}+mgz \\text{ (8)}\n",
- "\\end{equation}\n",
- "\n",
- "with the three terms the total translational, rotational and potential (gravitational) energy acting on the\n",
- "quadrotor, and $J$ the inertia matrix with respect to the Euler angles $\\eta$:\n",
- "\n",
- "\\begin{equation}\n",
- "J(\\eta)=T_E^B(\\eta)^T\\begin{bmatrix}\n",
- "Ixx & 0 & 0 \\\\\n",
- "0 & Iyy & 0 \\\\\n",
- "0 & 0 & Izz\n",
- "\\end{bmatrix}T_E^B(\\eta) = \\begin{bmatrix}\n",
- "I_x & 0 & -I_xs(\\theta) \\\\\n",
- "0 & I_yc^2(\\phi)+I_zs^2(phi) & (I_y - I_z)c(\\phi)s(phi)c(theta) \\\\\n",
- "-I_xs(\\theta) & (I_y - I_z)x(\\phi)s(phi)c(theta) & I_xs^2(\\theta)+I_ys^2(phi)c^2(theta)+I_zc^2(phi)c^2(theta)\n",
- "\\end{bmatrix} \\text{ (9)}\n",
- "\\end{equation}\n",
- "\n",
- "Ix,Iy and Iz are the moments of inertia corresponding to each axis, and $T_E^B$ is the translational matrix from frame $\\mathcal{E}$ to frame $\\mathcal{B}$.\n",
- "\n",
- "The dynamic equations of motion can be formulated using the Euler-Lagrange equation:\n",
- "\n",
- "\\begin{equation}\n",
- "\\frac{d}{dt}\\frac{\\partial L}{\\partial \\dot{P^E}}-\\frac{\\partial L}{\\partial P^E}=\\begin{bmatrix}\n",
- "F_{transl} \\\\\n",
- "F_{rot} \n",
- "\\end{bmatrix} \\text{ (10)}\n",
- "\\end{equation}\n",
- "\n",
- "with $F_{transl}$ the translational and Frot rotational forces acting on the quadrotor. Now, as there are no cross terms between the translational and rotational parts in the above relation, using the definition of the Lagrangian, (10) can be decomposed in the following two relations:\n",
- "\n",
- "\\begin{equation}\n",
- "F_{transl} = \\frac{d}{dt}\\frac{\\partial (E_{transl}-E_{pot})}{\\partial \\dot{P^E}}-\\frac{\\partial (E_{transl}-E_{pot})}{\\partial P^E} \\text{ (11)}\n",
- "\\end{equation}\n",
- "\n",
- "\\begin{equation}\n",
- "F_{rot} = \\frac{d}{dt}\\frac{\\partial E_{rot}}{\\partial \\dot{P^E}}-\\frac{\\partial E_{rot}}{\\partial P^E} \\text{ (12)}\n",
- "\\end{equation}\n",
- "\n",
- "which after some mathematical operations results in:\n",
- "\n",
- "\\begin{equation}\n",
- "F_{transl} = m\\ddot{\\xi} + \\begin{bmatrix}\n",
- "0 \\\\\n",
- "0 \\\\\n",
- "mg\n",
- "\\end{bmatrix} \\text{ (13)}\n",
- "\\end{equation}\n",
- "\n",
- "\\begin{equation}\n",
- "F_{rot} = J(\\eta)\\ddot{\\eta} + \\dot{J}(\\eta)\\dot{\\eta} - 0.5 \\frac{d}{d\\eta}(\\dot{\\eta}^TJ(\\eta)\\dot{\\eta}) = J(\\eta)\\ddot{\\eta} + C(\\eta, \\dot{\\eta})\\dot{\\eta} \\text{ (14)}\n",
- "\\end{equation}\n",
- "\n",
- "where $C(\\eta, \\dot{\\eta})$ denotes the Coriolis term in the rotational dynamics, and can be expressed as:\n",
- "\n",
- "\\begin{equation}\n",
- "C(\\eta, \\dot{\\eta})=\\begin{bmatrix}\n",
- "c_{11} & c_{12} & c_{13} \\\\\n",
- "c_{21} & c_{22} & c_{23} \\\\\n",
- "c_{31} & c_{32} & c_{33}\n",
- "\\end{bmatrix} \\text{ (15)}\n",
- "\\end{equation}\n",
- "\n",
- "with system of equations $(16)$:\n",
- "\n",
- "\\begin{equation}\\\n",
- "c_{11} = 0 \n",
- "\\end{equation} \n",
- "\n",
- "\\begin{equation}\\\n",
- "c_{12} = (I_y - I_z)\\dot{\\theta}c(\\phi)s(\\phi) + \\dot(\\psi)s^2(\\phi)c(\\theta) + (I_z - I_y)\\dot{\\psi}c^2(\\phi)c(\\theta) - I_x\\dot{\\psi}c(\\theta)\n",
- "\\end{equation} \n",
- "\n",
- "\\begin{equation}\\\n",
- "c_{13} = (I_z - I_y)\\dot{\\psi}c^2(\\theta)s(\\phi)c(\\phi)\n",
- "\\end{equation} \n",
- "\n",
- "\\begin{equation}\\\n",
- "c_{21} = (I_z - I_y)\\dot{\\theta}s(\\phi)c(\\phi) + \\dot{\\psi}s^2(\\phi)c(\\theta) + (I_y - I_z)\\dot{\\psi}c^2(\\phi)c(\\theta) + I_x\\dot{\\psi}c(\\theta)\n",
- "\\end{equation} \n",
- "\n",
- "\\begin{equation}\\\n",
- "c_{22} = (I_z - I_y)\\dot{\\phi}c^2(\\phi)s(\\phi) \n",
- "\\end{equation} \n",
- "\n",
- "\\begin{equation}\\\n",
- "c_{23} = -I_x\\dot{\\psi}s(\\theta)c(\\theta) + I_y\\dot{\\psi}s^2(\\phi)s(\\theta)c(\\theta) + I_z\\dot{\\psi}c^2(\\phi)c(\\theta)\n",
- "\\end{equation} \n",
- "\n",
- "\\begin{equation}\\\n",
- "c_{31} = (I_y - I_z)\\dot{\\psi}c^2(\\theta)s(\\phi)c(\\phi) - I_x\\dot{\\theta}c(\\theta)\n",
- "\\end{equation} \n",
- "\n",
- "\\begin{equation}\\\n",
- "c_{32} = (I_z - I_y)(\\dot{\\theta}s(\\phi)c(\\phi)s(\\theta) + \\dot(\\psi)s^2(\\phi)c(\\theta)) + (I_y - I_z)\\dot{\\psi}c^2(\\phi)c(\\theta) + I_x\\dot{\\psi}s(\\theta)c(\\theta) - I_y\\dot{\\psi}s^2(\\phi)s(\\theta)c(\\theta) - I_z\\dot{\\psi}c^2(\\phi)s(\\theta)c(\\theta)\n",
- "\\end{equation} \n",
- "\n",
- "\\begin{equation}\\\n",
- "c_{33} = (I_y - I_z)\\dot{\\psi}c^2(\\theta)s(\\phi)c(\\phi) + I_x\\dot{\\theta}s(\\theta)c(\\theta) - I_y\\dot{\\theta}s^2(\\phi)s(\\theta)c(\\theta) - I_z\\dot{\\theta}c^2(\\phi)s(\\theta)c(\\theta)\n",
- "\\end{equation} \n",
- "\n",
- "Now, for modelling the near-hovering operation, the translational forces can be\n",
- "summarized as $F_{transl} = R_B^E \\cdot [0,0,U_{coll}]^T$ . On the other hand, $F_{rot} = [U_{\\phi}, U_{\\theta}, U_{\\psi}]$. From these relations, the dynamics of the quadrotor in near-hovering operation can be summarized as:\n",
- "\n",
- "$$\\begin{equation}\n",
- "\\left\\{ \n",
- " \\begin{aligned}\n",
- " \\ddot{x} = [c(\\phi)s(\\theta)c(\\psi) + s(\\phi)s(\\psi)]\\frac{U_{coll}}{m}\\\\\n",
- " \\ddot{y} = [c(\\phi)s(\\theta)s(\\psi) - s(\\phi)c(\\psi)]\\frac{U_{coll}}{m}\\\\\n",
- " \\ddot{z} = -g +c(\\phi)c(\\theta)\\frac{U_{coll}}{m}\\\\\n",
- " \\begin{bmatrix}\n",
- "\\ddot{\\phi} \\\\\n",
- "\\ddot{\\theta} \\\\\n",
- "\\ddot{\\psi}\n",
- "\\end{bmatrix} = J^{-1}(\\eta)\\begin{bmatrix}\n",
- "U_{\\phi} \\\\\n",
- "U_{\\theta} \\\\\n",
- "U_{\\psi}\n",
- "\\end{bmatrix} - J^{-1}(\\eta)C(\\eta, \\dot{\\eta})\\dot{\\eta} \\\\\n",
- " \\end{aligned}\n",
- " \\right.\n",
- "\\end{equation} \\text{ (17)}\n",
- "$$\n",
- "\n",
- "This model can be simplified considering that the near-hovering operation means small variations of angle $\\alpha$, thus $sin(\\alpha) = \\alpha$ and $cos(\\alpha) = 1$. Also, $U_{coll}$can be expressed as $U_{coll} = mg + \\Delta U_{coll}$ where the variation of the collective forces is also assumed to be small. Thus, (17) can be simplified as:\n",
- "\n",
- "$$\\begin{equation}\n",
- "\\left\\{ \n",
- " \\begin{aligned}\n",
- " \\ddot{x} = \\theta g\\\\\n",
- " \\ddot{y} = -\\phi g\\\\\n",
- " \\ddot{z} = \\frac{\\Delta{U_{coll}}}{m}\\\\\n",
- " \\ddot{\\phi} = \\frac{1}{I_x}U_{\\phi}\\\\\n",
- " \\ddot{\\theta} = \\frac{1}{I_y}U_{\\theta}\\\\\n",
- " \\ddot{\\psi} = \\frac{1}{I_z}U_{\\psi}\n",
- " \\end{aligned}\n",
- " \\right.\n",
- "\\end{equation} \\text{ (18)}\n",
- "$$"
- ]
- },
- {
- "cell_type": "markdown",
- "metadata": {},
- "source": [
- "## 12.4 Achieving flight\n",
- "\n",
- "The dynamical model of a quadrotor accounts for inputs resulting from the forces and torques acting on the vehicle. In other words, the model takes as inputs the yawing, pitching and rolling of the quadrotor, and the collective lifting force. The relation between these inputs and individual motor torques and forces depends on the platform configuration in use. \n",
- "\n",
- "Quadrotors are commonly modelled using one of the two configurations presented in Fig. 12.3: “plus” configuration, when the quadrotor arms are aligned with the x and y axes; and “cross” configuration, when the quadrotor is rotated with 45° around the z axis compared to the previous configuration. Both configurations are valid and in use. The latter one is preferred when an ahead-looking camera is mounted on the cross, because the forward direction corresponds to the view direction of the camera. \n",
- "\n",
- "Motor numberings and corresponding angular velocities, marked with $\\omega_i$, are presented in Fig 12.3\n",
- "\n",
- "<center>\n",
- " <figure class=\"image\">\n",
- " <img src=\"artwork/drone/2.png\" width=100% />\n",
- " <figcaption>Figure 12.3: Quadrotor configuration: plus(left) and cross(right) </figcaption>\n",
- " </figure>\n",
- "</center>\n",
- "\n",
- "Define the forces and torques acting on each rotor as:\n",
- "\n",
- "\\begin{equation}\\\n",
- "F_i = b \\cdot \\omega_i^2 \\text{ (19)}\n",
- "\\end{equation} \n",
- "\n",
- "\\begin{equation}\\\n",
- "\\tau_i = d \\cdot \\omega_i^2 \\text{ (20)}\n",
- "\\end{equation} \n",
- "\n",
- "with $b$ the thrust coefficient, $d$ the drag coefficient, and $i$ the index of the rotor. Both the forces and torques act upwards along the z axis.\n",
- "\n",
- "For both configurations, vertical displacement results from the collective force generated by the rotors, achieved with $\\omega_1 = \\omega_2 = \\omega_3 = \\omega_4$. Depending on the magnitude of the generated forces compared to gravity, the quadrotor can be lifted, landed or kept hovering. \n",
- "\n",
- "All the rotations result from the generated torques. Yaw rotation (changing angle $\\psi$) results from the sum of the torques, and is achieved by having $\\omega_1 = \\omega_3$ and $\\omega_2 = \\omega_4$. In most cases of near hovering flights, the flight direction is altered by controlling the yaw angle of the vehicle.\n",
- "\n",
- "In the plus configuration, horizontal displacement on the $x$ axis can be achieved by pitch rotation (changing angle $\\theta$), which results from keeping $\\omega_1 = \\omega_3$ and setting $\\omega_2 \\neq \\omega_4$, i.e. a torque difference on rotors 2 and 4. For instance, having $\\omega_4 > \\omega_2$, the quadrotor will turn clockwise the $y$ axis. Keeping the new pitch angle constant, the quadrotor will start to fly along the $x$ axis. Displacement on the $y$ axis is obtained similarly, by performing roll rotation (changing angle $\\phi$), with $\\omega_2 = \\omega_4$ and $\\omega_1 \\neq \\omega_3$.\n",
- "\n",
- "In the cross configuration, rolling is performed by setting $\\omega_1 = \\omega_4$ and $\\omega_2 = \\omega_3$, while pitching with $\\omega_1 = \\omega_2$ and $\\omega_3 = \\omega_4$. The $x$ and $y$ axes displacements are obtained accordingly, keeping a constant pitch (for $x$ displacement) or roll (for $y$ displacement) of the vehicle.\n",
- "\n",
- "Summarizing these intuitive explanations, using eq(19) and eq(20) the control inputs of a quadrotor in plus configuration can be written as:\n",
- "\n",
- "$$\\begin{equation}\n",
- "\\left\\{ \n",
- " \\begin{aligned}\n",
- " U_{coll} = F_1 + F_2 + F_3 + F_4\\\\\n",
- " U_{\\phi} = l(F_1 - F_3)\\\\\n",
- " U_{\\theta} = l(F_2 - F_4)\\\\\n",
- " U_{\\psi} = \\tau_1 + \\tau_3 -\\tau_2 -\\tau_4\\\\\n",
- " \\end{aligned}\n",
- " \\right.\n",
- "\\end{equation} \\text{ (21)}\n",
- "$$\n",
- "\n",
- "where _l_ marks the distance between a rotor and the origin of frame $\\mathcal{B}$. In case of the plus configuration, the roll and pitch torques act on an arm of length $l \\cdot 1$.\n",
- "\n",
- "In the cross configuration, the inputs are defined as:\n",
- "\n",
- "$$\\begin{equation}\n",
- "\\left\\{ \n",
- " \\begin{aligned}\n",
- " U_{coll} = F_1 + F_2 + F_3 + F_4\\\\\n",
- " U_{\\phi} = \\frac{\\sqrt{2}}{2}l(F_1 + F_4 - F_2 - F_3)\\\\\n",
- " U_{\\theta} = \\frac{\\sqrt{2}}{2}l(F_1 - F_4 + F_2 - F_3)\\\\\n",
- " U_{\\psi} = \\tau_1 + \\tau_3 -\\tau_2 -\\tau_4\\\\\n",
- " \\end{aligned}\n",
- " \\right.\n",
- "\\end{equation} \\text{ (22)}\n",
- "$$\n",
- "\n",
- "Note that the roll and pitch torques act on an arm of $\\frac{\\sqrt{2}}{2}l$ due to the cross configuration.\n",
- "\n",
- "In both configurations, the quadrotor is underactuated since the x and y axes displacement can be controlled only through rolling and pitching the vehicle. In other words, only four out of the six degrees of freedom of the vehicle are directly controllable."
- ]
- },
- {
- "cell_type": "markdown",
- "metadata": {},
- "source": [
- "## 12.5 AR.Drone parameters\n",
- "\n",
- "We intend to use the dynamical model for controlling the AR.Drone 2.0 quadrotor. The models introduced in the previous section require some parameters, namely: the moments of inertia of the quadrotor, thrust and drag coefficients, the mass of the vehicle and the length of the axes that connect and hold the rotors. The parameters are summarized from literature in Table 1.\n",
- "\n",
- "<center>\n",
- " <figure class=\"image\">\n",
- " <img src=\"artwork/drone/tab1.PNG\" width=90% />\n",
- " <figcaption>Table 1: Parrot AR.Drone 2.0 parameters</figcaption>\n",
- " </figure>\n",
- "</center>\n",
- "\n",
- "The parameters depend on the actual quadrotor for which they were measured. In this context, the weight of the vehicle depends on the equipment used onboard. We measured the weight of two AR.Drone 2.0 quadrotors by measuring the major components. Results are shown in Table 2.\n",
- "\n",
- "<center>\n",
- " <figure class=\"image\">\n",
- " <img src=\"artwork/drone/tab2.PNG\" width=90% />\n",
- " <figcaption>Table 2: Parrot AR.Drone 2.0 parts weight measurements</figcaption>\n",
- " </figure>\n",
- "</center>\n",
- "\n",
- "Looking at Table 2, one may notice minor differences in the vehicle fuselage and outdoor hull weights. The difference is greater in case of the batteries, where the 1500 mAh battery is somewhat heavier, as expected, than the weaker battery. The difference in the weight of the two indoor hulls appears due to\n",
- "the rebalancing performed with the second hull, repair that added some weight to the hull. Based on these measurements, using e.g. the first quadrotor with the indoor hull and the GPS module one gets a total weight of 0.503 kg.\n",
- "\n",
- "### 12.5.1 Control inputs and platform inputs\n",
- "\n",
- "The control inputs of the dynamical model result from the forces and torques acting on the rotors. In terms, both the torques and forces are derived from the angular velocity of the propellers. Thus, to find the relationship between the control inputs and platform control commands, we need to relate propeller\n",
- "angular velocities and control commands sent to the quadrotor motors.\n",
- "\n",
- "According to the official product specifications, the motors are powered at P = 15 W, and have and RPM range between 10350 and 41400 RPM. Hovering is achieved at 28000 RPM. The motors of the AR.Drone 2.0 are commanded in PWM using control inputs $u_i$, defined in the range of 0–255. Thus, first we need to relate the input PWM values to the RPM of the motors. Denote the motor RPMs as $RPM_m$.\n",
- "\n",
- "Assuming a linear relationship between RPMm and ui, we obtain the input constant $c = 41400/255 = 162.35$. In our laboratory setup we can check this ratio only for the hovering state, for which we obtain from the recorded navigation data an average control input of 175. Thus, 28000/175 = 160 which confirms the almost linear relationship between motor RPM and PWM control\n",
- "inputs. Since flight is achieved in near-hovering mode, very small inputs are not interesting. Thus, the hovering state ratio can be taken, i.e. we use the input constant $c = 160$. Then, for each motor $i$ one gets:\n",
- "\n",
- "\\begin{equation}\n",
- "RPM_m = x \\cdot u_i \\text{ (23)}\n",
- "\\end{equation}\n",
- "\n",
- "As we are interested in the angular velocity of the propellers, the motor-propeller gear ratio has to be considered, which is 8:68 in case of the AR.Drone quadrotor:\n",
- "\n",
- "\\begin{equation}\n",
- "RPM_p = RPM_m \\cdot 8/68 \\text{ (24)}\n",
- "\\end{equation}\n",
- "\n",
- "where RPMp represents the RPM of the propellers. Converting RPM to angular velocity, we can write:\n",
- "\n",
- "\\begin{equation}\n",
- "\\omega_i = 2\\pi/60 \\cdot RPM_p \\text{ (25)}\n",
- "\\end{equation}\n",
- "\n",
- "Summarizing the above relations and replacing the known constants, finally one can write the direct relation between motor angular velocities and control inputs:\n",
- "\n",
- "\\begin{equation}\n",
- "\\omega_i = 1.9712 \\cdot u_i \\text{ (26)}\n",
- "\\end{equation}\n",
- "\n",
- "### 12.5.2 Saturation limits of control inputs\n",
- "\n",
- "Based on the motor RPM limits presented before, one can determine the angular velocity limits. Then, using the parameters from Table 1, the definition of forces and torques from literature, and the formulae of control inputs in cross configuration, presented, the minimum and maximum values of these inputs are summarized in Table 3.\n",
- "\n",
- "<center>\n",
- " <figure class=\"image\">\n",
- " <img src=\"artwork/drone/tab3.PNG\" width=60% />\n",
- " <figcaption>Table 3: Control inut limits </figcaption>\n",
- " </figure>\n",
- "</center>\n",
- "\n",
- "### 12.5.3 Linear control\n",
- "\n",
- "Consider the model rewritten in nonlinear state space form:\n",
- "\n",
- "\\begin{equation}\n",
- "\\dot{\\mathbf{x}} = \\mathbf{f(x,u)} \\text{ (27)}\n",
- "\\end{equation}\n",
- "\n",
- "where the states and inputs are:\n",
- "\n",
- "\\begin{equation}\n",
- "\\mathbf{x} = [\\xi, \\eta, \\rho, \\pi]^T \\text{ (28)}\n",
- "\\end{equation}\n",
- "\n",
- "\\begin{equation}\n",
- "\\mathbf{u} = [U_{coll}, U_{\\phi}, U_{\\theta}, U_{\\psi}]^T \\text{ (29)}\n",
- "\\end{equation}\n",
- "\n",
- "Let the equilibrium point in hovering mode be $\\mathbf{x^e} = [x^e, y^e, z^e, 0,0,0,0,0,0,0,0,0]^T$ with inputs $\\mathbf{u} = [U_{coll}^e, 0, 0, 0]^T$\n",
- "\n",
- "The linearized model is obtained as:\n",
- "\n",
- "\\begin{equation}\n",
- "\\mathbf{\\dot{x}} = \\frac{\\partial \\mathbf{f}}{\\partial \\mathbf{x}}|_{(x^e, u^e)} \\cdot \\mathbf{x} + \\frac{\\partial \\mathbf{f}}{\\partial \\mathbf{u}}|_{(x^e, u^e)} \\cdot \\mathbf{u} \\text{ (30)}\n",
- "\\end{equation}\n",
- "\n",
- "For the parameters of the drone the model can be written in state space as:\n",
- "\n",
- "\\begin{equation}\n",
- "\\dot{x} = Ax + Bu \\text{ (31)}\n",
- "\\end{equation}\n",
- "\n",
- "with \n",
- "\n",
- "\\begin{equation}\n",
- "A=\\begin{bmatrix}\n",
- "0 & 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 & 0 & 0 & 0 \\\\\n",
- "0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 & 0 & 0 \\\\\n",
- "0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 & 0 \\\\\n",
- "0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 \\\\\n",
- "0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 & 0 \\\\\n",
- "0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 \\\\\n",
- "0 & 0 & 0 & 0 & 9.8 & 0 & 0 & 0 & 0 & 0 & 0 & 0 \\\\\n",
- "0 & 0 & 0 & -9.8 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 \\\\\n",
- "0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 \\\\\n",
- "0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 \\\\\n",
- "0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 \\\\\n",
- "0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 \\\\\n",
- "\\end{bmatrix} \n",
- "\\end{equation}\n",
- "\n",
- "\\begin{equation}\n",
- "B=\\begin{bmatrix}\n",
- "0 & 0 & 0 & 0 \\\\\n",
- "0 & 0 & 0 & 0 \\\\\n",
- "0 & 0 & 0 & 0 \\\\\n",
- "0 & 0 & 0 & 0 \\\\\n",
- "0 & 0 & 0 & 0 \\\\\n",
- "0 & 0 & 0 & 0 \\\\\n",
- "0 & 0 & 0 & 0 \\\\\n",
- "0 & 0 & 0 & 0 \\\\\n",
- "2.2 & 0 & 0 & 0 \\\\\n",
- "0 & 500 & 0 & 0 \\\\\n",
- "0 & 0 & 625 & 0 \\\\\n",
- "0 & 0 & 0 & 285.7 \\\\\n",
- "\\end{bmatrix} \n",
- "\\end{equation}\n",
- "\n",
- "We will first design a state feedback control $u = -K \\cdot x$ to stabilize the system. We impose the following closed loop poles $p =[-20;-20.5;-21;-2.05;-2.1;-2.15;-21.5;-22;-22.5;-2.2;-2.25;-2.3]$,\n",
- "that ensure that the system is stable, without oscillations, and with a relative fast response. \n",
- "\n",
- "\\begin{equation}\n",
- "K=\\begin{bmatrix}\n",
- "24.7267 & 0.0001 & 22.1294 & -0.0002 & 54.2482 & -0.0000 & 22.3994 & 0.0001 & 10.8415 & -0.0000 & 2.0236 & -0.0000\\\\\n",
- "-0.0000 & -0.3909 & -0.0000 & 1.2201 & -0.0000 & 0.0001 & -0.0000 & -0.4084 & -0.0000 & 0.0914 & -0.0000 & 0.0000\\\\\t \n",
- "0.3998 & 0.0000 & 0.0000 & -0.0000 & 1.0992 & -0.0000 & 0.3919 & 0.0000 & 0.0000 & -0.0000 & 0.0776 & -0.0000\\\\ \n",
- "0.0000 & -0.1374 & 0.0000 & 0.0665 & 0.0000 & 0.1457 & 0.0000 & -0.0783 & 0.0000 & 0.0016 & 0.0000 & 0.0773\\\\\n",
- "\\end{bmatrix} \n",
- "\\end{equation}\n",
- "\t\n",
- "Next, we add an output feedback loop after the positions x; y; z (tracking) as in Figure 4. The new control signal now becomes:\n",
- "\n",
- "\\begin{equation}\n",
- "\\mathbf{u} = -\\mathbf{K} \\cdot \\mathbf{x} + \\mathbf{u}_0 \\text{ (32)}\n",
- "\\end{equation}\n",
- "\n",
- "with \n",
- "\n",
- "\\begin{equation}\n",
- "\\mathbf{u}_0 = [u_z, u_y, u_x, 0]^T \\text{ (33)}\n",
- "\\end{equation}\n",
- "\n",
- "and\n",
- "$$\\begin{equation}\n",
- "\\left\\{ \n",
- " \\begin{aligned}\n",
- " u_x = \\frac{k_{ix}}{s}(r_x-x)\\\\\n",
- " u_y = \\frac{k_{iy}}{s}(r_y-y)\\\\\n",
- " u_z = \\frac{k_{iz}}{s}(r_z-z)\\\\\n",
- " ref = [r_x, r_y, r_z]^T\\\\\n",
- " \\end{aligned}\n",
- " \\right.\n",
- "\\end{equation} \n",
- "$$\n",
- "\n",
- "The integrator gains can be tuned experimentally in order to obtain good tracking performances.\n",
- "\n",
- "<center>\n",
- " <figure class=\"image\">\n",
- " <img src=\"artwork/drone/4.PNG\" width=70% />\n",
- " <figcaption>Figure 12.4: Linear control of Quadcopter</figcaption>\n",
- " </figure>\n",
- "</center>"
- ]
- },
- {
- "cell_type": "markdown",
- "metadata": {},
- "source": [
- "## 11.6 Proposed problems"
- ]
- },
- {
- "cell_type": "markdown",
- "metadata": {},
- "source": [
- "1. Implement in Python the model and linear control for hovering action for the described quadcopter."
- ]
- },
- {
- "cell_type": "code",
- "execution_count": null,
- "metadata": {},
- "outputs": [],
- "source": [
- "import math as m \n",
- "import numpy as np\n",
- "from scipy.integrate import odeint\n",
- "import matplotlib.pyplot as plt\n",
- "from odeintw import *\n",
- "from math import pi\n",
- "from math import *\n",
- "from scipy.signal import place_poles\n",
- "from lab_functions import *\n",
- "\n",
- "# parameters\n",
- "m, g, l = 0.4, 9.8, 0.1778\n",
- "Ix, Iy, Iz = 0.002, 0.0016, 0.0035\n",
- "b, d = 192.32*10**(-7), 1*10**(-7)\n",
- "c = 1.9712\n",
- "\n",
- "def model(x,t,u):\n",
- " u = u.reshape(4,1)\n",
- " x = x.reshape(12,1)\n",
- " \n",
- " px = x[0]\n",
- " py = x[1]\n",
- " pz = x[2]\n",
- " phi = x[3]\n",
- " theta = x[4]\n",
- " psi = x[5]\n",
- " dx = x[6]\n",
- " dy = x[7]\n",
- " dz = x[8]\n",
- " dphi = x[9]\n",
- " dtheta = x[10]\n",
- " dpsi = x[11]\n",
- " \n",
- " J = np.array([[Ix, 0, -Ix*sin(theta)],\n",
- " [0, Iy*(cos(phi))**2+Iz*(sin(phi))**2, (Iy-Iz)*cos(phi)*sin(phi)*cos(theta)],\n",
- " [-Ix*sin(theta), (Iy-Iz)*cos(phi)*sin(phi)*cos(theta), Ix*(sin(theta))**2+Iy*(sin(phi))**2*(cos(theta))**2+Iz*(cos(phi))**2*(cos(theta))**2]])\n",
- " \n",
- " c11 = 0\n",
- " c12 = (Iy-Iz)*(dtheta*cos(phi)*sin(phi)+dpsi*(sin(phi))**2*cos(theta))+(Iz-Iy)*dpsi*(cos(phi))**2*cos(theta)-Ix*dpsi*cos(theta)\n",
- " c13 = (Iz-Iy)*dpsi*(cos(theta))**2*sin(phi)*cos(phi)\n",
- " c21 = (Iz-Iy)*(dtheta*sin(phi)*cos(phi)+dpsi*(sin(phi))**2*cos(theta))+(Iy-Iz)*dpsi*(cos(phi))**2*cos(theta)+Ix*dpsi*cos(theta)\n",
- " c22 = (Iz-Iy)*dphi*cos(phi)*sin(phi)\n",
- " c23 = -Ix*dpsi*sin(theta)*cos(theta)+Iy*dpsi*(sin(phi))**2*sin(theta)*cos(theta)+Iz*dpsi*(cos(phi))**2*sin(theta)*cos(theta)\n",
- " c31 = (Iy-Iz)*dpsi*(cos(theta))**2*sin(phi)*cos(phi)-Ix*dtheta*cos(theta)\n",
- " c32 = (Iz-Iy)*(dtheta*cos(phi)*sin(phi)*sin(theta)+dphi*(sin(phi))**2*cos(theta))+(Iy-Iz)*dphi*(cos(phi))**2*cos(theta)+Ix*dpsi*sin(theta)*cos(theta)-Iy*dpsi*(sin(phi))**2*sin(theta)*cos(theta)-Iz*dpsi*(cos(phi))**2*sin(theta)*cos(theta)\n",
- " c33 = (Iy-Iz)*dphi*(cos(theta))**2*sin(phi)*cos(phi)-Iy*dtheta*(sin(phi))**2*sin(theta)*cos(theta)-Iz*dtheta*(cos(phi))**2*sin(theta)*cos(theta)+Ix*dtheta*sin(theta)*cos(theta)\n",
- " \n",
- " if c12.shape == (1,): c12 = c12[0]\n",
- " if c13.shape == (1,): c13 = c13[0]\n",
- " if c21.shape == (1,): c21 = c21[0]\n",
- " if c22.shape == (1,): c22 = c22[0]\n",
- " if c23.shape == (1,): c23 = c23[0]\n",
- " if c31.shape == (1,): c31 = c31[0]\n",
- " if c32.shape == (1,): c32 = c32[0]\n",
- " if c33.shape == (1,): c33 = c33[0]\n",
- "\n",
- " C = np.array([[c11, c12, c13], [c21, c22, c23], [c31, c32, c33]])\n",
- "\n",
- " Jinv = np.linalg.inv(J)\n",
- " \n",
- " Ucoll = u[0] + m*g\n",
- " Uphi = u[1]\n",
- " Utheta = u[2]\n",
- " Upsi = u[3]\n",
- " \n",
- " ddx = theta*g \n",
- " ddy = -phi*g \n",
- " ddz = (Ucoll-m*g)/m \n",
- " \n",
- " ddphi = Uphi/Ix\n",
- " ddtheta = Utheta/Iy\n",
- " ddpsi = Upsi/Iz\n",
- "\n",
- " xddot = np.array([dx,dy,dz,dphi,dtheta,dpsi,ddx,ddy,ddz,ddphi,ddtheta,ddpsi])\n",
- "\n",
- " return xddot\n",
- "\n",
- "# time interval/sampling time\n",
- "dt = 0.01\n",
- "\n",
- "# final time\n",
- "tf = 20\n",
- "\n",
- "# nr of samples\n",
- "n = int(np.round(tf/dt))\n",
- "\n",
- "# Desired time samples for the solution.\n",
- "t = np.arange(0, tf, dt)\n",
- "\n",
- "radius = 0.2\n",
- "freq = 1\n",
- "height = 0.3\n",
- "\n",
- "# Trajectory should be a circle on the XY plane\n",
- "rx = \n",
- "ry = \n",
- "rz = \n",
- "\n",
- "# initial conditions for the states (P and V)\n",
- "px = 0.0\n",
- "py = 0.0\n",
- "pz = 0.0\n",
- "theta = 0.0\n",
- "phi = 0.0\n",
- "psi = 0.0\n",
- "dx = 0.0\n",
- "dy = 0.0\n",
- "dz = 0.0\n",
- "dphi = 0.0\n",
- "dtheta = 0.0\n",
- "dpsi = 0.0\n",
- "\n",
- "x0 = np.array([[px], [py], [pz], [phi], [theta], [psi], [dx], [dy], [dz], [dphi], [dtheta], [dpsi]])\n",
- "\n",
- "plotx = np.zeros(n-1)\n",
- "ploty = np.zeros(n-1)\n",
- "plotz = np.zeros(n-1)\n",
- "plotphi = np.zeros(n-1)\n",
- "plottheta = np.zeros(n-1)\n",
- "plotpsi = np.zeros(n-1)\n",
- "\n",
- "u = np.zeros((4,n))\n",
- "\n",
- "K = np.array([[24.7267, 0.0001, 22.1294, -0.0002, 54.2482, -0.0000, 22.3994, 0.0001, 10.8415, -0.0000, 2.0236, -0.0000],\n",
- " [-0.0000, -0.3909, -0.0000, 1.2201, -0.0000, 0.0001, -0.0000, -0.4084, -0.0000, 0.0914, -0.0000, 0.0000],\n",
- " [0.3998, 0.0000, 0.0000, -0.0000, 1.0992, -0.0000, 0.3919, 0.0000, 0.0000, -0.0000, 0.0776, -0.0000],\n",
- " [0.0000, -0.1374, 0.0000, 0.0665, 0.0000, 0.1457, 0.0000, -0.0783, 0.0000, 0.0016, 0.0000, 0.0773]])\n",
- "\n",
- "# integrator parameters\n",
- "px_prev = px\n",
- "py_prev = py\n",
- "pz_prev = pz\n",
- "phi_prev = phi\n",
- "theta_prev = theta\n",
- "psi_prev = psi\n",
- "\n",
- "xerror = 0\n",
- "yerror = 0\n",
- "zerror = 0\n",
- "\n",
- "# Integrator gains, try values in range [-1,1], tune based on the graphs and intuition\n",
- "kix = \n",
- "kiy = \n",
- "kiz = \n",
- "\n",
- "# solve ODE for each step\n",
- "for i in range(1,n):\n",
- " printProgressBar((i+1)/n, prefix=\"Progress:\", suffix=\"complete\", length=60)\n",
- "\n",
- " # span for next time step\n",
- " tspan = [t[i-1],t[i]]\n",
- " \n",
- " # state feedback\n",
- " \n",
- " \n",
- " # state feedback & follow (equations 32 and 33)\n",
- " \n",
- " \n",
- " # solve for next step\n",
- " x = odeintw(model,x0,tspan,args=(u[:,i],))\n",
- " \n",
- " # store solution for plotting\n",
- " px = x[1][0,0]\n",
- " py = x[1][1,0]\n",
- " pz = x[1][2,0]\n",
- " phi = x[1][3,0]\n",
- " theta = x[1][4,0]\n",
- " psi = x[1][5,0]\n",
- " \n",
- " dx = (px - px_prev)/dt\n",
- " dy = (py - py_prev)/dt\n",
- " dz = (pz - pz_prev)/dt\n",
- " dphi = (phi - phi_prev)/dt\n",
- " dtheta = (theta - theta_prev)/dt\n",
- " dpsi = (psi - psi_prev)/dt\n",
- "\n",
- " # next initial condition \n",
- " x0 = np.array([px, py, pz, phi, theta, psi, dx, dy, dz, dphi, dtheta, dpsi]).reshape(12,1)\n",
- "\n",
- " px_prev = px\n",
- " py_prev = py\n",
- " pz_prev = pz\n",
- " phi_prev = phi\n",
- " theta_prev = theta\n",
- " psi_prev = psi\n",
- " \n",
- " plotx[i-1] = px\n",
- " ploty[i-1] = py\n",
- " plotz[i-1] = pz\n",
- " plotphi[i-1] = phi\n",
- " plottheta[i-1] = theta\n",
- " plotpsi[i-1] = psi"
- ]
- },
- {
- "cell_type": "code",
- "execution_count": null,
- "metadata": {},
- "outputs": [],
- "source": [
- "# pose of the vechicle P = [xi, eta] \n",
- "fig = plt.figure()\n",
- "fig.set_size_inches(18.5, 10.5)\n",
- "\n",
- "plt.plot(t[:-1], plotx, label='x')\n",
- "plt.plot(t[:-1], ploty, label='y')\n",
- "plt.plot(t[:-1], plotz, label='z')\n",
- "plt.plot(t[:-1], plotphi, label='phi')\n",
- "plt.plot(t[:-1], plottheta, label='theta')\n",
- "plt.plot(t[:-1], plotpsi, label='psi')\n",
- "\n",
- "plt.legend(loc='best')\n",
- "plt.ylabel('all')\n",
- "plt.xlabel('time')\n",
- "plt.grid(True)\n",
- "\n",
- "plt.show()\n",
- " \n",
- "# x y plane motion\n",
- "fig = plt.figure()\n",
- "\n",
- "plt.plot(plotx, ploty, label='trajectory')\n",
- "plt.plot(rx, ry, label='reference')\n",
- "\n",
- "plt.legend(loc='best')\n",
- "plt.ylabel('py')\n",
- "plt.xlabel('px')\n",
- "plt.grid(True)\n",
- "\n",
- "plt.show()\n",
- "\n",
- "# z motion\n",
- "fig = plt.figure()\n",
- "\n",
- "plt.plot(t[:-1], plotz, label='trajectory')\n",
- "plt.plot(t, rz, label='reference')\n",
- "\n",
- "plt.legend(loc='best')\n",
- "plt.ylabel('time')\n",
- "plt.xlabel('pz')\n",
- "plt.grid(True)\n",
- "\n",
- "plt.show()"
- ]
- }
- ],
- "metadata": {
- "kernelspec": {
- "display_name": "Python 3 (ipykernel)",
- "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.10.8"
- },
- "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
-}
diff --git a/theory/lab00_Introduction.ipynb b/theory/lab00_Introduction.ipynb
new file mode 100644
index 0000000000000000000000000000000000000000..4143b465c2eb536974755d2ef163931ab5297324
--- /dev/null
+++ b/theory/lab00_Introduction.ipynb
@@ -0,0 +1,56 @@
+{
+ "cells": [
+ {
+ "cell_type": "markdown",
+ "id": "5b11fda8-fdc5-40b3-b30a-bd4f9549ced4",
+ "metadata": {},
+ "source": [
+ "# Alone on Mars: I wish I had studied robotics at school...\n",
+ "\n",
+ "The year is 2023. An international cooperation between [NASA](https://www.nasa.gov/), [ESA](https://www.esa.int/), [JAXA](https://global.jaxa.jp/), and [CSA](https://www.asc-csa.gc.ca/eng/) has managed to send the first crewed mission to Mars, our neighboring planet. Their aim was to establish a permanent human settlement on the red planet, furthering mankind's quest to explore and inhabit extraterrestrial frontiers.\n",
+ "\n",
+ "As their spacecraft, the Artemis, touched down on Mars' rugged surface, the team discovered an unforeseen obstacle in the form of a powerful dust storm. Unrelenting and ferocious, the tempest consumed their vessel, leaving only one member of the crew and a handful of robots intact amidst the inhospitable Martian wasteland.\n",
+ "\n",
+ "<center>\n",
+ " <figure>\n",
+ " <img src=\"../artwork/stranded/stranded.png\" width=60%/>\n",
+ " <figcaption><center>Figure 1: A lonely astronaut stranded on the surface of unhospitable Mars</center></figcaption>\n",
+ "</figure>\n",
+ "</center>\n",
+ "\n",
+ "Her name was... dr. Elena Vasilescu.\n",
+ "\n",
+ "Stranded and separated from humanity by millions of kilometers, Dr. Vasilescu fought to maintain her sanity in the face of her desolate surroundings. The relentless solitude stretched before her, every breath an arduous reminder of her isolation. However, her spirit remained unyielding, fueled by the belief that she was the only hope for survival.\n",
+ "\n",
+ "Dr. Vasilescu was not alone. She was accompanied by the best robots humans had ever made, each designed to assist the survival and scientific operations of the mission. The CREW (Cognitive Robotic Exploration Workforce) robots were assigned to collect samples, analyze atmospheric conditions, and scout for potential resources. However, the roboticist of the mission had vanished together with the rest of the crew, and it was now her task to operate those robots. Oh how she wished she would have paid attention to those robotics laboratories when she was a student....\n",
+ "\n",
+ "Now, she has to learn robotics again from scratch.\n",
+ "\n",
+ "Her life depended on it.\n",
+ "\n",
+ "But first, she needs to [brush up her python skills](../assignments/Assignment00_Python_micro_tutorial.ipynb)"
+ ]
+ }
+ ],
+ "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
+}