diff --git a/README.md b/README.md index 9a19daa..7b3a84f 100644 --- a/README.md +++ b/README.md @@ -62,8 +62,8 @@ The content shown in this work is based on my own expert knowledge (and interpre ## Contents -- [The Basics of Kinematic Modeling and Control of Serial-link Manipulators Using `numpy`](basic_lessons/README.md). -- [Kinematic modeling and control of serial-link robotic manipulators using `dqrobotics`: From zero to hero.](other/dqrobotics). +- [The Basics of Kinematic Modelling and Control of Serial-link Manipulators Using `numpy`](basic_lessons/README.md). +- [Kinematic modelling and control of serial-link robotic manipulators using `dqrobotics`: From zero to hero.](other/dqrobotics). diff --git a/basic_lessons/README.md b/basic_lessons/README.md index 8b335af..689c19a 100644 --- a/basic_lessons/README.md +++ b/basic_lessons/README.md @@ -1,4 +1,4 @@ -# The Basics of Kinematic Modeling and Control of Serial-link Manipulators Using `numpy` +# The Basics of Kinematic Modelling and Control of Serial-link Manipulators Using `numpy` In this five-lesson tutorial, we start from the very basics of scalar and matricial operations in Python using `numpy`, all the way until the basics of kinematic control. Until kinematic control, most is based on [@spong2020robot]. diff --git a/basic_lessons/lesson1_exercise_answers.ipynb b/basic_lessons/lesson1_exercise_answers.ipynb index a6b70e7..c4b024f 100644 --- a/basic_lessons/lesson1_exercise_answers.ipynb +++ b/basic_lessons/lesson1_exercise_answers.ipynb @@ -154,4 +154,4 @@ }, "nbformat": 4, "nbformat_minor": 2 -} +} \ No newline at end of file diff --git a/basic_lessons/lesson1_tutorial.ipynb b/basic_lessons/lesson1_tutorial.ipynb index 5538ec8..26b1781 100644 --- a/basic_lessons/lesson1_tutorial.ipynb +++ b/basic_lessons/lesson1_tutorial.ipynb @@ -10,7 +10,7 @@ "\n", "*Author: Murilo M. Marinho (murilo.marinho@manchester.ac.uk)*\n", "\n", - "### Pre-requisites\n", + "### Prerequisites\n", "The user of this notebook is expected to have prior knowledge in\n", "- Basic Python [[Tutorial]](https://docs.python.org/3/tutorial/index.html)\n", "- Numpy \n", @@ -21,7 +21,7 @@ "### I found an issue\n", "Thank you! Please report it at https://github.com/MarinhoLab/OpenExecutableBooksRobotics/issues\n", "\n", - "### Latex Macros" + "### Latex Macros\n" ] }, { @@ -33,7 +33,7 @@ }, "source": [ "$\\providecommand{\\myvec}[1]{{\\mathbf{\\boldsymbol{{#1}}}}}$\n", - "$\\providecommand{\\mymatrix}[1]{{\\mathbf{\\boldsymbol{{#1}}}}}$" + "$\\providecommand{\\mymatrix}[1]{{\\mathbf{\\boldsymbol{{#1}}}}}$\n" ] }, { @@ -52,7 +52,7 @@ "cell_type": "markdown", "metadata": {}, "source": [ - "# A quick Python refresher" + "# A quick Python refresher\n" ] }, { @@ -65,7 +65,7 @@ "\n", "$$a\\triangleq 10,b\\triangleq 5.$$\n", "\n", - "We can replicate the above in Python with" + "We can replicate the above in Python with\n" ] }, { @@ -83,7 +83,7 @@ "metadata": {}, "source": [ "## Output variables\n", - "Variables can be output using `print`. For example, for $a$" + "Variables can be output using `print`. For example, for $a$\n" ] }, { @@ -109,7 +109,7 @@ "source": [ "\n", "### Output text and variables using f-strings\n", - "To output $a$ and $b$ within a string, we can use `print` and f-strings as follows" + "To output $a$ and $b$ within a string, we can use `print` and f-strings as follows\n" ] }, { @@ -139,7 +139,7 @@ "\n", "#### Sum\n", "\n", - "$$c = a + b.$$" + "$$c = a + b.$$\n" ] }, { @@ -166,7 +166,7 @@ "metadata": {}, "source": [ "#### Subtraction\n", - "$$c = a - b.$$" + "$$c = a - b.$$\n" ] }, { @@ -193,7 +193,7 @@ "metadata": {}, "source": [ "#### Multiplication\n", - "$$c=ab$$" + "$$c=ab$$\n" ] }, { @@ -220,7 +220,7 @@ "metadata": {}, "source": [ "#### Division\n", - "$$c = \\frac{a}{b}$$" + "$$c = \\frac{a}{b}$$\n" ] }, { @@ -247,7 +247,7 @@ "metadata": {}, "source": [ "### Exponentiation\n", - "$$c = a^{b}$$" + "$$c = a^{b}$$\n" ] }, { @@ -275,7 +275,7 @@ "source": [ "### Math functions\n", "\n", - "For the following functions, we will need Python's built-in `math` module." + "For the following functions, we will need Python's built-in `math` module.\n" ] }, { @@ -293,7 +293,7 @@ "source": [ "### Square root\n", "\n", - "$$c = \\sqrt{a}$$" + "$$c = \\sqrt{a}$$\n" ] }, { @@ -314,7 +314,7 @@ "\n", "$$c= \\sqrt[n]{a}, n \\in \\mathbb{N},$$\n", "\n", - "does not seem to have a shorthanded version in Python, but can computed through simple properties such as\n", + "does not seem to have a shorthand version in Python, but can be computed through simple properties such as\n", "\n", "$$c = \\sqrt[n]{a} = a^{\\frac{1}{n}} = e^{\\frac{ln(a)}{n}}.$$\n", "\n", @@ -322,7 +322,7 @@ "\n", "$$n = 3.$$ \n", "\n", - "Then," + "Then,\n" ] }, { @@ -338,7 +338,7 @@ "cell_type": "markdown", "metadata": {}, "source": [ - "and we can calculate the n-th root like so" + "and we can calculate the n-th root like so\n" ] }, { @@ -365,7 +365,7 @@ "cell_type": "markdown", "metadata": {}, "source": [ - "or like so" + "or like so\n" ] }, { @@ -398,7 +398,7 @@ "$$ \\phi = \\frac{\\pi}{4},$$\n", "$$ s_{\\phi} = \\sin \\left( \\phi \\right),$$\n", "$$ c_{\\phi} = \\cos \\left( \\phi \\right),$$\n", - "$$ t_{\\phi} = \\tan \\left( \\phi \\right).$$" + "$$ t_{\\phi} = \\tan \\left( \\phi \\right).$$\n" ] }, { @@ -437,7 +437,7 @@ "\n", "### Installing the library\n", "\n", - "Just in case `numpy` is not already installed, we can install it with the following command. Nothing will happen if the library is already installed." + "Just in case `numpy` is not already installed, we can install it with the following command. Nothing will happen if the library is already installed.\n" ] }, { @@ -455,7 +455,7 @@ "cell_type": "markdown", "metadata": {}, "source": [ - "### Importing the library" + "### Importing the library\n" ] }, { @@ -472,12 +472,12 @@ "metadata": {}, "source": [ "### Instantiating vectors\n", - "A row vector can be instanteated from a list of list. For instance, for \n", + "A row vector can be instantiated from a list of lists. For instance, for \n", "$$\\myvec{v} = \\left[\\begin{array}{ccc}\n", " 1 & 2 \n", " \\end{array}\\right],\n", "$$ \n", - "we have" + "we have\n" ] }, { @@ -504,12 +504,12 @@ "cell_type": "markdown", "metadata": {}, "source": [ - "A column vector can be instanted from a list of singleton *lists*. For instance, for \n", + "A column vector can be instantiated from a list of singleton *lists*. For instance, for \n", "$$\\myvec{u} = \\left[\\begin{array}{ccc}\n", " 1 \\\\\n", " 2\n", " \\end{array}\\right],$$ \n", - "we have" + "we have\n" ] }, { @@ -540,7 +540,7 @@ "source": [ "### Dot product\n", "\n", - "$$\\myvec{c} = <\\myvec{u},\\myvec{u}>$$ " + "$$\\myvec{c} = <\\myvec{u},\\myvec{u}>$$ \n" ] }, { @@ -588,7 +588,7 @@ "\n", "we can obtain the cross product\n", "\n", - "$$\\myvec{c} = \\myvec{u}_3 \\times \\myvec{v}_3$$ " + "$$\\myvec{c} = \\myvec{u}_3 \\times \\myvec{v}_3$$ \n" ] }, { @@ -623,7 +623,7 @@ "\n", "
\n", "Note that the function is np.linalg.norm, as the norm calculation is within the module linalg.\n", - "
" + "\n" ] }, { @@ -659,7 +659,7 @@ " 5 & 6 \\\\\n", " 7 & 8 \n", " \\end{array}\\right] \n", - "$$" + "$$\n" ] }, { @@ -694,7 +694,7 @@ "source": [ "### Transpose\n", "\n", - "$$\\mymatrix{C} = \\mymatrix{A}^T$$ " + "$$\\mymatrix{C} = \\mymatrix{A}^T$$ \n" ] }, { @@ -723,7 +723,7 @@ "source": [ "### Sum\n", "\n", - "$$ \\mymatrix{C} = \\mymatrix{A} + \\mymatrix{B} $$" + "$$ \\mymatrix{C} = \\mymatrix{A} + \\mymatrix{B} $$\n" ] }, { @@ -752,7 +752,7 @@ "source": [ "### Subtraction\n", "\n", - "$$ \\mymatrix{C} = \\mymatrix{A} - \\mymatrix{B} $$" + "$$ \\mymatrix{C} = \\mymatrix{A} - \\mymatrix{B} $$\n" ] }, { @@ -788,7 +788,7 @@ "
\n", "The matrix multiplication operator, @, is very unusual. Pay close attention.\n", "Mistaking this can be a major source of bugs and confusion.\n", - "
" + "\n" ] }, { @@ -822,7 +822,7 @@ " \\end{array}\\right],\n", "$$\n", "$$ \\myvec{d} = \\myvec{v}\\myvec{u} = 5.\n", - "$$" + "$$\n" ] }, { @@ -858,7 +858,7 @@ "\n", "
\n", "We only use the \".\" sign to denote matrix multiplication when otherwise it would be difficult to read the equation.\n", - "
" + "\n" ] }, { @@ -888,7 +888,7 @@ "source": [ "### Diagonal matrices\n", "\n", - "Diagonal matrices get increasingly sparse with size, so it is important to have shorthanded commands for creating them. For instance, suppose that we have the following diagonal matrix\n", + "Diagonal matrices get increasingly sparse with size, so it is important to have shorthand commands for creating them. For instance, suppose that we have the following diagonal matrix\n", "\n", "$$\\mymatrix{D} = \\left[\\begin{array}{ccc}\n", " 1 & 0 & 0 \\\\\n", @@ -897,7 +897,7 @@ " \\end{array}\\right] ,\n", "$$\n", "\n", - "this can be instanteated in `numpy` with" + "this can be instantiated in `numpy` with\n" ] }, { @@ -936,7 +936,7 @@ " \\end{array}\\right],\n", "$$ \n", "\n", - "can be instanteated in `numpy` with" + "can be instantiated in `numpy` with\n" ] }, { @@ -977,7 +977,7 @@ "\n", "
\n", "The np.zeros function takes a tuple to generate a properly sized matrix. Do not confuse it with np.eye that accepts a scalar.\n", - "
" + "\n" ] }, { @@ -1013,7 +1013,7 @@ "\n", "$$ e_1 = \\sin(\\phi) + 4\\cos(\\frac{\\phi}{5}).$$\n", "\n", - "Using the `math` module which is already imported, calculate the value of $e_1$ and store it in the variable `e1` shown in the cell below." + "Using the `math` module which is already imported, calculate the value of $e_1$ and store it in the variable `e1` shown in the cell below.\n" ] }, { @@ -1045,7 +1045,7 @@ "Let\n", "$$\\mymatrix{C}_2 = \\mymatrix{A}_2 + \\mymatrix{B}_2 + \\mymatrix{A}_2\\mymatrix{B}_2 - \\mymatrix{B}_2\\mymatrix{A}_2.$$\n", "\n", - "Using the numpy module which is already imported, calculate the value of $\\mymatrix{C}_2$ and store it in the variable `C2` shown in the cell below." + "Using the numpy module which is already imported, calculate the value of $\\mymatrix{C}_2$ and store it in the variable `C2` shown in the cell below.\n" ] }, { @@ -1077,4 +1077,4 @@ }, "nbformat": 4, "nbformat_minor": 2 -} +} \ No newline at end of file diff --git a/basic_lessons/lesson2_exercise_answers.ipynb b/basic_lessons/lesson2_exercise_answers.ipynb index b1b36b9..14fc149 100644 --- a/basic_lessons/lesson2_exercise_answers.ipynb +++ b/basic_lessons/lesson2_exercise_answers.ipynb @@ -228,7 +228,9 @@ { "metadata": {}, "cell_type": "markdown", - "source": "`H_c` is *not* the same as `H_d`. This indicates that the order of operations matter. That is, sequential pose transformations are not commutative." + "source": [ + "`H_c` is *not* the same as `H_d`. This indicates that the order of operations matters. That is, sequential pose transformations are not commutative." + ] }, { "metadata": {}, @@ -329,4 +331,4 @@ }, "nbformat": 4, "nbformat_minor": 2 -} +} \ No newline at end of file diff --git a/basic_lessons/lesson2_tutorial.ipynb b/basic_lessons/lesson2_tutorial.ipynb index f856be3..184096f 100644 --- a/basic_lessons/lesson2_tutorial.ipynb +++ b/basic_lessons/lesson2_tutorial.ipynb @@ -10,14 +10,14 @@ "\n", "*Author: Murilo M. Marinho (murilo.marinho@manchester.ac.uk)*\n", "\n", - "## Pre-requisites for the learner\n", + "## Prerequisites for the learner\n", "The user of this notebook is expected to have prior knowledge in\n", - "- All the content and pre-requisites of lesson 1.\n", + "- All the content and prerequisites of lesson 1.\n", "\n", "## I found an issue\n", "Thank you! Please report it at https://github.com/MarinhoLab/OpenExecutableBooksRobotics/issues\n", "\n", - "## Latex Macros" + "## Latex Macros\n" ] }, { @@ -29,7 +29,7 @@ }, "source": [ "$\\providecommand{\\myvec}[1]{{\\mathbf{\\boldsymbol{{#1}}}}}$\n", - "$\\providecommand{\\mymatrix}[1]{{\\mathbf{\\boldsymbol{{#1}}}}}$" + "$\\providecommand{\\mymatrix}[1]{{\\mathbf{\\boldsymbol{{#1}}}}}$\n" ] }, { @@ -48,7 +48,7 @@ "cell_type": "markdown", "metadata": {}, "source": [ - "# Installing pre-requisites" + "# Installing prerequisites\n" ] }, { @@ -71,7 +71,7 @@ "cell_type": "markdown", "metadata": {}, "source": [ - "# Imports" + "# Imports\n" ] }, { @@ -98,7 +98,7 @@ "\n", "In other words, the object has no flexibility and the motion of the entire body can be prescribed by its *position* and *orientation* with respect to a given *reference frame*.\n", "\n", - "This tends to be the initial topic of robotics textbooks. That is because we can use this to derive the equations of motion for many classes of robots and objects from first principles." + "This tends to be the initial topic of robotics textbooks. That is because we can use this to derive the equations of motion for many classes of robots and objects from first principles.\n" ] }, { @@ -131,7 +131,7 @@ "\n", "
\n", "It is common for column and row vectors to not be distinguishable in Python with numpy. That is in general convenient but can cause problems when the dimension is important, so always pay close attention to what each function expects as input.\n", - "
" + "\n" ] }, { @@ -174,7 +174,7 @@ "\n", "with $$i \\in \\mathbb{N}$$ can be composed with sequential additions\n", "\n", - "$$\\myvec{p} = \\myvec{p}_{0} + \\myvec{p}_{1} + \\myvec{p}_{2} + \\myvec{p}_{3}.$$" + "$$\\myvec{p} = \\myvec{p}_{0} + \\myvec{p}_{1} + \\myvec{p}_{2} + \\myvec{p}_{3}.$$\n" ] }, { @@ -219,7 +219,7 @@ "$$\\myvec{p}' = \\myvec{p} - \\myvec{p}_{0} - \\myvec{p}_{1} - \\myvec{p}_{2} - \\myvec{p}_{3} = \\left[\\begin{array}{ccc}\n", " 0 \\\\\n", " 0\n", - " \\end{array}\\right].$$" + " \\end{array}\\right].$$\n" ] }, { @@ -274,7 +274,7 @@ " \\sin{\\theta} & \\cos{\\theta} \n", " \\end{array}\\right],$$\n", "\n", - "when $\\theta = \\frac{\\pi}{2}$, we have the following equivalent piece of code." + "when $\\theta = \\frac{\\pi}{2}$, we have the following equivalent piece of code.\n" ] }, { @@ -287,10 +287,10 @@ } }, "source": [ - "θ = pi/2\n", + "\u03b8 = pi/2\n", "\n", - "R = np.array([[cos(θ),-sin(θ)],\n", - " [sin(θ), cos(θ)]])\n", + "R = np.array([[cos(\u03b8),-sin(\u03b8)],\n", + " [sin(\u03b8), cos(\u03b8)]])\n", "\n", "print(f\"R={R}\")" ], @@ -332,10 +332,10 @@ " \\end{array}\\right].$$\n", "\n", "
\n", - "The translation and rotation order is extremelly important. Check the exercises at the end of this lesson.\n", + "The translation and rotation order is extremely important. Check the exercises at the end of this lesson.\n", "
\n", "\n", - "For $\\theta = \\frac{\\pi}{2}$, $x = 0.1$, and $y = 0.2$, we have the following equivalent piece of code." + "For $\\theta = \\frac{\\pi}{2}$, $x = 0.1$, and $y = 0.2$, we have the following equivalent piece of code.\n" ] }, { @@ -348,12 +348,12 @@ } }, "source": [ - "θ = pi/2\n", + "\u03b8 = pi/2\n", "x = 0.1\n", "y = 0.2\n", "\n", - "H = np.array([[cos(θ),-sin(θ), x],\n", - " [sin(θ), cos(θ), y],\n", + "H = np.array([[cos(\u03b8),-sin(\u03b8), x],\n", + " [sin(\u03b8), cos(\u03b8), y],\n", " [0, 0, 1]])\n", "\n", "\n", @@ -408,7 +408,7 @@ " 1 & 0 & 0 \\\\\n", " 0 & \\cos{\\theta} & -\\sin{\\theta} \\\\\n", " 0 & \\sin{\\theta} & \\cos{\\theta}\n", - "\\end{array}\\right].$$" + "\\end{array}\\right].$$\n" ] }, { @@ -421,17 +421,17 @@ } }, "source": [ - "Rz = np.array([[cos(θ),-sin(θ), 0],\n", - " [sin(θ), cos(θ), 0],\n", + "Rz = np.array([[cos(\u03b8),-sin(\u03b8), 0],\n", + " [sin(\u03b8), cos(\u03b8), 0],\n", " [0, 0, 1]])\n", "\n", - "Ry = np.array([[ cos(θ), 0, sin(θ)],\n", + "Ry = np.array([[ cos(\u03b8), 0, sin(\u03b8)],\n", " [ 0, 1, 0],\n", - " [-sin(θ), 0, cos(θ)]])\n", + " [-sin(\u03b8), 0, cos(\u03b8)]])\n", "\n", "Rx = np.array([[1, 0, 0],\n", - " [0, cos(θ), -sin(θ)],\n", - " [0, sin(θ), cos(θ)]])\n", + " [0, cos(\u03b8), -sin(\u03b8)],\n", + " [0, sin(\u03b8), cos(\u03b8)]])\n", "\n", "# A rotation about z\n", "print(f\"Rz={Rz}\")\n", @@ -471,7 +471,7 @@ "\n", "$$\\mymatrix{R}^0_c = \\mymatrix{R}^0_a\\mymatrix{R}^a_b\\mymatrix{R}^b_c.$$\n", "\n", - "This can be represented by the following equivalent piece of code using sample angles." + "This can be represented by the following equivalent piece of code using sample angles.\n" ] }, { @@ -484,20 +484,20 @@ } }, "source": [ - "θ0_a = pi/4\n", - "R0_a = np.array([[cos(θ0_a),-sin(θ0_a), 0],\n", - " [sin(θ0_a), cos(θ0_a), 0],\n", + "\u03b80_a = pi/4\n", + "R0_a = np.array([[cos(\u03b80_a),-sin(\u03b80_a), 0],\n", + " [sin(\u03b80_a), cos(\u03b80_a), 0],\n", " [0, 0, 1]])\n", "\n", - "θa_b = -pi/2\n", - "Ra_b = np.array([[ cos(θa_b), 0, sin(θa_b)],\n", + "\u03b8a_b = -pi/2\n", + "Ra_b = np.array([[ cos(\u03b8a_b), 0, sin(\u03b8a_b)],\n", " [ 0, 1, 0],\n", - " [-sin(θa_b), 0, cos(θa_b)]])\n", + " [-sin(\u03b8a_b), 0, cos(\u03b8a_b)]])\n", "\n", - "θb_c = pi/8\n", + "\u03b8b_c = pi/8\n", "Rb_c = np.array([[1, 0, 0],\n", - " [0, cos(θb_c), -sin(θb_c)],\n", - " [0, sin(θb_c), cos(θb_c)]])\n", + " [0, cos(\u03b8b_c), -sin(\u03b8b_c)],\n", + " [0, sin(\u03b8b_c), cos(\u03b8b_c)]])\n", "\n", "R0_c = R0_a @ Ra_b @ Rb_c\n", "print(f\"R0_c={R0_c}\")" @@ -530,7 +530,7 @@ "\\mymatrix{R}^0_a &= \\mymatrix{R}^0_c (\\mymatrix{R}^b_c)^T (\\mymatrix{R}^a_b)^T \\\\\n", "\\end{align}$$\n", "\n", - "In the example code below, we store this alternative calculation in `R0_a_` to compare it with `R0_a`." + "In the example code below, we store this alternative calculation in `R0_a_` to compare it with `R0_a`.\n" ] }, { @@ -584,8 +584,8 @@ " \\end{array}\\right].$$\n", "\n", "
\n", - "The translation and rotation order is extremelly important.\n", - "
" + "The translation and rotation order is extremely important.\n", + "\n" ] }, { @@ -596,7 +596,7 @@ "\n", "We can also perform pose transformations using sequential right multiplications when they are with respect to the _current_ frame. Note also that we can verify the lack of commutativity on these transformations.\n", "\n", - "For example, consider a translation in 3D along the _World_ frame, represented by the homogenous transformation matrix below.\n", + "For example, consider a translation in 3D along the _World_ frame, represented by the homogeneous transformation matrix below.\n", "\n", "$$\\mymatrix{H}^0_a = \\mymatrix{H}_a = \\left[\\begin{array}{ccc}\n", " 1 & 0 & 0 & 1 \\\\\n", @@ -605,7 +605,7 @@ " 0 & 0 & 0 & 1\n", " \\end{array}\\right].$$\n", "\n", - "Consider a rotation in 3D about the _current_ frame, represented by the homogenous transformation matrix below.\n", + "Consider a rotation in 3D about the _current_ frame, represented by the homogeneous transformation matrix below.\n", "\n", "$$\\mymatrix{H}^a_b = \\left[\\begin{array}{ccc}\n", " \\cos{\\theta_{ab}} & -\\sin{\\theta_{ab}} & 0 & 0 \\\\\n", @@ -622,7 +622,7 @@ "\n", "$$\\mymatrix{H}_a\\mymatrix{H}^a_b \\neq \\mymatrix{H}^a_b\\mymatrix{H}_a,$$\n", "\n", - "further reinforcing the importance of the order of operations." + "further reinforcing the importance of the order of operations.\n" ] }, { @@ -646,10 +646,10 @@ " [0, 0, 0, 1]])\n", "\n", "# An SE(3) rotation\n", - "θab = pi/4 # angle in radians\n", + "\u03b8ab = pi/4 # angle in radians\n", "\n", - "Ha_b = np.array([[cos(θab), -sin(θab), 0, 0],\n", - " [sin(θab), cos(θab), 0, 0],\n", + "Ha_b = np.array([[cos(\u03b8ab), -sin(\u03b8ab), 0, 0],\n", + " [sin(\u03b8ab), cos(\u03b8ab), 0, 0],\n", " [0, 0, 1, 0],\n", " [0, 0, 0, 1]])\n", "\n", @@ -688,7 +688,7 @@ " \\myvec{0} & 1 \n", " \\end{array}\\right],$$\n", "\n", - "so it is important to remember to _never_ invert the matrix with general matrix inversion algorithms. Using the matricial properties leads to a simpler, faster, and more accurate inversion. The only \"trick\" is to obtain the submatrix corresponding to the rotation matrix." + "so it is important to remember to _never_ invert the matrix with general matrix inversion algorithms. Using the matricial properties leads to a simpler, faster, and more accurate inversion. The only \"trick\" is to obtain the submatrix corresponding to the rotation matrix.\n" ] }, { @@ -701,23 +701,23 @@ } }, "source": [ - "# Extract R from H\n", - "R0_b = H0_b[0:3,0:3] # The 3x3 Rotation matrix\n", - "t0_b = H0_b[0:3,3].reshape((3,1)) # The 3x1 translation vector at the forth column. Reshape it into a column vector.\n", - "\n", - "A = R0_b.T\n", - "B = -R0_b.T @ t0_b\n", - "C = np.array([0,0,0])\n", - "D = np.array([1])\n", - "\n", - "# This is how you can use `np.block` to build a matrix like so\n", - "# | A B |\n", - "# | C D |\n", - "H0_b_inv = np.block([[A, B],\n", - " [C, D]])\n", - "\n", - "print(f\"H0_b_inv={H0_b_inv}\")\n", - "\n", + "# Extract R from H", + "R0_b = H0_b[0:3,0:3] # The 3x3 Rotation matrix", + "t0_b = H0_b[0:3,3].reshape((3,1)) # The 3x1 translation vector at the fourth column. Reshape it into a column vector.", + "", + "A = R0_b.T", + "B = -R0_b.T @ t0_b", + "C = np.array([0,0,0])", + "D = np.array([1])", + "", + "# This is how you can use `np.block` to build a matrix like so", + "# | A B |", + "# | C D |", + "H0_b_inv = np.block([[A, B],", + " [C, D]])", + "", + "print(f\"H0_b_inv={H0_b_inv}\")", + "", "print(f\"H0_b @ H0_b_inv = {H0_b @ H0_b_inv}\")" ], "outputs": [ @@ -752,7 +752,7 @@ "\n", "$$\\mymatrix{R}^0_{a} = \\mymatrix{R}_{a} = R(\\theta_a) \\in SO(2)$$\n", "\n", - "and store it in the variable `R_a` shown in the cell below." + "and store it in the variable `R_a` shown in the cell below.\n" ] }, { @@ -764,7 +764,7 @@ }, "cell_type": "code", "source": [ - "θ_a = pi/4.0 # As given in the exercise\n", + "\u03b8_a = pi/4.0 # As given in the exercise\n", "\n", "R_a = None # Replace None with your solution to this exercise." ], @@ -779,7 +779,7 @@ "\n", "Calculate the result of a rotation of $\\theta_{b1} = \\frac{\\pi}{12}$ followed by a rotation of $\\theta_{b2} = -\\frac{\\pi}{2}$, in the _current_ frame, using elements of SO(2).\n", "\n", - "Store the result in the variable `R_b` shown in the cell below." + "Store the result in the variable `R_b` shown in the cell below.\n" ] }, { @@ -791,8 +791,8 @@ }, "cell_type": "code", "source": [ - "θ_b1 = pi/12.0 # As given in the exercise\n", - "θ_b2 = -pi/2.0 # As given in the exercise\n", + "\u03b8_b1 = pi/12.0 # As given in the exercise\n", + "\u03b8_b2 = -pi/2.0 # As given in the exercise\n", "\n", "R_b = None # Replace None with your solution to this exercise." ], @@ -819,9 +819,10 @@ " \\sin{\\theta_c} & \\cos{\\theta_c}\n", " \\end{array}\\right].$$\n", "\n", - "Starting at the World frame, calculate the homogenous transformation representing the rotation $R(\\theta_c)$, about the _World_ frame, followed by the translation $\\myvec{p}_c$, in the _current_ frame.\n", + "Starting at the World frame, calculate the homogeneous transformation representing the rotation $R(\\theta_c)$, about the _World_ frame, followed by the translation $\\myvec{p}_c$, in the _current_ frame.\n", "\n", - "Consider $\\theta_c = \\frac{\\pi}{3}$, $x_c = 2$, $y_c = 5$, and store the result in the variable `H_c` shown in the cell below.\n" + "Consider $\\theta_c = \\frac{\\pi}{3}$, $x_c = 2$, $y_c = 5$, and store the result in the variable `H_c` shown in the cell below.\n", + "\n" ] }, { @@ -833,7 +834,7 @@ }, "cell_type": "code", "source": [ - "θ_c = pi/3.0 # As given in the exercise\n", + "\u03b8_c = pi/3.0 # As given in the exercise\n", "x_c = 2.0 # As given in the exercise\n", "y_c = 5.0 # As given in the exercise\n", "\n", @@ -850,7 +851,7 @@ "\n", "Consider the same variables as in `Exercise c` by replacing the subscripts with `d`. Calculate, instead, the translation followed by the rotation. Store the result in the variable `H_d` shown in the cell below.\n", "\n", - "Is `H_c` the same as `H_d`? What does that indicate?" + "Is `H_c` the same as `H_d`? What does that indicate?\n" ] }, { @@ -862,7 +863,7 @@ }, "cell_type": "code", "source": [ - "θ_d = pi/3.0 # As given in the exercise\n", + "\u03b8_d = pi/3.0 # As given in the exercise\n", "x_d = 2.0 # As given in the exercise\n", "y_d = 5.0 # As given in the exercise\n", "\n", @@ -883,7 +884,7 @@ " - The first transformation is a rotation of $\\theta=\\frac{\\pi}{10}$ about the $z$-axis of the _World_ frame.\n", " - The second transformation is a translation of $d=0.3$ about the $z$-axis of the _current_ frame.\n", " - The third transformation is a translation of $a=0.5$ about the $x$-axis of the _current_ frame.\n", - " - The forth and last transformation is a rotation of $\\alpha=-\\frac{\\pi}{2}$ about the $x$-axis of the _current_ frame." + " - The fourth and last transformation is a rotation of $\\alpha=-\\frac{\\pi}{2}$ about the $x$-axis of the _current_ frame.\n" ] } ], @@ -908,4 +909,4 @@ }, "nbformat": 4, "nbformat_minor": 4 -} +} \ No newline at end of file diff --git a/basic_lessons/lesson3_exercise_answers.ipynb b/basic_lessons/lesson3_exercise_answers.ipynb index 3b01e4d..44aa719 100644 --- a/basic_lessons/lesson3_exercise_answers.ipynb +++ b/basic_lessons/lesson3_exercise_answers.ipynb @@ -254,4 +254,4 @@ }, "nbformat": 4, "nbformat_minor": 2 -} +} \ No newline at end of file diff --git a/basic_lessons/lesson3_tutorial.ipynb b/basic_lessons/lesson3_tutorial.ipynb index 7124610..3086914 100644 --- a/basic_lessons/lesson3_tutorial.ipynb +++ b/basic_lessons/lesson3_tutorial.ipynb @@ -10,16 +10,16 @@ "\n", "*Author: Murilo M. Marinho (murilo.marinho@manchester.ac.uk)*\n", "\n", - "## Pre-requisites for the learner\n", + "## Prerequisites for the learner\n", "The user of this notebook is expected to have prior knowledge in\n", - "- All the content and pre-requisites of lessons 1 and 2.\n", + "- All the content and prerequisites of lessons 1 and 2.\n", "\n", "## I found an issue\n", "Thank you! Please report it at https://github.com/MarinhoLab/OpenExecutableBooksRobotics/issues\n", "\n", "#### Latex Macros\n", "$\\providecommand{\\myvec}[1]{{\\mathbf{\\boldsymbol{{#1}}}}}$\n", - "$\\providecommand{\\mymatrix}[1]{{\\mathbf{\\boldsymbol{{#1}}}}}$" + "$\\providecommand{\\mymatrix}[1]{{\\mathbf{\\boldsymbol{{#1}}}}}$\n" ] }, { @@ -38,7 +38,7 @@ "cell_type": "markdown", "metadata": {}, "source": [ - "# Pre-requisites" + "# Prerequisites\n" ] }, { @@ -56,7 +56,7 @@ "cell_type": "markdown", "metadata": {}, "source": [ - "# Imports" + "# Imports\n" ] }, { @@ -82,52 +82,7 @@ "cell_type": "markdown", "metadata": {}, "source": [ - "# Forward Kinematics Model (FKM)\n", - "\n", - "The forward kinematics model of a rigid serial-link manipulator is obtained through a sequence of transformations.\n", - "\n", - "The only real challenge in obtaining the FKM is understanding from a diagram, or a real robot, what transformations represent the robot and in what order they happen.\n", - "\n", - "Anyway, we can start with an example. As always, remember that angles are in radians and lengths are in meters.\n", - "\n", - "![Lesson4.png](Lesson4.png)\n", - "\n", - "Consider the 2-DoF planar robot shown in the figure. It is classed as an RR robot, because the two joints are revolute.\n", - "\n", - "Let $q_0\\triangleq q_0(t) \\in \\mathbb{R}$ and $q_1\\triangleq q_1(t) \\in \\mathbb{R}$ compose its configuration space. In addition, let $l_{0} \\in \\mathbb{R}$ and $l_{1} \\in \\mathbb{R}$ be the geometric parameters, which are quantities that cannot be controlled.\n", - "\n", - "The configuration space is what is used in practice to control the robot. You as the system designer will send configuration space values $q_0$ and $q_1$, or other signals related to those, to command the robot. You will make it move to perform a relevant task and hopefully earn your next month's salary. The parameters $l_{0}$ and $l_{1}$ are constant in time and represent time-invariant geometrical aspects of the robot, such as link lengths, that you cannot control.\n", - "\n", - "As a representative task for robotic manipulators, let us use the configuration space and geometric parameters to calculate the pose of the frame of the tip of the robot. This is represented mathematically as follows.\n", - "\n", - "$$\\mymatrix{H}^{0}_{2}( q_0, l_{0},q_1,l_{1}) \\in SE(2).$$\n", - "\n", - "The equation for the end-effector (tip) pose is what is called the forward kinematics model (FKM). We need this frequently when using a robotic manipulator because the end effector is likely to be its most useful part. For instance, it could be a gripper that is used to pick and place objects. To pick or place and object, the robot needs to move somewhere.\n", - "\n", - "The first step towards moving somewhere is knowing where you are. Thence, the first step towards controlling a robotic manipulator's end effector pose in any meaningful way is to obtain its FKM.\n", - "\n", - "## Understanding the problem\n", - "\n", - "The FKM is a mathematical description of the robot. Before we attempt any programming, we have to mathematically describe the sequential transformations that represent the robot being modeled.\n", - "\n", - "As shown in the figure, there are four transformations for this robot, taking us from the base, $\\mathcal{F}_0$, to the end-effector, $\\mathcal{F}_2$. The sequence can be summarised as follows.\n", - "\n", - "1. A rotation of $q_0$ about the current frame, from $\\mathcal{F}_0$ to $\\mathcal{F}_{0'}$.\n", - "2. A translation of $l_0$ along the $x$-axis of the current frame, from $\\mathcal{F}_{0'}$ to $\\mathcal{F}_{1}$.\n", - "3. A rotation of $q_1$ about the current frame, from $\\mathcal{F}_{1}$ to $\\mathcal{F}_{1'}$.\n", - "4. A translation of $l_1$ along the $x$-axis of the current frame, from $\\mathcal{F}_{1'}$ to $\\mathcal{F}_{2}$.\n", - "\n", - "### 1. From $\\mathcal{F}_0$ to $\\mathcal{F}_{0'}$\n", - "\n", - "We start with the rotation that can be described by the following homogenous transformation matrix.\n", - "\n", - "$$\\myvec H_{0'}^{0}\\left(q_0\\right)\t=\n", - "\\begin{bmatrix} \\cos(q_0) & -\\sin(q_0) & 0\\\\\n", - "\\sin(q_0) & \\cos(q_0) & 0\\\\\n", - "0 & 0 & 1\n", - "\\end{bmatrix}.$$\n", - "\n", - "Programmatically, supposing that $q_0 = \\frac{\\pi}{4}$, we arrive at the following piece of code." + "# Forward Kinematics Model (FKM)The forward kinematics model of a rigid serial-link manipulator is obtained through a sequence of transformations.The only real challenge in obtaining the FKM is understanding from a diagram, or a real robot, what transformations represent the robot and in what order they happen.Anyway, we can start with an example. As always, remember that angles are in radians and lengths are in meters.![Lesson4.png](Lesson4.png)Consider the 2-DoF planar robot shown in the figure. It is classed as an RR robot, because the two joints are revolute.Let $q_0\\triangleq q_0(t) \\in \\mathbb{R}$ and $q_1\\triangleq q_1(t) \\in \\mathbb{R}$ compose its configuration space. In addition, let $l_{0} \\in \\mathbb{R}$ and $l_{1} \\in \\mathbb{R}$ be the geometric parameters, which are quantities that cannot be controlled.The configuration space is what is used in practice to control the robot. You as the system designer will send configuration space values $q_0$ and $q_1$, or other signals related to those, to command the robot. You will make it move to perform a relevant task and hopefully earn your next month's salary. The parameters $l_{0}$ and $l_{1}$ are constant in time and represent time-invariant geometrical aspects of the robot, such as link lengths, that you cannot control.As a representative task for robotic manipulators, let us use the configuration space and geometric parameters to calculate the pose of the frame of the tip of the robot. This is represented mathematically as follows.$$\\mymatrix{H}^{0}_{2}( q_0, l_{0},q_1,l_{1}) \\in SE(2).$$The equation for the end-effector (tip) pose is what is called the forward kinematics model (FKM). We need this frequently when using a robotic manipulator because the end effector is likely to be its most useful part. For instance, it could be a gripper that is used to pick and place objects. To pick or place an object, the robot needs to move somewhere.The first step towards moving somewhere is knowing where you are. Thence, the first step towards controlling a robotic manipulator's end effector pose in any meaningful way is to obtain its FKM.## Understanding the problemThe FKM is a mathematical description of the robot. Before we attempt any programming, we have to mathematically describe the sequential transformations that represent the robot being modelled.As shown in the figure, there are four transformations for this robot, taking us from the base, $\\mathcal{F}_0$, to the end-effector, $\\mathcal{F}_2$. The sequence can be summarised as follows.1. A rotation of $q_0$ about the current frame, from $\\mathcal{F}_0$ to $\\mathcal{F}_{0'}$.2. A translation of $l_0$ along the $x$-axis of the current frame, from $\\mathcal{F}_{0'}$ to $\\mathcal{F}_{1}$.3. A rotation of $q_1$ about the current frame, from $\\mathcal{F}_{1}$ to $\\mathcal{F}_{1'}$.4. A translation of $l_1$ along the $x$-axis of the current frame, from $\\mathcal{F}_{1'}$ to $\\mathcal{F}_{2}$.### 1. From $\\mathcal{F}_0$ to $\\mathcal{F}_{0'}$We start with the rotation that can be described by the following homogeneous transformation matrix.$$\\myvec H_{0'}^{0}\\left(q_0\\right)\t=\\begin{bmatrix} \\cos(q_0) & -\\sin(q_0) & 0\\\\\\sin(q_0) & \\cos(q_0) & 0\\\\0 & 0 & 1\\end{bmatrix}.$$Programmatically, supposing that $q_0 = \\frac{\\pi}{4}$, we arrive at the following piece of code.\n" ] }, { @@ -153,7 +108,7 @@ "source": [ "### 2. From $\\mathcal{F}_{0'}$ to $\\mathcal{F}_{1}$\n", "\n", - "The second step is a translation that can be described by the following homogenous transformation matrix.\n", + "The second step is a translation that can be described by the following homogeneous transformation matrix.\n", "\n", "$$\\myvec H_{1}^{0'}\\left(l_0\\right)\t=\n", "\\begin{bmatrix}\n", @@ -162,7 +117,7 @@ "0 & 0 & 1\n", "\\end{bmatrix}.$$\n", "\n", - "Programmatically, supposing that $l_0 = 0.3$, we arrive at the following piece of code." + "Programmatically, supposing that $l_0 = 0.3$, we arrive at the following piece of code.\n" ] }, { @@ -186,7 +141,7 @@ "source": [ "### 3. From $\\mathcal{F}_{1}$ to $\\mathcal{F}_{1'}$\n", "\n", - "The third step is a rotation that can be described by the following homogenous transformation matrix.\n", + "The third step is a rotation that can be described by the following homogeneous transformation matrix.\n", "\n", "$$\\myvec H_{1'}^{1}\\left(q_1\\right)\t=\n", "\\begin{bmatrix} \\cos(q_1) & -\\sin(q_1) & 0\\\\\n", @@ -194,7 +149,7 @@ "0 & 0 & 1\n", "\\end{bmatrix}.$$\n", "\n", - "Programmatically, supposing that $q_1 = -\\frac{\\pi}{14}$, we arrive at the following piece of code." + "Programmatically, supposing that $q_1 = -\\frac{\\pi}{14}$, we arrive at the following piece of code.\n" ] }, { @@ -218,7 +173,7 @@ "source": [ "### 4. From $\\mathcal{F}_{1'}$ to $\\mathcal{F}_{2}$\n", "\n", - "The last step is a translation that can be described by the following homogenous transformation matrix.\n", + "The last step is a translation that can be described by the following homogeneous transformation matrix.\n", "\n", "$$\\myvec H_{2}^{1'}\\left(l_1\\right)\t=\n", "\\begin{bmatrix}\n", @@ -227,7 +182,7 @@ "0 & 0 & 1\n", "\\end{bmatrix}.$$\n", "\n", - "Programmatically, supposing that $l_1 = 0.95$, we arrive at the following piece of code." + "Programmatically, supposing that $l_1 = 0.95$, we arrive at the following piece of code.\n" ] }, { @@ -257,7 +212,7 @@ "\n", "The equation above is general and is the FKM for this robot.\n", "\n", - "Programmatically, we will compute the FKM at a given configuration. Using the configuration and parameters defined previously, we arrive at the following piece of code." + "Programmatically, we will compute the FKM at a given configuration. Using the configuration and parameters defined previously, we arrive at the following piece of code.\n" ] }, { @@ -351,7 +306,7 @@ "$$\n", "\\mymatrix{H}^{B0}_{B2}(q_{B0},q_{B1})=\\mymatrix{H}^{B0}_{B0'}(q_{B0})\\mymatrix{H}^{B0'}_{B1}\\mymatrix{H}^{B1}_{B2}(q_{B1}).$$\n", "\n", - "Programmatically, suppose $q_{B0}=0.2$ and $q_{B1}=0.3$. We obtain the following piece of code." + "Programmatically, suppose $q_{B0}=0.2$ and $q_{B1}=0.3$. We obtain the following piece of code.\n" ] }, { @@ -408,7 +363,7 @@ "| 0 | $q_{A0}(t)$ | 0.5 | 0 | $\\frac{\\pi}{2}$ |\n", "| 1 | 0 | $q_{A1}(t)$ | 0 | 0 |\n", "\n", - "Compute its FKM in SE(3) and store in `H_A0_A2` the result of the FKM given $q_{A0}(t) = \\frac{\\pi}{4}$ and $q_{A1}(t) = -0.1$." + "Compute its FKM in SE(3) and store in `H_A0_A2` the result of the FKM given $q_{A0}(t) = \\frac{\\pi}{4}$ and $q_{A1}(t) = -0.1$.\n" ] }, { @@ -438,7 +393,7 @@ "| 1 | $q_{C1}(t)$ | $l_{C1}$ | 0 | 0 |\n", "| 2 | $q_{C2}(t)$ | $l_{C2}$ | 0 | 0 |\n", "\n", - "Compute its FKM in SE(3) and store in `H_C0_C3` the result of the FKM given $q_{C0}(t)=q_{C1}(t)=q_{C2}(t) = \\frac{\\pi}{5}$ and $l_{C0} = l_{C1} = l_{C2} = 0.25$." + "Compute its FKM in SE(3) and store in `H_C0_C3` the result of the FKM given $q_{C0}(t)=q_{C1}(t)=q_{C2}(t) = \\frac{\\pi}{5}$ and $l_{C0} = l_{C1} = l_{C2} = 0.25$.\n" ] }, { @@ -479,4 +434,4 @@ }, "nbformat": 4, "nbformat_minor": 2 -} +} \ No newline at end of file diff --git a/basic_lessons/lesson4_exercise_answers.ipynb b/basic_lessons/lesson4_exercise_answers.ipynb index 708f75d..08980de 100644 --- a/basic_lessons/lesson4_exercise_answers.ipynb +++ b/basic_lessons/lesson4_exercise_answers.ipynb @@ -359,4 +359,4 @@ }, "nbformat": 4, "nbformat_minor": 2 -} +} \ No newline at end of file diff --git a/basic_lessons/lesson4_tutorial.ipynb b/basic_lessons/lesson4_tutorial.ipynb index 46adef9..396ede0 100644 --- a/basic_lessons/lesson4_tutorial.ipynb +++ b/basic_lessons/lesson4_tutorial.ipynb @@ -10,16 +10,16 @@ "\n", "*Author: Murilo M. Marinho (murilo.marinho@manchester.ac.uk)*\n", "\n", - "## Pre-requisites for the learner\n", + "## Prerequisites for the learner\n", "The user of this notebook is expected to have prior knowledge in\n", - "- All the content and pre-requisites of lessons 1, 2, and 3.\n", + "- All the content and prerequisites of lessons 1, 2, and 3.\n", "\n", "## I found an issue\n", "Thank you! Please report it at https://github.com/MarinhoLab/OpenExecutableBooksRobotics/issues\n", "\n", "#### Latex Macros\n", "$\\providecommand{\\myvec}[1]{{\\mathbf{\\boldsymbol{{#1}}}}}$\n", - "$\\providecommand{\\mymatrix}[1]{{\\mathbf{\\boldsymbol{{#1}}}}}$" + "$\\providecommand{\\mymatrix}[1]{{\\mathbf{\\boldsymbol{{#1}}}}}$\n" ] }, { @@ -38,7 +38,7 @@ "cell_type": "markdown", "metadata": {}, "source": [ - "# Package installation" + "# Package installation\n" ] }, { @@ -61,7 +61,7 @@ "cell_type": "markdown", "metadata": {}, "source": [ - "# Imports" + "# Imports\n" ] }, { @@ -205,7 +205,7 @@ "\n", "## Step 3: Computing the Jacobian\n", "\n", - "We're now equipped to solve the first question by doing the following" + "We're now equipped to solve the first question by doing the following\n" ] }, { @@ -256,7 +256,7 @@ "cell_type": "markdown", "metadata": {}, "source": [ - "With the correct definition of the Jacobian as above, we can calculate the second question as " + "With the correct definition of the Jacobian as above, we can calculate the second question as \n" ] }, { @@ -298,7 +298,7 @@ "\n", "1. What about if the robot had 3 degrees-of-freedom, that is RRR?\n", "2. What if the robot has one or more prismatic joints?\n", - "3. **Challenge.** What about if the robot had $n$ revolute degrees-of-freedom? Would it be much more complicated to solve?" + "3. **Challenge.** What about if the robot had $n$ revolute degrees-of-freedom? Would it be much more complicated to solve?\n" ] } ], @@ -323,4 +323,4 @@ }, "nbformat": 4, "nbformat_minor": 2 -} +} \ No newline at end of file diff --git a/basic_lessons/lesson5_exercise_answers.ipynb b/basic_lessons/lesson5_exercise_answers.ipynb index 4028b59..a6ee936 100644 --- a/basic_lessons/lesson5_exercise_answers.ipynb +++ b/basic_lessons/lesson5_exercise_answers.ipynb @@ -352,4 +352,4 @@ }, "nbformat": 4, "nbformat_minor": 2 -} +} \ No newline at end of file diff --git a/basic_lessons/lesson5_tutorial.ipynb b/basic_lessons/lesson5_tutorial.ipynb index f30d966..87e9181 100644 --- a/basic_lessons/lesson5_tutorial.ipynb +++ b/basic_lessons/lesson5_tutorial.ipynb @@ -10,16 +10,16 @@ "\n", "*Author: Murilo M. Marinho (murilo.marinho@manchester.ac.uk)*\n", "\n", - "## Pre-requisites for the learner\n", + "## Prerequisites for the learner\n", "The user of this notebook is expected to have prior knowledge in\n", - "- All the content and pre-requisites of lessons 1, 2, 3, and 4.\n", + "- All the content and prerequisites of lessons 1, 2, 3, and 4.\n", "\n", "## I found an issue\n", "Thank you! Please report it at https://github.com/MarinhoLab/OpenExecutableBooksRobotics/issues\n", "\n", "#### Latex Macros\n", "$\\providecommand{\\myvec}[1]{{\\mathbf{\\boldsymbol{{#1}}}}}$\n", - "$\\providecommand{\\mymatrix}[1]{{\\mathbf{\\boldsymbol{{#1}}}}}$" + "$\\providecommand{\\mymatrix}[1]{{\\mathbf{\\boldsymbol{{#1}}}}}$\n" ] }, { @@ -38,7 +38,7 @@ "cell_type": "markdown", "metadata": {}, "source": [ - "# Package installation" + "# Package installation\n" ] }, { @@ -56,7 +56,7 @@ "cell_type": "markdown", "metadata": {}, "source": [ - "# Imports" + "# Imports\n" ] }, { @@ -111,7 +111,7 @@ " \\phi_{z}\n", " \\end{array}\\right],$$\n", "\n", - "in which $p_{x}$, $p_{y}$, and $\\phi_{z}$ are, respectively, the $x$-axis position, the $y$-axis position, and the $z$-axis angle of $\\mathcal{F}_{2}$. Notice that $\\dot{l}_{0}=\\dot{l}_{1}=0$. " + "in which $p_{x}$, $p_{y}$, and $\\phi_{z}$ are, respectively, the $x$-axis position, the $y$-axis position, and the $z$-axis angle of $\\mathcal{F}_{2}$. Notice that $\\dot{l}_{0}=\\dot{l}_{1}=0$. \n" ] }, { @@ -134,7 +134,7 @@ "\\phi_{z} & = q_0 + q_1.\n", "\\end{align}$$\n", "\n", - "With those, we can obtain the task space values with a function called `planar_robot_fkm` so that it can be easily reused" + "With those, we can obtain the task space values with a function called `planar_robot_fkm` so that it can be easily reused\n" ] }, { @@ -170,7 +170,7 @@ "\n", "### Consider the analytical Jacobian\n", "\n", - "We first calculate the Jacobian by hand, because programatically there's nothing for you to do yet. As we did in the previous tutorial, here is the Jacobian\n", + "We first calculate the Jacobian by hand, because programmatically there's nothing for you to do yet. As we did in the previous tutorial, here is the Jacobian\n", "\n", "$$ \\mymatrix{J} = \\left[\\begin{array}{ccc}\n", " -l_{0}\\sin{q_0} - l_{1}\\sin{(q_0 + q_1)} & -l_{1}\\sin{(q_0 + q_1)} \\\\\n", @@ -178,7 +178,7 @@ " 1 & 1 \n", " \\end{array}\\right].$$\n", "\n", - "We transform this Jacobian calculation into the function `planar_robot_jacobian` shown below" + "We transform this Jacobian calculation into the function `planar_robot_jacobian` shown below\n" ] }, { @@ -224,7 +224,7 @@ "\n", "$$\\tilde{\\myvec{x}}= \\myvec{x} - \\myvec{x}_d$$\n", "\n", - "that can be implemented as the function `get_error` below" + "that can be implemented as the function `get_error` below\n" ] }, { @@ -320,16 +320,14 @@ "cell_type": "markdown", "metadata": {}, "source": [ - "# Optional: Print the data" + "# Optional: Print the data\n" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ - "To help visualise the behavior of the controller, you can optionally plot the data. These data show that with the defined parameters the qualitative behavior is very close to exponential decay. \n", - "\n", - "However the error does not converge to zero, meaning that the manipulator did not reach its target. Indeed, this is usually the case when the target is outside the reach of the manipulator or all task-space desired values cannot be simultenously satisfied." + "To help visualise the behaviour of the controller, you can optionally plot the data. These data show that with the defined parameters the qualitative behaviour is very close to exponential decay. However the error does not converge to zero, meaning that the manipulator did not reach its target. Indeed, this is usually the case when the target is outside the reach of the manipulator or all task-space desired values cannot be simultaneously satisfied.\n" ] }, { @@ -362,13 +360,13 @@ "source": [ "# Alternative Jacobian Inversion Strategies\n", "\n", - "The SVD-based Moore-Penrose pseudo-invese is a common strategy for matrix inversion. It is important to notice, however, that is is not the only one. In addition, it is not always the best choice for robot control.\n", + "The SVD-based Moore-Penrose pseudo-inverse is a common strategy for matrix inversion. It is important to notice, however, that this is not the only one. In addition, it is not always the best choice for robot control.\n", "\n", - "Another common strategy is the so-called damped pseudo-inverse, described below. It is usually the easiest choice to embbed robustness to singularities in the controller.\n", + "Another common strategy is the so-called damped pseudo-inverse, described below. It is usually the easiest choice to embed robustness to singularities in the controller.\n", "\n", "$$\\mymatrix{J}^{\\dagger}=\\mymatrix{J}^T\\left(\\mymatrix{J}\\mymatrix{J}^T + \\lambda^2\\mymatrix{I}\\right)^{-1},$$\n", "\n", - "defined in the function `damped_pseudo_inverse()` below in which we call $\\lambda$ as the variable `damping`" + "defined in the function `damped_pseudo_inverse()` below in which we call $\\lambda$ as the variable `damping`\n" ] }, { @@ -389,7 +387,7 @@ "cell_type": "markdown", "metadata": {}, "source": [ - "with that, our controller becomes, changing only the inversion strategy," + "with that, our controller becomes, changing only the inversion strategy,\n" ] }, { @@ -477,9 +475,10 @@ "\n", "in continuous time, hence the convergence depends only on $\\eta$.\n", "\n", - "However, given that our implementation is in discrete time, the convergence is affected by our sampling time, $T$. In practice, we set $T$ as the fastest sampling time that can be achieved by the hardware, e.g. the robot controller. In many cases, this is about 1 milisecond in pratice.\n", + "However, given that our implementation is in discrete time, the convergence is affected by our sampling time, $T$. In practice, we set $T$ as the fastest sampling time that can be achieved by the hardware, e.g. the robot controller. In many cases, this is about 1 millisecond in practice.\n", "\n", "The effect on the convergence for different $\\eta$ can be seen, computationally, below\n", + "\n", "\n" ] }, @@ -574,7 +573,7 @@ "\n", "Engineering is the art of trade-off. Whenever we have a large $\\eta$, that means the configuration-space velocities will comparatively be higher.\n", "\n", - "Using our example, we can see that the control signal norm is heavily affected by $\\eta$. Therefore, for feasibility, it is important to keep the $\\eta$ so that the system can handle it. In fact, gains that are too high are one of the major risks that can break robots or hurt people when real hardware is used without the proper safeguards." + "Using our example, we can see that the control signal norm is heavily affected by $\\eta$. Therefore, for feasibility, it is important to keep the $\\eta$ so that the system can handle it. In fact, gains that are too high are one of the major risks that can break robots or hurt people when real hardware is used without the proper safeguards.\n" ] }, { @@ -618,7 +617,7 @@ "- Obtaining the analytical Jacobian so that the kinematic control can be applied.\n", "- The kinematic control itself, aside from dimensions, does not need to change in general.\n", "\n", - "For instance, we can even solve with no diagram as long as this information is given. Consider an PRR manipulator with configuration space\n", + "For instance, we can even solve with no diagram as long as this information is given. Consider a PRR manipulator with configuration space\n", "\n", "$$\\mathbb{R}^3 \\ni \\myvec{q}_C \\triangleq \\left[\\begin{array}{ccc}\n", " q_0 \\\\\n", @@ -627,7 +626,7 @@ " \\end{array}\\right].$$\n", "\n", "\n", - "And link lenghts $l_{0}, l_{1}, l_{2}\\in \\mathbb{R}$.\n", + "And link lengths $l_{0}, l_{1}, l_{2}\\in \\mathbb{R}$.\n", "\n", "## Task Space & Forward Kinematics Model (FKM)\n", "\n", @@ -648,7 +647,7 @@ " 0 & 0 & 1\n", " \\end{array}\\right].$$\n", "\n", - "Because this robot is planar, the following task space is necessary and sufficient to fully describe the reacheable space\n", + "Because this robot is planar, the following task space is necessary and sufficient to fully describe the reachable space\n", "\n", "$$\\myvec{x} = \\left[\\begin{array}{ccc}\n", " p_{x} \\\\\n", @@ -664,7 +663,7 @@ "\\phi_{z} & = q_1 + q_2,\n", "\\end{align*}$$\n", "\n", - "which means that the FKM, $\\myvec{x}=f(\\myvec{q})$, in this case is equivalent to" + "which means that the FKM, $\\myvec{x}=f(\\myvec{q})$, in this case is equivalent to\n" ] }, { @@ -713,7 +712,7 @@ " 1 & - l_{1}s_{1} - l_{2}s_{12} & -l_{2}s_{12}\\\\\n", " 0 & l_{1}c_{1} + l_{2}c_{12} & l_{2}c_{12}\\\\\n", " 0 & 1 & 1 \n", - " \\end{array}\\right].$$" + " \\end{array}\\right].$$\n" ] }, { @@ -772,7 +771,7 @@ "\n", "
\n", "The rest of the control loop is unchanged! This is one of the advantages of kinematic control.\n", - "
" + "\n" ] }, { @@ -873,7 +872,7 @@ "Try to modify the code above to calculate the control action for a:\n", "\n", "1. `PP` robot\n", - "2. `RP` robot" + "2. `RP` robot\n" ] } ], @@ -898,4 +897,4 @@ }, "nbformat": 4, "nbformat_minor": 2 -} +} \ No newline at end of file diff --git a/convert_to_myst.py b/convert_to_myst.py new file mode 100644 index 0000000..e1f3777 --- /dev/null +++ b/convert_to_myst.py @@ -0,0 +1,137 @@ +#!/usr/bin/env python3 +"""Convert Jupyter notebooks (.ipynb) to MyST text notebooks (.md). + +Each code cell becomes a ````{code-cell}```` directive. +Markdown cells are preserved as-is. +Raw cells and latex macro cells are removed (handled by myst.yml). +""" + +import json +import re +import sys +from pathlib import Path + + +def _join_source(source): + """Join cell source lines, ensuring proper newlines between them. + + Handles both formats: + - ["line1\n", "line2\n"] (original ipynb format) + - ["line1", "line2"] (json.dump re-saved without trailing newlines) + """ + if isinstance(source, str): + return source + parts = list(source) + if not parts: + return "" + # Check if lines already have trailing newlines + if parts and parts[0].endswith("\n"): + return "".join(parts) + # No trailing newlines — join with \n and add one at the end + return "\n".join(parts) + "\n" + + +def notebook_to_myst(nb_path: Path, output_path: Path, title_prefix: str = ""): + """Convert a single notebook to a MyST text notebook.""" + with open(nb_path) as f: + nb = json.load(f) + + lines: list[str] = [] + + # Frontmatter + lines.append("---") + lines.append("kernelspec:") + lines.append(" name: python3") + lines.append(" display_name: 'Python 3'") + lines.append("---") + lines.append("") + + for cell in nb["cells"]: + cell_type = cell["cell_type"] + source = _join_source(cell["source"]) + + if not source.strip(): + continue + + # Skip raw cells + if cell_type == "raw": + continue + + # Skip latex macro definition cells + lower_src = source.lower().strip() + if "providecommand" in lower_src and ("myvec" in lower_src or "mymatrix" in lower_src): + continue + + if cell_type == "markdown": + # Fix ipynb attachment syntax: ![img](attachment:img.png) -> ![img](img.png) + fixed = re.sub( + r'!\[(.*?)\]\(attachment:(.*?)\)', + r'![\1](\2)', + source, + ) + lines.append(fixed.rstrip("\n")) + lines.append("") + + elif cell_type == "code": + code = source.rstrip("\n") + lines.append("````{code-cell}") + lines.append(code) + lines.append("````") + lines.append("") + + # Remove trailing blank lines but keep one + while len(lines) > 1 and not lines[-1].strip(): + lines.pop() + lines.append("") + + output_path.write_text("\n".join(lines), encoding="utf-8") + print(f" {nb_path} -> {output_path}") + + +def main(): + base = Path(__file__).parent + src_dir = base / "basic_lessons" + dst_dir = base / "unstable" + dst_dir.mkdir(exist_ok=True) + + # Copy images + for img in src_dir.glob("*.*"): + if img.suffix.lower() in (".png", ".svg"): + dst = dst_dir / img.name + dst.write_bytes(img.read_bytes()) + print(f" Copied {img.name}") + + # Convert tutorial notebooks + tutorials = [ + "lesson1_tutorial.ipynb", + "lesson2_tutorial.ipynb", + "lesson3_tutorial.ipynb", + "lesson4_tutorial.ipynb", + "lesson5_tutorial.ipynb", + ] + + exercise_answers = [ + "lesson1_exercise_answers.ipynb", + "lesson2_exercise_answers.ipynb", + "lesson3_exercise_answers.ipynb", + "lesson4_exercise_answers.ipynb", + "lesson5_exercise_answers.ipynb", + ] + + print("Converting tutorials...") + for nb in tutorials: + src = src_dir / nb + dst = dst_dir / nb.replace(".ipynb", ".md") + notebook_to_myst(src, dst) + + print("\nConverting exercise answers...") + for nb in exercise_answers: + src = src_dir / nb + dst = dst_dir / nb.replace(".ipynb", ".md") + notebook_to_myst(src, dst) + + print("\nDone.") + + +if __name__ == "__main__": + main() \ No newline at end of file diff --git a/unstable/.gitignore b/unstable/.gitignore new file mode 100644 index 0000000..650b39f --- /dev/null +++ b/unstable/.gitignore @@ -0,0 +1,2 @@ +# MyST build artifacts +_build/ \ No newline at end of file diff --git a/unstable/Lesson4.png b/unstable/Lesson4.png new file mode 100644 index 0000000..4ca25c7 Binary files /dev/null and b/unstable/Lesson4.png differ diff --git a/unstable/Lesson4.svg b/unstable/Lesson4.svg new file mode 100644 index 0000000..b5df4c6 --- /dev/null +++ b/unstable/Lesson4.svg @@ -0,0 +1,290 @@ + + + + diff --git a/unstable/README.md b/unstable/README.md new file mode 100644 index 0000000..cfedd11 --- /dev/null +++ b/unstable/README.md @@ -0,0 +1,27 @@ +# [WIP] The Basics of Kinematic Modeling and Control of Serial-link Manipulators Using `numpy` + +> **Warning:** These are text-based (MyST) notebooks under active development. The canonical `.ipynb` versions remain in [`basic_lessons/`](../basic_lessons/). + +This directory contains the same five-lesson tutorial as [`basic_lessons/`](../basic_lessons/) but converted to +[MyST text notebooks](https://mystmd.org/guide/notebooks-with-markdown). The content is identical; only the file format +has changed from `.ipynb` to `.md` with `{code-cell}` directives. + +## Contents + +| Number | Title and Link | Content | +|--------|------------------------------|--------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------| +| 1 | [](./lesson1_tutorial.md) | Basic operations in Python and `numpy` | +| 2 | [](./lesson2_tutorial.md) | Learn about elements and operations in $\mathbb{R}^n$, $SO(n)$, and $SE(n)$ with $n\in{\{2,3\}}$ related to positions, orientations, and poses, respectively. | +| 3 | [](./lesson3_tutorial.md) | Learn about the composition of rigid body motion in series to obtain the forward kinematics model of a robotic manipulator. | +| 4 | [](./lesson4_tutorial.md) | Learn about the first-order differential mapping $\dot{\myvec{x}}=\mymatrix{J}\dot{\myvec{q}}$ through the calculation of the Jacobian $\mymatrix{J}$. | +| 5 | [](./lesson5_tutorial.md) | Employ the previous knowledge in all previous lessons to employ a Lyapunov-stable control law to move a manipulator in task space using configuration-space signals. | + +### Exercise Answers + +| Lesson | Link | +|--------|------| +| L1 | [](./lesson1_exercise_answers.md) | +| L2 | [](./lesson2_exercise_answers.md) | +| L3 | [](./lesson3_exercise_answers.md) | +| L4 | [](./lesson4_exercise_answers.md) | +| L5 | [](./lesson5_exercise_answers.md) | \ No newline at end of file diff --git a/unstable/lesson1_exercise_answers.md b/unstable/lesson1_exercise_answers.md new file mode 100644 index 0000000..c9860be --- /dev/null +++ b/unstable/lesson1_exercise_answers.md @@ -0,0 +1,50 @@ +--- +kernelspec: + name: python3 + display_name: 'Python 3' +--- + +# L1 Exercise Answers + +*License: CC-BY-NC-SA 4.0* + +*Author: Murilo M. Marinho (murilo.marinho@manchester.ac.uk)* + +### I found an issue +Thank you! Please report it at https://github.com/MarinhoLab/OpenExecutableBooksRobotics/issues + +### Latex Macros + +# Valid imports + +````{code-cell} +from math import pi, sin, cos +import numpy as np +```` + +# Exercises + +## Exercise 1 + +````{code-cell} +phi = pi/4.0 + +e1 = sin(phi) + 4 * cos(phi / 5) + +# Printing the result is NOT a mandatory part of the answer. +print(f'e1 = {e1}') +```` + +## Exercise 2 + +````{code-cell} +A2 = np.array([[5, 2], + [3, 5]]) +B2 = np.array([[5, 3], + [3, 8]]) + +C2 = A2 + B2 + (A2 @ B2) - (B2 @ A2) + +# Printing the result is NOT a mandatory part of the answer. +print(f'C2 = {C2}') +```` diff --git a/unstable/lesson1_tutorial.md b/unstable/lesson1_tutorial.md new file mode 100644 index 0000000..6fe1d15 --- /dev/null +++ b/unstable/lesson1_tutorial.md @@ -0,0 +1,477 @@ +--- +kernelspec: + name: python3 + display_name: 'Python 3' +--- + +# L1 A quick Python refresher + +*License: CC-BY-NC-SA 4.0* + +*Author: Murilo M. Marinho (murilo.marinho@manchester.ac.uk)* + +### Prerequisites +The user of this notebook is expected to have prior knowledge in +- Basic Python [[Tutorial]](https://docs.python.org/3/tutorial/index.html) +- Numpy + - [[Tutorial: basics for beginners]](https://numpy.org/doc/stable/user/absolute_beginners.html) + - [[Tutorial: for MATLAB users]](https://numpy.org/doc/stable/user/numpy-for-matlab-users.html) +- Jupyter Notebook Basics [[Tutorial]](https://docs.jupyter.org/en/latest/) + +### I found an issue +Thank you! Please report it at https://github.com/MarinhoLab/OpenExecutableBooksRobotics/issues + +### Latex Macros + +# A quick Python refresher + +## Variable assignment + +Let + +$$a\triangleq 10,b\triangleq 5.$$ + +We can replicate the above in Python with + +````{code-cell} +a = 10 +b = 5 +```` + +## Output variables +Variables can be output using `print`. For example, for $a$ + +````{code-cell} +print(a) +```` + + +### Output text and variables using f-strings +To output $a$ and $b$ within a string, we can use `print` and f-strings as follows + +````{code-cell} +print(f'The value of a = {a} and b = {b}.') +```` + +# Basic Arithmetics + +Basic mathematical operations are trivially performed as follows. + +#### Sum + +$$c = a + b.$$ + +````{code-cell} +c = a + b + +print(f'c={c}') +```` + +#### Subtraction +$$c = a - b.$$ + +````{code-cell} +c = a - b + +print(f'c={c}') +```` + +#### Multiplication +$$c=ab$$ + +````{code-cell} +c = a * b + +print(f'c={c}') +```` + +#### Division +$$c = \frac{a}{b}$$ + +````{code-cell} +c = a / b + +print(f'c={c}') +```` + +### Exponentiation +$$c = a^{b}$$ + +````{code-cell} +c = a ** b + +print(f'c={c}') +```` + +### Math functions + +For the following functions, we will need Python's built-in `math` module. + +````{code-cell} +from math import sqrt, exp, log, pi, sin, cos, tan +```` + +### Square root + +$$c = \sqrt{a}$$ + +````{code-cell} +c = sqrt(a) +```` + +### n-th root +The nth root, + +$$c= \sqrt[n]{a}, n \in \mathbb{N},$$ + +does not seem to have a shorthand version in Python, but can be computed through simple properties such as + +$$c = \sqrt[n]{a} = a^{\frac{1}{n}} = e^{\frac{ln(a)}{n}}.$$ + +For example, suppose that + +$$n = 3.$$ + +Then, + +````{code-cell} +n=3 +```` + +and we can calculate the n-th root like so + +````{code-cell} +# n-th root using fractional exponent. Might be easier but most languages do not support a similar syntax +c = a ** (1/n) + +print(f'c={c}') +```` + +or like so + +````{code-cell} +c = exp(log(a)/n) + +print(f'c={c}') +```` + +and both should output the same value. + +### Trigonometric functions + +$$ \phi = \frac{\pi}{4},$$ +$$ s_{\phi} = \sin \left( \phi \right),$$ +$$ c_{\phi} = \cos \left( \phi \right),$$ +$$ t_{\phi} = \tan \left( \phi \right).$$ + +````{code-cell} +phi = pi/4.0 +s_phi = sin(phi) +c_phi = cos(phi) +t_phi = tan(phi) + +print(f'phi={phi}') +print(f's_phi={s_phi}') +print(f'c_phi={c_phi}') +print(f't_phi={t_phi}') +```` + +# Linear Algebra with Numpy + +### Installing the library + +Just in case `numpy` is not already installed, we can install it with the following command. Nothing will happen if the library is already installed. + +````{code-cell} +%%capture +%pip install numpy +%pip install numpy --break-system-packages +```` + +### Importing the library + +````{code-cell} +import numpy as np +```` + +### Instantiating vectors +A row vector can be instantiated from a list of lists. For instance, for +$$\myvec{v} = \left[\begin{array}{ccc} + 1 & 2 + \end{array}\right], +$$ +we have + +````{code-cell} +# Note the double [[]] to instanteate a vector with explicit row shape. +v = np.array([[1, 2]]) + +print(f'v={v}') +```` + +A column vector can be instantiated from a list of singleton *lists*. For instance, for +$$\myvec{u} = \left[\begin{array}{ccc} + 1 \\ + 2 + \end{array}\right],$$ +we have + +````{code-cell} +# Note that each row is defined by a single element within a [], while the whole vector is within an external [] +u = np.array([[1], + [2]]) + +print(f'u={u}') +```` + +### Dot product + +$$\myvec{c} = <\myvec{u},\myvec{u}>$$ + +````{code-cell} +c = np.vdot(u,u) + +print(f'c={c}') +```` + +### Cross product + +Cross product is defined for vectors in $\mathbb{R}^3$. + +For example, for + +$$\myvec{u}_3 = \left[\begin{array}{ccc} + 1 \\ + 2 \\ + 3 + \end{array}\right],$$ + +and + +$$\myvec{v}_3 = \left[\begin{array}{ccc} + 4 \\ + 5 \\ + 6 + \end{array}\right],$$ + +we can obtain the cross product + +$$\myvec{c} = \myvec{u}_3 \times \myvec{v}_3$$ + +````{code-cell} +u3 = np.array([[1, 2, 3]]) +v3 = np.array([[4, 5, 6]]) + +c = np.cross(u3,v3) + +print(f'c={c}') +```` + +### Euclidean norm + +$$\myvec{c} = ||\myvec{u}||$$ + +
+Note that the function is np.linalg.norm, as the norm calculation is within the module linalg. +
+ +````{code-cell} +c = np.linalg.norm(u) + +print(f'c={c}') +```` + +### Instantiating matrices +For instance, suppose that we want to instantiate two real square matrices +$$\mymatrix{A} = \left[\begin{array}{ccc} + 1 & 2 \\ + 3 & 4 + \end{array}\right], +\mymatrix{B} = \left[\begin{array}{ccc} + 5 & 6 \\ + 7 & 8 + \end{array}\right] +$$ + +````{code-cell} +A = np.array([[1, 2], + [3, 4]]) +B = np.array([[5, 6], + [7, 8]]) + +print(f'A={A},\n\nB={B}') +```` + +### Transpose + +$$\mymatrix{C} = \mymatrix{A}^T$$ + +````{code-cell} +C = A.T + +print(f'C={C}') +```` + +### Sum + +$$ \mymatrix{C} = \mymatrix{A} + \mymatrix{B} $$ + +````{code-cell} +C = A + B + +print(f'C={C}') +```` + +### Subtraction + +$$ \mymatrix{C} = \mymatrix{A} - \mymatrix{B} $$ + +````{code-cell} +C = A - B + +print(f'C={C}') +```` + +### Matrix multiplication + +For instance, +$$C = AB$$ +is implemented with + +
+The matrix multiplication operator, @, is very unusual. Pay close attention. +Mistaking this can be a major source of bugs and confusion. +
+ +````{code-cell} +C = A @ B # Alternatively C = np.matmul(A,B), but that is too verbose + +print(f'C={C}') +```` + +which will naturally work for the vectors we defined. For example +$$\myvec{c} = \myvec{u}\myvec{v} = \left[\begin{array}{ccc} + 1 & 2 \\ + 2 & 4 + \end{array}\right], +$$ +$$ \myvec{d} = \myvec{v}\myvec{u} = 5. +$$ + +````{code-cell} +c = u @ v +d = v @ u + +print(f'c={c},\n\nd={d}') +```` + +and, of course, matrices and vectors + +$$ \myvec{c} = \myvec{A}\myvec{u} $$ + +
+We only use the "." sign to denote matrix multiplication when otherwise it would be difficult to read the equation. +
+ +````{code-cell} +c = A @ u + +print(f'c={c},\n') +```` + +### Diagonal matrices + +Diagonal matrices get increasingly sparse with size, so it is important to have shorthand commands for creating them. For instance, suppose that we have the following diagonal matrix + +$$\mymatrix{D} = \left[\begin{array}{ccc} + 1 & 0 & 0 \\ + 0 & 2 & 0 \\ + 0 & 0 & 3 + \end{array}\right] , +$$ + +this can be instantiated in `numpy` with + +````{code-cell} +D = np.diag([1, 2, 3]) + +print(f'D={D}.') +```` + +### Identity matrix + +Among frequently used diagonal matrices, the identity matrix appears frequently. For instance, + +$$ \mymatrix{I}_3 = \left[\begin{array}{ccc} + 1 & 0 & 0 \\ + 0 & 1 & 0 \\ + 0 & 0 & 1 + \end{array}\right], +$$ + +can be instantiated in `numpy` with + +````{code-cell} +I_3 = np.eye(3) + +print(f'I_3={I_3}.') +```` + +### Zero matrix + +Another frequently used matrix is the zero matrix. For instance, + +$$ \mymatrix{O}_3 = \left[\begin{array}{ccc} + 0 & 0 & 0 \\ + 0 & 0 & 0 \\ + 0 & 0 & 0 + \end{array}\right], +$$ + +
+The np.zeros function takes a tuple to generate a properly sized matrix. Do not confuse it with np.eye that accepts a scalar. +
+ +````{code-cell} +O_3 = np.zeros((3,3)) + +print(f'O_3={O_3}.') +```` + +# Exercises + +## Exercise 1 + +For $\phi = \pi/4$, let + +$$ e_1 = \sin(\phi) + 4\cos(\frac{\phi}{5}).$$ + +Using the `math` module which is already imported, calculate the value of $e_1$ and store it in the variable `e1` shown in the cell below. + +````{code-cell} +e1 = None # Replace None with your solution to this exercise. +```` + +## Exercise 2 + +Given + +$$\mymatrix{A}_2 = \left[\begin{array}{ccc} + 5 & 2 \\ + 3 & 5 + \end{array}\right]$$ +and +$$ +\mymatrix{B}_2 = \left[\begin{array}{ccc} + 5 & 3 \\ + 3 & 8 + \end{array}\right].$$ + +Let +$$\mymatrix{C}_2 = \mymatrix{A}_2 + \mymatrix{B}_2 + \mymatrix{A}_2\mymatrix{B}_2 - \mymatrix{B}_2\mymatrix{A}_2.$$ + +Using the numpy module which is already imported, calculate the value of $\mymatrix{C}_2$ and store it in the variable `C2` shown in the cell below. + +````{code-cell} +C2 = None # replace None with your solution to this exercise. +```` diff --git a/unstable/lesson2_exercise_answers.md b/unstable/lesson2_exercise_answers.md new file mode 100644 index 0000000..a49ae1b --- /dev/null +++ b/unstable/lesson2_exercise_answers.md @@ -0,0 +1,155 @@ +--- +kernelspec: + name: python3 + display_name: 'Python 3' +--- + +# L2 Exercise Answers + +*License: CC-BY-NC-SA 4.0* + +*Author: Murilo M. Marinho (murilo.marinho@manchester.ac.uk)* + +### I found an issue +Thank you! Please report it at https://github.com/MarinhoLab/OpenExecutableBooksRobotics/issues + +### Latex Macros + +# Valid imports + +````{code-cell} +from math import pi, sin, cos +import numpy as np +```` + +# Exercises + +## Exercise a + +````{code-cell} +θ_a = pi/4.0 + +R_a = np.array([[cos(θ_a),-sin(θ_a)], + [sin(θ_a), cos(θ_a)]]) + +# Printing the result is NOT a mandatory part of the answer. +print(f'R_a = {R_a}') +```` + +## Exercise b + +````{code-cell} +θ_b1 = pi/12.0 +θ_b2 = -pi/2.0 + +R_b1 = np.array([[cos(θ_b1),-sin(θ_b1)], + [sin(θ_b1), cos(θ_b1)]]) + +R_b2 = np.array([[cos(θ_b2),-sin(θ_b2)], + [sin(θ_b2), cos(θ_b2)]]) + +R_b = R_b1 @ R_b2 + +print(f'R_b = {R_b}') +```` + +## Exercise c + +````{code-cell} +θ_c = pi/3.0 +x_c = 2.0 +y_c = 5.0 + +H_c1 = np.array([[cos(θ_c),-sin(θ_c), 0], + [sin(θ_c), cos(θ_c), 0], + [0, 0, 1]]) + +H_c2 = np.array([[1,0,x_c], + [0,1,y_c], + [0,0,1]]) + +H_c = H_c1 @ H_c2 + +print(f'H_c = {H_c}') +```` + +## Exercise d + +````{code-cell} +θ_d = pi/3.0 +x_d = 2.0 +y_d = 5.0 + + +H_d1 = np.array([[1,0,x_d], + [0,1,y_d], + [0,0,1]]) + +H_d2 = np.array([[cos(θ_d),-sin(θ_d), 0], + [sin(θ_d), cos(θ_d), 0], + [0, 0, 1]]) + + +H_d = H_d1 @ H_d2 + +print(f'H_d = {H_d}') +```` + +`H_c` is *not* the same as `H_d`. This indicates that the order of operations matters. That is, sequential pose transformations are not commutative. + +# Extra challenge 1 + +$$R = \left[\begin{array}{cc} + \cos\left(\sin(t) + 2\cos(t)\right) & -\sin\left(\sin(t) + 2\cos(t)\right) \\ + \sin\left(\sin(t) + 2\cos(t)\right) & \cos\left(\sin(t) + 2\cos(t)\right) + \end{array}\right].$$ + +````{code-cell} +t = 10.0 + +θ = sin(t) + 2 * cos(t) + +R = np.array([[cos(θ),-sin(θ)], + [sin(θ), cos(θ)]]) +```` + +# Extra challenge 2 + +See DH parameters in lesson 3. + +````{code-cell} +θ = pi/10.0 +d = 0.3 +a = 0.5 +α = -pi/2.0 + +H1 = np.array( + [[cos(θ), -sin(θ), 0, 0], + [ sin(θ), cos(θ), 0, 0], + [ 0, 0, 1, 0], + [ 0, 0, 0, 1]] +) + +H2 = np.array( + [[1, 0, 0, 0], + [0, 1, 0, 0], + [0, 0, 1, d], + [0, 0, 0, 1]] +) + +H3 = np.array( + [[1, 0, 0, a], + [0, 1, 0, 0], + [0, 0, 1, 0], + [0, 0, 0, 1]] +) + +H4 = np.array( + [[1, 0, 0, 0], + [0, cos(α), -sin(α), 0], + [0, sin(α), cos(α), 0], + [0, 0, 0, 1]] +) + +H = H1 @ H2 @ H3 @ H4 +```` diff --git a/unstable/lesson2_tutorial.md b/unstable/lesson2_tutorial.md new file mode 100644 index 0000000..1e3419c --- /dev/null +++ b/unstable/lesson2_tutorial.md @@ -0,0 +1,497 @@ +--- +kernelspec: + name: python3 + display_name: 'Python 3' +--- + +# L2 Rigid Body Motion + +*License: CC-BY-NC-SA 4.0* + +*Author: Murilo M. Marinho (murilo.marinho@manchester.ac.uk)* + +## Prerequisites for the learner +The user of this notebook is expected to have prior knowledge in +- All the content and prerequisites of lesson 1. + +## I found an issue +Thank you! Please report it at https://github.com/MarinhoLab/OpenExecutableBooksRobotics/issues + +## Latex Macros + +# Installing prerequisites + +````{code-cell} +%%capture +%pip install numpy +%pip install numpy --break-system-packages +```` + +# Imports + +````{code-cell} +import numpy as np +from math import pi, sin, cos +```` + +# Rigid bodies +If the relative transformation between all points of a given object remain the same regardless of motion, it is a rigid body. + +In other words, the object has no flexibility and the motion of the entire body can be prescribed by its *position* and *orientation* with respect to a given *reference frame*. + +This tends to be the initial topic of robotics textbooks. That is because we can use this to derive the equations of motion for many classes of robots and objects from first principles. + +# Reference frames + +Positions/translations and orientations/rotations of objects are always defined with respect to a reference frame. Reference frames can be attached to rigid bodies or at fixed locations in space. Reference frames are usually defined in such way to make the mathematical derivations simpler. + +In this tutorial, the World (or neutral) reference frame has the following notation + +$$\mathcal{F}.$$ + +**Unless otherwise stated, a given position/orientation/pose is given with respect to the World frame**. When other frames are needed we usually rely on notations such as $$\mathcal{F}',\mathcal{F}''$$ when frames are sequential or $$\mathcal{F}_a,\mathcal{F}_b$$ when relationships are more complex. + +# 2D Position/translation + +Positions/translations in 2D can be uniquely defined as any $$\myvec{p} \in \mathbb{R}^2.$$ Hence, if we would like to define + +$$\myvec{p} = \left[\begin{array}{ccc} + x \\ + y + \end{array}\right] =\left[\begin{array}{ccc} + 1 \\ + 2 + \end{array}\right],$$ + +we can do so programmatically with the following piece of code. + +
+It is common for column and row vectors to not be distinguishable in Python with numpy. That is in general convenient but can cause problems when the dimension is important, so always pay close attention to what each function expects as input. +
+ +````{code-cell} +p = np.array([1.0, 2.0]) + +print(f"p={p}") +```` + +## Composition of translations + +Sequential translations, such as + +$$\mathbb{R}^2 \ni \myvec{p}_i = \left[\begin{array}{ccc} + x_i \\ + y_i + \end{array}\right],$$ + +with $$i \in \mathbb{N}$$ can be composed with sequential additions + +$$\myvec{p} = \myvec{p}_{0} + \myvec{p}_{1} + \myvec{p}_{2} + \myvec{p}_{3}.$$ + +````{code-cell} +p0 = np.array([1.0, 2.0]) +p1 = np.array([2.0, 3.0]) +p2 = np.array([3.0, 4.0]) +p3 = np.array([4.0, 5.0]) + +p = p0 + p1 + p2 + p3 + +print(f"p={p}") +```` + + +## Inverse translation + +The inverse translation can be obtained by subtractions and the element of no translation is the zero vector, that is, if we unwind all translations we're back to the origin of the reference frame + +$$\myvec{p}' = \myvec{p} - \myvec{p}_{0} - \myvec{p}_{1} - \myvec{p}_{2} - \myvec{p}_{3} = \left[\begin{array}{ccc} + 0 \\ + 0 + \end{array}\right].$$ + +````{code-cell} +p0 = np.array([1.0, 2.0]) +p1 = np.array([2.0, 3.0]) +p2 = np.array([3.0, 4.0]) +p3 = np.array([4.0, 5.0]) +p = p0 + p1 + p2 + p3 + +p_ = p - p0 - p1 - p2 - p3 + +print(f"p_={p_}") +```` + +# 2D orientation/rotation + +Orientations/rotations in 2D can be defined in many different ways. In this tutorial, we will address the special orthogonal group for two dimensions, i.e., SO(2). These rotations are defined, in this representation, as a matrix + +$$\mymatrix{R} \in \mathbb{R}^{2 \times 2}.$$ + +The identity rotation means no rotation. For any frame $\mathcal{F}_a$, + +$$\mymatrix{R}^{a}_{a} = \mymatrix{I}_2.$$ + +An element of SO(2) is simply a matrix with the correct properties, therefore we can define one directly in `numpy`. For + +$$\mymatrix{R}(\theta) = \left[\begin{array}{ccc} + \cos{\theta} & -\sin{\theta} \\ + \sin{\theta} & \cos{\theta} + \end{array}\right],$$ + +when $\theta = \frac{\pi}{2}$, we have the following equivalent piece of code. + +````{code-cell} +θ = pi/2 + +R = np.array([[cos(θ),-sin(θ)], + [sin(θ), cos(θ)]]) + +print(f"R={R}") +```` + +# 2D poses (combined translation/orientation) + +2D poses can be represented using elements of SE(2). A translation followed by a rotation can be combined into a single $\mymatrix{H}\in\mathbb{R}^{3 \times 3}$ with the following structure + +$$\mymatrix{H}(x,y,\theta) = +\left[\begin{array}{ccc} + 1 & 0 & x \\ + 0 & 1 & y \\ + 0 & 0 & 1 + \end{array}\right] +\left[\begin{array}{ccc} + \cos{\theta} & -\sin{\theta} & 0 \\ + \sin{\theta} & \cos{\theta} & 0 \\ + 0 & 0 & 1 + \end{array}\right] = +\left[\begin{array}{ccc} + \cos{\theta} & -\sin{\theta} & x \\ + \sin{\theta} & \cos{\theta} & y \\ + 0 & 0 & 1 + \end{array}\right].$$ + +
+The translation and rotation order is extremely important. Check the exercises at the end of this lesson. +
+ +For $\theta = \frac{\pi}{2}$, $x = 0.1$, and $y = 0.2$, we have the following equivalent piece of code. + +````{code-cell} +θ = pi/2 +x = 0.1 +y = 0.2 + +H = np.array([[cos(θ),-sin(θ), x], + [sin(θ), cos(θ), y], + [0, 0, 1]]) + + +print(f"H = {H}") +```` + +# 3D Position/translation + +The 3D position/translations are a trivial extention of the 2D ones with one extra dimension. + +$$\mathbb{R}^3 \ni \myvec{p}_i = \left[\begin{array}{ccc} + x_i \\ + y_i \\ + z_i + \end{array}\right].$$ + +There is nothing surprising in terms of properties, so we will move on to SO(3). + +# 3D orientation/rotation + +For rotational matrices in 3D, we usually compose basic rotations. For rotations about the basis vectors, we have + +$$\mymatrix{R}(z,\theta) = \left[\begin{array}{ccc} + \cos{\theta} & -\sin{\theta} & 0 \\ + \sin{\theta} & \cos{\theta} & 0 \\ + 0 & 0 & 1 + \end{array}\right],$$ + +$$\mymatrix{R}(y,\theta) = \left[\begin{array}{ccc} + \cos{\theta} & 0 & \sin{\theta} \\ + 0 & 1 & 0 \\ + -\sin{\theta} & 0 & \cos{\theta} +\end{array}\right],$$ + +$$\mymatrix{R}(x,\theta) = \left[\begin{array}{ccc} + 1 & 0 & 0 \\ + 0 & \cos{\theta} & -\sin{\theta} \\ + 0 & \sin{\theta} & \cos{\theta} +\end{array}\right].$$ + +````{code-cell} +Rz = np.array([[cos(θ),-sin(θ), 0], + [sin(θ), cos(θ), 0], + [0, 0, 1]]) + +Ry = np.array([[ cos(θ), 0, sin(θ)], + [ 0, 1, 0], + [-sin(θ), 0, cos(θ)]]) + +Rx = np.array([[1, 0, 0], + [0, cos(θ), -sin(θ)], + [0, sin(θ), cos(θ)]]) + +# A rotation about z +print(f"Rz={Rz}") + +# A rotation about y +print(f"Ry={Ry}") + +# A rotation about x +print(f"Rx={Rx}") +```` + +## Compositions of rotations in SO(3) + +Compositions in SO(3) follow the same rules as SO(2), where sequential rotations are represented by right multiplications. For instance, + +$$\mymatrix{R}^0_c = \mymatrix{R}^0_a\mymatrix{R}^a_b\mymatrix{R}^b_c.$$ + +This can be represented by the following equivalent piece of code using sample angles. + +````{code-cell} +θ0_a = pi/4 +R0_a = np.array([[cos(θ0_a),-sin(θ0_a), 0], + [sin(θ0_a), cos(θ0_a), 0], + [0, 0, 1]]) + +θa_b = -pi/2 +Ra_b = np.array([[ cos(θa_b), 0, sin(θa_b)], + [ 0, 1, 0], + [-sin(θa_b), 0, cos(θa_b)]]) + +θb_c = pi/8 +Rb_c = np.array([[1, 0, 0], + [0, cos(θb_c), -sin(θb_c)], + [0, sin(θb_c), cos(θb_c)]]) + +R0_c = R0_a @ Ra_b @ Rb_c +print(f"R0_c={R0_c}") +```` + +## Inverse rotations in SO(3) + +Inverse operations in SO(3) are analogous to the inversions in SO(2), which are simply matrix transpositions. + +For example, we can recover $\mymatrix{R}^0_a$ from $\mymatrix{R}^0_c$ using inverse relative rotations as follows. + +$$\begin{align} +\mymatrix{R}^0_a &= \mymatrix{R}^0_c \mymatrix{R}^c_b \mymatrix{R}^b_a \\ +\mymatrix{R}^0_a &= \mymatrix{R}^0_c (\mymatrix{R}^b_c)^T (\mymatrix{R}^a_b)^T \\ +\end{align}$$ + +In the example code below, we store this alternative calculation in `R0_a_` to compare it with `R0_a`. + +````{code-cell} +R0_a_ = R0_c @ Rb_c.T @ Ra_b.T + +if np.allclose(R0_a, R0_a_): + print('The results are pretty much the same!') +else: + print('The results are too different.') +```` + +# 3D poses + +In 3D, a translation followed by a rotation in the _current_ frame is represented with elements in SE(3). These elements are matrices $\mymatrix{H}\in\mathbb{R}^{4 \times 4}$ with the following structure + +$$\mymatrix{H}(\myvec{t},\mymatrix{R}) = +\left[\begin{array}{ccc} + \mymatrix{I} & \myvec{t} \\ + \myvec{0} & 1 + \end{array}\right] +\left[\begin{array}{ccc} + \mymatrix{R} & \myvec{0} \\ + \myvec{0} & 1 + \end{array}\right] = +\left[\begin{array}{ccc} + \mymatrix{R} & \myvec{t} \\ + \myvec{0} & 1 + \end{array}\right].$$ + +
+The translation and rotation order is extremely important. +
+ +## Sequential pose transformations + +We can also perform pose transformations using sequential right multiplications when they are with respect to the _current_ frame. Note also that we can verify the lack of commutativity on these transformations. + +For example, consider a translation in 3D along the _World_ frame, represented by the homogeneous transformation matrix below. + +$$\mymatrix{H}^0_a = \mymatrix{H}_a = \left[\begin{array}{ccc} + 1 & 0 & 0 & 1 \\ + 0 & 1 & 0 & 2 \\ + 0 & 0 & 1 & 3 \\ + 0 & 0 & 0 & 1 + \end{array}\right].$$ + +Consider a rotation in 3D about the _current_ frame, represented by the homogeneous transformation matrix below. + +$$\mymatrix{H}^a_b = \left[\begin{array}{ccc} + \cos{\theta_{ab}} & -\sin{\theta_{ab}} & 0 & 0 \\ + \sin{\theta_{ab}} & \cos{\theta_{ab}} & 0 & 0 \\ + 0 & 0 & 1 & 0 \\ + 0 & 0 & 0 & 1 + \end{array}\right].$$ + +We can calculate their sequential combination as follows. + +$$\mymatrix{H}^0_b = \mymatrix{H}_b = \mymatrix{H}_a\mymatrix{H}^a_b$$ + +We can compute this result with $\theta_{ab} = \frac{\pi}{4}$ as shown in the piece of code below. We also show that + +$$\mymatrix{H}_a\mymatrix{H}^a_b \neq \mymatrix{H}^a_b\mymatrix{H}_a,$$ + +further reinforcing the importance of the order of operations. + +````{code-cell} +# An SE(3) translation +x = 1 # distance in metres +y = 2 # distance in metres +z = 3 # distance in metres + +H0_a = np.array([[1, 0, 0, x], + [0, 1, 0, y], + [0, 0, 1, z], + [0, 0, 0, 1]]) + +# An SE(3) rotation +θab = pi/4 # angle in radians + +Ha_b = np.array([[cos(θab), -sin(θab), 0, 0], + [sin(θab), cos(θab), 0, 0], + [0, 0, 1, 0], + [0, 0, 0, 1]]) + +# H0_a then Ha_b +H0_b = H0_a @ Ha_b + +# Ha_b then Ha +H_wrong = Ha_b @ H0_a + +if np.isclose(H0_b,H_wrong).all(): + print('The results are close!') +else: + print('The results are far, therefore the operation is not commutative') +```` + +## Inverse transformations + +As you have learned in theory, the inverse transformation can be found as + +$$\mymatrix{H^{-1}}(\myvec{t},\mymatrix{R}) = \left[\begin{array}{ccc} + \mymatrix{R}^T & -\mymatrix{R}^T\myvec{t} \\ + \myvec{0} & 1 + \end{array}\right],$$ + +so it is important to remember to _never_ invert the matrix with general matrix inversion algorithms. Using the matricial properties leads to a simpler, faster, and more accurate inversion. The only "trick" is to obtain the submatrix corresponding to the rotation matrix. + +````{code-cell} +# Extract R from H +R0_b = H0_b[0:3,0:3] # The 3x3 Rotation matrix +t0_b = H0_b[0:3,3].reshape((3,1)) # The 3x1 translation vector at the fourth column. Reshape it into a column vector. + +A = R0_b.T +B = -R0_b.T @ t0_b +C = np.array([0,0,0]) +D = np.array([1]) + +# This is how you can use `np.block` to build a matrix like so +# | A B | +# | C D | +H0_b_inv = np.block([[A, B], + [C, D]]) + +print(f"H0_b_inv={H0_b_inv}") + +print(f"H0_b @ H0_b_inv = {H0_b @ H0_b_inv}") +```` + +# Exercises + +Consider that the `numpy` and `math` modules are already imported as shown earlier in this lesson. + +## Exercise a + +For $\theta_a = \frac{\pi}{4}$, calculate + +$$\mymatrix{R}^0_{a} = \mymatrix{R}_{a} = R(\theta_a) \in SO(2)$$ + +and store it in the variable `R_a` shown in the cell below. + +````{code-cell} +θ_a = pi/4.0 # As given in the exercise + +R_a = None # Replace None with your solution to this exercise. +```` + +## Exercise b + +Calculate the result of a rotation of $\theta_{b1} = \frac{\pi}{12}$ followed by a rotation of $\theta_{b2} = -\frac{\pi}{2}$, in the _current_ frame, using elements of SO(2). + +Store the result in the variable `R_b` shown in the cell below. + +````{code-cell} +θ_b1 = pi/12.0 # As given in the exercise +θ_b2 = -pi/2.0 # As given in the exercise + +R_b = None # Replace None with your solution to this exercise. +```` + +## Exercise c + +Consider the translation + +$$\myvec{p}_c = \left[\begin{array}{ccc} + x_c \\ + y_c + \end{array}\right],$$ + +and the rotation + +$$R(\theta_c) = \left[\begin{array}{ccc} + \cos{\theta_c} & -\sin{\theta_c} \\ + \sin{\theta_c} & \cos{\theta_c} + \end{array}\right].$$ + +Starting at the World frame, calculate the homogeneous transformation representing the rotation $R(\theta_c)$, about the _World_ frame, followed by the translation $\myvec{p}_c$, in the _current_ frame. + +Consider $\theta_c = \frac{\pi}{3}$, $x_c = 2$, $y_c = 5$, and store the result in the variable `H_c` shown in the cell below. + +````{code-cell} +θ_c = pi/3.0 # As given in the exercise +x_c = 2.0 # As given in the exercise +y_c = 5.0 # As given in the exercise + +H_c = None # Replace None with your solution to this exercise. +```` + +## Exercise d + +Consider the same variables as in `Exercise c` by replacing the subscripts with `d`. Calculate, instead, the translation followed by the rotation. Store the result in the variable `H_d` shown in the cell below. + +Is `H_c` the same as `H_d`? What does that indicate? + +````{code-cell} +θ_d = pi/3.0 # As given in the exercise +x_d = 2.0 # As given in the exercise +y_d = 5.0 # As given in the exercise + +H_d = None # Replace None with your solution to this exercise. +```` + +# Extra challenge(s) +1. Let a rotation represented by a SO(2) element have a time-varying angle of $\theta(t) = \sin(t) + 2\cos(t)$. + - Write down its general form in SO(2) so that all four elements are clearly visible. + - Using this written down solution, compute the SO(2) representation at $t=10$. +2. Using SE(3) elements, calculate the final rigid body motion after four sequential transformations. + - The first transformation is a rotation of $\theta=\frac{\pi}{10}$ about the $z$-axis of the _World_ frame. + - The second transformation is a translation of $d=0.3$ about the $z$-axis of the _current_ frame. + - The third transformation is a translation of $a=0.5$ about the $x$-axis of the _current_ frame. + - The fourth and last transformation is a rotation of $\alpha=-\frac{\pi}{2}$ about the $x$-axis of the _current_ frame. diff --git a/unstable/lesson3_exercise_answers.md b/unstable/lesson3_exercise_answers.md new file mode 100644 index 0000000..f742275 --- /dev/null +++ b/unstable/lesson3_exercise_answers.md @@ -0,0 +1,122 @@ +--- +kernelspec: + name: python3 + display_name: 'Python 3' +--- + +# L3 Exercise Answers + +*License: CC-BY-NC-SA 4.0* + +*Author: Murilo M. Marinho (murilo.marinho@manchester.ac.uk)* + +### I found an issue +Thank you! Please report it at https://github.com/MarinhoLab/OpenExecutableBooksRobotics/issues + +### Latex Macros + +# Valid imports + +````{code-cell} +from math import pi, sin, cos +import numpy as np +```` + +# Exercises + +## Exercise a + +````{code-cell} +q_A0 = pi/4.0 # As given in the exercise +q_A1 = -0.1 # As given in the exercise + +H_A0_A0p = np.array( + [[cos(q_A0), -sin(q_A0), 0, 0], + [sin(q_A0), cos(q_A0), 0, 0], + [0, 0, 1, 0], + [0, 0, 0, 1]] +) + +H_A0p_A0pp = np.array( + [[1, 0, 0, 0], + [0, 1, 0, 0], + [0, 0, 1, 0.5], + [0, 0, 0, 1]] +) + +H_A0pp_A1 = np.array( + [[1, 0, 0, 0], + [0, cos(pi/2), -sin(pi/2), 0], + [0, sin(pi/2), cos(pi/2), 0], + [0, 0, 0, 1]] +) + +H_A1_A2 = np.array( + [[1, 0, 0, 0], + [0, 1, 0, 0], + [0, 0, 1, q_A1], + [0, 0, 0, 1]] +) + + +H_A0_A2 = H_A0_A0p @ H_A0p_A0pp @ H_A0pp_A1 @ H_A1_A2 + +# Printing the result is NOT a mandatory part of the answer. +print(f'H_A0_A2 = {H_A0_A2}') +```` + +## Exercise c + +````{code-cell} +# All rotations are the same +H_Rz = np.array( + [[cos(pi/5.0), -sin(pi/5.0), 0, 0], + [sin(pi/5.0), cos(pi/5.0), 0, 0], + [0, 0, 1, 0], + [0, 0, 0, 1]] +) + +# All translations are the same +H_Tx = np.array( + [[1, 0, 0, 0], + [0, 1, 0, 0], + [0, 0, 1, 0.25], + [0, 0, 0, 1]] +) + +H_C0_C3 = H_Rz @ H_Tx @ H_Rz @ H_Tx @ H_Rz @ H_Tx + +# Printing the result is NOT a mandatory part of the answer. +print(f'H_C0_C3 = {H_C0_C3}') +```` + +# Challenge 1 + +````{code-cell} +# Consider manipulator DoFs as the length of the following lists. +# Consider it as the configuration space of the RRR...RRR robot +q = [pi/2, pi/10, -pi/10, pi/2] # Increase length of q if you'd like to check +l = [1, 2, 3, 4] # l must be same size of q + +if len(q) != len(l): + raise Exception("q and l are not the same length") + +def link_rotation(qi): + return np.array( + [[cos(qi), -sin(qi), 0], + [sin(qi), cos(qi), 0], + [0, 0, 1]]) + +def link_translation(li): + return np.array( + [[1, 0, li], + [0, 1, 0], + [0, 0, 1]]) + +H = np.eye(3) +for qi, li in zip(q, l): + H = H @ link_rotation(qi) @ link_translation(li) + +# Printing the result is NOT a mandatory part of the answer. +print(f"Final answer is {H}") +```` diff --git a/unstable/lesson3_tutorial.md b/unstable/lesson3_tutorial.md new file mode 100644 index 0000000..ff334c3 --- /dev/null +++ b/unstable/lesson3_tutorial.md @@ -0,0 +1,269 @@ +--- +kernelspec: + name: python3 + display_name: 'Python 3' +--- + +# Prerequisites + +````{code-cell} +%%capture +%pip install numpy +%pip install numpy --break-system-packages +```` + +# Imports + +````{code-cell} +import numpy as np +from math import pi, sin, cos +```` + +# Forward Kinematics Model (FKM)The forward kinematics model of a rigid serial-link manipulator is obtained through a sequence of transformations.The only real challenge in obtaining the FKM is understanding from a diagram, or a real robot, what transformations represent the robot and in what order they happen.Anyway, we can start with an example. As always, remember that angles are in radians and lengths are in meters.![Lesson4.png](Lesson4.png)Consider the 2-DoF planar robot shown in the figure. It is classed as an RR robot, because the two joints are revolute.Let $q_0\triangleq q_0(t) \in \mathbb{R}$ and $q_1\triangleq q_1(t) \in \mathbb{R}$ compose its configuration space. In addition, let $l_{0} \in \mathbb{R}$ and $l_{1} \in \mathbb{R}$ be the geometric parameters, which are quantities that cannot be controlled.The configuration space is what is used in practice to control the robot. You as the system designer will send configuration space values $q_0$ and $q_1$, or other signals related to those, to command the robot. You will make it move to perform a relevant task and hopefully earn your next month's salary. The parameters $l_{0}$ and $l_{1}$ are constant in time and represent time-invariant geometrical aspects of the robot, such as link lengths, that you cannot control.As a representative task for robotic manipulators, let us use the configuration space and geometric parameters to calculate the pose of the frame of the tip of the robot. This is represented mathematically as follows.$$\mymatrix{H}^{0}_{2}( q_0, l_{0},q_1,l_{1}) \in SE(2).$$The equation for the end-effector (tip) pose is what is called the forward kinematics model (FKM). We need this frequently when using a robotic manipulator because the end effector is likely to be its most useful part. For instance, it could be a gripper that is used to pick and place objects. To pick or place an object, the robot needs to move somewhere.The first step towards moving somewhere is knowing where you are. Thence, the first step towards controlling a robotic manipulator's end effector pose in any meaningful way is to obtain its FKM.## Understanding the problemThe FKM is a mathematical description of the robot. Before we attempt any programming, we have to mathematically describe the sequential transformations that represent the robot being modelled.As shown in the figure, there are four transformations for this robot, taking us from the base, $\mathcal{F}_0$, to the end-effector, $\mathcal{F}_2$. The sequence can be summarised as follows.1. A rotation of $q_0$ about the current frame, from $\mathcal{F}_0$ to $\mathcal{F}_{0'}$.2. A translation of $l_0$ along the $x$-axis of the current frame, from $\mathcal{F}_{0'}$ to $\mathcal{F}_{1}$.3. A rotation of $q_1$ about the current frame, from $\mathcal{F}_{1}$ to $\mathcal{F}_{1'}$.4. A translation of $l_1$ along the $x$-axis of the current frame, from $\mathcal{F}_{1'}$ to $\mathcal{F}_{2}$.### 1. From $\mathcal{F}_0$ to $\mathcal{F}_{0'}$We start with the rotation that can be described by the following homogeneous transformation matrix.$$\myvec H_{0'}^{0}\left(q_0\right) =\begin{bmatrix} \cos(q_0) & -\sin(q_0) & 0\\\sin(q_0) & \cos(q_0) & 0\\0 & 0 & 1\end{bmatrix}.$$Programmatically, supposing that $q_0 = \frac{\pi}{4}$, we arrive at the following piece of code. + +````{code-cell} +H_0_0p = np.array( + [[cos(pi/4), -sin(pi/4), 0], + [sin(pi/4), cos(pi/4), 0], + [0, 0, 1]] +) + +print(f"The first transformation is\n\n H_0_0p = \n{H_0_0p}") +```` + +### 2. From $\mathcal{F}_{0'}$ to $\mathcal{F}_{1}$ + +The second step is a translation that can be described by the following homogeneous transformation matrix. + +$$\myvec H_{1}^{0'}\left(l_0\right) = +\begin{bmatrix} +1 & 0 & l_0 \\ +0 & 1 & 0 \\ +0 & 0 & 1 +\end{bmatrix}.$$ + +Programmatically, supposing that $l_0 = 0.3$, we arrive at the following piece of code. + +````{code-cell} +H_0p_1 = np.array( + [[1, 0, 0.3], + [0, 1, 0], + [0, 0, 1]] +) + +print(f"The second transformation is\n\n H_0p_1 = \n{H_0p_1}") +```` + +### 3. From $\mathcal{F}_{1}$ to $\mathcal{F}_{1'}$ + +The third step is a rotation that can be described by the following homogeneous transformation matrix. + +$$\myvec H_{1'}^{1}\left(q_1\right) = +\begin{bmatrix} \cos(q_1) & -\sin(q_1) & 0\\ +\sin(q_1) & \cos(q_1) & 0\\ +0 & 0 & 1 +\end{bmatrix}.$$ + +Programmatically, supposing that $q_1 = -\frac{\pi}{14}$, we arrive at the following piece of code. + +````{code-cell} +H_1_1p = np.array( + [[cos(-pi/14), -sin(-pi/14), 0], + [sin(-pi/14), cos(-pi/14), 0], + [0, 0, 1]] +) + +print(f"The third transformation is\n\n H_1_1p = \n{H_1_1p}") +```` + +### 4. From $\mathcal{F}_{1'}$ to $\mathcal{F}_{2}$ + +The last step is a translation that can be described by the following homogeneous transformation matrix. + +$$\myvec H_{2}^{1'}\left(l_1\right) = +\begin{bmatrix} +1 & 0 & l_1 \\ +0 & 1 & 0 \\ +0 & 0 & 1 +\end{bmatrix}.$$ + +Programmatically, supposing that $l_1 = 0.95$, we arrive at the following piece of code. + +````{code-cell} +H_1p_2 = np.array( + [[1, 0, 0.95], + [0, 1, 0], + [0, 0, 1]] +) + +print(f"The second transformation is\n\n H_1p_2 = \n{H_1p_2}") +```` + +## Ok, so where's the FKM for the RR robot? + +As we summarised earlier, the planar RR robot used in this lesson is composed of four sequential transformations. We obtained each of them individually, therefore the final step for the FKM is to compose them in sequence. + +$$\mymatrix{H}^{0}_{2}( q_0, l_{0},q_1,l_{1}) = \myvec H_{0'}^{0}\left(q_0\right)\myvec H_{1}^{0'}\left(l_0\right)\myvec H_{1'}^{1}\left(q_1\right)\myvec H_{2}^{1'}\left(l_1\right).$$ + +The equation above is general and is the FKM for this robot. + +Programmatically, we will compute the FKM at a given configuration. Using the configuration and parameters defined previously, we arrive at the following piece of code. + +````{code-cell} +H_0_2 = H_0_0p @ H_0p_1 @ H_1_1p @ H_1p_2 + +print(f"The FKM for the RR robot at the specified configuration is\n\n H_0_2 = \n{H_0_2}") +```` + +# Denavit-Hartenberg (DH) Parameters + +Despite being the most mistyped concept in my career, DH parameters are ubiquitous and frequently used to describe commercial robots. + +The process can be divided into two major parts, with different levels of difficulty. +1. Obtaining the DH parameters for a given robot. +2. Using given DH parameters to obtain the robot's FKM. + +Obtaining the DH parameters of a robot usually requires some thought, in particular if the robot has many degrees-of-freedom. This is not the objective of this lesson, because that does not involve programming. It is a pen-and-paper exercise. + +After the DH parameters are obtained, calculating the FKM of the robot is trivial. + +## DH parameter table for an RR robot + +Let us start with a sample table, shown below. + +| Joint | $\theta$ | $d$ | $a$ | $\alpha$ | +|-------|----------|-----|---------|----------| +| 0 | $q_0(t)$ | 0 | $l_{0}$ | 0 | +| 1 | $q_1(t)$ | 0 | $l_{1}$ | 0 | + +Each joint transformation is represented by a row. The transformations of each row will be done in the following sequence. + +1. A rotation about the $z$-axis of the current frame, related to column $\theta$. +2. A translation about the $z$-axis of the current frame, related to column $d$. +3. A translation about the $x$-axis of the current frame, related to column $a$. +4. A rotation about the $x$-axis of the current frame, related to column $\alpha$. + +Note that the sequence of transformations in the table represent the same FKM of our RR robot derived previously. The only difference is that the result will be an element of SE(3). + +## DH parameter table for a PP robot + +Instead of working again on the RR robot, let's derive the FKM for a PP robot, composed of two prismatic joints. Let its configuration be composed of $q_{B0}(t)$ and $q_{B1}(t)$, with different subscripts to clarify that it is a different robot. + +Consider that the FKM is represented by the table below. Note that another benefit of a table is that there's no need to interpret a robot diagram. + +| Joint | $\theta$ | $d$ | $a$ | $\alpha$ | +|-------|----------|-------------|-----|-----------------| +| 0 | 0 | $q_{B0}(t)$ | 0 | $\frac{\pi}{2}$ | +| 1 | 0 | $q_{B1}(t)$ | 0 | 0 | + +Given that we can ignore any cells with zeros, this robot will be composed of three transformations. +1. A translation of $q_{B0}(t)$ along the $z$-axis of the current frame, from $\mathcal{F}_{B0}$ to $\mathcal{F}_{B0'}$. +2. A rotation of $\frac{\pi}{2}$ about the $x$-axis of the current frame, from $\mathcal{F}_{B0'}$ to $\mathcal{F}_{B1}$. +3. A translation of $q_{B1}(t)$ along the $z$-axis of the current frame, from $\mathcal{F}_{B1}$ to $\mathcal{F}_{B2}$. + +## FKM for the PP robot + +Using elements of SE(3), we obtain each of the three transformations, as follows. + +$$\begin{align} +\mymatrix{H}^{B0}_{B0'}(q_{B0})&=\left[\begin{array}{cccc} + 1 & 0 & 0 & 0 \\ + 0 & 1 & 0 & 0 \\ + 0 & 0 & 1 & q_{B0}(t) \\ + 0 & 0 & 0 & 1 +\end{array}\right], \\ +\mymatrix{H}^{B0'}_{B1}&=\left[\begin{array}{cccc} + 1 & 0 & 0 & 0\\ + 0 & \cos{\frac{\pi}{2}} & -\sin{\frac{\pi}{2}} & 0\\ + 0 & \sin{\frac{\pi}{2}} & \cos{\frac{\pi}{2}} & 0\\ + 0 & 0 & 0 & 1 +\end{array}\right], \\ +\mymatrix{H}^{B1}_{B2}(q_{B1})&=\left[\begin{array}{cccc} + 1 & 0 & 0 & 0 \\ + 0 & 1 & 0 & 0 \\ + 0 & 0 & 1 & q_{B1}(t) \\ + 0 & 0 & 0 & 1 +\end{array}\right]. +\end{align}$$ + +Then, we sequentially compose them to obtain the FKM. + +$$ +\mymatrix{H}^{B0}_{B2}(q_{B0},q_{B1})=\mymatrix{H}^{B0}_{B0'}(q_{B0})\mymatrix{H}^{B0'}_{B1}\mymatrix{H}^{B1}_{B2}(q_{B1}).$$ + +Programmatically, suppose $q_{B0}=0.2$ and $q_{B1}=0.3$. We obtain the following piece of code. + +````{code-cell} +q_B0 = 0.2 +q_B1 = 0.3 +theta_B0 = pi/2.0 + +H_B0_B0p = np.array( + [[1, 0, 0, 0], + [0, 1, 0, 0], + [0, 0, 1, q_B0], + [0, 0, 0, 1]] +) + +H_B0p_B1 = np.array( + [[1, 0, 0, 0], + [0, cos(theta_B0), -sin(theta_B0), 0], + [0, sin(theta_B0), cos(theta_B0), 0], + [0, 0, 0, 1]] +) + +H_B1_B2 = np.array( + [[1, 0, 0, 0], + [0, 1, 0, 0], + [0, 0, 1, q_B1], + [0, 0, 0, 1]] +) + +# FKM +H_B0_B2 = H_B0_B0p @ H_B0p_B1 @ H_B1_B2 + +print(f"The FKM for the PP robot at the specified configuration is\n\n H_B0_B2 = \n{H_B0_B2}") +```` + +# Exercises + +Consider that the `numpy` and `math` modules are already imported as shown earlier in this lesson. + +## Exercise a + +Consider a robotic manipulator that has the following DH-parameter table. + +| Joint | $\theta$ | $d$ | $a$ | $\alpha$ | +|-------|-------------|-------------|-----|-----------------| +| 0 | $q_{A0}(t)$ | 0.5 | 0 | $\frac{\pi}{2}$ | +| 1 | 0 | $q_{A1}(t)$ | 0 | 0 | + +Compute its FKM in SE(3) and store in `H_A0_A2` the result of the FKM given $q_{A0}(t) = \frac{\pi}{4}$ and $q_{A1}(t) = -0.1$. + +````{code-cell} +q_A0 = pi/4.0 # As given in the exercise +q_A1 = -0.1 # As given in the exercise + +H_A0_A2 = None # Replace None with your solution to this exercise. +```` + + +## Exercise c + +Consider a robotic manipulator that has the following DH-parameter table. + +| Joint | $\theta$ | $d$ | $a$ | $\alpha$ | +|-------|-------------|----------|-----|----------| +| 0 | $q_{C0}(t)$ | $l_{C0}$ | 0 | 0 | +| 1 | $q_{C1}(t)$ | $l_{C1}$ | 0 | 0 | +| 2 | $q_{C2}(t)$ | $l_{C2}$ | 0 | 0 | + +Compute its FKM in SE(3) and store in `H_C0_C3` the result of the FKM given $q_{C0}(t)=q_{C1}(t)=q_{C2}(t) = \frac{\pi}{5}$ and $l_{C0} = l_{C1} = l_{C2} = 0.25$. + +````{code-cell} +H_C0_C3 = None # Replace None with your solution to this exercise. +```` + + +# Extra challenge(s) +1. What about if it was a planar revolute manipulator with `n` degrees-of-freedom? diff --git a/unstable/lesson4_exercise_answers.md b/unstable/lesson4_exercise_answers.md new file mode 100644 index 0000000..a14c164 --- /dev/null +++ b/unstable/lesson4_exercise_answers.md @@ -0,0 +1,222 @@ +--- +kernelspec: + name: python3 + display_name: 'Python 3' +--- + +# L4 Exercise Answers + +*License: CC-BY-NC-SA 4.0* + +*Author: Murilo M. Marinho (murilo.marinho@manchester.ac.uk)* + +### I found an issue +Thank you! Please report it at https://github.com/MarinhoLab/OpenExecutableBooksRobotics/issues + +### Latex Macros + +# Valid imports + +````{code-cell} +from math import pi, sin, cos +import numpy as np +```` + +# Exercises + +## Exercise a + +First, we calculate FKM by hand. You'll notice that it is given by the equation below. +$$\mymatrix{H}^{0}_{3} = \left[\begin{array}{ccc} + c_{012} & -s_{012} & l_{0}c_0 + l_{1}c_{01} + l_{2}c_{012}\\ + s_{012} & c_{012} & l_{0}s_0 + l_{1}s_{01} + l_{2}s_{012}\\ + 0 & 0 & 1 + \end{array}\right].$$ + +We don't need to compute it explicitly, we just need the task-space values. + +$$\begin{align} +p_{x}&=l_{0}c_0 + l_{1}c_{01} + l_{2}c_{012} \\ +p_{y}&=l_{0}s_0 + l_{1}s_{01} + l_{2}s_{012} \\ +\phi&=q_0 + q_1 + q_2.\\ +\end{align}$$ + +Then, take the derivative to find the Jacobian. + +$$\mymatrix{J} = \left[\begin{array}{ccc} + -l_{0}s_0 - l_{1}s_{01} - l_{2}s_{012} & - l_{1}s_{01} - l_{2}s_{012} & - l_{2}s_{012} \\ + l_{0}c_0 + l_{1}c_{01} + l_{2}c_{012} & l_{1}c_{01} + l_{2}c_{012} & l_{2}c_{012}\\ + 1 & 1 & 1 + \end{array}\right].$$ + +````{code-cell} +q_0 = pi/4.0 +q_1 = -pi/8.0 +q_2 = pi/12.0 + +l_0 = 1 +l_1 = 1 +l_2 = 1 + +c0 = cos(q_0) +c01 = cos(q_0 + q_1) +c012 = cos(q_0 + q_1 + q_2) + +s0 = sin(q_0) +s01 = sin(q_0 + q_1) +s012 = sin(q_0 + q_1 + q_2) + +# Task space +p_x = l_0 * c0 + l_1 * c01 + l_2 * c012 +p_y = l_0 * s0 + l_1 * s01 + l_2 * s012 +phi = q_0 + q_1 + q_2 + +# Jacobian +J_1_1 = - l_0 * s0 - l_1 * s01 - l_2 * s012 +J_1_2 = - l_1 * s01 - l_2 * s012 +J_1_3 = - l_2 * s012 + +J_2_1 = l_0 * c0 + l_1 * c01 + l_2 * c012 +J_2_2 = l_1 * c01 + l_2 * c012 +J_2_3 = l_2 * c012 + +J_3_1 = 1 +J_3_2 = 1 +J_3_3 = 1 + +J = np.array( + [[J_1_1, J_1_2, J_1_3], + [J_2_1, J_2_2, J_2_3], + [J_3_1, J_3_2, J_3_3]] +) + +print(f"The analytical Jacobian at q_0={q_0}, q_1={q_1}, and q_2={q_2} is \n J={J}") +```` + +## Exercise b + +Suppose that we have a `PP` robot defined by the following transformations: +- a translation of $q_0$ along the $x-$axis of the base frame. +- a rotation of 90 degrees with respect to the current frame. +- a translation of $q_1$ along the $x-$axis of the current frame. + + +First, we calculate FKM by hand. You'll notice that it is given by the equation below. +$$\mymatrix{H}^{0}_{3} = \left[\begin{array}{ccc} + 0 & -1 & q_0\\ + 1 & 0 & q_1\\ + 0 & 0 & 1 + \end{array}\right].$$ + +We don't need to compute it explicitly, we just need the task-space values. + +$$\begin{align} +p_{x}&= q_0 \\ +p_{y}&= q_1 \\ +\phi&= \frac{\pi}{2}.\\ +\end{align}$$ + +Then, take the derivative to find the Jacobian. + +$$\mymatrix{J} = \left[\begin{array}{cc} + 1 & 0\\ + 0 & 1\\ + 0 & 0 + \end{array}\right].$$ + +````{code-cell} +# Jacobian is trivial so we don't need to compute each term separately. +# As you can see, in this example, it the Jacobian always has the same value. +J = np.array( + [[1, 0], + [0, 1], + [0, 0]] +) +print(f"The analytical Jacobian is always\n J={J}") +```` + +# Challenge 1 + +To solve this challenge, we notice the patterns in the computation. You can change to the same link values of exercise a to test that this is correct. + +````{code-cell} +# Consider manipulator DoFs as the length of the following lists. +# Consider it as the configuration space of the RRR...RRR robot +q = [pi/4.0, -pi/8.0, pi/12.0, -pi/3.0] # Increase length of q if you'd like to check +l = [1, 1, 1, 1] # l must be same size of q + +if len(q) != len(l): + raise Exception("q and l are not the same length") + +def c_n(q, n_frame): + """ + Get cos(q_0 + q_1 + ... + q_n). + """ + q_sum = 0 + c_result = 0 + for i in range(n_frame+1): + qi = q[i] + q_sum += qi # First will be q_0, then q_0 + q_1, then... + c_result = cos(q_sum) # First will be cos(q_0), then cos(q_0 + q_1), then... + return c_result + +def s_n(q, n_frame): + """ + Get sin(q_0 + q_1 + ... + q_n). + """ + q_sum = 0 + s_result = 0 + for i in range(n_frame+1): + qi = q[i] + q_sum += qi # First will be q_0, then q_0 + q_1, then... + s_result = sin(q_sum) # First will be sin(q_0), then sin(q_0 + q_1), then... + return s_result + +def px_n(q, l, n_frame): + """ + Get l_0*cos(q_0) + l_1*cos(q_0 + q_1) + ... + l_n*cos(q_0 + q_1 + ... + q_n). + """ + px = 0 + for i in range(n_frame + 1): + li = l[i] + px += li * c_n(q, i) + return px + +def py_n(q, l, n_frame): + """ + Get l_0*sin(q_0) + l_1*sin(q_0 + q_1) + ... + l_n*sin(q_0 + q_1 + ... + q_n). + """ + py = 0 + for i in range(n_frame + 1): + li = l[i] + py += li * s_n(q, i) + return py + + +def j_n(q, l, n_frame): + """ + Construct the n-th column of the Jacobian matrix. + """ + N = len(q) + + pnx = px_n(q, l, n_frame-1) # starts at 0 + pny = py_n(q, l, n_frame-1) # starts at 0 + pNx = px_n(q, l, N-1) + pNy = py_n(q, l, N-1) + + px = pNx - pnx + py = pNy - pny + + jn = np.array( + [[-py], + [px], + [1]] + ) + return jn + +# Jacobian +J = j_n(q, l, 0) +for i in range(1,len(q)): + J = np.hstack((J, j_n(q, l, i))) # We stack the columns horizontally +print(J) +```` diff --git a/unstable/lesson4_tutorial.md b/unstable/lesson4_tutorial.md new file mode 100644 index 0000000..0141e30 --- /dev/null +++ b/unstable/lesson4_tutorial.md @@ -0,0 +1,173 @@ +--- +kernelspec: + name: python3 + display_name: 'Python 3' +--- + +# Package installation + +````{code-cell} +%%capture +%pip install numpy +%pip install numpy --break-system-packages +```` + +# Imports + +````{code-cell} +import numpy as np +from math import pi, sin, cos +```` + +# Differential Kinematics Model (DFKM) + +As we have seen in the previous lesson, the FKM relates configuration-space position with task-space position. For a manipulator, inserting a valid set of joint configurations into the FKM leads to the task-space values of the end-effector. + +We also managed to systematise the process to find the FKM, using DH parameters. That way, the FKM of any serial-link manipulator with any number of degrees-of-freedom can be defined with a table. From the table we can derive the analytical FKM and compute it efficiently. + +Although the FKM is, then, straightforward to compute, the _inverse_ FKM does not have a general closed form for manipulators with any number of degrees-of-freedom. Naturally, there are ways to invert the FKM iteratively using its first-order derivative. This is where the differential kinematics model (DFKM) comes into play. + +The DFKM is the process of finding Jacobians, because the FKM is a vector-valued function. It was once said that + + "Robotics is the art of finding Jacobians." + Bruno Siciliano @ Rosenbrock Lecture Series 2024 + +The importance of Jacobians for robotics cannot be overstated. In conclusion, the DFKM is a central process of robotics. + +Despite all the fancy words, it's a rather simple process for simple manipulators. Let's start with our usual toy example. + +![Lesson4.png](Lesson4.png) + +For the 2-DoF planar robot shown in the figure, let us use the FKM obtained in the previous lesson. It is given as + +$$\mymatrix{H}^{0}_{2}( q_0, l_{0},q_1,l_{1}) \in SE(2).$$ + +Also remember that the configuration space of this manipulator is + +$$\myvec{q} = \left[\begin{array}{c} + q_0 \\ + q_1 + \end{array}\right].$$ + +The DFKM is the process of calculating a Jacobian relevant for a given task. Therefore, herein we obtain the Jacobian $\mymatrix{J}\left(\myvec{q}\right)$ such that + +$$\dot{\myvec{x}}=\mymatrix{J}\left(\myvec{q}\right) \dot{\myvec{q}}$$ + +where + +$$\myvec{x} = \left[\begin{array}{c} + p_{x} \\ + p_{y} \\ + \phi + \end{array}\right],$$ + +in which $p_{x}$, $p_{y}$, and $\phi$ are, respectively, the $x$-axis position, the $y$-axis position, and the rotation angle of $\mathcal{F}_{2}$. Notice that $\dot{l}_{0}=\dot{l}_{1}=0$, because, as we defined in the previous lesson, they do not vary in time. + +As defined above, the Jacobian $\mymatrix{J}\left(\myvec{q}\right)$ is a function of the configuration-space values. Therefore, when computing it, we need to know at what $\myvec{q}$. + +As a second part of this toy example, let us calculate what is the end-effector velocity given a configuration-space velocity. Mathematically, let us calculate $\dot{\myvec{x}}$ when + +$$\dot{\myvec{q}} = \left[\begin{array}{c} + 5 \\ + 10 + \end{array}\right].$$ + + +## Step 1: Calculate the forward kinematics by hand + +It is not possible to calculate the Jacobian without the FKM. + +We saw how to do that in the previous lesson, so here is the answer for this robot. + +$$\mymatrix{H}^{0}_{2} = \left[\begin{array}{ccc} + \cos{(q_0 + q_1)} & -\sin{(q_0 + q_1)} & l_{0}\cos{q_0} + l_{1}\cos{(q_0 + q_1)}\\ + \sin{(q_0 + q_1)} & \cos{(q_0 + q_1)} & l_{0}\sin{q_0} + l_{1}\sin{(q_0 + q_1)}\\ + 0 & 0 & 1 + \end{array}\right].$$ + +This means that, from inspection, + +$$\begin{align} +p_{x}&=l_{0}\cos{q_0} + l_{1}\cos{(q_0 + q_1)} \\ +p_{y}&=l_{0}\sin{q_0} + l_{1}\sin{(q_0 + q_1)} \\ +\phi&=q_0 + q_1 .\\ +\end{align}$$ + + +## Step 2: Calculate the differential kinematics by hand + +We first calculate the Jacobian by hand, because programmatically there's nothing for us to do yet. + +The analytical Jacobian is given by + +$$ \mymatrix{J} = \left[\begin{array}{ccc} + \frac{\partial p_{x}}{\partial q_0} & \frac{\partial p_{x}}{\partial q_1} \\ + \frac{\partial p_{y}}{\partial q_0} & \frac{\partial p_{y}}{\partial q_1} \\ + \frac{\partial \phi}{\partial q_0} & \frac{\partial \phi}{\partial q_1} + \end{array}\right].$$ + +As we did in class, we find each element by calculating the partial derivative of the respective task-space value with respect to the configuration-space value + +$$\begin{align} +\frac{\partial p_{x}}{\partial q_0} &= -l_{0}\sin{q_0} - l_{1}\sin{(q_0 + q_1)} \\ +\frac{\partial p_{x}}{\partial q_1} &= -l_{1}\sin{(q_0 + q_1)} \\ +\frac{\partial p_{y}}{\partial q_0} &= l_{0}\cos{q_0} + l_{1}\cos{(q_0 + q_1)} \\ +\frac{\partial p_{y}}{\partial q_1} &= l_{1}\cos{(q_0 + q_1)} \\ +\frac{\partial \phi}{\partial q_0} &= 1 \\ +\frac{\partial \phi}{\partial q_1} &= 1 +\end{align}$$ + +resulting in + +$$\mymatrix{J} = \left[\begin{array}{ccc} + -l_{0}\sin{q_0} - l_{1}\sin{(q_0 + q_1)} & -l_{1}\sin{(q_0 + q_1)} \\ + l_{0}\cos{q_0} + l_{1}\cos{(q_0 + q_1)} & l_{1}\cos{(q_0 + q_1)} \\ + 1 & 1 + \end{array}\right].$$ + +## Step 3: Computing the Jacobian + +We're now equipped to solve the first question by doing the following + +````{code-cell} +# Sample values, the particular values do not matter +q_0 = pi / 4 +q_1 = pi / 3 +l_0 = 0.2 +l_1 = 0.1 + +# To possibly make it easier for you to read +J_1_1 = -l_0 * sin(q_0) - l_1 * sin(q_0 + q_1) +J_1_2 = -l_1 * sin(q_0 + q_1) +J_2_1 = l_0 * cos(q_0) + l_1 * cos(q_0 + q_1) +J_2_2 = l_1 * cos(q_0 + q_1) +J_3_1 = 1 +J_3_2 = 1 + +J = np.array( + [[J_1_1, J_1_2], + [J_2_1, J_2_2], + [J_3_1, J_3_2]] +) + +print(f"The analytical Jacobian at {q_0} and {q_1} is {J}") +```` + +With the correct definition of the Jacobian as above, we can calculate the second question as + +````{code-cell} +q_dot = np.array( + [[5], + [10]] +) + +x_dot = J @ q_dot + +print(f"In these conditions, x_dot = {x_dot}") +```` + +# Suggested exercises + +1. What about if the robot had 3 degrees-of-freedom, that is RRR? +2. What if the robot has one or more prismatic joints? +3. **Challenge.** What about if the robot had $n$ revolute degrees-of-freedom? Would it be much more complicated to solve? diff --git a/unstable/lesson5_exercise_answers.md b/unstable/lesson5_exercise_answers.md new file mode 100644 index 0000000..824f37f --- /dev/null +++ b/unstable/lesson5_exercise_answers.md @@ -0,0 +1,185 @@ +--- +kernelspec: + name: python3 + display_name: 'Python 3' +--- + +# Package installation + +````{code-cell} +%%capture +%pip install numpy matplotlib +%pip install numpy matplotlib --break-system-packages +```` + +# Imports + +````{code-cell} +%matplotlib inline +import numpy as np +import matplotlib.pyplot as plt +from math import pi, sin, cos +```` + +# Definition(s) + +The function(s) defined below are valid for all solutions. + +````{code-cell} +def damped_pseudo_inverse(A: np.array, damping: float = 0.01): + """Calculates the damped pseudo inverse of A""" + if damping == 0: + raise Exception(f"Damping is {damping} but should be different from zero") + + return A.T @ np.linalg.inv(A @ A.T + (damping ** 2) * np.eye(A.shape[0])) +```` + +## `PP` Robot + +Suppose that we have a `PP` robot defined by the following transformations: +- a translation of $q_0$ along the $x-$axis of the base frame. +- a rotation of 90 degrees with respect to the current frame. +- a translation of $q_1$ along the $x-$axis of the current frame. + +For FKM and Jacobian calculation, see lesson 4. + +For the control parameters, see lesson 5. + +````{code-cell} +def get_error(x, xd): + return x - xd + +def planar_robot_pp_fkm(q: np.array) -> np.array: + q_0 = q[0] + q_1 = q[1] + return np.array([q_0, + q_1, + pi/2.0]) + +def planar_robot_pp_jacobian(q): + # We can leave the parameter `q` for compatibility, but it's not used for this robot. + return np.array( + [[1, 0], + [0, 1], + [0, 0]] + ) + +eta = 1 # Control gain +T = 0.001 # Sampling time +# A desired task-space value +xd = np.array([2.0, + 2.0, + pi/10]) +print(f"xd = {xd}") + +# Lists to store the value of each control iteration +x_tilde_norm_list = [] +t_list = [] +u_norm_list = [] + +# Starting conditions +t = 0 +q_0 = 0.0 +q_1 = 0.0 +q = np.array([q_0, + q_1]) + +# Control for 10 seconds +while t < 10: + x = planar_robot_pp_fkm(q) + x_tilde = get_error(x, xd) + J = planar_robot_pp_jacobian(q) + J_inv = damped_pseudo_inverse(J) + u = -eta * J_inv @ x_tilde + + ## Store values + x_tilde_norm_list.append(np.linalg.norm(x_tilde)) + t_list.append(t) + u_norm_list.append(np.linalg.norm(u)) + + ## Variable updated for the next loop + q = q + u * T + t = t + T + +plt.plot(t_list,x_tilde_norm_list, label=f"$\\eta$={eta}") + +plt.title('(PP) Error exponential decay visualization') +plt.xlabel("Time [s]") +plt.ylabel("$||\\tilde{ \\bf{x} } (t) ||$") +plt.show() +```` + +## `RP` Robot + +Suppose that we have a `RP` robot defined by the following transformations: +- a rotation of $q_0$ of the base frame. +- a translation of $q_1$ along the $x-$axis of the current frame. + +For FKM and Jacobian calculation process, see lesson 4. + +For the control parameters, see lesson 5. + +````{code-cell} +def get_error(x, xd): + return x - xd + +def planar_robot_rp_fkm(q: np.array) -> np.array: + q_0 = q[0] + q_1 = q[1] + return np.array([q_1*cos(q_0), + q_1*sin(q_0), + q_0]) + +def planar_robot_rp_jacobian(q): + q_0 = q[0] + q_1 = q[1] + return np.array( + [[-q_1*sin(q_0), cos(q_0)], + [ q_1*cos(q_0), sin(q_0)], + [ 1, 0]] + ) + +eta = 1 # Control gain +T = 0.001 # Sampling time +# A desired task-space value +xd = np.array([2.0, + 2.0, + pi/10]) +print(f"xd = {xd}") + +# Lists to store the value of each control iteration +x_tilde_norm_list = [] +t_list = [] +u_norm_list = [] + +# Starting conditions +t = 0 +q_0 = 0.0 +q_1 = 0.0 +q = np.array([q_0, + q_1]) + +# Control for 10 seconds +while t < 10: + x = planar_robot_rp_fkm(q) + x_tilde = get_error(x, xd) + J = planar_robot_rp_jacobian(q) + J_inv = damped_pseudo_inverse(J) + u = -eta * J_inv @ x_tilde + + ## Store values + x_tilde_norm_list.append(np.linalg.norm(x_tilde)) + t_list.append(t) + u_norm_list.append(np.linalg.norm(u)) + + ## Variable updated for the next loop + q = q + u * T + t = t + T + +plt.plot(t_list,x_tilde_norm_list, label=f"$\\eta$={eta}") + +plt.title('(PP) Error exponential decay visualization') +plt.xlabel("Time [s]") +plt.ylabel("$||\\tilde{ \\bf{x} } (t) ||$") +plt.show() +```` diff --git a/unstable/lesson5_tutorial.md b/unstable/lesson5_tutorial.md new file mode 100644 index 0000000..f4a81f4 --- /dev/null +++ b/unstable/lesson5_tutorial.md @@ -0,0 +1,596 @@ +--- +kernelspec: + name: python3 + display_name: 'Python 3' +--- + +# Package installation + +````{code-cell} +%%capture +%pip install numpy matplotlib +%pip install numpy matplotlib --break-system-packages +```` + +# Imports + +````{code-cell} +%matplotlib inline +import numpy as np +import matplotlib.pyplot as plt +from math import pi, sin, cos +```` + +# 2 DoF planar robot (RR) + +![Lesson4.png](Lesson4.png) + +For the 2-DoF planar robot shown in the figure, let $q_0\triangleq q_0(t)$, $q_1\triangleq q_1(t)$, $l_{0}$, $l_{1} \in \mathbb{R}$ be the parameters to calculate + +$$\mymatrix{H}^{0}_{2}( q_0, l_{0},q_1,l_{1}) \in SE(2),$$ + +that is, the forward kinematics model of the 2 DoF planar manipulator with configuration space given by + +$$\myvec{q} = \left[\begin{array}{ccc} + q_0 \\ + q_1 + \end{array}\right].$$ + +and given the analytical Jacobian below + +$$\dot{\myvec{x}}=\mymatrix{J} \dot{\myvec{q}}$$ + +where + +$$\myvec{x} = \left[\begin{array}{ccc} + p_{x} \\ + p_{y} \\ + \phi_{z} + \end{array}\right],$$ + +in which $p_{x}$, $p_{y}$, and $\phi_{z}$ are, respectively, the $x$-axis position, the $y$-axis position, and the $z$-axis angle of $\mathcal{F}_{2}$. Notice that $\dot{l}_{0}=\dot{l}_{1}=0$. + +### Consider the forward kinematics model calculated as in the past lesson + +$$ \mymatrix{H}^{0}_{2} = \left[\begin{array}{ccc} + \cos{(q_0 + q_1)} & -\sin{(q_0 + q_1)} & l_{0}\cos{q_0} + l_{1}\cos{(q_0 + q_1)}\\ + \sin{(q_0 + q_1)} & \cos{(q_0 + q_1)} & l_{0}\sin{q_0} + l_{1}\sin{(q_0 + q_1)}\\ + 0 & 0 & 1 + \end{array}\right].$$ + +This means that, from inspection, + +$$\begin{align} +p_{x} & = l_{0}\cos{q_0} + l_{1}\cos{(q_0 + q_1)} \\ +p_{y} & = l_{0}\sin{q_0} + l_{1}\sin{(q_0 + q_1)} \\ +\phi_{z} & = q_0 + q_1. +\end{align}$$ + +With those, we can obtain the task space values with a function called `planar_robot_fkm` so that it can be easily reused + +````{code-cell} +def planar_robot_fkm(q): + """ + q: The configuration space values in radians. + returns the x, this, the current task space value where x = [p_x p_y phi_z]^T. + """ + l_0 = 0.2 # The robot parameters. They don't change in time, so they are constant here. + l_1 = 0.1 + + q_0 = q[0] # Just to make it more readable. + q_1 = q[1] + + p_x = l_0 * cos(q_0) + l_1 * cos(q_0 + q_1) + p_y = l_0 * sin(q_0) + l_1 * sin(q_0 + q_1) + phi_z = q_0 + q_1 + + return np.array([p_x, + p_y, + phi_z]) +```` + + +### Consider the analytical Jacobian + +We first calculate the Jacobian by hand, because programmatically there's nothing for you to do yet. As we did in the previous tutorial, here is the Jacobian + +$$ \mymatrix{J} = \left[\begin{array}{ccc} + -l_{0}\sin{q_0} - l_{1}\sin{(q_0 + q_1)} & -l_{1}\sin{(q_0 + q_1)} \\ + l_{0}\cos{q_0} + l_{1}\cos{(q_0 + q_1)} & l_{1}\cos{(q_0 + q_1)} \\ + 1 & 1 + \end{array}\right].$$ + +We transform this Jacobian calculation into the function `planar_robot_jacobian` shown below + +````{code-cell} +def planar_robot_jacobian(q): + """ + q: The configuration space values in radians. + returns the 3x2 Jacobian mapping [q_0 q_1]^T to [px py phi_z]^T. + """ + l_0 = 0.2 # The robot parameters. They don't change in time, so they are constant here. + l_1 = 0.1 + + q_0 = q[0] # Just to make it more readable. + q_1 = q[1] + + J_1_1 = -l_0 * sin(q_0) - l_1 * sin(q_0 + q_1) + J_1_2 = -l_1 * sin(q_0 + q_1) + J_2_1 = l_0 * cos(q_0) + l_1 * cos(q_0 + q_1) + J_2_2 = l_1 * cos(q_0 + q_1) + J_3_1 = 1 + J_3_2 = 1 + + return np.array( + [[J_1_1, J_1_2], + [J_2_1, J_2_2], + [J_3_1, J_3_2]] + ) +```` + +## Kinematic Control + +### 1. Define an error function + +We usually use + +$$\tilde{\myvec{x}}= \myvec{x} - \myvec{x}_d$$ + +that can be implemented as the function `get_error` below + +````{code-cell} +def get_error(x, xd): + """In this case, we use the difference as the error. + x: current task-space vector. + xd: desired task-space vector. + """ + return x - xd +```` + +### 2. Define the control loop + +We resort to the following control law + +$$\myvec{u} = -\eta \mymatrix{J}^{+}\tilde{\myvec{x}}$$ + +that can be easily implemented. Notice that we can resort to `np.linalg.pinv()` to perform the pseudo inversion. According to its [documentation](https://numpy.org/doc/2.2/reference/generated/numpy.linalg.pinv.html), it implements the Moore-Penrose pseudo-inversion based on the Singular Value Decomposition of all "large" singular values. + + + +For the control loop itself, we use all previous functions. The idea here is to show that the norm of the error decreases with the control action. The desired task space vector will not always be achievable. Nonetheless, we can show, as below, that the control action reduces the task-space error, $\tilde{\myvec{x}}$. + +Let $\eta=0.5$ be the controller gain, $T=0.001$ be the sampling time, and $\myvec{x}_d=\left[\begin{array}{ccc} 0.1 & 0.1 & \frac{\pi}{10} \end{array}\right]^T$. In addition, let the manipulator start at $t=0$ with $q_0=q_1=0$. + +````{code-cell} +eta = 0.5 # Controller proportional gain +T = 0.001 # Sampling time +# Define initial values for the joint positions +q_0 = 0.0 +q_1 = 0.0 +q = np.array([q_0, + q_1]) + +# A desired task-space value, defined by the problem at hand +xd = np.array([0.1, + 0.1, + pi/10]) +print(f"xd = {xd}") + +# Define a stop criteria, in this case let's control for 10 seconds +t = 0 # Current time + +# Lists to store the value of each control iteration +x_tilde_norm_list = [] +t_list = [] + +while t < 10: + # Calculate task-space value, x + x = planar_robot_fkm(q) + # Calculate task-space error, x_tilde + x_tilde = get_error(x, xd) + # Get the Jacobian + J = planar_robot_jacobian(q) + # Invert the Jacobian, for example, with numpy's implementation of it + J_inv = np.linalg.pinv(J) + # Calculate the control action + u = -eta * J_inv @ x_tilde + + ## Store values in the list so that we can print them later + x_tilde_norm_list.append(np.linalg.norm(x_tilde)) + t_list.append(t) + + ## Variable updated for the next loop + q = q + u * T # Update law using the sampling time + t = t + T +```` + +# Optional: Print the data + +To help visualise the behaviour of the controller, you can optionally plot the data. These data show that with the defined parameters the qualitative behaviour is very close to exponential decay. However the error does not converge to zero, meaning that the manipulator did not reach its target. Indeed, this is usually the case when the target is outside the reach of the manipulator or all task-space desired values cannot be simultaneously satisfied. + +````{code-cell} +plt.plot(t_list,x_tilde_norm_list) +plt.title('(RR) Error exponential decay visualization (Moore-Penrose)') +plt.xlabel("Time [s]") +plt.ylabel("$||\\tilde{ \\bf{x} } (t) ||$") +plt.show() +```` + +# Alternative Jacobian Inversion Strategies + +The SVD-based Moore-Penrose pseudo-inverse is a common strategy for matrix inversion. It is important to notice, however, that this is not the only one. In addition, it is not always the best choice for robot control. + +Another common strategy is the so-called damped pseudo-inverse, described below. It is usually the easiest choice to embed robustness to singularities in the controller. + +$$\mymatrix{J}^{\dagger}=\mymatrix{J}^T\left(\mymatrix{J}\mymatrix{J}^T + \lambda^2\mymatrix{I}\right)^{-1},$$ + +defined in the function `damped_pseudo_inverse()` below in which we call $\lambda$ as the variable `damping` + +````{code-cell} +def damped_pseudo_inverse(A: np.array, damping: float = 0.01): + """Calculates the damped pseudo inverse of A""" + if damping == 0: + raise Exception(f"Damping is {damping} but should be different from zero") + + return A.T @ np.linalg.inv(A @ A.T + (damping ** 2) * np.eye(A.shape[0])) +```` + +with that, our controller becomes, changing only the inversion strategy, + +````{code-cell} +eta = 0.5 # Controller proportional gain +T = 0.001 # Sampling time +# Define initial values for the joint positions +q_0 = 0.0 +q_1 = 0.0 +q = np.array([q_0, + q_1]) + +# A desired task-space value, defined by the problem at hand +xd = np.array([0.1, + 0.1, + pi/10]) +print(f"xd = {xd}") + +# Define a stop criteria, in this case let's control for 10 seconds +t = 0 # Current time + +# Lists to store the value of each control iteration +x_tilde_norm_list = [] +t_list = [] + +while t < 10: + # Calculate task-space value, x + x = planar_robot_fkm(q) + # Calculate task-space error, x_tilde + x_tilde = get_error(x, xd) + # Get the Jacobian + J = planar_robot_jacobian(q) + # Invert the Jacobian using the damped pseudo-inverse + J_inv = damped_pseudo_inverse(J) + # Calculate the control action + u = -eta * J_inv @ x_tilde + + ## Store values in the list so that we can print them later + x_tilde_norm_list.append(np.linalg.norm(x_tilde)) + t_list.append(t) + + ## Variable updated for the next loop + q = q + u * T # Update law using the sampling time + t = t + T + +# (Optional) plot the data +plt.plot(t_list,x_tilde_norm_list) +plt.title('(RR) Error exponential decay visualization (damped pseudo-inverse)') +plt.xlabel("Time [s]") +plt.ylabel("$||\\tilde{ \\bf{x} } (t) ||$") +plt.show() +```` + +# Convergence rate of discrete-time systems + +Although we usually attempt to find $\myvec{u}$ such that, the error $\tilde{\myvec{x}}$ converges exponentially. This means that, in the right conditions, we will have a convergence + +$$\tilde{\myvec{x}}(t)=\tilde{\myvec{x}}(0)e^{-\eta t}$$ + +in continuous time, hence the convergence depends only on $\eta$. + +However, given that our implementation is in discrete time, the convergence is affected by our sampling time, $T$. In practice, we set $T$ as the fastest sampling time that can be achieved by the hardware, e.g. the robot controller. In many cases, this is about 1 millisecond in practice. + +The effect on the convergence for different $\eta$ can be seen, computationally, below + +````{code-cell} +etas = [0.01, 0.1, 1, 10] # Different gains to iterate over the same control goals +T = 0.001 # Sampling time +# A desired task-space value, defined by the problem at hand +xd = np.array([0.1, + 0.1, + pi/10]) +print(f"xd = {xd}") + +# Lists to store the value of each control iteration +x_tilde_norm_list = [] +t_list = [] +u_norm_list = [] + +# Run the controller again for each gain +for i in range(0, len(etas)): + # Define a stop criteria, in this case let's control for 10 seconds + t = 0 # Current time + eta = etas[i] + # Define initial values for the joint positions + q_0 = 0.0 + q_1 = 0.0 + q = np.array([q_0, + q_1]) + + x_tilde_norm_list.append([]) + t_list.append([]) + u_norm_list.append([]) + + while t < 10: + # Calculate task-space value, x + x = planar_robot_fkm(q) + # Calculate task-space error, x_tilde + x_tilde = get_error(x, xd) + # Get the Jacobian + J = planar_robot_jacobian(q) + # Invert the Jacobian using the damped pseudo-inverse + J_inv = damped_pseudo_inverse(J) + # Calculate the control action + u = -eta * J_inv @ x_tilde + + ## Store values in the list so that we can print them later + x_tilde_norm_list[i].append(np.linalg.norm(x_tilde)) + t_list[i].append(t) + u_norm_list[i].append(np.linalg.norm(u)) + + ## Variable updated for the next loop + q = q + u * T # Update law using the sampling time + t = t + T + + + plt.plot(t_list[i],x_tilde_norm_list[i], label=f"$\\eta$={eta}") + +plt.title('(RR) Error exponential decay visualization for multiple $\\eta$') +plt.legend(loc="upper right") +plt.xlabel("Time [s]") +plt.ylabel("$||\\tilde{ \\bf{x} } (t) ||$") +plt.show() +```` + +## Alright, then why don't we just choose the largest $\eta \approx \infty$? + +Engineering is the art of trade-off. Whenever we have a large $\eta$, that means the configuration-space velocities will comparatively be higher. + +Using our example, we can see that the control signal norm is heavily affected by $\eta$. Therefore, for feasibility, it is important to keep the $\eta$ so that the system can handle it. In fact, gains that are too high are one of the major risks that can break robots or hurt people when real hardware is used without the proper safeguards. + +````{code-cell} +# Run the controller again for each gain +for i in range(0, len(etas)): + eta=etas[i] + plt.plot(t_list[i],u_norm_list[i], label=f"$\\eta$={eta}") + +plt.title('(RR) Control signal norm for different $\\eta$') +plt.legend(loc="upper right") +plt.xlabel("Time [s]") +plt.ylabel("$||u (t) ||$") +plt.show() +```` + +# 3 DoF Planar Robot (PRR) + +As you noticed from the previous discussion, what is important for any new robot after you understand these concepts are +- Defining a task space, $\myvec{x}$, that correctly represents the task (and manipulator if applicable). +- Obtaining the Forward Kinematics Model (FKM) $f(\myvec{q})=\myvec{x}$ that maps the configuration space, $\myvec{q}$, to the task space. +- Obtaining the analytical Jacobian so that the kinematic control can be applied. +- The kinematic control itself, aside from dimensions, does not need to change in general. + +For instance, we can even solve with no diagram as long as this information is given. Consider a PRR manipulator with configuration space + +$$\mathbb{R}^3 \ni \myvec{q}_C \triangleq \left[\begin{array}{ccc} + q_0 \\ + q_1 \\ + q_2 + \end{array}\right].$$ + + +And link lengths $l_{0}, l_{1}, l_{2}\in \mathbb{R}$. + +## Task Space & Forward Kinematics Model (FKM) + +Let + +$$\begin{align*} +s_1 & \triangleq \sin{q_1} \\ +s_{12} & \triangleq \sin{(q_1 + q_2)} \\ +c_1 & \triangleq \cos{q_1} \\ +c_{12} & \triangleq \cos{(q_1 + q_2)}. +\end{align*}$$ + +The forward kinematics of this manipulator is therefore given by + +$$ SE(2) \ni \mymatrix{H}^{0}_{3}(q_0,q_1,q_2) = \left[\begin{array}{ccc} + c_{12} & -s_{12} & (l_0 + q_0) + l_{1}c_{1} + l_{2}c_{12}\\ + s_{12} & c_{12} & l_{1}s_{1} + l_{2}s_{12}\\ + 0 & 0 & 1 + \end{array}\right].$$ + +Because this robot is planar, the following task space is necessary and sufficient to fully describe the reachable space + +$$\myvec{x} = \left[\begin{array}{ccc} + p_{x} \\ + p_{y} \\ + \phi_{z} + \end{array}\right].$$ + +The values can be obtained from inspection of $\mymatrix{H}^{0}_{3}(q_0,q_1,q_2)$, + +$$\begin{align*} +p_{x} & = (l_0 + q_0) + l_{1}c_{1} + l_{2}c_{12} \\ +p_{y} & = l_{1}s_{1} + l_{2}s_{12} \\ +\phi_{z} & = q_1 + q_2, +\end{align*}$$ + +which means that the FKM, $\myvec{x}=f(\myvec{q})$, in this case is equivalent to + +````{code-cell} +def planar_robot_prr_fkm(q: np.array) -> np.array: + """ + q: The configuration space values in radians. + returns the x, this, the current task space value where x = [p_x p_y phi_z]^T. + """ + l_0 = 0.2 # The robot parameters. They don't change in time, so they are constant here. + l_1 = 0.1 + l_2 = 0.3 + + q_0 = q[0] # Just to make it more readable. + q_1 = q[1] + q_2 = q[2] + + s1 = sin(q_1) + c1 = cos(q_1) + s12 = sin(q_1 + q_2) + c12 = cos(q_1 + q_2) + + p_x = (l_0 + q_0) + l_1*c1 + l_2*c12 + p_y = l_1*s1 + l_2*s12 + phi_z = q_1 + q_2 + + return np.array([p_x, + p_y, + phi_z]) +```` + + +## Analytical Jacobian + +The analytical Jacobian is + +$$ \mymatrix{J}_C(q_0,q_1,q_2) = \left[\begin{array}{ccc} + 1 & - l_{1}s_{1} - l_{2}s_{12} & -l_{2}s_{12}\\ + 0 & l_{1}c_{1} + l_{2}c_{12} & l_{2}c_{12}\\ + 0 & 1 & 1 + \end{array}\right].$$ + +````{code-cell} +def planar_robot_prr_jacobian(q): + """ + q: The configuration space values in radians. + returns the 3x3 Jacobian mapping [q_0 q_1 q_2]^T to [px py phi_z]^T. + """ + l_0 = 0.2 # The robot parameters. They don't change in time, so they are constant here. + l_1 = 0.1 + l_2 = 0.3 + + q_0 = q[0] # Just to make it more readable. + q_1 = q[1] + q_2 = q[2] + + s1 = sin(q_1) + c1 = cos(q_1) + s12 = sin(q_1 + q_2) + c12 = cos(q_1 + q_2) + + J_1_1 = 1 + J_2_1 = 0 + J_3_1 = 0 + + J_1_2 = -l_1*s1 - l_2*s12 + J_2_2 = l_1*c1 + l_2*c12 + J_3_2 = 1 + + J_1_3 = -l_2*s12 + J_2_3 = l_2*c12 + J_3_3 = 1 + + return np.array( + [[J_1_1, J_1_2, J_1_3], + [J_2_1, J_2_2, J_2_3], + [J_3_1, J_3_2, J_3_3]] + ) +```` + +## Kinematic Control + +With this, a similar control as before, but changing the FKM and Jacobian can be easily achieved. We change only +1. The configuration space in lines 20 to 25. +2. `planar_robot_prr_fkm` in line 33 +3. `planar_robot_prr_jacobian` in line 37. + +
+The rest of the control loop is unchanged! This is one of the advantages of kinematic control. +
+ +````{code-cell} +etas = [0.01, 0.1, 1, 10] # Different gains to iterate over the same control goals +T = 0.001 # Sampling time +# A desired task-space value, defined by the problem at hand +xd = np.array([0.1, + 0.1, + pi/10]) +print(f"xd = {xd}") + +# Lists to store the value of each control iteration +x_tilde_norm_list = [] +t_list = [] +u_norm_list = [] + +# Run the controller again for each gain +for i in range(0, len(etas)): + # Define a stop criteria, in this case let's control for 10 seconds + t = 0 # Current time + eta = etas[i] + # Define initial values for the joint positions + q_0 = 0.0 + q_1 = 0.0 + q_2 = 0.0 + q = np.array([q_0, + q_1, + q_2]) + + x_tilde_norm_list.append([]) + t_list.append([]) + u_norm_list.append([]) + + while t < 10: + # Calculate task-space value, x + x = planar_robot_prr_fkm(q) + # Calculate task-space error, x_tilde + x_tilde = get_error(x, xd) + # Get the Jacobian + J = planar_robot_prr_jacobian(q) + # Invert the Jacobian using the damped pseudo-inverse + J_inv = damped_pseudo_inverse(J) + # Calculate the control action + u = -eta * J_inv @ x_tilde + + ## Store values in the list so that we can print them later + x_tilde_norm_list[i].append(np.linalg.norm(x_tilde)) + t_list[i].append(t) + u_norm_list[i].append(np.linalg.norm(u)) + + ## Variable updated for the next loop + q = q + u * T # Update law using the sampling time + t = t + T + + + plt.plot(t_list[i],x_tilde_norm_list[i], label=f"$\\eta$={eta}") + +plt.title('(PRR) Error exponential decay visualization for multiple $\\eta$') +plt.legend(loc="upper right") +plt.xlabel("Time [s]") +plt.ylabel("$||\\tilde{ \\bf{x} } (t) ||$") +plt.show() +```` + +# Exercise + +Consider the `PRR` solution above as a solved exercise. + +# Suggested exercises + +Try to modify the code above to calculate the control action for a: + +1. `PP` robot +2. `RP` robot diff --git a/unstable/myst.yml b/unstable/myst.yml new file mode 100644 index 0000000..9dcc603 --- /dev/null +++ b/unstable/myst.yml @@ -0,0 +1,47 @@ +# See docs at: https://mystmd.org/guide/frontmatter +version: 1 + +project: + id: f420092a-e58c-429b-aca2-fb33c47059c7 + jupyter: true + title: "OXBR (Unstable)" + github: https://github.com/MarinhoLab/OpenExecutableBooksRobotics + math: + '\myvec': '\mathbf{\boldsymbol{ #1 }}' + '\mymatrix': '\mathbf{\boldsymbol{ #1 }}' + '\quat': '\mathbf{\boldsymbol{ #1 }}' + '\dual': '\varepsilon' + authors: + - name: Murilo M. Marinho + email: murilo.marinho@manchester.ac.uk + url: https://mmmarinho.github.io + note: Lecturer in Robotics + orcid: 0000-0003-2795-9484 + affiliations: + - id: MMM + institution: The University of Manchester + department: EEE + ror: 027m9bs27 + license: + content: + id: CC-BY-NC-SA-4.0 + toc: + - file: README.md + - file: lesson1_tutorial.md + - file: lesson2_tutorial.md + - file: lesson3_tutorial.md + - file: lesson4_tutorial.md + - file: lesson5_tutorial.md + - file: lesson1_exercise_answers.md + - file: lesson2_exercise_answers.md + - file: lesson3_exercise_answers.md + - file: lesson4_exercise_answers.md + - file: lesson5_exercise_answers.md + +site: + template: book-theme + title: OXBR Website (Unstable) + options: + logo: ../M3logo_black.png + logo_dark: ../M3logo_white.svg + logo_text: "OXBR (Unstable)" \ No newline at end of file