diff --git a/Lecture/TutorialTask_DH_solution.ipynb b/Lecture/TutorialTask_DH_solution.ipynb
deleted file mode 100644
index 19fa910e8b39ed97f0f51d22d4e883bf7300b570..0000000000000000000000000000000000000000
--- a/Lecture/TutorialTask_DH_solution.ipynb
+++ /dev/null
@@ -1,1348 +0,0 @@
-{
- "cells": [
- {
- "attachments": {},
- "cell_type": "markdown",
- "id": "e4ccc978",
- "metadata": {},
- "source": [
- "# Forward Kinematics - Denavit-Hartenberg Matrix\n"
- ]
- },
- {
- "attachments": {},
- "cell_type": "markdown",
- "id": "79d129c2",
- "metadata": {},
- "source": [
- ""
- ]
- },
- {
- "attachments": {},
- "cell_type": "markdown",
- "id": "d138648a",
- "metadata": {},
- "source": [
- "## Exercise 1:\n",
- "\n",
- "The frame attachment information and the DH table of the Puma 560 robot are given below. \n",
- "\n",
- "The constant parameters are\n",
- "- h = 40 cm,\n",
- "- $a_2$ = 50 cm,\n",
- "- $a_3$ = 10 cm,\n",
- "- $d_3$ = 20 cm and\n",
- "- $d_4$ = 30 cm.\n",
- "\n",
- "1. Find transformation matrices between every consectuive frame.\n",
- "\n",
- " **Remember**:\n",
- " \n",
- " $T_{i-1}^i = \n",
- " \\begin{bmatrix}\n",
- " c_{\\theta} & -s_{\\theta}c_{\\alpha} & s_{\\theta}s_{\\alpha} & a_i c_{\\theta} \\\\\n",
- " s_{\\theta} & c_{\\theta}c_{\\alpha} & -c_{\\theta}s_{\\alpha} & a_i s_{\\theta}\\\\\n",
- " 0 & s_{\\alpha} & c_{\\alpha} & d_i\\\\\n",
- " 0 & 0 & 0 & 1\\\\\n",
- " \\end{bmatrix}$\n",
- " \n",
- "\n",
- "2. Find the angular Jacobian matrix ($J_{\\omega}$) between the world frame and the end-effector.\n",
- " \n",
- " **Remember**:\n",
- " \n",
- " $J_{\\omega} = \\begin{bmatrix}\n",
- " R_{0}^1 \\begin{bmatrix}0\\\\0\\\\1\\end{bmatrix} & R_{0}^2 \\begin{bmatrix}0\\\\0\\\\1\\end{bmatrix} & R_{0}^3 \\begin{bmatrix}0\\\\0\\\\1\\end{bmatrix} & R_{0}^4 \\begin{bmatrix}0\\\\0\\\\1\\end{bmatrix} & R_{0}^5 \\begin{bmatrix}0\\\\0\\\\1\\end{bmatrix} & R_{0}^6 \\begin{bmatrix}0\\\\0\\\\1\\end{bmatrix} \n",
- " \\end{bmatrix}$"
- ]
- },
- {
- "cell_type": "code",
- "execution_count": 1,
- "id": "125482c8",
- "metadata": {},
- "outputs": [
- {
- "name": "stdout",
- "output_type": "stream",
- "text": [
- "\u001b[33mDEPRECATION: Configuring installation scheme with distutils config files is deprecated and will no longer work in the near future. If you are using a Homebrew or Linuxbrew Python, please see discussion at https://github.com/Homebrew/homebrew-core/issues/76621\u001b[0m\u001b[33m\n",
- "\u001b[0mRequirement already satisfied: sympy in /opt/homebrew/lib/python3.9/site-packages (1.11.1)\n",
- "Requirement already satisfied: mpmath>=0.19 in /opt/homebrew/lib/python3.9/site-packages (from sympy) (1.3.0)\n",
- "\u001b[33mDEPRECATION: Configuring installation scheme with distutils config files is deprecated and will no longer work in the near future. If you are using a Homebrew or Linuxbrew Python, please see discussion at https://github.com/Homebrew/homebrew-core/issues/76621\u001b[0m\u001b[33m\n",
- "\u001b[0m\n",
- "\u001b[1m[\u001b[0m\u001b[34;49mnotice\u001b[0m\u001b[1;39;49m]\u001b[0m\u001b[39;49m A new release of pip is available: \u001b[0m\u001b[31;49m23.0.1\u001b[0m\u001b[39;49m -> \u001b[0m\u001b[32;49m23.1.2\u001b[0m\n",
- "\u001b[1m[\u001b[0m\u001b[34;49mnotice\u001b[0m\u001b[1;39;49m]\u001b[0m\u001b[39;49m To update, run: \u001b[0m\u001b[32;49mpython3.9 -m pip install --upgrade pip\u001b[0m\n"
- ]
- }
- ],
- "source": [
- "!pip install sympy"
- ]
- },
- {
- "cell_type": "code",
- "execution_count": 2,
- "id": "856e8c26",
- "metadata": {},
- "outputs": [],
- "source": [
- "# importing necessary modules\n",
- "import numpy as np\n",
- "import math \n",
- "import sympy as sym\n",
- "#setting the precision for decimal values.|\n",
- "%precision 4 \n",
- "sym.init_printing() #this will make all the symbolic print pretty."
- ]
- },
- {
- "attachments": {},
- "cell_type": "markdown",
- "id": "3e160bf2",
- "metadata": {},
- "source": [
- "Define your constant parameter **$h$**, **$a_i$** and **$d_i$**. You can define the variable parameters **$\\theta_i$** using the [`Symbol`](https://www.tutorialspoint.com/sympy/sympy_symbols.htm) function available in the `sympy` library. A quick start tutorial to the sympy library can be found [here](https://docs.sympy.org/latest/tutorials/intro-tutorial/gotchas.html). We use the library [sympy](https://www.sympy.org/en/index.html) in the context of this task to allow for matrix multiplication with elements of the matrix being symbolic, since the $\\theta_i$ values are variable parameters. "
- ]
- },
- {
- "cell_type": "code",
- "execution_count": 3,
- "id": "2a7a4011",
- "metadata": {},
- "outputs": [],
- "source": [
- "#YOUR CODE HERE"
- ]
- },
- {
- "cell_type": "code",
- "execution_count": 4,
- "id": "946fe40a",
- "metadata": {},
- "outputs": [],
- "source": [
- "#SAMPLE SOLUTION\n",
- "# constant parameters\n",
- "h = 0.4 # in meter\n",
- "a_2 = 0.5 # in meter\n",
- "a_3 = 0.1 # in meter\n",
- "d_3 = 0.2 # in meter\n",
- "d_4 = 0.3 # in meter\n",
- "\n",
- "# variable parameters\n",
- "#A quick start tutorial to the sympy library - https://docs.sympy.org/latest/tutorials/intro-tutorial/gotchas.html\n",
- "theta_1 = sym.Symbol('theta_1')\n",
- "theta_2 = sym.Symbol('theta_2')\n",
- "theta_3 = sym.Symbol('theta_3')\n",
- "theta_4 = sym.Symbol('theta_4')\n",
- "theta_5 = sym.Symbol('theta_5')\n",
- "theta_6 = sym.Symbol('theta_6')\n"
- ]
- },
- {
- "attachments": {},
- "cell_type": "markdown",
- "id": "023cc991",
- "metadata": {},
- "source": [
- "Create a table to store the value of the DH table.\n",
- "\n",
- "**Hint:** Try using the [`array`](https://numpy.org/doc/stable/reference/generated/numpy.array.html) function from the `numpy` library. Each row of the array will represent row from the DH table."
- ]
- },
- {
- "cell_type": "code",
- "execution_count": 5,
- "id": "6fb13c2f",
- "metadata": {},
- "outputs": [],
- "source": [
- "#YOUR CODE HERE"
- ]
- },
- {
- "cell_type": "code",
- "execution_count": 6,
- "id": "1bd37565",
- "metadata": {},
- "outputs": [
- {
- "name": "stdout",
- "output_type": "stream",
- "text": [
- "[[0 0 0.4 theta_1]\n",
- " [-90 0 0 theta_2]\n",
- " [0 0.5 0.2 theta_3]\n",
- " [-90 0.1 0.3 theta_4]\n",
- " [90 0 0 theta_5]\n",
- " [-90 0 0 theta_6]]\n"
- ]
- }
- ],
- "source": [
- "#SAMPLE SOLUTION\n",
- "# creating an array for the DH table\n",
- "#the values are in the order shown in the table\n",
- "dh_table = np.array([(0,0,h,theta_1),\n",
- " (-90,0,0,theta_2),\n",
- " (0,a_2,d_3,theta_3),\n",
- " (-90,a_3,d_4,theta_4),\n",
- " (90,0,0,theta_5),\n",
- " (-90,0,0,theta_6)])\n",
- "print(dh_table)"
- ]
- },
- {
- "attachments": {},
- "cell_type": "markdown",
- "id": "af6b38c9",
- "metadata": {},
- "source": [
- "Create a function `calc_T` that takes as input the values from each row of the DH table and then calculates the transformation matrix $T_{i-1}^i$ for each row of the DH Table.\n",
- "\n",
- "**Hint:** Check the [`Matrix`](https://docs.sympy.org/latest/tutorials/intro-tutorial/matrices.html) function provided in the `sympy` package\n",
- "\n",
- "**Note:** Once you implement the function remove the `pass` keyword from the function."
- ]
- },
- {
- "cell_type": "code",
- "execution_count": 7,
- "id": "5ae5f46a",
- "metadata": {},
- "outputs": [],
- "source": [
- "# function to calculate transformation matrix\n",
- "def calc_T(alpha, a, d, theta):\n",
- " #YOUR CODE HERE\n",
- " pass"
- ]
- },
- {
- "cell_type": "code",
- "execution_count": 8,
- "id": "0ee7c6bd",
- "metadata": {},
- "outputs": [],
- "source": [
- "#SAMPLE SOLUTION\n",
- "# function to calculate transformation matrix\n",
- "def calc_T(alpha, a, d, theta):\n",
- " \n",
- " s_theta = sym.sin(theta)\n",
- " c_theta = sym.cos(theta)\n",
- " s_alpha = math.sin(math.radians(alpha))\n",
- " c_alpha = math.cos(math.radians(alpha))\n",
- " \n",
- " # Sympy Matrix Documentation - https://docs.sympy.org/latest/tutorials/intro-tutorial/matrices.html\n",
- " T = sym.Matrix([[c_theta, -s_theta*s_alpha, s_theta*s_alpha,a*c_theta],\n",
- " [s_theta, c_theta*c_alpha, -c_theta*s_alpha, a*s_theta],\n",
- " [0, s_alpha, c_alpha, d],\n",
- " [0,0,0,1]])\n",
- " return T"
- ]
- },
- {
- "attachments": {},
- "cell_type": "markdown",
- "id": "9d36c06b",
- "metadata": {},
- "source": [
- "## To Kemal and Jan\n",
- "I think we can leave the below cell as such so that they will understand how the data is being input to the function. We can leave out this section for Task 2."
- ]
- },
- {
- "cell_type": "code",
- "execution_count": 9,
- "id": "fd3975c1",
- "metadata": {},
- "outputs": [],
- "source": [
- "n_frame = dh_table.shape[0]\n",
- "T_list = []\n",
- "for i in range(0,n_frame):\n",
- " T = calc_T(alpha=dh_table[i,0], a=dh_table[i,1], d=dh_table[i,2], theta=dh_table[i,3])\n",
- " T_list.append(T)"
- ]
- },
- {
- "attachments": {},
- "cell_type": "markdown",
- "id": "56230bbd",
- "metadata": {},
- "source": [
- "Print the transformation matrix $T_{0}^1$:"
- ]
- },
- {
- "cell_type": "code",
- "execution_count": 10,
- "id": "fb9fff68",
- "metadata": {},
- "outputs": [],
- "source": [
- "#Print statement here"
- ]
- },
- {
- "cell_type": "code",
- "execution_count": 11,
- "id": "5a2f93fc",
- "metadata": {},
- "outputs": [
- {
- "data": {
- "text/latex": [
- "$\\displaystyle \\left[\\begin{matrix}\\cos{\\left(\\theta_{1} \\right)} & 0 & 0 & 0\\\\\\sin{\\left(\\theta_{1} \\right)} & 1.0 \\cos{\\left(\\theta_{1} \\right)} & 0 & 0\\\\0 & 0 & 1.0 & 0.4\\\\0 & 0 & 0 & 1\\end{matrix}\\right]$"
- ],
- "text/plain": [
- "⎡cos(θ₁) 0 0 0 ⎤\n",
- "⎢ ⎥\n",
- "⎢sin(θ₁) 1.0⋅cos(θ₁) 0 0 ⎥\n",
- "⎢ ⎥\n",
- "⎢ 0 0 1.0 0.4⎥\n",
- "⎢ ⎥\n",
- "⎣ 0 0 0 1 ⎦"
- ]
- },
- "execution_count": 11,
- "metadata": {},
- "output_type": "execute_result"
- }
- ],
- "source": [
- "#SAMPLE SOLUTION\n",
- "T_list[0]"
- ]
- },
- {
- "attachments": {},
- "cell_type": "markdown",
- "id": "2fbdb3d4",
- "metadata": {},
- "source": [
- "Print the transformation matrix $T_{1}^2$:"
- ]
- },
- {
- "cell_type": "code",
- "execution_count": 12,
- "id": "aee3978b",
- "metadata": {},
- "outputs": [],
- "source": [
- "#Print statement here"
- ]
- },
- {
- "cell_type": "code",
- "execution_count": 13,
- "id": "92c4262b",
- "metadata": {},
- "outputs": [
- {
- "data": {
- "text/latex": [
- "$\\displaystyle \\left[\\begin{matrix}\\cos{\\left(\\theta_{2} \\right)} & 1.0 \\sin{\\left(\\theta_{2} \\right)} & - 1.0 \\sin{\\left(\\theta_{2} \\right)} & 0\\\\\\sin{\\left(\\theta_{2} \\right)} & 6.12323399573677 \\cdot 10^{-17} \\cos{\\left(\\theta_{2} \\right)} & 1.0 \\cos{\\left(\\theta_{2} \\right)} & 0\\\\0 & -1.0 & 6.12323399573677 \\cdot 10^{-17} & 0\\\\0 & 0 & 0 & 1\\end{matrix}\\right]$"
- ],
- "text/plain": [
- "⎡cos(θ₂) 1.0⋅sin(θ₂) -1.0⋅sin(θ₂) 0⎤\n",
- "⎢ ⎥\n",
- "⎢sin(θ₂) 6.12323399573677e-17⋅cos(θ₂) 1.0⋅cos(θ₂) 0⎥\n",
- "⎢ ⎥\n",
- "⎢ 0 -1.0 6.12323399573677e-17 0⎥\n",
- "⎢ ⎥\n",
- "⎣ 0 0 0 1⎦"
- ]
- },
- "execution_count": 13,
- "metadata": {},
- "output_type": "execute_result"
- }
- ],
- "source": [
- "#SAMPLE SOLUTION\n",
- "T_list[1]"
- ]
- },
- {
- "attachments": {},
- "cell_type": "markdown",
- "id": "08d7ff99",
- "metadata": {},
- "source": [
- "Print the transformation matrix $T_{2}^3$:"
- ]
- },
- {
- "cell_type": "code",
- "execution_count": 14,
- "id": "4e2ceb82",
- "metadata": {},
- "outputs": [],
- "source": [
- "#Print statement here"
- ]
- },
- {
- "cell_type": "code",
- "execution_count": 15,
- "id": "3013564b",
- "metadata": {},
- "outputs": [
- {
- "data": {
- "text/latex": [
- "$\\displaystyle \\left[\\begin{matrix}\\cos{\\left(\\theta_{3} \\right)} & 0 & 0 & 0.5 \\cos{\\left(\\theta_{3} \\right)}\\\\\\sin{\\left(\\theta_{3} \\right)} & 1.0 \\cos{\\left(\\theta_{3} \\right)} & 0 & 0.5 \\sin{\\left(\\theta_{3} \\right)}\\\\0 & 0 & 1.0 & 0.2\\\\0 & 0 & 0 & 1\\end{matrix}\\right]$"
- ],
- "text/plain": [
- "⎡cos(θ₃) 0 0 0.5⋅cos(θ₃)⎤\n",
- "⎢ ⎥\n",
- "⎢sin(θ₃) 1.0⋅cos(θ₃) 0 0.5⋅sin(θ₃)⎥\n",
- "⎢ ⎥\n",
- "⎢ 0 0 1.0 0.2 ⎥\n",
- "⎢ ⎥\n",
- "⎣ 0 0 0 1 ⎦"
- ]
- },
- "execution_count": 15,
- "metadata": {},
- "output_type": "execute_result"
- }
- ],
- "source": [
- "#SAMPLE SOLUTION\n",
- "T_list[2]"
- ]
- },
- {
- "attachments": {},
- "cell_type": "markdown",
- "id": "66b1f49b",
- "metadata": {},
- "source": [
- "Print the transformation matrix $T_{3}^4$:"
- ]
- },
- {
- "cell_type": "code",
- "execution_count": 16,
- "id": "15a390dc",
- "metadata": {},
- "outputs": [],
- "source": [
- "#Print statement here"
- ]
- },
- {
- "cell_type": "code",
- "execution_count": 17,
- "id": "27ad3be0",
- "metadata": {},
- "outputs": [
- {
- "data": {
- "text/latex": [
- "$\\displaystyle \\left[\\begin{matrix}\\cos{\\left(\\theta_{4} \\right)} & 1.0 \\sin{\\left(\\theta_{4} \\right)} & - 1.0 \\sin{\\left(\\theta_{4} \\right)} & 0.1 \\cos{\\left(\\theta_{4} \\right)}\\\\\\sin{\\left(\\theta_{4} \\right)} & 6.12323399573677 \\cdot 10^{-17} \\cos{\\left(\\theta_{4} \\right)} & 1.0 \\cos{\\left(\\theta_{4} \\right)} & 0.1 \\sin{\\left(\\theta_{4} \\right)}\\\\0 & -1.0 & 6.12323399573677 \\cdot 10^{-17} & 0.3\\\\0 & 0 & 0 & 1\\end{matrix}\\right]$"
- ],
- "text/plain": [
- "⎡cos(θ₄) 1.0⋅sin(θ₄) -1.0⋅sin(θ₄) 0.1⋅cos(θ₄)⎤\n",
- "⎢ ⎥\n",
- "⎢sin(θ₄) 6.12323399573677e-17⋅cos(θ₄) 1.0⋅cos(θ₄) 0.1⋅sin(θ₄)⎥\n",
- "⎢ ⎥\n",
- "⎢ 0 -1.0 6.12323399573677e-17 0.3 ⎥\n",
- "⎢ ⎥\n",
- "⎣ 0 0 0 1 ⎦"
- ]
- },
- "execution_count": 17,
- "metadata": {},
- "output_type": "execute_result"
- }
- ],
- "source": [
- "#SAMPLE SOLUTION\n",
- "T_list[3]"
- ]
- },
- {
- "attachments": {},
- "cell_type": "markdown",
- "id": "5f1a09eb",
- "metadata": {},
- "source": [
- "Print the transformation matrix $T_{4}^5$:"
- ]
- },
- {
- "cell_type": "code",
- "execution_count": 18,
- "id": "a2bda339",
- "metadata": {},
- "outputs": [],
- "source": [
- "#Print statement here"
- ]
- },
- {
- "cell_type": "code",
- "execution_count": 19,
- "id": "e7272bd3",
- "metadata": {},
- "outputs": [
- {
- "data": {
- "text/latex": [
- "$\\displaystyle \\left[\\begin{matrix}\\cos{\\left(\\theta_{5} \\right)} & - 1.0 \\sin{\\left(\\theta_{5} \\right)} & 1.0 \\sin{\\left(\\theta_{5} \\right)} & 0\\\\\\sin{\\left(\\theta_{5} \\right)} & 6.12323399573677 \\cdot 10^{-17} \\cos{\\left(\\theta_{5} \\right)} & - 1.0 \\cos{\\left(\\theta_{5} \\right)} & 0\\\\0 & 1.0 & 6.12323399573677 \\cdot 10^{-17} & 0\\\\0 & 0 & 0 & 1\\end{matrix}\\right]$"
- ],
- "text/plain": [
- "⎡cos(θ₅) -1.0⋅sin(θ₅) 1.0⋅sin(θ₅) 0⎤\n",
- "⎢ ⎥\n",
- "⎢sin(θ₅) 6.12323399573677e-17⋅cos(θ₅) -1.0⋅cos(θ₅) 0⎥\n",
- "⎢ ⎥\n",
- "⎢ 0 1.0 6.12323399573677e-17 0⎥\n",
- "⎢ ⎥\n",
- "⎣ 0 0 0 1⎦"
- ]
- },
- "execution_count": 19,
- "metadata": {},
- "output_type": "execute_result"
- }
- ],
- "source": [
- "#SAMPLE SOLUTION\n",
- "T_list[4]"
- ]
- },
- {
- "attachments": {},
- "cell_type": "markdown",
- "id": "139344a9",
- "metadata": {},
- "source": [
- "Print the transformation matrix $T_{5}^6$:"
- ]
- },
- {
- "cell_type": "code",
- "execution_count": 20,
- "id": "71646159",
- "metadata": {},
- "outputs": [],
- "source": [
- "#Print statement here"
- ]
- },
- {
- "cell_type": "code",
- "execution_count": 21,
- "id": "80000505",
- "metadata": {},
- "outputs": [
- {
- "data": {
- "text/latex": [
- "$\\displaystyle \\left[\\begin{matrix}\\cos{\\left(\\theta_{6} \\right)} & 1.0 \\sin{\\left(\\theta_{6} \\right)} & - 1.0 \\sin{\\left(\\theta_{6} \\right)} & 0\\\\\\sin{\\left(\\theta_{6} \\right)} & 6.12323399573677 \\cdot 10^{-17} \\cos{\\left(\\theta_{6} \\right)} & 1.0 \\cos{\\left(\\theta_{6} \\right)} & 0\\\\0 & -1.0 & 6.12323399573677 \\cdot 10^{-17} & 0\\\\0 & 0 & 0 & 1\\end{matrix}\\right]$"
- ],
- "text/plain": [
- "⎡cos(θ₆) 1.0⋅sin(θ₆) -1.0⋅sin(θ₆) 0⎤\n",
- "⎢ ⎥\n",
- "⎢sin(θ₆) 6.12323399573677e-17⋅cos(θ₆) 1.0⋅cos(θ₆) 0⎥\n",
- "⎢ ⎥\n",
- "⎢ 0 -1.0 6.12323399573677e-17 0⎥\n",
- "⎢ ⎥\n",
- "⎣ 0 0 0 1⎦"
- ]
- },
- "execution_count": 21,
- "metadata": {},
- "output_type": "execute_result"
- }
- ],
- "source": [
- "#SAMPLE SOLUTION\n",
- "T_list[5]"
- ]
- },
- {
- "attachments": {},
- "cell_type": "markdown",
- "id": "95844079",
- "metadata": {},
- "source": [
- "## Note To kemal\n",
- "I don't understand how you want the rotational jacobian to be formed. The lecture say that the jacobian is made of derivatives. But the task mentions to use the rotation matrix. So, I am not sure how to set up this task"
- ]
- },
- {
- "attachments": {},
- "cell_type": "markdown",
- "id": "86f1dd3f",
- "metadata": {},
- "source": [
- "## Exercise 2.1\n",
- ""
- ]
- },
- {
- "attachments": {},
- "cell_type": "markdown",
- "id": "b0494415",
- "metadata": {},
- "source": [
- "For this exercise we will use the same steps as before with the **only** difference being the values of $a_{i-1}$ is also a variable. \n",
- "We will follow the steps below to complete this task\n",
- "\n",
- "1. Define the variables as symbols using the `Symbol` function from the `sympy` library.\n",
- "2. Create a DH table using the `array` function from the `numpy` library.\n",
- "3. Define a function `calc_T` which takes as input the variable required to calculate the transformation matrix $T_{i-1}^i$ .\n",
- "4. Create a `for` loop that will call the `calc_T` repeatedly to calculate the tranformation matrix one by one."
- ]
- },
- {
- "cell_type": "code",
- "execution_count": 22,
- "id": "33a02c35",
- "metadata": {},
- "outputs": [],
- "source": [
- "# 1. Define the variables as symbols using the `Symbol` function from the `sympy` library.\n",
- "length_1 = sym.Symbol('l1')\n",
- "length_2 = sym.Symbol('l2')\n",
- "\n",
- "theta_1 = sym.Symbol('theta_1')\n",
- "theta_2 = sym.Symbol('theta_2')"
- ]
- },
- {
- "cell_type": "code",
- "execution_count": 23,
- "id": "ced0882f",
- "metadata": {},
- "outputs": [],
- "source": [
- "# 2. Create a DH table using the `array` function from the `numpy` library.\n",
- "dh_table = np.array([(0,0,0,theta_1),\n",
- " (0,length_1,0,theta_2),\n",
- " (0,length_2,0,0)])"
- ]
- },
- {
- "cell_type": "code",
- "execution_count": 24,
- "id": "7ad86c87",
- "metadata": {},
- "outputs": [],
- "source": [
- "#3. Define a function `calc_T` which takes as input the variable required to calculate the transformation matrix T_{i-1}^i.\n",
- "#YOUR CODE"
- ]
- },
- {
- "cell_type": "code",
- "execution_count": 25,
- "id": "d735ae4e",
- "metadata": {},
- "outputs": [],
- "source": [
- "#4. Create a `for` loop that will call the `calc_T` repeatedly to calculate the tranformation matrix one by one.\n",
- "n_frame = dh_table.shape[0]\n",
- "T_list = []\n",
- "for i in range(0,n_frame):\n",
- " T = calc_T(alpha=dh_table[i,0], a=dh_table[i,1], d=dh_table[i,2], theta=dh_table[i,3])\n",
- " T_list.append(T)"
- ]
- },
- {
- "attachments": {},
- "cell_type": "markdown",
- "id": "0409b179",
- "metadata": {},
- "source": [
- "Finally calculate the transformation matrix $T_{0}^3$ from the base frame `0` to the end effector frame `3`."
- ]
- },
- {
- "cell_type": "code",
- "execution_count": 26,
- "id": "fcb08280",
- "metadata": {},
- "outputs": [],
- "source": [
- "#calculate the transformation between the base and end-effector frame."
- ]
- },
- {
- "attachments": {},
- "cell_type": "markdown",
- "id": "e40d584d",
- "metadata": {},
- "source": [
- "## Exercise 2.2\n",
- ""
- ]
- },
- {
- "attachments": {},
- "cell_type": "markdown",
- "id": "c87b1985",
- "metadata": {},
- "source": [
- "**Some useful tips:** \n",
- "\n",
- "If you define a single row in the `Matrix` function of the `sympy` package, then it will be taken as a vector. Do not get confused between using the same `Matrix` function to create both a matrix and a vector. The number of rows you define will decide if you are creating a matrix of a vector.\n",
- "- 1 row -> Vector\n",
- "- More than 1 row -> Matrix\n",
- "\n",
- "You can use the `@` operator available in the `sympy` package to do `Matrix-vector` multiplication."
- ]
- },
- {
- "cell_type": "code",
- "execution_count": 27,
- "id": "af96e087",
- "metadata": {},
- "outputs": [
- {
- "data": {
- "text/latex": [
- "$\\displaystyle \\left[\\begin{matrix}14\\\\32\\\\50\\end{matrix}\\right]$"
- ],
- "text/plain": [
- "⎡14⎤\n",
- "⎢ ⎥\n",
- "⎢32⎥\n",
- "⎢ ⎥\n",
- "⎣50⎦"
- ]
- },
- "execution_count": 27,
- "metadata": {},
- "output_type": "execute_result"
- }
- ],
- "source": [
- "#example of how to define a vector.\n",
- "vec_ = sym.Matrix([1,2,3])\n",
- "\n",
- "#printing the vector. Note that you will get a pretty looking vector form if you don't use the print() function.\n",
- "#uncomment the following line to print the vector\n",
- "#vec_\n",
- "\n",
- "#matric vector multiplication.\n",
- "mat_ = sym.Matrix([[1,2,3], \n",
- " [4,5,6],\n",
- " [7,8,9]]) #pay attention to more rows we have added here\n",
- "\n",
- "result = mat_@vec_ # <--- matrix vector multiplication\n",
- "result"
- ]
- },
- {
- "attachments": {},
- "cell_type": "markdown",
- "id": "077dc768",
- "metadata": {},
- "source": [
- "## Outward Iteration\n",
- "\n",
- "**Step 1.1:** Create an empty numpy array where every row will hold the angular velocity and other calculated variable for a particular frame. The size of this array is `number_of_links X 5` since we have 5 dynamic values that we are calculating per frame.\n",
- "\n",
- "**Step 1.2:** Then update the initial values of the links given in the question.\n",
- "\n",
- "$outward\\_itr\\_table = \n",
- "\\begin{bmatrix}\n",
- " {\\omega}_{0}^0 & \\dot{{\\omega}}_{0}^0 & \\dot{{v}}_{0}^{0} & [0] & [0] \\\\\n",
- " {\\omega}_{1}^1 & \\dot{{\\omega}}_{1}^1 & \\dot{{v}}_{1}^{1} & {F}_{0}^{0} & {N}_{0}^{0} \\\\\n",
- " {\\omega}_{2}^2 & \\dot{{\\omega}}_{2}^2 & \\dot{{v}}_{1}^{1} & {F}_{1}^{1} & {N}_{1}^{1} \\\\\n",
- " {\\omega}_{3}^3 & \\dot{{\\omega}}_{3}^3 & \\dot{{v}}_{2}^{2} & {F}_{2}^{2} & {N}_{2}^{2} \\\\\n",
- " \\end{bmatrix}\n",
- "$\n",
- "\n",
- "**Note:** Create this array with type \"`dtype=object`\" since we will be storing vectors will symbolic values"
- ]
- },
- {
- "cell_type": "code",
- "execution_count": 28,
- "id": "ab0a1eaf",
- "metadata": {},
- "outputs": [],
- "source": [
- "#create a empty numpy array of size (number_of_links X number_of_equ_for_forward_iteration)\n",
- "outward_itr_table = np.empty((dh_table.shape[0]+1, 5), dtype=object) #we have five equations per frame\n"
- ]
- },
- {
- "attachments": {},
- "cell_type": "markdown",
- "id": "da3e4f7f",
- "metadata": {},
- "source": [
- "**Step 2:** Create a function `get_rotation_matrix` that takes as input the transformation matrix and return the `3x3` rotation matrix from the transformation matrix.\n",
- "\n",
- "***Hint:*** You can use [matrix slicing](https://docs.sympy.org/latest/modules/matrices/matrices.html#basic-manipulation:~:text=also%20able%20to-,slice%20submatrices,-%3B%20slices%20always%20give) to easily obtain the rotation matrix."
- ]
- },
- {
- "cell_type": "code",
- "execution_count": 29,
- "id": "db15f92b",
- "metadata": {},
- "outputs": [],
- "source": [
- "def get_rotation_matrix(tf_matrix):\n",
- " return tf_matrix[0:3, 0:3]"
- ]
- },
- {
- "attachments": {},
- "cell_type": "markdown",
- "id": "ffa0cb71",
- "metadata": {},
- "source": [
- "**Step 3:** define a function that calculates the forward iteration formula for a given link `i`."
- ]
- },
- {
- "cell_type": "code",
- "execution_count": 30,
- "id": "6f40d19a",
- "metadata": {},
- "outputs": [],
- "source": [
- "def outward_itr(transformation_mat, previous_link_data, link_no, link_len):\n",
- " rot_mat = get_rotation_matrix(transformation_mat)\n",
- " rot_transpose = rot_mat.transpose()\n",
- " q_d = sym.symbols(\"q_d\"+str(link_no)) * sym.Matrix([0, 0, 1]) #q_dot\n",
- " q_dd = sym.symbols(\"q_dd\"+str(link_no)) * sym.Matrix([0, 0, 1]) #q_double_dot\n",
- "\n",
- " omega_ = rot_transpose@previous_link_data[0] + q_d\n",
- " omega_dot_ = rot_transpose@previous_link_data[1] + (rot_transpose@previous_link_data[0]).cross(q_d) + q_dd\n",
- "\n",
- " p_cog = sym.Matrix([link_len/2, 0, 0])\n",
- " p_ = sym.Matrix([link_len, 0, 0])\n",
- "\n",
- " v_dot_cog = previous_link_data[1].cross(p_cog) + previous_link_data[1].cross(previous_link_data[1].cross(p_cog)) + previous_link_data[2]\n",
- " v_dot = rot_transpose@(previous_link_data[1].cross(p_) + previous_link_data[1].cross(previous_link_data[1].cross(p_)) + previous_link_data[2])\n",
- " \n",
- " m_ = sym.symbols(\"m\"+str(link_no-1))\n",
- " F = m_*v_dot_cog\n",
- "\n",
- " i_ = sym.symbols(\"I\"+str(link_no-1))\n",
- " I_ = sym.Matrix([[0,0,0], \n",
- " [0,0,0],\n",
- " [0,0,i_]])\n",
- " N = I_@previous_link_data[1] + previous_link_data[0].cross(I_@previous_link_data[0])\n",
- "\n",
- " return list((omega_, omega_dot_, v_dot, F, N))"
- ]
- },
- {
- "cell_type": "code",
- "execution_count": 31,
- "id": "31297d7b",
- "metadata": {},
- "outputs": [],
- "source": [
- "#define the values for link 0\n",
- "zero_vec = sym.zeros(3,1)\n",
- "outward_itr_table[0] = list((zero_vec, zero_vec, zero_vec, zero_vec, zero_vec))\n",
- "\n",
- "no_of_frames = 4\n",
- "l = [0, length_1, length_2, 0] #length between links\n",
- "\n",
- "for i in range (1, no_of_frames):\n",
- " outward_itr_table[i] = outward_itr(T_list[i-1], outward_itr_table[i-1], i, l[i])\n"
- ]
- },
- {
- "attachments": {},
- "cell_type": "markdown",
- "id": "d1f488ef",
- "metadata": {},
- "source": [
- "Now we will print the data in reach row one by one. Note that we are not using the `print` function here so that we can get a pretty and readable vector form."
- ]
- },
- {
- "cell_type": "code",
- "execution_count": 32,
- "id": "97954636",
- "metadata": {},
- "outputs": [
- {
- "data": {
- "text/latex": [
- "$\\displaystyle \\left( \\left[\\begin{matrix}0\\\\0\\\\0\\end{matrix}\\right], \\ \\left[\\begin{matrix}0\\\\0\\\\0\\end{matrix}\\right], \\ \\left[\\begin{matrix}0\\\\0\\\\0\\end{matrix}\\right], \\ \\left[\\begin{matrix}0\\\\0\\\\0\\end{matrix}\\right], \\ \\left[\\begin{matrix}0\\\\0\\\\0\\end{matrix}\\right]\\right)$"
- ],
- "text/plain": [
- "⎛⎡0⎤ ⎡0⎤ ⎡0⎤ ⎡0⎤ ⎡0⎤⎞\n",
- "⎜⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥⎟\n",
- "⎜⎢0⎥, ⎢0⎥, ⎢0⎥, ⎢0⎥, ⎢0⎥⎟\n",
- "⎜⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥⎟\n",
- "⎝⎣0⎦ ⎣0⎦ ⎣0⎦ ⎣0⎦ ⎣0⎦⎠"
- ]
- },
- "execution_count": 32,
- "metadata": {},
- "output_type": "execute_result"
- }
- ],
- "source": [
- "outward_itr_table[0,0], outward_itr_table[0,1], outward_itr_table[0,2], outward_itr_table[0,3], outward_itr_table[0,4]"
- ]
- },
- {
- "cell_type": "code",
- "execution_count": 33,
- "id": "e6ec791a",
- "metadata": {},
- "outputs": [
- {
- "data": {
- "text/latex": [
- "$\\displaystyle \\left( \\left[\\begin{matrix}0\\\\0\\\\q_{d1}\\end{matrix}\\right], \\ \\left[\\begin{matrix}0\\\\0\\\\q_{dd1}\\end{matrix}\\right], \\ \\left[\\begin{matrix}0\\\\0\\\\0\\end{matrix}\\right], \\ \\left[\\begin{matrix}0\\\\0\\\\0\\end{matrix}\\right], \\ \\left[\\begin{matrix}0\\\\0\\\\0\\end{matrix}\\right]\\right)$"
- ],
- "text/plain": [
- "⎛⎡ 0 ⎤ ⎡ 0 ⎤ ⎡0⎤ ⎡0⎤ ⎡0⎤⎞\n",
- "⎜⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥⎟\n",
- "⎜⎢ 0 ⎥, ⎢ 0 ⎥, ⎢0⎥, ⎢0⎥, ⎢0⎥⎟\n",
- "⎜⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥⎟\n",
- "⎝⎣q_d1⎦ ⎣q_dd1⎦ ⎣0⎦ ⎣0⎦ ⎣0⎦⎠"
- ]
- },
- "execution_count": 33,
- "metadata": {},
- "output_type": "execute_result"
- }
- ],
- "source": [
- "outward_itr_table[1,0], outward_itr_table[1,1], outward_itr_table[1,2], outward_itr_table[1,3], outward_itr_table[1,4]"
- ]
- },
- {
- "cell_type": "code",
- "execution_count": 34,
- "id": "151eac4d",
- "metadata": {},
- "outputs": [
- {
- "data": {
- "text/latex": [
- "$\\displaystyle \\left( \\left[\\begin{matrix}0\\\\0\\\\1.0 q_{d1} + q_{d2}\\end{matrix}\\right], \\ \\left[\\begin{matrix}0\\\\0\\\\1.0 q_{dd1} + q_{dd2}\\end{matrix}\\right], \\ \\left[\\begin{matrix}- l_{2} q_{dd1}^{2} \\cos{\\left(\\theta_{2} \\right)} + l_{2} q_{dd1} \\sin{\\left(\\theta_{2} \\right)}\\\\1.0 l_{2} q_{dd1} \\cos{\\left(\\theta_{2} \\right)}\\\\0\\end{matrix}\\right], \\ \\left[\\begin{matrix}- \\frac{l_{2} m_{1} q_{dd1}^{2}}{2}\\\\\\frac{l_{2} m_{1} q_{dd1}}{2}\\\\0\\end{matrix}\\right], \\ \\left[\\begin{matrix}0\\\\0\\\\I_{1} q_{dd1}\\end{matrix}\\right]\\right)$"
- ],
- "text/plain": [
- "⎛ \n",
- "⎜ \n",
- "⎜ ⎡ 2 \n",
- "⎜⎡ 0 ⎤ ⎡ 0 ⎤ ⎢- l₂⋅q_dd1 ⋅cos(θ₂) + l₂⋅q_dd1⋅sin(θ\n",
- "⎜⎢ ⎥ ⎢ ⎥ ⎢ \n",
- "⎜⎢ 0 ⎥, ⎢ 0 ⎥, ⎢ 1.0⋅l₂⋅q_dd1⋅cos(θ₂) \n",
- "⎜⎢ ⎥ ⎢ ⎥ ⎢ \n",
- "⎜⎣1.0⋅q_d1 + q_d2⎦ ⎣1.0⋅q_dd1 + q_dd2⎦ ⎣ 0 \n",
- "⎜ \n",
- "⎝ \n",
- "\n",
- " ⎡ 2 ⎤ ⎞\n",
- " ⎢-l₂⋅m₁⋅q_dd1 ⎥ ⎟\n",
- " ⎤ ⎢──────────────⎥ ⎟\n",
- "₂)⎥ ⎢ 2 ⎥ ⎡ 0 ⎤⎟\n",
- " ⎥ ⎢ ⎥ ⎢ ⎥⎟\n",
- " ⎥, ⎢ l₂⋅m₁⋅q_dd1 ⎥, ⎢ 0 ⎥⎟\n",
- " ⎥ ⎢ ─────────── ⎥ ⎢ ⎥⎟\n",
- " ⎦ ⎢ 2 ⎥ ⎣I₁⋅q_dd1⎦⎟\n",
- " ⎢ ⎥ ⎟\n",
- " ⎣ 0 ⎦ ⎠"
- ]
- },
- "execution_count": 34,
- "metadata": {},
- "output_type": "execute_result"
- }
- ],
- "source": [
- "outward_itr_table[2,0], outward_itr_table[2,1], outward_itr_table[2,2], outward_itr_table[2,3], outward_itr_table[2,4]"
- ]
- },
- {
- "cell_type": "code",
- "execution_count": 35,
- "id": "84a94f64",
- "metadata": {},
- "outputs": [
- {
- "data": {
- "text/latex": [
- "$\\displaystyle \\left( \\left[\\begin{matrix}0\\\\0\\\\1.0 q_{d1} + 1.0 q_{d2} + q_{d3}\\end{matrix}\\right], \\ \\left[\\begin{matrix}0\\\\0\\\\1.0 q_{dd1} + 1.0 q_{dd2} + q_{dd3}\\end{matrix}\\right], \\ \\left[\\begin{matrix}- l_{2} q_{dd1}^{2} \\cos{\\left(\\theta_{2} \\right)} + l_{2} q_{dd1} \\sin{\\left(\\theta_{2} \\right)}\\\\1.0 l_{2} q_{dd1} \\cos{\\left(\\theta_{2} \\right)}\\\\0\\end{matrix}\\right], \\ \\left[\\begin{matrix}m_{2} \\left(- l_{2} q_{dd1}^{2} \\cos{\\left(\\theta_{2} \\right)} + l_{2} q_{dd1} \\sin{\\left(\\theta_{2} \\right)}\\right)\\\\1.0 l_{2} m_{2} q_{dd1} \\cos{\\left(\\theta_{2} \\right)}\\\\0\\end{matrix}\\right], \\ \\left[\\begin{matrix}0\\\\0\\\\I_{2} \\cdot \\left(1.0 q_{dd1} + q_{dd2}\\right)\\end{matrix}\\right]\\right)$"
- ],
- "text/plain": [
- "⎛ ⎡ 2 \n",
- "⎜⎡ 0 ⎤ ⎡ 0 ⎤ ⎢- l₂⋅q_dd1 ⋅c\n",
- "⎜⎢ ⎥ ⎢ ⎥ ⎢ \n",
- "⎜⎢ 0 ⎥, ⎢ 0 ⎥, ⎢ 1.0⋅\n",
- "⎜⎢ ⎥ ⎢ ⎥ ⎢ \n",
- "⎝⎣1.0⋅q_d1 + 1.0⋅q_d2 + q_d3⎦ ⎣1.0⋅q_dd1 + 1.0⋅q_dd2 + q_dd3⎦ ⎣ \n",
- "\n",
- " ⎤ ⎡ ⎛ 2 ⎞⎤ \n",
- "os(θ₂) + l₂⋅q_dd1⋅sin(θ₂)⎥ ⎢m₂⋅⎝- l₂⋅q_dd1 ⋅cos(θ₂) + l₂⋅q_dd1⋅sin(θ₂)⎠⎥ ⎡ \n",
- " ⎥ ⎢ ⎥ ⎢ \n",
- "l₂⋅q_dd1⋅cos(θ₂) ⎥, ⎢ 1.0⋅l₂⋅m₂⋅q_dd1⋅cos(θ₂) ⎥, ⎢ \n",
- " ⎥ ⎢ ⎥ ⎢ \n",
- " 0 ⎦ ⎣ 0 ⎦ ⎣I₂\n",
- "\n",
- " ⎞\n",
- " 0 ⎤⎟\n",
- " ⎥⎟\n",
- " 0 ⎥⎟\n",
- " ⎥⎟\n",
- "⋅(1.0⋅q_dd1 + q_dd2)⎦⎠"
- ]
- },
- "execution_count": 35,
- "metadata": {},
- "output_type": "execute_result"
- }
- ],
- "source": [
- "outward_itr_table[3,0], outward_itr_table[3,1], outward_itr_table[3,2], outward_itr_table[3,3], outward_itr_table[3,4]"
- ]
- },
- {
- "attachments": {},
- "cell_type": "markdown",
- "id": "4a3ef886",
- "metadata": {},
- "source": [
- "## Inward Iteration\n",
- "Now we will follow similar steps to create the values for the inward iteration. \n",
- "1. Create a numpy array with size `number_of_links X 3` since we have 3 equations per link. Then update this table with the initial values for the last frame.\n",
- "2. Create a fun `inward_itr` that will take the values from the previous step and compute the values for the current link. Note that during inward iteration **we iterate from the last link(end-effector) to the base/first link.**\n",
- "\n",
- "$inward\\_itr\\_table = \n",
- "\\begin{bmatrix}\n",
- " {f}_{0}^0 & {n}_{0}^0 & {\\tau}_{0}^0 \\\\\n",
- " {f}_{1}^1 & {n}_{1}^1 & {\\tau}_{1}^1 \\\\\n",
- " {f}_{2}^2 & {n}_{2}^2 & {\\tau}_{2}^2 \\\\\n",
- " {f}_{3}^3 & {n}_{3}^3 & {\\tau}_{3}^3 \\\\\n",
- " \\end{bmatrix}\n",
- " $\n"
- ]
- },
- {
- "cell_type": "code",
- "execution_count": 36,
- "id": "4f488b95",
- "metadata": {},
- "outputs": [],
- "source": [
- "#creating the inward iteration table.\n",
- "inward_itr_table = np.empty((4, 3), dtype=object) #we have 3 equations per frame"
- ]
- },
- {
- "cell_type": "code",
- "execution_count": 37,
- "id": "54647d68",
- "metadata": {},
- "outputs": [],
- "source": [
- "def inward_itr(transformation_matrix, prev_outward_it, prev_inward_itr, link_len):\n",
- " rot_mat = get_rotation_matrix(transformation_matrix)\n",
- " f_ = rot_mat@prev_inward_itr[0] + prev_outward_it[3]\n",
- "\n",
- " p_cog = sym.Matrix([link_len/2, 0, 0])\n",
- " p_ = sym.Matrix([link_len, 0, 0])\n",
- "\n",
- " n_ = rot_mat@prev_inward_itr[1] + prev_outward_it[4] + p_cog.cross(prev_outward_it[3]) + p_.cross(rot_mat@prev_inward_itr[0])\n",
- "\n",
- " t_ = n_.transpose()*sym.Matrix([0,0,1])\n",
- "\n",
- " return list((f_, n_, t_))"
- ]
- },
- {
- "cell_type": "code",
- "execution_count": 38,
- "id": "f8113379",
- "metadata": {},
- "outputs": [],
- "source": [
- "zero_vec = sym.zeros(3,1)\n",
- "inward_itr_table[3] = list((zero_vec, zero_vec, zero_vec))\n",
- "\n",
- "for i in reversed(range(2+1)): #here we iterate from 2 untill 0 in step of -1\n",
- " inward_itr_table[i] = inward_itr(T_list[i-1], outward_itr_table[i+1], inward_itr_table[i+1], l[i])"
- ]
- },
- {
- "cell_type": "code",
- "execution_count": 39,
- "id": "697fd3af",
- "metadata": {},
- "outputs": [
- {
- "data": {
- "text/latex": [
- "$\\displaystyle \\left( \\left[\\begin{matrix}- \\frac{l_{2} m_{1} q_{dd1}^{2}}{2} + m_{2} \\left(- l_{2} q_{dd1}^{2} \\cos{\\left(\\theta_{2} \\right)} + l_{2} q_{dd1} \\sin{\\left(\\theta_{2} \\right)}\\right) \\cos{\\left(\\theta_{1} \\right)}\\\\0.5 l_{2} m_{1} q_{dd1} + 1.0 l_{2} m_{2} q_{dd1} \\cos{\\left(\\theta_{1} \\right)} \\cos{\\left(\\theta_{2} \\right)} + 1.0 m_{2} \\left(- l_{2} q_{dd1}^{2} \\cos{\\left(\\theta_{2} \\right)} + l_{2} q_{dd1} \\sin{\\left(\\theta_{2} \\right)}\\right) \\sin{\\left(\\theta_{1} \\right)}\\\\0\\end{matrix}\\right], \\ \\left[\\begin{matrix}0\\\\0\\\\1.0 I_{1} q_{dd1} + 1.0 I_{2} \\cdot \\left(1.0 q_{dd1} + q_{dd2}\\right) + 0.25 l_{1} l_{2} m_{1} q_{dd1} + 1.0 l_{1} \\cdot \\left(1.0 l_{2} m_{2} q_{dd1} \\cos{\\left(\\theta_{1} \\right)} \\cos{\\left(\\theta_{2} \\right)} + m_{2} \\left(- l_{2} q_{dd1}^{2} \\cos{\\left(\\theta_{2} \\right)} + l_{2} q_{dd1} \\sin{\\left(\\theta_{2} \\right)}\\right) \\sin{\\left(\\theta_{1} \\right)}\\right) + 0.5 l_{2}^{2} m_{2} q_{dd1} \\cos{\\left(\\theta_{2} \\right)}\\end{matrix}\\right], \\ \\left[\\begin{matrix}1.0 I_{1} q_{dd1} + 1.0 I_{2} \\cdot \\left(1.0 q_{dd1} + q_{dd2}\\right) + 0.25 l_{1} l_{2} m_{1} q_{dd1} + 1.0 l_{1} \\cdot \\left(1.0 l_{2} m_{2} q_{dd1} \\cos{\\left(\\theta_{1} \\right)} \\cos{\\left(\\theta_{2} \\right)} + m_{2} \\left(- l_{2} q_{dd1}^{2} \\cos{\\left(\\theta_{2} \\right)} + l_{2} q_{dd1} \\sin{\\left(\\theta_{2} \\right)}\\right) \\sin{\\left(\\theta_{1} \\right)}\\right) + 0.5 l_{2}^{2} m_{2} q_{dd1} \\cos{\\left(\\theta_{2} \\right)}\\end{matrix}\\right]\\right)$"
- ],
- "text/plain": [
- "⎛⎡ 2 \n",
- "⎜⎢ l₂⋅m₁⋅q_dd1 ⎛ 2 \n",
- "⎜⎢ - ──────────── + m₂⋅⎝- l₂⋅q_dd1 ⋅cos(θ₂) + l₂⋅q_dd1⋅sin(θ\n",
- "⎜⎢ 2 \n",
- "⎜⎢ \n",
- "⎜⎢ ⎛ 2 \n",
- "⎜⎢0.5⋅l₂⋅m₁⋅q_dd1 + 1.0⋅l₂⋅m₂⋅q_dd1⋅cos(θ₁)⋅cos(θ₂) + 1.0⋅m₂⋅⎝- l₂⋅q_dd1 ⋅cos(\n",
- "⎜⎢ \n",
- "⎝⎣ 0 \n",
- "\n",
- " ⎤ \n",
- " ⎞ ⎥ ⎡ \n",
- "₂)⎠⋅cos(θ₁) ⎥ ⎢ \n",
- " ⎥ ⎢ \n",
- " ⎥, ⎢ \n",
- " ⎞ ⎥ ⎢ \n",
- "θ₂) + l₂⋅q_dd1⋅sin(θ₂)⎠⋅sin(θ₁)⎥ ⎣1.0⋅I₁⋅q_dd1 + 1.0⋅I₂⋅(1.0⋅q_dd1 + q_dd2) +\n",
- " ⎥ \n",
- " ⎦ \n",
- "\n",
- " \n",
- " 0 \n",
- " \n",
- " 0 \n",
- " \n",
- " ⎛ ⎛ \n",
- " 0.25⋅l₁⋅l₂⋅m₁⋅q_dd1 + 1.0⋅l₁⋅⎝1.0⋅l₂⋅m₂⋅q_dd1⋅cos(θ₁)⋅cos(θ₂) + m₂⋅⎝- l₂⋅q_dd\n",
- " \n",
- " \n",
- "\n",
- " \n",
- " ⎤ \n",
- " ⎥ \n",
- " ⎥ ⎡ \n",
- " ⎥, ⎣1.0⋅I₁⋅q\n",
- " 2 ⎞ ⎞ 2 ⎥ \n",
- "1 ⋅cos(θ₂) + l₂⋅q_dd1⋅sin(θ₂)⎠⋅sin(θ₁)⎠ + 0.5⋅l₂ ⋅m₂⋅q_dd1⋅cos(θ₂)⎦ \n",
- " \n",
- " \n",
- "\n",
- " \n",
- " \n",
- " \n",
- " ⎛ \n",
- "_dd1 + 1.0⋅I₂⋅(1.0⋅q_dd1 + q_dd2) + 0.25⋅l₁⋅l₂⋅m₁⋅q_dd1 + 1.0⋅l₁⋅⎝1.0⋅l₂⋅m₂⋅q_\n",
- " \n",
- " \n",
- " \n",
- " \n",
- "\n",
- " \n",
- " \n",
- " \n",
- " ⎛ 2 ⎞ ⎞ \n",
- "dd1⋅cos(θ₁)⋅cos(θ₂) + m₂⋅⎝- l₂⋅q_dd1 ⋅cos(θ₂) + l₂⋅q_dd1⋅sin(θ₂)⎠⋅sin(θ₁)⎠ + 0\n",
- " \n",
- " \n",
- " \n",
- " \n",
- "\n",
- " ⎞\n",
- " ⎟\n",
- " ⎟\n",
- " 2 ⎤⎟\n",
- ".5⋅l₂ ⋅m₂⋅q_dd1⋅cos(θ₂)⎦⎟\n",
- " ⎟\n",
- " ⎟\n",
- " ⎟\n",
- " ⎠"
- ]
- },
- "execution_count": 39,
- "metadata": {},
- "output_type": "execute_result"
- }
- ],
- "source": [
- "inward_itr_table[0,0], inward_itr_table[0,1], inward_itr_table[0,2]"
- ]
- },
- {
- "cell_type": "code",
- "execution_count": 40,
- "id": "279d596d",
- "metadata": {},
- "outputs": [
- {
- "data": {
- "text/latex": [
- "$\\displaystyle \\left( \\left[\\begin{matrix}- \\frac{l_{2} m_{1} q_{dd1}^{2}}{2} + m_{2} \\left(- l_{2} q_{dd1}^{2} \\cos{\\left(\\theta_{2} \\right)} + l_{2} q_{dd1} \\sin{\\left(\\theta_{2} \\right)}\\right) \\cos{\\left(\\theta_{1} \\right)}\\\\\\frac{l_{2} m_{1} q_{dd1}}{2} + 1.0 l_{2} m_{2} q_{dd1} \\cos{\\left(\\theta_{1} \\right)} \\cos{\\left(\\theta_{2} \\right)} + m_{2} \\left(- l_{2} q_{dd1}^{2} \\cos{\\left(\\theta_{2} \\right)} + l_{2} q_{dd1} \\sin{\\left(\\theta_{2} \\right)}\\right) \\sin{\\left(\\theta_{1} \\right)}\\\\0\\end{matrix}\\right], \\ \\left[\\begin{matrix}0\\\\0\\\\I_{1} q_{dd1} + 1.0 I_{2} \\cdot \\left(1.0 q_{dd1} + q_{dd2}\\right) + \\frac{l_{1} l_{2} m_{1} q_{dd1}}{4} + l_{1} \\cdot \\left(1.0 l_{2} m_{2} q_{dd1} \\cos{\\left(\\theta_{1} \\right)} \\cos{\\left(\\theta_{2} \\right)} + m_{2} \\left(- l_{2} q_{dd1}^{2} \\cos{\\left(\\theta_{2} \\right)} + l_{2} q_{dd1} \\sin{\\left(\\theta_{2} \\right)}\\right) \\sin{\\left(\\theta_{1} \\right)}\\right) + 0.5 l_{2}^{2} m_{2} q_{dd1} \\cos{\\left(\\theta_{2} \\right)}\\end{matrix}\\right], \\ \\left[\\begin{matrix}I_{1} q_{dd1} + 1.0 I_{2} \\cdot \\left(1.0 q_{dd1} + q_{dd2}\\right) + \\frac{l_{1} l_{2} m_{1} q_{dd1}}{4} + l_{1} \\cdot \\left(1.0 l_{2} m_{2} q_{dd1} \\cos{\\left(\\theta_{1} \\right)} \\cos{\\left(\\theta_{2} \\right)} + m_{2} \\left(- l_{2} q_{dd1}^{2} \\cos{\\left(\\theta_{2} \\right)} + l_{2} q_{dd1} \\sin{\\left(\\theta_{2} \\right)}\\right) \\sin{\\left(\\theta_{1} \\right)}\\right) + 0.5 l_{2}^{2} m_{2} q_{dd1} \\cos{\\left(\\theta_{2} \\right)}\\end{matrix}\\right]\\right)$"
- ],
- "text/plain": [
- "⎛⎡ 2 \n",
- "⎜⎢ l₂⋅m₁⋅q_dd1 ⎛ 2 ⎞ \n",
- "⎜⎢ - ──────────── + m₂⋅⎝- l₂⋅q_dd1 ⋅cos(θ₂) + l₂⋅q_dd1⋅sin(θ₂)⎠⋅\n",
- "⎜⎢ 2 \n",
- "⎜⎢ \n",
- "⎜⎢l₂⋅m₁⋅q_dd1 ⎛ 2 \n",
- "⎜⎢─────────── + 1.0⋅l₂⋅m₂⋅q_dd1⋅cos(θ₁)⋅cos(θ₂) + m₂⋅⎝- l₂⋅q_dd1 ⋅cos(θ₂) + l₂\n",
- "⎜⎢ 2 \n",
- "⎜⎢ \n",
- "⎝⎣ 0 \n",
- "\n",
- " ⎤ \n",
- " ⎥ \n",
- "cos(θ₁) ⎥ ⎡ \n",
- " ⎥ ⎢ \n",
- " ⎥ ⎢ \n",
- " ⎞ ⎥, ⎢ \n",
- "⋅q_dd1⋅sin(θ₂)⎠⋅sin(θ₁)⎥ ⎢ l₁⋅l₂⋅m₁⋅q_\n",
- " ⎥ ⎢I₁⋅q_dd1 + 1.0⋅I₂⋅(1.0⋅q_dd1 + q_dd2) + ───────────\n",
- " ⎥ ⎣ 4 \n",
- " ⎦ \n",
- "\n",
- " \n",
- " \n",
- " 0 \n",
- " \n",
- " 0 \n",
- " \n",
- "dd1 ⎛ ⎛ 2 \n",
- "─── + l₁⋅⎝1.0⋅l₂⋅m₂⋅q_dd1⋅cos(θ₁)⋅cos(θ₂) + m₂⋅⎝- l₂⋅q_dd1 ⋅cos(θ₂) + l₂⋅q_dd1\n",
- " \n",
- " \n",
- "\n",
- " \n",
- " \n",
- " ⎤ \n",
- " ⎥ \n",
- " ⎥ ⎡ \n",
- " ⎥, ⎢I₁⋅q_dd1 + 1.0⋅I₂⋅(1.0⋅q_dd1 \n",
- " ⎞ ⎞ 2 ⎥ ⎣ \n",
- "⋅sin(θ₂)⎠⋅sin(θ₁)⎠ + 0.5⋅l₂ ⋅m₂⋅q_dd1⋅cos(θ₂)⎥ \n",
- " ⎦ \n",
- " \n",
- "\n",
- " \n",
- " \n",
- " \n",
- " \n",
- " l₁⋅l₂⋅m₁⋅q_dd1 ⎛ ⎛ \n",
- "+ q_dd2) + ────────────── + l₁⋅⎝1.0⋅l₂⋅m₂⋅q_dd1⋅cos(θ₁)⋅cos(θ₂) + m₂⋅⎝- l₂⋅q_d\n",
- " 4 \n",
- " \n",
- " \n",
- " \n",
- "\n",
- " ⎞\n",
- " ⎟\n",
- " ⎟\n",
- " ⎟\n",
- " 2 ⎞ ⎞ 2 ⎤⎟\n",
- "d1 ⋅cos(θ₂) + l₂⋅q_dd1⋅sin(θ₂)⎠⋅sin(θ₁)⎠ + 0.5⋅l₂ ⋅m₂⋅q_dd1⋅cos(θ₂)⎥⎟\n",
- " ⎦⎟\n",
- " ⎟\n",
- " ⎟\n",
- " ⎠"
- ]
- },
- "execution_count": 40,
- "metadata": {},
- "output_type": "execute_result"
- }
- ],
- "source": [
- "inward_itr_table[1,0], inward_itr_table[1,1], inward_itr_table[1,2]"
- ]
- },
- {
- "cell_type": "code",
- "execution_count": 41,
- "id": "23a5192e",
- "metadata": {},
- "outputs": [
- {
- "data": {
- "text/latex": [
- "$\\displaystyle \\left( \\left[\\begin{matrix}m_{2} \\left(- l_{2} q_{dd1}^{2} \\cos{\\left(\\theta_{2} \\right)} + l_{2} q_{dd1} \\sin{\\left(\\theta_{2} \\right)}\\right)\\\\1.0 l_{2} m_{2} q_{dd1} \\cos{\\left(\\theta_{2} \\right)}\\\\0\\end{matrix}\\right], \\ \\left[\\begin{matrix}0\\\\0\\\\I_{2} \\cdot \\left(1.0 q_{dd1} + q_{dd2}\\right) + 0.5 l_{2}^{2} m_{2} q_{dd1} \\cos{\\left(\\theta_{2} \\right)}\\end{matrix}\\right], \\ \\left[\\begin{matrix}I_{2} \\cdot \\left(1.0 q_{dd1} + q_{dd2}\\right) + 0.5 l_{2}^{2} m_{2} q_{dd1} \\cos{\\left(\\theta_{2} \\right)}\\end{matrix}\\right]\\right)$"
- ],
- "text/plain": [
- "⎛⎡ ⎛ 2 ⎞⎤ ⎡ 0 \n",
- "⎜⎢m₂⋅⎝- l₂⋅q_dd1 ⋅cos(θ₂) + l₂⋅q_dd1⋅sin(θ₂)⎠⎥ ⎢ \n",
- "⎜⎢ ⎥ ⎢ 0 \n",
- "⎜⎢ 1.0⋅l₂⋅m₂⋅q_dd1⋅cos(θ₂) ⎥, ⎢ \n",
- "⎜⎢ ⎥ ⎢ \n",
- "⎝⎣ 0 ⎦ ⎣I₂⋅(1.0⋅q_dd1 + q_dd2) + 0.5⋅\n",
- "\n",
- " ⎤ ⎞\n",
- " ⎥ ⎟\n",
- " ⎥ ⎡ 2 ⎤⎟\n",
- " ⎥, ⎣I₂⋅(1.0⋅q_dd1 + q_dd2) + 0.5⋅l₂ ⋅m₂⋅q_dd1⋅cos(θ₂)⎦⎟\n",
- " 2 ⎥ ⎟\n",
- "l₂ ⋅m₂⋅q_dd1⋅cos(θ₂)⎦ ⎠"
- ]
- },
- "execution_count": 41,
- "metadata": {},
- "output_type": "execute_result"
- }
- ],
- "source": [
- "inward_itr_table[2,0], inward_itr_table[2,1], inward_itr_table[2,2]"
- ]
- },
- {
- "cell_type": "code",
- "execution_count": 42,
- "id": "6bd8a8be",
- "metadata": {},
- "outputs": [
- {
- "data": {
- "text/latex": [
- "$\\displaystyle \\left( \\left[\\begin{matrix}0\\\\0\\\\0\\end{matrix}\\right], \\ \\left[\\begin{matrix}0\\\\0\\\\0\\end{matrix}\\right], \\ \\left[\\begin{matrix}0\\\\0\\\\0\\end{matrix}\\right]\\right)$"
- ],
- "text/plain": [
- "⎛⎡0⎤ ⎡0⎤ ⎡0⎤⎞\n",
- "⎜⎢ ⎥ ⎢ ⎥ ⎢ ⎥⎟\n",
- "⎜⎢0⎥, ⎢0⎥, ⎢0⎥⎟\n",
- "⎜⎢ ⎥ ⎢ ⎥ ⎢ ⎥⎟\n",
- "⎝⎣0⎦ ⎣0⎦ ⎣0⎦⎠"
- ]
- },
- "execution_count": 42,
- "metadata": {},
- "output_type": "execute_result"
- }
- ],
- "source": [
- "inward_itr_table[3,0], inward_itr_table[3,1], inward_itr_table[3,2]"
- ]
- }
- ],
- "metadata": {
- "kernelspec": {
- "display_name": "Python 3 (ipykernel)",
- "language": "python",
- "name": "python3"
- },
- "language_info": {
- "codemirror_mode": {
- "name": "ipython",
- "version": 3
- },
- "file_extension": ".py",
- "mimetype": "text/x-python",
- "name": "python",
- "nbconvert_exporter": "python",
- "pygments_lexer": "ipython3",
- "version": "3.9.12"
- }
- },
- "nbformat": 4,
- "nbformat_minor": 5
-}