├── .gitignore ├── LICENSE ├── Part 1 ├── 1 Manipulator Kinematics.ipynb ├── 2 The Manipulator Jacobian.ipynb ├── 3 Resolved-Rate Motion Control.ipynb ├── 4 Numerical Inverse Kinematics.ipynb ├── 5 Manipulator Performance Measures.ipynb ├── img │ ├── cover.png │ ├── elementary_transforms_dark.png │ ├── hessian_dark.png │ ├── jacobian_dark.png │ ├── rotation_dark.png │ ├── skew_dark.png │ ├── title.png │ └── transform_dark.png └── style │ └── style.css ├── Part 2 ├── 1 The Manipulator Hessian.ipynb ├── 2 Higher Order Derivatives.ipynb ├── 3 Analytic Forms.ipynb ├── 4 Null-Space Projection for Motion Control.ipynb ├── 5 Quadratic Programming for Motion Control.ipynb ├── 6 Advanced Numerical Inverse Kinematics.ipynb ├── 7 Quadratic-Rate Motion Control.ipynb ├── img │ ├── cover.png │ ├── elementary_transforms_dark.png │ ├── hessian_dark.png │ ├── jacobian_dark.png │ ├── rotation_dark.png │ ├── skew_dark.png │ ├── title.png │ └── transform_dark.png └── style │ └── style.css ├── img ├── article1.png ├── article2.png └── qcr.png └── readme.md /.gitignore: -------------------------------------------------------------------------------- 1 | 2 | .vscode/** 3 | 4 | .DS_Store 5 | 6 | # Byte-compiled / optimized / DLL files 7 | __pycache__/ 8 | *.py[cod] 9 | *$py.class 10 | 11 | # C extensions 12 | *.so 13 | 14 | # Distribution / packaging 15 | .Python 16 | build/ 17 | develop-eggs/ 18 | dist/ 19 | downloads/ 20 | eggs/ 21 | .eggs/ 22 | lib/ 23 | lib64/ 24 | parts/ 25 | sdist/ 26 | var/ 27 | wheels/ 28 | pip-wheel-metadata/ 29 | share/python-wheels/ 30 | *.egg-info/ 31 | .installed.cfg 32 | *.egg 33 | MANIFEST 34 | 35 | # PyInstaller 36 | # Usually these files are written by a python script from a template 37 | # before PyInstaller builds the exe, so as to inject date/other infos into it. 38 | *.manifest 39 | *.spec 40 | 41 | # Installer logs 42 | pip-log.txt 43 | pip-delete-this-directory.txt 44 | 45 | # Unit test / coverage reports 46 | htmlcov/ 47 | .tox/ 48 | .nox/ 49 | .coverage 50 | .coverage.* 51 | .cache 52 | nosetests.xml 53 | coverage.xml 54 | *.cover 55 | *.py,cover 56 | .hypothesis/ 57 | .pytest_cache/ 58 | 59 | # Translations 60 | *.mo 61 | *.pot 62 | 63 | # Django stuff: 64 | *.log 65 | local_settings.py 66 | db.sqlite3 67 | db.sqlite3-journal 68 | 69 | # Flask stuff: 70 | instance/ 71 | .webassets-cache 72 | 73 | # Scrapy stuff: 74 | .scrapy 75 | 76 | # Sphinx documentation 77 | docs/_build/ 78 | 79 | # PyBuilder 80 | target/ 81 | 82 | # Jupyter Notebook 83 | .ipynb_checkpoints 84 | 85 | # IPython 86 | profile_default/ 87 | ipython_config.py 88 | 89 | # pyenv 90 | .python-version 91 | 92 | # pipenv 93 | # According to pypa/pipenv#598, it is recommended to include Pipfile.lock in version control. 94 | # However, in case of collaboration, if having platform-specific dependencies or dependencies 95 | # having no cross-platform support, pipenv may install dependencies that don't work, or not 96 | # install all needed dependencies. 97 | #Pipfile.lock 98 | 99 | # PEP 582; used by e.g. github.com/David-OConnor/pyflow 100 | __pypackages__/ 101 | 102 | # Celery stuff 103 | celerybeat-schedule 104 | celerybeat.pid 105 | 106 | # SageMath parsed files 107 | *.sage.py 108 | 109 | # Environments 110 | .env 111 | .venv 112 | env/ 113 | venv/ 114 | ENV/ 115 | env.bak/ 116 | venv.bak/ 117 | 118 | # Spyder project settings 119 | .spyderproject 120 | .spyproject 121 | 122 | # Rope project settings 123 | .ropeproject 124 | 125 | # mkdocs documentation 126 | /site 127 | 128 | # mypy 129 | .mypy_cache/ 130 | .dmypy.json 131 | dmypy.json 132 | 133 | # Pyre type checker 134 | .pyre/ 135 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2022 Jesse Haviland 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /Part 1/1 Manipulator Kinematics.ipynb: -------------------------------------------------------------------------------- 1 | { 2 | "cells": [ 3 | { 4 | "cell_type": "code", 5 | "execution_count": 1, 6 | "metadata": {}, 7 | "outputs": [], 8 | "source": [ 9 | "try:\n", 10 | " # We must install required packages if we are in Google Colab\n", 11 | " import google.colab\n", 12 | " %pip install roboticstoolbox-python>=1.0.2\n", 13 | "except:\n", 14 | " # We are not in Google Colab\n", 15 | " # Apply custon style to notebook\n", 16 | " from IPython.core.display import HTML\n", 17 | " import pathlib\n", 18 | " styles_path = pathlib.Path(pathlib.Path().absolute(), \"style\", \"style.css\")\n", 19 | " styles = open(styles_path, \"r\").read()\n", 20 | " HTML(f\"\")" 21 | ] 22 | }, 23 | { 24 | "attachments": {}, 25 | "cell_type": "markdown", 26 | "metadata": {}, 27 | "source": [ 28 | "# 1.0 Manipulator Kinematics\n", 29 | "\n", 30 | "$\\large{\\text{Manipulator Differential Kinematics}} \\\\ \\large{\\text{Part I: Kinematics, Velocity, and Applications}}$\n", 31 | "\n", 32 | "$\\text{By Jesse Haviland and Peter Corke}$\n", 33 | "\n", 34 | "
\n", 35 | "\n", 36 | "The sections of the Tutorial paper related to this notebook are listed next to each contents entry.\n", 37 | "It is beneficial to read these sections of the paper before attempting the notebook Section.\n", 38 | "\n", 39 | "### Contents\n", 40 | "\n", 41 | "[1.1 The Elementary Transform Sequence](#ets)\n", 42 | "* Introduction\n", 43 | "* Forward Kinematics\n", 44 | "\n", 45 | "[1.2 Forward Kinematics](#fk)\n", 46 | "* Forward Kinematics\n" 47 | ] 48 | }, 49 | { 50 | "cell_type": "code", 51 | "execution_count": 2, 52 | "metadata": {}, 53 | "outputs": [], 54 | "source": [ 55 | "# We will do the imports required for this notebook here\n", 56 | "\n", 57 | "# numpy provides import array and linear algebra utilities\n", 58 | "import numpy as np\n", 59 | "\n", 60 | "# the robotics toolbox provides robotics specific functionality\n", 61 | "import roboticstoolbox as rtb\n", 62 | "\n", 63 | "# spatial math provides objects for representing transformations\n", 64 | "import spatialmath as sm" 65 | ] 66 | }, 67 | { 68 | "attachments": {}, 69 | "cell_type": "markdown", 70 | "metadata": {}, 71 | "source": [ 72 | "
\n", 73 | "\n", 74 | "\n", 75 | "### 1.1 The Elementary Transform Sequence\n", 76 | "---" 77 | ] 78 | }, 79 | { 80 | "attachments": {}, 81 | "cell_type": "markdown", 82 | "metadata": {}, 83 | "source": [ 84 | "The elementary transform sequence (ETS) provides a universal method for describing the kinematics of any manipulator. An ETS comprises a number of elementary transforms $\\bf{E}_i$ -- translations and rotations -- from the base frame to the robot's end-effector.\n", 85 | "\n", 86 | "\\begin{align*}\n", 87 | " \\bf{E}_i = \n", 88 | " \\left\\{\n", 89 | " \\begin{matrix}\n", 90 | " \\bf{T}_{t_{x}}(\\eta_i) \\\\\n", 91 | " \\bf{T}_{t_{y}}(\\eta_i) \\\\\n", 92 | " \\bf{T}_{t_{z}}(\\eta_i) \\\\\n", 93 | " \\bf{T}_{R_{x}}(\\eta_i) \\\\\n", 94 | " \\bf{T}_{R_{y}}(\\eta_i) \\\\\n", 95 | " \\bf{T}_{R_{z}}(\\eta_i)\n", 96 | " \\end{matrix}\n", 97 | " \\right.\n", 98 | "\\end{align*}\n", 99 | "\n", 100 | "where the parameter $\\eta_i$ is either a constant $c_i$ (translational offset or rotation) or a joint variable $q_j(t)$\n", 101 | "\n", 102 | "\\begin{equation*}\n", 103 | " \\eta_i = \n", 104 | " \\left\\{\n", 105 | " \\begin{matrix}\n", 106 | " c_i \\\\\n", 107 | " q_j(t) \\\\\n", 108 | " \\end{matrix}\n", 109 | " \\right. \n", 110 | "\\end{equation*}\n", 111 | "\n", 112 | "and the joint variable is\n", 113 | "\n", 114 | "\\begin{equation*}\n", 115 | " q_j(t) = \n", 116 | " \\left\\{\n", 117 | " \\begin{matrix}\n", 118 | " \\theta_j(t) & \\quad \\text{for a revolute joint}\\\\\n", 119 | " d_j(t) & \\quad \\text{for a prismatic joint}\\\\\n", 120 | " \\end{matrix}\n", 121 | " \\right. \n", 122 | "\\end{equation*}\n", 123 | "where $\\theta_j(t)$ represents a joint angle, and $d_j(t)$ represents a joint translation.\n", 124 | "\n", 125 | "Each of the 6 possible ETs are displayed below\n", 126 | "\n", 127 | "
\n", 128 | "\n", 129 | "
\n", 130 | "\n", 131 | "\"drawing\"\n", 132 | "\n", 133 | "
\n", 134 | "\n", 135 | "
\n", 136 | "\n", 137 | "Using the Robotics Toolbox for Python we can create an ET" 138 | ] 139 | }, 140 | { 141 | "cell_type": "code", 142 | "execution_count": 3, 143 | "metadata": {}, 144 | "outputs": [ 145 | { 146 | "name": "stdout", 147 | "output_type": "stream", 148 | "text": [ 149 | "Rx(q)\n", 150 | "Rx(90°)\n" 151 | ] 152 | } 153 | ], 154 | "source": [ 155 | "# This is a variable rotation around the x-axis\n", 156 | "rx_var = rtb.ET.Rx()\n", 157 | "\n", 158 | "# This is a constant rotation around the x-axis by 90 degrees\n", 159 | "rx_cons = rtb.ET.Rx(np.pi / 2)\n", 160 | "\n", 161 | "# By printing each of the ET's we made, we can view them in a more readable format\n", 162 | "print(rx_var)\n", 163 | "print(rx_cons)" 164 | ] 165 | }, 166 | { 167 | "cell_type": "code", 168 | "execution_count": 4, 169 | "metadata": {}, 170 | "outputs": [ 171 | { 172 | "name": "stdout", 173 | "output_type": "stream", 174 | "text": [ 175 | "Numpy array SE3: \n", 176 | "[[ 1.000000e+00 0.000000e+00 0.000000e+00 0.000000e+00]\n", 177 | " [ 0.000000e+00 6.123234e-17 -1.000000e+00 0.000000e+00]\n", 178 | " [ 0.000000e+00 1.000000e+00 6.123234e-17 0.000000e+00]\n", 179 | " [ 0.000000e+00 0.000000e+00 0.000000e+00 1.000000e+00]]\n", 180 | "\n", 181 | "Spatialmath SE3: \n", 182 | " \u001b[38;5;1m 1 \u001b[0m \u001b[38;5;1m 0 \u001b[0m \u001b[38;5;1m 0 \u001b[0m \u001b[38;5;4m 0 \u001b[0m \u001b[0m\n", 183 | " \u001b[38;5;1m 0 \u001b[0m \u001b[38;5;1m 0 \u001b[0m \u001b[38;5;1m-1 \u001b[0m \u001b[38;5;4m 0 \u001b[0m \u001b[0m\n", 184 | " \u001b[38;5;1m 0 \u001b[0m \u001b[38;5;1m 1 \u001b[0m \u001b[38;5;1m 0 \u001b[0m \u001b[38;5;4m 0 \u001b[0m \u001b[0m\n", 185 | " \u001b[38;5;244m 0 \u001b[0m \u001b[38;5;244m 0 \u001b[0m \u001b[38;5;244m 0 \u001b[0m \u001b[38;5;244m 1 \u001b[0m \u001b[0m\n", 186 | "\n" 187 | ] 188 | } 189 | ], 190 | "source": [ 191 | "# We can calculate the transform resulting from the rx_cons ET using the .A method\n", 192 | "transform = rx_cons.A()\n", 193 | "\n", 194 | "# The .A method returns a numpy array\n", 195 | "# Using the spatialmath package, we can create an SE3 object from the array\n", 196 | "sm_transform = sm.SE3(transform)\n", 197 | "\n", 198 | "# The spatialmath package provides great utility for working with transforms \n", 199 | "# and will print SE3s in a more intuitive way a plain numpy array\n", 200 | "\n", 201 | "print(f\"Numpy array SE3: \\n{transform}\")\n", 202 | "print()\n", 203 | "print(f\"Spatialmath SE3: \\n{sm_transform}\")" 204 | ] 205 | }, 206 | { 207 | "cell_type": "code", 208 | "execution_count": 5, 209 | "metadata": {}, 210 | "outputs": [ 211 | { 212 | "name": "stdout", 213 | "output_type": "stream", 214 | "text": [ 215 | "Resulting SE3 at 45 degrees: \n", 216 | " \u001b[38;5;1m 1 \u001b[0m \u001b[38;5;1m 0 \u001b[0m \u001b[38;5;1m 0 \u001b[0m \u001b[38;5;4m 0 \u001b[0m \u001b[0m\n", 217 | " \u001b[38;5;1m 0 \u001b[0m \u001b[38;5;1m 0.7071 \u001b[0m \u001b[38;5;1m-0.7071 \u001b[0m \u001b[38;5;4m 0 \u001b[0m \u001b[0m\n", 218 | " \u001b[38;5;1m 0 \u001b[0m \u001b[38;5;1m 0.7071 \u001b[0m \u001b[38;5;1m 0.7071 \u001b[0m \u001b[38;5;4m 0 \u001b[0m \u001b[0m\n", 219 | " \u001b[38;5;244m 0 \u001b[0m \u001b[38;5;244m 0 \u001b[0m \u001b[38;5;244m 0 \u001b[0m \u001b[38;5;244m 1 \u001b[0m \u001b[0m\n", 220 | "\n" 221 | ] 222 | } 223 | ], 224 | "source": [ 225 | "# To calculate the transform resulting from the rx_var ET, \n", 226 | "# we must supply a joint coordinate when using the .A method\n", 227 | "\n", 228 | "# Make the joint at 45 degrees\n", 229 | "q = np.pi / 4\n", 230 | "transform = rx_var.A(q)\n", 231 | "sm_transform = sm.SE3(transform)\n", 232 | "print(f\"Resulting SE3 at 45 degrees: \\n{sm_transform}\")" 233 | ] 234 | }, 235 | { 236 | "cell_type": "code", 237 | "execution_count": 6, 238 | "metadata": {}, 239 | "outputs": [ 240 | { 241 | "name": "stdout", 242 | "output_type": "stream", 243 | "text": [ 244 | "ty(q)\n", 245 | "ty(0.25)\n" 246 | ] 247 | } 248 | ], 249 | "source": [ 250 | "# We can also create prismatic joints\n", 251 | "# This is a variable translation around the y-axis\n", 252 | "ty_var = rtb.ET.ty()\n", 253 | "\n", 254 | "# This is a constant translation along the y-axis by 25 cm\n", 255 | "ty_cons = rtb.ET.ty(0.25)\n", 256 | "\n", 257 | "# View the ETs\n", 258 | "print(ty_var)\n", 259 | "print(ty_cons)" 260 | ] 261 | }, 262 | { 263 | "cell_type": "code", 264 | "execution_count": 7, 265 | "metadata": {}, 266 | "outputs": [ 267 | { 268 | "name": "stdout", 269 | "output_type": "stream", 270 | "text": [ 271 | "SE3: \n", 272 | " \u001b[38;5;1m 1 \u001b[0m \u001b[38;5;1m 0 \u001b[0m \u001b[38;5;1m 0 \u001b[0m \u001b[38;5;4m 0 \u001b[0m \u001b[0m\n", 273 | " \u001b[38;5;1m 0 \u001b[0m \u001b[38;5;1m 1 \u001b[0m \u001b[38;5;1m 0 \u001b[0m \u001b[38;5;4m 0.25 \u001b[0m \u001b[0m\n", 274 | " \u001b[38;5;1m 0 \u001b[0m \u001b[38;5;1m 0 \u001b[0m \u001b[38;5;1m 1 \u001b[0m \u001b[38;5;4m 0 \u001b[0m \u001b[0m\n", 275 | " \u001b[38;5;244m 0 \u001b[0m \u001b[38;5;244m 0 \u001b[0m \u001b[38;5;244m 0 \u001b[0m \u001b[38;5;244m 1 \u001b[0m \u001b[0m\n", 276 | "\n" 277 | ] 278 | } 279 | ], 280 | "source": [ 281 | "# We can calculate the transform resulting from the ty_cons ET using the .A method\n", 282 | "transform = ty_cons.A()\n", 283 | "\n", 284 | "# Create an SE3 object from the array\n", 285 | "sm_transform = sm.SE3(transform)\n", 286 | "\n", 287 | "print(f\"SE3: \\n{sm_transform}\")" 288 | ] 289 | }, 290 | { 291 | "cell_type": "code", 292 | "execution_count": 8, 293 | "metadata": {}, 294 | "outputs": [ 295 | { 296 | "name": "stdout", 297 | "output_type": "stream", 298 | "text": [ 299 | "Resulting SE3 at 15 cm: \n", 300 | " \u001b[38;5;1m 1 \u001b[0m \u001b[38;5;1m 0 \u001b[0m \u001b[38;5;1m 0 \u001b[0m \u001b[38;5;4m 0 \u001b[0m \u001b[0m\n", 301 | " \u001b[38;5;1m 0 \u001b[0m \u001b[38;5;1m 1 \u001b[0m \u001b[38;5;1m 0 \u001b[0m \u001b[38;5;4m 0.15 \u001b[0m \u001b[0m\n", 302 | " \u001b[38;5;1m 0 \u001b[0m \u001b[38;5;1m 0 \u001b[0m \u001b[38;5;1m 1 \u001b[0m \u001b[38;5;4m 0 \u001b[0m \u001b[0m\n", 303 | " \u001b[38;5;244m 0 \u001b[0m \u001b[38;5;244m 0 \u001b[0m \u001b[38;5;244m 0 \u001b[0m \u001b[38;5;244m 1 \u001b[0m \u001b[0m\n", 304 | "\n" 305 | ] 306 | } 307 | ], 308 | "source": [ 309 | "# To calculate the transform resulting from the ty_var ET, \n", 310 | "# we must supply a joint coordinate when using the .A method\n", 311 | "\n", 312 | "# Make the joint at 15 cm\n", 313 | "q = 0.15\n", 314 | "transform = ty_var.A(q)\n", 315 | "sm_transform = sm.SE3(transform)\n", 316 | "print(f\"Resulting SE3 at 15 cm: \\n{sm_transform}\")" 317 | ] 318 | }, 319 | { 320 | "attachments": {}, 321 | "cell_type": "markdown", 322 | "metadata": {}, 323 | "source": [ 324 | "\n", 325 | "Below is a schematic of a seven degree-of-freedom Franka Emika Panda manipulator. It contains 15 ETs to represent its kinematics.\n", 326 | "\n", 327 | "\"drawing\"\n", 328 | "\n", 329 | "Using the ET class we can make all 15 of these ETs in Python\n" 330 | ] 331 | }, 332 | { 333 | "attachments": {}, 334 | "cell_type": "markdown", 335 | "metadata": {}, 336 | "source": [] 337 | }, 338 | { 339 | "cell_type": "code", 340 | "execution_count": 9, 341 | "metadata": {}, 342 | "outputs": [ 343 | { 344 | "name": "stdout", 345 | "output_type": "stream", 346 | "text": [ 347 | "tz(0.333) ⊕ Rz(q0) ⊕ Ry(q1)\n", 348 | "tz(0.333) ⊕ Rz(q0) ⊕ Ry(q1)\n", 349 | "tz(0.333) ⊕ Rz(q0) ⊕ Ry(q1) ⊕ tz(0.316)\n", 350 | "tz(0.333) ⊕ Rz(q0) ⊕ Ry(q1) ⊕ tz(0.316) ⊕ Rz(q0)\n" 351 | ] 352 | } 353 | ], 354 | "source": [ 355 | "# Note for for E7 and E11 in the figure above and code below, we use flip=True\n", 356 | "# as the variable rotation is in the negative direction.\n", 357 | "\n", 358 | "E1 = rtb.ET.tz(0.333)\n", 359 | "E2 = rtb.ET.Rz()\n", 360 | "E3 = rtb.ET.Ry()\n", 361 | "E4 = rtb.ET.tz(0.316)\n", 362 | "E5 = rtb.ET.Rz()\n", 363 | "E6 = rtb.ET.tx(0.0825)\n", 364 | "E7 = rtb.ET.Ry(flip=True)\n", 365 | "E8 = rtb.ET.tx(-0.0825)\n", 366 | "E9 = rtb.ET.tz(0.384)\n", 367 | "E10 = rtb.ET.Rz()\n", 368 | "E11 = rtb.ET.Ry(flip=True)\n", 369 | "E12 = rtb.ET.tx(0.088)\n", 370 | "E13 = rtb.ET.Rx(np.pi)\n", 371 | "E14 = rtb.ET.tz(0.107)\n", 372 | "E15 = rtb.ET.Rz()\n", 373 | "\n", 374 | "# We can create and ETS in a number of ways\n", 375 | "\n", 376 | "# Firstly if we use the * operator between two or more ETs, we get an ETS\n", 377 | "ets1 = E1 * E2 * E3\n", 378 | "\n", 379 | "# Secondly, we can use the ETS constructor and pass in a list of ETs\n", 380 | "ets2 = rtb.ETS([E1, E2, E3])\n", 381 | "\n", 382 | "# We can also use the * operator between ETS' and ETs to concatenate\n", 383 | "ets3 = ets2 * E4\n", 384 | "ets4 = ets2 * rtb.ETS([E4, E5])\n", 385 | "\n", 386 | "print(ets1)\n", 387 | "print(ets2)\n", 388 | "print(ets3)\n", 389 | "print(ets4)\n" 390 | ] 391 | }, 392 | { 393 | "cell_type": "code", 394 | "execution_count": 10, 395 | "metadata": {}, 396 | "outputs": [ 397 | { 398 | "name": "stdout", 399 | "output_type": "stream", 400 | "text": [ 401 | "tz(0.333) ⊕ Rz(q0) ⊕ Ry(q1) ⊕ tz(0.316) ⊕ Rz(q2) ⊕ tx(0.0825) ⊕ Ry(-q3) ⊕ tx(-0.0825) ⊕ tz(0.384) ⊕ Rz(q4) ⊕ Ry(-q5) ⊕ tx(0.088) ⊕ Rx(180°) ⊕ tz(0.107) ⊕ Rz(q6)\n", 402 | "\n", 403 | "The panda has 7 joints\n", 404 | "The panda has 15 ETs\n", 405 | "The second ET in the ETS is Rz(q0)\n", 406 | "The first variable joint has a jindex of 0, while the second has a jindex of 1\n", 407 | "\n", 408 | "All variable liks in the Panda ETS: \n", 409 | "[ET.Rz(jindex=0), ET.Ry(jindex=1), ET.Rz(jindex=2), ET.Ry(jindex=3, flip=True), ET.Rz(jindex=4), ET.Ry(jindex=5, flip=True), ET.Rz(jindex=6)]\n" 410 | ] 411 | } 412 | ], 413 | "source": [ 414 | "# We can make an ETS representing a Panda by incorprating all 15 ETs into an ETS\n", 415 | "panda = E1 * E2 * E3 * E4 * E5 * E6 * E7 * E8 * E9 * E10 * E11 * E12 * E13 * E14 * E15\n", 416 | "\n", 417 | "# View the ETS\n", 418 | "print(panda)\n", 419 | "print()\n", 420 | "\n", 421 | "# The ETS class has many usefull properties\n", 422 | "# print the number of joints in the panda model\n", 423 | "print(f\"The panda has {panda.n} joints\")\n", 424 | "\n", 425 | "# print the number of ETs in the panda model\n", 426 | "print(f\"The panda has {panda.m} ETs\")\n", 427 | "\n", 428 | "# We can access an ET from an ETS as if the ETS were a Python list\n", 429 | "print(f\"The second ET in the ETS is {panda[1]}\")\n", 430 | "\n", 431 | "# When a variable ET is added to an ETS, it is assigned a jindex, which is short for joint index\n", 432 | "# When given an array of joint coordinates (i.e. joint angles), the ETS will use the jindices of each\n", 433 | "# variable ET to correspond with elements of the given joint coordiante array\n", 434 | "print(f\"The first variable joint has a jindex of {panda[1].jindex}, while the second has a jindex of {panda[2].jindex}\")\n", 435 | "\n", 436 | "# We can extract all of the variable ETs from the panda model as a list\n", 437 | "print(f\"\\nAll variable liks in the Panda ETS: \\n{panda.joints()}\")" 438 | ] 439 | }, 440 | { 441 | "attachments": {}, 442 | "cell_type": "markdown", 443 | "metadata": {}, 444 | "source": [ 445 | "
\n", 446 | "\n", 447 | "\n", 448 | "### 1.2 Forward Kinematics\n", 449 | "---" 450 | ] 451 | }, 452 | { 453 | "attachments": {}, 454 | "cell_type": "markdown", 455 | "metadata": {}, 456 | "source": [ 457 | "\n", 458 | "The forward kinematics is the first and most basic relationship between the link geometry and robot configuration.\n", 459 | "\n", 460 | "The forward kinematics of a manipulator provides a non-linear mapping\n", 461 | "\\begin{equation*}\n", 462 | " {^0\\bf{T}_e(t)} = {\\cal K}(\\bf{q}(t))\n", 463 | "\\end{equation*}\n", 464 | "between the joint space and Cartesian task space,\n", 465 | "where $\\bf{q}(t) = (q_1(t), q_2(t), \\cdots q_n(t)) \\in \\mathbb{R}^n$ is the vector of joint generalised coordinates, $n$ is the number of joints, and ${^0\\bf{T}_e} \\in \\bf{SE}{3}$ is a homogeneous transformation matrix representing the pose of the robot's end-effector in the world-coordinate frame. The ETS model defines $\\cal{K}(\\cdot)$ as the product of $M$ elementary transforms $\\bf{E}_i \\in \\bf{SE}{3}$\n", 466 | "\\begin{align*}\n", 467 | " {^0\\bf{T}_e(t)} &= \\bf{E}_1(\\eta_1) \\ \\bf{E}_2(\\eta_2) \\ \\cdot\\cdot\\cdot \\ \\bf{E}_M(\\eta_M) \\\\\n", 468 | " &= \\prod_{i=1}^{M} \\bf{E}_i(\\eta_i).\n", 469 | "\\end{align*}\n" 470 | ] 471 | }, 472 | { 473 | "cell_type": "code", 474 | "execution_count": 11, 475 | "metadata": {}, 476 | "outputs": [ 477 | { 478 | "name": "stdout", 479 | "output_type": "stream", 480 | "text": [ 481 | " \u001b[38;5;1m 0.7003 \u001b[0m \u001b[38;5;1m-0.7068 \u001b[0m \u001b[38;5;1m 0.09983 \u001b[0m \u001b[38;5;4m 0.4737 \u001b[0m \u001b[0m\n", 482 | " \u001b[38;5;1m-0.7104 \u001b[0m \u001b[38;5;1m-0.7038 \u001b[0m \u001b[38;5;1m 0 \u001b[0m \u001b[38;5;4m 0 \u001b[0m \u001b[0m\n", 483 | " \u001b[38;5;1m 0.07027 \u001b[0m \u001b[38;5;1m-0.07092 \u001b[0m \u001b[38;5;1m-0.995 \u001b[0m \u001b[38;5;4m 0.5155 \u001b[0m \u001b[0m\n", 484 | " \u001b[38;5;244m 0 \u001b[0m \u001b[38;5;244m 0 \u001b[0m \u001b[38;5;244m 0 \u001b[0m \u001b[38;5;244m 1 \u001b[0m \u001b[0m\n", 485 | "\n" 486 | ] 487 | } 488 | ], 489 | "source": [ 490 | "# Using the above methodolgy, we can calculate the forward kinematics of our Panda model\n", 491 | "# First, we must define the joint coordinates q, to calculate the forward kinematics at\n", 492 | "q = np.array([0, -0.3, 0, -2.2, 0, 2, 0.79])\n", 493 | "\n", 494 | "# Allocate the resulting forward kinematics array\n", 495 | "fk = np.eye(4)\n", 496 | "\n", 497 | "# Now we must loop over the ETs in the Panda\n", 498 | "for et in panda:\n", 499 | " if et.isjoint:\n", 500 | " # This ET is a variable joint\n", 501 | " # Use the q array to specify the joint angle for the variable ET\n", 502 | " fk = fk @ et.A(q[et.jindex])\n", 503 | " else:\n", 504 | " # This ET is static\n", 505 | " fk = fk @ et.A()\n", 506 | "\n", 507 | "# Pretty print our resulting forward kinematics using an SE3 object\n", 508 | "print(sm.SE3(fk))" 509 | ] 510 | }, 511 | { 512 | "cell_type": "code", 513 | "execution_count": 12, 514 | "metadata": {}, 515 | "outputs": [ 516 | { 517 | "name": "stdout", 518 | "output_type": "stream", 519 | "text": [ 520 | "The fkine method: \n", 521 | " \u001b[38;5;1m 0.7003 \u001b[0m \u001b[38;5;1m-0.7068 \u001b[0m \u001b[38;5;1m 0.09983 \u001b[0m \u001b[38;5;4m 0.4737 \u001b[0m \u001b[0m\n", 522 | " \u001b[38;5;1m-0.7104 \u001b[0m \u001b[38;5;1m-0.7038 \u001b[0m \u001b[38;5;1m 0 \u001b[0m \u001b[38;5;4m 0 \u001b[0m \u001b[0m\n", 523 | " \u001b[38;5;1m 0.07027 \u001b[0m \u001b[38;5;1m-0.07092 \u001b[0m \u001b[38;5;1m-0.995 \u001b[0m \u001b[38;5;4m 0.5155 \u001b[0m \u001b[0m\n", 524 | " \u001b[38;5;244m 0 \u001b[0m \u001b[38;5;244m 0 \u001b[0m \u001b[38;5;244m 0 \u001b[0m \u001b[38;5;244m 1 \u001b[0m \u001b[0m\n", 525 | "\n", 526 | "The eval method: \n", 527 | "[[ 7.00329021e-01 -7.06804465e-01 9.98334166e-02 4.73724040e-01]\n", 528 | " [-7.10353272e-01 -7.03845316e-01 -1.22464680e-16 -1.31037208e-17]\n", 529 | " [ 7.02672827e-02 -7.09169942e-02 -9.95004165e-01 5.15513206e-01]\n", 530 | " [ 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00]]\n" 531 | ] 532 | } 533 | ], 534 | "source": [ 535 | "# The ETS class has the .fkine method which can calculate the forward kinematics\n", 536 | "# The .fkine methods returns an SE3 object\n", 537 | "print(f\"The fkine method: \\n{panda.fkine(q)}\")\n", 538 | "\n", 539 | "# The .eval method also calculates the forward kinematics but returns an numpy array\n", 540 | "# instead of an SE3 object (use this if speed is a priority)\n", 541 | "print(f\"The eval method: \\n{panda.eval(q)}\")" 542 | ] 543 | } 544 | ], 545 | "metadata": { 546 | "kernelspec": { 547 | "display_name": "rtb", 548 | "language": "python", 549 | "name": "python3" 550 | }, 551 | "language_info": { 552 | "codemirror_mode": { 553 | "name": "ipython", 554 | "version": 3 555 | }, 556 | "file_extension": ".py", 557 | "mimetype": "text/x-python", 558 | "name": "python", 559 | "nbconvert_exporter": "python", 560 | "pygments_lexer": "ipython3", 561 | "version": "3.11.0" 562 | }, 563 | "vscode": { 564 | "interpreter": { 565 | "hash": "4e8f2602a343ae62ff483b4df7805ab0eb447ccc1a56caf0631d5620d36b21ab" 566 | } 567 | } 568 | }, 569 | "nbformat": 4, 570 | "nbformat_minor": 2 571 | } 572 | -------------------------------------------------------------------------------- /Part 1/2 The Manipulator Jacobian.ipynb: -------------------------------------------------------------------------------- 1 | { 2 | "cells": [ 3 | { 4 | "cell_type": "code", 5 | "execution_count": 1, 6 | "metadata": {}, 7 | "outputs": [], 8 | "source": [ 9 | "try:\n", 10 | " # We must install required packages if we are in Google Colab\n", 11 | " import google.colab\n", 12 | " %pip install roboticstoolbox-python>=1.0.2\n", 13 | "except:\n", 14 | " # We are not in Google Colab\n", 15 | " # Apply custon style to notebook\n", 16 | " from IPython.core.display import HTML\n", 17 | " import pathlib\n", 18 | " styles_path = pathlib.Path(pathlib.Path().absolute(), \"style\", \"style.css\")\n", 19 | " styles = open(styles_path, \"r\").read()\n", 20 | " HTML(f\"\")" 21 | ] 22 | }, 23 | { 24 | "attachments": {}, 25 | "cell_type": "markdown", 26 | "metadata": {}, 27 | "source": [ 28 | "# 2.0 The Manipulator Jacobian\n", 29 | "\n", 30 | "\n", 31 | "\n", 32 | "\n", 33 | "$\\large{\\text{Manipulator Differential Kinematics}} \\\\ \\large{\\text{Part I: Kinematics, Velocity, and Applications}}$\n", 34 | "\n", 35 | "$\\text{By Jesse Haviland and Peter Corke}$\n", 36 | "\n", 37 | "
\n", 38 | "\n", 39 | "The sections of the Tutorial paper related to this notebook are listed next to each contents entry.\n", 40 | "It is beneficial to read these sections of the paper before attempting the notebook Section.\n", 41 | "\n", 42 | "### Contents\n", 43 | "\n", 44 | "[2.1 Utility Functions](#utility)\n", 45 | "\n", 46 | "[2.2 Derivative of a Pure Rotation](#dR)\n", 47 | "* Deriving the Manipulator Jacobian\n", 48 | "* First Derivative of an Elementary Transform\n", 49 | " * Derivative of a Pure Rotation\n", 50 | "\n", 51 | "[2.3 Derivative of a Pure Translation](#dt)\n", 52 | "* Deriving the Manipulator Jacobian\n", 53 | "* First Derivative of an Elementary Transform\n", 54 | " * Derivative of a Pure Translation\n", 55 | "\n", 56 | "[2.4 The Manipulator Jacobian](#J)\n", 57 | "* The Manipulator Jacobian\n", 58 | "\n", 59 | "[2.5 Fast Manipulator Jacobian](#Jf)\n", 60 | "* Fast Manipulator Jacobian\n", 61 | "\n", 62 | "[2.6 Changing Jacobian Frame](#frame)\n", 63 | "* Deriving the Manipulator Jacobian\n", 64 | "\n", 65 | "[2.7 Robotics Toolbox Jacobians](#rtb)\n", 66 | "\n", 67 | "[2.8 Speed Analysis](#fast)\n" 68 | ] 69 | }, 70 | { 71 | "cell_type": "code", 72 | "execution_count": 2, 73 | "metadata": {}, 74 | "outputs": [], 75 | "source": [ 76 | "# We will do the imports required for this notebook here\n", 77 | "\n", 78 | "# numpy provides import array and linear algebra utilities\n", 79 | "import numpy as np\n", 80 | "\n", 81 | "# the robotics toolbox provides robotics specific functionality\n", 82 | "import roboticstoolbox as rtb\n", 83 | "\n", 84 | "# spatial math provides objects for representing transformations\n", 85 | "import spatialmath as sm\n", 86 | "import spatialmath.base as smb\n", 87 | "\n", 88 | "# use timeit to benchmark some methods\n", 89 | "from timeit import default_timer as timer\n", 90 | "\n", 91 | "# ansitable is a great package for printing tables in a terminal\n", 92 | "from ansitable import ANSITable" 93 | ] 94 | }, 95 | { 96 | "attachments": {}, 97 | "cell_type": "markdown", 98 | "metadata": {}, 99 | "source": [ 100 | "
\n", 101 | "\n", 102 | "\n", 103 | "\n", 104 | "### 2.1 Utility Functions\n", 105 | "---" 106 | ] 107 | }, 108 | { 109 | "attachments": {}, 110 | "cell_type": "markdown", 111 | "metadata": {}, 112 | "source": [ 113 | "\n", 114 | "In this section we will use operations on skew symmetric and augmented skew-symmetric matrices\n", 115 | "\n", 116 | "\"drawing\"\n", 117 | "\n", 118 | "_Shown on the left is a vector $\\bf{s}\\in \\mathbb{R}^3$ along with its corresponding skew symmetric matrix $\\bf{S} \\in \\bf{so}(3) \\subset \\mathbb{R}^{3 \\times 3}$. Shown on the right is a vector $\\bf{\\hat{s}}\\in \\mathbb{R}^6$ along with its corresponding augmented skew symmetric matrix $\\bf{\\hat{S}} \\in \\bf{se}(3) \\subset \\mathbb{R}^{4 \\times 4}$. The skew functions $[\\cdot]_\\times : \\mathbb{R}^3 \\mapsto \\bf{so}(3)$ maps a vector to a skew symmetric matrix, and $[\\cdot] : \\mathbb{R}^6 \\mapsto \\bf{se}(3)$ maps a vector to an augmented skew symmetric matrix. The inverse skew functions $\\mathsf{V}_\\times(\\cdot) : \\bf{so}(3) \\mapsto \\mathbb{R}^3$ maps a skew symmetric matrix to a vector and $\\mathsf{V}(\\cdot) : \\bf{se}(3) \\mapsto \\mathbb{R}^6$ maps an augmented skew symmetric matrix to a vector._\n", 119 | "\n", 120 | "\n", 121 | "\n" 122 | ] 123 | }, 124 | { 125 | "cell_type": "code", 126 | "execution_count": 3, 127 | "metadata": {}, 128 | "outputs": [], 129 | "source": [ 130 | "\n", 131 | "# We can program these in Python\n", 132 | "# These methods assume the inputs are correctly sized and numpy arrays\n", 133 | "\n", 134 | "def vex(mat):\n", 135 | " '''\n", 136 | " Converts a 3x3 skew symmetric matric to a 3 vector\n", 137 | " '''\n", 138 | "\n", 139 | " return np.array([mat[2, 1], mat[0, 2], mat[1, 0]])\n", 140 | "\n", 141 | "def skew(vec):\n", 142 | " '''\n", 143 | " Converts a 3 vector to a 3x3 skew symmetric matrix\n", 144 | " '''\n", 145 | "\n", 146 | " return np.array([\n", 147 | " [0, -vec[2], vec[1]],\n", 148 | " [vec[2], 0, -vec[0]],\n", 149 | " [-vec[1], vec[0], 0]\n", 150 | " ])\n", 151 | "\n", 152 | "\n", 153 | "def vexa(mat):\n", 154 | " '''\n", 155 | " Converts a 4x4 augmented skew symmetric matric to a 6 vector\n", 156 | " '''\n", 157 | "\n", 158 | " return np.array([mat[0, 3], mat[1, 3], mat[2, 3], mat[2, 1], mat[0, 2], mat[1, 0]])\n", 159 | "\n", 160 | "def skewa(vec):\n", 161 | " '''\n", 162 | " Converts a 6 vector to a 4x4 augmented skew symmetric matrix\n", 163 | " '''\n", 164 | "\n", 165 | " return np.array([\n", 166 | " [0, -vec[5], vec[4], vec[0]],\n", 167 | " [vec[5], 0, -vec[3], vec[1]],\n", 168 | " [-vec[4], vec[3], 0, vec[2]],\n", 169 | " [0, 0, 0, 0]\n", 170 | " ])\n" 171 | ] 172 | }, 173 | { 174 | "cell_type": "code", 175 | "execution_count": 4, 176 | "metadata": {}, 177 | "outputs": [ 178 | { 179 | "name": "stdout", 180 | "output_type": "stream", 181 | "text": [ 182 | "x: \n", 183 | "[1 2 3]\n", 184 | "skew form of x: \n", 185 | "[[ 0 -3 2]\n", 186 | " [ 3 0 -1]\n", 187 | " [-2 1 0]]\n", 188 | "Perform vex operation on that, we should get the original a vector back: \n", 189 | "[1 2 3]\n" 190 | ] 191 | } 192 | ], 193 | "source": [ 194 | "# Test our skew and vex methods out\n", 195 | "\n", 196 | "x = np.array([1, 2, 3])\n", 197 | "\n", 198 | "sk_x = skew(x)\n", 199 | "\n", 200 | "print(f\"x: \\n{x}\")\n", 201 | "print(f\"skew form of x: \\n{sk_x}\")\n", 202 | "print(f\"Perform vex operation on that, we should get the original a vector back: \\n{vex(sk_x)}\")" 203 | ] 204 | }, 205 | { 206 | "cell_type": "code", 207 | "execution_count": 5, 208 | "metadata": {}, 209 | "outputs": [ 210 | { 211 | "name": "stdout", 212 | "output_type": "stream", 213 | "text": [ 214 | "y: \n", 215 | "[1 2 3 4 5 6]\n", 216 | "augmented skew form of y: \n", 217 | "[[ 0 -6 5 1]\n", 218 | " [ 6 0 -4 2]\n", 219 | " [-5 4 0 3]\n", 220 | " [ 0 0 0 0]]\n", 221 | "Perform vexa operation on that, we should get the original y vector back: \n", 222 | "[1 2 3 4 5 6]\n" 223 | ] 224 | } 225 | ], 226 | "source": [ 227 | "y = np.array([1, 2, 3, 4, 5, 6])\n", 228 | "\n", 229 | "ska_y = skewa(y)\n", 230 | "\n", 231 | "print(f\"y: \\n{y}\")\n", 232 | "print(f\"augmented skew form of y: \\n{ska_y}\")\n", 233 | "print(f\"Perform vexa operation on that, we should get the original y vector back: \\n{vexa(ska_y)}\")" 234 | ] 235 | }, 236 | { 237 | "attachments": {}, 238 | "cell_type": "markdown", 239 | "metadata": {}, 240 | "source": [ 241 | "\n", 242 | "We also need a function to extract the translational or rotational component from an $\\bf{SE}(3)$ or augmented skew-symmetric matrix\n", 243 | "\n", 244 | "
\n", 245 | "\n", 246 | "
\n", 247 | "\n", 248 | "\"drawing\"\n", 249 | "\n", 250 | "\n", 251 | "_Visualisation of a homogeneous transformation matrix (the derivatives share the form of $\\ \\bf{T}$, except will have a 0 instead of a 1 located at $\\bf{T}_{44}$). Where the matrix $\\rho(\\bf{T}) \\in \\mathbb{R}^{3 \\times 3}$ of green boxes forms the rotation component, and the vector $\\tau(\\bf{T}) \\in \\mathbb{R}^{3}$ of blue boxes form the translation component. The rotation component can be extracted through the function $\\rho(\\cdot) : \\mathbb{R}^{4\\times 4} \\mapsto \\mathbb{R}^{3\\times 3}$, while the translation component can be extracted through the function $\\tau(\\cdot) : \\mathbb{R}^{4\\times 4} \\mapsto \\mathbb{R}^{3}$._\n" 252 | ] 253 | }, 254 | { 255 | "cell_type": "code", 256 | "execution_count": 6, 257 | "metadata": {}, 258 | "outputs": [], 259 | "source": [ 260 | "def ρ(tf):\n", 261 | " '''\n", 262 | " The method extracts the rotational component from an SE3\n", 263 | " '''\n", 264 | " return tf[:3, :3]\n", 265 | "\n", 266 | "def τ(tf):\n", 267 | " '''\n", 268 | " The method extracts the translation component from an SE3\n", 269 | " '''\n", 270 | " return tf[:3, 3]" 271 | ] 272 | }, 273 | { 274 | "attachments": {}, 275 | "cell_type": "markdown", 276 | "metadata": {}, 277 | "source": [ 278 | "
\n", 279 | "\n", 280 | "\n", 281 | "### 2.2 Derivative of a Pure Rotation\n", 282 | "---\n" 283 | ] 284 | }, 285 | { 286 | "attachments": {}, 287 | "cell_type": "markdown", 288 | "metadata": {}, 289 | "source": [ 290 | "\n", 291 | "The derivative of a rotation matrix with respect to the rotation angle $\\theta$ is required when considering a revolute joint and\n", 292 | "can be shown to be\n", 293 | "\n", 294 | "\\begin{align*}\n", 295 | " \\frac{\\mathrm{d}\\bf{R}(\\theta)}\n", 296 | " {\\mathrm{d} \\theta} \n", 297 | " &= {[\\bf{\\hat{\\omega}}]_\\times} \\ \\bf{R} \\big( \\theta(t) \\big)\n", 298 | "\\end{align*}\n", 299 | "\n", 300 | "where the unit vector $\\bf{\\hat{\\omega}}$ is the joint rotation axis.\n", 301 | "\n", 302 | "The rotation axis $\\bf{\\hat{\\omega}}$ can be recovered using the inverse skew operator\n", 303 | "\n", 304 | "\\begin{align*}\n", 305 | " \\bf{\\hat{\\omega}} &=\n", 306 | " \\mathsf{V}_\\times\n", 307 | " \\left(\n", 308 | " \\frac{\\mathrm{d} \\bf{R}(\\theta)}\n", 309 | " {\\mathrm{d} \\theta}\n", 310 | " \\bf{R} \\big( \\theta(t)\\big)^\\top\n", 311 | " \\right)\n", 312 | "\\end{align*}\n", 313 | "\n", 314 | "since $ \\bf{R} \\in \\bf{SO}(3)$, then $\\bf{R}^{-1} = \\bf{R}^\\top$.\n", 315 | "\n", 316 | "For an ETS, we only need to consider the elementary rotations $\\bf{R}_x$, $\\bf{R}_y$, and $\\bf{R}_z$ which are embedded within $\\bf{SE}(3)$, as $\\bf{T}_{R_{x}}$, $\\bf{T}_{R_{y}}$, and $\\bf{T}_{R_{z}}$ i.e. pure rotations with no translational component. We can show that the derivative of each elementary rotation with respect to a rotation angle is\n", 317 | "\n", 318 | "\\begin{align*}\n", 319 | " \\dfrac{\\mathrm{d} \\bf{T}_{R_{x}}(\\theta)}\n", 320 | " {\\mathrm{d} \\theta}\n", 321 | " &= \n", 322 | " \\begin{pmatrix}\n", 323 | " 0 & 0 & 0 & 0 \\\\\n", 324 | " 0 & 0 & -1 & 0 \\\\\n", 325 | " 0 & 1 & 0 & 0 \\\\\n", 326 | " 0 & 0 & 0 & 0 \n", 327 | " \\end{pmatrix} \\bf{T}_{R_{x}}(\\theta) = \\big[ \\bf{\\hat{R}}_x \\big] \\bf{T}_{R_{x}}(\\theta) \\\\\n", 328 | " \\dfrac{\\mathrm{d} \\bf{T}_{R_{y}}(\\theta)}\n", 329 | " {\\mathrm{d} \\theta}\n", 330 | " &= \n", 331 | " \\begin{pmatrix}\n", 332 | " 0 & 0 & 1 & 0 \\\\\n", 333 | " 0 & 0 & 0 & 0 \\\\\n", 334 | " -1 & 0 & 0 & 0 \\\\\n", 335 | " 0 & 0 & 0 & 0 \n", 336 | " \\end{pmatrix} \\bf{T}_{R_{y}}(\\theta) = \\big[ \\bf{\\hat{R}}_y \\big] \\bf{T}_{R_{y}}(\\theta) \\\\\n", 337 | " \\dfrac{\\mathrm{d} \\bf{T}_{R_{z}}(\\theta)}\n", 338 | " {\\mathrm{d} \\theta}\n", 339 | " &= \n", 340 | " \\begin{pmatrix}\n", 341 | " 0 & -1 & 0 & 0 \\\\\n", 342 | " 1 & 0 & 0 & 0 \\\\\n", 343 | " 0 & 0 & 0 & 0 \\\\\n", 344 | " 0 & 0 & 0 & 0 \n", 345 | " \\end{pmatrix} \\bf{T}_{R_{z}}(\\theta) = \\big[ \\bf{\\hat{R}}_z \\big] \\bf{T}_{R_{z}}(\\theta)\n", 346 | "\\end{align*}\n", 347 | "\n", 348 | "where each of the augmented skew-symmetric matrices $[\\bf{\\hat{R}} ]$ above corresponds to one of the generators of $\\bf{SE}(3)$ which lies in $\\bf{se}(3)$, the tangent space of $\\bf{SE}(3)$.\n", 349 | "If a joints defined positive rotation is a negative rotation about the axis, , then $\\bf{T}_{\\bf{R}_i}(\\theta)^\\top$ is used to calculate the derivative.\n" 350 | ] 351 | }, 352 | { 353 | "cell_type": "code", 354 | "execution_count": 7, 355 | "metadata": {}, 356 | "outputs": [], 357 | "source": [ 358 | "# We can make Python functions which perform these derivatives\n", 359 | "\n", 360 | "def dTRx(θ, flip):\n", 361 | " '''\n", 362 | " Calculates the derivative of an SE3 which is a pure rotation around\n", 363 | " the x-axis by amount θ\n", 364 | " '''\n", 365 | " # This is the [Rhat_x] matrix in the maths above\n", 366 | " Rhx = np.array([\n", 367 | " [0, 0, 0, 0],\n", 368 | " [0, 0, -1, 0],\n", 369 | " [0, 1, 0, 0],\n", 370 | " [0, 0, 0, 0]\n", 371 | " ])\n", 372 | "\n", 373 | " # This is the T_R_x matrix in the maths above\n", 374 | " Trx = smb.trotx(θ)\n", 375 | "\n", 376 | " if flip:\n", 377 | " return Rhx.T @ Trx.T\n", 378 | " else:\n", 379 | " return Rhx @ Trx\n", 380 | "\n", 381 | "def dTRy(θ, flip):\n", 382 | " '''\n", 383 | " Calculates the derivative of an SE3 which is a pure rotation around\n", 384 | " the y-axis by amount θ\n", 385 | " '''\n", 386 | " # This is the [Rhat_y] matrix in the maths above\n", 387 | " Rhy = np.array([\n", 388 | " [0, 0, 1, 0],\n", 389 | " [0, 0, 0, 0],\n", 390 | " [-1, 0, 0, 0],\n", 391 | " [0, 0, 0, 0]\n", 392 | " ])\n", 393 | "\n", 394 | " # This is the T_R_y matrix in the maths above\n", 395 | " Try = smb.troty(θ)\n", 396 | "\n", 397 | " if flip:\n", 398 | " return Rhy.T @ Try.T\n", 399 | " else:\n", 400 | " return Rhy @ Try\n", 401 | "\n", 402 | "def dTRz(θ, flip):\n", 403 | " '''\n", 404 | " Calculates the derivative of an SE3 which is a pure rotation around\n", 405 | " the z-axis by amount θ\n", 406 | " '''\n", 407 | " # This is the [Rhat_z] matrix in the maths above\n", 408 | " Rhz = np.array([\n", 409 | " [0, -1, 0, 0],\n", 410 | " [1, 0, 0, 0],\n", 411 | " [0, 0, 0, 0],\n", 412 | " [0, 0, 0, 0]\n", 413 | " ])\n", 414 | "\n", 415 | " # This is the T_R_z matrix in the maths above\n", 416 | " Trz = smb.trotz(θ)\n", 417 | "\n", 418 | " if flip:\n", 419 | " return Rhz.T @ Trz.T\n", 420 | " else:\n", 421 | " return Rhz @ Trz\n" 422 | ] 423 | }, 424 | { 425 | "attachments": {}, 426 | "cell_type": "markdown", 427 | "metadata": {}, 428 | "source": [ 429 | "
\n", 430 | "\n", 431 | "\n", 432 | "### 2.3 Derivative of a Pure Translation\n", 433 | "---" 434 | ] 435 | }, 436 | { 437 | "attachments": {}, 438 | "cell_type": "markdown", 439 | "metadata": {}, 440 | "source": [ 441 | "\n", 442 | "The derivative of a homogeneous transformation matrix with respect to translation is required when considering a prismatic joint. For an ETS, these translations are embedded in $\\bf{SE}(3)$ as $\\bf{T}_{t_x}$, $\\bf{T}_{t_y}$, and $\\bf{T}_{t_z}$ which are pure translations with zero rotational component. We can show that the derivative of each elementary translation with respect to a translation is \n", 443 | "\n", 444 | "\\begin{align*}\n", 445 | " \\dfrac{\\mathrm{d} \\bf{T}_{t_x}(d)}\n", 446 | " {\\mathrm{d} d}\n", 447 | " &= \n", 448 | " \\begin{pmatrix}\n", 449 | " 0 & 0 & 0 & 1 \\\\\n", 450 | " 0 & 0 & 0 & 0 \\\\\n", 451 | " 0 & 0 & 0 & 0 \\\\\n", 452 | " 0 & 0 & 0 & 0 \n", 453 | " \\end{pmatrix} = [\\bf{\\hat{t}}_x], \\\\\n", 454 | " \\dfrac{\\mathrm{d} \\bf{T}_{t_y}(d)}\n", 455 | " {\\mathrm{d} d}\n", 456 | " &= \n", 457 | " \\begin{pmatrix}\n", 458 | " 0 & 0 & 0 & 0 \\\\\n", 459 | " 0 & 0 & 0 & 1 \\\\\n", 460 | " 0 & 0 & 0 & 0 \\\\\n", 461 | " 0 & 0 & 0 & 0 \n", 462 | " \\end{pmatrix} = [\\bf{\\hat{t}}_y], \\\\\n", 463 | " \\dfrac{\\mathrm{d} \\bf{T}_{t_z}(d)}\n", 464 | " {\\mathrm{d} d}\n", 465 | " &= \n", 466 | " \\begin{pmatrix}\n", 467 | " 0 & 0 & 0 & 0 \\\\\n", 468 | " 0 & 0 & 0 & 0 \\\\\n", 469 | " 0 & 0 & 0 & 1 \\\\\n", 470 | " 0 & 0 & 0 & 0 \n", 471 | " \\end{pmatrix} = [\\bf{\\hat{t}}_z],\n", 472 | "\\end{align*}\n", 473 | "\n", 474 | "where each of the augmented skew symmetric matrices $[ \\, \\bf{\\hat{t}} \\, ]$ above are the remaining three generators of $\\bf{SE}(3)$ which lie in $\\bf{se}(3)$.\n", 475 | "If a joints defined positive translation is a negative translation along the axis, \n", 476 | "then $-[ \\, \\bf{\\hat{t}} \\, ]$ should be used to calculate the derivative.\n" 477 | ] 478 | }, 479 | { 480 | "cell_type": "code", 481 | "execution_count": 8, 482 | "metadata": {}, 483 | "outputs": [], 484 | "source": [ 485 | "# We can make Python functions which perform these derivatives\n", 486 | "# Since these are constant, we don't need to handle joints being\n", 487 | "# flipped like we do with revolute joints\n", 488 | "\n", 489 | "def dTtx():\n", 490 | " '''\n", 491 | " Calculates the derivative of an SE3 which is a pure translation along\n", 492 | " the x-axis by amount d\n", 493 | " '''\n", 494 | " # This is the [That_x] matrix in the maths above\n", 495 | " Thx = np.array([\n", 496 | " [0, 0, 0, 1],\n", 497 | " [0, 0, 0, 0],\n", 498 | " [0, 0, 0, 0],\n", 499 | " [0, 0, 0, 0]\n", 500 | " ])\n", 501 | "\n", 502 | " return Thx\n", 503 | "\n", 504 | "def dTty():\n", 505 | " '''\n", 506 | " Calculates the derivative of an SE3 which is a pure translation along\n", 507 | " the y-axis by amount d\n", 508 | " '''\n", 509 | " # This is the [That_y] matrix in the maths above\n", 510 | " Thy = np.array([\n", 511 | " [0, 0, 0, 0],\n", 512 | " [0, 0, 0, 1],\n", 513 | " [0, 0, 0, 0],\n", 514 | " [0, 0, 0, 0]\n", 515 | " ])\n", 516 | "\n", 517 | " return Thy\n", 518 | "\n", 519 | "def dTtz():\n", 520 | " '''\n", 521 | " Calculates the derivative of an SE3 which is a pure translation along\n", 522 | " the z-axis by amount d\n", 523 | " '''\n", 524 | " # This is the [That_z] matrix in the maths above\n", 525 | " Thz = np.array([\n", 526 | " [0, 0, 0, 0],\n", 527 | " [0, 0, 0, 0],\n", 528 | " [0, 0, 0, 1],\n", 529 | " [0, 0, 0, 0]\n", 530 | " ])\n", 531 | "\n", 532 | " return Thz" 533 | ] 534 | }, 535 | { 536 | "attachments": {}, 537 | "cell_type": "markdown", 538 | "metadata": {}, 539 | "source": [ 540 | "
\n", 541 | "\n", 542 | "\n", 543 | "### 2.4 The Manipulator Jacobian\n", 544 | "---\n" 545 | ] 546 | }, 547 | { 548 | "cell_type": "code", 549 | "execution_count": 9, 550 | "metadata": {}, 551 | "outputs": [], 552 | "source": [ 553 | "# Before going any further, lets remake the panda model we made in the first notebook\n", 554 | "E1 = rtb.ET.tz(0.333) \n", 555 | "E2 = rtb.ET.Rz()\n", 556 | "E3 = rtb.ET.Ry() \n", 557 | "E4 = rtb.ET.tz(0.316) \n", 558 | "E5 = rtb.ET.Rz()\n", 559 | "E6 = rtb.ET.tx(0.0825) \n", 560 | "E7 = rtb.ET.Ry(flip=True) \n", 561 | "E8 = rtb.ET.tx(-0.0825) \n", 562 | "E9 = rtb.ET.tz(0.384) \n", 563 | "E10 = rtb.ET.Rz()\n", 564 | "E11 = rtb.ET.Ry(flip=True)\n", 565 | "E12 = rtb.ET.tx(0.088) \n", 566 | "E13 = rtb.ET.Rx(np.pi / 2) \n", 567 | "E14 = rtb.ET.tz(0.107)\n", 568 | "E15 = rtb.ET.Rz()\n", 569 | "panda = E1 * E2 * E3 * E4 * E5 * E6 * E7 * E8 * E9 * E10 * E11 * E12 * E13 * E14 * E15\n", 570 | "\n", 571 | "# And make a joint coordinate array q\n", 572 | "q = np.array([0, -0.3, 0, -2.2, 0, 2, 0.79])" 573 | ] 574 | }, 575 | { 576 | "attachments": {}, 577 | "cell_type": "markdown", 578 | "metadata": {}, 579 | "source": [ 580 | "\n", 581 | "Now we can calculate the derivative of an ETS. To find out how the $j^{th}$ joint affects the end-effector pose, apply the chain rule to (1 in paper)\n", 582 | "\n", 583 | "\\begin{align*}\n", 584 | " \\frac{\\partial \\bf{T}(\\bf{q})}\n", 585 | " {\\partial q_j} \n", 586 | " &=\n", 587 | " \\frac{\\partial} \n", 588 | " {\\partial q_j} \n", 589 | " \\left(\n", 590 | " \\bf{E}_1(\\eta_1) \\bf{E}_2(\\eta_2) \\cdot\\cdot\\cdot \\bf{E}_M(\\eta_M) \n", 591 | " \\right)\\\\\n", 592 | " &= \n", 593 | " \\prod_{i=1}^{\\mu(j)-1} \\!\\!\\!\\bf{E}_i(\\eta_i) \n", 594 | " \\ \\ \n", 595 | " \\frac{\\mathrm{d} \\bf{E}_{\\mu(j)}(q_j)} \n", 596 | " {\\mathrm{d} q_j}\n", 597 | " \\prod_{i=\\mu(j)+1}^{M} \\!\\!\\!\\!\\!\\!\\bf{E}_i(\\eta_i).\n", 598 | "\\end{align*}\n", 599 | "\n", 600 | "The derivative of the elementary transform with respect to a joint coordinate above is obtained using one of the functions in 2.2 for a revolute joint, or 2.3 for a prismatic joint.\n" 601 | ] 602 | }, 603 | { 604 | "cell_type": "code", 605 | "execution_count": 10, 606 | "metadata": {}, 607 | "outputs": [], 608 | "source": [ 609 | "\n", 610 | "def dET(et, η):\n", 611 | " '''\n", 612 | " This method takes an ET and returns the derivative with respect to the joint coordinate\n", 613 | " This is here for convenience\n", 614 | " '''\n", 615 | "\n", 616 | " if et.axis == 'Rx':\n", 617 | " return dTRx(η, et.isflip)\n", 618 | " elif et.axis == 'Ry':\n", 619 | " return dTRy(η, et.isflip)\n", 620 | " elif et.axis == 'Rz':\n", 621 | " return dTRz(η, et.isflip)\n", 622 | " elif et.axis == 'tx':\n", 623 | " return dTtx()\n", 624 | " elif et.axis == 'ty':\n", 625 | " return dTty()\n", 626 | " else:\n", 627 | " return dTtz()\n" 628 | ] 629 | }, 630 | { 631 | "cell_type": "code", 632 | "execution_count": 11, 633 | "metadata": {}, 634 | "outputs": [], 635 | "source": [ 636 | "def dT_j(ets, j, q):\n", 637 | " '''\n", 638 | " This methods calculates the dervative of an SE3 with respect to joint coordinate j\n", 639 | " '''\n", 640 | "\n", 641 | " # Allocate an identity matrix for the result\n", 642 | " dT = np.eye(4)\n", 643 | "\n", 644 | " # Find the jth variable et in the ets\n", 645 | " et_j = ets.joints()[j]\n", 646 | "\n", 647 | " # Loop overs the ETs in the robot ETS\n", 648 | " for et in ets:\n", 649 | " if et == et_j:\n", 650 | " # This ET is variable and corresponds to joint j\n", 651 | " dT = dT @ dET(et, q[et.jindex])\n", 652 | " elif et.isjoint:\n", 653 | " # This ET is a variable joint\n", 654 | " # Use the q array to specify the joint angle for the variable ET\n", 655 | " dT = dT @ et.A(q[et.jindex])\n", 656 | " else:\n", 657 | " # This ET is static\n", 658 | " dT = dT @ et.A()\n", 659 | "\n", 660 | " return dT" 661 | ] 662 | }, 663 | { 664 | "attachments": {}, 665 | "cell_type": "markdown", 666 | "metadata": {}, 667 | "source": [ 668 | "\n", 669 | "\n", 670 | "We can form the angular velocity component of the $j^{th}$ column of the manipulator Jacobian\n", 671 | "\n", 672 | "\\begin{align*}\n", 673 | " \\bf{J}_{\\omega_j}(\\bf{q}) \n", 674 | " &=\n", 675 | " \\mathsf{V}_\\times\n", 676 | " \\left(\n", 677 | " \\rho\n", 678 | " \\left(\n", 679 | " \\frac{\\partial \\bf{T}(\\bf{q})}\n", 680 | " {\\partial q_j} \n", 681 | " \\right)\n", 682 | " \\rho\n", 683 | " \\left(\n", 684 | " \\bf{T}(\\bf{q})\n", 685 | " \\right)^\\top\n", 686 | " \\right)\n", 687 | "\\end{align*}\n" 688 | ] 689 | }, 690 | { 691 | "cell_type": "code", 692 | "execution_count": 12, 693 | "metadata": {}, 694 | "outputs": [], 695 | "source": [ 696 | "def Jω_j(ets, j, q):\n", 697 | " '''\n", 698 | " This method calculates the rotational component of the jth column of the manipulator Jacobian\n", 699 | " '''\n", 700 | "\n", 701 | " # Calculate the forward kinematics at q\n", 702 | " T = ets.eval(q)\n", 703 | "\n", 704 | " # Calculate the derivative of T with respect to q_j\n", 705 | " dT = dT_j(ets, j, q)\n", 706 | "\n", 707 | " Jω = vex(ρ(dT) @ (ρ(T).T))\n", 708 | "\n", 709 | " return Jω\n", 710 | " " 711 | ] 712 | }, 713 | { 714 | "cell_type": "code", 715 | "execution_count": 13, 716 | "metadata": {}, 717 | "outputs": [ 718 | { 719 | "name": "stdout", 720 | "output_type": "stream", 721 | "text": [ 722 | "[-2.95520207e-01 -6.22199256e-33 9.55336489e-01]\n" 723 | ] 724 | } 725 | ], 726 | "source": [ 727 | "# Calculate the angular component of the third column of Panda manipulator Jacobian\n", 728 | "print(Jω_j(panda, 2, q))" 729 | ] 730 | }, 731 | { 732 | "attachments": {}, 733 | "cell_type": "markdown", 734 | "metadata": {}, 735 | "source": [ 736 | "\n", 737 | "We can form the translational velocity component of the $j^{th}$ column of the manipulator Jacobian using\n", 738 | "\n", 739 | "\\begin{align*}\n", 740 | " \\bf{J}_{v_j}(\\bf{q}) \n", 741 | " &=\n", 742 | " \\tau\n", 743 | " \\left(\n", 744 | " \\frac{\\partial \\bf{T}(\\bf{q})}\n", 745 | " {\\partial q_j} \n", 746 | " \\right).\n", 747 | "\\end{align*}\n" 748 | ] 749 | }, 750 | { 751 | "cell_type": "code", 752 | "execution_count": 14, 753 | "metadata": {}, 754 | "outputs": [], 755 | "source": [ 756 | "def Jv_j(ets, j, q):\n", 757 | " '''\n", 758 | " This method calculates the tranlation component of the jth column of the manipulator Jacobian\n", 759 | " '''\n", 760 | "\n", 761 | " # Calculate the derivative of T with respect to q_j\n", 762 | " dT = dT_j(ets, j, q)\n", 763 | "\n", 764 | " Jv = τ(dT)\n", 765 | "\n", 766 | " return Jv\n" 767 | ] 768 | }, 769 | { 770 | "cell_type": "code", 771 | "execution_count": 15, 772 | "metadata": {}, 773 | "outputs": [ 774 | { 775 | "name": "stdout", 776 | "output_type": "stream", 777 | "text": [ 778 | "[0.0372881 0. 0.47761099]\n" 779 | ] 780 | } 781 | ], 782 | "source": [ 783 | "# Calculate the translational component of the third column of Panda manipulator Jacobian\n", 784 | "print(Jv_j(panda, 3, q))" 785 | ] 786 | }, 787 | { 788 | "attachments": {}, 789 | "cell_type": "markdown", 790 | "metadata": {}, 791 | "source": [ 792 | "\n", 793 | "Stacking the translational and angular velocity components, the $j^{th}$ column of the manipulator Jacobian becomes \n", 794 | "\n", 795 | "\\begin{equation*}\n", 796 | " \\bf{J}_j(\\bf{q}) = \n", 797 | " \\begin{pmatrix} \n", 798 | " \\bf{J}_{v_j}(\\bf{q}) \\\\ \n", 799 | " \\bf{J}_{\\omega_j}(\\bf{q}) \n", 800 | " \\end{pmatrix} \\in \\mathbb{R}^{6}\n", 801 | "\\end{equation*}\n", 802 | "\n", 803 | "where the full manipulator Jacobian is \n", 804 | "\n", 805 | "\\begin{equation*}\n", 806 | " \\bf{J}(\\bf{q}) = \n", 807 | " \\begin{pmatrix} \n", 808 | " \\bf{J}_1(\\bf{q}) & \\cdots & \\bf{J}_n(\\bf{q})\n", 809 | " \\end{pmatrix} \\in \\mathbb{R}^{6 \\times n}.\n", 810 | "\\end{equation*}\n", 811 | "\n", 812 | "
\n", 813 | "\n", 814 | "
\n", 815 | "\n", 816 | "\"drawing\"\n", 817 | "\n", 818 | "_Visualisation of the Jacobian $\\bf{J}(\\bf{q})$ of a 7-jointed manipulator. Each column describes how the end-effector pose changes due to motion of the corresponding joint. The top three rows $\\bf{J}_v$ correspond to the linear velocity of the end-effector while the bottom three rows $\\bf{J}_\\omega$ correspond to the angular velocity of the end-effector._\n" 819 | ] 820 | }, 821 | { 822 | "cell_type": "code", 823 | "execution_count": 16, 824 | "metadata": {}, 825 | "outputs": [], 826 | "source": [ 827 | "# By using our previously defined methods, we can now calculate the manipulator Jacobian\n", 828 | "\n", 829 | "def jacob0(ets, q):\n", 830 | " '''\n", 831 | " This method calculates the manipulator Jacobian in the world frame\n", 832 | " '''\n", 833 | "\n", 834 | " # Allocate array for the Jacobian\n", 835 | " # It is a 6xn matrix\n", 836 | " J = np.zeros((6, ets.n))\n", 837 | "\n", 838 | " for j in range(ets.n):\n", 839 | " Jv = Jv_j(ets, j, q)\n", 840 | " Jω = Jω_j(ets, j, q)\n", 841 | "\n", 842 | " J[:3, j] = Jv\n", 843 | " J[3:, j] = Jω\n", 844 | "\n", 845 | " return J" 846 | ] 847 | }, 848 | { 849 | "cell_type": "code", 850 | "execution_count": 17, 851 | "metadata": {}, 852 | "outputs": [ 853 | { 854 | "name": "stdout", 855 | "output_type": "stream", 856 | "text": [ 857 | "The manipulator Jacobian (world-frame) \n", 858 | "in configuration q = [ 0. -0.3 0. -2.2 0. 2. 0.79] \n", 859 | "is: \n", 860 | "[[ 0.11 0.29 0.1 0.04 -0.03 -0.01 0. ]\n", 861 | " [ 0.46 0. 0.53 0. -0.04 0. 0. ]\n", 862 | " [ 0. -0.46 0.03 0.48 -0.1 0.09 0. ]\n", 863 | " [ 0. -0. -0.3 0. 0.95 0. -0. ]\n", 864 | " [ 0. 1. -0. -1. -0. -1. -1. ]\n", 865 | " [ 1. 0. 0.96 0. -0.32 0. 0. ]]\n" 866 | ] 867 | } 868 | ], 869 | "source": [ 870 | "# Calculate the manipulator Jacobian of the Panda in the world frame\n", 871 | "J = jacob0(panda, q)\n", 872 | "\n", 873 | "print(f\"The manipulator Jacobian (world-frame) \\nin configuration q = {q} \\nis: \\n{np.round(J, 2)}\")" 874 | ] 875 | }, 876 | { 877 | "attachments": {}, 878 | "cell_type": "markdown", 879 | "metadata": {}, 880 | "source": [ 881 | "
\n", 882 | " \n", 883 | "\n", 884 | "### 2.5 Fast Manipulator Jacobian\n", 885 | "---" 886 | ] 887 | }, 888 | { 889 | "attachments": {}, 890 | "cell_type": "markdown", 891 | "metadata": {}, 892 | "source": [ 893 | "\n", 894 | "\"drawing\"\n", 895 | "\n", 896 | "_The two vector representation of a rotation matrix $\\R \\in \\bf{SO}(3)$. The rotation matrix $\\mathbb{R}$ describes the coordinate frame in terms of three orthogonal vectors $\\bf{\\hat{n}}$, $\\bf{\\hat{o}}$, and $\\bf{\\hat{a}}$ which are the axes of the rotated frame expressed in the reference coordinate frame $\\bf{\\hat{x}}$, $\\bf{\\hat{y}}$, and $\\bf{\\hat{z}}$. As shown above, each of the vectors $\\bf{\\hat{n}}$, $\\bf{\\hat{o}}$, and $\\bf{\\hat{a}}$ can be calculated using the cross product of the other two._\n" 897 | ] 898 | }, 899 | { 900 | "attachments": {}, 901 | "cell_type": "markdown", 902 | "metadata": {}, 903 | "source": [ 904 | "\n", 905 | "Calculating the manipulator Jacobian using the method in 2.4 is easy to understand, but has $\\mathcal{O}(n^2)$ time complexity -- we can do better.\n", 906 | "\n", 907 | "\n", 908 | "Expanding the above equations (26) and (27), and simplifying using $\\bf{R} \\bf{R}^\\top = \\bf{1}$ gives\n", 909 | "\n", 910 | "\\begin{align*}\n", 911 | " \\bf{J}_{\\omega_j}(\\bf{q}) \n", 912 | " &=\n", 913 | " \\mathsf{V}_\\times \\bigg(\n", 914 | " \\rho\n", 915 | " \\left(\n", 916 | " ^0\\bf{T}_j\n", 917 | " \\right)\n", 918 | " \\rho\n", 919 | " \\left(\n", 920 | " [\\bf{\\hat{G}}_{\\mu(j)}]\n", 921 | " \\right)\n", 922 | " \\left(\n", 923 | " \\rho(^0\\bf{T}_j)^\\top\n", 924 | " \\right)\n", 925 | " \\bigg)\n", 926 | "\\end{align*}\n", 927 | "\n", 928 | "where $^0\\bf{T}_j$ represents the transform from the base frame to joint $j$, and $[\\bf{\\hat{G}}_{\\mu(j)}]$ corresponds to one of the 6 generators from equations in 2.4 or (20 - 25) in the paper. \n", 929 | "\n", 930 | "In the case of a prismatic joint, $\\rho(\\bf{\\hat{G}}_{\\mu(j)})$ will be a $3 \\times 3$ matrix of zeros which results in zero angular velocity. In the case of a revolute joint, the angular velocity is parallel to the axis of joint rotation.\n", 931 | "\n", 932 | "\\begin{align*}\n", 933 | " \\bf{J}_{\\omega_j}(\\bf{q}) \n", 934 | " &=\n", 935 | " \\left\\{\n", 936 | " \\begin{matrix}\n", 937 | " \\bf{\\hat{n}}_j & \\text{if} \\ \\ \\bf{E}_m = \\bf{T}_{\\bf{R}_x}\\\\\n", 938 | " \\bf{\\hat{o}}_j & \\text{if} \\ \\ \\bf{E}_m = \\bf{T}_{\\bf{R}_y}\\\\\n", 939 | " \\bf{\\hat{a}}_j & \\text{if} \\ \\ \\bf{E}_m = \\bf{T}_{\\bf{R}_z}\\\\\n", 940 | " (0, \\ 0, \\ 0)^\\top & \\text{if} \\ \\ \\bf{E}_m = \\bf{T}_{t}\n", 941 | " \\end{matrix}\n", 942 | " \\right.\n", 943 | "\\end{align*}\n", 944 | "\n", 945 | "where $\\begin{pmatrix} \\bf{\\hat{n}}_j & \\bf{\\hat{o}}_j & \\bf{\\hat{a}}_j \\end{pmatrix} = \\rho(^0\\bf{T}_j)$ are the columns of a rotation matrix as shown in the Figure above.\n", 946 | "\n", 947 | "\n", 948 | "Expanding (28) using (26) provides\n", 949 | "\n", 950 | "\\begin{align*}\n", 951 | " \\bf{J}_{v_j}(\\bf{q})\n", 952 | " &=\n", 953 | " \\tau\n", 954 | " \\left(\n", 955 | " ^0\\bf{T}_j\n", 956 | " [\\bf{\\hat{G}}_{\\mu(j)}]\n", 957 | " {^j\\bf{T}}_{e}\n", 958 | " \\right)\n", 959 | "\\end{align*}\n", 960 | "\n", 961 | "which reduces to \n", 962 | "\n", 963 | "\\begin{align*}\n", 964 | " \\bf{J}_{v_j}(\\bf{q}) \n", 965 | " &=\n", 966 | " \\left\\{ \n", 967 | " \\begin{matrix}\n", 968 | " \\bf{\\hat{a}}_j y_e - \\bf{\\hat{o}}_j z_e & \\text{if} \\ \\ \\bf{E}_m = \\bf{T}_{\\bf{R}_x}\\\\\n", 969 | " \\bf{\\hat{n}}_j z_e - \\bf{\\hat{a}}_j x_e & \\text{if} \\ \\ \\bf{E}_m = \\bf{T}_{\\bf{R}_y}\\\\\n", 970 | " \\bf{\\hat{o}}_j x_e - \\bf{\\hat{y}}_j y_e & \\text{if} \\ \\ \\bf{E}_m = \\bf{T}_{\\bf{R}_z}\\\\\n", 971 | " \\bf{\\hat{n}}_j & \\text{if} \\ \\ \\bf{E}_m = \\bf{T}_{t_x}\\\\\n", 972 | " \\bf{\\hat{o}}_j & \\text{if} \\ \\ \\bf{E}_m = \\bf{T}_{t_y}\\\\\n", 973 | " \\bf{\\hat{a}}_j & \\text{if} \\ \\ \\bf{E}_m = \\bf{T}_{t_z}\\\\\n", 974 | " \\end{matrix}\n", 975 | " \\right.\n", 976 | "\\end{align*}\n", 977 | "\n", 978 | "where\n", 979 | "$\\begin{pmatrix}x_e & y_e & z_e \\end{pmatrix}^\\top = \\tau({^j\\bf{T}}_{e})$.\n", 980 | "\n", 981 | "This simplification reduces the time complexity of computation of the manipulator Jacobian to $\\mathcal{O}(n)$.\n" 982 | ] 983 | }, 984 | { 985 | "cell_type": "code", 986 | "execution_count": 18, 987 | "metadata": {}, 988 | "outputs": [], 989 | "source": [ 990 | "# Now making the efficient version in Python\n", 991 | "\n", 992 | "def jacob0_efficient(ets, q):\n", 993 | " '''\n", 994 | " This method calculates the manipulator Jacobian in the world frame using a more efficient method\n", 995 | " '''\n", 996 | " T = ets.eval(q)\n", 997 | "\n", 998 | " U = np.eye(4)\n", 999 | " j = 0\n", 1000 | " J = np.zeros((6, ets.n))\n", 1001 | "\n", 1002 | " for ets in ets:\n", 1003 | " jindex = ets.jindex\n", 1004 | "\n", 1005 | " if ets.isjoint:\n", 1006 | " U = U @ ets.A(q[jindex]) # type: ignore\n", 1007 | "\n", 1008 | " Tu = sm.SE3(U, check=False).inv().A @ T\n", 1009 | " n = U[:3, 0]\n", 1010 | " o = U[:3, 1]\n", 1011 | " a = U[:3, 2]\n", 1012 | " x = Tu[0, 3]\n", 1013 | " y = Tu[1, 3]\n", 1014 | " z = Tu[2, 3]\n", 1015 | "\n", 1016 | " if ets.isflip:\n", 1017 | " sign = -1.0\n", 1018 | " else:\n", 1019 | " sign = 1.0\n", 1020 | "\n", 1021 | " if ets.axis == \"Rz\":\n", 1022 | " J[:3, j] = sign * ((o * x) - (n * y))\n", 1023 | " J[3:, j] = sign * a\n", 1024 | "\n", 1025 | " elif ets.axis == \"Ry\":\n", 1026 | " J[:3, j] = sign * ((n * z) - (a * x))\n", 1027 | " J[3:, j] = sign * o\n", 1028 | "\n", 1029 | " elif ets.axis == \"Rx\":\n", 1030 | " J[:3, j] = sign * ((a * y) - (o * z))\n", 1031 | " J[3:, j] = sign * n\n", 1032 | "\n", 1033 | " elif ets.axis == \"tx\":\n", 1034 | " J[:3, j] = n\n", 1035 | " J[3:, j] = np.array([0, 0, 0])\n", 1036 | "\n", 1037 | " elif ets.axis == \"ty\":\n", 1038 | " J[:3, j] = o\n", 1039 | " J[3:, j] = np.array([0, 0, 0])\n", 1040 | "\n", 1041 | " elif ets.axis == \"tz\":\n", 1042 | " J[:3, j] = a\n", 1043 | " J[3:, j] = np.array([0, 0, 0])\n", 1044 | "\n", 1045 | " j += 1\n", 1046 | " else:\n", 1047 | " U = U @ ets.A()\n", 1048 | "\n", 1049 | " return J" 1050 | ] 1051 | }, 1052 | { 1053 | "cell_type": "code", 1054 | "execution_count": 19, 1055 | "metadata": {}, 1056 | "outputs": [ 1057 | { 1058 | "name": "stdout", 1059 | "output_type": "stream", 1060 | "text": [ 1061 | "The manipulator Jacobian (world-frame) \n", 1062 | "in configuration q = [ 0. -0.3 0. -2.2 0. 2. 0.79] \n", 1063 | "is: \n", 1064 | "[[ 0.11 0.29 0.1 0.04 -0.03 -0.01 -0. ]\n", 1065 | " [ 0.46 0. 0.53 -0. -0.04 -0. 0. ]\n", 1066 | " [ 0. -0.46 0.03 0.48 -0.1 0.09 0. ]\n", 1067 | " [ 0. 0. -0.3 -0. 0.95 -0. -0. ]\n", 1068 | " [ 0. 1. 0. -1. 0. -1. -1. ]\n", 1069 | " [ 1. 0. 0.96 -0. -0.32 -0. 0. ]]\n" 1070 | ] 1071 | } 1072 | ], 1073 | "source": [ 1074 | "J = jacob0_efficient(panda, q)\n", 1075 | "\n", 1076 | "print(f\"The manipulator Jacobian (world-frame) \\nin configuration q = {q} \\nis: \\n{np.round(J, 2)}\")" 1077 | ] 1078 | }, 1079 | { 1080 | "attachments": {}, 1081 | "cell_type": "markdown", 1082 | "metadata": {}, 1083 | "source": [ 1084 | "
\n", 1085 | "\n", 1086 | "\n", 1087 | "### 2.6 Changing Jacobian Frame\n", 1088 | "---" 1089 | ] 1090 | }, 1091 | { 1092 | "attachments": {}, 1093 | "cell_type": "markdown", 1094 | "metadata": {}, 1095 | "source": [ 1096 | "\n", 1097 | "We have been calculating the manipulator Jacobian in the world-frame\n", 1098 | "\n", 1099 | "\\begin{equation*}\n", 1100 | " {^0\\bf{J}}(\\bf{q}).\n", 1101 | "\\end{equation*}\n", 1102 | "\n", 1103 | "is the manipulator Jacobian matrix expressed in the world-coordinate frame.\n", 1104 | "\n", 1105 | "The Jacobian expressed in the end-effector frame is\n", 1106 | "\n", 1107 | "\\begin{equation*}\n", 1108 | " {^e\\bf{J}}(\\bf{q}) =\n", 1109 | " \\begin{pmatrix} \n", 1110 | " {{^0\\bf{R}}_e^\\top} & \\bf{0} \\\\ \n", 1111 | " \\bf{0} & {{^0\\bf{R}}_e^\\top}\n", 1112 | " \\end{pmatrix}\n", 1113 | " {^0\\bf{J}}(\\bf{q})\n", 1114 | "\\end{equation*}\n", 1115 | "\n", 1116 | "where ${^0\\bf{R}}_e$ is a rotation matrix representing the orientation of the end-effector in the world frame.\n", 1117 | "\n" 1118 | ] 1119 | }, 1120 | { 1121 | "cell_type": "code", 1122 | "execution_count": 20, 1123 | "metadata": {}, 1124 | "outputs": [], 1125 | "source": [ 1126 | "# Now making it in Python\n", 1127 | "\n", 1128 | "def jacobe(ets, q):\n", 1129 | " '''\n", 1130 | " This method calculates the manipulator Jacobian in the end-effector frame\n", 1131 | " '''\n", 1132 | "\n", 1133 | " # Calculate world-frame Jacobian\n", 1134 | " J0 = jacob0_efficient(ets, q)\n", 1135 | "\n", 1136 | " # Calculate the forward kineamtics\n", 1137 | " T = ets.eval(q)\n", 1138 | "\n", 1139 | " # Extract rotation from T and transpose\n", 1140 | " R = T[:3, :3].T\n", 1141 | "\n", 1142 | " # Make our velocity transform matrix\n", 1143 | " tr = np.zeros((6, 6))\n", 1144 | "\n", 1145 | " # Put the rotation in the top left and bottom right of the matrix\n", 1146 | " tr[:3, :3] = R\n", 1147 | " tr[3:, 3:] = R\n", 1148 | "\n", 1149 | " # Perform the matrix multiplication to calculate the Jacobian\n", 1150 | " # in the end-effector frame\n", 1151 | " Je = tr @ J0\n", 1152 | "\n", 1153 | " return Je\n", 1154 | " " 1155 | ] 1156 | }, 1157 | { 1158 | "cell_type": "code", 1159 | "execution_count": 21, 1160 | "metadata": {}, 1161 | "outputs": [ 1162 | { 1163 | "name": "stdout", 1164 | "output_type": "stream", 1165 | "text": [ 1166 | "The manipulator Jacobian (end-effector) \n", 1167 | "in configuration q = [ 0. -0.3 0. -2.2 0. 2. 0.79] \n", 1168 | "is: \n", 1169 | "[[ 0.07 -0.18 0.09 0.39 -0.1 0.06 0. ]\n", 1170 | " [-0.08 -0.52 -0.06 0.27 -0.04 0.06 0. ]\n", 1171 | " [-0.46 -0. -0.53 0. 0.04 0. 0. ]\n", 1172 | " [ 0.78 0. 0.56 -0. 0.34 -0. -0. ]\n", 1173 | " [ 0.63 0. 0.83 -0. -0.94 -0. 0. ]\n", 1174 | " [ 0. -1. 0. 1. -0. 1. 1. ]]\n" 1175 | ] 1176 | } 1177 | ], 1178 | "source": [ 1179 | "# Calculate the manipulator Jacobian of the Panda in the end-effector frame\n", 1180 | "J = jacobe(panda, q)\n", 1181 | "\n", 1182 | "print(f\"The manipulator Jacobian (end-effector) \\nin configuration q = {q} \\nis: \\n{np.round(J, 2)}\")" 1183 | ] 1184 | }, 1185 | { 1186 | "attachments": {}, 1187 | "cell_type": "markdown", 1188 | "metadata": {}, 1189 | "source": [ 1190 | "
\n", 1191 | "\n", 1192 | "\n", 1193 | "### 2.7 Robotics Toolbox Jacobians\n", 1194 | "---" 1195 | ] 1196 | }, 1197 | { 1198 | "attachments": {}, 1199 | "cell_type": "markdown", 1200 | "metadata": {}, 1201 | "source": [ 1202 | "\n", 1203 | "\n", 1204 | "The `ETS` class within the Robotics Toolbox supports many kinematic methods (such as the previously discussed `.eval` and `.fkine` methods).\n", 1205 | "\n", 1206 | "The `.jacob0` and `.jacobe` methods in the `ETS` class calculate the world and end-effector frame Jacobians respectively.\n" 1207 | ] 1208 | }, 1209 | { 1210 | "cell_type": "code", 1211 | "execution_count": 22, 1212 | "metadata": {}, 1213 | "outputs": [ 1214 | { 1215 | "name": "stdout", 1216 | "output_type": "stream", 1217 | "text": [ 1218 | "The manipulator Jacobian (world frame) is: \n", 1219 | "[[ 0.11 0.29 0.1 0.04 -0.03 -0.01 0. ]\n", 1220 | " [ 0.46 0. 0.53 0. -0.04 0. 0. ]\n", 1221 | " [-0. -0.46 0.03 0.48 -0.1 0.09 0. ]\n", 1222 | " [ 0. -0. -0.3 0. 0.95 0. -0. ]\n", 1223 | " [ 0. 1. 0. -1. 0. -1. -1. ]\n", 1224 | " [ 1. 0. 0.96 0. -0.32 0. 0. ]]\n", 1225 | "\n", 1226 | "The manipulator Jacobian (end-effector frame) is: \n", 1227 | "[[ 0.07 -0.18 0.09 0.39 -0.1 0.06 0. ]\n", 1228 | " [-0.08 -0.52 -0.06 0.27 -0.04 0.06 0. ]\n", 1229 | " [-0.46 -0. -0.53 0. 0.04 0. 0. ]\n", 1230 | " [ 0.78 0. 0.56 -0. 0.34 -0. 0. ]\n", 1231 | " [ 0.63 0. 0.83 -0. -0.94 -0. 0. ]\n", 1232 | " [ 0. -1. 0. 1. -0. 1. 1. ]]\n" 1233 | ] 1234 | } 1235 | ], 1236 | "source": [ 1237 | "# Note: The panda object which we have been using is an instance of the ETS class\n", 1238 | "\n", 1239 | "# Calculate the world frame Jacobian using the ETS class\n", 1240 | "J0 = panda.jacob0(q)\n", 1241 | "\n", 1242 | "# Calculate the end-effector frame Jacobian using the ETS class\n", 1243 | "Je = panda.jacobe(q)\n", 1244 | "\n", 1245 | "# View our Jacobians\n", 1246 | "print(f\"The manipulator Jacobian (world frame) is: \\n{np.round(J0, 2)}\")\n", 1247 | "print(f\"\\nThe manipulator Jacobian (end-effector frame) is: \\n{np.round(Je, 2)}\")" 1248 | ] 1249 | }, 1250 | { 1251 | "attachments": {}, 1252 | "cell_type": "markdown", 1253 | "metadata": {}, 1254 | "source": [ 1255 | "
\n", 1256 | "\n", 1257 | "\n", 1258 | "### 2.8 Speed Comparison\n", 1259 | "---" 1260 | ] 1261 | }, 1262 | { 1263 | "attachments": {}, 1264 | "cell_type": "markdown", 1265 | "metadata": {}, 1266 | "source": [ 1267 | "\n", 1268 | "\n", 1269 | "In this section we will briefly cover a speed comparison between the two `jacob0` methods shown here and the Jacobian implementation provided by the Robotics Toolbox.\n", 1270 | "\n", 1271 | "Kinematic methods in the toolbox are written using C extensions and wrapped by python methods. These C methods also have Python fallbacks to rely on if something goes wrong.\n" 1272 | ] 1273 | }, 1274 | { 1275 | "cell_type": "code", 1276 | "execution_count": 23, 1277 | "metadata": {}, 1278 | "outputs": [ 1279 | { 1280 | "name": "stdout", 1281 | "output_type": "stream", 1282 | "text": [ 1283 | "\n", 1284 | "Timing to Calculate Manipulator Jacobian over 1000 iterations\n", 1285 | "\n", 1286 | "┌─────────────────────┬─────────────────┬───────────────────┐\n", 1287 | "│ Function │ Total Time (ms) │ Average Time (us) │\n", 1288 | "├─────────────────────┼─────────────────┼───────────────────┤\n", 1289 | "│ our jacob0\u001b[0m │ 766.2759\u001b[0m │ 766.2759\u001b[0m │\n", 1290 | "│our jacob0_efficient\u001b[0m │ 84.0204\u001b[0m │ 84.0204\u001b[0m │\n", 1291 | "│ toolbox jacob0\u001b[0m │ 0.6925\u001b[0m │ 0.6925\u001b[0m │\n", 1292 | "└─────────────────────┴─────────────────┴───────────────────┘\n", 1293 | "\n" 1294 | ] 1295 | } 1296 | ], 1297 | "source": [ 1298 | "# Number of iterations to test over\n", 1299 | "n = 1000\n", 1300 | "\n", 1301 | "# Make a random matrix of q values\n", 1302 | "qt = np.random.random((n, 7))\n", 1303 | "\n", 1304 | "# Slow Jacobian implementation\n", 1305 | "start = timer()\n", 1306 | "for q in qt:\n", 1307 | " jacob0(panda, q)\n", 1308 | "slow_time = timer() - start\n", 1309 | "\n", 1310 | "# Improved Jacobian implementation\n", 1311 | "start = timer()\n", 1312 | "for q in qt:\n", 1313 | " jacob0_efficient(panda, q)\n", 1314 | "improved_time = timer() - start\n", 1315 | "\n", 1316 | "# Toolbox C improved Jacobian implementation\n", 1317 | "start = timer()\n", 1318 | "for q in qt:\n", 1319 | " panda.jacob0(q)\n", 1320 | "rtb_time = timer() - start\n", 1321 | "\n", 1322 | "# Make a table\n", 1323 | "print(f\"\\nTiming to Calculate Manipulator Jacobian over {n} iterations\\n\")\n", 1324 | "table = ANSITable(\n", 1325 | " \"Function\",\n", 1326 | " \"Total Time (ms)\",\n", 1327 | " \"Average Time (us)\",\n", 1328 | " border=\"thin\",\n", 1329 | ")\n", 1330 | "\n", 1331 | "table.row(\n", 1332 | " \"our jacob0\",\n", 1333 | " np.round(slow_time * 1000.0, 4),\n", 1334 | " np.round((slow_time / n) * 1000000.0, 4),\n", 1335 | ")\n", 1336 | "\n", 1337 | "table.row(\n", 1338 | " \"our jacob0_efficient\",\n", 1339 | " np.round(improved_time * 1000.0, 4),\n", 1340 | " np.round((improved_time / n) * 1000000.0, 4),\n", 1341 | ")\n", 1342 | "\n", 1343 | "table.row(\n", 1344 | " \"toolbox jacob0\",\n", 1345 | " np.round(rtb_time * 1000.0, 4),\n", 1346 | " np.round((rtb_time / n) * 1000000.0, 4),\n", 1347 | ")\n", 1348 | "table.print()\n" 1349 | ] 1350 | } 1351 | ], 1352 | "metadata": { 1353 | "kernelspec": { 1354 | "display_name": "rtb", 1355 | "language": "python", 1356 | "name": "python3" 1357 | }, 1358 | "language_info": { 1359 | "codemirror_mode": { 1360 | "name": "ipython", 1361 | "version": 3 1362 | }, 1363 | "file_extension": ".py", 1364 | "mimetype": "text/x-python", 1365 | "name": "python", 1366 | "nbconvert_exporter": "python", 1367 | "pygments_lexer": "ipython3", 1368 | "version": "3.11.0" 1369 | }, 1370 | "vscode": { 1371 | "interpreter": { 1372 | "hash": "4e8f2602a343ae62ff483b4df7805ab0eb447ccc1a56caf0631d5620d36b21ab" 1373 | } 1374 | } 1375 | }, 1376 | "nbformat": 4, 1377 | "nbformat_minor": 2 1378 | } 1379 | -------------------------------------------------------------------------------- /Part 1/5 Manipulator Performance Measures.ipynb: -------------------------------------------------------------------------------- 1 | { 2 | "cells": [ 3 | { 4 | "cell_type": "code", 5 | "execution_count": 1, 6 | "metadata": {}, 7 | "outputs": [], 8 | "source": [ 9 | "try:\n", 10 | " # We must install required packages if we are in Google Colab\n", 11 | " import google.colab\n", 12 | " %pip install roboticstoolbox-python>=1.0.2\n", 13 | "except:\n", 14 | " # We are not in Google Colab\n", 15 | " # Apply custon style to notebook\n", 16 | " from IPython.core.display import HTML\n", 17 | " import pathlib\n", 18 | " styles_path = pathlib.Path(pathlib.Path().absolute(), \"style\", \"style.css\")\n", 19 | " styles = open(styles_path, \"r\").read()\n", 20 | " HTML(f\"\")" 21 | ] 22 | }, 23 | { 24 | "attachments": {}, 25 | "cell_type": "markdown", 26 | "metadata": {}, 27 | "source": [ 28 | "# 5.0 Manipulator Performance Measures\n", 29 | "\n", 30 | "$\\large{\\text{Manipulator Differential Kinematics}} \\\\ \\large{\\text{Part I: Kinematics, Velocity, and Applications}}$\n", 31 | "\n", 32 | "$\\text{By Jesse Haviland and Peter Corke}$\n", 33 | "\n", 34 | "\n", 35 | "
\n", 36 | "\n", 37 | "The sections of the Tutorial paper related to this notebook are listed next to each contents entry.\n", 38 | "It is beneficial to read these sections of the paper before attempting the notebook Section.\n", 39 | "\n", 40 | "### Contents\n", 41 | "\n", 42 | "[5.1 Manipulability Measure](#m)\n", 43 | "* Manipulator Performance Metrics\n", 44 | "\n", 45 | "[5.2 Condition Number](#cond)\n", 46 | "* Manipulator Performance Metrics\n" 47 | ] 48 | }, 49 | { 50 | "cell_type": "code", 51 | "execution_count": 2, 52 | "metadata": {}, 53 | "outputs": [], 54 | "source": [ 55 | "# We will do the imports required for this notebook here\n", 56 | "\n", 57 | "# numpy provides import array and linear algebra utilities\n", 58 | "import numpy as np\n", 59 | "\n", 60 | "# the robotics toolbox provides robotics specific functionality\n", 61 | "import roboticstoolbox as rtb" 62 | ] 63 | }, 64 | { 65 | "attachments": {}, 66 | "cell_type": "markdown", 67 | "metadata": {}, 68 | "source": [ 69 | "Manipulator performance metrics seek to quantify the performance of a manipulator in a given configuration. In this section, we explore two common manipulator performance metrics based on the manipulator Jacobian.\n", 70 | "\n", 71 | "Things to note\n", 72 | "* The metrics are unitless, and the upper bound of a metric depends on the manipulator kinematic model (i.e. joint types and link lengths). Consequently, metrics computed for different manipulators are not directly comparable.\n", 73 | "* The manipulator Jacobian contains three rows corresponding to translational rates, and three rows corresponding to angular rates. Therefore, any metric using the whole Jacobian will produce a non-homogeneous result due to the mixed units. Depending on the manipulator scale, this can cause either the translational or rotational component to dominate the result. This problem also arises in manipulators with mixed prismatic and revolute joints. In general, the most intuitive use of performance metrics comes from using only the translational or rotational rows of the manipulator Jacobian (where the choice of which depends on the use case), and only using the metric on a manipulator comprising with a single joint type." 74 | ] 75 | }, 76 | { 77 | "attachments": {}, 78 | "cell_type": "markdown", 79 | "metadata": {}, 80 | "source": [ 81 | "
\n", 82 | "\n", 83 | "\n", 84 | "\n", 85 | "### 5.1 Manipulability Measure\n", 86 | "---" 87 | ] 88 | }, 89 | { 90 | "attachments": {}, 91 | "cell_type": "markdown", 92 | "metadata": {}, 93 | "source": [ 94 | "The Yoshikawa manipulability index is the most widely used and accepted performance metric. The index is calculated as\n", 95 | "\n", 96 | "\\begin{align*}\n", 97 | " m(\\bf{q}) = \\sqrt{\n", 98 | " \\text{det}\n", 99 | " \\Big(\n", 100 | " \\hat{\\bf{J}}(\\bf{q})\n", 101 | " \\hat{\\bf{J}}(\\bf{q})^\\top\n", 102 | " \\Big)\n", 103 | " }\n", 104 | "\\end{align*}\n", 105 | "\n", 106 | "where $\\hat{\\bf{J}}(\\bf{q}) \\in \\mathbb{R}^{3 \\times n}$ is either the translational or rotational rows of $\\bf{J}(\\bf{q})$ causing $m(\\bf{q})$ to describe the corresponding component of manipulability. \n", 107 | "Note that Yoshikawa used $\\bf{J}(\\bf{q})$ instead of $\\hat{\\bf{J}}(\\bf{q})$ in his original paper but we describe it so due to limitations of Jacobian based measures discussed above.\n", 108 | "The scalar $m(\\bf{q})$ describes the volume of a 3-dimensional ellipsoid -- if this ellipsoid is close to spherical, then the manipulator can achieve any arbitrary end-effector (translational or rotational depending on $\\hat{\\bf{J}}(\\vec{q})$) velocity. The ellipsoid is described by three radii aligned with its principal axes. A small radius indicates the robot's inability to achieve a velocity in the corresponding direction. At a singularity, the ellipsoid's radius becomes zero along the corresponding axis and the volume becomes zero. If the manipulator's configuration is well conditioned, these ellipsoids will have a larger volume. Therefore, the manipulability index is essentially a measure of how easily a manipulator can achieve an arbitrary velocity." 109 | ] 110 | }, 111 | { 112 | "attachments": {}, 113 | "cell_type": "markdown", 114 | "metadata": {}, 115 | "source": [ 116 | "We first make a method to calculate the manipulability of a robot using the above description." 117 | ] 118 | }, 119 | { 120 | "cell_type": "code", 121 | "execution_count": 3, 122 | "metadata": {}, 123 | "outputs": [], 124 | "source": [ 125 | "def manipulability(robot, q, axes):\n", 126 | " \"\"\"\n", 127 | " Calculates the manipulability of the robot at joint configuration q\n", 128 | "\n", 129 | " :robot: A Robot object to find the manipulability of\n", 130 | " :q: The joint coordinates of the robot (ndarray)\n", 131 | " :axes: A boolean list which correspond with the Cartesian axes to\n", 132 | " find the manipulability of (6 boolean values in a list)\n", 133 | " \"\"\"\n", 134 | " # calculate the Jacobian\n", 135 | " J = robot.jacobe(q)\n", 136 | "\n", 137 | " # only keep the selected axes of J\n", 138 | " J = J[axes, :]\n", 139 | "\n", 140 | " # calculate the manipulability\n", 141 | " m = np.sqrt(np.linalg.det(J @ J.T))\n", 142 | "\n", 143 | " return m" 144 | ] 145 | }, 146 | { 147 | "attachments": {}, 148 | "cell_type": "markdown", 149 | "metadata": {}, 150 | "source": [ 151 | "We make a robot and define some axes selections. The lists `trans_axes`, `rot_axes`, `all_axes` each correspond to a set of Cartesian axes." 152 | ] 153 | }, 154 | { 155 | "cell_type": "code", 156 | "execution_count": 4, 157 | "metadata": {}, 158 | "outputs": [], 159 | "source": [ 160 | "# Make a Panda\n", 161 | "panda = rtb.models.Panda()\n", 162 | "\n", 163 | "# Only the translation axes\n", 164 | "trans_axes = [True, True, True, False, False, False]\n", 165 | "\n", 166 | "# Only the rotation aces\n", 167 | "rot_axes = [False, False, False, True, True, True]\n", 168 | "\n", 169 | "# All axes\n", 170 | "all_axes = [True, True, True, True, True, True]" 171 | ] 172 | }, 173 | { 174 | "attachments": {}, 175 | "cell_type": "markdown", 176 | "metadata": {}, 177 | "source": [ 178 | "Lets test out our method using the Panda robot in the ready `panda.qr` configuration." 179 | ] 180 | }, 181 | { 182 | "cell_type": "code", 183 | "execution_count": 5, 184 | "metadata": {}, 185 | "outputs": [ 186 | { 187 | "name": "stdout", 188 | "output_type": "stream", 189 | "text": [ 190 | "Translational manipulability: 0.14384031993097537\n", 191 | "Rotational manipulability: 2.745582183072283\n", 192 | "All axes manipulability: 0.08375150968113343\n" 193 | ] 194 | } 195 | ], 196 | "source": [ 197 | "print(f\"Translational manipulability: {manipulability(panda, panda.qr, trans_axes)}\")\n", 198 | "print(f\"Rotational manipulability: {manipulability(panda, panda.qr, rot_axes)}\")\n", 199 | "print(f\"All axes manipulability: {manipulability(panda, panda.qr, all_axes)}\")" 200 | ] 201 | }, 202 | { 203 | "attachments": {}, 204 | "cell_type": "markdown", 205 | "metadata": {}, 206 | "source": [ 207 | "In the `panda.qr` configuration, the Panda is well configured. That is to say that it is in a manipulable configuration and can easily move in any direction.\n", 208 | "\n", 209 | "For the Panda, 0.14 is around the peak of the translational manipulability and 2.74 is in the upper range of the rotational manipulability. These two numbers are not comparable to each other due to being made from different units. We also can not compare these numbers to translational/rotational manipulabilities on other robots.\n", 210 | "\n", 211 | "We can also see that the rotational manipulability is an order of magnitude greater than the translational manipulability. This difference can cause unintuitive results in the all axes manipulability measure." 212 | ] 213 | }, 214 | { 215 | "attachments": {}, 216 | "cell_type": "markdown", 217 | "metadata": {}, 218 | "source": [ 219 | "In the next cell, we can see a test the manipulability of the Panda in a poorly configured pose `panda.qz`. This is the zero joint angle configuration in which the Panda is in a singularity." 220 | ] 221 | }, 222 | { 223 | "cell_type": "code", 224 | "execution_count": 6, 225 | "metadata": {}, 226 | "outputs": [ 227 | { 228 | "name": "stdout", 229 | "output_type": "stream", 230 | "text": [ 231 | "Translational manipulability: 0.009904977685365267\n", 232 | "Rotational manipulability: nan\n", 233 | "All axes manipulability: nan\n" 234 | ] 235 | }, 236 | { 237 | "name": "stderr", 238 | "output_type": "stream", 239 | "text": [ 240 | "/var/folders/jm/65py5sf558zb166cykvzz9zw0000gq/T/ipykernel_50710/2288985554.py:17: RuntimeWarning: invalid value encountered in sqrt\n", 241 | " m = np.sqrt(np.linalg.det(J @ J.T))\n" 242 | ] 243 | } 244 | ], 245 | "source": [ 246 | "print(f\"Translational manipulability: {manipulability(panda, panda.qz, trans_axes)}\")\n", 247 | "print(f\"Rotational manipulability: {manipulability(panda, panda.qz, rot_axes)}\")\n", 248 | "print(f\"All axes manipulability: {manipulability(panda, panda.qz, all_axes)}\")" 249 | ] 250 | }, 251 | { 252 | "attachments": {}, 253 | "cell_type": "markdown", 254 | "metadata": {}, 255 | "source": [ 256 | "
\n", 257 | "\n", 258 | "\n", 259 | "\n", 260 | "### 5.2 Condition Number\n", 261 | "---" 262 | ] 263 | }, 264 | { 265 | "attachments": {}, 266 | "cell_type": "markdown", 267 | "metadata": {}, 268 | "source": [ 269 | "The condition number of the manipulator Jacobian was proposed as a performance measure. The condition number is\n", 270 | "\n", 271 | "\\begin{align*}\n", 272 | " \\kappa =\n", 273 | " \\dfrac{\\sigma_{\\text{max}}}\n", 274 | " {\\sigma_{\\text{min}}}\n", 275 | " \\in [1, \\ \\infty ]\n", 276 | "\\end{align*}\n", 277 | "\n", 278 | "where $\\sigma_{\\text{max}}$ and $\\sigma_{\\text{min}}$ are the maximum and minimum singular values of $\\hat{\\bf{J}}(\\bf{q})$ respectively. The condition number is a measure of velocity isotropy. A condition number close to $1$ means that the manipulator can achieve a velocity in a direction equally as easily as any other direction. However, a high condition number does not guarantee a high manipulability index where the manipulator may struggle to move in all directions." 279 | ] 280 | }, 281 | { 282 | "attachments": {}, 283 | "cell_type": "markdown", 284 | "metadata": {}, 285 | "source": [ 286 | "We first make a method to calculate the condition number of a robot using the above description." 287 | ] 288 | }, 289 | { 290 | "cell_type": "code", 291 | "execution_count": 7, 292 | "metadata": {}, 293 | "outputs": [], 294 | "source": [ 295 | "def cond(robot, q, axes):\n", 296 | " \"\"\"\n", 297 | " Calculates the condition number of the robot's Jacobian at\n", 298 | " joint configuration q\n", 299 | "\n", 300 | " :robot: A Robot object to find the manipulability of\n", 301 | " :q: The joint coordinates of the robot (ndarray)\n", 302 | " :axes: A boolean list which correspond with the Cartesian axes to\n", 303 | " find the manipulability of (6 boolean values in a list)\n", 304 | " \"\"\"\n", 305 | " # calculate the Jacobian\n", 306 | " J = robot.jacobe(q)\n", 307 | "\n", 308 | " # only keep the selected axes of J\n", 309 | " J = J[axes, :]\n", 310 | "\n", 311 | " _, s, _ = np.linalg.svd(J)\n", 312 | "\n", 313 | " Jcond = np.max(s) / np.min(s)\n", 314 | "\n", 315 | " ## Note: we could just use the cond method from numpy\n", 316 | " # Jcond = np.linalg.cond(J)\n", 317 | "\n", 318 | " return Jcond" 319 | ] 320 | }, 321 | { 322 | "attachments": {}, 323 | "cell_type": "markdown", 324 | "metadata": {}, 325 | "source": [ 326 | "Lets test out our method using the Panda robot in the ready `panda.qr` configuration." 327 | ] 328 | }, 329 | { 330 | "cell_type": "code", 331 | "execution_count": 8, 332 | "metadata": {}, 333 | "outputs": [ 334 | { 335 | "name": "stdout", 336 | "output_type": "stream", 337 | "text": [ 338 | "Translational condition number: 2.5512647717700543\n", 339 | "Rotational condition number: 2.031042407741737\n", 340 | "All axes condition number: 8.910974536808437\n" 341 | ] 342 | } 343 | ], 344 | "source": [ 345 | "print(f\"Translational condition number: {cond(panda, panda.qr, trans_axes)}\")\n", 346 | "print(f\"Rotational condition number: {cond(panda, panda.qr, rot_axes)}\")\n", 347 | "print(f\"All axes condition number: {cond(panda, panda.qr, all_axes)}\")" 348 | ] 349 | }, 350 | { 351 | "attachments": {}, 352 | "cell_type": "markdown", 353 | "metadata": {}, 354 | "source": [ 355 | "Lets test out our method using the Panda robot in the ready `panda.qz` configuration." 356 | ] 357 | }, 358 | { 359 | "cell_type": "code", 360 | "execution_count": 9, 361 | "metadata": {}, 362 | "outputs": [ 363 | { 364 | "name": "stdout", 365 | "output_type": "stream", 366 | "text": [ 367 | "Translational condition number: 4.866993326664893\n", 368 | "Rotational condition number: 1.0400617828738614e+16\n", 369 | "All axes condition number: 6.968343580985044e+16\n" 370 | ] 371 | } 372 | ], 373 | "source": [ 374 | "print(f\"Translational condition number: {cond(panda, panda.qz, trans_axes)}\")\n", 375 | "print(f\"Rotational condition number: {cond(panda, panda.qz, rot_axes)}\")\n", 376 | "print(f\"All axes condition number: {cond(panda, panda.qz, all_axes)}\")" 377 | ] 378 | }, 379 | { 380 | "attachments": {}, 381 | "cell_type": "markdown", 382 | "metadata": {}, 383 | "source": [ 384 | "Remember that a condition number of 1 is perfect and infinity is the worst." 385 | ] 386 | } 387 | ], 388 | "metadata": { 389 | "interpreter": { 390 | "hash": "528815e074ebcdb9b34bcb695d4aa9d425bdb2cc6656b4ca45050b51a4125937" 391 | }, 392 | "kernelspec": { 393 | "display_name": "Python 3.8.10 ('rtb')", 394 | "language": "python", 395 | "name": "python3" 396 | }, 397 | "language_info": { 398 | "codemirror_mode": { 399 | "name": "ipython", 400 | "version": 3 401 | }, 402 | "file_extension": ".py", 403 | "mimetype": "text/x-python", 404 | "name": "python", 405 | "nbconvert_exporter": "python", 406 | "pygments_lexer": "ipython3", 407 | "version": "3.11.0" 408 | }, 409 | "orig_nbformat": 4 410 | }, 411 | "nbformat": 4, 412 | "nbformat_minor": 2 413 | } 414 | -------------------------------------------------------------------------------- /Part 1/img/cover.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jhavl/dkt/aef11b61881f15d20604572982ab88405b62d9e9/Part 1/img/cover.png -------------------------------------------------------------------------------- /Part 1/img/elementary_transforms_dark.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jhavl/dkt/aef11b61881f15d20604572982ab88405b62d9e9/Part 1/img/elementary_transforms_dark.png -------------------------------------------------------------------------------- /Part 1/img/hessian_dark.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jhavl/dkt/aef11b61881f15d20604572982ab88405b62d9e9/Part 1/img/hessian_dark.png -------------------------------------------------------------------------------- /Part 1/img/jacobian_dark.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jhavl/dkt/aef11b61881f15d20604572982ab88405b62d9e9/Part 1/img/jacobian_dark.png -------------------------------------------------------------------------------- /Part 1/img/rotation_dark.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jhavl/dkt/aef11b61881f15d20604572982ab88405b62d9e9/Part 1/img/rotation_dark.png -------------------------------------------------------------------------------- /Part 1/img/skew_dark.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jhavl/dkt/aef11b61881f15d20604572982ab88405b62d9e9/Part 1/img/skew_dark.png -------------------------------------------------------------------------------- /Part 1/img/title.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jhavl/dkt/aef11b61881f15d20604572982ab88405b62d9e9/Part 1/img/title.png -------------------------------------------------------------------------------- /Part 1/img/transform_dark.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jhavl/dkt/aef11b61881f15d20604572982ab88405b62d9e9/Part 1/img/transform_dark.png -------------------------------------------------------------------------------- /Part 1/style/style.css: -------------------------------------------------------------------------------- 1 | /* div { 2 | max-width: 900px !important; 3 | text-align: justify; 4 | } */ 5 | 6 | 7 | /* .inner_cell { 8 | max-width: 900px !important; 9 | text-align: justify; 10 | } */ 11 | 12 | .cell { 13 | /* max-width: 900px !important; */ 14 | /* line-height: 1.6em; */ 15 | padding-bottom: 15px; 16 | text-align: justify; 17 | } 18 | 19 | .markup { 20 | max-width: 900px !important; 21 | text-align: justify; 22 | } 23 | 24 | img { 25 | display: block; 26 | margin-left: auto; 27 | margin-right: auto; 28 | } 29 | 30 | /* αt */ 31 | /* βt */ 32 | /* γt */ -------------------------------------------------------------------------------- /Part 2/2 Higher Order Derivatives.ipynb: -------------------------------------------------------------------------------- 1 | { 2 | "cells": [ 3 | { 4 | "cell_type": "code", 5 | "execution_count": 1, 6 | "metadata": {}, 7 | "outputs": [], 8 | "source": [ 9 | "try:\n", 10 | " # We must install required packages if we are in Google Colab\n", 11 | " import google.colab\n", 12 | " %pip install roboticstoolbox-python>=1.0.2\n", 13 | "except:\n", 14 | " # We are not in Google Colab\n", 15 | " # Apply custon style to notebook\n", 16 | " from IPython.core.display import HTML\n", 17 | " import pathlib\n", 18 | " styles_path = pathlib.Path(pathlib.Path().absolute(), \"style\", \"style.css\")\n", 19 | " styles = open(styles_path, \"r\").read()\n", 20 | " HTML(f\"\")" 21 | ] 22 | }, 23 | { 24 | "attachments": {}, 25 | "cell_type": "markdown", 26 | "metadata": {}, 27 | "source": [ 28 | "# 2.0 Higher Order Derivatives\n", 29 | "\n", 30 | "$\\large{\\text{Manipulator Differential Kinematics}} \\\\ \\large{\\text{Part II: Acceleration and Advanced Applications}}$\n", 31 | "\n", 32 | "$\\text{By Jesse Haviland and Peter Corke}$\n", 33 | "\n", 34 | "
\n", 35 | "\n", 36 | "The sections of the Tutorial paper related to this notebook are listed next to each contents entry.\n", 37 | "It is beneficial to read these sections of the paper before attempting the notebook Section.\n", 38 | "\n", 39 | "### Contents\n", 40 | "\n", 41 | "[2.1 Higher Order Derivatives](#pre)\n", 42 | "* Part I of the Tutorial\n", 43 | " * Deriving the Manipulator Jacobian\n", 44 | " * First Derivative of an Elementary Transform\n", 45 | " * The Manipulator Jacobian\n", 46 | " * Fast Manipulator Jacobian\n" 47 | ] 48 | }, 49 | { 50 | "cell_type": "code", 51 | "execution_count": 2, 52 | "metadata": {}, 53 | "outputs": [], 54 | "source": [ 55 | "# We will do the imports required for this notebook here\n", 56 | "\n", 57 | "# numpy provides import array and linear algebra utilities\n", 58 | "import numpy as np\n", 59 | "\n", 60 | "# the robotics toolbox provides robotics specific functionality\n", 61 | "import roboticstoolbox as rtb" 62 | ] 63 | }, 64 | { 65 | "attachments": {}, 66 | "cell_type": "markdown", 67 | "metadata": {}, 68 | "source": [ 69 | "
\n", 70 | "\n", 71 | "\n", 72 | "### 2.1 Higher Order Derivatives\n", 73 | "---\n" 74 | ] 75 | }, 76 | { 77 | "attachments": {}, 78 | "cell_type": "markdown", 79 | "metadata": {}, 80 | "source": [ 81 | "Starting from $n = 3$, obtaining the matrix of $n^{th}$ partial derivative of the manipulator kinematics can be obtained using the product rule on the $(n - 1)^{th}$ partial derivative while considering their partitioned form.\n", 82 | "\n", 83 | "For example, to obtain the $3^{rd}$ partial derivative, we take the partial derivative of the manipulator Hessian with respect to the joint coordinates, in its partitioned form\n", 84 | "\n", 85 | "\\begin{align*}\n", 86 | " \\frac{\\partial \\bf{H}_{jk}(\\bf{q})}\n", 87 | " {\\partial q_l}\n", 88 | " &=\n", 89 | " \\begin{pmatrix}\n", 90 | " \\dfrac{\\partial \\bf{H}_{a_{jk}}(\\bf{q})}\n", 91 | " {\\partial q_l} \\\\\n", 92 | " \\dfrac{\\partial \\bf{H}_{\\alpha_{jk}}(\\bf{q})}\n", 93 | " {\\partial q_l}\n", 94 | " \\end{pmatrix} \\\\\n", 95 | " &=\n", 96 | " \\begin{pmatrix}\n", 97 | " \\dfrac{\\partial }\n", 98 | " {\\partial q_l}\n", 99 | " \\left( \\bf{J}_{\\omega_k} \\times \\bf{J}_{\\omega_j} \\right) \\\\\n", 100 | " \\dfrac{\\partial}\n", 101 | " {\\partial q_l}\n", 102 | " \\left( \\bf{J}_{\\omega_k} \\times \\bf{J}_{v_j} \\right)\n", 103 | " \\end{pmatrix} \\\\\n", 104 | " &=\n", 105 | " \\begin{pmatrix}\n", 106 | " \\left( \\bf{H}_{\\omega_{kl}} \\times \\bf{J}_{\\omega_j} \\right) +\n", 107 | " \\left( \\bf{J}_{\\omega_k} \\times \\bf{H}_{\\omega_{jl}} \\right) \\\\\n", 108 | " \\left( \\bf{H}_{\\omega_{kl}} \\times \\bf{J}_{v_j} \\right) +\n", 109 | " \\left( \\bf{J}_{\\omega_k} \\times \\bf{H}_{v_{jl}} \\right)\n", 110 | " \\end{pmatrix}\n", 111 | "\\end{align*}\n", 112 | "\n", 113 | "where $\\frac{\\partial \\bf{H}_{jk}(\\bf{q})}{\\partial q_l} \\in \\mathbb{R}^{6}$. Continuing, we obtain the following\n", 114 | "\n", 115 | "\\begin{align*}\n", 116 | " \\frac{\\partial \\bf{H}_{jk}(\\bf{q})}\n", 117 | " {\\partial \\bf{q}}\n", 118 | " &=\n", 119 | " \\begin{pmatrix}\n", 120 | " \\dfrac{\\partial \\bf{H}_{jk}(\\bf{q})}\n", 121 | " {\\partial q_0} & \\cdots & \n", 122 | " \\dfrac{\\partial \\bf{H}_{jk}(\\bf{q})}\n", 123 | " {\\partial q_n}\n", 124 | " \\end{pmatrix}\n", 125 | "\\end{align*}\n", 126 | "\n", 127 | "where $\\frac{\\partial \\bf{H}_{jk}(\\bf{q})}{\\partial q_l} \\in \\mathbb{R}^{6 \\times n}$,\n", 128 | "\n", 129 | "\\begin{align*}\n", 130 | " \\frac{\\partial \\bf{H}_{j}(\\bf{q})}\n", 131 | " {\\partial \\bf{q}}\n", 132 | " &=\n", 133 | " \\begin{pmatrix}\n", 134 | " \\dfrac{\\partial \\bf{H}_{j_0}(\\bf{q})}\n", 135 | " {\\partial \\bf{q}} & \\cdots & \n", 136 | " \\dfrac{\\partial \\bf{H}_{j_n}(\\bf{q})}\n", 137 | " {\\partial \\bf{q}}\n", 138 | " \\end{pmatrix}\n", 139 | "\\end{align*}\n", 140 | "\n", 141 | "where $\\frac{\\partial \\bf{H}_{jk}(\\bf{q})}{\\partial \\bf{q}} \\in \\mathbb{R}^{6 \\times n \\times n}$, and finally \n", 142 | "\n", 143 | "\\begin{align*}\n", 144 | " \\frac{\\partial \\bf{H}(\\bf{q})}\n", 145 | " {\\partial \\bf{q}}\n", 146 | " &=\n", 147 | " \\begin{pmatrix}\n", 148 | " \\dfrac{\\partial \\bf{H}_{0}(\\bf{q})}\n", 149 | " {\\partial \\bf{q}} & \\cdots & \n", 150 | " \\dfrac{\\partial \\bf{H}_{n}(\\bf{q})}\n", 151 | " {\\partial \\bf{q}}\n", 152 | " \\end{pmatrix}\n", 153 | "\\end{align*}\n", 154 | "\n", 155 | "where $\\frac{\\partial \\bf{H}(\\bf{q})}{\\partial \\bf{q}} \\in \\mathbb{R}^{6 \\times n \\times n \\times n}$ is the 4-dimensional tensor representing the $3^{rd}$ partial derivative of the manipulator kinematics.\n", 156 | "\n", 157 | "Note that the function has $\\mathcal{O}(n^{order})$ time complexity, where $order$ represents the order of the partial derivative being calculated." 158 | ] 159 | }, 160 | { 161 | "cell_type": "code", 162 | "execution_count": 3, 163 | "metadata": {}, 164 | "outputs": [], 165 | "source": [ 166 | "def partial_fkine0(ets, q, n):\n", 167 | " r\"\"\"\n", 168 | " Manipulator Forward Kinematics nth Partial Derivative\n", 169 | "\n", 170 | " :return: The nth Partial Derivative of the forward kinematics\n", 171 | " \"\"\"\n", 172 | "\n", 173 | " # Calculate the Jacobian and Hessian\n", 174 | " J = ets.jacob0(q)\n", 175 | " H = ets.hessian0(q)\n", 176 | "\n", 177 | " # A list of derivatives, starting with the jacobian and hessian\n", 178 | " dT = [J, H]\n", 179 | "\n", 180 | " # The tensor dimensions of the latest derivative\n", 181 | " # Set to the current size of the Hessian\n", 182 | " size = [ets.n, 6, ets.n]\n", 183 | "\n", 184 | " # An array which keeps track of the index of the partial derivative\n", 185 | " # we are calculating\n", 186 | " # It stores the indices in the order: \"j, k, l. m, n, o, ...\"\n", 187 | " # where count is extended to match oder of the partial derivative\n", 188 | " count = np.array([0, 0])\n", 189 | "\n", 190 | " # The order of derivative for which we are calculating\n", 191 | " # The Hessian is the 2nd-order so we start with c = 2\n", 192 | " c = 2\n", 193 | "\n", 194 | " def add_indices(indices, c):\n", 195 | " total = len(indices * 2)\n", 196 | " new_indices = []\n", 197 | "\n", 198 | " for i in range(total):\n", 199 | " j = i // 2\n", 200 | " new_indices.append([])\n", 201 | " new_indices[i].append(indices[j][0].copy())\n", 202 | " new_indices[i].append(indices[j][1].copy())\n", 203 | "\n", 204 | " if i % 2 == 0:\n", 205 | " # if even number\n", 206 | " new_indices[i][0].append(c)\n", 207 | " else:\n", 208 | " # if odd number\n", 209 | " new_indices[i][1].append(c)\n", 210 | "\n", 211 | " return new_indices\n", 212 | "\n", 213 | " def add_pdi(pdi):\n", 214 | " total = len(pdi * 2)\n", 215 | " new_pdi = []\n", 216 | "\n", 217 | " for i in range(total):\n", 218 | " j = i // 2\n", 219 | " new_pdi.append([])\n", 220 | " new_pdi[i].append(pdi[j][0])\n", 221 | " new_pdi[i].append(pdi[j][1])\n", 222 | "\n", 223 | " # if even number\n", 224 | " if i % 2 == 0:\n", 225 | " new_pdi[i][0] += 1\n", 226 | " # if odd number\n", 227 | " else:\n", 228 | " new_pdi[i][1] += 1\n", 229 | "\n", 230 | " return new_pdi\n", 231 | "\n", 232 | " # these are the indices used for the hessian\n", 233 | " indices = [[[1], [0]]]\n", 234 | "\n", 235 | " # The partial derivative indices (pdi)\n", 236 | " # the are the pd indices used in the cross products\n", 237 | " pdi = [[0, 0]]\n", 238 | "\n", 239 | " # The length of dT correspods to the number of derivatives we have calculated\n", 240 | " while len(dT) != n:\n", 241 | "\n", 242 | " # Add to the start of the tensor size list \n", 243 | " size.insert(0, ets.n)\n", 244 | "\n", 245 | " # Add an axis to the count array\n", 246 | " count = np.concatenate(([0], count))\n", 247 | "\n", 248 | " # This variables corresponds to indices within the previous partial derivatives\n", 249 | " # to be cross prodded\n", 250 | " # The order is: \"[j, k, l, m, n, o, ...]\"\n", 251 | " # Although, our partial derivatives have the order: pd[..., o, n, m, l, k, cartesian DoF, j]\n", 252 | " # For example, consider the Hessian Tensor H[n, 6, n], the index H[k, :, j]. This corrsponds\n", 253 | " # to the second partial derivative of the kinematics of joint j with respect to joint k.\n", 254 | " indices = add_indices(indices, c)\n", 255 | "\n", 256 | " # This variable corresponds to the indices in Td which corresponds to the \n", 257 | " # partial derivatives we need to use\n", 258 | " pdi = add_pdi(pdi)\n", 259 | "\n", 260 | " c += 1\n", 261 | "\n", 262 | " # Allocate our new partial derivative tensor\n", 263 | " pd = np.zeros(size)\n", 264 | "\n", 265 | " # We need to loop n^c times\n", 266 | " # There are n^c columns to calculate\n", 267 | " for _ in range(ets.n**c):\n", 268 | " \n", 269 | " # Allocate the rotation and translation components\n", 270 | " rot = np.zeros(3)\n", 271 | " trn = np.zeros(3)\n", 272 | "\n", 273 | " # This loop calculates a single column ([trn, rot]) of the tensor for dT(x)\n", 274 | " for j in range(len(indices)):\n", 275 | " pdr0 = dT[pdi[j][0]]\n", 276 | " pdr1 = dT[pdi[j][1]]\n", 277 | "\n", 278 | " idx0 = count[indices[j][0]]\n", 279 | " idx1 = count[indices[j][1]]\n", 280 | "\n", 281 | " # This is a list of indices selecting the slices of the previous tensor\n", 282 | " idx0_slices = np.flip(idx0[1:])\n", 283 | " idx1_slices = np.flip(idx1[1:])\n", 284 | "\n", 285 | " # This index selecting the column within the 2d slice of the previous tensor\n", 286 | " idx0_n = idx0[0]\n", 287 | " idx1_n = idx1[0]\n", 288 | "\n", 289 | " # Use our indices to select the rotational column from pdr0 and pdr1\n", 290 | " col0_rot = pdr0[(*idx0_slices, slice(3, 6), idx0_n)]\n", 291 | " col1_rot = pdr1[(*idx1_slices, slice(3, 6), idx1_n)]\n", 292 | "\n", 293 | " # Use our indices to select the translational column from pdr1\n", 294 | " col1_trn = pdr1[(*idx1_slices, slice(0, 3), idx1_n)]\n", 295 | "\n", 296 | " # Perform the cross product as described in the maths above\n", 297 | " rot += np.cross(col0_rot, col1_rot)\n", 298 | " trn += np.cross(col0_rot, col1_trn)\n", 299 | "\n", 300 | " pd[(*np.flip(count[1:]), slice(0, 3), count[0])] = trn\n", 301 | " pd[(*np.flip(count[1:]), slice(3, 6), count[0])] = rot\n", 302 | "\n", 303 | " count[0] += 1\n", 304 | " for j in range(len(count)):\n", 305 | " if count[j] == ets.n:\n", 306 | " count[j] = 0\n", 307 | " if j != len(count) - 1:\n", 308 | " count[j + 1] += 1\n", 309 | "\n", 310 | " dT.append(pd)\n", 311 | "\n", 312 | " return dT[-1]" 313 | ] 314 | }, 315 | { 316 | "attachments": {}, 317 | "cell_type": "markdown", 318 | "metadata": {}, 319 | "source": [ 320 | "Lets try it out" 321 | ] 322 | }, 323 | { 324 | "cell_type": "code", 325 | "execution_count": 4, 326 | "metadata": {}, 327 | "outputs": [], 328 | "source": [ 329 | "# Make a panda robot\n", 330 | "panda = rtb.models.Panda()\n", 331 | "\n", 332 | "# Get the ets of our robot\n", 333 | "ets = panda.ets()\n", 334 | "\n", 335 | "# Make a joint coordinate vector for the Panda\n", 336 | "q = [0.21, -0.03, 0.35, -1.90, -0.04, 1.96, 1.36]" 337 | ] 338 | }, 339 | { 340 | "cell_type": "code", 341 | "execution_count": 5, 342 | "metadata": {}, 343 | "outputs": [ 344 | { 345 | "name": "stdout", 346 | "output_type": "stream", 347 | "text": [ 348 | "(7, 7, 6, 7)\n" 349 | ] 350 | } 351 | ], 352 | "source": [ 353 | "# Lets do the third-order differential dinematics\n", 354 | "pd3 = partial_fkine0(ets, q, 3)\n", 355 | "\n", 356 | "# When using the Panda, this will have dimensions 7 x 7 x 6 x 7\n", 357 | "print(pd3.shape)" 358 | ] 359 | }, 360 | { 361 | "cell_type": "code", 362 | "execution_count": 6, 363 | "metadata": {}, 364 | "outputs": [ 365 | { 366 | "name": "stdout", 367 | "output_type": "stream", 368 | "text": [ 369 | "[[[[ 0.3 -0.08 0.3 ... 0.09 -0.17 0. ]\n", 370 | " [-0.49 -0.02 -0.49 ... -0.14 -0.11 0. ]\n", 371 | " [-0. 0. -0. ... -0. 0. 0. ]\n", 372 | " [ 0. 0.21 0.03 ... -0.81 -0.54 -0.1 ]\n", 373 | " [ 0. -0.98 0.01 ... -0.5 0.84 -0.01]\n", 374 | " [ 0. -0. -0. ... 0. 0. 0. ]]\n", 375 | "\n", 376 | " [[ 0. 0.11 0. ... 0. -0.02 0. ]\n", 377 | " [-0. -0.53 -0.01 ... -0.01 0.1 0. ]\n", 378 | " [ 0. 0. 0. ... 0. 0. 0. ]\n", 379 | " [-0.21 0. -0.21 ... 0.06 -0.01 0.21]\n", 380 | " [ 0.98 0. 0.98 ... -0.29 0.05 -0.97]\n", 381 | " [ 0. 0. 0. ... 0. 0. -0. ]]\n", 382 | "\n", 383 | " [[ 0.3 -0.06 0.3 ... 0.09 -0.17 0. ]\n", 384 | " [-0.49 -0.01 -0.49 ... -0.14 -0.11 0. ]\n", 385 | " [-0. 0. -0. ... -0. 0. 0. ]\n", 386 | " [-0.03 0.21 0. ... -0.8 -0.54 -0.07]\n", 387 | " [-0.01 -0.98 0. ... -0.5 0.84 -0. ]\n", 388 | " [ 0. -0. 0. ... 0. 0. 0. ]]\n", 389 | "\n", 390 | " ...\n", 391 | "\n", 392 | " [[-0.09 -0.41 -0.09 ... -0.03 0.14 0. ]\n", 393 | " [ 0.14 -0.27 0.14 ... 0.04 0.09 0. ]\n", 394 | " [ 0. 0. 0. ... 0. 0. 0. ]\n", 395 | " [ 0.81 -0.06 0.8 ... 0. 0.2 -0.78]\n", 396 | " [ 0.5 0.29 0.5 ... 0. -0.22 -0.5 ]\n", 397 | " [-0. 0. -0. ... 0. -0. 0. ]]\n", 398 | "\n", 399 | " [[ 0.01 -0.29 0.01 ... -0. 0.05 0. ]\n", 400 | " [-0.02 0.45 -0.02 ... 0. -0.09 0. ]\n", 401 | " [ 0. 0. 0. ... 0. 0. 0. ]\n", 402 | " [ 0.54 0.01 0.54 ... -0.2 0. -0.54]\n", 403 | " [-0.84 -0.05 -0.84 ... 0.22 0. 0.84]\n", 404 | " [-0. 0. -0. ... 0. 0. 0. ]]\n", 405 | "\n", 406 | " [[-0.3 0.03 -0.3 ... -0.09 0.18 0. ]\n", 407 | " [ 0.48 0.01 0.49 ... 0.13 0.11 0. ]\n", 408 | " [ 0. -0. 0. ... 0. -0. 0. ]\n", 409 | " [ 0.1 -0.21 0.07 ... 0.78 0.54 0. ]\n", 410 | " [ 0.01 0.97 0. ... 0.5 -0.84 0. ]\n", 411 | " [-0. 0. -0. ... -0. -0. 0. ]]]\n", 412 | "\n", 413 | "\n", 414 | " [[[-0.08 0.11 0. ... 0. -0.02 0. ]\n", 415 | " [-0.02 -0.53 -0.01 ... -0.01 0.1 0. ]\n", 416 | " [ 0. -0. -0. ... -0. 0. 0. ]\n", 417 | " [ 0. 0. -0.21 ... 0.06 -0.01 0.21]\n", 418 | " [ 0. 0. 0.98 ... -0.29 0.05 -0.97]\n", 419 | " [ 0. -0. 0. ... -0. 0. -0. ]]\n", 420 | "\n", 421 | " [[-0. -0.08 0.19 ... 0.06 -0.18 0. ]\n", 422 | " [-0. -0.02 0.04 ... 0.01 -0.04 0. ]\n", 423 | " [ 0. 0.54 0.01 ... 0.01 -0.11 0. ]\n", 424 | " [-0. 0. 0.03 ... -0.88 -0.35 -0.1 ]\n", 425 | " [-0. 0. 0.01 ... -0.19 -0.07 -0.02]\n", 426 | " [ 0. 0. -1. ... 0.3 -0.05 0.99]]\n", 427 | "\n", 428 | " [[-0.09 0. -0.02 ... -0. -0. 0. ]\n", 429 | " [-0.03 0. -0. ... -0. -0. 0. ]\n", 430 | " [ 0.54 0. 0.54 ... 0.15 0.08 0. ]\n", 431 | " [ 0.21 -0.03 0. ... -0.01 0.03 0. ]\n", 432 | " [-0.98 -0.01 0. ... -0. 0.01 0. ]\n", 433 | " [-0. 1. 0. ... 0.32 -0.93 -0.01]]\n", 434 | "\n", 435 | " ...\n", 436 | "\n", 437 | " [[ 0.46 -0.03 0.54 ... 0.15 0.01 0. ]\n", 438 | " [ 0.27 -0.01 0.11 ... 0.03 0. 0. ]\n", 439 | " [-0.09 0.17 -0.16 ... -0.04 -0.06 0. ]\n", 440 | " [-0.06 0.88 0.01 ... 0. -0.93 -0.04]\n", 441 | " [ 0.29 0.19 0. ... 0. -0.2 -0.01]\n", 442 | " [ 0. -0.3 -0.32 ... 0. 0.26 0.33]]\n", 443 | "\n", 444 | " [[ 0.17 0.07 0.01 ... 0. 0.2 0. ]\n", 445 | " [ 0.1 0.02 0. ... 0. 0.04 0. ]\n", 446 | " [ 0.05 -0.5 0.02 ... -0. 0.1 0. ]\n", 447 | " [ 0.01 0.35 -0.03 ... 0.93 0. 0.09]\n", 448 | " [-0.05 0.07 -0.01 ... 0.2 0. 0.02]\n", 449 | " [-0. 0.05 0.93 ... -0.26 0. -0.93]]\n", 450 | "\n", 451 | " [[ 0.13 0. 0.05 ... 0.01 0.01 0. ]\n", 452 | " [ 0.05 0. 0.01 ... 0. 0. 0. ]\n", 453 | " [-0.53 -0.01 -0.54 ... -0.15 -0.07 0. ]\n", 454 | " [-0.21 0.1 -0. ... 0.04 -0.09 0. ]\n", 455 | " [ 0.97 0.02 -0. ... 0.01 -0.02 0. ]\n", 456 | " [ 0. -0.99 0.01 ... -0.33 0.93 0. ]]]\n", 457 | "\n", 458 | "\n", 459 | " [[[ 0.3 0. 0.3 ... 0.09 -0.17 0. ]\n", 460 | " [-0.49 -0.01 -0.49 ... -0.14 -0.11 0. ]\n", 461 | " [-0. -0. -0. ... -0. 0. 0. ]\n", 462 | " [ 0. 0. -0. ... -0.8 -0.54 -0.07]\n", 463 | " [ 0. 0. -0. ... -0.5 0.84 -0. ]\n", 464 | " [ 0. 0. 0. ... 0. 0. 0. ]]\n", 465 | "\n", 466 | " [[ 0. 0.19 -0.02 ... -0. -0. 0. ]\n", 467 | " [-0. 0.04 -0. ... -0. -0. 0. ]\n", 468 | " [ 0.54 0.01 0.54 ... 0.15 0.08 0. ]\n", 469 | " [ 0. 0. 0. ... -0.01 0.03 0. ]\n", 470 | " [ 0. 0. 0. ... -0. 0.01 0. ]\n", 471 | " [ 0. 0. 0. ... 0.32 -0.93 -0.01]]\n", 472 | "\n", 473 | " [[ 0.3 0. 0.3 ... 0.09 -0.17 0. ]\n", 474 | " [-0.49 -0. -0.49 ... -0.14 -0.11 0. ]\n", 475 | " [ 0.01 0. 0.01 ... 0. -0.01 0. ]\n", 476 | " [ 0. -0. 0. ... -0.8 -0.54 -0.07]\n", 477 | " [ 0. -0. 0. ... -0.5 0.84 -0. ]\n", 478 | " [-0. -0. 0. ... -0.03 -0.01 -0. ]]\n", 479 | "\n", 480 | " ...\n", 481 | "\n", 482 | " [[-0.08 -0.34 -0.1 ... -0.03 0.14 0. ]\n", 483 | " [ 0.15 -0.42 0.16 ... 0.04 0.09 0. ]\n", 484 | " [-0. -0.07 -0. ... -0. 0. 0. ]\n", 485 | " [ 0.8 0.01 0.8 ... 0. 0.21 -0.78]\n", 486 | " [ 0.5 0. 0.5 ... 0. -0.25 -0.5 ]\n", 487 | " [-0. -0.32 0.03 ... 0. 0. -0.03]]\n", 488 | "\n", 489 | " [[ 0. -0.45 0.01 ... -0. 0.05 0. ]\n", 490 | " [-0.03 0.35 -0.02 ... 0. -0.09 0. ]\n", 491 | " [-0. -0.03 0. ... 0. 0. 0. ]\n", 492 | " [ 0.54 -0.03 0.54 ... -0.21 0. -0.54]\n", 493 | " [-0.84 -0.01 -0.84 ... 0.25 0. 0.84]\n", 494 | " [-0. 0.93 0.01 ... -0. 0. -0.01]]\n", 495 | "\n", 496 | " [[-0.3 -0.04 -0.3 ... -0.09 0.18 0. ]\n", 497 | " [ 0.49 -0.01 0.49 ... 0.14 0.11 0. ]\n", 498 | " [-0.01 -0.01 -0.01 ... -0. 0.01 0. ]\n", 499 | " [ 0.07 -0. 0.07 ... 0.78 0.54 0. ]\n", 500 | " [ 0. -0. 0. ... 0.5 -0.84 0. ]\n", 501 | " [-0. 0.01 0. ... 0.03 0.01 0. ]]]\n", 502 | "\n", 503 | "\n", 504 | " ...\n", 505 | "\n", 506 | "\n", 507 | " [[[ 0.09 0. 0.09 ... -0.03 0.14 0. ]\n", 508 | " [-0.14 -0.01 -0.14 ... 0.04 0.09 0. ]\n", 509 | " [-0. -0. -0. ... 0. -0. 0. ]\n", 510 | " [ 0. 0. 0. ... 0. 0.2 -0.78]\n", 511 | " [ 0. 0. 0. ... -0. -0.22 -0.5 ]\n", 512 | " [ 0. 0. 0. ... -0. -0. 0. ]]\n", 513 | "\n", 514 | " [[ 0. 0.06 -0. ... 0.15 0.01 0. ]\n", 515 | " [-0. 0.01 -0. ... 0.03 0. 0. ]\n", 516 | " [ 0.15 0.01 0.15 ... -0.04 -0.06 0. ]\n", 517 | " [ 0. 0. 0. ... 0. -0.93 -0.04]\n", 518 | " [ 0. 0. 0. ... 0. -0.2 -0.01]\n", 519 | " [ 0. 0. 0. ... 0. 0.26 0.33]]\n", 520 | "\n", 521 | " [[ 0.09 0. 0.09 ... -0.03 0.14 0. ]\n", 522 | " [-0.14 -0.01 -0.14 ... 0.04 0.09 0. ]\n", 523 | " [ 0. 0. 0. ... -0. 0. 0. ]\n", 524 | " [ 0. 0. 0. ... 0. 0.21 -0.78]\n", 525 | " [ 0. 0. 0. ... 0. -0.25 -0.5 ]\n", 526 | " [ 0. 0. 0. ... 0. 0. -0.03]]\n", 527 | "\n", 528 | " ...\n", 529 | "\n", 530 | " [[-0.03 0.03 -0.03 ... 0.09 -0.04 0. ]\n", 531 | " [ 0.04 -0.04 0.04 ... -0.14 -0.03 0. ]\n", 532 | " [-0. 0. -0. ... 0.01 -0.15 0. ]\n", 533 | " [-0. -0. -0. ... 0. -0.54 0.21]\n", 534 | " [ 0. -0. -0. ... 0. 0.84 0.18]\n", 535 | " [ 0. -0. -0. ... 0. -0.05 0.88]]\n", 536 | "\n", 537 | " [[ 0.47 0.08 0.48 ... 0. 0.09 0. ]\n", 538 | " [ 0.28 -0.23 0.28 ... -0. -0.14 0. ]\n", 539 | " [-0.33 0. -0.33 ... 0. 0.01 0. ]\n", 540 | " [-0.2 0.93 -0.21 ... 0.54 0. 0.21]\n", 541 | " [ 0.22 0.2 0.25 ... -0.84 0. -0.32]\n", 542 | " [ 0. -0.26 -0. ... 0.05 0. 0.02]]\n", 543 | "\n", 544 | " [[-0.06 -0.42 -0.07 ... 0.03 -0.05 0. ]\n", 545 | " [ 0.15 -0.27 0.15 ... -0.05 -0.04 0. ]\n", 546 | " [-0.02 -0.07 -0.02 ... 0. -0.2 0. ]\n", 547 | " [ 0.78 0.04 0.78 ... -0.21 -0.21 0. ]\n", 548 | " [ 0.5 0.01 0.5 ... -0.18 0.32 0. ]\n", 549 | " [-0. -0.33 0.03 ... -0.88 -0.02 0. ]]]\n", 550 | "\n", 551 | "\n", 552 | " [[[-0.17 -0.02 -0.17 ... 0.14 0.05 0. ]\n", 553 | " [-0.11 0.1 -0.11 ... 0.09 -0.09 0. ]\n", 554 | " [ 0. 0. 0. ... -0. -0. 0. ]\n", 555 | " [ 0. 0. 0. ... 0. -0. -0.54]\n", 556 | " [ 0. 0. 0. ... 0. 0. 0.84]\n", 557 | " [ 0. 0. 0. ... 0. 0. 0. ]]\n", 558 | "\n", 559 | " [[-0. -0.18 -0. ... 0.01 0.2 0. ]\n", 560 | " [-0. -0.04 -0. ... 0. 0.04 0. ]\n", 561 | " [ 0.08 -0.11 0.08 ... -0.06 0.1 0. ]\n", 562 | " [ 0. 0. 0. ... 0. 0. 0.09]\n", 563 | " [ 0. 0. 0. ... 0. 0. 0.02]\n", 564 | " [ 0. 0. 0. ... 0. -0. -0.93]]\n", 565 | "\n", 566 | " [[-0.17 -0.02 -0.17 ... 0.14 0.05 0. ]\n", 567 | " [-0.11 0.1 -0.11 ... 0.09 -0.09 0. ]\n", 568 | " [-0.01 0. -0.01 ... 0. 0. 0. ]\n", 569 | " [ 0. 0. 0. ... 0. -0. -0.54]\n", 570 | " [ 0. 0. 0. ... 0. 0. 0.84]\n", 571 | " [ 0. 0. 0. ... 0. -0. -0.01]]\n", 572 | "\n", 573 | " ...\n", 574 | "\n", 575 | " [[ 0.05 -0.09 0.05 ... -0.04 0.09 0. ]\n", 576 | " [ 0.03 0.12 0.04 ... -0.03 -0.14 0. ]\n", 577 | " [ 0.19 -0.03 0.2 ... -0.15 0.01 0. ]\n", 578 | " [ 0. 0. 0. ... 0. 0. 0.21]\n", 579 | " [ 0. 0. 0. ... 0. -0. -0.32]\n", 580 | " [ 0. 0. 0. ... 0. 0. 0.02]]\n", 581 | "\n", 582 | " [[-0.01 0.16 -0.01 ... -0. -0.17 0. ]\n", 583 | " [-0.01 0.11 -0. ... -0. -0.11 0. ]\n", 584 | " [-0.01 0.1 -0. ... -0. -0.11 0. ]\n", 585 | " [ 0. -0. 0. ... -0. 0. -0.1 ]\n", 586 | " [-0. -0. -0. ... 0. 0. -0.01]\n", 587 | " [-0. 0. 0. ... -0. 0. 0.99]]\n", 588 | "\n", 589 | " [[ 0.12 -0.27 0.12 ... -0.15 0. 0. ]\n", 590 | " [ 0.09 0.37 0.09 ... -0.09 -0. 0. ]\n", 591 | " [ 0.59 -0.03 0.59 ... 0.15 -0. 0. ]\n", 592 | " [ 0.54 -0.09 0.54 ... -0.21 0.1 0. ]\n", 593 | " [-0.84 -0.02 -0.84 ... 0.32 0.01 0. ]\n", 594 | " [-0. 0.93 0.01 ... -0.02 -0.99 0. ]]]\n", 595 | "\n", 596 | "\n", 597 | " [[[ 0. 0. 0. ... 0. 0. 0. ]\n", 598 | " [ 0. 0. 0. ... 0. 0. 0. ]\n", 599 | " [ 0. 0. 0. ... 0. 0. 0. ]\n", 600 | " [ 0. 0. 0. ... 0. 0. 0. ]\n", 601 | " [ 0. 0. 0. ... 0. 0. -0. ]\n", 602 | " [ 0. 0. 0. ... 0. 0. -0. ]]\n", 603 | "\n", 604 | " [[ 0. 0. 0. ... 0. 0. 0. ]\n", 605 | " [ 0. 0. 0. ... 0. 0. 0. ]\n", 606 | " [ 0. 0. 0. ... 0. 0. 0. ]\n", 607 | " [ 0. 0. 0. ... 0. 0. 0. ]\n", 608 | " [ 0. 0. 0. ... 0. 0. 0. ]\n", 609 | " [ 0. 0. 0. ... 0. 0. 0. ]]\n", 610 | "\n", 611 | " [[ 0. 0. 0. ... 0. 0. 0. ]\n", 612 | " [ 0. 0. 0. ... 0. 0. 0. ]\n", 613 | " [ 0. 0. 0. ... 0. 0. 0. ]\n", 614 | " [ 0. 0. 0. ... 0. 0. 0. ]\n", 615 | " [ 0. 0. 0. ... 0. 0. -0. ]\n", 616 | " [ 0. 0. 0. ... 0. 0. 0. ]]\n", 617 | "\n", 618 | " ...\n", 619 | "\n", 620 | " [[ 0. 0. 0. ... 0. 0. 0. ]\n", 621 | " [ 0. 0. 0. ... 0. 0. 0. ]\n", 622 | " [ 0. 0. 0. ... 0. 0. 0. ]\n", 623 | " [ 0. 0. 0. ... 0. 0. -0. ]\n", 624 | " [ 0. 0. 0. ... 0. 0. 0. ]\n", 625 | " [ 0. 0. 0. ... 0. 0. -0. ]]\n", 626 | "\n", 627 | " [[ 0. 0. 0. ... 0. 0. 0. ]\n", 628 | " [ 0. 0. 0. ... 0. 0. 0. ]\n", 629 | " [ 0. 0. 0. ... 0. 0. 0. ]\n", 630 | " [ 0. 0. 0. ... 0. 0. 0. ]\n", 631 | " [ 0. 0. 0. ... 0. 0. -0. ]\n", 632 | " [ 0. 0. 0. ... 0. 0. -0. ]]\n", 633 | "\n", 634 | " [[-0. 0. 0. ... 0. -0. 0. ]\n", 635 | " [-0. -0. -0. ... -0. 0. 0. ]\n", 636 | " [-0. 0. -0. ... -0. 0. 0. ]\n", 637 | " [-0. -0. -0. ... 0. -0. 0. ]\n", 638 | " [ 0. -0. 0. ... -0. 0. 0. ]\n", 639 | " [ 0. -0. -0. ... 0. 0. 0. ]]]]\n" 640 | ] 641 | } 642 | ], 643 | "source": [ 644 | "# We can view the tensor, but it's very large! It has 49 6x7 slices\n", 645 | "print(np.round(pd3, 2))" 646 | ] 647 | }, 648 | { 649 | "cell_type": "code", 650 | "execution_count": 7, 651 | "metadata": {}, 652 | "outputs": [ 653 | { 654 | "name": "stdout", 655 | "output_type": "stream", 656 | "text": [ 657 | "(7, 7, 7, 7, 6, 7)\n" 658 | ] 659 | } 660 | ], 661 | "source": [ 662 | "# We can work out any order of the differential kinematics, but the\n", 663 | "# higher we go, the loger it will take\n", 664 | "# Lets do the fifth-order differential dinematics\n", 665 | "pd5 = partial_fkine0(ets, q, 5)\n", 666 | "\n", 667 | "# View the dimension\n", 668 | "print(pd5.shape)" 669 | ] 670 | } 671 | ], 672 | "metadata": { 673 | "interpreter": { 674 | "hash": "528815e074ebcdb9b34bcb695d4aa9d425bdb2cc6656b4ca45050b51a4125937" 675 | }, 676 | "kernelspec": { 677 | "display_name": "Python 3.8.10 ('rtb')", 678 | "language": "python", 679 | "name": "python3" 680 | }, 681 | "language_info": { 682 | "codemirror_mode": { 683 | "name": "ipython", 684 | "version": 3 685 | }, 686 | "file_extension": ".py", 687 | "mimetype": "text/x-python", 688 | "name": "python", 689 | "nbconvert_exporter": "python", 690 | "pygments_lexer": "ipython3", 691 | "version": "3.11.0" 692 | }, 693 | "orig_nbformat": 4 694 | }, 695 | "nbformat": 4, 696 | "nbformat_minor": 2 697 | } 698 | -------------------------------------------------------------------------------- /Part 2/4 Null-Space Projection for Motion Control.ipynb: -------------------------------------------------------------------------------- 1 | { 2 | "cells": [ 3 | { 4 | "cell_type": "code", 5 | "execution_count": 1, 6 | "metadata": {}, 7 | "outputs": [ 8 | { 9 | "data": { 10 | "text/html": [ 11 | "" 43 | ], 44 | "text/plain": [ 45 | "" 46 | ] 47 | }, 48 | "execution_count": 1, 49 | "metadata": {}, 50 | "output_type": "execute_result" 51 | } 52 | ], 53 | "source": [ 54 | "# Apply custon style to notebook\n", 55 | "from IPython.core.display import HTML\n", 56 | "import pathlib\n", 57 | "styles_path = pathlib.Path(pathlib.Path().absolute(), \"style\", \"style.css\")\n", 58 | "styles = open(styles_path, \"r\").read()\n", 59 | "HTML(f\"\")" 60 | ] 61 | }, 62 | { 63 | "attachments": {}, 64 | "cell_type": "markdown", 65 | "metadata": {}, 66 | "source": [ 67 | "_Note: This notebook must be run locally. Due to how the Swift Simulator operates, this notebook will not run on Google Colab_" 68 | ] 69 | }, 70 | { 71 | "attachments": {}, 72 | "cell_type": "markdown", 73 | "metadata": {}, 74 | "source": [ 75 | "# 4.0 Null-Space Projection for Motion Control\n", 76 | "\n", 77 | "$\\large{\\text{Manipulator Differential Kinematics}} \\\\ \\large{\\text{Part II: Acceleration and Advanced Applications}}$\n", 78 | "\n", 79 | "$\\text{By Jesse Haviland and Peter Corke}$\n", 80 | "\n", 81 | "
\n", 82 | "\n", 83 | "The sections of the Tutorial paper related to this notebook are listed next to each contents entry.\n", 84 | "It is beneficial to read these sections of the paper before attempting the notebook Section.\n", 85 | "\n", 86 | "### Contents\n", 87 | "\n", 88 | "[4.1 Swift and Robot Setup](#swift)\n", 89 | "\n", 90 | "[4.2 Null-Space Projection](#nsp)\n", 91 | "* Advanced Velocity Control\n", 92 | " * Null-space Projection\n", 93 | "\n", 94 | "[4.3 Manipulability Maximising](#mm)\n", 95 | "* Advanced Velocity Control\n", 96 | " * Null-space Projection\n" 97 | ] 98 | }, 99 | { 100 | "cell_type": "code", 101 | "execution_count": 2, 102 | "metadata": {}, 103 | "outputs": [], 104 | "source": [ 105 | "# We will do the imports required for this notebook here\n", 106 | "\n", 107 | "# numpy provides import array and linear algebra utilities\n", 108 | "import numpy as np\n", 109 | "\n", 110 | "# the robotics toolbox provides robotics specific functionality\n", 111 | "import roboticstoolbox as rtb\n", 112 | "\n", 113 | "# spatial math provides objects for representing transformations\n", 114 | "import spatialmath as sm\n", 115 | "\n", 116 | "# swift is a lightweight browser-based simulator which comes eith the toolbox\n", 117 | "from swift import Swift\n", 118 | "\n", 119 | "# spatialgeometry is a utility package for dealing with geometric objects\n", 120 | "import spatialgeometry as sg" 121 | ] 122 | }, 123 | { 124 | "attachments": {}, 125 | "cell_type": "markdown", 126 | "metadata": {}, 127 | "source": [ 128 | "
\n", 129 | "\n", 130 | "\n", 131 | "\n", 132 | "### 4.1 Swift and Robot Setup\n", 133 | "---" 134 | ] 135 | }, 136 | { 137 | "attachments": {}, 138 | "cell_type": "markdown", 139 | "metadata": {}, 140 | "source": [ 141 | "In this notebook we will be using the Robotics Toolbox for Python and Swift to simulate our motion controllers. Check out Part 1 Notebook 3 for an introduction to these packages." 142 | ] 143 | }, 144 | { 145 | "cell_type": "code", 146 | "execution_count": 3, 147 | "metadata": {}, 148 | "outputs": [ 149 | { 150 | "data": { 151 | "text/html": [ 152 | "\n", 153 | " \n", 161 | " " 162 | ], 163 | "text/plain": [ 164 | "" 165 | ] 166 | }, 167 | "metadata": {}, 168 | "output_type": "display_data" 169 | }, 170 | { 171 | "data": { 172 | "text/plain": [ 173 | "2" 174 | ] 175 | }, 176 | "execution_count": 3, 177 | "metadata": {}, 178 | "output_type": "execute_result" 179 | } 180 | ], 181 | "source": [ 182 | "# Make the environment\n", 183 | "env = Swift()\n", 184 | "\n", 185 | "# Launch the simulator\n", 186 | "# The realtime flag will ask the simulator to simulate as close as\n", 187 | "# possible to realtime as apposed to as fast as possible\n", 188 | "env.launch(realtime=True, browser=\"notebook\")\n", 189 | "env.set_camera_pose([1.3, 0, 0.4], [0, 0, 0.3])\n", 190 | "\n", 191 | "# Make a panda robot\n", 192 | "panda = rtb.models.Panda()\n", 193 | "\n", 194 | "# Set the joint coordinates to qr\n", 195 | "panda.q = panda.qr\n", 196 | "\n", 197 | "# We can then add our robot to the simulator envionment\n", 198 | "env.add(panda)\n", 199 | "\n", 200 | "# end-effector axes\n", 201 | "ee_axes = sg.Axes(0.1)\n", 202 | "\n", 203 | "# goal axes\n", 204 | "goal_axes = sg.Axes(0.1)\n", 205 | "\n", 206 | "# Add the axes to the environment\n", 207 | "env.add(ee_axes)\n", 208 | "env.add(goal_axes) " 209 | ] 210 | }, 211 | { 212 | "attachments": {}, 213 | "cell_type": "markdown", 214 | "metadata": {}, 215 | "source": [ 216 | "
\n", 217 | "\n", 218 | "\n", 219 | "\n", 220 | "### 4.2 Null-Space Projection\n", 221 | "---" 222 | ] 223 | }, 224 | { 225 | "attachments": {}, 226 | "cell_type": "markdown", 227 | "metadata": {}, 228 | "source": [ 229 | "Many modern manipulators are redundant — they have more than 6 degrees of freedom. We can exploit this redundancy by having the robot optimize a performance measure while still achieving the original goal. In this Section, we start with the resolved-rate motion control (RRMC) algorithm explained in Part 1 Notebook 3\n", 230 | "\n", 231 | "\\begin{equation*}\n", 232 | " \\dot{\\bf{q}} = \\bf{J}(\\bf{q})^{+} \\ \\bf{\\nu}.\n", 233 | "\\end{equation*}" 234 | ] 235 | }, 236 | { 237 | "attachments": {}, 238 | "cell_type": "markdown", 239 | "metadata": {}, 240 | "source": [ 241 | "The Jacobian of a redundant manipulator has a null space. Any joint velocity vector which is a linear combination of the manipulator Jacobian's null-space will result in no end-effector motion ($\\bf{\\nu} = 0$). We can augment RRMC to add a joint velocity vector $\\dot{\\bf{q}}_{null}$ which can be projected into the null-space resulting in zero end-effector spatial velocity\n", 242 | "\n", 243 | "\\begin{equation*}\n", 244 | " \\dot{\\bf{q}} =\n", 245 | " \\underbrace{\n", 246 | " \\bf{J}(\\bf{q})^{+} \\ \\bf{\\nu}\n", 247 | " }_{\\text{end-effector \\ motion}} + \\ \\\n", 248 | " \\underbrace{\n", 249 | " \\left(\n", 250 | " \\bf{1}_n - \\bf{J}(\\bf{q})^{+} \\bf{J}(\\bf{q})\n", 251 | " \\right)\n", 252 | " \\dot{\\bf{q}}_\\text{null}\n", 253 | " }_{\\text{null-space \\ motion}}\n", 254 | "\\end{equation*}\n", 255 | "\n", 256 | "where $\\bf{1}_n \\in \\mathbb{R}^{n \\times n}$ is an identity matrix, and $\\dot{\\bf{q}}_\\text{null}$ is the desired joint velocities for the null-space motion." 257 | ] 258 | }, 259 | { 260 | "attachments": {}, 261 | "cell_type": "markdown", 262 | "metadata": {}, 263 | "source": [ 264 | "We can set $\\dot{\\bf{q}}_\\text{null}$ to be the gradient of any scalar performance measure $\\gamma(\\bf{q})$ -- the performance measure must be a differentiable function of the joint coordinates $\\bf{q}$.\n", 265 | "\n", 266 | "Lets make a Python method to do this" 267 | ] 268 | }, 269 | { 270 | "cell_type": "code", 271 | "execution_count": 4, 272 | "metadata": {}, 273 | "outputs": [], 274 | "source": [ 275 | "def null_project(robot, q, qnull, ev, λ):\n", 276 | " \"\"\"\n", 277 | " Calculates the required joint velocities qd to achieve the desired\n", 278 | " end-effector velocity ev while projecting the null-space motion qnull\n", 279 | " into the null-space of the jacobian of the robot.\n", 280 | "\n", 281 | " robot: a Robot object (must be redundant with robot.n > 6)\n", 282 | " q: the robots current joint coordinates\n", 283 | " qnull: the null-space motion to be projected into the solution\n", 284 | " ev: the desired end-effector velocity (expressed in the base-frame\n", 285 | " of the robot)\n", 286 | " λ: a gain to apply to the null-space motion\n", 287 | "\n", 288 | " Note: If you would like to express ev in the end-effector frame,\n", 289 | " change the `jacob0` below to `jacobe`\n", 290 | " \"\"\"\n", 291 | "\n", 292 | " # Calculate the base-frame manipulator Jacobian\n", 293 | " J0 = robot.jacob0(q)\n", 294 | "\n", 295 | " # Calculate the pseudoinverse of the base-frame manipulator Jacobian\n", 296 | " J0_pinv = np.linalg.pinv(J0)\n", 297 | "\n", 298 | " # Calculate the joint velocity vector according to the equation above\n", 299 | " qd = J0_pinv @ ev + (1.0 / λ) * (np.eye(robot.n) - J0_pinv @ J0) @ qnull.reshape(robot.n,)\n", 300 | "\n", 301 | " return qd" 302 | ] 303 | }, 304 | { 305 | "attachments": {}, 306 | "cell_type": "markdown", 307 | "metadata": {}, 308 | "source": [ 309 | "
\n", 310 | "\n", 311 | "\n", 312 | "\n", 313 | "### 4.3 Manipulability Maximising\n", 314 | "---" 315 | ] 316 | }, 317 | { 318 | "attachments": {}, 319 | "cell_type": "markdown", 320 | "metadata": {}, 321 | "source": [ 322 | "Park [4] proposed using the gradient of the Yoshikawa manipulability index as $\\dot{\\bf{q}}_\\text{null}$. As detailed in Part 1 of this Tutorial, the manipulability index is calculated as\n", 323 | "\n", 324 | "\\begin{align*}\n", 325 | " m(\\bf{q}) = \\sqrt{\n", 326 | " \\text{det}\n", 327 | " \\Big(\n", 328 | " \\tilde{\\bf{J}}(\\bf{q})\n", 329 | " \\tilde{\\bf{J}}(\\bf{q})^\\top\n", 330 | " \\Big)\n", 331 | " }\n", 332 | "\\end{align*}\n", 333 | "\n", 334 | "where $\\tilde{\\bf{J}}(\\bf{q}) \\in \\mathbb{R}^{3 \\times n}$ is either the translational or rotational rows of $\\bf{J}(\\bf{q})$ causing $m(\\bf{q})$ to describe the corresponding component of manipulability.\n", 335 | "\n", 336 | "Taking the time derivative of $m(\\bf{q})$ using the chain rule\n", 337 | "\\begin{align*}\n", 338 | " \\frac{\\text{d} \\ m(t)}\n", 339 | " {\\text{d} t} = \n", 340 | " \\dfrac{1}\n", 341 | " {2m(t)} \n", 342 | " \\frac{\\text{d} \\ \\text{det} \\left( \\tilde{\\bf{J}}(\\bf{q}) \\tilde{\\bf{J}}(\\bf{q})^\\top \\right)}\n", 343 | " {\\text{d} t} \n", 344 | "\\end{align*}\n", 345 | "\n", 346 | "we can write this as\n", 347 | "\n", 348 | "\\begin{align*}\n", 349 | " \\dot{m}\n", 350 | " &=\n", 351 | " \\bf{J}_m^\\top(\\bf{q}) \\ \\dot{\\bf{q}}\n", 352 | "\\end{align*}\n", 353 | "\n", 354 | "where\n", 355 | "\n", 356 | "\\begin{equation*}\n", 357 | " \\bf{J}_m^\\top(\\bf{q})\n", 358 | " =\n", 359 | " \\begin{pmatrix}\n", 360 | " m \\, \\text{vec} \\left( \\tilde{\\bf{J}}(\\bf{q}) \\tilde{\\bf{H}}(\\bf{q})_1^\\top \\right)^\\top \n", 361 | " \\text{vec} \\left( (\\tilde{\\bf{J}}(\\bf{q})\\tilde{\\bf{J}}(\\bf{q})^\\top)^{-1} \\right) \\\\\n", 362 | " m \\, \\text{vec} \\left( \\tilde{\\bf{J}}(\\bf{q}) \\tilde{\\bf{H}}(\\bf{q})_2^\\top \\right)^\\top \n", 363 | " \\text{vec} \\left( (\\tilde{\\bf{J}}(\\bf{q})\\tilde{\\bf{J}}(\\bf{q})^\\top)^{-1} \\right) \\\\\n", 364 | " \\vdots \\\\\n", 365 | " m \\, \\text{vec} \\left( \\tilde{\\bf{J}}(\\bf{q}) \\tilde{\\bf{H}}(\\bf{q})_n^\\top \\right)^\\top \n", 366 | " \\text{vec} \\left( (\\tilde{\\bf{J}}(\\bf{q})\\tilde{\\bf{J}}(\\bf{q})^\\top)^{-1} \\right) \\\\\n", 367 | " \\end{pmatrix}\n", 368 | "\\end{equation*}\n", 369 | "\n", 370 | "is the manipulability Jacobian $\\bf{J}^\\top_m \\in \\R^n$ and where the vector operation $\\text{vec}(\\cdot) : \\R^{a \\times b} \\rightarrow \\R^{ab}$ converts a matrix column-wise into a vector,\n", 371 | "and $\\tilde{\\bf{H}}_i \\in \\R^{3 \\times n}$ is the translational or rotational component (matching the choice of $\\tilde{\\bf{J}}(\\bf{q})$)\n", 372 | "of $\\bf{H}_i \\in \\R^{6 \\times n}$ which is the $i^{th}$ component of the manipulator Hessian tensor $\\bf{H} \\in \\R^{n \\times 6 \\times n}$.\n", 373 | "\n", 374 | "The complete equation proposed by Park is\n", 375 | "\n", 376 | "\\begin{equation*}\n", 377 | " \\dot{\\bf{q}} =\n", 378 | " \\bf{J}(\\bf{q})^{+} \\ \\bf{\\nu} +\n", 379 | " \\frac{1}{\\lambda} \n", 380 | " \\Big(\n", 381 | " \\left(\n", 382 | " \\bf{1}_n - \\bf{J}(\\bf{q})^{+} \\bf{J}(\\bf{q})\n", 383 | " \\right)\n", 384 | " \\bf{J_m}(\\bf{q})\n", 385 | " \\Big)\n", 386 | "\\end{equation*}\n", 387 | "\n", 388 | "where $\\lambda$ is a gain which scales the magnitude of the null-space velocities. This equation will choose joint velocities $\\dot{\\bf{q}}$ which will achieve the end-effector spatial velocity $\\bf{\\nu}$ while also improving the translational and/or rotational manipulability of the robot.\n", 389 | "\n", 390 | "Lets make this in Python\n", 391 | "\n" 392 | ] 393 | }, 394 | { 395 | "cell_type": "code", 396 | "execution_count": 5, 397 | "metadata": {}, 398 | "outputs": [], 399 | "source": [ 400 | "def jacobm(robot, q, axes):\n", 401 | " \"\"\"\n", 402 | " Calculates the manipulability Jacobian. This measure relates the rate\n", 403 | " of change of the manipulability to the joint velocities of the robot.\n", 404 | "\n", 405 | " q: The joint angles/configuration of the robot\n", 406 | " axes: A boolean list which correspond with the Cartesian axes to\n", 407 | " find the manipulability Jacobian of (6 boolean values in a list)\n", 408 | "\n", 409 | " returns the manipulability Jacobian\n", 410 | " \"\"\"\n", 411 | "\n", 412 | " # Calculate the base-frame manipulator Jacobian\n", 413 | " J0 = robot.jacob0(q)\n", 414 | "\n", 415 | " # only keep the selected axes of J\n", 416 | " J0 = J0[axes, :]\n", 417 | "\n", 418 | " # Calculate the base-frame manipulator Hessian\n", 419 | " H0 = robot.hessian0(q)\n", 420 | "\n", 421 | " # only keep the selected axes of H\n", 422 | " H0 = H0[:, axes, :]\n", 423 | "\n", 424 | " # Calculate the manipulability of the robot\n", 425 | " manipulability = np.sqrt(np.linalg.det(J0 @ J0.T))\n", 426 | "\n", 427 | " # Calculate component of the Jacobian\n", 428 | " b = np.linalg.inv(J0 @ J0.T)\n", 429 | "\n", 430 | " # Allocate manipulability Jacobian\n", 431 | " Jm = np.zeros((robot.n, 1))\n", 432 | "\n", 433 | " # Calculate manipulability Jacobian\n", 434 | " for i in range(robot.n):\n", 435 | " c = J0 @ H0[i, :, :].T\n", 436 | " Jm[i, 0] = manipulability * (c.flatten(\"F\")).T @ b.flatten(\"F\")\n", 437 | "\n", 438 | " return Jm" 439 | ] 440 | }, 441 | { 442 | "attachments": {}, 443 | "cell_type": "markdown", 444 | "metadata": {}, 445 | "source": [ 446 | "As with RRMC, Park's method will provide the joint velocities for a desired end-effector velocity. As we did in Part 1, we can employ this in a position-based servoing (PBS) controller to get the end-effector to travel towards some desired pose. The PBS scheme is\n", 447 | "\n", 448 | "\\begin{align*}\n", 449 | " \\bf{\\nu} = \\bf{k} \\bf{e}\n", 450 | "\\end{align*}\n", 451 | "\n", 452 | "where $\\bf{\\nu}$ is the desired end-effector spatial velocity to be used." 453 | ] 454 | }, 455 | { 456 | "attachments": {}, 457 | "cell_type": "markdown", 458 | "metadata": {}, 459 | "source": [ 460 | "Lets make a try the PBS scheme while maximising the translational manipulability" 461 | ] 462 | }, 463 | { 464 | "cell_type": "code", 465 | "execution_count": 6, 466 | "metadata": {}, 467 | "outputs": [ 468 | { 469 | "data": { 470 | "text/html": [ 471 | "\n", 472 | " \n", 480 | " " 481 | ], 482 | "text/plain": [ 483 | "" 484 | ] 485 | }, 486 | "metadata": {}, 487 | "output_type": "display_data" 488 | } 489 | ], 490 | "source": [ 491 | "# Make a new environment and add objects\n", 492 | "env = Swift()\n", 493 | "env.launch(realtime=True, browser=\"notebook\")\n", 494 | "env.set_camera_pose([1.3, 0, 0.4], [0, 0, 0.3])\n", 495 | "env.add(panda)\n", 496 | "env.add(ee_axes)\n", 497 | "env.add(goal_axes) \n", 498 | "\n", 499 | "# Change the robot configuration to a ready position\n", 500 | "panda.q = [0.21, -0.03, 0.35, -1.90, -0.04, 1.96, 1.36]\n", 501 | "\n", 502 | "# Only the translation axes\n", 503 | "trans_axes = [True, True, True, False, False, False]\n", 504 | "\n", 505 | "# Only the rotation aces\n", 506 | "rot_axes = [False, False, False, True, True, True]\n", 507 | "\n", 508 | "# All axes\n", 509 | "all_axes = [True, True, True, True, True, True]\n", 510 | "\n", 511 | "# Step the sim to view the robot in this configuration\n", 512 | "env.step(0)\n", 513 | "\n", 514 | "# A variable to specify when to break the loop\n", 515 | "arrived = False\n", 516 | "\n", 517 | "# Specify our timestep\n", 518 | "dt = 0.05\n", 519 | "\n", 520 | "Tep = (\n", 521 | " panda.fkine(panda.q)\n", 522 | " * sm.SE3.Tx(-0.1)\n", 523 | " * sm.SE3.Ty(0.6)\n", 524 | " * sm.SE3.Tz(0.4)\n", 525 | ")\n", 526 | "Tep = Tep.A\n", 527 | "\n", 528 | "# Set the goal axes to Tep\n", 529 | "goal_axes.T = Tep\n", 530 | "\n", 531 | "# Set the gain on the manipulability maximisation\n", 532 | "λ = 0.1\n", 533 | "\n", 534 | "# Specify the gain for the p_servo method\n", 535 | "kt = 1.0\n", 536 | "kr = 1.3\n", 537 | "k = np.array([kt, kt, kt, kr, kr, kr])\n", 538 | "\n", 539 | "# Run the simulation until the robot arrives at the goal\n", 540 | "while not arrived:\n", 541 | "\n", 542 | " # Work out the base frame manipulator Jacobian using the current robot configuration\n", 543 | " J = panda.jacob0(panda.q)\n", 544 | "\n", 545 | " # Calculate the desired null-space motion\n", 546 | " qnull = jacobm(panda, panda.q, trans_axes)\n", 547 | "\n", 548 | " # The end-effector pose of the panda (using .A to get a numpy array instead of an SE3 object)\n", 549 | " Te = panda.fkine(panda.q).A\n", 550 | "\n", 551 | " # Calculate the required end-effector velocity and whether the robot has arrived\n", 552 | " ev, arrived = rtb.p_servo(Te, Tep, gain=k, threshold=0.001, method=\"angle-axis\")\n", 553 | "\n", 554 | " # Calculate the required joint velocities and apply to the robot\n", 555 | " panda.qd = null_project(panda, panda.q, qnull, ev, λ)\n", 556 | "\n", 557 | " # Update the ee axes\n", 558 | " ee_axes.T = Te\n", 559 | "\n", 560 | " # Step the simulator by dt seconds\n", 561 | " env.step(dt)" 562 | ] 563 | }, 564 | { 565 | "attachments": {}, 566 | "cell_type": "markdown", 567 | "metadata": {}, 568 | "source": [ 569 | "Lets make a try again while maximising the angular manipulability" 570 | ] 571 | }, 572 | { 573 | "cell_type": "code", 574 | "execution_count": 7, 575 | "metadata": {}, 576 | "outputs": [ 577 | { 578 | "data": { 579 | "text/html": [ 580 | "\n", 581 | " \n", 589 | " " 590 | ], 591 | "text/plain": [ 592 | "" 593 | ] 594 | }, 595 | "metadata": {}, 596 | "output_type": "display_data" 597 | } 598 | ], 599 | "source": [ 600 | "# Make a new environment and add objects\n", 601 | "env = Swift()\n", 602 | "env.launch(realtime=True, browser=\"notebook\")\n", 603 | "env.set_camera_pose([1.3, 0, 0.4], [0, 0, 0.3])\n", 604 | "env.add(panda)\n", 605 | "env.add(ee_axes)\n", 606 | "env.add(goal_axes) \n", 607 | "\n", 608 | "# Change the robot configuration to a ready position\n", 609 | "panda.q = [0.21, -0.03, 0.35, -1.90, -0.04, 1.96, 1.36]\n", 610 | "\n", 611 | "# Step the sim to view the robot in this configuration\n", 612 | "env.step(0)\n", 613 | "\n", 614 | "# A variable to specify when to break the loop\n", 615 | "arrived = False\n", 616 | "\n", 617 | "# Run the simulation until the robot arrives at the goal\n", 618 | "while not arrived:\n", 619 | "\n", 620 | " # Work out the base frame manipulator Jacobian using the current robot configuration\n", 621 | " J = panda.jacob0(panda.q)\n", 622 | "\n", 623 | " # Calculate the desired null-space motion\n", 624 | " qnull = jacobm(panda, panda.q, rot_axes)\n", 625 | "\n", 626 | " # The end-effector pose of the panda (using .A to get a numpy array instead of an SE3 object)\n", 627 | " Te = panda.fkine(panda.q).A\n", 628 | "\n", 629 | " # Calculate the required end-effector velocity and whether the robot has arrived\n", 630 | " ev, arrived = rtb.p_servo(Te, Tep, gain=k, threshold=0.001, method=\"angle-axis\")\n", 631 | "\n", 632 | " # Calculate the required joint velocities and apply to the robot\n", 633 | " panda.qd = null_project(panda, panda.q, qnull, ev, λ)\n", 634 | "\n", 635 | " # Update the ee axes\n", 636 | " ee_axes.T = Te\n", 637 | "\n", 638 | " # Step the simulator by dt seconds\n", 639 | " env.step(dt)" 640 | ] 641 | } 642 | ], 643 | "metadata": { 644 | "interpreter": { 645 | "hash": "528815e074ebcdb9b34bcb695d4aa9d425bdb2cc6656b4ca45050b51a4125937" 646 | }, 647 | "kernelspec": { 648 | "display_name": "Python 3.8.10 ('rtb')", 649 | "language": "python", 650 | "name": "python3" 651 | }, 652 | "language_info": { 653 | "codemirror_mode": { 654 | "name": "ipython", 655 | "version": 3 656 | }, 657 | "file_extension": ".py", 658 | "mimetype": "text/x-python", 659 | "name": "python", 660 | "nbconvert_exporter": "python", 661 | "pygments_lexer": "ipython3", 662 | "version": "3.11.0" 663 | }, 664 | "orig_nbformat": 4 665 | }, 666 | "nbformat": 4, 667 | "nbformat_minor": 2 668 | } 669 | -------------------------------------------------------------------------------- /Part 2/5 Quadratic Programming for Motion Control.ipynb: -------------------------------------------------------------------------------- 1 | { 2 | "cells": [ 3 | { 4 | "cell_type": "code", 5 | "execution_count": 1, 6 | "metadata": {}, 7 | "outputs": [ 8 | { 9 | "data": { 10 | "text/html": [ 11 | "" 43 | ], 44 | "text/plain": [ 45 | "" 46 | ] 47 | }, 48 | "execution_count": 1, 49 | "metadata": {}, 50 | "output_type": "execute_result" 51 | } 52 | ], 53 | "source": [ 54 | "# Apply custon style to notebook\n", 55 | "from IPython.core.display import HTML\n", 56 | "import pathlib\n", 57 | "styles_path = pathlib.Path(pathlib.Path().absolute(), \"style\", \"style.css\")\n", 58 | "styles = open(styles_path, \"r\").read()\n", 59 | "HTML(f\"\")" 60 | ] 61 | }, 62 | { 63 | "attachments": {}, 64 | "cell_type": "markdown", 65 | "metadata": {}, 66 | "source": [ 67 | "_Note: This notebook must be run locally. Due to how the Swift Simulator operates, this notebook will not run on Google Colab_" 68 | ] 69 | }, 70 | { 71 | "attachments": {}, 72 | "cell_type": "markdown", 73 | "metadata": {}, 74 | "source": [ 75 | "# 5.0 Quadratic Programming for Motion Control\n", 76 | "\n", 77 | "$\\large{\\text{Manipulator Differential Kinematics}} \\\\ \\large{\\text{Part II: Acceleration and Advanced Applications}}$\n", 78 | "\n", 79 | "$\\text{By Jesse Haviland and Peter Corke}$\n", 80 | "\n", 81 | "
\n", 82 | "\n", 83 | "The sections of the Tutorial paper related to this notebook are listed next to each contents entry.\n", 84 | "It is beneficial to read these sections of the paper before attempting the notebook Section.\n", 85 | "\n", 86 | "### Contents\n", 87 | "\n", 88 | "[5.1 Swift and Robot Setup](#swift)\n", 89 | "\n", 90 | "[5.2 Basic Quadratic Programming](#bqp)\n", 91 | "* Advanced Velocity Control\n", 92 | " * Null-space Projection\n", 93 | " * Quadratic Programming\n", 94 | "\n", 95 | "[5.3 Velocity Dampers](#vd)\n", 96 | "* Advanced Velocity Control\n", 97 | " * Null-space Projection\n", 98 | " * Quadratic Programming\n", 99 | "\n", 100 | "[5.4 Adding Slack](#slack)\n", 101 | "* Advanced Velocity Control\n", 102 | " * Null-space Projection\n", 103 | " * Quadratic Programming" 104 | ] 105 | }, 106 | { 107 | "cell_type": "code", 108 | "execution_count": 2, 109 | "metadata": {}, 110 | "outputs": [], 111 | "source": [ 112 | "# We will do the imports required for this notebook here\n", 113 | "\n", 114 | "# numpy provides import array and linear algebra utilities\n", 115 | "import numpy as np\n", 116 | "\n", 117 | "# the robotics toolbox provides robotics specific functionality\n", 118 | "import roboticstoolbox as rtb\n", 119 | "\n", 120 | "# spatial math provides objects for representing transformations\n", 121 | "import spatialmath as sm\n", 122 | "\n", 123 | "# swift is a lightweight browser-based simulator which comes eith the toolbox\n", 124 | "from swift import Swift\n", 125 | "\n", 126 | "# spatialgeometry is a utility package for dealing with geometric objects\n", 127 | "import spatialgeometry as sg\n", 128 | "\n", 129 | "# this package provides several solvers for solving quadratic programmes\n", 130 | "import qpsolvers as qp" 131 | ] 132 | }, 133 | { 134 | "attachments": {}, 135 | "cell_type": "markdown", 136 | "metadata": {}, 137 | "source": [ 138 | "
\n", 139 | "\n", 140 | "\n", 141 | "\n", 142 | "### 5.1 Swift and Robot Setup\n", 143 | "---" 144 | ] 145 | }, 146 | { 147 | "attachments": {}, 148 | "cell_type": "markdown", 149 | "metadata": {}, 150 | "source": [ 151 | "In this notebook we will be using the Robotics Toolbox for Python and Swift to simulate our motion controllers. Check out Part 1 Notebook 3 for an introduction to these packages." 152 | ] 153 | }, 154 | { 155 | "cell_type": "code", 156 | "execution_count": 3, 157 | "metadata": {}, 158 | "outputs": [ 159 | { 160 | "data": { 161 | "text/html": [ 162 | "\n", 163 | " \n", 171 | " " 172 | ], 173 | "text/plain": [ 174 | "" 175 | ] 176 | }, 177 | "metadata": {}, 178 | "output_type": "display_data" 179 | }, 180 | { 181 | "data": { 182 | "text/plain": [ 183 | "2" 184 | ] 185 | }, 186 | "execution_count": 3, 187 | "metadata": {}, 188 | "output_type": "execute_result" 189 | } 190 | ], 191 | "source": [ 192 | "# Make the environment\n", 193 | "env = Swift()\n", 194 | "\n", 195 | "# Launch the simulator\n", 196 | "# The realtime flag will ask the simulator to simulate as close as\n", 197 | "# possible to realtime as apposed to as fast as possible\n", 198 | "env.launch(realtime=True, browser=\"notebook\")\n", 199 | "\n", 200 | "# Make a panda robot\n", 201 | "panda = rtb.models.Panda()\n", 202 | "\n", 203 | "# Set the joint coordinates to qr\n", 204 | "panda.q = panda.qr\n", 205 | "\n", 206 | "# We can then add our robot to the simulator envionment\n", 207 | "env.add(panda)\n", 208 | "\n", 209 | "# end-effector axes\n", 210 | "ee_axes = sg.Axes(0.1)\n", 211 | "\n", 212 | "# goal axes\n", 213 | "goal_axes = sg.Axes(0.1)\n", 214 | "\n", 215 | "# Add the axes to the environment\n", 216 | "env.add(ee_axes)\n", 217 | "env.add(goal_axes) " 218 | ] 219 | }, 220 | { 221 | "cell_type": "code", 222 | "execution_count": 4, 223 | "metadata": {}, 224 | "outputs": [], 225 | "source": [ 226 | "# Make a variable for the upper and lower limits of the robot\n", 227 | "qd_lb = -20.0 * np.ones(panda.n)\n", 228 | "qd_ub = 20.0 * np.ones(panda.n)" 229 | ] 230 | }, 231 | { 232 | "attachments": {}, 233 | "cell_type": "markdown", 234 | "metadata": {}, 235 | "source": [ 236 | "
\n", 237 | "\n", 238 | "\n", 239 | "\n", 240 | "### 4.2 Basic Quadratic Programming\n", 241 | "---" 242 | ] 243 | }, 244 | { 245 | "attachments": {}, 246 | "cell_type": "markdown", 247 | "metadata": {}, 248 | "source": [ 249 | "In this Notebook, we are going redefine our motion controllers as a quadratic programming (QP) optimisation problem rather than a matrix equation. In general, a constrained QP is formulated as\n", 250 | "\n", 251 | "\\begin{align*} \n", 252 | " \\min_x \\quad f_o(\\bf{x}) &= \\frac{1}{2} \\bf{x}^\\top \\bf{Q} \\bf{x}+ \\bf{c}^\\top \\bf{x}, \\\\ \n", 253 | " \\text{subject to} \\quad \\bf{A}_1 \\bf{x} &= \\bf{b}_1, \\\\\n", 254 | " \\bf{A}_2 \\bf{x} &\\leq \\bf{b}_2, \\\\\n", 255 | " \\bf{g} &\\leq \\bf{x} \\leq \\bf{h}. \n", 256 | "\\end{align*}\n", 257 | "\n", 258 | "where $f_o(\\bf{x})$ is the objective function which is subject to the following equality and inequality constraints, and $\\bf{h}$ and $\\bf{g}$ represent the upper and lower bounds of $\\bf{x}$. A quadratic program is strictly convex when the matrix $\\bf{Q}$ is positive definite. This framework allows us to solve the same problems as above, but with additional flexibility.\n", 259 | "\n", 260 | "We can rewrite RRMC (see Part 1 Notebook 3) as a QP\n", 261 | "\n", 262 | "\\begin{align*} \n", 263 | "\t\\min_{\\dot{\\bf{q}}} \\quad f_o(\\dot{\\bf{q}}) \n", 264 | "\t&= \n", 265 | " \\frac{1}{2} \n", 266 | " \\dot{\\bf{q}}^\\top \n", 267 | " \\bf{1}_n \n", 268 | " \\dot{\\bf{q}}, \\\\\n", 269 | "\t\\text{subject to} \\quad\n", 270 | "\t\\bf{J}(\\bf{q}) \\dot{\\bf{q}} &= \\bf{\\nu},\n", 271 | "\t \\\\\n", 272 | "\t\\dot{\\bf{q}}^- &\\leq \\dot{\\bf{q}} \\leq \\dot{\\bf{q}}^+ \n", 273 | "\\end{align*}\n", 274 | "\n", 275 | "where $\\bf{1}_n \\in \\mathbb{R}^{n \\times n}$ is an identity matrix, and we have imposed $\\dot{\\bf{q}}^-$ and $\\dot{\\bf{q}}^+$ as upper and lower joint-velocity limits. If the robot has more degrees-of-freedom than necessary to reach its entire task space, the QP will achieve the desired end-effector velocity with the minimum joint-velocity norm (the same result as the psuedoinverse solution).\n", 276 | "\n", 277 | "Lets test this out in Python" 278 | ] 279 | }, 280 | { 281 | "cell_type": "code", 282 | "execution_count": 5, 283 | "metadata": {}, 284 | "outputs": [ 285 | { 286 | "data": { 287 | "text/html": [ 288 | "\n", 289 | " \n", 297 | " " 298 | ], 299 | "text/plain": [ 300 | "" 301 | ] 302 | }, 303 | "metadata": {}, 304 | "output_type": "display_data" 305 | } 306 | ], 307 | "source": [ 308 | "# Make a new environment and add objects\n", 309 | "env = Swift()\n", 310 | "env.launch(realtime=True, browser=\"notebook\")\n", 311 | "env.set_camera_pose([1.3, 0, 0.4], [0, 0, 0.3])\n", 312 | "env.add(panda)\n", 313 | "env.add(ee_axes)\n", 314 | "env.add(goal_axes)\n", 315 | "\n", 316 | "# Change the robot configuration to a ready position\n", 317 | "panda.q = [0.21, -0.03, 0.35, -1.90, -0.04, 1.96, 1.36]\n", 318 | "\n", 319 | "# A variable to specify when to break the loop\n", 320 | "arrived = False\n", 321 | "\n", 322 | "# Specify the gain for the p_servo method\n", 323 | "kt = 1.0\n", 324 | "kr = 1.3\n", 325 | "k = np.array([kt, kt, kt, kr, kr, kr])\n", 326 | "\n", 327 | "# Specify our timestep\n", 328 | "dt = 0.05\n", 329 | "\n", 330 | "Tep = (\n", 331 | " panda.fkine(panda.q)\n", 332 | " * sm.SE3.Tx(-0.1)\n", 333 | " * sm.SE3.Ty(0.6)\n", 334 | " * sm.SE3.Tz(0.4)\n", 335 | ")\n", 336 | "Tep = Tep.A\n", 337 | "\n", 338 | "# Set the goal axes to Tep\n", 339 | "goal_axes.T = Tep\n", 340 | "\n", 341 | "# The quadratic component of the objective function\n", 342 | "Q = np.eye(panda.n)\n", 343 | "\n", 344 | "# Linear component of objective function\n", 345 | "c = np.ones(panda.n)\n", 346 | "\n", 347 | "# Run the simulation until the robot arrives at the goal\n", 348 | "while not arrived:\n", 349 | "\n", 350 | " # Work out the base frame manipulator Jacobian using the current robot configuration\n", 351 | " J = panda.jacob0(panda.q)\n", 352 | "\n", 353 | " # The end-effector pose of the panda (using .A to get a numpy array instead of an SE3 object)\n", 354 | " Te = panda.fkine(panda.q).A\n", 355 | "\n", 356 | " # Calculate the required end-effector velocity and whether the robot has arrived\n", 357 | " ev, arrived = rtb.p_servo(Te, Tep, gain=k, threshold=0.001, method=\"angle-axis\")\n", 358 | "\n", 359 | " ### Calculate each component of the quadratic programme\n", 360 | " # The equality contraints\n", 361 | " Aeq = J\n", 362 | " beq = ev.reshape((6,))\n", 363 | "\n", 364 | " # The inequality constraints\n", 365 | " Ain = None\n", 366 | " bin = None\n", 367 | "\n", 368 | " # The lower and upper bounds on the joint velocity\n", 369 | " lb = qd_lb\n", 370 | " ub = qd_ub\n", 371 | "\n", 372 | " # Solve for the joint velocities qd and apply to the robot\n", 373 | " panda.qd = qp.solve_qp(Q, c, Ain, bin, Aeq, beq, lb=lb, ub=ub, solver='quadprog')\n", 374 | "\n", 375 | " # Update the ee axes\n", 376 | " ee_axes.T = Te\n", 377 | "\n", 378 | " # Step the simulator by dt seconds\n", 379 | " env.step(dt)" 380 | ] 381 | }, 382 | { 383 | "attachments": {}, 384 | "cell_type": "markdown", 385 | "metadata": {}, 386 | "source": [ 387 | "We can rewrite Park's controller (see Part 2 Notebook 4) as\n", 388 | "\n", 389 | "\\begin{align*} \n", 390 | " \\min_{\\dot{\\bf{q}}} \\quad f_o(\\dot{\\bf{q}}) \n", 391 | " &= \n", 392 | " \\frac{1}\n", 393 | " {2} \n", 394 | " \\dot{\\bf{q}}^\\top \\lambda_q \\bf{1}_n \\dot{\\bf{q}} - \\lambda_m \\ \\bf{J}_m^\\top(\\bf{q}) \\dot{\\bf{q}}, \\\\ \n", 395 | " \\text{subject to} \\quad \\bf{J}(\\bf{q}) \\dot{\\bf{q}} &= \\bf{\\nu},\n", 396 | "\t \\\\\n", 397 | "\t\\dot{\\bf{q}}^- &\\leq \\dot{\\bf{q}} \\leq \\dot{\\bf{q}}^+ \n", 398 | "\\end{align*}\n", 399 | "\n", 400 | "where the manipulability Jacobian fits into the linear component of the objective function.\n", 401 | "\n", 402 | "Lets implement this below" 403 | ] 404 | }, 405 | { 406 | "cell_type": "code", 407 | "execution_count": 6, 408 | "metadata": {}, 409 | "outputs": [ 410 | { 411 | "data": { 412 | "text/html": [ 413 | "\n", 414 | " \n", 422 | " " 423 | ], 424 | "text/plain": [ 425 | "" 426 | ] 427 | }, 428 | "metadata": {}, 429 | "output_type": "display_data" 430 | } 431 | ], 432 | "source": [ 433 | "# Make a new environment and add objects\n", 434 | "env = Swift()\n", 435 | "env.launch(realtime=True, browser=\"notebook\")\n", 436 | "env.set_camera_pose([1.3, 0, 0.4], [0, 0, 0.3])\n", 437 | "env.add(panda)\n", 438 | "env.add(ee_axes)\n", 439 | "env.add(goal_axes) \n", 440 | "\n", 441 | "# Change the robot configuration to a ready position\n", 442 | "panda.q = [0.21, -0.03, 0.35, -1.90, -0.04, 1.96, 1.36]\n", 443 | "\n", 444 | "# A variable to specify when to break the loop\n", 445 | "arrived = False\n", 446 | "\n", 447 | "# Specify the gain for the p_servo method\n", 448 | "kt = 1.0\n", 449 | "kr = 1.3\n", 450 | "k = np.array([kt, kt, kt, kr, kr, kr])\n", 451 | "\n", 452 | "# Specify our timestep\n", 453 | "dt = 0.05\n", 454 | "\n", 455 | "Tep = (\n", 456 | " panda.fkine(panda.q)\n", 457 | " * sm.SE3.Tx(-0.1)\n", 458 | " * sm.SE3.Ty(0.6)\n", 459 | " * sm.SE3.Tz(0.4)\n", 460 | ")\n", 461 | "Tep = Tep.A\n", 462 | "\n", 463 | "# Set the goal axes to Tep\n", 464 | "goal_axes.T = Tep\n", 465 | "\n", 466 | "# Set the gain on the manipulability maximisation\n", 467 | "λm = 10.0\n", 468 | "\n", 469 | "# Set the gain on the joint velocity norm minimisation\n", 470 | "λq = 1.0\n", 471 | "\n", 472 | "Q = λq * np.eye(panda.n)\n", 473 | "\n", 474 | "# Run the simulation until the robot arrives at the goal\n", 475 | "while not arrived:\n", 476 | "\n", 477 | " # Work out the base frame manipulator Jacobian using the current robot configuration\n", 478 | " J = panda.jacob0(panda.q)\n", 479 | "\n", 480 | " # Calculate the manipulability Jacobian\n", 481 | " Jm = panda.jacobm(panda.q, axes='rot')\n", 482 | "\n", 483 | " # The end-effector pose of the panda (using .A to get a numpy array instead of an SE3 object)\n", 484 | " Te = panda.fkine(panda.q).A\n", 485 | "\n", 486 | " # Calculate the required end-effector velocity and whether the robot has arrived\n", 487 | " ev, arrived = rtb.p_servo(Te, Tep, gain=k, threshold=0.001, method=\"angle-axis\")\n", 488 | "\n", 489 | " ### Calculate each component of the quadratic programme\n", 490 | " # The equality contraints\n", 491 | " Aeq = J\n", 492 | " beq = ev.reshape((6,))\n", 493 | "\n", 494 | " # The inequality constraints\n", 495 | " Ain = None\n", 496 | " bin = None\n", 497 | "\n", 498 | " # Linear component of objective function: the manipulability Jacobian\n", 499 | " c = λm * -Jm.reshape((panda.n,))\n", 500 | "\n", 501 | " # The lower and upper bounds on the joint velocity\n", 502 | " lb = qd_lb\n", 503 | " ub = qd_ub\n", 504 | "\n", 505 | " # Solve for the joint velocities qd and apply to the robot\n", 506 | " panda.qd = qp.solve_qp(Q, c, Ain, bin, Aeq, beq, lb=lb, ub=ub, solver='quadprog')\n", 507 | "\n", 508 | " # Update the ee axes\n", 509 | " ee_axes.T = Te\n", 510 | "\n", 511 | " # Step the simulator by dt seconds\n", 512 | " env.step(dt)" 513 | ] 514 | }, 515 | { 516 | "attachments": {}, 517 | "cell_type": "markdown", 518 | "metadata": {}, 519 | "source": [ 520 | "
\n", 521 | "\n", 522 | "\n", 523 | "\n", 524 | "### 5.3 Velocity Dampers\n", 525 | "---" 526 | ] 527 | }, 528 | { 529 | "attachments": {}, 530 | "cell_type": "markdown", 531 | "metadata": {}, 532 | "source": [ 533 | "We can expand the above with velocity dampers to enable joint-position limit avoidance. Velocity dampers are used to constrain velocities and dampen an input velocity as some limit is approached. The velocity is only damped if it is within some influence distance to the limit. A joint velocity is constrained to prevent joint limits using a velocity damper as\n", 534 | "\n", 535 | "\\begin{align*} \n", 536 | " \\dot{q} \\leq\n", 537 | " \\eta\n", 538 | " \\frac{\\rho - \\rho_s}\n", 539 | " {\\rho_i - \\rho_s}\n", 540 | " \\qquad \\text{if} \\ \\rho < \\rho_i\n", 541 | "\\end{align*}\n", 542 | "\n", 543 | "where $\\rho \\in \\mathbb{R}^+$ is the distance or angle to the nearest joint limit, $\\eta \\in \\mathbb{R}^+$ is a gain which adjusts the aggressiveness of the damper, $\\rho_i$ is the influence distance in which to activate the damper, and $\\rho_s$ is the stopping distance in which the distance $\\rho$ will never be able to reach or enter. We can stack velocity dampers to perform joint position limit avoidance for each joint within a robot, and incorporate it into our QP as an inequality constraint\n", 544 | "\n", 545 | "\\begin{align*} \n", 546 | " \\bf{1}_n\n", 547 | " \\dot{\\bf{q}} &\\leq\n", 548 | " \\eta\n", 549 | " \\begin{pmatrix}\n", 550 | " \\frac{\\rho_0 - \\rho_s}\n", 551 | " {\\rho_i - \\rho_s} \\\\\n", 552 | " \\vdots \\\\\n", 553 | " \\frac{\\rho_n - \\rho_s}\n", 554 | " {\\rho_i - \\rho_s} \n", 555 | " \\end{pmatrix}\n", 556 | "\\end{align*}\n", 557 | "\n", 558 | "where the identity $\\bf{1}_n$ is included to show how the equation fits into the general form $\\bf{A}\\bf{x} \\leq \\bf{b}$ of an inequality constraint.\n", 559 | "\n", 560 | "Lets make a Python method to construct these joint position limit velocity dampers" 561 | ] 562 | }, 563 | { 564 | "cell_type": "code", 565 | "execution_count": 7, 566 | "metadata": {}, 567 | "outputs": [], 568 | "source": [ 569 | "def joint_velocity_damper(robot, ps, pi, gain):\n", 570 | " \"\"\"\n", 571 | " Formulates an inequality contraint which, when optimised for will\n", 572 | " make it impossible for the robot to run into joint limits. Requires\n", 573 | " the joint limits of the robot to be specified.\n", 574 | "\n", 575 | " ps: The minimum angle (in radians) in which the joint is\n", 576 | " allowed to approach to its limit\n", 577 | " pi: The influence angle (in radians) in which the velocity\n", 578 | " damper becomes active\n", 579 | " gain: The gain for the velocity damper\n", 580 | "\n", 581 | " returns: Ain, Bin as the inequality contraints for an qp\n", 582 | " \"\"\"\n", 583 | "\n", 584 | " Ain = np.zeros((robot.n, robot.n))\n", 585 | " Bin = np.zeros(robot.n)\n", 586 | "\n", 587 | " for i in range(robot.n):\n", 588 | " if robot.q[i] - robot.qlim[0, i] <= pi:\n", 589 | " Bin[i] = -gain * (((robot.qlim[0, i] - robot.q[i]) + ps) / (pi - ps))\n", 590 | " Ain[i, i] = -1\n", 591 | " if robot.qlim[1, i] - robot.q[i] <= pi:\n", 592 | " Bin[i] = gain * ((robot.qlim[1, i] - robot.q[i]) - ps) / (pi - ps)\n", 593 | " Ain[i, i] = 1\n", 594 | "\n", 595 | " return Ain, Bin" 596 | ] 597 | }, 598 | { 599 | "attachments": {}, 600 | "cell_type": "markdown", 601 | "metadata": {}, 602 | "source": [ 603 | "
\n", 604 | "\n", 605 | "\n", 606 | "\n", 607 | "### 5.4 Adding Slack\n", 608 | "---" 609 | ] 610 | }, 611 | { 612 | "attachments": {}, 613 | "cell_type": "markdown", 614 | "metadata": {}, 615 | "source": [ 616 | "It is possible that the robot will fail to reach the goal when the constraints create local minima. In such a scenario, the error $\\bf{e}$ in the PBS scheme is no longer decreasing and the robot can no longer progress due to the constraints in the QP.\n", 617 | "\n", 618 | "The methods shown up to this point require a redundant robot, where the number of joints is greater than 6. In [8], extra redundancy was introduced to the QP by relaxing the equality constraint in the QP to allow for intentional deviation from the desired end-effector velocity $\\bf{\\nu}$. The slack has the additional benefit of providing the solver extra flexibility to meet constraints and avoid local minima. The augmented QP is defined as\n", 619 | "\n", 620 | "\\begin{align*} \n", 621 | " \\min_x \\quad f_o(\\bf{x}) &= \\frac{1}{2} \\bf{x}^\\top \\mathcal{Q} \\bf{x}+ \\mathcal{C}^\\top \\bf{x}, \\\\ \n", 622 | " \\text{subject to} \\quad \\mathcal{J} \\bf{x} &= \\bf{\\nu}, \\\\\n", 623 | " \\mathcal{A} \\bf{x} &\\leq \\mathcal{B}, \\\\\n", 624 | " \\bf{x}^- &\\leq \\bf{x} \\leq \\bf{x}^+ \n", 625 | "\\end{align*}\n", 626 | "\n", 627 | "with\n", 628 | "\n", 629 | "\\begin{align*}\n", 630 | " \\bf{x} &= \n", 631 | " \\begin{pmatrix}\n", 632 | " \\dot{\\bf{q}} \\\\ \\bf{\\delta}\n", 633 | " \\end{pmatrix} \\in \\mathbb{R}^{(n+6)} \\\\\n", 634 | " \\mathcal{Q} &=\n", 635 | " \\begin{pmatrix}\n", 636 | " \\lambda_q \\bf{1}_{n} & \\mathbf{0}_{6 \\times 6} \\\\ \\mathbf{0}_{n \\times n} & \\lambda_\\delta \\bf{1}_{6}\n", 637 | " \\end{pmatrix} \\in \\mathbb{R}^{(n+6) \\times (n+6)} \\\\\n", 638 | " \\mathcal{J} &=\n", 639 | " \\begin{pmatrix}\n", 640 | " \\bf{J}(\\bf{q}) & \\bf{1}_{6}\n", 641 | " \\end{pmatrix} \\in \\mathbb{R}^{6 \\times (n+6)} \\\\\n", 642 | " \\mathcal{C} &= \n", 643 | " \\begin{pmatrix}\n", 644 | " \\bf{J}_m \\\\ \\bf{0}_{6 \\times 1}\n", 645 | " \\end{pmatrix} \\in \\mathbb{R}^{(n + 6)} \\\\\n", 646 | " \\mathcal{A} &= \n", 647 | " \\begin{pmatrix}\n", 648 | " \\bf{1}_{n \\times n + 6} \\\\\n", 649 | " \\end{pmatrix} \\in \\mathbb{R}^{(l + n) \\times (n + 6)} \\\\\n", 650 | " \\mathcal{B} &= \n", 651 | " \\eta\n", 652 | " \\begin{pmatrix}\n", 653 | " \\frac{\\rho_0 - \\rho_s}\n", 654 | " {\\rho_i - \\rho_s} \\\\\n", 655 | " \\vdots \\\\\n", 656 | " \\frac{\\rho_n - \\rho_s}\n", 657 | " {\\rho_i - \\rho_s} \n", 658 | " \\end{pmatrix} \\in \\mathbb{R}^{n} \\\\\n", 659 | " \\bf{x}^{-, +} &= \n", 660 | " \\begin{pmatrix}\n", 661 | " \\dot{\\bf{q}}^{-, +} \\\\\n", 662 | " \\bf{\\delta}^{-, +}\n", 663 | " \\end{pmatrix} \\in \\mathbb{R}^{(n+6)},\n", 664 | "\\end{align*}\n", 665 | "\n", 666 | "where\n", 667 | "$\\bf{\\delta} \\in \\mathbb{R}^6$ is the slack vector,\n", 668 | "$\\lambda_\\delta \\in \\mathbb{R}^+$ is a gain term which adjusts the cost of the norm of the slack vector in the optimiser,\n", 669 | "$\\dot{\\bf{q}}^{-,+}$ are the minimum and maximum joint velocities, and \n", 670 | "$\\dot{\\bf{\\delta}}^{-,+}$ are the minimum and maximum slack velocities.\n", 671 | "Each of the gains can be adjusted dynamically. For example, in practice $\\lambda_\\delta$ is typically large when far from the goal, but reduces towards 0 as the goal approaches.\n", 672 | "\n", 673 | "The effect of this augmented optimisation problem is that the equality constraint is equivalent to\n", 674 | "\n", 675 | "\\begin{align*} \n", 676 | " \\bf{\\nu}(t) - \\bf{\\delta}(t) = \\bf{J}(\\bf{q}) \\dot{\\bf{q}}(t)\n", 677 | "\\end{align*}\n", 678 | "\n", 679 | "which clearly demonstrates that the slack is essentially intentional error, where the optimiser can choose to move components of the desired end-effector motion into the slack vector. For both redundant and non-redundant robots, this means that the robot may stray from the straight line motion to improve manipulability and avoid a singularity, avoid running into joint position limits, or stay bounded by the joint velocity limits.\n", 680 | "\n", 681 | "Lets put this algorithm to the test in Python" 682 | ] 683 | }, 684 | { 685 | "cell_type": "code", 686 | "execution_count": 8, 687 | "metadata": {}, 688 | "outputs": [ 689 | { 690 | "data": { 691 | "text/html": [ 692 | "\n", 693 | " \n", 701 | " " 702 | ], 703 | "text/plain": [ 704 | "" 705 | ] 706 | }, 707 | "metadata": {}, 708 | "output_type": "display_data" 709 | } 710 | ], 711 | "source": [ 712 | "# Make a new environment and add objects\n", 713 | "env = Swift()\n", 714 | "env.launch(realtime=True, browser=\"notebook\")\n", 715 | "env.set_camera_pose([1.3, 0, 0.4], [0, 0, 0.3])\n", 716 | "env.add(panda)\n", 717 | "env.add(ee_axes)\n", 718 | "env.add(goal_axes) \n", 719 | "\n", 720 | "# Change the robot configuration to a ready position\n", 721 | "panda.q = [0.21, -0.03, 0.35, -1.90, -0.04, 1.96, 1.36]\n", 722 | "\n", 723 | "# A variable to specify when to break the loop\n", 724 | "arrived = False\n", 725 | "\n", 726 | "# Specify the gain for the p_servo method\n", 727 | "kt = 1.0\n", 728 | "kr = 1.3\n", 729 | "k = np.array([kt, kt, kt, kr, kr, kr])\n", 730 | "\n", 731 | "# Specify our timestep\n", 732 | "dt = 0.05\n", 733 | "\n", 734 | "Tep = (\n", 735 | " panda.fkine(panda.q)\n", 736 | " * sm.SE3.Tx(-0.1)\n", 737 | " * sm.SE3.Ty(0.6)\n", 738 | " * sm.SE3.Tz(0.4)\n", 739 | ")\n", 740 | "Tep = Tep.A\n", 741 | "\n", 742 | "# Set the goal axes to Tep\n", 743 | "goal_axes.T = Tep\n", 744 | "\n", 745 | "# Set the gain on the manipulability maximisation\n", 746 | "λm = 1.0\n", 747 | "\n", 748 | "# Set the gain on the joint velocity norm minimisation\n", 749 | "λq = 0.1\n", 750 | "\n", 751 | "# Run the simulation until the robot arrives at the goal\n", 752 | "while not arrived:\n", 753 | "\n", 754 | " # Work out the base frame manipulator Jacobian using the current robot configuration\n", 755 | " J = panda.jacob0(panda.q)\n", 756 | "\n", 757 | " # Calculate the manipulability Jacobian\n", 758 | " Jm = panda.jacobm(panda.q, axes='rot')\n", 759 | "\n", 760 | " # The end-effector pose of the panda (using .A to get a numpy array instead of an SE3 object)\n", 761 | " Te = panda.fkine(panda.q).A\n", 762 | "\n", 763 | " # Spatial error\n", 764 | " e = np.sum(np.abs(rtb.angle_axis(Te, Tep)))\n", 765 | "\n", 766 | " # Calculate the required end-effector velocity and whether the robot has arrived\n", 767 | " ev, arrived = rtb.p_servo(Te, Tep, gain=k, threshold=0.001, method=\"angle-axis\")\n", 768 | "\n", 769 | " ### Calculate each component of the quadratic programme\n", 770 | " # Quadratic component of objective function\n", 771 | " Q = np.eye(panda.n + 6)\n", 772 | "\n", 773 | " # Joint velocity component of Q\n", 774 | " Q[:panda.n, :panda.n] *= λq\n", 775 | "\n", 776 | " # Slack component of Q\n", 777 | " Q[panda.n:, panda.n:] = (1 / e) * np.eye(6)\n", 778 | "\n", 779 | " # The equality contraints\n", 780 | " Aeq = np.c_[J, np.eye(6)]\n", 781 | " beq = ev.reshape((6,))\n", 782 | "\n", 783 | " # The inequality constraints for joint limit avoidance\n", 784 | " Ain = np.zeros((panda.n + 6, panda.n + 6))\n", 785 | " bin = np.zeros(panda.n + 6)\n", 786 | "\n", 787 | " # The minimum angle (in radians) in which the joint is allowed to approach\n", 788 | " # to its limit\n", 789 | " ps = 0.05\n", 790 | "\n", 791 | " # The influence angle (in radians) in which the velocity damper\n", 792 | " # becomes active\n", 793 | " pi = 0.9\n", 794 | "\n", 795 | " # The gain for the velocity damper\n", 796 | " k_qlim = 1.0\n", 797 | "\n", 798 | " # Form the joint limit velocity damper\n", 799 | " Ain[:panda.n, :panda.n], bin[:panda.n] = joint_velocity_damper(panda, ps, pi, k_qlim)\n", 800 | "\n", 801 | " # Linear component of objective function: the manipulability Jacobian\n", 802 | " c = np.r_[λm * -Jm.reshape((panda.n,)), np.zeros(6)]\n", 803 | "\n", 804 | " # The lower and upper bounds on the joint velocity and slack variable\n", 805 | " lb = np.r_[qd_lb, -10 * np.ones(6)]\n", 806 | " ub = np.r_[qd_ub, 10 * np.ones(6)]\n", 807 | "\n", 808 | " # Solve for the joint velocities qd and apply to the robot\n", 809 | " xd = qp.solve_qp(Q, c, Ain, bin, Aeq, beq, lb=lb, ub=ub, solver='quadprog')\n", 810 | "\n", 811 | " # Apply the joint velocities to the Panda\n", 812 | " panda.qd[:panda.n] = xd[:panda.n]\n", 813 | "\n", 814 | " # Update the ee axes\n", 815 | " ee_axes.T = Te\n", 816 | "\n", 817 | " # Step the simulator by dt seconds\n", 818 | " env.step(dt)" 819 | ] 820 | } 821 | ], 822 | "metadata": { 823 | "interpreter": { 824 | "hash": "528815e074ebcdb9b34bcb695d4aa9d425bdb2cc6656b4ca45050b51a4125937" 825 | }, 826 | "kernelspec": { 827 | "display_name": "Python 3.9.5 ('rtb')", 828 | "language": "python", 829 | "name": "python3" 830 | }, 831 | "language_info": { 832 | "codemirror_mode": { 833 | "name": "ipython", 834 | "version": 3 835 | }, 836 | "file_extension": ".py", 837 | "mimetype": "text/x-python", 838 | "name": "python", 839 | "nbconvert_exporter": "python", 840 | "pygments_lexer": "ipython3", 841 | "version": "3.11.0" 842 | }, 843 | "orig_nbformat": 4 844 | }, 845 | "nbformat": 4, 846 | "nbformat_minor": 2 847 | } 848 | -------------------------------------------------------------------------------- /Part 2/7 Quadratic-Rate Motion Control.ipynb: -------------------------------------------------------------------------------- 1 | { 2 | "cells": [ 3 | { 4 | "cell_type": "code", 5 | "execution_count": 1, 6 | "metadata": {}, 7 | "outputs": [ 8 | { 9 | "data": { 10 | "text/html": [ 11 | "" 43 | ], 44 | "text/plain": [ 45 | "" 46 | ] 47 | }, 48 | "execution_count": 1, 49 | "metadata": {}, 50 | "output_type": "execute_result" 51 | } 52 | ], 53 | "source": [ 54 | "# Apply custon style to notebook\n", 55 | "from IPython.core.display import HTML\n", 56 | "import pathlib\n", 57 | "styles_path = pathlib.Path(pathlib.Path().absolute(), \"style\", \"style.css\")\n", 58 | "styles = open(styles_path, \"r\").read()\n", 59 | "HTML(f\"\")" 60 | ] 61 | }, 62 | { 63 | "attachments": {}, 64 | "cell_type": "markdown", 65 | "metadata": {}, 66 | "source": [ 67 | "_Note: This notebook must be run locally. Due to how the Swift Simulator operates, this notebook will not run on Google Colab_" 68 | ] 69 | }, 70 | { 71 | "attachments": {}, 72 | "cell_type": "markdown", 73 | "metadata": {}, 74 | "source": [ 75 | "# 7.0 Quadratic Rate Motion Control\n", 76 | "\n", 77 | "$\\large{\\text{Manipulator Differential Kinematics}} \\\\ \\large{\\text{Part II: Acceleration and Advanced Applications}}$\n", 78 | "\n", 79 | "$\\text{By Jesse Haviland and Peter Corke}$\n", 80 | "\n", 81 | "
\n", 82 | "\n", 83 | "The sections of the Tutorial paper related to this notebook are listed next to each contents entry.\n", 84 | "It is beneficial to read these sections of the paper before attempting the notebook Section.\n", 85 | "\n", 86 | "### Contents\n", 87 | "\n", 88 | "[7.1 Swift and Robot Setup](#1)\n", 89 | "\n", 90 | "[7.2 Quadratic Rate Motion Control](#2)\n", 91 | "* Singularity Escapability\n", 92 | " * Quadratic-Rate Motion Control\n", 93 | "\n", 94 | "[7.3 Position based Servoing with QRMC](#3)\n", 95 | "\n", 96 | "[7.4 QRMC Finishing in a Singularity](#4)\n", 97 | "\n", 98 | "[7.5 QRMC Starting and Finishing in a Singularity](#5)" 99 | ] 100 | }, 101 | { 102 | "cell_type": "code", 103 | "execution_count": 2, 104 | "metadata": {}, 105 | "outputs": [], 106 | "source": [ 107 | "# We will do the imports required for this notebook here\n", 108 | "\n", 109 | "# numpy provides import array and linear algebra utilities\n", 110 | "import numpy as np\n", 111 | "\n", 112 | "# the robotics toolbox provides robotics specific functionality\n", 113 | "import roboticstoolbox as rtb\n", 114 | "\n", 115 | "# spatial math provides objects for representing transformations\n", 116 | "import spatialmath as sm\n", 117 | "\n", 118 | "# swift is a lightweight browser-based simulator which comes eith the toolbox\n", 119 | "from swift import Swift\n", 120 | "\n", 121 | "# spatialgeometry is a utility package for dealing with geometric objects\n", 122 | "import spatialgeometry as sg" 123 | ] 124 | }, 125 | { 126 | "attachments": {}, 127 | "cell_type": "markdown", 128 | "metadata": {}, 129 | "source": [ 130 | "
\n", 131 | "\n", 132 | "\n", 133 | "\n", 134 | "### 7.1 Swift and Robot Setup\n", 135 | "---" 136 | ] 137 | }, 138 | { 139 | "attachments": {}, 140 | "cell_type": "markdown", 141 | "metadata": {}, 142 | "source": [ 143 | "In this notebook we will be using the Robotics Toolbox for Python and Swift to simulate our motion controllers. Check out Part 1 Notebook 3 for an introduction to these packages." 144 | ] 145 | }, 146 | { 147 | "cell_type": "code", 148 | "execution_count": 3, 149 | "metadata": {}, 150 | "outputs": [ 151 | { 152 | "data": { 153 | "text/html": [ 154 | "\n", 155 | " \n", 163 | " " 164 | ], 165 | "text/plain": [ 166 | "" 167 | ] 168 | }, 169 | "metadata": {}, 170 | "output_type": "display_data" 171 | }, 172 | { 173 | "data": { 174 | "text/plain": [ 175 | "2" 176 | ] 177 | }, 178 | "execution_count": 3, 179 | "metadata": {}, 180 | "output_type": "execute_result" 181 | } 182 | ], 183 | "source": [ 184 | "# Make the environment\n", 185 | "env = Swift()\n", 186 | "\n", 187 | "# Launch the simulator, will open a browser tab in your default\n", 188 | "# browser (chrome is recommended)\n", 189 | "# The realtime flag will ask the simulator to simulate as close as\n", 190 | "# possible to realtime as apposed to as fast as possible\n", 191 | "env.launch(realtime=True, browser=\"notebook\")\n", 192 | "\n", 193 | "# Make a ur5 robot\n", 194 | "robot = rtb.models.UR5()\n", 195 | "\n", 196 | "# Set the joint coordinates to qr\n", 197 | "robot.q = robot.qr\n", 198 | "\n", 199 | "# We can then add our robot to the simulator envionment\n", 200 | "env.add(robot)\n", 201 | "\n", 202 | "# end-effector axes\n", 203 | "ee_axes = sg.Axes(0.1)\n", 204 | "\n", 205 | "# goal axes\n", 206 | "goal_axes = sg.Axes(0.1)\n", 207 | "\n", 208 | "# Add the axes to the environment\n", 209 | "env.add(ee_axes)\n", 210 | "env.add(goal_axes) " 211 | ] 212 | }, 213 | { 214 | "attachments": {}, 215 | "cell_type": "markdown", 216 | "metadata": {}, 217 | "source": [ 218 | "
\n", 219 | "\n", 220 | "\n", 221 | "\n", 222 | "### 7.2 Quadratic Rate Motion Control\n", 223 | "---" 224 | ] 225 | }, 226 | { 227 | "attachments": {}, 228 | "cell_type": "markdown", 229 | "metadata": {}, 230 | "source": [ 231 | "Quadratic-rate motion control is a method of controlling a manipulator through or near a singularity. By using the second-order differential kinematics, the controller does not break down at or near a singularity, like the resolved-rate motion controller would. We start by considering the end-effector pose $\\bf{x} = f(\\bf{q}) \\in \\mathbb{R}^6$ in terms of the forward kinematics.\n", 232 | "Introducing a small change to the joint coordinates $\\Delta{\\bf{q}}$, we can write\n", 233 | "\n", 234 | "\\begin{align*}\n", 235 | " \\bf{x} + \\Delta{\\bf{x}} &= f(\\bf{q} + \\Delta{\\bf{q}}).\n", 236 | "\\end{align*}\n", 237 | "\n", 238 | "and the Taylor series expansion is\n", 239 | "\n", 240 | "\\begin{align*}\n", 241 | " \\bf{x} + \\Delta{\\bf{x}}\n", 242 | " &= f(\\bf{q}) +\n", 243 | " \\dfrac{\\partial f}{\\partial \\bf{q}} \\Delta{\\bf{q}} +\n", 244 | " \\dfrac{1}{2}\n", 245 | " \\left(\n", 246 | " \\dfrac{\\partial^2 f}{\\partial \\bf{q}^2} \\Delta{\\bf{q}}\n", 247 | " \\right) \\Delta{\\bf{q}} +\n", 248 | " \\cdots \\\\\n", 249 | " \\Delta{\\bf{x}}\n", 250 | " &=\n", 251 | " \\bf{J}(\\bf{q}) \\Delta{\\bf{q}} +\n", 252 | " \\frac{1}{2}\n", 253 | " \\Big(\n", 254 | " \\bf{H}(\\bf{q}) \\Delta{\\bf{q}}\n", 255 | " \\Big) \\Delta{\\bf{q}} +\n", 256 | " \\cdots\n", 257 | "\\end{align*}\n", 258 | "\n", 259 | "where for quadratic-rate control we wish to retain both the linear and quadratic terms (the first two terms) of the expansion.\n", 260 | "\n", 261 | "To form our controller, we use an iteration-based Newton-Raphson approach (as we did with resolved-rate motion control), where $k$ indicates the current step. Firstly, we rearrange the above to\n", 262 | "\n", 263 | "\\begin{align*}\n", 264 | " \\bf{g}(\\Delta{\\bf{q}}_k)\n", 265 | " &=\n", 266 | " \\bf{J}(\\bf{q}_k) \\Delta{\\bf{q}}_k +\n", 267 | " \\frac{1}{2}\n", 268 | " \\Big(\n", 269 | " \\bf{H}(\\bf{q}_k) \\Delta{\\bf{q}}_k\n", 270 | " \\Big) \\Delta{\\bf{q}}_k -\n", 271 | " \\Delta{\\bf{x}}\n", 272 | "\\end{align*}\n", 273 | "\n", 274 | "where $\\bf{q}_k$ and $\\Delta{\\bf{q}}_k$ are the manipulator's current joint coordinates and change in joint coordinates respectively, and $\\Delta{\\bf{x}}$ is the desired change in end-effector position. Taking the derivative of $\\bf{g}$ with respect to $\\Delta{\\bf{q}}$\n", 275 | "\n", 276 | "\\begin{align*}\n", 277 | " \\dfrac{\\partial \\bf{g}(\\Delta{\\bf{q}}_k)}\n", 278 | " {\\partial \\Delta{\\bf{q}}}\n", 279 | " &=\n", 280 | " \\bf{J}(\\bf{q}_k) + \\bf{H}(\\bf{q}_k)\\Delta{\\bf{q}}_k \\\\\n", 281 | " &= \\tilde{\\bf{J}}(\\bf{q}_k, \\Delta{\\bf{q}}_k)\n", 282 | "\\end{align*}\n", 283 | "\n", 284 | "we get a new Jacobian $\\tilde{\\bf{J}}(\\bf{q}_k, \\Delta{\\bf{q}}_k)$. Using this Jacobian we can create a new linear system\n", 285 | "\n", 286 | "\\begin{align*}\n", 287 | " \\tilde{\\bf{J}}(\\bf{q}_k, \\Delta{\\bf{q}}_k) \\bf{\\delta}_{\\Delta{\\bf{q}}} &= \\bf{g}(\\Delta{\\bf{q}}_k) \\\\\n", 288 | " \\bf{\\delta}_{\\Delta{\\bf{q}}} &= \\tilde{\\bf{J}}(\\bf{q}_k, \\Delta{\\bf{q}}_k)^{-1} \\bf{g}(\\Delta{\\bf{q}}_k)\n", 289 | "\\end{align*}\n", 290 | "\n", 291 | "where $\\bf{\\delta}_{\\Delta{\\bf{q}}}$ is the update to $\\Delta{\\bf{q}}_k$. The change in joint coordinates at the next step are\n", 292 | "\n", 293 | "\\begin{align*}\n", 294 | " \\Delta{\\bf{q}}_{k+1} = \\Delta{\\bf{q}}_{k} - \\bf{\\delta}_{\\Delta{\\bf{q}}.}\n", 295 | "\\end{align*}\n", 296 | "\n", 297 | "In the case that $\\Delta{\\bf{q}} = \\bf{0}$, quadratic-rate control reduces to resolved-rate control. This is not suitable if the robot is in or near a singularity. Therefore, a value near $0$ may be used to seed the initial value with $\\Delta{\\bf{q}} = \\bf{0.1}$, or the pseudo-inverse approach could be used\n", 298 | "\n", 299 | "\\begin{align*}\n", 300 | " \\Delta{\\bf{q}} = \\bf{J}(\\bf{q})^{+} \\Delta{\\bf{x}}.\n", 301 | "\\end{align*}\n", 302 | "\n", 303 | "This controller can be used in the same manner as resolved-rate motion control described in Part 1. The manipulator Jacobian and Hessian are calculated in the robot's base frame, and the end-effector velocity $\\Delta{\\bf{x}}$ can be calculated using the angle-axis method described in Part 1.\n" 304 | ] 305 | }, 306 | { 307 | "attachments": {}, 308 | "cell_type": "markdown", 309 | "metadata": {}, 310 | "source": [ 311 | "Lets make it happen in Python" 312 | ] 313 | }, 314 | { 315 | "cell_type": "code", 316 | "execution_count": 4, 317 | "metadata": {}, 318 | "outputs": [ 319 | { 320 | "data": { 321 | "text/html": [ 322 | "\n", 323 | " \n", 331 | " " 332 | ], 333 | "text/plain": [ 334 | "" 335 | ] 336 | }, 337 | "metadata": {}, 338 | "output_type": "display_data" 339 | } 340 | ], 341 | "source": [ 342 | "# Make the environment, launch and add objects\n", 343 | "env = Swift()\n", 344 | "env.launch(realtime=True, browser=\"notebook\")\n", 345 | "env.add(robot)\n", 346 | "env.add(ee_axes)\n", 347 | "\n", 348 | "# Change the robot configuration to the ready position\n", 349 | "robot.q = np.array([0, -np.pi / 2.2, np.pi / 2, 0.0, -0.0, 0])\n", 350 | "robot.qd = np.zeros(robot.n)\n", 351 | "\n", 352 | "# Step the sim to view the robot in this configuration\n", 353 | "env.step(0)\n", 354 | "\n", 355 | "# Specify our desired end-effector velocity\n", 356 | "ev = [0.05, 0.0, 0.0, 0.0, 0.0, 0.0]\n", 357 | "\n", 358 | "# Specify our timestep\n", 359 | "dt = 0.005\n", 360 | "\n", 361 | "# Run the simulation for 4 seconds\n", 362 | "for _ in range(400):\n", 363 | "\n", 364 | " # Work out the manipulator Jacobian, Hessian using the\n", 365 | " # current robot configuration\n", 366 | " J = robot.jacob0(robot.q)\n", 367 | " H = robot.hessian0(robot.q)\n", 368 | " \n", 369 | " # Check if the joint velocity vector norm is close to zero\n", 370 | " # If it is close to 0, use the pseudoinverse technique\n", 371 | " # outlined above\n", 372 | " if np.allclose(np.zeros(robot.n), robot.qd):\n", 373 | " qd = np.linalg.pinv(J) @ ev\n", 374 | " else:\n", 375 | " qd = robot.qd\n", 376 | "\n", 377 | " # Calculate H * qd\n", 378 | " Hq = np.tensordot(H, qd, axes=(0, 0))\n", 379 | "\n", 380 | " # Calculate the new Jacobian Jhat\n", 381 | " Jhat = J + Hq\n", 382 | "\n", 383 | " # Calculate the g vector\n", 384 | " g = J @ qd + 0.5 * Hq @ qd - ev\n", 385 | "\n", 386 | " # Calculate δq, the change to qd\n", 387 | " δq = np.linalg.inv(Jhat) @ g\n", 388 | "\n", 389 | " robot.qd = qd - δq\n", 390 | "\n", 391 | " # Update the ee axes\n", 392 | " ee_axes.T = robot.fkine(robot.q)\n", 393 | "\n", 394 | " # Step the simulator by dt seconds\n", 395 | " env.step(dt)" 396 | ] 397 | }, 398 | { 399 | "attachments": {}, 400 | "cell_type": "markdown", 401 | "metadata": {}, 402 | "source": [ 403 | "In the next example we use the end-effector frames of the Jacobian and Hessian and rotate around the z-axis" 404 | ] 405 | }, 406 | { 407 | "cell_type": "code", 408 | "execution_count": 5, 409 | "metadata": {}, 410 | "outputs": [ 411 | { 412 | "data": { 413 | "text/html": [ 414 | "\n", 415 | " \n", 423 | " " 424 | ], 425 | "text/plain": [ 426 | "" 427 | ] 428 | }, 429 | "metadata": {}, 430 | "output_type": "display_data" 431 | } 432 | ], 433 | "source": [ 434 | "# Make the environment, launch and add objects\n", 435 | "env = Swift()\n", 436 | "env.launch(realtime=True, browser=\"notebook\")\n", 437 | "env.add(robot)\n", 438 | "env.add(ee_axes)\n", 439 | "\n", 440 | "# Change the robot configuration to the ready position\n", 441 | "robot.q = np.array([0, -np.pi / 2, np.pi / 2, 0.0, -0.0, 0])\n", 442 | "robot.qd = np.zeros(robot.n)\n", 443 | "\n", 444 | "# Step the sim to view the robot in this configuration\n", 445 | "env.step(0)\n", 446 | "\n", 447 | "# Specify our desired end-effector velocity\n", 448 | "ev = [0.0, 0.0, 0.0, 0.0, 0.0, 0.5]\n", 449 | "\n", 450 | "# Specify our timestep\n", 451 | "dt = 0.005\n", 452 | "\n", 453 | "# Run the simulation for 4 seconds\n", 454 | "for _ in range(400):\n", 455 | "\n", 456 | " # Work out the manipulator Jacobian, Hessian using the\n", 457 | " # current robot configuration\n", 458 | " J = robot.jacobe(robot.q)\n", 459 | " H = robot.hessiane(robot.q)\n", 460 | " \n", 461 | " # Check if the joint velocity vector norm is close to zero\n", 462 | " # If it is close to 0, use the pseudoinverse technique\n", 463 | " # outlined above\n", 464 | " if np.allclose(np.zeros(robot.n), robot.qd):\n", 465 | " qd = np.linalg.pinv(J) @ ev\n", 466 | " else:\n", 467 | " qd = robot.qd\n", 468 | "\n", 469 | " # Calculate H * qd\n", 470 | " Hq = np.tensordot(H, qd, axes=(0, 0))\n", 471 | "\n", 472 | " # Calculate the new Jacobian Jhat\n", 473 | " Jhat = J + Hq\n", 474 | "\n", 475 | " # Calculate the g vector\n", 476 | " g = J @ qd + 0.5 * Hq @ qd - ev\n", 477 | "\n", 478 | " # Calculate δq, the change to qd\n", 479 | " δq = np.linalg.inv(Jhat) @ g\n", 480 | "\n", 481 | " robot.qd = qd - δq\n", 482 | "\n", 483 | " # Update the ee axes\n", 484 | " ee_axes.T = robot.fkine(robot.q)\n", 485 | "\n", 486 | " # Step the simulator by dt seconds\n", 487 | " env.step(dt)" 488 | ] 489 | }, 490 | { 491 | "attachments": {}, 492 | "cell_type": "markdown", 493 | "metadata": {}, 494 | "source": [ 495 | "
\n", 496 | "\n", 497 | "\n", 498 | "\n", 499 | "### 7.3 Position based Servoing with QRMC\n", 500 | "---" 501 | ] 502 | }, 503 | { 504 | "attachments": {}, 505 | "cell_type": "markdown", 506 | "metadata": {}, 507 | "source": [ 508 | "As with previous motion controllers we have developed (Part 1 Notebook 3, Part 2 Notebooks 4, 5) this motion controller is best used with position-based servoing." 509 | ] 510 | }, 511 | { 512 | "cell_type": "code", 513 | "execution_count": 6, 514 | "metadata": {}, 515 | "outputs": [ 516 | { 517 | "data": { 518 | "text/html": [ 519 | "\n", 520 | " \n", 528 | " " 529 | ], 530 | "text/plain": [ 531 | "" 532 | ] 533 | }, 534 | "metadata": {}, 535 | "output_type": "display_data" 536 | } 537 | ], 538 | "source": [ 539 | "# Make the environment, launch and add objects\n", 540 | "env = Swift()\n", 541 | "env.launch(realtime=True, browser=\"notebook\")\n", 542 | "env.add(robot)\n", 543 | "env.add(ee_axes)\n", 544 | "env.add(goal_axes) \n", 545 | "\n", 546 | "# Change the robot configuration to the ready position\n", 547 | "robot.q = np.array([0, -np.pi / 2, np.pi / 2, 0.0, -0.0, 0])\n", 548 | "robot.qd = np.zeros(robot.n)\n", 549 | "\n", 550 | "# Step the sim to view the robot in this configuration\n", 551 | "env.step(0)\n", 552 | "\n", 553 | "Tep = (\n", 554 | " robot.fkine(robot.q)\n", 555 | " * sm.SE3.Tx(0.2)\n", 556 | " * sm.SE3.Ty(0.2)\n", 557 | " * sm.SE3.Tz(0.2)\n", 558 | " * sm.SE3.Rx(-np.pi / 2)\n", 559 | " * sm.SE3.Rz(-np.pi / 2)\n", 560 | ")\n", 561 | "Tep = Tep.A\n", 562 | "\n", 563 | "# Set the goal axes to Tep\n", 564 | "goal_axes.T = Tep\n", 565 | "\n", 566 | "# A variable to specify when to break the loop\n", 567 | "arrived = False\n", 568 | "\n", 569 | "# Specify our timestep\n", 570 | "dt = 0.005\n", 571 | "\n", 572 | "# Specify the gain for the p_servo method\n", 573 | "k = np.ones(6)\n", 574 | "\n", 575 | "# Run the simulation until the robot arrives at the goal\n", 576 | "while not arrived:\n", 577 | "\n", 578 | " # The end-effector pose of the panda (using .A to get a numpy array instead of an SE3 object)\n", 579 | " Te = robot.fkine(robot.q).A\n", 580 | "\n", 581 | " # Calculate the required end-effector velocity and whether the robot has arrived\n", 582 | " ev, arrived = rtb.p_servo(Te, Tep, gain=k, threshold=0.001, method=\"angle-axis\")\n", 583 | "\n", 584 | " # Work out the manipulator Jacobian, Hessian using the\n", 585 | " # current robot configuration\n", 586 | " J = robot.jacob0(robot.q)\n", 587 | " H = robot.hessian0(robot.q)\n", 588 | " \n", 589 | " # Check if the joint velocity vector norm is close to zero\n", 590 | " # If it is close to 0, use the pseudoinverse technique\n", 591 | " # outlined in Section 7.1\n", 592 | " if np.allclose(np.zeros(robot.n), robot.qd):\n", 593 | " qd = np.linalg.pinv(J) @ ev\n", 594 | " else:\n", 595 | " qd = robot.qd\n", 596 | "\n", 597 | " # Calculate H * qd\n", 598 | " Hq = np.tensordot(H, qd, axes=(0, 0))\n", 599 | "\n", 600 | " # Calculate the new Jacobian Jhat\n", 601 | " Jhat = J + Hq\n", 602 | "\n", 603 | " # Calculate the g vector\n", 604 | " g = J @ qd + 0.5 * Hq @ qd - ev\n", 605 | "\n", 606 | " # Calculate δq, the change to qd\n", 607 | " δq = np.linalg.inv(Jhat) @ g\n", 608 | "\n", 609 | " robot.qd = qd - δq\n", 610 | "\n", 611 | " # Update the ee axes\n", 612 | " ee_axes.T = Te\n", 613 | "\n", 614 | " # Step the simulator by dt seconds\n", 615 | " env.step(dt)" 616 | ] 617 | }, 618 | { 619 | "attachments": {}, 620 | "cell_type": "markdown", 621 | "metadata": {}, 622 | "source": [ 623 | "
\n", 624 | "\n", 625 | "\n", 626 | "\n", 627 | "### 7.4 QRMC Finishing in a Singularity\n", 628 | "---" 629 | ] 630 | }, 631 | { 632 | "attachments": {}, 633 | "cell_type": "markdown", 634 | "metadata": {}, 635 | "source": [ 636 | "In this example the robot must finish its motion at a singularity." 637 | ] 638 | }, 639 | { 640 | "cell_type": "code", 641 | "execution_count": 7, 642 | "metadata": {}, 643 | "outputs": [ 644 | { 645 | "data": { 646 | "text/html": [ 647 | "\n", 648 | " \n", 656 | " " 657 | ], 658 | "text/plain": [ 659 | "" 660 | ] 661 | }, 662 | "metadata": {}, 663 | "output_type": "display_data" 664 | } 665 | ], 666 | "source": [ 667 | "# Make the environment, launch and add objects\n", 668 | "env = Swift()\n", 669 | "env.launch(realtime=True, browser=\"notebook\")\n", 670 | "env.add(robot)\n", 671 | "env.add(ee_axes)\n", 672 | "env.add(goal_axes) \n", 673 | "\n", 674 | "# Change the robot configuration to the ready position\n", 675 | "robot.q = np.array([0, -np.pi / 2, np.pi / 2, 0.0, -0.0, 0])\n", 676 | "robot.qd = np.zeros(robot.n)\n", 677 | "\n", 678 | "# Step the sim to view the robot in this configuration\n", 679 | "env.step(0)\n", 680 | "\n", 681 | "Tep = robot.fkine(np.array([np.pi, 0.0, 0.0, 0.0, np.pi/2, 0.0]))\n", 682 | "Tep = Tep.A\n", 683 | "\n", 684 | "# Set the goal axes to Tep\n", 685 | "goal_axes.T = Tep\n", 686 | "\n", 687 | "# A variable to specify when to break the loop\n", 688 | "arrived = False\n", 689 | "\n", 690 | "# Specify our timestep\n", 691 | "dt = 0.005\n", 692 | "\n", 693 | "# Specify the gain for the p_servo method\n", 694 | "k = np.ones(6)\n", 695 | "\n", 696 | "# Run the simulation until the robot arrives at the goal\n", 697 | "while not arrived:\n", 698 | "\n", 699 | " # The end-effector pose of the panda (using .A to get a numpy array instead of an SE3 object)\n", 700 | " Te = robot.fkine(robot.q).A\n", 701 | "\n", 702 | " # Calculate the required end-effector velocity and whether the robot has arrived\n", 703 | " ev, arrived = rtb.p_servo(Te, Tep, gain=k, threshold=0.001, method=\"angle-axis\")\n", 704 | "\n", 705 | " # Work out the manipulator Jacobian, Hessian using the\n", 706 | " # current robot configuration\n", 707 | " J = robot.jacob0(robot.q)\n", 708 | " H = robot.hessian0(robot.q)\n", 709 | " \n", 710 | " # Check if the joint velocity vector norm is close to zero\n", 711 | " # If it is close to 0, use the pseudoinverse technique\n", 712 | " # outlined in Section 7.1\n", 713 | " if np.allclose(np.zeros(robot.n), robot.qd):\n", 714 | " # qd = np.linalg.pinv(J) @ ev\n", 715 | " qd = 0.1 * np.ones(robot.n)\n", 716 | " else:\n", 717 | " qd = robot.qd\n", 718 | "\n", 719 | " # Calculate H * qd\n", 720 | " Hq = np.tensordot(H, qd, axes=(0, 0))\n", 721 | "\n", 722 | " # Calculate the new Jacobian Jhat\n", 723 | " Jhat = J + Hq\n", 724 | "\n", 725 | " # Calculate the g vector\n", 726 | " g = J @ qd + 0.5 * Hq @ qd - ev\n", 727 | "\n", 728 | " # Calculate δq, the change to qd\n", 729 | " δq = np.linalg.inv(Jhat) @ g\n", 730 | "\n", 731 | " robot.qd = qd - δq\n", 732 | "\n", 733 | " # Update the ee axes\n", 734 | " ee_axes.T = Te\n", 735 | "\n", 736 | " # Step the simulator by dt seconds\n", 737 | " env.step(dt)" 738 | ] 739 | }, 740 | { 741 | "attachments": {}, 742 | "cell_type": "markdown", 743 | "metadata": {}, 744 | "source": [ 745 | "
\n", 746 | "\n", 747 | "\n", 748 | "\n", 749 | "### 7.5 QRMC Starting and Finishing in a Singularity\n", 750 | "---" 751 | ] 752 | }, 753 | { 754 | "attachments": {}, 755 | "cell_type": "markdown", 756 | "metadata": {}, 757 | "source": [ 758 | "In this example the robot must start and finish its motion at a singularity." 759 | ] 760 | }, 761 | { 762 | "cell_type": "code", 763 | "execution_count": 8, 764 | "metadata": {}, 765 | "outputs": [ 766 | { 767 | "data": { 768 | "text/html": [ 769 | "\n", 770 | " \n", 778 | " " 779 | ], 780 | "text/plain": [ 781 | "" 782 | ] 783 | }, 784 | "metadata": {}, 785 | "output_type": "display_data" 786 | } 787 | ], 788 | "source": [ 789 | "# Make the environment, launch and add objects\n", 790 | "env = Swift()\n", 791 | "env.launch(realtime=True, browser=\"notebook\")\n", 792 | "env.add(robot)\n", 793 | "env.add(ee_axes)\n", 794 | "env.add(goal_axes)\n", 795 | "\n", 796 | "# Change the robot configuration\n", 797 | "robot.q = np.zeros(robot.n)\n", 798 | "robot.qd = np.zeros(robot.n)\n", 799 | "\n", 800 | "# Step the sim to view the robot in this configuration\n", 801 | "env.step(0)\n", 802 | "\n", 803 | "Tep = robot.fkine([0, -np.pi / 2, np.pi / 2, 0.0, -0.0, 0])\n", 804 | "Tep = Tep.A\n", 805 | "\n", 806 | "# Set the goal axes to Tep\n", 807 | "goal_axes.T = Tep\n", 808 | "\n", 809 | "# A variable to specify when to break the loop\n", 810 | "arrived = False\n", 811 | "\n", 812 | "# Specify our timestep\n", 813 | "dt = 0.005\n", 814 | "\n", 815 | "# Specify the gain for the p_servo method\n", 816 | "k = np.ones(6)\n", 817 | "\n", 818 | "# Run the simulation until the robot arrives at the goal\n", 819 | "while not arrived:\n", 820 | "\n", 821 | " # The end-effector pose of the panda (using .A to get a numpy array instead of an SE3 object)\n", 822 | " Te = robot.fkine(robot.q).A\n", 823 | "\n", 824 | " # Calculate the required end-effector velocity and whether the robot has arrived\n", 825 | " ev, arrived = rtb.p_servo(Te, Tep, gain=k, threshold=0.001, method=\"angle-axis\")\n", 826 | "\n", 827 | " # Work out the manipulator Jacobian, Hessian using the\n", 828 | " # current robot configuration\n", 829 | " J = robot.jacob0(robot.q)\n", 830 | " H = robot.hessian0(robot.q)\n", 831 | " \n", 832 | " # Check if the joint velocity vector norm is close to zero\n", 833 | " # If it is close to 0, use the pseudoinverse technique\n", 834 | " # outlined in Section 7.1\n", 835 | " if np.allclose(np.zeros(robot.n), robot.qd):\n", 836 | " # qd = np.linalg.pinv(J) @ ev\n", 837 | " qd = 0.1 * np.ones(robot.n)\n", 838 | " else:\n", 839 | " qd = robot.qd\n", 840 | "\n", 841 | " # Calculate H * qd\n", 842 | " Hq = np.tensordot(H, qd, axes=(0, 0))\n", 843 | "\n", 844 | " # Calculate the new Jacobian Jhat\n", 845 | " Jhat = J + Hq\n", 846 | "\n", 847 | " # Calculate the g vector\n", 848 | " g = J @ qd + 0.5 * Hq @ qd - ev\n", 849 | "\n", 850 | " # Calculate δq, the change to qd\n", 851 | " δq = np.linalg.inv(Jhat) @ g\n", 852 | "\n", 853 | " robot.qd = qd - δq\n", 854 | "\n", 855 | " # Update the ee axes\n", 856 | " ee_axes.T = Te\n", 857 | "\n", 858 | " # Step the simulator by dt seconds\n", 859 | " env.step(dt)" 860 | ] 861 | } 862 | ], 863 | "metadata": { 864 | "interpreter": { 865 | "hash": "528815e074ebcdb9b34bcb695d4aa9d425bdb2cc6656b4ca45050b51a4125937" 866 | }, 867 | "kernelspec": { 868 | "display_name": "Python 3.9.5 ('rtb')", 869 | "language": "python", 870 | "name": "python3" 871 | }, 872 | "language_info": { 873 | "codemirror_mode": { 874 | "name": "ipython", 875 | "version": 3 876 | }, 877 | "file_extension": ".py", 878 | "mimetype": "text/x-python", 879 | "name": "python", 880 | "nbconvert_exporter": "python", 881 | "pygments_lexer": "ipython3", 882 | "version": "3.11.0" 883 | }, 884 | "orig_nbformat": 4 885 | }, 886 | "nbformat": 4, 887 | "nbformat_minor": 2 888 | } 889 | -------------------------------------------------------------------------------- /Part 2/img/cover.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jhavl/dkt/aef11b61881f15d20604572982ab88405b62d9e9/Part 2/img/cover.png -------------------------------------------------------------------------------- /Part 2/img/elementary_transforms_dark.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jhavl/dkt/aef11b61881f15d20604572982ab88405b62d9e9/Part 2/img/elementary_transforms_dark.png -------------------------------------------------------------------------------- /Part 2/img/hessian_dark.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jhavl/dkt/aef11b61881f15d20604572982ab88405b62d9e9/Part 2/img/hessian_dark.png -------------------------------------------------------------------------------- /Part 2/img/jacobian_dark.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jhavl/dkt/aef11b61881f15d20604572982ab88405b62d9e9/Part 2/img/jacobian_dark.png -------------------------------------------------------------------------------- /Part 2/img/rotation_dark.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jhavl/dkt/aef11b61881f15d20604572982ab88405b62d9e9/Part 2/img/rotation_dark.png -------------------------------------------------------------------------------- /Part 2/img/skew_dark.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jhavl/dkt/aef11b61881f15d20604572982ab88405b62d9e9/Part 2/img/skew_dark.png -------------------------------------------------------------------------------- /Part 2/img/title.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jhavl/dkt/aef11b61881f15d20604572982ab88405b62d9e9/Part 2/img/title.png -------------------------------------------------------------------------------- /Part 2/img/transform_dark.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jhavl/dkt/aef11b61881f15d20604572982ab88405b62d9e9/Part 2/img/transform_dark.png -------------------------------------------------------------------------------- /Part 2/style/style.css: -------------------------------------------------------------------------------- 1 | /* div { 2 | max-width: 900px !important; 3 | text-align: justify; 4 | } */ 5 | 6 | 7 | /* .inner_cell { 8 | max-width: 900px !important; 9 | text-align: justify; 10 | } */ 11 | 12 | .cell { 13 | /* max-width: 900px !important; */ 14 | /* line-height: 1.6em; */ 15 | padding-bottom: 15px; 16 | text-align: justify; 17 | } 18 | 19 | .markup { 20 | max-width: 900px !important; 21 | text-align: justify; 22 | } 23 | 24 | img { 25 | display: block; 26 | margin-left: auto; 27 | margin-right: auto; 28 | } 29 | 30 | /* αt */ 31 | /* βt */ 32 | /* γt */ -------------------------------------------------------------------------------- /img/article1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jhavl/dkt/aef11b61881f15d20604572982ab88405b62d9e9/img/article1.png -------------------------------------------------------------------------------- /img/article2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jhavl/dkt/aef11b61881f15d20604572982ab88405b62d9e9/img/article2.png -------------------------------------------------------------------------------- /img/qcr.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jhavl/dkt/aef11b61881f15d20604572982ab88405b62d9e9/img/qcr.png -------------------------------------------------------------------------------- /readme.md: -------------------------------------------------------------------------------- 1 | # A Tutorial on Manipulator Differential Kinematics 2 | 3 | [![Powered by the Robotics Toolbox](https://raw.githubusercontent.com/petercorke/robotics-toolbox-python/master/.github/svg/rtb_powered.min.svg)](https://github.com/petercorke/robotics-toolbox-python) 4 | ![PyPI - Python Version](https://img.shields.io/pypi/pyversions/roboticstoolbox-python.svg) 5 | [![QUT Centre for Robotics Open Source](https://github.com/qcr/qcr.github.io/raw/master/misc/badge.svg)](https://qcr.github.io) 6 | [![License: MIT](https://img.shields.io/badge/License-MIT-yellow.svg)](https://opensource.org/licenses/MIT) 7 | 8 | **By [Jesse Haviland](https://jhavl.com/) and [Peter Corke](https://petercorke.com/)** 9 | 10 | This repository contains a collection of Jupyter Notebooks designed to accompany the two-part Tutorial on Manipulator Differential Kinematics published in the IEEE Robotics and Automation Magazine. Each Notebook corresponds to a section within the tutorial articles. The notebooks are easily extensible to encourage experimentation. The intention is that you read a section of the tutorial and then complete the corresponding Notebook. The articles can be accessed via PDF via the links below. 11 | 12 | 13 | 14 | 17 | 82 | 83 |
15 | 16 | 18 | Part 1: Kinematics, Velocity, and Applications 19 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 36 | 41 | 42 | 43 | 46 | 51 | 52 | 53 | 56 | 59 | 60 | 61 | 64 | 69 | 70 | 71 | 74 | 79 | 80 |
Jupyter NotebooksOpen in Colab Link
34 | 1 Manipulator Kinematics 35 | 37 | 38 | Open In Colab 39 | 40 |
44 | 2 The Manipulator Jacobian 45 | 47 | 48 | Open In Colab 49 | 50 |
54 | 3 Resolved-Rate Motion Control 55 | 57 | Will not run on Colab 58 |
62 | 4 Numerical Inverse Kinematics 63 | 65 | 66 | Open In Colab 67 | 68 |
72 | 5 Manipulator Performance Measures 73 | 75 | 76 | Open In Colab 77 | 78 |
81 |
84 | 85 | 86 | 87 | 90 | 171 | 172 |
88 | 89 | 91 | Part 2: Acceleration and Advanced Applications 92 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 109 | 114 | 115 | 116 | 119 | 124 | 125 | 126 | 129 | 134 | 135 | 136 | 139 | 142 | 143 | 144 | 147 | 150 | 151 | 152 | 155 | 160 | 161 | 162 | 165 | 168 | 169 |
Jupyter NotebooksOpen in Colab Link
107 | 1 The Manipulator Hessian 108 | 110 | 111 | Open In Colab 112 | 113 |
117 | 2 Higher Order Derivatives 118 | 120 | 121 | Open In Colab 122 | 123 |
127 | 3 Analytic Forms 128 | 130 | 131 | Open In Colab 132 | 133 |
137 | 4 Null-Space Projection for Motion Control 138 | 140 | Will not run on Colab 141 |
145 | 5 Quadratic Programming for Motion Control 146 | 148 | Will not run on Colab 149 |
153 | 6 Advanced Numerical Inverse Kinematics 154 | 156 | 157 | Open In Colab 158 | 159 |
163 | 7 Quadratic-Rate Motion Control 164 | 166 | Will not run on Colab 167 |
170 |
173 | 174 |
175 | 176 | ## Contents 177 | 178 | - [Synopsis](#1) 179 | - [Python Setup Guide](#2) 180 | - [Running Notebooks Locally](#3) 181 | - [Running Notebooks on Google Colab](#4) 182 | - [Citation Info](#5) 183 | - [Acknowledgements](#6) 184 | 185 |
186 | 187 | 188 | 189 | ## Synopsis 190 | 191 | Manipulator kinematics is concerned with the motion of each link within a manipulator without considering mass or force. A serial-link manipulator, which we refer to as a manipulator, is the formal name for a robot that comprises a chain of rigid links and joints, it may contain branches, but it can not have closed loops. Each joint provides one degree of freedom, which may be a prismatic joint providing translational freedom or a revolute joint providing rotational freedom. The base frame of a manipulator represents the reference frame of the first link in the chain, while the last link is known as the end-effector. 192 | 193 | In Part I of the two-part Tutorial, we provide an introduction to modelling manipulator kinematics using the elementary transform sequence (ETS). Then we formulate the first-order differential kinematics, which leads to the manipulator Jacobian, which is the basis for velocity control and inverse kinematics. We describe essential classical techniques which rely on the manipulator Jacobian before exhibiting some contemporary applications. 194 | 195 | In Part II of the Tutorial, we formulate the second-order differential kinematics, leading to a definition of manipulator Hessian. We then describe the differential kinematics' analytical forms, which are essential to dynamics applications. Subsequently, we provide a general formula for higher-order derivatives. The first application we consider is advanced velocity control. In this Section, we extend resolved-rate motion control to perform sub-tasks while still achieving the goal before redefining the algorithm as a quadratic program to enable greater flexibility and additional constraints. We then take another look at numerical inverse kinematics with an emphasis on adding constraints. Finally, we analyse how the manipulator Hessian can help to escape singularities. 196 | 197 |
198 | 199 | 200 | 201 | ## Python Setup Guide 202 | 203 | The Notebooks are written using Python and we use several python packages. We recommend that you set up a virtual environment/Python environment manager. We provide a guide to setting up Conda below but feel free to use any alternative such as `virtualenv` or `venv`. 204 | 205 | The Notebooks have been tested to run on Ubuntu, Windows and Mac OS with any currently supported version of Python (currently 3.7, 3.8 3.9 and 3.10). 206 | 207 | ### Conda Environment Setup Guide 208 | 209 | Download `miniconda` from [here](https://docs.conda.io/en/latest/miniconda.html#latest-miniconda-installer-links) while choosing the link for your operating system and architecture. 210 | 211 | Follow the Conda install instructions from [here](https://conda.io/projects/conda/en/latest/user-guide/install/index.html#installation). 212 | 213 | In the terminal, make a new `conda` environment. We called our environment `dktutorial` and recommend choosing Python version `3.10` 214 | 215 | ```bash 216 | conda create --name dktutorial python=3.10 217 | ``` 218 | 219 | We need to activate our environment to use it 220 | 221 | ```bash 222 | conda activate dktutorial 223 | ``` 224 | 225 | Check out this [link](https://docs.conda.io/projects/conda/en/4.6.0/_downloads/52a95608c49671267e40c689e0bc00ca/conda-cheatsheet.pdf) for a handy Conda command cheat sheet. There is also a ~30 minute Conda Tutorial available [here](https://conda.io/projects/conda/en/latest/user-guide/getting-started.html). 226 | 227 | ### Python Package Install Guide 228 | 229 | We require several Python packages to run the Notebooks. You should activate the Conda environment before completing this stage. 230 | 231 | We use IPython and Jupyter notebook 232 | 233 | ```bash 234 | pip install ipython notebook 235 | ``` 236 | 237 | Install the Robotics Toolbox for Python and associated packages. This will also install other requirements such as Swift and Spatialmath-Python. 238 | 239 | ```bash 240 | pip install "roboticstoolbox-python>=1.1.0" 241 | ``` 242 | 243 | For Notebooks in Part II we need `sympy` and `qpsolvers` 244 | 245 | ```bash 246 | pip install sympy qpsolvers[quadprog] 247 | ``` 248 | 249 |
250 | 251 | 252 | 253 | ## Running Notebooks Locally 254 | 255 | We have tested the Notebooks in the default Jupyter Notebook web interface and the VSCode Notebook extension. 256 | 257 | ### Clone the Repository 258 | 259 | Firstly, you must clone the repository 260 | 261 | ```bash 262 | git clone https://github.com/jhavl/dkt.git 263 | cd dkt 264 | ``` 265 | 266 | ### Running in the Jupyter Notebook Web Interface 267 | 268 | In terminal, activate the conda environment, navigate to the repository folder and run 269 | 270 | ```bash 271 | conda activate dktutorial 272 | cd "path_to_repo/dkt" 273 | jupyter-notebook 274 | ``` 275 | 276 | ### Running in the VSCode Interface 277 | 278 | Download and install VSCode from [here](https://code.visualstudio.com/). 279 | 280 | Add the `Python` extension, see this [link](https://marketplace.visualstudio.com/items?itemName=ms-python.python) (if not already installed). 281 | 282 | Add the `Jupyter` extension, see this [link](https://marketplace.visualstudio.com/items?itemName=ms-toolsai.jupyter) (if not already installed). 283 | 284 | From VSCode, select `Open Folder...` and navigate to and select the repository folder. You may be prompted to select if you trust the contents of the folder. **Warning** If you decline to trust the folder, it is unlikely that you will be able to run any of the Notebooks. 285 | 286 | After selecting the folder, choose which Notebook you would like to run from the Explorer menu on the left side of the screen. 287 | 288 | Once a Notebook is open, you must select the kernel from the `Select Kernel` button on the top right side of the screen. Choose the conda environment we created earlier `dktutorial (Python 3.10.X)`. 289 | 290 |
291 | 292 | 293 | 294 | ## Running Notebooks on Google Colab 295 | 296 | For the fastest and smoothest experience, it is recommended to run the Notebooks locally. However, most Notebooks can be run online on the Google Colab platform. Click the links in the table at the top of the page to open a Notebook in Colab. 297 | 298 |
299 | 300 | 301 | 302 | ## Citation Info 303 | 304 | Please cite us if you use this work in your research, for Part 1: 305 | 306 | ``` 307 | @article{haviland2023dkt1, 308 | author={Haviland, Jesse and Corke, Peter}, 309 | title={Manipulator Differential Kinematics: Part I: Kinematics, Velocity, and Applications}, 310 | journal={IEEE Robotics \& Automation Magazine}, 311 | year={2023}, 312 | pages={2-12}, 313 | doi={10.1109/MRA.2023.3270228} 314 | } 315 | ``` 316 | 317 | and for Part 2: 318 | 319 | ``` 320 | @article{haviland2023dkt2, 321 | author={Haviland, Jesse and Corke, Peter}, 322 | title={Manipulator Differential Kinematics: Part II: Acceleration and Advanced Applications}, 323 | journal={IEEE Robotics \& Automation Magazine}, 324 | year={2023}, 325 | pages={2-12}, 326 | doi={10.1109/MRA.2023.3270221} 327 | } 328 | ``` 329 | 330 |
331 | 332 | 333 | 334 | ## Acknowledgements 335 | 336 | This research was supported by the Queensland University of Technology Centre for Robotics ([QCR](https://research.qut.edu.au/qcr/)). 337 | 338 | 339 | --------------------------------------------------------------------------------