From 6db136e4893c17ecea6f3577121355c878403acb Mon Sep 17 00:00:00 2001 From: Peter Corke Date: Tue, 20 Oct 2026 06:13:39 +1000 Subject: [PATCH 1/2] docs(notebooks): the first cell is hand-written markdown again, and cell ids are committed The generated header is removed: the first cell should reflect the notebook, as in MVTB. The logo is an absolute URL so that Colab shows it. sync_bootstrap.py gives every cell an id, so that VS Code no longer changes a notebook when it opens it. Co-Authored-By: Claude Sonnet 5.5 --- docs/notebooks/README.md | 30 ++- docs/notebooks/branched-robot.ipynb | 105 +++++--- docs/notebooks/dynamics.ipynb | 110 +++++--- docs/notebooks/ets.ipynb | 285 +++++++++++++------- docs/notebooks/graphics-test.ipynb | 22 +- docs/notebooks/icra2021.ipynb | 185 ++++++++----- docs/notebooks/icra2026.ipynb | 15 +- docs/notebooks/ik-symbolic.ipynb | 13 +- docs/notebooks/ik_benchmark.ipynb | 35 +-- docs/notebooks/index.ipynb | 5 +- docs/notebooks/iros2020.ipynb | 335 ++++++++++++++--------- docs/notebooks/kinematics.ipynb | 152 +++++++---- docs/notebooks/symbolic.ipynb | 351 ++++++++++++++++--------- docs/notebooks/sync_bootstrap.py | 83 ++---- docs/notebooks/two-link-dynamics.ipynb | 84 +++--- tests/test_notebook_bootstrap.py | 63 +++-- 16 files changed, 1140 insertions(+), 733 deletions(-) diff --git a/docs/notebooks/README.md b/docs/notebooks/README.md index 6cc0bfd6a..7a8a8bea0 100644 --- a/docs/notebooks/README.md +++ b/docs/notebooks/README.md @@ -21,21 +21,27 @@ found by that line, not by its position) and is folded by default. Then it installs the toolbox with its dependencies, from the wheel bundled with the JupyterLite site if there is one, or else from PyPI. -## The standard first cell +## The first cell -Every notebook starts with the same markdown cell: the logo and title, the copyright -line, and a short description. It is generated from the notebook's own metadata by -`sync_bootstrap.py`, like the bootstrap cell, and the `\n", "\n", " \n", " \n", - " \n", + " \n", " \n", "
Robotics Toolbox for Python
\n", "\n", @@ -21,7 +20,8 @@ "A robot with more than one end-effector: its forward kinematics and Jacobian for each branch, extracting the ETS of a branch, and drawing the robot as a graph. Drawing the graphs needs [Graphviz](https://graphviz.org) (the `dot` command) installed.\n", "\n", "> **Modernised 2026-10-19** to address API drift: this notebook was written for an earlier release of the Toolbox, and has been updated so that it runs with the current one." - ] + ], + "id": "2aa1a0db" }, { "cell_type": "code", @@ -75,7 +75,8 @@ " return colab\n", "\n", "COLAB = await ensure_installed()\n" - ] + ], + "id": "a13e6155" }, { "cell_type": "code", @@ -85,14 +86,16 @@ "source": [ "import roboticstoolbox as rtb\n", "from IPython.display import SVG" - ] + ], + "id": "9daa4a69" }, { "cell_type": "markdown", "metadata": {}, "source": [ "We will load a model for a simple Y-shaped planar robot. Check out the source code to see how the model was constructed using `Link` and `Robot` classes." - ] + ], + "id": "5a617374" }, { "cell_type": "code", @@ -102,7 +105,8 @@ "source": [ "robot = rtb.models.ETS.Planar_Y()\n", "print(robot)" - ] + ], + "id": "d91fc922" }, { "cell_type": "markdown", @@ -111,7 +115,8 @@ "From the displayed table we see that this robot has two end-effectors, indicated by the links shown in blue and prefixed with the @ symbol.\n", "\n", "We can easily create a graphical representation of the turn the robot description into a [GraphViz dotfile](https://graphviz.org)\n" - ] + ], + "id": "ee1f4eb9" }, { "cell_type": "code", @@ -120,14 +125,16 @@ "outputs": [], "source": [ "robot.dotfile(\"planar_y.dot\")" - ] + ], + "id": "0f9b8925" }, { "cell_type": "markdown", "metadata": {}, "source": [ "which writes a file in the current folder. We then render it to SVG format and [load and display it](https://ipython.readthedocs.io/en/stable/api/generated/IPython.display.html?highlight=svg#IPython.display.SVG)" - ] + ], + "id": "9be2bf18" }, { "cell_type": "code", @@ -137,7 +144,8 @@ "source": [ "!dot -Tsvg planar_y.dot -x > planar_y.svg\n", "SVG(filename=\"planar_y.svg\")" - ] + ], + "id": "36593abb" }, { "cell_type": "markdown", @@ -148,14 +156,16 @@ "The ETS transforms in black indicate the joints, and they contain the five joint angles `q0` to `q5`. The assignement of joint numbers is arbitrary and decided by the creator of the robot model. All operations such as forward kinematics or Jacobians that accept a joint angle vector, organize it as `[q0, q1, q2, q3, q4 q5]`.\n", "\n", "Fixed joints are shown in blue. " - ] + ], + "id": "ee7b73be" }, { "cell_type": "markdown", "metadata": {}, "source": [ "The forward kinematics is quite straightforward, but we must specify which end-effector we are interested in" - ] + ], + "id": "11846548" }, { "cell_type": "code", @@ -164,14 +174,16 @@ "outputs": [], "source": [ "robot.fkine(robot.qz, end=\"eea\")" - ] + ], + "id": "fdcdda90" }, { "cell_type": "markdown", "metadata": {}, "source": [ "which is exactly the same as the pose of the second branch" - ] + ], + "id": "2fec3923" }, { "cell_type": "code", @@ -180,14 +192,16 @@ "outputs": [], "source": [ "robot.fkine(robot.qz, end=\"eeb\")" - ] + ], + "id": "9506e063" }, { "cell_type": "markdown", "metadata": {}, "source": [ "In a similar fashion we can also compute the Jacobian, in this case as a $6 \\times 4$ matrix" - ] + ], + "id": "a46a6bcd" }, { "cell_type": "code", @@ -196,14 +210,16 @@ "outputs": [], "source": [ "robot.jacob0(robot.qz, end=\"eea\")" - ] + ], + "id": "1ea285df" }, { "cell_type": "markdown", "metadata": {}, "source": [ "We can generate an ETS for the bottom-branch of the robot" - ] + ], + "id": "990a8207" }, { "cell_type": "code", @@ -213,14 +229,16 @@ "source": [ "p = robot.ets(end=\"eeb\")\n", "p" - ] + ], + "id": "fac103c3" }, { "cell_type": "markdown", "metadata": {}, "source": [ "and we could easily turn this into an unbranched robot represented by an `Robot` instance" - ] + ], + "id": "ec1eafbd" }, { "cell_type": "code", @@ -230,7 +248,8 @@ "source": [ "r = rtb.Robot(p, check_jindex=False)\n", "print(r)" - ] + ], + "id": "47dd0ff7" }, { "cell_type": "markdown", @@ -239,7 +258,8 @@ "This robot has 5 links but only 4 joint angles which are inherited from the original branched robot: `q0`, `q1`, `q4` and `q5`. Its `fkine` method accepts either a vector of the four joint angles, in order, or a vector of six elements indexed by the original joint numbers, in which case `q2` and `q3` are ignored.\n", "\n", "We can also find the transformation between any two links in the robot. In this case, from one end effector to the other, from link eea to link eeb" - ] + ], + "id": "216c43d3" }, { "cell_type": "code", @@ -249,14 +269,16 @@ "source": [ "p = robot.ets(start=\"eea\", end=\"eeb\")\n", "p" - ] + ], + "id": "5c4851ce" }, { "cell_type": "markdown", "metadata": {}, "source": [ "which is expressed as an ETS. Note that it is not in minimal form and the terms in the middle `tx(-1) * tx(1)` cancel out. We can precompile the constants which will take care of this for us" - ] + ], + "id": "bfa5dd51" }, { "cell_type": "code", @@ -265,7 +287,8 @@ "outputs": [], "source": [ "p.compile()" - ] + ], + "id": "8081cc08" }, { "cell_type": "markdown", @@ -274,7 +297,8 @@ "# Graph display variations\n", "\n", "This robot is entirely revolute, but we can visualize the joint types in the graphical representation by using the `jtype` keyword argument" - ] + ], + "id": "fce34f81" }, { "cell_type": "code", @@ -285,7 +309,8 @@ "robot.dotfile(\"planar_y.dot\", jtype=True)\n", "!dot -Tsvg planar_y.dot > planar_y.svg\n", "SVG(filename=\"planar_y.svg\")" - ] + ], + "id": "733672f5" }, { "cell_type": "markdown", @@ -294,7 +319,8 @@ "and the input to a link frame is a black dot if it is preceded by a revolute joint, and a black box if it preceded by a prismatic joint.\n", "\n", "We can also highlight the ETS by placing them in boxes, rather than being just edge labels, using the `etsbox` option" - ] + ], + "id": "fb39355d" }, { "cell_type": "code", @@ -305,7 +331,8 @@ "robot.dotfile(\"planar_y.dot\", etsbox=True)\n", "!dot -Tsvg planar_y.dot > planar_y.svg\n", "SVG(filename=\"planar_y.svg\")" - ] + ], + "id": "6f999740" }, { "cell_type": "markdown", @@ -314,7 +341,8 @@ "## PR2 model\n", "\n", "The PR2 is a very complex branched robot with nearly 90 links" - ] + ], + "id": "173f9240" }, { "cell_type": "code", @@ -326,14 +354,16 @@ "pr2.dotfile(\"pr2.dot\")\n", "!dot -Tsvg pr2.dot > pr2.svg\n", "SVG(filename=\"pr2.svg\")" - ] + ], + "id": "454a5ced" }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], - "source": [] + "source": [], + "id": "644b4e65" } ], "metadata": { @@ -382,15 +412,8 @@ "_Feature" ], "window_display": false - }, - "rtb_header": { - "title": "A branched robot", - "paragraphs": [ - "A robot with more than one end-effector: its forward kinematics and Jacobian for each branch, extracting the ETS of a branch, and drawing the robot as a graph. Drawing the graphs needs [Graphviz](https://graphviz.org) (the `dot` command) installed." - ], - "modernised": "2026-10-19" } }, "nbformat": 4, - "nbformat_minor": 4 + "nbformat_minor": 5 } diff --git a/docs/notebooks/dynamics.ipynb b/docs/notebooks/dynamics.ipynb index 76a003333..1fcc530d3 100644 --- a/docs/notebooks/dynamics.ipynb +++ b/docs/notebooks/dynamics.ipynb @@ -4,11 +4,10 @@ "cell_type": "markdown", "metadata": {}, "source": [ - "\n", "\n", " \n", " \n", - " \n", + " \n", " \n", "
Robotics Toolbox for Python
\n", "\n", @@ -19,7 +18,8 @@ "(c) Peter Corke 2026\n", "\n", "The inertia, velocity terms and gravity load of a Puma 560 robot, and its forward dynamics." - ] + ], + "id": "28f9bef8" }, { "cell_type": "code", @@ -73,7 +73,8 @@ " return colab\n", "\n", "COLAB = await ensure_installed()\n" - ] + ], + "id": "643c6a7b" }, { "cell_type": "code", @@ -98,14 +99,16 @@ "%matplotlib ipympl\n", "plt.close(\"all\")\n", "%config NotebookBackend.figure_format = 'retina'" - ] + ], + "id": "b51522bd" }, { "cell_type": "markdown", "metadata": {}, "source": [ "We will instantiate a model of the Puma 560 robot which has well known inertial parameters" - ] + ], + "id": "7058a6d2" }, { "cell_type": "code", @@ -116,14 +119,16 @@ "outputs": [], "source": [ "p560 = rtb.models.DH.Puma560()" - ] + ], + "id": "05a35b38" }, { "cell_type": "markdown", "metadata": {}, "source": [ "and show its configuration in a typical desktop working pose" - ] + ], + "id": "037b8347" }, { "cell_type": "code", @@ -134,7 +139,8 @@ "outputs": [], "source": [ "p560.plot(p560.qn);" - ] + ], + "id": "55c9eafa" }, { "cell_type": "markdown", @@ -154,7 +160,8 @@ "\\mathbf{g}(\\mathit{q}) = \\mathit{\\tau}\n", "$$\n", "where $\\mathit{\\tau}$ is the torque required for this condition $\\mathit{q} = \\mathit{\\dot{q}} = 0$ to be true, that is, the torque required to stop the robot falling under its own weight. The toolbox can compute this" - ] + ], + "id": "92192d3b" }, { "cell_type": "code", @@ -165,7 +172,8 @@ "outputs": [], "source": [ "p560.gravload(p560.qn)" - ] + ], + "id": "4e6cb1bb" }, { "cell_type": "markdown", @@ -174,7 +182,8 @@ "and it shows, as expected, that the shoulder is exerting significant torque to hold the arm up and stationary.\n", "\n", "The inertia matrix relates torque to joint acceleration and is the mass in a multi-dimensional version of Newton's second law $F = m a$. In this configuration the inertia matrix is" - ] + ], + "id": "4f123bc5" }, { "cell_type": "code", @@ -185,7 +194,8 @@ "outputs": [], "source": [ "p560.inertia(p560.qn)" - ] + ], + "id": "ff4a51b0" }, { "cell_type": "markdown", @@ -196,7 +206,8 @@ "The matrix is symmetric and the off-diagonal terms $M_{ij} = M_{ji}$ couple acceleration of one joint into a disturbance torque on another joint, ie. $\\tau_j = M_{ij} \\ddot{q}_i$.\n", "\n", "The inertia matrix is a function of joint configuration, that is, the elements of the inertia matrix change as we vary the angles of joints 1 and 2, ie. $q_2$ and $q_3$. It is this configuration varying inertia and coupling between joints that is a fundamental challenge for high-quality joint control." - ] + ], + "id": "62f5cd82" }, { "cell_type": "code", @@ -215,14 +226,16 @@ " M = p560.inertia(np.r_[0, Q2[i, j], Q3[i, j], 0, 0, 0])\n", " M11[i, j] = M[0, 0]\n", " M12[i, j] = M[0, 1]" - ] + ], + "id": "348a5802" }, { "cell_type": "markdown", "metadata": {}, "source": [ "The inertia \"seen\" by joint 1 varies as a function of $q_2$ and $q_3$ as shown below" - ] + ], + "id": "ef040af1" }, { "cell_type": "code", @@ -238,14 +251,16 @@ "ax.set_xlabel(\"$q_2$ (rad)\")\n", "ax.set_ylabel(\"$q_3$ (rad)\")\n", "ax.set_zlabel(\"$M_{11}$ ($kg.m^2$)\")" - ] + ], + "id": "9507f059" }, { "cell_type": "markdown", "metadata": {}, "source": [ "The ratio of maximum to minimum values is" - ] + ], + "id": "7d71f5fa" }, { "cell_type": "code", @@ -256,14 +271,16 @@ "outputs": [], "source": [ "M11.max() / M11.min()" - ] + ], + "id": "d6326739" }, { "cell_type": "markdown", "metadata": {}, "source": [ "The coupling inertia between joints 1 and 2 also varies with configuration and we can plot that as well" - ] + ], + "id": "d45959e5" }, { "cell_type": "code", @@ -279,14 +296,16 @@ "ax.set_xlabel(\"$q_2$ (rad)\")\n", "ax.set_ylabel(\"$q_3$ (rad)\")\n", "ax.set_zlabel(\"$M_{12}$ ($kg.m^2$)\")" - ] + ], + "id": "9eaffd99" }, { "cell_type": "markdown", "metadata": {}, "source": [ "The velocity terms are a bit harder to comprehend but they mean that rotation of one joint (and its link) can exert a torque on other joints. Consider that the should joint is rotating at 1 rad/sec, then the torque will be " - ] + ], + "id": "76e6c451" }, { "cell_type": "code", @@ -298,7 +317,8 @@ "source": [ "qd = np.r_[0, 1, 0, 0, 0, 0]\n", "p560.coriolis(p560.qn, qd) @ qd" - ] + ], + "id": "5f7d4b51" }, { "cell_type": "markdown", @@ -307,7 +327,8 @@ "and we see that it exerts a torque on the waist and elbow joints.\n", "\n", "The algorithms to compute the various terms in the rigid-body equations of motion are based on the recursive Newton-Euler algorithm" - ] + ], + "id": "1eec4c42" }, { "cell_type": "code", @@ -318,7 +339,8 @@ "outputs": [], "source": [ "p560.rne(p560.qn, np.zeros((6,)), np.zeros((6,)))" - ] + ], + "id": "80332c44" }, { "cell_type": "markdown", @@ -327,7 +349,8 @@ "which computes $\\tau = \\mbox{rne}(\\mathit{q}, \\mathit{\\dot{q}}, \\mathit{\\dot{q}})$ and can accept additional arguments such as gravity or a wrench applied to the end-effector. In the Toolbox this algorithm is implemented in C for maximum performance.\n", "\n", "The example above computes the gravity load, cross check it with the result computed earlier.\n" - ] + ], + "id": "6db5db6b" }, { "cell_type": "markdown", @@ -340,7 +363,8 @@ "which is the joint acceleration for a robot in the state $(\\mathit{q}, \\mathit{\\dot{q}})$ with an applied torque of $\\mathit{\\tau}$.\n", "\n", "If we integrate this we can compute the trajectory of the joint coordinates (and velocities) as a function of time" - ] + ], + "id": "c9546c97" }, { "cell_type": "code", @@ -352,7 +376,8 @@ "source": [ "p560nf = p560.nofriction()\n", "tg = p560nf.fdyn(5, p560.qn, dt=0.05)" - ] + ], + "id": "6b90ebc6" }, { "cell_type": "markdown", @@ -365,7 +390,8 @@ "Coulomb friction is a _harsh_ non-linearity and it causes the numerical integrator to take very small times steps, so the result will take many minutes to compute. To speed things up, at the expense of some modeling fidelity, we set the Coulomb friction to zero, but retain the viscous friction. The `nofriction()` method returns a clone of the robot with its friction parameters modified.\n", "\n", "The computed joint configuration trajectory is" - ] + ], + "id": "d3bf77a9" }, { "cell_type": "code", @@ -376,14 +402,16 @@ "outputs": [], "source": [ "tg.q" - ] + ], + "id": "536f859c" }, { "cell_type": "markdown", "metadata": {}, "source": [ "which we can plot using a Toolbox convenience function" - ] + ], + "id": "7c5477ba" }, { "cell_type": "code", @@ -394,14 +422,16 @@ "outputs": [], "source": [ "rtb.tools.plot.xplot(tg.t, tg.q)" - ] + ], + "id": "f7a070a7" }, { "cell_type": "markdown", "metadata": {}, "source": [ "or we can animate it, showing the robot collapsing under gravity" - ] + ], + "id": "0400ddbc" }, { "cell_type": "code", @@ -412,7 +442,8 @@ "outputs": [], "source": [ "p560.plot(tg.q);" - ] + ], + "id": "fdd61a45" }, { "cell_type": "markdown", @@ -421,7 +452,8 @@ "_Note: this animation is not working in Jupyter notebook..._\n", "\n", "The motion of the robot quickly dies out and it hangs downward, this loss of energy is due to the viscous friction in the robot's joints." - ] + ], + "id": "210b9389" } ], "metadata": { @@ -490,14 +522,8 @@ "version_major": 2, "version_minor": 0 } - }, - "rtb_header": { - "title": "Robot dynamics", - "paragraphs": [ - "The inertia, velocity terms and gravity load of a Puma 560 robot, and its forward dynamics." - ] } }, "nbformat": 4, - "nbformat_minor": 4 + "nbformat_minor": 5 } diff --git a/docs/notebooks/ets.ipynb b/docs/notebooks/ets.ipynb index 250df85ed..3fcc9e4dd 100644 --- a/docs/notebooks/ets.ipynb +++ b/docs/notebooks/ets.ipynb @@ -4,11 +4,10 @@ "cell_type": "markdown", "metadata": {}, "source": [ - "\n", "\n", " \n", " \n", - " \n", + " \n", " \n", "
Robotics Toolbox for Python
\n", "\n", @@ -21,7 +20,8 @@ "An introduction to the `ET` and `ETS` classes: building a robot model as a sequence of elementary transforms, evaluating its forward kinematics and Jacobians, and converting between an ETS and a robot.\n", "\n", "> **Modernised 2026-10-19** to address API drift: this notebook was written for an earlier release of the Toolbox, and has been updated so that it runs with the current one." - ] + ], + "id": "10582ade" }, { "cell_type": "code", @@ -75,7 +75,8 @@ " return colab\n", "\n", "COLAB = await ensure_installed()\n" - ] + ], + "id": "9160be55" }, { "cell_type": "code", @@ -89,7 +90,8 @@ "from spatialmath.base import sym\n", "\n", "%matplotlib ipympl" - ] + ], + "id": "aa0c8891" }, { "cell_type": "markdown", @@ -112,7 +114,8 @@ "Let's take a step back and consider defining a robot from a sequence of transforms drawn from the complete set of all possible elementary (or canonical) transformations $\\{r_x, r_y, r_z, t_x, t_y, t_z\\}$ any of which can be a function of a constant or a joint variable.\n", "\n", "The Robotics Toolbox implements this functionality. We will start simply\n" - ] + ], + "id": "3f6052da" }, { "cell_type": "code", @@ -123,7 +126,8 @@ "from roboticstoolbox import ET as E\n", "\n", "E.Rx(45, \"deg\")" - ] + ], + "id": "f95ded42" }, { "cell_type": "code", @@ -132,7 +136,8 @@ "outputs": [], "source": [ "E.ty(2)" - ] + ], + "id": "450063ee" }, { "cell_type": "code", @@ -141,7 +146,8 @@ "outputs": [], "source": [ "E.Ry()" - ] + ], + "id": "ef1861e6" }, { "cell_type": "markdown", @@ -152,7 +158,8 @@ "o-------o--<>---x \n", "\n", "where 'o' is a joint, 'x' is the end effector, the first link is 1m long, and the second link can change its length - it is a prismatic joint. The robot can be represented by just 4 elementary transforms" - ] + ], + "id": "ec7cbbf3" }, { "cell_type": "code", @@ -162,14 +169,16 @@ "source": [ "e = E.Rz() * E.tx(1) * E.Rz() * E.tx()\n", "e" - ] + ], + "id": "8f76fadf" }, { "cell_type": "markdown", "metadata": {}, "source": [ "`e` is an ETS object" - ] + ], + "id": "25bf41e4" }, { "cell_type": "code", @@ -178,14 +187,16 @@ "outputs": [], "source": [ "type(e)" - ] + ], + "id": "0df1b638" }, { "cell_type": "markdown", "metadata": {}, "source": [ "which has a number of methods including " - ] + ], + "id": "86669081" }, { "cell_type": "code", @@ -194,14 +205,16 @@ "outputs": [], "source": [ "len(e)" - ] + ], + "id": "a887d3db" }, { "cell_type": "markdown", "metadata": {}, "source": [ "indicating it has four transforms" - ] + ], + "id": "59214ecb" }, { "cell_type": "code", @@ -210,14 +223,16 @@ "outputs": [], "source": [ "e.n" - ] + ], + "id": "aeed5bd7" }, { "cell_type": "markdown", "metadata": {}, "source": [ "indicating it has three joint variables, and they occur at the indices given by" - ] + ], + "id": "9bb00c35" }, { "cell_type": "code", @@ -226,14 +241,16 @@ "outputs": [], "source": [ "e.joints()" - ] + ], + "id": "506f0f73" }, { "cell_type": "markdown", "metadata": {}, "source": [ "The ETS object acts a lot like a list, and we can slice it" - ] + ], + "id": "d7959cae" }, { "cell_type": "code", @@ -242,7 +259,8 @@ "outputs": [], "source": [ "e[1]" - ] + ], + "id": "43b2e0f6" }, { "cell_type": "code", @@ -251,14 +269,16 @@ "outputs": [], "source": [ "e[2:]" - ] + ], + "id": "3fa3fa85" }, { "cell_type": "markdown", "metadata": {}, "source": [ "There are also a number of predicates" - ] + ], + "id": "0242faeb" }, { "cell_type": "code", @@ -267,7 +287,8 @@ "outputs": [], "source": [ "e[0].isjoint" - ] + ], + "id": "2c7a8c92" }, { "cell_type": "code", @@ -276,7 +297,8 @@ "outputs": [], "source": [ "e[1].isjoint" - ] + ], + "id": "a7c3b2ef" }, { "cell_type": "code", @@ -285,7 +307,8 @@ "outputs": [], "source": [ "e[0].isrotation" - ] + ], + "id": "6f4aa252" }, { "cell_type": "code", @@ -294,7 +317,8 @@ "outputs": [], "source": [ "e[3].isrotation" - ] + ], + "id": "837d7307" }, { "cell_type": "code", @@ -303,14 +327,16 @@ "outputs": [], "source": [ "e[3].istranslation" - ] + ], + "id": "d34b4e70" }, { "cell_type": "markdown", "metadata": {}, "source": [ "We can substitute in values" - ] + ], + "id": "6377ab22" }, { "cell_type": "code", @@ -319,7 +345,8 @@ "outputs": [], "source": [ "e.eval([0, 0, 1])" - ] + ], + "id": "b0c00e67" }, { "cell_type": "markdown", @@ -328,7 +355,8 @@ "and the result is an SE(3) matrix encapsulated in an `SE3` instance. As expected is a translation of 2m in the x-direction. \n", "\n", "Let try another configuration" - ] + ], + "id": "be8e39ec" }, { "cell_type": "code", @@ -337,14 +365,16 @@ "outputs": [], "source": [ "e.eval([np.deg2rad(90), np.deg2rad(-90), 2])" - ] + ], + "id": "868b757f" }, { "cell_type": "markdown", "metadata": {}, "source": [ "Joint angles are in radians, so we converted the angles from degrees with `np.deg2rad`. The prismatic joint value is a length in metres and is left as it is." - ] + ], + "id": "905574d2" }, { "cell_type": "markdown", @@ -353,7 +383,8 @@ "## Joint flip\n", "\n", "Sometimes when we are describing a system of joints and constant transforms, the joint coordinate sense needs to be reversed to meet the user's definition. In the example above, consider that the first joint increases positively in the clockwise direction. In that case we could write" - ] + ], + "id": "943c4184" }, { "cell_type": "code", @@ -363,14 +394,16 @@ "source": [ "e = E.Rz(flip=True) * E.tx(1) * E.Rz() * E.tx()\n", "e" - ] + ], + "id": "f1c519dc" }, { "cell_type": "markdown", "metadata": {}, "source": [ "and the flip is indicated by a negative sign `Rz(-q0)` when the ETS is displayed." - ] + ], + "id": "f013f36a" }, { "cell_type": "code", @@ -379,7 +412,8 @@ "outputs": [], "source": [ "e.eval([np.deg2rad(-90), np.deg2rad(-90), 2])" - ] + ], + "id": "8971de1e" }, { "cell_type": "markdown", @@ -388,7 +422,8 @@ "Now if we negate the passed value of q0 we see that we have compute the same answer as above.\n", "\n", "Joint flip is handled implicitly, but can be tested" - ] + ], + "id": "3da01f41" }, { "cell_type": "code", @@ -397,7 +432,8 @@ "outputs": [], "source": [ "e[0].isflip" - ] + ], + "id": "61e98e5b" }, { "cell_type": "code", @@ -406,7 +442,8 @@ "outputs": [], "source": [ "e[1].isflip" - ] + ], + "id": "645710e5" }, { "cell_type": "markdown", @@ -414,7 +451,8 @@ "source": [ "## Symbolics\n", "\n" - ] + ], + "id": "492b1d4b" }, { "cell_type": "code", @@ -424,14 +462,16 @@ "source": [ "q = sym.symbol(\"q_:3\")\n", "print(q)" - ] + ], + "id": "24641d96" }, { "cell_type": "markdown", "metadata": {}, "source": [ "Which creates a vector of three joint angle variables. The advantage of having the underscore in the name is that Jupyter considers this as LaTeX subscript notation when it pretty prints the symbols" - ] + ], + "id": "c2a4d56a" }, { "cell_type": "code", @@ -440,14 +480,16 @@ "outputs": [], "source": [ "q[0]" - ] + ], + "id": "c0d66855" }, { "cell_type": "markdown", "metadata": {}, "source": [ "We can substitute in the symbolic values just as easily as we did the numerical values to find the resulting overall transform" - ] + ], + "id": "ee17b8a5" }, { "cell_type": "code", @@ -456,7 +498,8 @@ "outputs": [], "source": [ "e.eval(q)" - ] + ], + "id": "1f2f6981" }, { "cell_type": "markdown", @@ -465,7 +508,8 @@ "## ETS in 2D\n", "\n", "Everything that we've done in 3D we can do in 2D. In fact the example above was a planar problem, so we can express it easily using 2D ETS" - ] + ], + "id": "d75bb33b" }, { "cell_type": "code", @@ -476,7 +520,8 @@ "from roboticstoolbox import ET2 as E2\n", "\n", "e2 = E2.R() * E2.tx(1) * E2.R() * E2.tx()" - ] + ], + "id": "dbaa7737" }, { "cell_type": "markdown", @@ -485,7 +530,8 @@ "The main difference is that in 2D there are three elementary transforms to choose from $\\{r, t_x, t_y\\}$ - rotation and translation in the plane.\n", "\n", "If we evaluate this" - ] + ], + "id": "dc29781c" }, { "cell_type": "code", @@ -494,14 +540,16 @@ "outputs": [], "source": [ "e2.eval([np.deg2rad(90), np.deg2rad(-90), 2])" - ] + ], + "id": "7d6b9f09" }, { "cell_type": "markdown", "metadata": {}, "source": [ "and the result this time is an SE(2) matrix encapsulated in an `SE2` instance." - ] + ], + "id": "4cd9a23f" }, { "cell_type": "markdown", @@ -513,7 +561,8 @@ "\n", "Reference:\n", "- [A simple and systematic approach to assigning Denavit-Hartenberg parameters](https://petercorke.com/robotics/a-simple-and-systematic-approach-to-assigning-denavit-hartenberg-parameters), Peter I. Corke, IEEE Transactions on Robotics, 23(3), pp 590-594, June 2007." - ] + ], + "id": "159d08f4" }, { "cell_type": "code", @@ -527,14 +576,16 @@ "l4 = -0.0837\n", "l5 = 0.4318\n", "l6 = 0.0203" - ] + ], + "id": "5e622b60" }, { "cell_type": "markdown", "metadata": {}, "source": [ "and we can describe the tip of the robot by a sequence of elementary transforms" - ] + ], + "id": "3904e7f8" }, { "cell_type": "code", @@ -558,7 +609,8 @@ " * E.tx(0.2)\n", ")\n", "e" - ] + ], + "id": "f944d7c7" }, { "cell_type": "code", @@ -567,7 +619,8 @@ "outputs": [], "source": [ "e.n" - ] + ], + "id": "f3403ac2" }, { "cell_type": "code", @@ -576,7 +629,8 @@ "outputs": [], "source": [ "e.joints()" - ] + ], + "id": "5cc84d5f" }, { "cell_type": "code", @@ -585,14 +639,16 @@ "outputs": [], "source": [ "e.eval([0] * 6)" - ] + ], + "id": "4b359ba4" }, { "cell_type": "markdown", "metadata": {}, "source": [ "We can compute not only forward kinematics, but also the differential kinematics" - ] + ], + "id": "6cd527a2" }, { "cell_type": "code", @@ -602,7 +658,8 @@ "source": [ "J = e.jacobe([0] * 6)\n", "J" - ] + ], + "id": "cf24b992" }, { "cell_type": "markdown", @@ -612,14 +669,16 @@ "The rate of change of the end-effector position is its velocity, but because it moves in 3D we need to describe its translational velocity (a 3-vector) and its angular velocity (another 3-vector). We pack these into a 6-vector which is known as _spatial velocity_ and denoted by $\\nu$. These quantities are related by\n", "\n", "$\\nu = \\mathbf{J}(q) \\dot{q}$" - ] + ], + "id": "519116f1" }, { "cell_type": "markdown", "metadata": {}, "source": [ "The Hessian is a tensor (3-dimensional matrix) that relates joint velocity to end-effector acceleration $\\dot{\\nu}$" - ] + ], + "id": "357b6ba5" }, { "cell_type": "code", @@ -628,7 +687,8 @@ "outputs": [], "source": [ "e.hessian0([0] * 6)" - ] + ], + "id": "4461a5cc" }, { "cell_type": "markdown", @@ -637,7 +697,8 @@ "## Compilation\n", "\n", "If we were to evaluate this ETS many times we would be peforming lots of unneccessary multiplication of constant terms such as " - ] + ], + "id": "83559b57" }, { "cell_type": "code", @@ -646,14 +707,16 @@ "outputs": [], "source": [ "e[4:7]" - ] + ], + "id": "7288738e" }, { "cell_type": "markdown", "metadata": {}, "source": [ "To make an ETS more efficient to run we can \"compile it\"" - ] + ], + "id": "9ffd4455" }, { "cell_type": "code", @@ -663,14 +726,16 @@ "source": [ "ec = e.compile()\n", "ec" - ] + ], + "id": "1873953a" }, { "cell_type": "markdown", "metadata": {}, "source": [ "and we can see that consecutive constant transforms have been folded into new \"not so elementary\" transforms denoted by `Ci`. The value of the ETS, is of course the same." - ] + ], + "id": "15fac11a" }, { "cell_type": "code", @@ -679,7 +744,8 @@ "outputs": [], "source": [ "ec.eval([0] * 6)" - ] + ], + "id": "e0288d66" }, { "cell_type": "markdown", @@ -688,7 +754,8 @@ "## Explicit joint indices\n", "\n", "Consider that we carve out a chunk of this ETS that represents the Puma560, say the wrist bit" - ] + ], + "id": "a4004687" }, { "cell_type": "code", @@ -698,14 +765,16 @@ "source": [ "wrist = e[9:]\n", "wrist" - ] + ], + "id": "2dd8c273" }, { "cell_type": "markdown", "metadata": {}, "source": [ "we see that by default the wrist joints, joint 3-5 in the robot are associated with joints 0-2 in the wrist. If we had a vector of six joint angles, corresponding to the whole robot " - ] + ], + "id": "d966ca43" }, { "cell_type": "code", @@ -714,14 +783,16 @@ "outputs": [], "source": [ "q = [0.1, 0.2, 0.3, 0.4, 0.5, 0.6]" - ] + ], + "id": "ea0ef9d1" }, { "cell_type": "markdown", "metadata": {}, "source": [ "then to evaluate the wrist we'd have to write" - ] + ], + "id": "a6bbabf7" }, { "cell_type": "code", @@ -730,7 +801,8 @@ "outputs": [], "source": [ "wrist.eval(q[3:])" - ] + ], + "id": "67a160ee" }, { "cell_type": "markdown", @@ -739,7 +811,8 @@ "which is quite doable, but imagine now that the ETS represents the transforms from the right hand to the left hand of a two armed robot. The joint numbers in that ETS would not be sequential with respect to the overall joint coordinate vector of the whole robot.\n", "\n", "To help deal with such situations we can assign a joint index to each ETS. We'll do this for the Puma 560" - ] + ], + "id": "03d7473d" }, { "cell_type": "code", @@ -763,14 +836,16 @@ " * E.tx(0.2)\n", ")\n", "e" - ] + ], + "id": "53383d22" }, { "cell_type": "markdown", "metadata": {}, "source": [ "and when we display the ETS it looks no different, since the joint indices we assigned match the default sequential assignment. However if we now take the wrist part" - ] + ], + "id": "acd9ac3f" }, { "cell_type": "code", @@ -780,7 +855,8 @@ "source": [ "wrist = e[9:]\n", "wrist" - ] + ], + "id": "9bd1d80f" }, { "cell_type": "markdown", @@ -789,7 +865,8 @@ "we see that the explicit joint indices are displayed as arguments of the ETs.\n", "\n", "Now if evaluate the wrist part" - ] + ], + "id": "8a03ec2c" }, { "cell_type": "code", @@ -798,14 +875,16 @@ "outputs": [], "source": [ "wrist.eval(q)" - ] + ], + "id": "2856e577" }, { "cell_type": "markdown", "metadata": {}, "source": [ "we get the same answer as earlier because it is referencing `q[3]`, `q[4]` and `q[5]`." - ] + ], + "id": "18dec36c" }, { "cell_type": "markdown", @@ -825,7 +904,8 @@ "\n", "\n", "These extra transformation incur minimal computational overhead, since after compilation, they will be folded in with other constant transforms.\n" - ] + ], + "id": "529d6c3c" }, { "cell_type": "markdown", @@ -834,7 +914,8 @@ "## Converting an ETS to a robot\n", "\n", "The ETS succinctly describes the forward kinematics of a robot and we can also compute its Jacobian and Hessian. However Toolbox robot objects have additional capability for graphics, inverse kinematics and dynamics. We can _promote_ an ETS to a robot by" - ] + ], + "id": "eb9d0f73" }, { "cell_type": "code", @@ -844,7 +925,8 @@ "source": [ "robot = rtb.ERobot(e)\n", "print(robot)" - ] + ], + "id": "bcebdbc3" }, { "cell_type": "markdown", @@ -853,7 +935,8 @@ "The ETS has been chopped into segments that connect links. Each link is a rigid-body with an attached coordinate frame. Relative to that frame is a short ETS that ends with joint transform that describes the coordinate frame of the next link in the chain.\n", "\n", "Note that the last link, with its name in blue, has no joint transform. It is simply a constant relative pose with respect to the link 5 coordinate frame." - ] + ], + "id": "28861093" }, { "cell_type": "markdown", @@ -862,7 +945,8 @@ "## Converting a robot to ETS\n", "\n", "We can also perform the inverse transformation. Consider a model defined using Denavit-Hartenberg notation" - ] + ], + "id": "3dfbbb58" }, { "cell_type": "code", @@ -872,7 +956,8 @@ "source": [ "puma = rtb.models.DH.Puma560()\n", "print(puma)" - ] + ], + "id": "e075473b" }, { "cell_type": "code", @@ -881,14 +966,16 @@ "outputs": [], "source": [ "print(puma.ets())" - ] + ], + "id": "8e5dcad8" }, { "cell_type": "markdown", "metadata": {}, "source": [ "Or for a robot with a prismatic joint" - ] + ], + "id": "735dfa27" }, { "cell_type": "code", @@ -898,7 +985,8 @@ "source": [ "stanford = rtb.models.DH.Stanford()\n", "print(stanford)" - ] + ], + "id": "ba16fc91" }, { "cell_type": "code", @@ -907,14 +995,16 @@ "outputs": [], "source": [ "print(stanford.ets())" - ] + ], + "id": "97bf38a1" }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], - "source": [] + "source": [], + "id": "c551fbe0" } ], "metadata": { @@ -963,15 +1053,8 @@ "_Feature" ], "window_display": false - }, - "rtb_header": { - "title": "Elementary transform sequences", - "paragraphs": [ - "An introduction to the `ET` and `ETS` classes: building a robot model as a sequence of elementary transforms, evaluating its forward kinematics and Jacobians, and converting between an ETS and a robot." - ], - "modernised": "2026-10-19" } }, "nbformat": 4, - "nbformat_minor": 4 + "nbformat_minor": 5 } diff --git a/docs/notebooks/graphics-test.ipynb b/docs/notebooks/graphics-test.ipynb index 94d6112c0..6092f8401 100644 --- a/docs/notebooks/graphics-test.ipynb +++ b/docs/notebooks/graphics-test.ipynb @@ -4,11 +4,10 @@ "cell_type": "markdown", "metadata": {}, "source": [ - "\n", "\n", " \n", " \n", - " \n", + " \n", " \n", "
Robotics Toolbox for Python
\n", "\n", @@ -19,7 +18,8 @@ "(c) Peter Corke 2026\n", "\n", "A one-cell check that plotting a robot works in your environment." - ] + ], + "id": "362a8fec" }, { "cell_type": "code", @@ -73,7 +73,8 @@ " return colab\n", "\n", "COLAB = await ensure_installed()\n" - ] + ], + "id": "b243eb35" }, { "cell_type": "code", @@ -88,7 +89,8 @@ "puma = models.DH.Puma560()\n", "\n", "env = puma.plot(puma.qn)" - ] + ], + "id": "bb43571a" } ], "metadata": { @@ -112,14 +114,8 @@ "pygments_lexer": "ipython3", "version": "3.8.5" }, - "orig_nbformat": 4, - "rtb_header": { - "title": "Graphics test", - "paragraphs": [ - "A one-cell check that plotting a robot works in your environment." - ] - } + "orig_nbformat": 4 }, "nbformat": 4, - "nbformat_minor": 2 + "nbformat_minor": 5 } diff --git a/docs/notebooks/icra2021.ipynb b/docs/notebooks/icra2021.ipynb index 8e6b2d387..df8f0c5c4 100644 --- a/docs/notebooks/icra2021.ipynb +++ b/docs/notebooks/icra2021.ipynb @@ -4,11 +4,10 @@ "cell_type": "markdown", "metadata": {}, "source": [ - "\n", "\n", " \n", " \n", - " \n", + " \n", " \n", "
Robotics Toolbox for Python
\n", "\n", @@ -23,7 +22,8 @@ "This is the code for the examples in the paper published at ICRA 2021.\n", "\n", "> **Modernised 2026-10-19** to address API drift: this notebook was written for an earlier release of the Toolbox, and has been updated so that it runs with the current one." - ] + ], + "id": "0065faab" }, { "cell_type": "code", @@ -77,7 +77,8 @@ " return colab\n", "\n", "COLAB = await ensure_installed()\n" - ] + ], + "id": "1a59f089" }, { "cell_type": "code", @@ -110,14 +111,16 @@ " )\n", ")\n", "%matplotlib ipympl" - ] + ], + "id": "9bdb77dd" }, { "cell_type": "markdown", "metadata": {}, "source": [ "# III.SPATIAL MATHEMATICS" - ] + ], + "id": "fccd7bc3" }, { "cell_type": "code", @@ -128,7 +131,8 @@ "from spatialmath.base import *\n", "\n", "T = transl(0.5, 0.0, 0.0) @ rpy2tr(0.1, 0.2, 0.3, order=\"xyz\") @ trotx(-90, \"deg\")" - ] + ], + "id": "575d2335" }, { "cell_type": "code", @@ -139,7 +143,8 @@ "from spatialmath import *\n", "\n", "T = SE3(0.5, 0.0, 0.0) * SE3.RPY([0.1, 0.2, 0.3], order=\"xyz\") * SE3.Rx(-90, unit=\"deg\")" - ] + ], + "id": "5eafdde4" }, { "cell_type": "code", @@ -148,7 +153,8 @@ "outputs": [], "source": [ "T.eul()" - ] + ], + "id": "4765e0c8" }, { "cell_type": "code", @@ -157,7 +163,8 @@ "outputs": [], "source": [ "T.R" - ] + ], + "id": "87f59a29" }, { "cell_type": "code", @@ -166,7 +173,8 @@ "outputs": [], "source": [ "T.plot(color=\"red\", frame=\"2\")" - ] + ], + "id": "b9ab96bc" }, { "cell_type": "code", @@ -176,7 +184,8 @@ "source": [ "UnitQuaternion.Rx(0.3)\n", "UnitQuaternion.AngVec(0.3, [1, 0, 0])" - ] + ], + "id": "5887c919" }, { "cell_type": "code", @@ -186,7 +195,8 @@ "source": [ "R = SE3.Rx(np.linspace(0, pi / 2, num=100))\n", "len(R)" - ] + ], + "id": "a8c9c5b0" }, { "cell_type": "markdown", @@ -194,7 +204,8 @@ "source": [ "# IV. ROBOTICS TOOLBOX\n", "## A. Robot models" - ] + ], + "id": "73993f11" }, { "cell_type": "code", @@ -210,7 +221,8 @@ "a2 = 0.360\n", "d4 = 0.380\n", "d6 = 0.065;" - ] + ], + "id": "e8ce61e1" }, { "cell_type": "code", @@ -226,7 +238,8 @@ " ],\n", " name=\"my IRB140\",\n", ")" - ] + ], + "id": "95745a32" }, { "cell_type": "code", @@ -235,7 +248,8 @@ "outputs": [], "source": [ "puma = models.DH.Puma560()" - ] + ], + "id": "a350f331" }, { "cell_type": "code", @@ -244,7 +258,8 @@ "outputs": [], "source": [ "T = puma.fkine([0.1, 0.2, 0.3, 0.4, 0.5, 0.6])" - ] + ], + "id": "d202e6f5" }, { "cell_type": "code", @@ -253,7 +268,8 @@ "outputs": [], "source": [ "sol = puma.ikine_LM(T)" - ] + ], + "id": "efc6d9ee" }, { "cell_type": "code", @@ -262,7 +278,8 @@ "outputs": [], "source": [ "puma.plot(sol.q);" - ] + ], + "id": "df8baf59" }, { "cell_type": "code", @@ -271,7 +288,8 @@ "outputs": [], "source": [ "puma.ikine_a(T, config=\"lun\")" - ] + ], + "id": "9dc57bf5" }, { "cell_type": "code", @@ -280,7 +298,8 @@ "outputs": [], "source": [ "from roboticstoolbox import ET" - ] + ], + "id": "98410a92" }, { "cell_type": "code", @@ -295,7 +314,8 @@ "l4 = 0.0203\n", "l5 = 0.0837\n", "l6 = 0.4318;" - ] + ], + "id": "e32ae17a" }, { "cell_type": "code", @@ -317,7 +337,8 @@ " * ET.Ry()\n", " * ET.Rz()\n", ")" - ] + ], + "id": "ed5d45bc" }, { "cell_type": "code", @@ -327,7 +348,8 @@ "source": [ "robot = Robot(e)\n", "print(robot)" - ] + ], + "id": "7fedc2ec" }, { "cell_type": "code", @@ -339,14 +361,16 @@ "source": [ "panda = models.URDF.Panda()\n", "print(panda)" - ] + ], + "id": "477c2901" }, { "cell_type": "markdown", "metadata": {}, "source": [ "## B. Trajectories" - ] + ], + "id": "796c2401" }, { "cell_type": "code", @@ -356,7 +380,8 @@ "source": [ "traj = jtraj(puma.qz, puma.qr, 100)\n", "traj.qplot()" - ] + ], + "id": "f0c3fa38" }, { "cell_type": "code", @@ -366,7 +391,8 @@ "source": [ "t = np.arange(0, 2, 0.010)\n", "T0 = SE3(0.6, -0.5, 0.3)" - ] + ], + "id": "b275c405" }, { "cell_type": "code", @@ -375,7 +401,8 @@ "outputs": [], "source": [ "T1 = SE3(0.4, 0.5, 0.2)" - ] + ], + "id": "6b690343" }, { "cell_type": "code", @@ -385,7 +412,8 @@ "source": [ "Ts = ctraj(T0, T1, t)\n", "len(Ts)" - ] + ], + "id": "127e2a37" }, { "cell_type": "code", @@ -395,14 +423,16 @@ "source": [ "sol = puma.ikine_LM(Ts)\n", "sol.q.shape" - ] + ], + "id": "b04aa6d3" }, { "cell_type": "markdown", "metadata": {}, "source": [ "## C. Symbolic manipulation" - ] + ], + "id": "71fd6549" }, { "cell_type": "code", @@ -414,7 +444,8 @@ "\n", "phi, theta, psi = sym.symbol(\"φ, ϴ, ψ\")\n", "rpy2r(phi, theta, psi)" - ] + ], + "id": "9dc16ca6" }, { "cell_type": "code", @@ -424,7 +455,8 @@ "source": [ "q = sym.symbol(\"q_:6\") # q = (q_1, q_2, ... q_5)\n", "T = puma.fkine(q);" - ] + ], + "id": "a333f26b" }, { "cell_type": "code", @@ -435,7 +467,8 @@ "puma = models.DH.Puma560(symbolic=True)\n", "T = puma.fkine(q)\n", "T.t[0]" - ] + ], + "id": "221ef01e" }, { "cell_type": "code", @@ -445,7 +478,8 @@ "source": [ "puma = models.DH.Puma560(symbolic=False)\n", "J = puma.jacob0(puma.qn)" - ] + ], + "id": "c10327f1" }, { "cell_type": "code", @@ -454,14 +488,16 @@ "outputs": [], "source": [ "J = puma.jacobe(puma.qn)" - ] + ], + "id": "d26cf89c" }, { "cell_type": "markdown", "metadata": {}, "source": [ "## D. Differential kinematics" - ] + ], + "id": "3e1af871" }, { "cell_type": "code", @@ -470,7 +506,8 @@ "outputs": [], "source": [ "J = puma.jacob0(puma.qr)" - ] + ], + "id": "06a7bfae" }, { "cell_type": "code", @@ -479,7 +516,8 @@ "outputs": [], "source": [ "np.linalg.matrix_rank(J)" - ] + ], + "id": "bfd86d43" }, { "cell_type": "code", @@ -488,7 +526,8 @@ "outputs": [], "source": [ "jsingu(J)" - ] + ], + "id": "f19c45e1" }, { "cell_type": "code", @@ -498,7 +537,8 @@ "source": [ "H = panda.hessian0(panda.qz)\n", "H.shape" - ] + ], + "id": "bf603029" }, { "cell_type": "code", @@ -507,7 +547,8 @@ "outputs": [], "source": [ "puma.manipulability(puma.qn)" - ] + ], + "id": "645ff100" }, { "cell_type": "code", @@ -516,7 +557,8 @@ "outputs": [], "source": [ "puma.manipulability(puma.qn, method=\"asada\")" - ] + ], + "id": "7267b88a" }, { "cell_type": "code", @@ -525,7 +567,8 @@ "outputs": [], "source": [ "puma.manipulability(puma.qn, axes=\"trans\")" - ] + ], + "id": "bb095c57" }, { "cell_type": "code", @@ -536,14 +579,16 @@ "outputs": [], "source": [ "panda.jacobm(panda.qr)" - ] + ], + "id": "3ef4e6ec" }, { "cell_type": "markdown", "metadata": {}, "source": [ "## E. Dynamics" - ] + ], + "id": "df72eb26" }, { "cell_type": "code", @@ -554,7 +599,8 @@ "outputs": [], "source": [ "tau = puma.rne(puma.qn, np.zeros((6,)), np.zeros((6,)))" - ] + ], + "id": "dcbd3b3e" }, { "cell_type": "code", @@ -565,7 +611,8 @@ "outputs": [], "source": [ "J = puma.inertia(puma.qn)" - ] + ], + "id": "facc4147" }, { "cell_type": "code", @@ -576,7 +623,8 @@ "outputs": [], "source": [ "C = puma.coriolis(puma.qn, 0.1 * np.ones((6,)))" - ] + ], + "id": "4447e527" }, { "cell_type": "code", @@ -587,7 +635,8 @@ "outputs": [], "source": [ "g = puma.gravload(puma.qn)" - ] + ], + "id": "436a1633" }, { "cell_type": "code", @@ -598,7 +647,8 @@ "outputs": [], "source": [ "qdd = puma.accel(puma.qn, tau, np.zeros((6,)))" - ] + ], + "id": "9f1cd0c9" }, { "cell_type": "markdown", @@ -606,7 +656,8 @@ "source": [ "# V. NEW CAPABILITY\n", "## B. Collision checking" - ] + ], + "id": "931b155c" }, { "cell_type": "code", @@ -619,7 +670,8 @@ "obstacle = Cuboid([1, 1, 1], pose=SE3(1, 0, 0))\n", "iscollision = panda.iscollided(panda.qr, obstacle) # boolean\n", "iscollision = panda.links[0].iscollided(obstacle)" - ] + ], + "id": "839ce028" }, { "cell_type": "code", @@ -631,14 +683,16 @@ "print(d, p1, p2)\n", "d, p1, p2 = panda.links[0].closest_point(obstacle)\n", "print(d, p1, p2)" - ] + ], + "id": "4e67c4d9" }, { "cell_type": "markdown", "metadata": {}, "source": [ "## C. Interfaces" - ] + ], + "id": "c1fcd470" }, { "cell_type": "code", @@ -647,7 +701,8 @@ "outputs": [], "source": [ "panda.plot(panda.qr);" - ] + ], + "id": "7d052b79" }, { "cell_type": "code", @@ -662,7 +717,8 @@ "backend.add(panda) # add robot to the world\n", "panda.q = panda.qr # update the robot\n", "backend.step() # display the world" - ] + ], + "id": "b3d35544" } ], "metadata": { @@ -720,17 +776,8 @@ "_Feature" ], "window_display": false - }, - "rtb_header": { - "title": "Not your grandmother’s toolbox: the Robotics Toolbox reinvented for Python", - "paragraphs": [ - "Peter Corke and Jesse Haviland", - "This is the code for the examples in the paper published at ICRA 2021." - ], - "copyright": "Peter Corke and Jesse Haviland 2021, updated 2026", - "modernised": "2026-10-19" } }, "nbformat": 4, - "nbformat_minor": 4 + "nbformat_minor": 5 } diff --git a/docs/notebooks/icra2026.ipynb b/docs/notebooks/icra2026.ipynb index 77a186e40..3d0e299d5 100644 --- a/docs/notebooks/icra2026.ipynb +++ b/docs/notebooks/icra2026.ipynb @@ -4,11 +4,10 @@ "cell_type": "markdown", "metadata": {}, "source": [ - "\n", "\n", " \n", " \n", - " \n", + " \n", " \n", "
Robotics Toolbox for Python
\n", "\n", @@ -19,7 +18,8 @@ "(c) Peter Corke 2026\n", "\n", "Presented at ICRA 2026 as Keynote Tutorial 3 (the \"lost tutorial\")" - ] + ], + "id": "4b9f8521" }, { "cell_type": "code", @@ -73,7 +73,8 @@ " return colab\n", "\n", "COLAB = await ensure_installed()\n" - ] + ], + "id": "8c2c33ae" }, { "cell_type": "code", @@ -1127,12 +1128,6 @@ "nbconvert_exporter": "python", "pygments_lexer": "ipython3", "version": "3.10.16" - }, - "rtb_header": { - "title": "🤖🚀 Robotics without the cruft", - "paragraphs": [ - "Presented at ICRA 2026 as Keynote Tutorial 3 (the \"lost tutorial\")" - ] } }, "nbformat": 4, diff --git a/docs/notebooks/ik-symbolic.ipynb b/docs/notebooks/ik-symbolic.ipynb index 338edf904..67c78990d 100644 --- a/docs/notebooks/ik-symbolic.ipynb +++ b/docs/notebooks/ik-symbolic.ipynb @@ -4,11 +4,10 @@ "cell_type": "markdown", "metadata": {}, "source": [ - "\n", "\n", " \n", " \n", - " \n", + " \n", " \n", "
Robotics Toolbox for Python
\n", "\n", @@ -17,7 +16,8 @@ "

\n", "\n", "(c) Peter Corke 2026" - ] + ], + "id": "f20c8df0" }, { "cell_type": "markdown", @@ -92,7 +92,8 @@ " return colab\n", "\n", "COLAB = await ensure_installed(extras=['ik'])\n" - ] + ], + "id": "cf4a9e24" }, { "cell_type": "code", @@ -809,10 +810,6 @@ "extras": [ "ik" ] - }, - "rtb_header": { - "title": "Analytic inverse kinematics", - "paragraphs": [] } }, "nbformat": 4, diff --git a/docs/notebooks/ik_benchmark.ipynb b/docs/notebooks/ik_benchmark.ipynb index 3edc2761f..fcd43ff0a 100644 --- a/docs/notebooks/ik_benchmark.ipynb +++ b/docs/notebooks/ik_benchmark.ipynb @@ -4,11 +4,10 @@ "cell_type": "markdown", "metadata": {}, "source": [ - "\n", "\n", " \n", " \n", - " \n", + " \n", " \n", "
Robotics Toolbox for Python
\n", "\n", @@ -17,7 +16,8 @@ "

\n", "\n", "(c) Peter Corke 2026" - ] + ], + "id": "d8cda822" }, { "cell_type": "code", @@ -71,7 +71,8 @@ " return colab\n", "\n", "COLAB = await ensure_installed()\n" - ] + ], + "id": "c937d041" }, { "cell_type": "code", @@ -106,7 +107,8 @@ "import warnings\n", "\n", "warnings.filterwarnings(\"ignore\")" - ] + ], + "id": "81f01e8a" }, { "attachments": {}, @@ -131,7 +133,8 @@ "\n", "[3.0 Python Class Based IK Methods](#3)\n", "\n" - ] + ], + "id": "08b20209" }, { "attachments": {}, @@ -144,7 +147,8 @@ "\n", "### 1.0 Fast IK Methods\n", "---" - ] + ], + "id": "ebf33971" }, { "cell_type": "code", @@ -155,7 +159,8 @@ "# Our robot and ETS\n", "robot = rtb.models.Panda()\n", "ets = robot.ets()" - ] + ], + "id": "c48854fb" }, { "cell_type": "code", @@ -188,7 +193,8 @@ "tol = 1e-6\n", "\n", "joint_limits = False" - ] + ], + "id": "4e755bd7" }, { "cell_type": "code", @@ -257,7 +263,8 @@ "]\n", "\n", "times = []" - ] + ], + "id": "17b2b8c8" }, { "cell_type": "code", @@ -296,7 +303,8 @@ " )\n", "\n", "table.print()" - ] + ], + "id": "6a0ddecb" } ], "metadata": { @@ -322,11 +330,8 @@ "interpreter": { "hash": "4e8f2602a343ae62ff483b4df7805ab0eb447ccc1a56caf0631d5620d36b21ab" } - }, - "rtb_header": { - "title": "Numerical inverse kinematics benchmark" } }, "nbformat": 4, - "nbformat_minor": 2 + "nbformat_minor": 5 } diff --git a/docs/notebooks/index.ipynb b/docs/notebooks/index.ipynb index 0f37cfddc..08419dbbd 100644 --- a/docs/notebooks/index.ipynb +++ b/docs/notebooks/index.ipynb @@ -7,7 +7,7 @@ "\n", " \n", " \n", - " \n", + " \n", " \n", "
Robotics Toolbox for Python
\n", "\n", @@ -20,7 +20,8 @@ "Notebooks:\n", "\n", "- [ICRA 2026 tutorial: a tour of the Robotics Toolbox](icra2026.ipynb)\n" - ] + ], + "id": "c63145b0" } ], "metadata": { diff --git a/docs/notebooks/iros2020.ipynb b/docs/notebooks/iros2020.ipynb index c2921cd35..f6c236c39 100644 --- a/docs/notebooks/iros2020.ipynb +++ b/docs/notebooks/iros2020.ipynb @@ -4,11 +4,10 @@ "cell_type": "markdown", "metadata": {}, "source": [ - "\n", "\n", " \n", " \n", - " \n", + " \n", " \n", "
Robotics Toolbox for Python
\n", "\n", @@ -19,7 +18,8 @@ "(c) Peter Corke 2020, updated 2026\n", "\n", "> **Modernised 2026-10-19** to address API drift: this notebook was written for an earlier release of the Toolbox, and has been updated so that it runs with the current one." - ] + ], + "id": "42f1fc26" }, { "cell_type": "markdown", @@ -28,7 +28,8 @@ "This notebook corresponds to my virtual tutorial at the IROS 2020 workshop on Screw Theory and Geometric Robot Dynamics.\n", "\n", "First, import some packages, and set things up just so." - ] + ], + "id": "84252cb7" }, { "cell_type": "code", @@ -82,7 +83,8 @@ " return colab\n", "\n", "COLAB = await ensure_installed()\n" - ] + ], + "id": "c7709dc8" }, { "cell_type": "code", @@ -104,7 +106,8 @@ "\n", "%matplotlib ipympl\n", "plt.close(\"all\")" - ] + ], + "id": "caa1b535" }, { "cell_type": "markdown", @@ -112,7 +115,8 @@ "source": [ "## Creating and using a simple Denavit-Hartenberg model\n", "First we define a robot, we will choose the classical Puma robot which includes both a kinematic and dynamic model" - ] + ], + "id": "d41c6dbc" }, { "cell_type": "code", @@ -121,14 +125,16 @@ "outputs": [], "source": [ "puma = rtb.models.DH.Puma560()" - ] + ], + "id": "823706e9" }, { "cell_type": "markdown", "metadata": {}, "source": [ "which has created an object in the local _workspace_ which subclasses the `DHRobot` class which subclasses the top-level `Robot` class. If we print this we get some detailed information about it's kinematics as well as the base and tool transforms, and some predefined joint configurations" - ] + ], + "id": "0b9b5814" }, { "cell_type": "code", @@ -137,14 +143,16 @@ "outputs": [], "source": [ "print(puma)" - ] + ], + "id": "e6532611" }, { "cell_type": "markdown", "metadata": {}, "source": [ "and we can plot the robot in its zero-joint-angle configuration" - ] + ], + "id": "c4a979a4" }, { "cell_type": "code", @@ -153,14 +161,16 @@ "outputs": [], "source": [ "puma.plot(puma.qn);" - ] + ], + "id": "cfe97914" }, { "cell_type": "markdown", "metadata": {}, "source": [ "For an arbitrary set of joint angles" - ] + ], + "id": "f1349186" }, { "cell_type": "code", @@ -169,14 +179,16 @@ "outputs": [], "source": [ "q = [0.1, 0.2, 0.3, 0.4, 0.5, 0.6]" - ] + ], + "id": "8fededc2" }, { "cell_type": "markdown", "metadata": {}, "source": [ "we can compute the end-effector pose using forward kinematics, taking into account the base and tool transforms" - ] + ], + "id": "85163e8f" }, { "cell_type": "code", @@ -186,7 +198,8 @@ "source": [ "T = puma.fkine(q)\n", "T" - ] + ], + "id": "3b8abe37" }, { "cell_type": "markdown", @@ -195,7 +208,8 @@ "the result is an SE(3) matrix, an instance of an `SE3` object. It is shown with the rotational part in red, the translational part in blue, and the constant part in grey.\n", "\n", "To find the inverse kinematics corresponding to this end-effector pose we solve the inverse kinematics numerically" - ] + ], + "id": "19493c65" }, { "cell_type": "code", @@ -205,7 +219,8 @@ "source": [ "q = puma.ikine_LM(T).q\n", "q" - ] + ], + "id": "2a7c48ad" }, { "cell_type": "markdown", @@ -214,7 +229,8 @@ "_Note: that `ikine_LM` returns an `IKSolution` object, with the joint coordinates in its `q` attribute and the status of the solution in other attributes, which we are ignoring here_\n", "\n", "The manipulability at this particular configuration is" - ] + ], + "id": "050ef302" }, { "cell_type": "code", @@ -223,14 +239,16 @@ "outputs": [], "source": [ "puma.manipulability(q)" - ] + ], + "id": "2eea6dde" }, { "cell_type": "markdown", "metadata": {}, "source": [ "and the Jacobian, in the world frame (indicated by the `0` suffix) is" - ] + ], + "id": "043a9acf" }, { "cell_type": "code", @@ -241,7 +259,8 @@ "outputs": [], "source": [ "puma.plot(puma.qn, vellipse=True);" - ] + ], + "id": "7d510083" }, { "cell_type": "code", @@ -250,14 +269,16 @@ "outputs": [], "source": [ "puma.jacob0(q)" - ] + ], + "id": "07222729" }, { "cell_type": "markdown", "metadata": {}, "source": [ "the Jacobian in the end-effector frame is given by `jacobe`." - ] + ], + "id": "8d952da9" }, { "cell_type": "markdown", @@ -265,7 +286,8 @@ "source": [ "## More models\n", "A list of all models currently defined in the Toolbox is given by" - ] + ], + "id": "087795dc" }, { "cell_type": "code", @@ -274,7 +296,8 @@ "outputs": [], "source": [ "rtb.models.list()" - ] + ], + "id": "975e3e96" }, { "cell_type": "markdown", @@ -285,7 +308,8 @@ "1. DH - expressed using standard or modified Denavit-Hartenberg parameters, and may include dynamic parameters, meshs and collision objects\n", "2. ETS - expressed as a string of elementary transforms, does not support dynamic parameters\n", "3. URDF - based on a URDF file with some additional meta-data, and may include dynamic parameters, meshs and collision objects" - ] + ], + "id": "30d0599f" }, { "cell_type": "markdown", @@ -293,7 +317,8 @@ "source": [ "### Elementary transform sequence (ETS) models\n", "We can express the Puma 560 simply and concisely as a string of elementary transforms. First we import the class and then define the key dimensions/offsets of the robot" - ] + ], + "id": "8cc47510" }, { "cell_type": "code", @@ -309,7 +334,8 @@ "l5 = 0.4318\n", "l4 = -0.0837\n", "l6 = 0.0203" - ] + ], + "id": "7a0b425a" }, { "cell_type": "markdown", @@ -319,7 +345,8 @@ "\n", "- constant, indicated by an argument to the method, or\n", "- a variable, due to a joint, and this is indicated by no argument to the method" - ] + ], + "id": "f3ef7f27" }, { "cell_type": "code", @@ -341,14 +368,16 @@ " * ET.Ry()\n", " * ET.Rz()\n", ")" - ] + ], + "id": "75f3a39a" }, { "cell_type": "markdown", "metadata": {}, "source": [ "The overloaded `*` indicates composition of the rigid-body motions. The total number of elementary transforms is" - ] + ], + "id": "fe827a2e" }, { "cell_type": "code", @@ -357,14 +386,16 @@ "outputs": [], "source": [ "len(e)" - ] + ], + "id": "adef41af" }, { "cell_type": "markdown", "metadata": {}, "source": [ "and the elements which are joints are" - ] + ], + "id": "ed94d528" }, { "cell_type": "code", @@ -373,14 +404,16 @@ "outputs": [], "source": [ "e.joints()" - ] + ], + "id": "0d4bc8d2" }, { "cell_type": "markdown", "metadata": {}, "source": [ "and we can pretty print the transform sequence by" - ] + ], + "id": "0472fc55" }, { "cell_type": "code", @@ -389,14 +422,16 @@ "outputs": [], "source": [ "print(e)" - ] + ], + "id": "ba3d35dc" }, { "cell_type": "markdown", "metadata": {}, "source": [ "We can also evaluate the transform sequence, by subsituting in values for the joint angles. They are taken from consecutive elemements of the joint-angle vector provide to the `eval` method" - ] + ], + "id": "49ef48bb" }, { "cell_type": "code", @@ -405,7 +440,8 @@ "outputs": [], "source": [ "e.eval([0, 0, 0, 0, 0, 0])" - ] + ], + "id": "587acb32" }, { "cell_type": "code", @@ -414,14 +450,16 @@ "outputs": [], "source": [ "puma.fkine(puma.qr)" - ] + ], + "id": "7c8d984e" }, { "cell_type": "markdown", "metadata": {}, "source": [ "and this is the forward kinematics of a robot described by this elementary transform sequence." - ] + ], + "id": "33adf87c" }, { "cell_type": "markdown", @@ -429,7 +467,8 @@ "source": [ "## Robot dynamics\n", "We can define a different joint configuration `q`, as well as the joint velocity `qd` and acceleration `qdd`" - ] + ], + "id": "5f4f21d4" }, { "cell_type": "code", @@ -440,14 +479,16 @@ "q = puma.qn\n", "qd = np.zeros((6,))\n", "qdd = np.zeros((6,))" - ] + ], + "id": "23d9a236" }, { "cell_type": "markdown", "metadata": {}, "source": [ "and then compute the inverse dynamics using the recursive Newton-Euler algorithm" - ] + ], + "id": "0c7272cc" }, { "cell_type": "code", @@ -456,7 +497,8 @@ "outputs": [], "source": [ "puma.rne(q, qd, qdd)" - ] + ], + "id": "880a2a2d" }, { "cell_type": "markdown", @@ -465,7 +507,8 @@ "which is written in C for performance. It's actually the same 20 year C-code used in the MATLAB version of the Toolbox!\n", "\n", "We can use this to compute the manipulator inertia matrix" - ] + ], + "id": "c3108943" }, { "cell_type": "code", @@ -474,14 +517,16 @@ "outputs": [], "source": [ "puma.inertia(q)" - ] + ], + "id": "485886bc" }, { "cell_type": "markdown", "metadata": {}, "source": [ "the velocity terms" - ] + ], + "id": "780d7706" }, { "cell_type": "code", @@ -490,7 +535,8 @@ "outputs": [], "source": [ "puma.coriolis(q, [0.1, 0.2, 0.3, 0.1, 0.2, 0.3])" - ] + ], + "id": "31a6ab42" }, { "cell_type": "markdown", @@ -499,7 +545,8 @@ "where the second argument is the joint velocuty.\n", "\n", "Finally, the gravity torque is" - ] + ], + "id": "78ca8306" }, { "cell_type": "code", @@ -508,14 +555,16 @@ "outputs": [], "source": [ "puma.gravload(q)" - ] + ], + "id": "23e4794d" }, { "cell_type": "markdown", "metadata": {}, "source": [ "If we set the joint torques to zero, ie. turn the motors off" - ] + ], + "id": "d0a28297" }, { "cell_type": "code", @@ -524,14 +573,16 @@ "outputs": [], "source": [ "tau = np.zeros((6,))" - ] + ], + "id": "3475f70c" }, { "cell_type": "markdown", "metadata": {}, "source": [ "then the resulting joint angle acceleration will be" - ] + ], + "id": "e72fdc09" }, { "cell_type": "code", @@ -540,14 +591,16 @@ "outputs": [], "source": [ "puma.accel(q, qd, tau)" - ] + ], + "id": "2ee705dd" }, { "cell_type": "markdown", "metadata": {}, "source": [ "which we can integrate over time to obtain a joint angle trajectory" - ] + ], + "id": "fb0f1a31" }, { "cell_type": "code", @@ -557,14 +610,16 @@ "source": [ "tg = puma.nofriction().fdyn(5, puma.qn, dt=0.05)\n", "rtb.tools.plot.xplot(tg.t, tg.q)" - ] + ], + "id": "666f40b2" }, { "cell_type": "markdown", "metadata": {}, "source": [ "Note: _Coulomb friction is a harsh non-linearity and it causes the numerical integrator to take very small times steps, so the result will take many minutes to compute. To speed things up, at the expense of some modeling fidelity, we set the Coulomb friction to zero, but retain the viscous friction. The `nofriction()` method returns a clone of the robot with its friction parameters modified._" - ] + ], + "id": "964e3465" }, { "cell_type": "markdown", @@ -574,7 +629,8 @@ "\n", "## Starting simply\n", "The Toolbox provides functions to convert to and from vectors to skew-symmetric matrices (elements of so(2), so(3)) and augmented skew-symmetric matrices (elements of se(2), se(3))" - ] + ], + "id": "8bb71d63" }, { "cell_type": "code", @@ -584,7 +640,8 @@ "source": [ "S = skew([1, 2, 3])\n", "S" - ] + ], + "id": "1af83003" }, { "cell_type": "code", @@ -593,7 +650,8 @@ "outputs": [], "source": [ "vex(S)" - ] + ], + "id": "95d63981" }, { "cell_type": "code", @@ -603,7 +661,8 @@ "source": [ "S = skewa([1, 2, 3, 4, 5, 6])\n", "S" - ] + ], + "id": "dca10a9e" }, { "cell_type": "code", @@ -612,7 +671,8 @@ "outputs": [], "source": [ "vexa(S)" - ] + ], + "id": "76017aec" }, { "cell_type": "markdown", @@ -620,7 +680,8 @@ "source": [ "## 3D twists\n", "We can create a revolute twist by specifying the direction of its line of action and a point" - ] + ], + "id": "264adf0b" }, { "cell_type": "code", @@ -629,14 +690,16 @@ "outputs": [], "source": [ "Twist3.UnitRevolute([1, 0, 0], [0, 0, 0])" - ] + ], + "id": "420331bf" }, { "cell_type": "markdown", "metadata": {}, "source": [ " which is a 6-vector encapsulated in an instance of a `Twist3` object. A prismatic twist has just the direction of its line of action" - ] + ], + "id": "02b7149e" }, { "cell_type": "code", @@ -645,14 +708,16 @@ "outputs": [], "source": [ "Twist3.UnitPrismatic([0, 1, 0])" - ] + ], + "id": "455a5bc6" }, { "cell_type": "markdown", "metadata": {}, "source": [ "We can convert an SE(3) matrix, encapsulated in an instance of an `SE3` object to a 3D twist" - ] + ], + "id": "d6302fb0" }, { "cell_type": "code", @@ -663,14 +728,16 @@ "T = SE3.Rand()\n", "tw = Twist3(T)\n", "tw" - ] + ], + "id": "6cca3eb4" }, { "cell_type": "markdown", "metadata": {}, "source": [ "Twists have a number of properties and methods. The moment is a 3-vector" - ] + ], + "id": "d28e730e" }, { "cell_type": "code", @@ -679,14 +746,16 @@ "outputs": [], "source": [ "tw.v" - ] + ], + "id": "0d8bf900" }, { "cell_type": "markdown", "metadata": {}, "source": [ "as is the direction part" - ] + ], + "id": "e21d9b81" }, { "cell_type": "code", @@ -695,14 +764,16 @@ "outputs": [], "source": [ "tw.w" - ] + ], + "id": "e333a7b5" }, { "cell_type": "markdown", "metadata": {}, "source": [ "A twist also has a pitch" - ] + ], + "id": "e6a42e38" }, { "cell_type": "code", @@ -711,14 +782,16 @@ "outputs": [], "source": [ "tw.pitch" - ] + ], + "id": "47de6e4b" }, { "cell_type": "markdown", "metadata": {}, "source": [ "and a pole" - ] + ], + "id": "3bfc9a07" }, { "cell_type": "code", @@ -727,14 +800,16 @@ "outputs": [], "source": [ "tw.pole" - ] + ], + "id": "974c6771" }, { "cell_type": "markdown", "metadata": {}, "source": [ "The line of action of the twist is a line in 3D space which can be represented by a Toolbox Plucker object" - ] + ], + "id": "08449d6d" }, { "cell_type": "code", @@ -743,14 +818,16 @@ "outputs": [], "source": [ "tw.line()" - ] + ], + "id": "39f22934" }, { "cell_type": "markdown", "metadata": {}, "source": [ "which we could plot, by first defining a plot volume" - ] + ], + "id": "28b6c645" }, { "cell_type": "code", @@ -764,7 +841,8 @@ "ax.set_zlim3d(-10, 10)\n", "\n", "tw.line().plot(color=\"red\", linewidth=2)" - ] + ], + "id": "f3f444b3" }, { "cell_type": "markdown", @@ -772,7 +850,8 @@ "source": [ "## Exponentiation of twists\n", "Importantly, we can exponeniate the twist, ie. compute $T = e^{[S]\\theta}$, where $\\theta$ is the argument" - ] + ], + "id": "e322fbb0" }, { "cell_type": "code", @@ -781,14 +860,16 @@ "outputs": [], "source": [ "tw.exp(0)" - ] + ], + "id": "5b728c95" }, { "cell_type": "markdown", "metadata": {}, "source": [ "which is of course the identity matrix, and" - ] + ], + "id": "0e73d2eb" }, { "cell_type": "code", @@ -797,14 +878,16 @@ "outputs": [], "source": [ "tw.exp(1)" - ] + ], + "id": "1e19870e" }, { "cell_type": "markdown", "metadata": {}, "source": [ "All Toolbox objects can contain multiple values, so in this example, we exponentiate the twist with a sequence of values, resulting in a single `SE3` object that contains a sequence of SE(3) values." - ] + ], + "id": "cf13c21e" }, { "cell_type": "code", @@ -814,14 +897,16 @@ "source": [ "T = tw.exp([0, 1])\n", "T" - ] + ], + "id": "3b039f93" }, { "cell_type": "markdown", "metadata": {}, "source": [ "and we access them in a familiar Pythonic way" - ] + ], + "id": "e24981ab" }, { "cell_type": "code", @@ -830,7 +915,8 @@ "outputs": [], "source": [ "len(T)" - ] + ], + "id": "2465a8a7" }, { "cell_type": "code", @@ -839,21 +925,24 @@ "outputs": [], "source": [ "T[1]" - ] + ], + "id": "ef1a9fd2" }, { "cell_type": "markdown", "metadata": {}, "source": [ "## Product of exponentials" - ] + ], + "id": "129bd6bd" }, { "cell_type": "markdown", "metadata": {}, "source": [ "For the robot model we created earlier, we can extract its product of exponentials representation, a set of six twists and a transformation" - ] + ], + "id": "4b762a6d" }, { "cell_type": "code", @@ -862,14 +951,16 @@ "outputs": [], "source": [ "S, T0 = puma.twists()" - ] + ], + "id": "f0806c59" }, { "cell_type": "markdown", "metadata": {}, "source": [ "where the transformation for zero-joint-angles is" - ] + ], + "id": "fa4b8a6d" }, { "cell_type": "code", @@ -878,14 +969,16 @@ "outputs": [], "source": [ "T0" - ] + ], + "id": "1fe17d94" }, { "cell_type": "markdown", "metadata": {}, "source": [ "and the twists are" - ] + ], + "id": "053f469e" }, { "cell_type": "code", @@ -894,14 +987,16 @@ "outputs": [], "source": [ "S" - ] + ], + "id": "4b17d142" }, { "cell_type": "markdown", "metadata": {}, "source": [ "For a nominal joint-angle configuration" - ] + ], + "id": "46012989" }, { "cell_type": "code", @@ -910,14 +1005,16 @@ "outputs": [], "source": [ "puma.qn" - ] + ], + "id": "f2ff1f05" }, { "cell_type": "markdown", "metadata": {}, "source": [ "we can exponentiate the twists" - ] + ], + "id": "00f9a9c8" }, { "cell_type": "code", @@ -926,7 +1023,8 @@ "outputs": [], "source": [ "S.exp(puma.qn)" - ] + ], + "id": "aa15acca" }, { "cell_type": "markdown", @@ -935,7 +1033,8 @@ "where the result is again an `SE3` object with multiple values, six in this case.\n", "\n", "The full expression for the forward kinematics is" - ] + ], + "id": "3e37280e" }, { "cell_type": "code", @@ -944,14 +1043,16 @@ "outputs": [], "source": [ "S.exp(puma.qn).prod() * T0" - ] + ], + "id": "b738bfac" }, { "cell_type": "markdown", "metadata": {}, "source": [ "which of course is identical to the classical approach " - ] + ], + "id": "67a0d5f2" }, { "cell_type": "code", @@ -960,14 +1061,16 @@ "outputs": [], "source": [ "puma.fkine(puma.qn)" - ] + ], + "id": "07a8cca5" }, { "cell_type": "markdown", "metadata": {}, "source": [ "Extending an earlier example, the lines of actions of the twists are" - ] + ], + "id": "de069bd8" }, { "cell_type": "code", @@ -977,14 +1080,16 @@ "source": [ "lines = S.line()\n", "lines" - ] + ], + "id": "05725261" }, { "cell_type": "markdown", "metadata": {}, "source": [ "Which we can overlay on a plot of the robot in its zero-angle configuration" - ] + ], + "id": "39324a5f" }, { "cell_type": "code", @@ -995,7 +1100,8 @@ "outputs": [], "source": [ "fig = puma.plot(puma.qz)" - ] + ], + "id": "25fd9ebd" }, { "cell_type": "code", @@ -1005,7 +1111,8 @@ "source": [ "fig = puma.plot(puma.qz)\n", "lines.plot(\"k--\", linewidth=1.5, bounds=np.r_[-1, 1, -1, 1, -1, 1.5])" - ] + ], + "id": "894bd19e" } ], "metadata": { @@ -1062,14 +1169,8 @@ "_Feature" ], "window_display": false - }, - "rtb_header": { - "title": "IROS 2020 workshop: review on screw theory and geometric robot dynamics", - "paragraphs": [], - "copyright": "Peter Corke 2020, updated 2026", - "modernised": "2026-10-19" } }, "nbformat": 4, - "nbformat_minor": 4 + "nbformat_minor": 5 } diff --git a/docs/notebooks/kinematics.ipynb b/docs/notebooks/kinematics.ipynb index 955325fa3..476d3f275 100644 --- a/docs/notebooks/kinematics.ipynb +++ b/docs/notebooks/kinematics.ipynb @@ -4,11 +4,10 @@ "cell_type": "markdown", "metadata": {}, "source": [ - "\n", "\n", " \n", " \n", - " \n", + " \n", " \n", "
Robotics Toolbox for Python
\n", "\n", @@ -19,7 +18,8 @@ "(c) Peter Corke 2026\n", "\n", "Forward and inverse kinematics, a joint-space trajectory and the Jacobian of a Puma 560 robot." - ] + ], + "id": "4eb2352d" }, { "cell_type": "code", @@ -73,7 +73,8 @@ " return colab\n", "\n", "COLAB = await ensure_installed()\n" - ] + ], + "id": "d7d8472b" }, { "cell_type": "code", @@ -95,14 +96,16 @@ "\n", "%matplotlib ipympl\n", "plt.close(\"all\")" - ] + ], + "id": "3f97f1fa" }, { "cell_type": "markdown", "metadata": {}, "source": [ "The Toolbox supports models defined using a number of different conventions. We will load a very classical model, a Puma560 robot defined in terms of standard Denavit-Hartenberg parameters" - ] + ], + "id": "ef971274" }, { "cell_type": "code", @@ -111,14 +114,16 @@ "outputs": [], "source": [ "p560 = rtb.models.DH.Puma560()" - ] + ], + "id": "acacd545" }, { "cell_type": "markdown", "metadata": {}, "source": [ "Now we can display the simple Denavit-Hartenberg parameter model" - ] + ], + "id": "329070d5" }, { "cell_type": "code", @@ -127,7 +132,8 @@ "outputs": [], "source": [ "print(p560)" - ] + ], + "id": "47e6f7f0" }, { "cell_type": "markdown", @@ -136,7 +142,8 @@ "The first table shows the kinematic parameters, and from the column titles we can see clearly that this is expressed in terms of standard Denavit-Hartenberg parameters. The first column shows that the joint variables qi are rotations since they are in the θ column. Joint limits are also shown. Joint flip (motion in the opposite sense) would be indicated by the joint variable being shown as for example like `-q3`, and joint offsets by being shown as for example like `q2 + 45°`.\n", "\n", "The second table shows some named joint configurations. For example `p560.qr` is " - ] + ], + "id": "bdf3fbed" }, { "cell_type": "code", @@ -145,7 +152,8 @@ "outputs": [], "source": [ "p560.qr" - ] + ], + "id": "bfd5ed4b" }, { "cell_type": "markdown", @@ -155,14 +163,16 @@ "\n", "This object is a subclass of `DHRobot`, equivalent to the `SerialLink` class in the MATLAB version of the Toolbox.\n", "This class has many methods and attributes, and we will explore some of them in this notebook." - ] + ], + "id": "c021e3fa" }, { "cell_type": "markdown", "metadata": {}, "source": [ "We can easily display the robot graphically" - ] + ], + "id": "953eb52b" }, { "cell_type": "code", @@ -173,7 +183,8 @@ "outputs": [], "source": [ "p560.plot(p560.qn);" - ] + ], + "id": "5bb698bb" }, { "cell_type": "markdown", @@ -182,7 +193,8 @@ "where `qn` is one of the named configurations shown above, and has the robot positioned to work above a table top. You can use the mouse to rotate the plot and view the robot from different directions. The grey line is the _shadow_ which is a projection of the robot onto the xy-plane.\n", "\n", "In this particular case the end-effector pose is given by the forward kinematics" - ] + ], + "id": "dd3a1595" }, { "cell_type": "code", @@ -191,7 +203,8 @@ "outputs": [], "source": [ "p560.fkine(p560.qn)" - ] + ], + "id": "fccd07f1" }, { "cell_type": "markdown", @@ -202,7 +215,8 @@ "You can verify the end-effector position, the blue numbers are from top to bottom the x-, y- and z-coordinates of the end-effector position, match the plot shown above.\n", "\n", "We can manually adjust the joint angles of this robot (click and drag the sliders) to see how the shape of the robot changes and how the end-effector pose changes" - ] + ], + "id": "64933906" }, { "cell_type": "code", @@ -213,7 +227,8 @@ "outputs": [], "source": [ "# p560.teach(); # works from console, hangs in Jupyter" - ] + ], + "id": "874cb125" }, { "cell_type": "markdown", @@ -227,7 +242,8 @@ "\n", "1. a pure translation\n", "2. a pure rotation defined in terms of the orientation and approach vectors" - ] + ], + "id": "2e177824" }, { "cell_type": "code", @@ -237,14 +253,16 @@ "source": [ "T = SE3(0.5, 0.2, 0.5) * SE3.OA([0, 0, 1], [1, 0, 0])\n", "T" - ] + ], + "id": "2f2dd38c" }, { "cell_type": "markdown", "metadata": {}, "source": [ "Now we can compute the joint angles that results in this pose" - ] + ], + "id": "9ad7d122" }, { "cell_type": "code", @@ -253,14 +271,16 @@ "outputs": [], "source": [ "sol = p560.ikine_LM(T)" - ] + ], + "id": "44561647" }, { "cell_type": "markdown", "metadata": {}, "source": [ "which returns the joint coordinates as well as solution status" - ] + ], + "id": "af5040f8" }, { "cell_type": "code", @@ -269,14 +289,16 @@ "outputs": [], "source": [ "sol" - ] + ], + "id": "3439b7da" }, { "cell_type": "markdown", "metadata": {}, "source": [ "indicating, in this case, that there is no failure. The joint coordinates are" - ] + ], + "id": "78f05e6a" }, { "cell_type": "code", @@ -285,14 +307,16 @@ "outputs": [], "source": [ "sol.q" - ] + ], + "id": "dcde0ff3" }, { "cell_type": "markdown", "metadata": {}, "source": [ "and we can confirm that this is indeed an inverse kinematic solution by computing the forward kinematics" - ] + ], + "id": "e650dd68" }, { "cell_type": "code", @@ -301,21 +325,24 @@ "outputs": [], "source": [ "p560.fkine(sol.q)" - ] + ], + "id": "ef04ecf0" }, { "cell_type": "markdown", "metadata": {}, "source": [ "which matches the original transform." - ] + ], + "id": "851b1a98" }, { "cell_type": "markdown", "metadata": {}, "source": [ "A simple trajectory between two joint configuration is" - ] + ], + "id": "3374002f" }, { "cell_type": "code", @@ -324,7 +351,8 @@ "outputs": [], "source": [ "qt = rtb.tools.trajectory.jtraj(p560.qz, sol.q, 50)" - ] + ], + "id": "b017a430" }, { "cell_type": "markdown", @@ -333,7 +361,8 @@ "The result is a _namedtuple_ with attributes `q` containing the joint angles, as well as `qd`, `qdd` and `t` which hold the joint velocity, joint accelerations and time respectively. \n", "\n", "The joint angles are a matrix with one column per joint and one row per timestep, and time increasing with row number." - ] + ], + "id": "d503f249" }, { "cell_type": "code", @@ -344,14 +373,16 @@ "outputs": [], "source": [ "qt.q" - ] + ], + "id": "8ace240a" }, { "cell_type": "markdown", "metadata": {}, "source": [ "We can plot this trajectory as a function of time using the convenience method `qplot`" - ] + ], + "id": "72b2d59e" }, { "cell_type": "code", @@ -360,14 +391,16 @@ "outputs": [], "source": [ "qt.qplot()" - ] + ], + "id": "634d1126" }, { "cell_type": "markdown", "metadata": {}, "source": [ "and then we can animate this" - ] + ], + "id": "3b54aa99" }, { "cell_type": "code", @@ -376,14 +409,16 @@ "outputs": [], "source": [ "p560.plot(qt.q, dt=0.1);" - ] + ], + "id": "e18004d7" }, { "cell_type": "markdown", "metadata": {}, "source": [ "_Note: animation not working in Jupyter..._" - ] + ], + "id": "adc7bfe2" }, { "cell_type": "markdown", @@ -399,7 +434,8 @@ " 180° about the gripper axis leaves the fingers in the same configuration.\n", "\n", "Most industrial robots have a _spherical wrist_ which means that the last three joint axes intersect at a single point in the middle of the wrist mechanism. We can test for this condition" - ] + ], + "id": "a05c484c" }, { "cell_type": "code", @@ -408,7 +444,8 @@ "outputs": [], "source": [ "p560.isspherical()" - ] + ], + "id": "d7b4b125" }, { "cell_type": "markdown", @@ -417,7 +454,8 @@ "This greatly simplifies things because the last three joints only control orientation and have no effect on the end-effector position. This means that only the first three joints define position $(x_e, y_e, z_e)$. Three joints that control three unknowns is relatively easy to solve for, and analytical solutions (complex trigonmetric equations) can be found, and in fact have been published for most industrial robot manipulators.\n", "\n", "The Puma560 has an analytical solution. We can request the solution with the arm to the left and the elbow up, and the wrist not flipped by using the configuration string `\"lun\"`\n" - ] + ], + "id": "cd7b6def" }, { "cell_type": "code", @@ -427,14 +465,16 @@ "source": [ "sol = p560.ikine_a(T, \"lun\")\n", "sol" - ] + ], + "id": "ee3adf9e" }, { "cell_type": "markdown", "metadata": {}, "source": [ "which is different to the values found earlier, but we can verify it is a valid solution" - ] + ], + "id": "dcbb23c1" }, { "cell_type": "code", @@ -443,14 +483,16 @@ "outputs": [], "source": [ "p560.fkine(sol.q)" - ] + ], + "id": "7f989d9c" }, { "cell_type": "markdown", "metadata": {}, "source": [ "In fact the solution we found earlier, but didn't explicitly specify, is the right-handed elbow-up configuration" - ] + ], + "id": "064fb467" }, { "cell_type": "code", @@ -460,14 +502,16 @@ "source": [ "sol = p560.ikine_a(T, \"run\")\n", "sol.q" - ] + ], + "id": "dc237281" }, { "cell_type": "markdown", "metadata": {}, "source": [ "Other useful functions include the manipulator Jacobian which maps joint velocity to end-effector velocity expressed in the world frame" - ] + ], + "id": "4d65d29d" }, { "cell_type": "code", @@ -476,14 +520,16 @@ "outputs": [], "source": [ "p560.jacob0(p560.qn)" - ] + ], + "id": "0952d140" }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], - "source": [] + "source": [], + "id": "1d5bea0e" } ], "metadata": { @@ -545,14 +591,8 @@ "_Feature" ], "window_display": false - }, - "rtb_header": { - "title": "Robot kinematics", - "paragraphs": [ - "Forward and inverse kinematics, a joint-space trajectory and the Jacobian of a Puma 560 robot." - ] } }, "nbformat": 4, - "nbformat_minor": 4 + "nbformat_minor": 5 } diff --git a/docs/notebooks/symbolic.ipynb b/docs/notebooks/symbolic.ipynb index dbad257d6..c008ca4d3 100644 --- a/docs/notebooks/symbolic.ipynb +++ b/docs/notebooks/symbolic.ipynb @@ -4,11 +4,10 @@ "cell_type": "markdown", "metadata": {}, "source": [ - "\n", "\n", " \n", " \n", - " \n", + " \n", " \n", "
Robotics Toolbox for Python
\n", "\n", @@ -21,7 +20,8 @@ "Forward kinematics and inverse dynamics of a Puma 560 robot with symbolic joint variables, and generating code from the results. The dynamics section takes a few minutes to run in full.\n", "\n", "> **Modernised 2026-10-19** to address API drift: this notebook was written for an earlier release of the Toolbox, and has been updated so that it runs with the current one." - ] + ], + "id": "0e39b490" }, { "cell_type": "code", @@ -75,7 +75,8 @@ " return colab\n", "\n", "COLAB = await ensure_installed(extras=['ik'])\n" - ] + ], + "id": "8fb6aeb6" }, { "cell_type": "code", @@ -91,7 +92,8 @@ "import spatialmath.base.symbolic as sym\n", "\n", "%matplotlib ipympl" - ] + ], + "id": "3c085bb6" }, { "cell_type": "markdown", @@ -101,7 +103,8 @@ "\n", "## Creating a symbolic variable\n", "We will first create a symbolic variable" - ] + ], + "id": "70cda885" }, { "cell_type": "code", @@ -110,7 +113,8 @@ "outputs": [], "source": [ "theta = sym.symbol('theta')" - ] + ], + "id": "1a5df959" }, { "cell_type": "code", @@ -119,14 +123,16 @@ "outputs": [], "source": [ "theta" - ] + ], + "id": "51ddfd57" }, { "cell_type": "markdown", "metadata": {}, "source": [ "which displays as a Greek letter, it has the type" - ] + ], + "id": "1c08445e" }, { "cell_type": "code", @@ -135,14 +141,16 @@ "outputs": [], "source": [ "type(theta)" - ] + ], + "id": "130a18ae" }, { "cell_type": "markdown", "metadata": {}, "source": [ "The function specifies that the symbolic variables are real valued by default, which will simplify subsequent simplification steps" - ] + ], + "id": "f46e3050" }, { "cell_type": "code", @@ -151,14 +159,16 @@ "outputs": [], "source": [ "theta.is_real" - ] + ], + "id": "c1f363bf" }, { "cell_type": "markdown", "metadata": {}, "source": [ "We can test if a variable is symbolic" - ] + ], + "id": "7d03e967" }, { "cell_type": "code", @@ -167,7 +177,8 @@ "outputs": [], "source": [ "sym.issymbol(theta)" - ] + ], + "id": "c19eb5ac" }, { "cell_type": "code", @@ -176,7 +187,8 @@ "outputs": [], "source": [ "sym.issymbol(3.7)" - ] + ], + "id": "b4aa94d7" }, { "cell_type": "markdown", @@ -184,7 +196,8 @@ "source": [ "## Symbolics with the Spatial Math Toolbox\n", "Many Spatial Toolbox functions handle symbolic variables" - ] + ], + "id": "5016dfa9" }, { "cell_type": "code", @@ -194,7 +207,8 @@ "source": [ "R = rot2(theta)\n", "R" - ] + ], + "id": "e612eba7" }, { "cell_type": "markdown", @@ -203,7 +217,8 @@ "and return a NumPy array with symbolic values. \n", "\n", "The 3D case is similar" - ] + ], + "id": "f34c6ea9" }, { "cell_type": "code", @@ -213,7 +228,8 @@ "source": [ "R = rotx(theta)\n", "R" - ] + ], + "id": "f9e71c5f" }, { "cell_type": "code", @@ -223,14 +239,16 @@ "source": [ "T = trotx(theta)\n", "T" - ] + ], + "id": "be3f7dff" }, { "cell_type": "markdown", "metadata": {}, "source": [ "The elements of this NumPy array are all objects, as indicated by the `dtype`. However when we index the elements, they will be converted back to numeric types if possible" - ] + ], + "id": "3e169bc8" }, { "cell_type": "code", @@ -239,7 +257,8 @@ "outputs": [], "source": [ "type(T[0,0])" - ] + ], + "id": "a7caa2fd" }, { "cell_type": "code", @@ -248,14 +267,16 @@ "outputs": [], "source": [ "type(T[1,1])" - ] + ], + "id": "a749c06d" }, { "cell_type": "markdown", "metadata": {}, "source": [ "We can perform arithmetic on such matrices, for example" - ] + ], + "id": "3df2affa" }, { "cell_type": "code", @@ -264,7 +285,8 @@ "outputs": [], "source": [ "T @ T" - ] + ], + "id": "b0ec7923" }, { "cell_type": "markdown", @@ -272,7 +294,8 @@ "source": [ "## Pose classes\n", "The symbolic capability extends to the pose classes" - ] + ], + "id": "d2567a90" }, { "cell_type": "code", @@ -282,7 +305,8 @@ "source": [ "T = SE3.Rx(theta)\n", "T" - ] + ], + "id": "2707cd5c" }, { "cell_type": "code", @@ -292,28 +316,32 @@ "source": [ "T2 = T * T\n", "T2" - ] + ], + "id": "a5ce3099" }, { "cell_type": "markdown", "metadata": {}, "source": [ "but the colored layout is problematic." - ] + ], + "id": "4a06bc35" }, { "cell_type": "markdown", "metadata": {}, "source": [ "## Robot forward kinematics" - ] + ], + "id": "bb8a15e0" }, { "cell_type": "markdown", "metadata": {}, "source": [ "We will create a symbolic version of the robot model" - ] + ], + "id": "e18a7cd2" }, { "cell_type": "code", @@ -322,7 +350,8 @@ "outputs": [], "source": [ "puma = rtb.models.DH.Puma560(symbolic=True)" - ] + ], + "id": "9408a564" }, { "cell_type": "code", @@ -331,21 +360,24 @@ "outputs": [], "source": [ "print(puma)" - ] + ], + "id": "c9759f40" }, { "cell_type": "markdown", "metadata": {}, "source": [ "We see that the $\\alpha_j$ values are now given in radians and are colored red. This means the value is a symbolic expression, for the first link it is $\\pi/2$ which is a precise value, compared to the non-symbolic case which is a floating point number 1.5707963267948966 that only approximates $\\pi/2$." - ] + ], + "id": "ffa2df67" }, { "cell_type": "markdown", "metadata": {}, "source": [ "The next thing we need to do is create a vector of joint coordinates which are all symbols" - ] + ], + "id": "7cb9952d" }, { "cell_type": "code", @@ -355,14 +387,16 @@ "source": [ "q = sym.symbol('q_:6')\n", "q" - ] + ], + "id": "61352f75" }, { "cell_type": "markdown", "metadata": {}, "source": [ "We use the underscore, because the value of the symbol is pretty printed by SymPy as a subscript (just as it is with LaTeX math notation)" - ] + ], + "id": "e2349534" }, { "cell_type": "code", @@ -371,14 +405,16 @@ "outputs": [], "source": [ "q[0]" - ] + ], + "id": "86f820c2" }, { "cell_type": "markdown", "metadata": {}, "source": [ "We are now set to compute the forward kinematics which will be a matrix whose elements will be complicated expressions of symbolic joint variables and kinematic parameters" - ] + ], + "id": "55f7574e" }, { "cell_type": "code", @@ -388,14 +424,16 @@ "source": [ "T = puma.fkine(q)\n", "T" - ] + ], + "id": "d33f041b" }, { "cell_type": "markdown", "metadata": {}, "source": [ "The color coding helps us identify the rotational and translational parts, but the format is not very readable. We can display the underlying NumPy array" - ] + ], + "id": "2222d8f6" }, { "cell_type": "code", @@ -404,7 +442,8 @@ "outputs": [], "source": [ "T.A" - ] + ], + "id": "19ff6ef3" }, { "cell_type": "markdown", @@ -413,7 +452,8 @@ "which is not a lot better.\n", "\n", "As this stage it is far better to convert the result to a SymPy matrix" - ] + ], + "id": "88827405" }, { "cell_type": "code", @@ -423,7 +463,8 @@ "source": [ "from sympy import Matrix\n", "Matrix(T.A)" - ] + ], + "id": "5a8f1894" }, { "cell_type": "markdown", @@ -432,7 +473,8 @@ "which is decently pretty printed by SymPy.\n", "\n", "Often after a round of symbolic calculations there are simplifications that can be achieved. We can symbolically simplify each element of the `SE3` object by" - ] + ], + "id": "ab43bcd3" }, { "cell_type": "code", @@ -444,7 +486,8 @@ "\n", "M = Matrix(Ts.A)\n", "M" - ] + ], + "id": "ca1bad2d" }, { "cell_type": "markdown", @@ -453,7 +496,8 @@ "which is more compact (it takes a few seconds to compute). We can see that a trigometric _sum of angles_ substition has been performed, there are instances of sine and cosine of $q_1 + q_2$, the shoulder and elbow joints. This is to be expected since these joints are adjacent and have with parallel axes.\n", "\n", "We can _slice_ the end-effector translationfrom the SymPy matrix" - ] + ], + "id": "ca0ab88f" }, { "cell_type": "code", @@ -462,14 +506,16 @@ "outputs": [], "source": [ "M[:3,3]" - ] + ], + "id": "ab687736" }, { "cell_type": "markdown", "metadata": {}, "source": [ "The floating point constants here have been inherited from the kinematic model we imported at [this step](#Robot-forward-kinematics). It would be possible to replace the non-zero kinematic constants $a_i$ and $d_i$ with symbols created using `sym.symbol` as shown in [this section](#Creating-a-symbolic-variable)." - ] + ], + "id": "a659ed23" }, { "cell_type": "markdown", @@ -477,7 +523,8 @@ "source": [ "## Code generation\n", "We can now use some of SymPy's superpowers to turn our forward kinematic expression into code" - ] + ], + "id": "66ac5b98" }, { "cell_type": "code", @@ -487,7 +534,8 @@ "source": [ "from sympy import ccode, pycode, octave_code\n", "print(ccode(M, assign_to=\"T\"))" - ] + ], + "id": "3a0312f9" }, { "cell_type": "markdown", @@ -496,7 +544,8 @@ "which is pure C code that does not require any linear algebra package to compute. We simply need to define the values of `q_0` to `q_5` in order to determine the end-effector pose. The result is computed using symbolically simplified expressions. The code is not optimized, but we could expect the compiler to perform some additional simplification.\n", "\n", "The equivalent MATLAB code is" - ] + ], + "id": "8228338a" }, { "cell_type": "code", @@ -505,14 +554,16 @@ "outputs": [], "source": [ "octave_code(M)" - ] + ], + "id": "22aaf482" }, { "cell_type": "markdown", "metadata": {}, "source": [ "We can also output Python code" - ] + ], + "id": "19de3fd2" }, { "cell_type": "code", @@ -521,14 +572,16 @@ "outputs": [], "source": [ "print(pycode(M))" - ] + ], + "id": "9e03efae" }, { "cell_type": "markdown", "metadata": {}, "source": [ "Which constructs an instance of a SymPy `ImmutableDenseMatrix` which we can turn into a function" - ] + ], + "id": "d8a37469" }, { "cell_type": "code", @@ -538,14 +591,16 @@ "source": [ "from sympy import lambdify\n", "T_func = lambdify(q, M, modules='numpy')" - ] + ], + "id": "22e110f7" }, { "cell_type": "markdown", "metadata": {}, "source": [ "If we pass in the zero joint angles we get the familiar forward kinematic solution result" - ] + ], + "id": "545c42a8" }, { "cell_type": "code", @@ -554,7 +609,8 @@ "outputs": [], "source": [ "T_func(0, 0, 0, 0, 0, 0)" - ] + ], + "id": "da379113" }, { "cell_type": "markdown", @@ -563,7 +619,8 @@ "SymPy also supports printing code in C++ (`cxxcode`), Fortran (`fcode`), JavaScript (`jscode`), Julia (`julia_code`), Rust (`rust_code`), Mathematica (`mathematica_code`) and Octave/MATLAB (`octave_code`).\n", "\n", "The [SymPy autowrap](https://docs.sympy.org/latest/modules/codegen.html#autowrap) capability automatically generates code, writes it to disk, compiles it, and imports it into the current session. It creates a wrapper using Cython and creates a numerical function." - ] + ], + "id": "2fed14fe" }, { "cell_type": "markdown", @@ -574,7 +631,8 @@ "We can also compute the dynamics symbolically. To do this we must use the Python version of the inverse dynamics rather than the default efficient C-code implementation.\n", "\n", "We need to setup the problem. Firstly we make the gravitational constant a symbol" - ] + ], + "id": "84ec9a92" }, { "cell_type": "code", @@ -584,14 +642,16 @@ "source": [ "g = sym.symbol('g')\n", "puma.gravity = [0, 0, -g]" - ] + ], + "id": "c2c2765d" }, { "cell_type": "markdown", "metadata": {}, "source": [ "Next we create symbolic vectors for joint velocity and acceleration, just as we did earlier for joint coordinates" - ] + ], + "id": "e1fa5bf1" }, { "cell_type": "code", @@ -601,7 +661,8 @@ "source": [ "qd = sym.symbol('qd_:6')\n", "qd" - ] + ], + "id": "7a855919" }, { "cell_type": "code", @@ -611,7 +672,8 @@ "source": [ "qdd = sym.symbol('qdd_:6')\n", "qdd" - ] + ], + "id": "52774649" }, { "cell_type": "markdown", @@ -620,7 +682,8 @@ "Now we compute the inverse dynamics as a function of symbolic joint coordinates, velocities, accelerations and gravity as well as a lot of numerical kinematic and dynamic parameters.\n", "\n", "This takes a second or so. The result is impressive: each torque is an expression with thousands of terms. Note that `gravity` is the acceleration due to gravity, so for the usual orientation of the Puma it is `[0, 0, -g]`." - ] + ], + "id": "5c967a12" }, { "cell_type": "code", @@ -631,7 +694,8 @@ "outputs": [], "source": [ "%time tau = puma.rne_python(q, qd, qdd)" - ] + ], + "id": "b6db73a3" }, { "cell_type": "code", @@ -640,7 +704,8 @@ "outputs": [], "source": [ "from sympy import trigsimp, simplify" - ] + ], + "id": "051cec6a" }, { "cell_type": "code", @@ -649,21 +714,24 @@ "outputs": [], "source": [ "%time z = simplify(tau[5])" - ] + ], + "id": "effe62d5" }, { "cell_type": "markdown", "metadata": {}, "source": [ "SymPy's `simplify` can reduce these expressions a lot, but it takes longer the more terms there are. It took about a second for the last joint, 15 seconds for $\\tau_3$, 50 seconds for $\\tau_4$ and several minutes for $\\tau_2$, which we will not wait for." - ] + ], + "id": "3d6e6daa" }, { "cell_type": "markdown", "metadata": {}, "source": [ "The result `tau` is a NumPy array, as it is for the numeric case, but with `dtype=object` since each element is a SymPy expression" - ] + ], + "id": "eade206a" }, { "cell_type": "code", @@ -672,14 +740,16 @@ "outputs": [], "source": [ "type(tau)" - ] + ], + "id": "00437d19" }, { "cell_type": "markdown", "metadata": {}, "source": [ "and it has the expected shape" - ] + ], + "id": "43780f20" }, { "cell_type": "code", @@ -688,14 +758,16 @@ "outputs": [], "source": [ "tau.shape" - ] + ], + "id": "00d293ec" }, { "cell_type": "markdown", "metadata": {}, "source": [ "The torque on the first joint $\\tau_0$ is" - ] + ], + "id": "d4594129" }, { "cell_type": "code", @@ -704,14 +776,16 @@ "outputs": [], "source": [ "tau[0]" - ] + ], + "id": "db07968d" }, { "cell_type": "markdown", "metadata": {}, "source": [ "which is a complicated expression. To make it easier to work with, we will expand it" - ] + ], + "id": "53b9c432" }, { "cell_type": "code", @@ -721,14 +795,16 @@ "source": [ "tau_0 = tau[0].expand()\n", "tau_0" - ] + ], + "id": "c5248007" }, { "cell_type": "markdown", "metadata": {}, "source": [ "to form a multinomial, a sum of products of trigonometric functions. The number of product terms is" - ] + ], + "id": "6297390b" }, { "cell_type": "code", @@ -737,14 +813,16 @@ "outputs": [], "source": [ "len(tau_0.args)" - ] + ], + "id": "1a7fbeeb" }, { "cell_type": "markdown", "metadata": {}, "source": [ "With the expression in this form we can find all the terms that contain $\\ddot{q}_0$" - ] + ], + "id": "87bf7dec" }, { "cell_type": "code", @@ -754,7 +832,8 @@ "source": [ "m = tau_0.coeff(qdd[0]).args\n", "m" - ] + ], + "id": "ae97b209" }, { "cell_type": "markdown", @@ -763,7 +842,8 @@ "so we can write our torque expression as $\\tau_0 = m \\ddot{q}_0 + \\cdots$ which means that $m$ must be element $\\textbf{M}_{00}$ of the manipulator inertia matrix. We see it has contributions due to the centre of mass of the arm which depends on the configuration of joints 1 to 5, and motor inertia. \n", "\n", "Similarly " - ] + ], + "id": "9e27d63d" }, { "cell_type": "code", @@ -773,7 +853,8 @@ "source": [ "m = tau_0.coeff(qdd[1]).args\n", "m" - ] + ], + "id": "eaf3b9a3" }, { "cell_type": "markdown", @@ -782,7 +863,8 @@ "must be the element $\\textbf{M}_{01}$, an off-diagonal element of the inertia matrix. In this way we can generate expressions for each element of the manipulator inertia matrix. Remember that this matrix is symmetric so we only need to compute half of the off-diagnonal terms.\n", "\n", "In a similar way we can find elements of the Coriolis and centripetal matrix. Terms containing $\\dot{q}_0$ are" - ] + ], + "id": "d7594f8b" }, { "cell_type": "code", @@ -792,7 +874,8 @@ "source": [ "c = tau_0.coeff(qd[0]).args\n", "c" - ] + ], + "id": "e66008fa" }, { "cell_type": "markdown", @@ -800,7 +883,8 @@ "source": [ "so we can write our torque expression as $\\tau_0 = c \\dot{q}_0 + \\cdots$\n", "A subset of these terms still contain $\\dot{q}_0$" - ] + ], + "id": "a23fb8be" }, { "cell_type": "code", @@ -810,7 +894,8 @@ "source": [ "C = tau_0.coeff(qd[0], 2).args\n", "C" - ] + ], + "id": "6d0e77ea" }, { "cell_type": "markdown", @@ -819,7 +904,8 @@ "ie. they originally contained $\\dot{q}_0^2$. The squared velocity terms belong on the diagonal of the Coriolis matrix and represent centripetal torques. This particular expression is the element $\\textbf{C}_{00}$.\n", "\n", "The off-diagonal terms are coefficients of $\\dot{q}_i \\dot{q}_j$ and we can find $\\dot{q}_0 \\dot{q}_1$ by" - ] + ], + "id": "4e89d40c" }, { "cell_type": "code", @@ -829,7 +915,8 @@ "c" ], "execution_count": null, - "outputs": [] + "outputs": [], + "id": "fbba5177" }, { "cell_type": "markdown", @@ -838,14 +925,16 @@ "We add $C/2$ to both $C_{01}$ and $C_{10}$ and repeat for all the off-diagonal terms. $C_{ij} = \\Sum_{i=0}^n \\Sum_{j=0}^n c\n", "\n", "The elements of this matrix map velocities to force and therefore have the same dimensions as viscous friction. In fact the motor friction parameter $B_m$ will be mixed up in these expressions, so we should first set $B_m = 0$ for all joints before proceeding." - ] + ], + "id": "c024f72d" }, { "cell_type": "markdown", "metadata": {}, "source": [ "SymPy provides tools that allow us to pull apart the expression's parse tree. The top node is given by `.func` which in this case is" - ] + ], + "id": "dd52ac06" }, { "cell_type": "code", @@ -854,14 +943,16 @@ "outputs": [], "source": [ "tau_0.func" - ] + ], + "id": "66950865" }, { "cell_type": "markdown", "metadata": {}, "source": [ "indicating an addition node, and its arguments are" - ] + ], + "id": "0dd4dc63" }, { "cell_type": "code", @@ -872,14 +963,16 @@ "outputs": [], "source": [ "tau_0.args" - ] + ], + "id": "34645872" }, { "cell_type": "markdown", "metadata": {}, "source": [ "expressed as a tuple. A term, say the first one, can be further decomposed" - ] + ], + "id": "8772b5fd" }, { "cell_type": "code", @@ -888,7 +981,8 @@ "outputs": [], "source": [ "tau_0.args[0].func" - ] + ], + "id": "ea341725" }, { "cell_type": "code", @@ -897,21 +991,24 @@ "outputs": [], "source": [ "tau_0.args[0].args" - ] + ], + "id": "dd92407d" }, { "cell_type": "markdown", "metadata": {}, "source": [ "which is we see here is a product of three terms. There is a standard ordering of the arguments and it looks like constants are always first." - ] + ], + "id": "40e07b83" }, { "cell_type": "markdown", "metadata": {}, "source": [ "We can see that many of the terms have very small coefficients, they literally don't add much to the result, so we can cull them out, or rather, we can select those with significant coefficients" - ] + ], + "id": "31e6ee9d" }, { "cell_type": "code", @@ -921,14 +1018,16 @@ "source": [ "signif_terms = [t for t in tau_0.args if abs(t.args[0]) > 1e-2]\n", "signif_terms" - ] + ], + "id": "b3276582" }, { "cell_type": "markdown", "metadata": {}, "source": [ "and there are now only" - ] + ], + "id": "37cb8924" }, { "cell_type": "code", @@ -937,7 +1036,8 @@ "outputs": [], "source": [ "len(signif_terms)" - ] + ], + "id": "7ddacfaa" }, { "cell_type": "markdown", @@ -946,7 +1046,8 @@ "product terms remaining, just 25% of the number we started with.\n", "\n", "We can reconstruct the expression with just the significant terms" - ] + ], + "id": "a8286e0d" }, { "cell_type": "code", @@ -956,7 +1057,8 @@ "source": [ "tau_0s = tau_0.func(*signif_terms)\n", "tau_0s" - ] + ], + "id": "7180d48f" }, { "cell_type": "markdown", @@ -965,7 +1067,8 @@ "and the top-level node function, `tau_0.func` (which is `Add`) acts as a constructor to which we pass the significant terms.\n", "\n", "We can generate executable code in C to compute the torque on joint 0 which could be used in a feed-forward controller for example. " - ] + ], + "id": "e28872d0" }, { "cell_type": "code", @@ -974,7 +1077,8 @@ "outputs": [], "source": [ "print(ccode(tau_0s, assign_to=\"tau0\"))" - ] + ], + "id": "5e7f04f5" }, { "cell_type": "markdown", @@ -983,14 +1087,16 @@ "We can do this for any of the expressions we generated above, ie. for elements of the inertia matrix or the velocity matrix.\n", "\n", "The gravity vector is obtained when $\\dot{q}_j =0$ and $\\ddot{q}_j =0$" - ] + ], + "id": "6a829be4" }, { "cell_type": "markdown", "metadata": {}, "source": [ "To set these symbolic values to zero we define a substituion dictionary where a key is the symbols to be substituted and the value is its new (substituted) value." - ] + ], + "id": "6f6fb8b9" }, { "cell_type": "code", @@ -1002,7 +1108,8 @@ "for j in range(puma.n):\n", " subsdict[qd[j]] = 0\n", " subsdict[qdd[j]] = 0" - ] + ], + "id": "18037d42" }, { "cell_type": "code", @@ -1012,7 +1119,8 @@ "source": [ "tau_g = [t.subs(subsdict) for t in tau]\n", "tau_g" - ] + ], + "id": "84832fe9" }, { "cell_type": "code", @@ -1029,7 +1137,8 @@ " subsdict[sym.cos(q[j])] = C\n", " sqdict[S**2] = 1 - C**2\n", "subsdict" - ] + ], + "id": "02ee0c13" }, { "cell_type": "code", @@ -1038,7 +1147,8 @@ "outputs": [], "source": [ "sqdict" - ] + ], + "id": "0889139e" }, { "cell_type": "code", @@ -1047,7 +1157,8 @@ "outputs": [], "source": [ "t1 = tau[1].subs(subsdict)" - ] + ], + "id": "9c4271d7" }, { "cell_type": "code", @@ -1058,7 +1169,8 @@ "outputs": [], "source": [ "t1.expand().subs(sqdict)" - ] + ], + "id": "2742b4c0" } ], "metadata": { @@ -1121,13 +1233,6 @@ ], "window_display": false }, - "rtb_header": { - "title": "Symbolic computation", - "paragraphs": [ - "Forward kinematics and inverse dynamics of a Puma 560 robot with symbolic joint variables, and generating code from the results. The dynamics section takes a few minutes to run in full." - ], - "modernised": "2026-10-19" - }, "rtb_bootstrap": { "extras": [ "ik" @@ -1135,5 +1240,5 @@ } }, "nbformat": 4, - "nbformat_minor": 4 + "nbformat_minor": 5 } diff --git a/docs/notebooks/sync_bootstrap.py b/docs/notebooks/sync_bootstrap.py index b9e29db62..94b4f9636 100644 --- a/docs/notebooks/sync_bootstrap.py +++ b/docs/notebooks/sync_bootstrap.py @@ -18,6 +18,7 @@ import ast import json import sys +import uuid from pathlib import Path MARKER = "# RTB_BOOTSTRAP_CELL" @@ -71,46 +72,28 @@ def notebook_extras(nb: dict) -> list[str]: return list(nb.get("metadata", {}).get("rtb_bootstrap", {}).get("extras", [])) -HEADER_MARKER = "\n" - "\n" - " \n" - ' \n' - ' \n' - " \n" - "
Robotics Toolbox for Python
\n\n" - "

\n" - f'
{header["title"]}
\n' - "

\n\n" - f'(c) {header.get("copyright", "Peter Corke 2026")}' - ] - parts.extend(header.get("paragraphs", [])) - if "modernised" in header: - parts.append( - f"> **Modernised {header['modernised']}** to address API drift: this " - "notebook was written for an earlier release of the Toolbox, and has been " - "updated so that it runs with the current one." - ) - return "\n\n".join(parts).splitlines(keepends=True) - - -def is_header_cell(cell: dict) -> bool: - return ( - cell.get("cell_type") == "markdown" - and cell_source_text(cell).lstrip().startswith(HEADER_MARKER) - ) + cells = nb.get("cells", []) + used = {c["id"] for c in cells if "id" in c} + changed = False + for cell in cells: + if "id" not in cell: + new = uuid.uuid4().hex[:8] + while new in used: + new = uuid.uuid4().hex[:8] + cell["id"] = new + used.add(new) + changed = True + if changed and nb.get("nbformat_minor", 0) < 5: + nb["nbformat_minor"] = 5 + return changed def cell_source_text(cell: dict) -> str: @@ -127,30 +110,14 @@ def is_bootstrap_cell(cell: dict) -> bool: def process_notebook(path: Path, template: str, fix: bool) -> bool: - """Check (and optionally fix) one notebook's header and bootstrap cells. + """Check (and optionally fix) one notebook's bootstrap cell and cell ids. - :returns: True if the notebook's header or bootstrap cell was (or would be) changed. + :returns: True if the notebook's bootstrap cell or cell ids were (or would be) changed. """ with path.open("r", encoding="utf-8") as f: nb = json.load(f) - changed = False - header = nb.get("metadata", {}).get("rtb_header") - if header is not None: - desired = header_source(header) - cells = nb.setdefault("cells", []) - existing = next((c for c in cells if is_header_cell(c)), None) - if existing is None: - changed = True - if fix: - cells.insert( - 0, {"cell_type": "markdown", "metadata": {}, "source": desired} - ) - elif existing.get("source") != desired: - changed = True - if fix: - existing["source"] = desired - + changed = ensure_cell_ids(nb) if fix else any("id" not in c for c in nb.get("cells", [])) for cell in nb.get("cells", []): if not is_bootstrap_cell(cell): continue @@ -204,9 +171,9 @@ def main() -> int: any_changed = True rel = nb_path.relative_to(HERE.parent.parent) if fix: - print(f"{rel}: header or bootstrap cell out of date, regenerating") + print(f"{rel}: bootstrap cell or cell ids out of date, regenerating") else: - print(f"{rel}: header or bootstrap cell out of date") + print(f"{rel}: bootstrap cell or cell ids out of date") if args.check and any_changed: return 1 diff --git a/docs/notebooks/two-link-dynamics.ipynb b/docs/notebooks/two-link-dynamics.ipynb index 00a15e496..6d44d68f4 100644 --- a/docs/notebooks/two-link-dynamics.ipynb +++ b/docs/notebooks/two-link-dynamics.ipynb @@ -4,11 +4,10 @@ "cell_type": "markdown", "metadata": {}, "source": [ - "\n", "\n", " \n", " \n", - " \n", + " \n", " \n", "
Robotics Toolbox for Python
\n", "\n", @@ -21,7 +20,8 @@ "The gravity and velocity torques of the two-link planar arm in Spong, Hutchinson and Vidyasagar, computed by the Toolbox from a DH model and from an ETS model, and compared with the analytic Euler-Lagrange equations.\n", "\n", "> **Modernised 2026-10-19** to address API drift: this notebook was written for an earlier release of the Toolbox, and has been updated so that it runs with the current one." - ] + ], + "id": "59e40ae3" }, { "cell_type": "markdown", @@ -34,7 +34,8 @@ "Robot Modeling and Control, Spong, Vidyasagar, Hutchinson, Wiley 2006\n", "\n", "We will choose the parameters to be" - ] + ], + "id": "1181a25a" }, { "cell_type": "code", @@ -88,7 +89,8 @@ " return colab\n", "\n", "COLAB = await ensure_installed()\n" - ] + ], + "id": "44ef7c64" }, { "cell_type": "code", @@ -123,14 +125,16 @@ "\n", "# global parameters\n", "g = 9.81" - ] + ], + "id": "fff04355" }, { "cell_type": "markdown", "metadata": {}, "source": [ "## Denavit-Hartenberg notation" - ] + ], + "id": "97dca5bc" }, { "cell_type": "code", @@ -141,14 +145,16 @@ "L1 = RevoluteDH(a=l1, m=m1, r=[-lc1, 0, 0])\n", "L2 = RevoluteDH(a=l2, m=m2, r=[-lc2, 0, 0])\n", "robot = DHRobot([L1, L2], gravity=[0, -g, 0])" - ] + ], + "id": "5bc9a4b1" }, { "cell_type": "markdown", "metadata": {}, "source": [ "The DH notation only allows rotation about the z-axis, which is out of the page in the Figure above. The `gravity` argument is the acceleration due to gravity, a vector in the world frame. Gravity acts downward, in the $-y$ direction of the figure, so we set it to `[0, -g, 0]` (the default is `[0, 0, -9.81]`). The gravity load is the torque the joints must exert to hold the arm still against gravity, so it is positive for an arm stretched out horizontally." - ] + ], + "id": "90faa0b7" }, { "cell_type": "code", @@ -158,7 +164,8 @@ "source": [ "print(robot)\n", "print(robot.dynamics())" - ] + ], + "id": "54518096" }, { "cell_type": "code", @@ -169,14 +176,16 @@ "for q in qq:\n", " tau = robot.gravload(q)\n", " print(tau)" - ] + ], + "id": "9ac9cf16" }, { "cell_type": "markdown", "metadata": {}, "source": [ "Using equations (7.85) and (7.86) we can write an expression for gravity torque derived analytically using Euler-Lagrange (rather than Newton-Euler) approach" - ] + ], + "id": "23cb99c3" }, { "cell_type": "code", @@ -189,7 +198,8 @@ " (m1 * lc1 + m2 * l1) * g * cos(q[0]) + m2 * lc2 * g * cos(q[0] + q[1]),\n", " m2 * lc2 * g * cos(q[0] + q[1]),\n", " ]" - ] + ], + "id": "7a06438a" }, { "cell_type": "code", @@ -201,21 +211,24 @@ " tau_rne = robot.gravload(q)\n", " tau_el = EL_grav(q)\n", " print(tau_rne, \"vs\", tau_el)" - ] + ], + "id": "353ac8f8" }, { "cell_type": "markdown", "metadata": {}, "source": [ "In fact we could use symbolic values of the constants above, repeat the above process, and generate the symbolic equations for gravity load." - ] + ], + "id": "df4b8940" }, { "cell_type": "markdown", "metadata": {}, "source": [ "Now we can explore the velocity coupling terms. To isolate them we will set gravity and joint acceleration to zero" - ] + ], + "id": "fc0719d4" }, { "cell_type": "code", @@ -225,14 +238,16 @@ "source": [ "robot.gravity = [0, 0, 0]\n", "robot.rne(q2, [1, 1], z)" - ] + ], + "id": "934c0ed1" }, { "cell_type": "markdown", "metadata": {}, "source": [ "Using equations on page 261 and (7.87) we can write the velocity dependent terms as" - ] + ], + "id": "a4f17632" }, { "cell_type": "code", @@ -252,7 +267,8 @@ " c121 * qd[0] * qd[1] + c211 * qd[1] * qd[0] + c221 * qd[1] ** 2,\n", " c112 * qd[0] ** 2,\n", " ]" - ] + ], + "id": "a8398927" }, { "cell_type": "code", @@ -265,7 +281,8 @@ " tau_rne = robot.rne(q, qd, z)\n", " tau_el = EL_velocity(q, qd)\n", " print(tau_rne, \"vs\", tau_el)" - ] + ], + "id": "a59f1388" }, { "cell_type": "code", @@ -295,21 +312,24 @@ "q = symbol(\"q_:2\")\n", "qd = symbol(\"qd_:2\")\n", "robot.rne_python(q, qd, [0, 0])" - ] + ], + "id": "3a4abe52" }, { "cell_type": "markdown", "metadata": {}, "source": [ "## ETS notation" - ] + ], + "id": "c90a3625" }, { "cell_type": "markdown", "metadata": {}, "source": [ "We will redefine the parameters of the model to numerical values" - ] + ], + "id": "6a72b4cf" }, { "cell_type": "code", @@ -337,7 +357,8 @@ "\n", "# global parameters\n", "g = 9.81" - ] + ], + "id": "7ceead6a" }, { "cell_type": "code", @@ -348,7 +369,8 @@ "L1 = Link(ets=ET.Rz(), m=1, r=[0.5, 0, 0], name=\"L1\")\n", "L2 = Link(ets=ET.tx(1) * ET.Rz(), m=1, r=[0.5, 0, 0], parent=L1, name=\"L2\")\n", "robot = Robot([L1, L2], gravity=[0, -g, 0])" - ] + ], + "id": "04fbffa0" }, { "cell_type": "code", @@ -360,7 +382,8 @@ " tau_rne = robot.gravload(q)\n", " tau_el = EL_grav(q)\n", " print(tau_rne, \"vs\", tau_el)" - ] + ], + "id": "dbeb857e" } ], "metadata": { @@ -409,15 +432,8 @@ "_Feature" ], "window_display": false - }, - "rtb_header": { - "title": "Dynamics of a 2-link manipulator", - "paragraphs": [ - "The gravity and velocity torques of the two-link planar arm in Spong, Hutchinson and Vidyasagar, computed by the Toolbox from a DH model and from an ETS model, and compared with the analytic Euler-Lagrange equations." - ], - "modernised": "2026-10-19" } }, "nbformat": 4, - "nbformat_minor": 4 + "nbformat_minor": 5 } diff --git a/tests/test_notebook_bootstrap.py b/tests/test_notebook_bootstrap.py index 8bcab3dd5..8092a8ccd 100644 --- a/tests/test_notebook_bootstrap.py +++ b/tests/test_notebook_bootstrap.py @@ -37,8 +37,12 @@ def _write_notebook(path: Path, cells: list[dict]) -> None: path.write_text(json.dumps(nb, indent=1) + "\n", encoding="utf-8") +_ids = iter(f"{n:08x}" for n in range(10**6)) + + def _code_cell(source: str, outputs=None, execution_count=None) -> dict: return { + "id": next(_ids), "cell_type": "code", "metadata": {}, "execution_count": execution_count, @@ -225,44 +229,39 @@ def test_no_extras_gives_the_plain_call(): assert "".join(lines).rstrip().endswith("COLAB = await ensure_installed()") -def test_header_cell_is_generated_from_metadata(tmp_path): +def test_cells_without_ids_get_unique_ids_and_nbformat_5(tmp_path): nb_path = tmp_path / "demo.ipynb" - nb = { - "cells": [_code_cell("x = 1\n")], - "metadata": { - "rtb_header": { - "title": "A title", - "paragraphs": ["About it."], - "modernised": "2026-10-19", - } - }, - "nbformat": 4, - "nbformat_minor": 5, - } + cells = [_code_cell("x = 1\n"), _code_cell("y = 2\n"), _code_cell("z = 3\n")] + for cell in cells[:2]: + del cell["id"] + nb = {"cells": cells, "metadata": {}, "nbformat": 4, "nbformat_minor": 4} nb_path.write_text(json.dumps(nb), encoding="utf-8") + kept = cells[2]["id"] - # check mode reports the missing header and does not write + # check mode reports the missing ids and does not write assert sync_bootstrap.process_notebook(nb_path, template="", fix=False) - assert json.loads(nb_path.read_text(encoding="utf-8"))["cells"][0]["cell_type"] == "code" + assert "id" not in json.loads(nb_path.read_text(encoding="utf-8"))["cells"][0] assert sync_bootstrap.process_notebook(nb_path, template="", fix=True) - cells = json.loads(nb_path.read_text(encoding="utf-8"))["cells"] - assert sync_bootstrap.is_header_cell(cells[0]) - text = "".join(cells[0]["source"]) - assert "A title" in text and "About it." in text - assert "Modernised 2026-10-19" in text - assert len(cells) == 2 # inserted once, the original cell kept - - # idempotent, and a change to the metadata is noticed - assert not sync_bootstrap.process_notebook(nb_path, template="", fix=True) - nb = json.loads(nb_path.read_text(encoding="utf-8")) - nb["metadata"]["rtb_header"]["title"] = "Another" - nb_path.write_text(json.dumps(nb), encoding="utf-8") - assert sync_bootstrap.process_notebook(nb_path, template="", fix=True) - assert "Another" in "".join(json.loads(nb_path.read_text(encoding="utf-8"))["cells"][0]["source"]) + out = json.loads(nb_path.read_text(encoding="utf-8")) + ids = [c["id"] for c in out["cells"]] + assert len(set(ids)) == 3 and ids[2] == kept # an existing id is kept + assert out["nbformat_minor"] == 5 + + # nothing more to do, and the check passes + assert not sync_bootstrap.process_notebook(nb_path, template="", fix=False) -def test_notebook_without_header_metadata_gets_no_header(tmp_path): +def test_markdown_cells_get_ids_too(tmp_path): nb_path = tmp_path / "demo.ipynb" - _write_notebook(nb_path, [_code_cell("x = 1\n")]) - assert not sync_bootstrap.process_notebook(nb_path, template="", fix=True) + nb = { + "cells": [{"cell_type": "markdown", "metadata": {}, "source": ["# Title\n"]}], + "metadata": {}, + "nbformat": 4, + "nbformat_minor": 5, + } + nb_path.write_text(json.dumps(nb), encoding="utf-8") + + assert sync_bootstrap.process_notebook(nb_path, template="", fix=True) + + assert "id" in json.loads(nb_path.read_text(encoding="utf-8"))["cells"][0] From ff00cd808f403a5e6f1a71d462c1b84fda18b79a Mon Sep 17 00:00:00 2001 From: Peter Corke Date: Tue, 20 Oct 2026 06:17:11 +1000 Subject: [PATCH 2/2] docs(notebooks): clear_outputs.py also resets editor metadata and writes notebooks as nbformat does Co-Authored-By: Claude Sonnet 5.5 --- docs/notebooks/README.md | 7 +- docs/notebooks/branched-robot.ipynb | 164 +++----- docs/notebooks/clear_outputs.py | 43 ++- docs/notebooks/dynamics.ipynb | 192 ++++------ docs/notebooks/ets.ipynb | 404 +++++++++----------- docs/notebooks/graphics-test.ipynb | 31 +- docs/notebooks/icra2021.ipynb | 277 ++++++-------- docs/notebooks/icra2026.ipynb | 19 +- docs/notebooks/ik-symbolic.ipynb | 21 +- docs/notebooks/ik_benchmark.ipynb | 55 +-- docs/notebooks/index.ipynb | 4 +- docs/notebooks/iros2020.ipynb | 480 +++++++++++------------ docs/notebooks/kinematics.ipynb | 241 +++++------- docs/notebooks/symbolic.ipynb | 509 +++++++++++-------------- docs/notebooks/sync_bootstrap.py | 2 +- docs/notebooks/two-link-dynamics.ipynb | 136 +++---- tests/test_notebook_bootstrap.py | 42 ++ 17 files changed, 1144 insertions(+), 1483 deletions(-) diff --git a/docs/notebooks/README.md b/docs/notebooks/README.md index 7a8a8bea0..ce914c8cd 100644 --- a/docs/notebooks/README.md +++ b/docs/notebooks/README.md @@ -34,8 +34,11 @@ The second cell is the generated bootstrap cell, below. Edit it as you like, in VS Code or anywhere. Before you commit: -1. **Clear the outputs**, with *Clear All Outputs* in VS Code or - `python docs/notebooks/clear_outputs.py`. Notebooks are committed without outputs. +1. **Run `python docs/notebooks/clear_outputs.py`.** Notebooks are committed without + outputs (clearing them in VS Code is not enough on its own). The script also resets + the metadata an editor writes (the name and version of your Python environment, and + so on) to a canonical form, and writes the file as Jupyter does, so opening and + saving a notebook does not show up as a change. 2. That is all for an ordinary edit. `python docs/notebooks/sync_bootstrap.py` is only needed after you change `_rtb_nb_bootstrap.py` or a notebook's `rtb_bootstrap` metadata (it also adds cell ids, which VS Code would otherwise add whenever it diff --git a/docs/notebooks/branched-robot.ipynb b/docs/notebooks/branched-robot.ipynb index 8d582f3c5..f85b5f75d 100644 --- a/docs/notebooks/branched-robot.ipynb +++ b/docs/notebooks/branched-robot.ipynb @@ -2,6 +2,7 @@ "cells": [ { "cell_type": "markdown", + "id": "2aa1a0db", "metadata": {}, "source": [ "\n", @@ -20,12 +21,12 @@ "A robot with more than one end-effector: its forward kinematics and Jacobian for each branch, extracting the ETS of a branch, and drawing the robot as a graph. Drawing the graphs needs [Graphviz](https://graphviz.org) (the `dot` command) installed.\n", "\n", "> **Modernised 2026-10-19** to address API drift: this notebook was written for an earlier release of the Toolbox, and has been updated so that it runs with the current one." - ], - "id": "2aa1a0db" + ] }, { "cell_type": "code", "execution_count": null, + "id": "a13e6155", "metadata": { "jupyter": { "source_hidden": true @@ -75,80 +76,80 @@ " return colab\n", "\n", "COLAB = await ensure_installed()\n" - ], - "id": "a13e6155" + ] }, { "cell_type": "code", "execution_count": null, + "id": "9daa4a69", "metadata": {}, "outputs": [], "source": [ "import roboticstoolbox as rtb\n", "from IPython.display import SVG" - ], - "id": "9daa4a69" + ] }, { "cell_type": "markdown", + "id": "5a617374", "metadata": {}, "source": [ "We will load a model for a simple Y-shaped planar robot. Check out the source code to see how the model was constructed using `Link` and `Robot` classes." - ], - "id": "5a617374" + ] }, { "cell_type": "code", "execution_count": null, + "id": "d91fc922", "metadata": {}, "outputs": [], "source": [ "robot = rtb.models.ETS.Planar_Y()\n", "print(robot)" - ], - "id": "d91fc922" + ] }, { "cell_type": "markdown", + "id": "ee1f4eb9", "metadata": {}, "source": [ "From the displayed table we see that this robot has two end-effectors, indicated by the links shown in blue and prefixed with the @ symbol.\n", "\n", "We can easily create a graphical representation of the turn the robot description into a [GraphViz dotfile](https://graphviz.org)\n" - ], - "id": "ee1f4eb9" + ] }, { "cell_type": "code", "execution_count": null, + "id": "0f9b8925", "metadata": {}, "outputs": [], "source": [ "robot.dotfile(\"planar_y.dot\")" - ], - "id": "0f9b8925" + ] }, { "cell_type": "markdown", + "id": "9be2bf18", "metadata": {}, "source": [ "which writes a file in the current folder. We then render it to SVG format and [load and display it](https://ipython.readthedocs.io/en/stable/api/generated/IPython.display.html?highlight=svg#IPython.display.SVG)" - ], - "id": "9be2bf18" + ] }, { "cell_type": "code", "execution_count": null, + "id": "36593abb", "metadata": {}, "outputs": [], "source": [ "!dot -Tsvg planar_y.dot -x > planar_y.svg\n", "SVG(filename=\"planar_y.svg\")" - ], - "id": "36593abb" + ] }, { "cell_type": "markdown", + "id": "ee7b73be", "metadata": {}, "source": [ "The fixed base of the robot is shown as the dark grey node. Link reference frames are shown by circles and the edges are marked with the elementary transform sequences that define the relative poses of the link frames. End-effector nodes are marked by a blue double circle.\n", @@ -156,197 +157,197 @@ "The ETS transforms in black indicate the joints, and they contain the five joint angles `q0` to `q5`. The assignement of joint numbers is arbitrary and decided by the creator of the robot model. All operations such as forward kinematics or Jacobians that accept a joint angle vector, organize it as `[q0, q1, q2, q3, q4 q5]`.\n", "\n", "Fixed joints are shown in blue. " - ], - "id": "ee7b73be" + ] }, { "cell_type": "markdown", + "id": "11846548", "metadata": {}, "source": [ "The forward kinematics is quite straightforward, but we must specify which end-effector we are interested in" - ], - "id": "11846548" + ] }, { "cell_type": "code", "execution_count": null, + "id": "fdcdda90", "metadata": {}, "outputs": [], "source": [ "robot.fkine(robot.qz, end=\"eea\")" - ], - "id": "fdcdda90" + ] }, { "cell_type": "markdown", + "id": "2fec3923", "metadata": {}, "source": [ "which is exactly the same as the pose of the second branch" - ], - "id": "2fec3923" + ] }, { "cell_type": "code", "execution_count": null, + "id": "9506e063", "metadata": {}, "outputs": [], "source": [ "robot.fkine(robot.qz, end=\"eeb\")" - ], - "id": "9506e063" + ] }, { "cell_type": "markdown", + "id": "a46a6bcd", "metadata": {}, "source": [ "In a similar fashion we can also compute the Jacobian, in this case as a $6 \\times 4$ matrix" - ], - "id": "a46a6bcd" + ] }, { "cell_type": "code", "execution_count": null, + "id": "1ea285df", "metadata": {}, "outputs": [], "source": [ "robot.jacob0(robot.qz, end=\"eea\")" - ], - "id": "1ea285df" + ] }, { "cell_type": "markdown", + "id": "990a8207", "metadata": {}, "source": [ "We can generate an ETS for the bottom-branch of the robot" - ], - "id": "990a8207" + ] }, { "cell_type": "code", "execution_count": null, + "id": "fac103c3", "metadata": {}, "outputs": [], "source": [ "p = robot.ets(end=\"eeb\")\n", "p" - ], - "id": "fac103c3" + ] }, { "cell_type": "markdown", + "id": "ec1eafbd", "metadata": {}, "source": [ "and we could easily turn this into an unbranched robot represented by an `Robot` instance" - ], - "id": "ec1eafbd" + ] }, { "cell_type": "code", "execution_count": null, + "id": "47dd0ff7", "metadata": {}, "outputs": [], "source": [ "r = rtb.Robot(p, check_jindex=False)\n", "print(r)" - ], - "id": "47dd0ff7" + ] }, { "cell_type": "markdown", + "id": "216c43d3", "metadata": {}, "source": [ "This robot has 5 links but only 4 joint angles which are inherited from the original branched robot: `q0`, `q1`, `q4` and `q5`. Its `fkine` method accepts either a vector of the four joint angles, in order, or a vector of six elements indexed by the original joint numbers, in which case `q2` and `q3` are ignored.\n", "\n", "We can also find the transformation between any two links in the robot. In this case, from one end effector to the other, from link eea to link eeb" - ], - "id": "216c43d3" + ] }, { "cell_type": "code", "execution_count": null, + "id": "5c4851ce", "metadata": {}, "outputs": [], "source": [ "p = robot.ets(start=\"eea\", end=\"eeb\")\n", "p" - ], - "id": "5c4851ce" + ] }, { "cell_type": "markdown", + "id": "bfa5dd51", "metadata": {}, "source": [ "which is expressed as an ETS. Note that it is not in minimal form and the terms in the middle `tx(-1) * tx(1)` cancel out. We can precompile the constants which will take care of this for us" - ], - "id": "bfa5dd51" + ] }, { "cell_type": "code", "execution_count": null, + "id": "8081cc08", "metadata": {}, "outputs": [], "source": [ "p.compile()" - ], - "id": "8081cc08" + ] }, { "cell_type": "markdown", + "id": "fce34f81", "metadata": {}, "source": [ "# Graph display variations\n", "\n", "This robot is entirely revolute, but we can visualize the joint types in the graphical representation by using the `jtype` keyword argument" - ], - "id": "fce34f81" + ] }, { "cell_type": "code", "execution_count": null, + "id": "733672f5", "metadata": {}, "outputs": [], "source": [ "robot.dotfile(\"planar_y.dot\", jtype=True)\n", "!dot -Tsvg planar_y.dot > planar_y.svg\n", "SVG(filename=\"planar_y.svg\")" - ], - "id": "733672f5" + ] }, { "cell_type": "markdown", + "id": "fb39355d", "metadata": {}, "source": [ "and the input to a link frame is a black dot if it is preceded by a revolute joint, and a black box if it preceded by a prismatic joint.\n", "\n", "We can also highlight the ETS by placing them in boxes, rather than being just edge labels, using the `etsbox` option" - ], - "id": "fb39355d" + ] }, { "cell_type": "code", "execution_count": null, + "id": "6f999740", "metadata": {}, "outputs": [], "source": [ "robot.dotfile(\"planar_y.dot\", etsbox=True)\n", "!dot -Tsvg planar_y.dot > planar_y.svg\n", "SVG(filename=\"planar_y.svg\")" - ], - "id": "6f999740" + ] }, { "cell_type": "markdown", + "id": "173f9240", "metadata": {}, "source": [ "## PR2 model\n", "\n", "The PR2 is a very complex branched robot with nearly 90 links" - ], - "id": "173f9240" + ] }, { "cell_type": "code", "execution_count": null, + "id": "454a5ced", "metadata": {}, "outputs": [], "source": [ @@ -354,16 +355,15 @@ "pr2.dotfile(\"pr2.dot\")\n", "!dot -Tsvg pr2.dot > pr2.svg\n", "SVG(filename=\"pr2.svg\")" - ], - "id": "454a5ced" + ] }, { "cell_type": "code", "execution_count": null, + "id": "644b4e65", "metadata": {}, "outputs": [], - "source": [], - "id": "644b4e65" + "source": [] } ], "metadata": { @@ -373,45 +373,7 @@ "name": "python3" }, "language_info": { - "codemirror_mode": { - "name": "ipython", - "version": 3 - }, - "file_extension": ".py", - "mimetype": "text/x-python", - "name": "python", - "nbconvert_exporter": "python", - "pygments_lexer": "ipython3", - "version": "3.8.5" - }, - "varInspector": { - "cols": { - "lenName": 16, - "lenType": 16, - "lenVar": 40 - }, - "kernels_config": { - "python": { - "delete_cmd_postfix": "", - "delete_cmd_prefix": "del ", - "library": "var_list.py", - "varRefreshCmd": "print(var_dic_list())" - }, - "r": { - "delete_cmd_postfix": ") ", - "delete_cmd_prefix": "rm(", - "library": "var_list.r", - "varRefreshCmd": "cat(var_dic_list()) " - } - }, - "types_to_exclude": [ - "module", - "function", - "builtin_function_or_method", - "instance", - "_Feature" - ], - "window_display": false + "name": "python" } }, "nbformat": 4, diff --git a/docs/notebooks/clear_outputs.py b/docs/notebooks/clear_outputs.py index 8bba1e55a..96a2e04ce 100644 --- a/docs/notebooks/clear_outputs.py +++ b/docs/notebooks/clear_outputs.py @@ -1,5 +1,5 @@ #!/usr/bin/env python -"""Clear cell outputs and execution counts from every notebook in this folder. +"""Clear cell outputs, execution counts and editor metadata from every notebook here. Pure stdlib (no dependency on jupyter/nbconvert being installed), so it can run as a commit hook on any machine with just Python. Equivalent to @@ -21,6 +21,28 @@ HERE = Path(__file__).resolve().parent +# What an editor writes into a notebook's metadata depends on who opened it last: the +# name of their Python environment, its version, interpreter paths, widget state and +# so on. That is noise in every diff, so notebooks are committed with the same +# kernel and language, and with only the metadata that belongs to this repository. +KERNELSPEC = {"display_name": "Python 3", "language": "python", "name": "python3"} +LANGUAGE_INFO = {"name": "python"} +OWN_METADATA = ("rtb_bootstrap",) + + +def normalize_metadata(nb: dict) -> bool: + """Set a notebook's metadata to its canonical form, in place. + + :returns: True if it was (or would be) changed. + """ + old = nb.get("metadata", {}) + new = {"kernelspec": dict(KERNELSPEC), "language_info": dict(LANGUAGE_INFO)} + new.update({k: old[k] for k in OWN_METADATA if k in old}) + if old == new: + return False + nb["metadata"] = new + return True + def clear_cell(cell: dict) -> bool: """Clear one cell's outputs/execution count in place. @@ -44,22 +66,29 @@ def clear_cell(cell: dict) -> bool: return changed +def canonical_text(nb: dict) -> str: + """The notebook as nbformat (and so VS Code and Jupyter) writes it.""" + return json.dumps(nb, indent=1, ensure_ascii=False, sort_keys=True) + "\n" + + def process_notebook(path: Path, fix: bool) -> bool: - """Check (and optionally clear) one notebook's outputs. + """Check (and optionally clear) one notebook's outputs and metadata. :returns: True if the notebook was (or would be) changed. """ - with path.open("r", encoding="utf-8") as f: - nb = json.load(f) + raw = path.read_text(encoding="utf-8") + nb = json.loads(raw) # a list, not a generator: any() would stop at the first dirty cell and leave # the rest uncleared changed = any([clear_cell(cell) for cell in nb.get("cells", [])]) + changed = normalize_metadata(nb) or changed + text = canonical_text(nb) + changed = changed or text != raw if changed and fix: - with path.open("w", encoding="utf-8") as f: - json.dump(nb, f, indent=1, ensure_ascii=False) - f.write("\n") + with path.open("w", encoding="utf-8", newline="\n") as f: + f.write(text) return changed diff --git a/docs/notebooks/dynamics.ipynb b/docs/notebooks/dynamics.ipynb index 1fcc530d3..9b5b46c4c 100644 --- a/docs/notebooks/dynamics.ipynb +++ b/docs/notebooks/dynamics.ipynb @@ -2,6 +2,7 @@ "cells": [ { "cell_type": "markdown", + "id": "28f9bef8", "metadata": {}, "source": [ "
\n", @@ -18,12 +19,12 @@ "(c) Peter Corke 2026\n", "\n", "The inertia, velocity terms and gravity load of a Puma 560 robot, and its forward dynamics." - ], - "id": "28f9bef8" + ] }, { "cell_type": "code", "execution_count": null, + "id": "643c6a7b", "metadata": { "jupyter": { "source_hidden": true @@ -73,12 +74,12 @@ " return colab\n", "\n", "COLAB = await ensure_installed()\n" - ], - "id": "643c6a7b" + ] }, { "cell_type": "code", "execution_count": null, + "id": "b51522bd", "metadata": { "scrolled": false }, @@ -99,51 +100,51 @@ "%matplotlib ipympl\n", "plt.close(\"all\")\n", "%config NotebookBackend.figure_format = 'retina'" - ], - "id": "b51522bd" + ] }, { "cell_type": "markdown", + "id": "7058a6d2", "metadata": {}, "source": [ "We will instantiate a model of the Puma 560 robot which has well known inertial parameters" - ], - "id": "7058a6d2" + ] }, { "cell_type": "code", "execution_count": null, + "id": "05a35b38", "metadata": { "scrolled": false }, "outputs": [], "source": [ "p560 = rtb.models.DH.Puma560()" - ], - "id": "05a35b38" + ] }, { "cell_type": "markdown", + "id": "037b8347", "metadata": {}, "source": [ "and show its configuration in a typical desktop working pose" - ], - "id": "037b8347" + ] }, { "cell_type": "code", "execution_count": null, + "id": "55c9eafa", "metadata": { "scrolled": false }, "outputs": [], "source": [ "p560.plot(p560.qn);" - ], - "id": "55c9eafa" + ] }, { "cell_type": "markdown", + "id": "92192d3b", "metadata": {}, "source": [ "The rigid-body equations of motion for a robot are a set of coupled differential equations\n", @@ -160,45 +161,45 @@ "\\mathbf{g}(\\mathit{q}) = \\mathit{\\tau}\n", "$$\n", "where $\\mathit{\\tau}$ is the torque required for this condition $\\mathit{q} = \\mathit{\\dot{q}} = 0$ to be true, that is, the torque required to stop the robot falling under its own weight. The toolbox can compute this" - ], - "id": "92192d3b" + ] }, { "cell_type": "code", "execution_count": null, + "id": "4e6cb1bb", "metadata": { "scrolled": false }, "outputs": [], "source": [ "p560.gravload(p560.qn)" - ], - "id": "4e6cb1bb" + ] }, { "cell_type": "markdown", + "id": "4f123bc5", "metadata": {}, "source": [ "and it shows, as expected, that the shoulder is exerting significant torque to hold the arm up and stationary.\n", "\n", "The inertia matrix relates torque to joint acceleration and is the mass in a multi-dimensional version of Newton's second law $F = m a$. In this configuration the inertia matrix is" - ], - "id": "4f123bc5" + ] }, { "cell_type": "code", "execution_count": null, + "id": "ff4a51b0", "metadata": { "scrolled": false }, "outputs": [], "source": [ "p560.inertia(p560.qn)" - ], - "id": "ff4a51b0" + ] }, { "cell_type": "markdown", + "id": "62f5cd82", "metadata": {}, "source": [ "The diagonal elements $M_{jj}$ indicate the inertia experienced by the joint $j$, ie. Newton's second law for this joint is $\\tau_j = M_{jj} \\ddot{q}_j$.\n", @@ -206,12 +207,12 @@ "The matrix is symmetric and the off-diagonal terms $M_{ij} = M_{ji}$ couple acceleration of one joint into a disturbance torque on another joint, ie. $\\tau_j = M_{ij} \\ddot{q}_i$.\n", "\n", "The inertia matrix is a function of joint configuration, that is, the elements of the inertia matrix change as we vary the angles of joints 1 and 2, ie. $q_2$ and $q_3$. It is this configuration varying inertia and coupling between joints that is a fundamental challenge for high-quality joint control." - ], - "id": "62f5cd82" + ] }, { "cell_type": "code", "execution_count": null, + "id": "348a5802", "metadata": { "scrolled": false }, @@ -226,20 +227,20 @@ " M = p560.inertia(np.r_[0, Q2[i, j], Q3[i, j], 0, 0, 0])\n", " M11[i, j] = M[0, 0]\n", " M12[i, j] = M[0, 1]" - ], - "id": "348a5802" + ] }, { "cell_type": "markdown", + "id": "ef040af1", "metadata": {}, "source": [ "The inertia \"seen\" by joint 1 varies as a function of $q_2$ and $q_3$ as shown below" - ], - "id": "ef040af1" + ] }, { "cell_type": "code", "execution_count": null, + "id": "9507f059", "metadata": { "scrolled": false }, @@ -251,40 +252,40 @@ "ax.set_xlabel(\"$q_2$ (rad)\")\n", "ax.set_ylabel(\"$q_3$ (rad)\")\n", "ax.set_zlabel(\"$M_{11}$ ($kg.m^2$)\")" - ], - "id": "9507f059" + ] }, { "cell_type": "markdown", + "id": "7d71f5fa", "metadata": {}, "source": [ "The ratio of maximum to minimum values is" - ], - "id": "7d71f5fa" + ] }, { "cell_type": "code", "execution_count": null, + "id": "d6326739", "metadata": { "scrolled": false }, "outputs": [], "source": [ "M11.max() / M11.min()" - ], - "id": "d6326739" + ] }, { "cell_type": "markdown", + "id": "d45959e5", "metadata": {}, "source": [ "The coupling inertia between joints 1 and 2 also varies with configuration and we can plot that as well" - ], - "id": "d45959e5" + ] }, { "cell_type": "code", "execution_count": null, + "id": "9eaffd99", "metadata": { "scrolled": false }, @@ -296,20 +297,20 @@ "ax.set_xlabel(\"$q_2$ (rad)\")\n", "ax.set_ylabel(\"$q_3$ (rad)\")\n", "ax.set_zlabel(\"$M_{12}$ ($kg.m^2$)\")" - ], - "id": "9eaffd99" + ] }, { "cell_type": "markdown", + "id": "76e6c451", "metadata": {}, "source": [ "The velocity terms are a bit harder to comprehend but they mean that rotation of one joint (and its link) can exert a torque on other joints. Consider that the should joint is rotating at 1 rad/sec, then the torque will be " - ], - "id": "76e6c451" + ] }, { "cell_type": "code", "execution_count": null, + "id": "5f7d4b51", "metadata": { "scrolled": false }, @@ -317,43 +318,43 @@ "source": [ "qd = np.r_[0, 1, 0, 0, 0, 0]\n", "p560.coriolis(p560.qn, qd) @ qd" - ], - "id": "5f7d4b51" + ] }, { "cell_type": "markdown", + "id": "1eec4c42", "metadata": {}, "source": [ "and we see that it exerts a torque on the waist and elbow joints.\n", "\n", "The algorithms to compute the various terms in the rigid-body equations of motion are based on the recursive Newton-Euler algorithm" - ], - "id": "1eec4c42" + ] }, { "cell_type": "code", "execution_count": null, + "id": "80332c44", "metadata": { "scrolled": false }, "outputs": [], "source": [ "p560.rne(p560.qn, np.zeros((6,)), np.zeros((6,)))" - ], - "id": "80332c44" + ] }, { "cell_type": "markdown", + "id": "6db5db6b", "metadata": {}, "source": [ "which computes $\\tau = \\mbox{rne}(\\mathit{q}, \\mathit{\\dot{q}}, \\mathit{\\dot{q}})$ and can accept additional arguments such as gravity or a wrench applied to the end-effector. In the Toolbox this algorithm is implemented in C for maximum performance.\n", "\n", "The example above computes the gravity load, cross check it with the result computed earlier.\n" - ], - "id": "6db5db6b" + ] }, { "cell_type": "markdown", + "id": "c9546c97", "metadata": {}, "source": [ "We can rearrange the equations of motion as\n", @@ -363,12 +364,12 @@ "which is the joint acceleration for a robot in the state $(\\mathit{q}, \\mathit{\\dot{q}})$ with an applied torque of $\\mathit{\\tau}$.\n", "\n", "If we integrate this we can compute the trajectory of the joint coordinates (and velocities) as a function of time" - ], - "id": "c9546c97" + ] }, { "cell_type": "code", "execution_count": null, + "id": "6b90ebc6", "metadata": { "scrolled": false }, @@ -376,11 +377,11 @@ "source": [ "p560nf = p560.nofriction()\n", "tg = p560nf.fdyn(5, p560.qn, dt=0.05)" - ], - "id": "6b90ebc6" + ] }, { "cell_type": "markdown", + "id": "d3bf77a9", "metadata": {}, "source": [ "The first line needs some explanation. The Toolbox can model two types of joint friction:\n", @@ -390,70 +391,69 @@ "Coulomb friction is a _harsh_ non-linearity and it causes the numerical integrator to take very small times steps, so the result will take many minutes to compute. To speed things up, at the expense of some modeling fidelity, we set the Coulomb friction to zero, but retain the viscous friction. The `nofriction()` method returns a clone of the robot with its friction parameters modified.\n", "\n", "The computed joint configuration trajectory is" - ], - "id": "d3bf77a9" + ] }, { "cell_type": "code", "execution_count": null, + "id": "536f859c", "metadata": { "scrolled": true }, "outputs": [], "source": [ "tg.q" - ], - "id": "536f859c" + ] }, { "cell_type": "markdown", + "id": "7c5477ba", "metadata": {}, "source": [ "which we can plot using a Toolbox convenience function" - ], - "id": "7c5477ba" + ] }, { "cell_type": "code", "execution_count": null, + "id": "f7a070a7", "metadata": { "scrolled": false }, "outputs": [], "source": [ "rtb.tools.plot.xplot(tg.t, tg.q)" - ], - "id": "f7a070a7" + ] }, { "cell_type": "markdown", + "id": "0400ddbc", "metadata": {}, "source": [ "or we can animate it, showing the robot collapsing under gravity" - ], - "id": "0400ddbc" + ] }, { "cell_type": "code", "execution_count": null, + "id": "fdd61a45", "metadata": { "scrolled": false }, "outputs": [], "source": [ "p560.plot(tg.q);" - ], - "id": "fdd61a45" + ] }, { "cell_type": "markdown", + "id": "210b9389", "metadata": {}, "source": [ "_Note: this animation is not working in Jupyter notebook..._\n", "\n", "The motion of the robot quickly dies out and it hangs downward, this loss of energy is due to the viscous friction in the robot's joints." - ], - "id": "210b9389" + ] } ], "metadata": { @@ -463,65 +463,7 @@ "name": "python3" }, "language_info": { - "codemirror_mode": { - "name": "ipython", - "version": 3 - }, - "file_extension": ".py", - "mimetype": "text/x-python", - "name": "python", - "nbconvert_exporter": "python", - "pygments_lexer": "ipython3", - "version": "3.8.5" - }, - "toc": { - "base_numbering": 1, - "nav_menu": {}, - "number_sections": true, - "sideBar": true, - "skip_h1_title": false, - "title_cell": "Table of Contents", - "title_sidebar": "Contents", - "toc_cell": false, - "toc_position": {}, - "toc_section_display": true, - "toc_window_display": false - }, - "varInspector": { - "cols": { - "lenName": 16, - "lenType": 16, - "lenVar": 40 - }, - "kernels_config": { - "python": { - "delete_cmd_postfix": "", - "delete_cmd_prefix": "del ", - "library": "var_list.py", - "varRefreshCmd": "print(var_dic_list())" - }, - "r": { - "delete_cmd_postfix": ") ", - "delete_cmd_prefix": "rm(", - "library": "var_list.r", - "varRefreshCmd": "cat(var_dic_list()) " - } - }, - "types_to_exclude": [ - "module", - "function", - "builtin_function_or_method", - "instance", - "_Feature" - ], - "window_display": false - }, - "widgets": { - "application/vnd.jupyter.widget-state+json": { - "state": {}, - "version_major": 2, - "version_minor": 0 - } + "name": "python" } }, "nbformat": 4, diff --git a/docs/notebooks/ets.ipynb b/docs/notebooks/ets.ipynb index 3fcc9e4dd..3e3e66ade 100644 --- a/docs/notebooks/ets.ipynb +++ b/docs/notebooks/ets.ipynb @@ -2,6 +2,7 @@ "cells": [ { "cell_type": "markdown", + "id": "10582ade", "metadata": {}, "source": [ "
\n", @@ -20,12 +21,12 @@ "An introduction to the `ET` and `ETS` classes: building a robot model as a sequence of elementary transforms, evaluating its forward kinematics and Jacobians, and converting between an ETS and a robot.\n", "\n", "> **Modernised 2026-10-19** to address API drift: this notebook was written for an earlier release of the Toolbox, and has been updated so that it runs with the current one." - ], - "id": "10582ade" + ] }, { "cell_type": "code", "execution_count": null, + "id": "9160be55", "metadata": { "jupyter": { "source_hidden": true @@ -75,12 +76,12 @@ " return colab\n", "\n", "COLAB = await ensure_installed()\n" - ], - "id": "9160be55" + ] }, { "cell_type": "code", "execution_count": null, + "id": "aa0c8891", "metadata": {}, "outputs": [], "source": [ @@ -90,11 +91,11 @@ "from spatialmath.base import sym\n", "\n", "%matplotlib ipympl" - ], - "id": "aa0c8891" + ] }, { "cell_type": "markdown", + "id": "3f6052da", "metadata": {}, "source": [ "The traditional approach to robot kinematics is to use Denavit-Hartenberg notation where each link is deescribed by a very specific sequence of simple (elementary) transformations\n", @@ -114,43 +115,43 @@ "Let's take a step back and consider defining a robot from a sequence of transforms drawn from the complete set of all possible elementary (or canonical) transformations $\\{r_x, r_y, r_z, t_x, t_y, t_z\\}$ any of which can be a function of a constant or a joint variable.\n", "\n", "The Robotics Toolbox implements this functionality. We will start simply\n" - ], - "id": "3f6052da" + ] }, { "cell_type": "code", "execution_count": null, + "id": "f95ded42", "metadata": {}, "outputs": [], "source": [ "from roboticstoolbox import ET as E\n", "\n", "E.Rx(45, \"deg\")" - ], - "id": "f95ded42" + ] }, { "cell_type": "code", "execution_count": null, + "id": "450063ee", "metadata": {}, "outputs": [], "source": [ "E.ty(2)" - ], - "id": "450063ee" + ] }, { "cell_type": "code", "execution_count": null, + "id": "ef1861e6", "metadata": {}, "outputs": [], "source": [ "E.Ry()" - ], - "id": "ef1861e6" + ] }, { "cell_type": "markdown", + "id": "ec7cbbf3", "metadata": {}, "source": [ "Consider we want to create a simple 2 link planar robot arranged like\n", @@ -158,401 +159,401 @@ "o-------o--<>---x \n", "\n", "where 'o' is a joint, 'x' is the end effector, the first link is 1m long, and the second link can change its length - it is a prismatic joint. The robot can be represented by just 4 elementary transforms" - ], - "id": "ec7cbbf3" + ] }, { "cell_type": "code", "execution_count": null, + "id": "8f76fadf", "metadata": {}, "outputs": [], "source": [ "e = E.Rz() * E.tx(1) * E.Rz() * E.tx()\n", "e" - ], - "id": "8f76fadf" + ] }, { "cell_type": "markdown", + "id": "25bf41e4", "metadata": {}, "source": [ "`e` is an ETS object" - ], - "id": "25bf41e4" + ] }, { "cell_type": "code", "execution_count": null, + "id": "0df1b638", "metadata": {}, "outputs": [], "source": [ "type(e)" - ], - "id": "0df1b638" + ] }, { "cell_type": "markdown", + "id": "86669081", "metadata": {}, "source": [ "which has a number of methods including " - ], - "id": "86669081" + ] }, { "cell_type": "code", "execution_count": null, + "id": "a887d3db", "metadata": {}, "outputs": [], "source": [ "len(e)" - ], - "id": "a887d3db" + ] }, { "cell_type": "markdown", + "id": "59214ecb", "metadata": {}, "source": [ "indicating it has four transforms" - ], - "id": "59214ecb" + ] }, { "cell_type": "code", "execution_count": null, + "id": "aeed5bd7", "metadata": {}, "outputs": [], "source": [ "e.n" - ], - "id": "aeed5bd7" + ] }, { "cell_type": "markdown", + "id": "9bb00c35", "metadata": {}, "source": [ "indicating it has three joint variables, and they occur at the indices given by" - ], - "id": "9bb00c35" + ] }, { "cell_type": "code", "execution_count": null, + "id": "506f0f73", "metadata": {}, "outputs": [], "source": [ "e.joints()" - ], - "id": "506f0f73" + ] }, { "cell_type": "markdown", + "id": "d7959cae", "metadata": {}, "source": [ "The ETS object acts a lot like a list, and we can slice it" - ], - "id": "d7959cae" + ] }, { "cell_type": "code", "execution_count": null, + "id": "43b2e0f6", "metadata": {}, "outputs": [], "source": [ "e[1]" - ], - "id": "43b2e0f6" + ] }, { "cell_type": "code", "execution_count": null, + "id": "3fa3fa85", "metadata": {}, "outputs": [], "source": [ "e[2:]" - ], - "id": "3fa3fa85" + ] }, { "cell_type": "markdown", + "id": "0242faeb", "metadata": {}, "source": [ "There are also a number of predicates" - ], - "id": "0242faeb" + ] }, { "cell_type": "code", "execution_count": null, + "id": "2c7a8c92", "metadata": {}, "outputs": [], "source": [ "e[0].isjoint" - ], - "id": "2c7a8c92" + ] }, { "cell_type": "code", "execution_count": null, + "id": "a7c3b2ef", "metadata": {}, "outputs": [], "source": [ "e[1].isjoint" - ], - "id": "a7c3b2ef" + ] }, { "cell_type": "code", "execution_count": null, + "id": "6f4aa252", "metadata": {}, "outputs": [], "source": [ "e[0].isrotation" - ], - "id": "6f4aa252" + ] }, { "cell_type": "code", "execution_count": null, + "id": "837d7307", "metadata": {}, "outputs": [], "source": [ "e[3].isrotation" - ], - "id": "837d7307" + ] }, { "cell_type": "code", "execution_count": null, + "id": "d34b4e70", "metadata": {}, "outputs": [], "source": [ "e[3].istranslation" - ], - "id": "d34b4e70" + ] }, { "cell_type": "markdown", + "id": "6377ab22", "metadata": {}, "source": [ "We can substitute in values" - ], - "id": "6377ab22" + ] }, { "cell_type": "code", "execution_count": null, + "id": "b0c00e67", "metadata": {}, "outputs": [], "source": [ "e.eval([0, 0, 1])" - ], - "id": "b0c00e67" + ] }, { "cell_type": "markdown", + "id": "be8e39ec", "metadata": {}, "source": [ "and the result is an SE(3) matrix encapsulated in an `SE3` instance. As expected is a translation of 2m in the x-direction. \n", "\n", "Let try another configuration" - ], - "id": "be8e39ec" + ] }, { "cell_type": "code", "execution_count": null, + "id": "868b757f", "metadata": {}, "outputs": [], "source": [ "e.eval([np.deg2rad(90), np.deg2rad(-90), 2])" - ], - "id": "868b757f" + ] }, { "cell_type": "markdown", + "id": "905574d2", "metadata": {}, "source": [ "Joint angles are in radians, so we converted the angles from degrees with `np.deg2rad`. The prismatic joint value is a length in metres and is left as it is." - ], - "id": "905574d2" + ] }, { "cell_type": "markdown", + "id": "943c4184", "metadata": {}, "source": [ "## Joint flip\n", "\n", "Sometimes when we are describing a system of joints and constant transforms, the joint coordinate sense needs to be reversed to meet the user's definition. In the example above, consider that the first joint increases positively in the clockwise direction. In that case we could write" - ], - "id": "943c4184" + ] }, { "cell_type": "code", "execution_count": null, + "id": "f1c519dc", "metadata": {}, "outputs": [], "source": [ "e = E.Rz(flip=True) * E.tx(1) * E.Rz() * E.tx()\n", "e" - ], - "id": "f1c519dc" + ] }, { "cell_type": "markdown", + "id": "f013f36a", "metadata": {}, "source": [ "and the flip is indicated by a negative sign `Rz(-q0)` when the ETS is displayed." - ], - "id": "f013f36a" + ] }, { "cell_type": "code", "execution_count": null, + "id": "8971de1e", "metadata": {}, "outputs": [], "source": [ "e.eval([np.deg2rad(-90), np.deg2rad(-90), 2])" - ], - "id": "8971de1e" + ] }, { "cell_type": "markdown", + "id": "3da01f41", "metadata": {}, "source": [ "Now if we negate the passed value of q0 we see that we have compute the same answer as above.\n", "\n", "Joint flip is handled implicitly, but can be tested" - ], - "id": "3da01f41" + ] }, { "cell_type": "code", "execution_count": null, + "id": "61e98e5b", "metadata": {}, "outputs": [], "source": [ "e[0].isflip" - ], - "id": "61e98e5b" + ] }, { "cell_type": "code", "execution_count": null, + "id": "645710e5", "metadata": {}, "outputs": [], "source": [ "e[1].isflip" - ], - "id": "645710e5" + ] }, { "cell_type": "markdown", + "id": "492b1d4b", "metadata": {}, "source": [ "## Symbolics\n", "\n" - ], - "id": "492b1d4b" + ] }, { "cell_type": "code", "execution_count": null, + "id": "24641d96", "metadata": {}, "outputs": [], "source": [ "q = sym.symbol(\"q_:3\")\n", "print(q)" - ], - "id": "24641d96" + ] }, { "cell_type": "markdown", + "id": "c2a4d56a", "metadata": {}, "source": [ "Which creates a vector of three joint angle variables. The advantage of having the underscore in the name is that Jupyter considers this as LaTeX subscript notation when it pretty prints the symbols" - ], - "id": "c2a4d56a" + ] }, { "cell_type": "code", "execution_count": null, + "id": "c0d66855", "metadata": {}, "outputs": [], "source": [ "q[0]" - ], - "id": "c0d66855" + ] }, { "cell_type": "markdown", + "id": "ee17b8a5", "metadata": {}, "source": [ "We can substitute in the symbolic values just as easily as we did the numerical values to find the resulting overall transform" - ], - "id": "ee17b8a5" + ] }, { "cell_type": "code", "execution_count": null, + "id": "1f2f6981", "metadata": {}, "outputs": [], "source": [ "e.eval(q)" - ], - "id": "1f2f6981" + ] }, { "cell_type": "markdown", + "id": "d75bb33b", "metadata": {}, "source": [ "## ETS in 2D\n", "\n", "Everything that we've done in 3D we can do in 2D. In fact the example above was a planar problem, so we can express it easily using 2D ETS" - ], - "id": "d75bb33b" + ] }, { "cell_type": "code", "execution_count": null, + "id": "dbaa7737", "metadata": {}, "outputs": [], "source": [ "from roboticstoolbox import ET2 as E2\n", "\n", "e2 = E2.R() * E2.tx(1) * E2.R() * E2.tx()" - ], - "id": "dbaa7737" + ] }, { "cell_type": "markdown", + "id": "dc29781c", "metadata": {}, "source": [ "The main difference is that in 2D there are three elementary transforms to choose from $\\{r, t_x, t_y\\}$ - rotation and translation in the plane.\n", "\n", "If we evaluate this" - ], - "id": "dc29781c" + ] }, { "cell_type": "code", "execution_count": null, + "id": "7d6b9f09", "metadata": {}, "outputs": [], "source": [ "e2.eval([np.deg2rad(90), np.deg2rad(-90), 2])" - ], - "id": "7d6b9f09" + ] }, { "cell_type": "markdown", + "id": "4cd9a23f", "metadata": {}, "source": [ "and the result this time is an SE(2) matrix encapsulated in an `SE2` instance." - ], - "id": "4cd9a23f" + ] }, { "cell_type": "markdown", + "id": "159d08f4", "metadata": {}, "source": [ "## A more complex example\n", @@ -561,12 +562,12 @@ "\n", "Reference:\n", "- [A simple and systematic approach to assigning Denavit-Hartenberg parameters](https://petercorke.com/robotics/a-simple-and-systematic-approach-to-assigning-denavit-hartenberg-parameters), Peter I. Corke, IEEE Transactions on Robotics, 23(3), pp 590-594, June 2007." - ], - "id": "159d08f4" + ] }, { "cell_type": "code", "execution_count": null, + "id": "5e622b60", "metadata": {}, "outputs": [], "source": [ @@ -576,20 +577,20 @@ "l4 = -0.0837\n", "l5 = 0.4318\n", "l6 = 0.0203" - ], - "id": "5e622b60" + ] }, { "cell_type": "markdown", + "id": "3904e7f8", "metadata": {}, "source": [ "and we can describe the tip of the robot by a sequence of elementary transforms" - ], - "id": "3904e7f8" + ] }, { "cell_type": "code", "execution_count": null, + "id": "f944d7c7", "metadata": {}, "outputs": [], "source": [ @@ -609,214 +610,214 @@ " * E.tx(0.2)\n", ")\n", "e" - ], - "id": "f944d7c7" + ] }, { "cell_type": "code", "execution_count": null, + "id": "f3403ac2", "metadata": {}, "outputs": [], "source": [ "e.n" - ], - "id": "f3403ac2" + ] }, { "cell_type": "code", "execution_count": null, + "id": "5cc84d5f", "metadata": {}, "outputs": [], "source": [ "e.joints()" - ], - "id": "5cc84d5f" + ] }, { "cell_type": "code", "execution_count": null, + "id": "4b359ba4", "metadata": {}, "outputs": [], "source": [ "e.eval([0] * 6)" - ], - "id": "4b359ba4" + ] }, { "cell_type": "markdown", + "id": "6cd527a2", "metadata": {}, "source": [ "We can compute not only forward kinematics, but also the differential kinematics" - ], - "id": "6cd527a2" + ] }, { "cell_type": "code", "execution_count": null, + "id": "cf24b992", "metadata": {}, "outputs": [], "source": [ "J = e.jacobe([0] * 6)\n", "J" - ], - "id": "cf24b992" + ] }, { "cell_type": "markdown", + "id": "519116f1", "metadata": {}, "source": [ "which relates the rate of change of the end-effector position to the rate of change of joint coordinates.\n", "The rate of change of the end-effector position is its velocity, but because it moves in 3D we need to describe its translational velocity (a 3-vector) and its angular velocity (another 3-vector). We pack these into a 6-vector which is known as _spatial velocity_ and denoted by $\\nu$. These quantities are related by\n", "\n", "$\\nu = \\mathbf{J}(q) \\dot{q}$" - ], - "id": "519116f1" + ] }, { "cell_type": "markdown", + "id": "357b6ba5", "metadata": {}, "source": [ "The Hessian is a tensor (3-dimensional matrix) that relates joint velocity to end-effector acceleration $\\dot{\\nu}$" - ], - "id": "357b6ba5" + ] }, { "cell_type": "code", "execution_count": null, + "id": "4461a5cc", "metadata": {}, "outputs": [], "source": [ "e.hessian0([0] * 6)" - ], - "id": "4461a5cc" + ] }, { "cell_type": "markdown", + "id": "83559b57", "metadata": {}, "source": [ "## Compilation\n", "\n", "If we were to evaluate this ETS many times we would be peforming lots of unneccessary multiplication of constant terms such as " - ], - "id": "83559b57" + ] }, { "cell_type": "code", "execution_count": null, + "id": "7288738e", "metadata": {}, "outputs": [], "source": [ "e[4:7]" - ], - "id": "7288738e" + ] }, { "cell_type": "markdown", + "id": "9ffd4455", "metadata": {}, "source": [ "To make an ETS more efficient to run we can \"compile it\"" - ], - "id": "9ffd4455" + ] }, { "cell_type": "code", "execution_count": null, + "id": "1873953a", "metadata": {}, "outputs": [], "source": [ "ec = e.compile()\n", "ec" - ], - "id": "1873953a" + ] }, { "cell_type": "markdown", + "id": "15fac11a", "metadata": {}, "source": [ "and we can see that consecutive constant transforms have been folded into new \"not so elementary\" transforms denoted by `Ci`. The value of the ETS, is of course the same." - ], - "id": "15fac11a" + ] }, { "cell_type": "code", "execution_count": null, + "id": "e0288d66", "metadata": {}, "outputs": [], "source": [ "ec.eval([0] * 6)" - ], - "id": "e0288d66" + ] }, { "cell_type": "markdown", + "id": "a4004687", "metadata": {}, "source": [ "## Explicit joint indices\n", "\n", "Consider that we carve out a chunk of this ETS that represents the Puma560, say the wrist bit" - ], - "id": "a4004687" + ] }, { "cell_type": "code", "execution_count": null, + "id": "2dd8c273", "metadata": {}, "outputs": [], "source": [ "wrist = e[9:]\n", "wrist" - ], - "id": "2dd8c273" + ] }, { "cell_type": "markdown", + "id": "d966ca43", "metadata": {}, "source": [ "we see that by default the wrist joints, joint 3-5 in the robot are associated with joints 0-2 in the wrist. If we had a vector of six joint angles, corresponding to the whole robot " - ], - "id": "d966ca43" + ] }, { "cell_type": "code", "execution_count": null, + "id": "ea0ef9d1", "metadata": {}, "outputs": [], "source": [ "q = [0.1, 0.2, 0.3, 0.4, 0.5, 0.6]" - ], - "id": "ea0ef9d1" + ] }, { "cell_type": "markdown", + "id": "a6bbabf7", "metadata": {}, "source": [ "then to evaluate the wrist we'd have to write" - ], - "id": "a6bbabf7" + ] }, { "cell_type": "code", "execution_count": null, + "id": "67a160ee", "metadata": {}, "outputs": [], "source": [ "wrist.eval(q[3:])" - ], - "id": "67a160ee" + ] }, { "cell_type": "markdown", + "id": "03d7473d", "metadata": {}, "source": [ "which is quite doable, but imagine now that the ETS represents the transforms from the right hand to the left hand of a two armed robot. The joint numbers in that ETS would not be sequential with respect to the overall joint coordinate vector of the whole robot.\n", "\n", "To help deal with such situations we can assign a joint index to each ETS. We'll do this for the Puma 560" - ], - "id": "03d7473d" + ] }, { "cell_type": "code", "execution_count": null, + "id": "53383d22", "metadata": {}, "outputs": [], "source": [ @@ -836,58 +837,58 @@ " * E.tx(0.2)\n", ")\n", "e" - ], - "id": "53383d22" + ] }, { "cell_type": "markdown", + "id": "acd9ac3f", "metadata": {}, "source": [ "and when we display the ETS it looks no different, since the joint indices we assigned match the default sequential assignment. However if we now take the wrist part" - ], - "id": "acd9ac3f" + ] }, { "cell_type": "code", "execution_count": null, + "id": "9bd1d80f", "metadata": {}, "outputs": [], "source": [ "wrist = e[9:]\n", "wrist" - ], - "id": "9bd1d80f" + ] }, { "cell_type": "markdown", + "id": "8a03ec2c", "metadata": {}, "source": [ "we see that the explicit joint indices are displayed as arguments of the ETs.\n", "\n", "Now if evaluate the wrist part" - ], - "id": "8a03ec2c" + ] }, { "cell_type": "code", "execution_count": null, + "id": "2856e577", "metadata": {}, "outputs": [], "source": [ "wrist.eval(q)" - ], - "id": "2856e577" + ] }, { "cell_type": "markdown", + "id": "18dec36c", "metadata": {}, "source": [ "we get the same answer as earlier because it is referencing `q[3]`, `q[4]` and `q[5]`." - ], - "id": "18dec36c" + ] }, { "cell_type": "markdown", + "id": "529d6c3c", "metadata": {}, "source": [ "## Handling joint offsets\n", @@ -904,107 +905,106 @@ "\n", "\n", "These extra transformation incur minimal computational overhead, since after compilation, they will be folded in with other constant transforms.\n" - ], - "id": "529d6c3c" + ] }, { "cell_type": "markdown", + "id": "eb9d0f73", "metadata": {}, "source": [ "## Converting an ETS to a robot\n", "\n", "The ETS succinctly describes the forward kinematics of a robot and we can also compute its Jacobian and Hessian. However Toolbox robot objects have additional capability for graphics, inverse kinematics and dynamics. We can _promote_ an ETS to a robot by" - ], - "id": "eb9d0f73" + ] }, { "cell_type": "code", "execution_count": null, + "id": "bcebdbc3", "metadata": {}, "outputs": [], "source": [ "robot = rtb.ERobot(e)\n", "print(robot)" - ], - "id": "bcebdbc3" + ] }, { "cell_type": "markdown", + "id": "28861093", "metadata": {}, "source": [ "The ETS has been chopped into segments that connect links. Each link is a rigid-body with an attached coordinate frame. Relative to that frame is a short ETS that ends with joint transform that describes the coordinate frame of the next link in the chain.\n", "\n", "Note that the last link, with its name in blue, has no joint transform. It is simply a constant relative pose with respect to the link 5 coordinate frame." - ], - "id": "28861093" + ] }, { "cell_type": "markdown", + "id": "3dfbbb58", "metadata": {}, "source": [ "## Converting a robot to ETS\n", "\n", "We can also perform the inverse transformation. Consider a model defined using Denavit-Hartenberg notation" - ], - "id": "3dfbbb58" + ] }, { "cell_type": "code", "execution_count": null, + "id": "e075473b", "metadata": {}, "outputs": [], "source": [ "puma = rtb.models.DH.Puma560()\n", "print(puma)" - ], - "id": "e075473b" + ] }, { "cell_type": "code", "execution_count": null, + "id": "8e5dcad8", "metadata": {}, "outputs": [], "source": [ "print(puma.ets())" - ], - "id": "8e5dcad8" + ] }, { "cell_type": "markdown", + "id": "735dfa27", "metadata": {}, "source": [ "Or for a robot with a prismatic joint" - ], - "id": "735dfa27" + ] }, { "cell_type": "code", "execution_count": null, + "id": "ba16fc91", "metadata": {}, "outputs": [], "source": [ "stanford = rtb.models.DH.Stanford()\n", "print(stanford)" - ], - "id": "ba16fc91" + ] }, { "cell_type": "code", "execution_count": null, + "id": "97bf38a1", "metadata": {}, "outputs": [], "source": [ "print(stanford.ets())" - ], - "id": "97bf38a1" + ] }, { "cell_type": "code", "execution_count": null, + "id": "c551fbe0", "metadata": {}, "outputs": [], - "source": [], - "id": "c551fbe0" + "source": [] } ], "metadata": { @@ -1014,45 +1014,7 @@ "name": "python3" }, "language_info": { - "codemirror_mode": { - "name": "ipython", - "version": 3 - }, - "file_extension": ".py", - "mimetype": "text/x-python", - "name": "python", - "nbconvert_exporter": "python", - "pygments_lexer": "ipython3", - "version": "3.8.5" - }, - "varInspector": { - "cols": { - "lenName": 16, - "lenType": 16, - "lenVar": 40 - }, - "kernels_config": { - "python": { - "delete_cmd_postfix": "", - "delete_cmd_prefix": "del ", - "library": "var_list.py", - "varRefreshCmd": "print(var_dic_list())" - }, - "r": { - "delete_cmd_postfix": ") ", - "delete_cmd_prefix": "rm(", - "library": "var_list.r", - "varRefreshCmd": "cat(var_dic_list()) " - } - }, - "types_to_exclude": [ - "module", - "function", - "builtin_function_or_method", - "instance", - "_Feature" - ], - "window_display": false + "name": "python" } }, "nbformat": 4, diff --git a/docs/notebooks/graphics-test.ipynb b/docs/notebooks/graphics-test.ipynb index 6092f8401..d8c66e31f 100644 --- a/docs/notebooks/graphics-test.ipynb +++ b/docs/notebooks/graphics-test.ipynb @@ -2,6 +2,7 @@ "cells": [ { "cell_type": "markdown", + "id": "362a8fec", "metadata": {}, "source": [ "
\n", @@ -18,12 +19,12 @@ "(c) Peter Corke 2026\n", "\n", "A one-cell check that plotting a robot works in your environment." - ], - "id": "362a8fec" + ] }, { "cell_type": "code", "execution_count": null, + "id": "b243eb35", "metadata": { "jupyter": { "source_hidden": true @@ -73,12 +74,12 @@ " return colab\n", "\n", "COLAB = await ensure_installed()\n" - ], - "id": "b243eb35" + ] }, { "cell_type": "code", "execution_count": null, + "id": "bb43571a", "metadata": {}, "outputs": [], "source": [ @@ -89,32 +90,18 @@ "puma = models.DH.Puma560()\n", "\n", "env = puma.plot(puma.qn)" - ], - "id": "bb43571a" + ] } ], "metadata": { - "interpreter": { - "hash": "b7d6b0d76025b9176285a6442c3dd6dd39bcfe7241029b7898b7106bd5e9b472" - }, "kernelspec": { - "display_name": "Python 3.8.5 64-bit ('dev': conda)", + "display_name": "Python 3", "language": "python", "name": "python3" }, "language_info": { - "codemirror_mode": { - "name": "ipython", - "version": 3 - }, - "file_extension": ".py", - "mimetype": "text/x-python", - "name": "python", - "nbconvert_exporter": "python", - "pygments_lexer": "ipython3", - "version": "3.8.5" - }, - "orig_nbformat": 4 + "name": "python" + } }, "nbformat": 4, "nbformat_minor": 5 diff --git a/docs/notebooks/icra2021.ipynb b/docs/notebooks/icra2021.ipynb index df8f0c5c4..2f2e86322 100644 --- a/docs/notebooks/icra2021.ipynb +++ b/docs/notebooks/icra2021.ipynb @@ -2,6 +2,7 @@ "cells": [ { "cell_type": "markdown", + "id": "0065faab", "metadata": {}, "source": [ "
\n", @@ -22,12 +23,12 @@ "This is the code for the examples in the paper published at ICRA 2021.\n", "\n", "> **Modernised 2026-10-19** to address API drift: this notebook was written for an earlier release of the Toolbox, and has been updated so that it runs with the current one." - ], - "id": "0065faab" + ] }, { "cell_type": "code", "execution_count": null, + "id": "1a59f089", "metadata": { "jupyter": { "source_hidden": true @@ -77,12 +78,12 @@ " return colab\n", "\n", "COLAB = await ensure_installed()\n" - ], - "id": "1a59f089" + ] }, { "cell_type": "code", "execution_count": null, + "id": "9bdb77dd", "metadata": {}, "outputs": [], "source": [ @@ -111,105 +112,105 @@ " )\n", ")\n", "%matplotlib ipympl" - ], - "id": "9bdb77dd" + ] }, { "cell_type": "markdown", + "id": "fccd7bc3", "metadata": {}, "source": [ "# III.SPATIAL MATHEMATICS" - ], - "id": "fccd7bc3" + ] }, { "cell_type": "code", "execution_count": null, + "id": "575d2335", "metadata": {}, "outputs": [], "source": [ "from spatialmath.base import *\n", "\n", "T = transl(0.5, 0.0, 0.0) @ rpy2tr(0.1, 0.2, 0.3, order=\"xyz\") @ trotx(-90, \"deg\")" - ], - "id": "575d2335" + ] }, { "cell_type": "code", "execution_count": null, + "id": "5eafdde4", "metadata": {}, "outputs": [], "source": [ "from spatialmath import *\n", "\n", "T = SE3(0.5, 0.0, 0.0) * SE3.RPY([0.1, 0.2, 0.3], order=\"xyz\") * SE3.Rx(-90, unit=\"deg\")" - ], - "id": "5eafdde4" + ] }, { "cell_type": "code", "execution_count": null, + "id": "4765e0c8", "metadata": {}, "outputs": [], "source": [ "T.eul()" - ], - "id": "4765e0c8" + ] }, { "cell_type": "code", "execution_count": null, + "id": "87f59a29", "metadata": {}, "outputs": [], "source": [ "T.R" - ], - "id": "87f59a29" + ] }, { "cell_type": "code", "execution_count": null, + "id": "b9ab96bc", "metadata": {}, "outputs": [], "source": [ "T.plot(color=\"red\", frame=\"2\")" - ], - "id": "b9ab96bc" + ] }, { "cell_type": "code", "execution_count": null, + "id": "5887c919", "metadata": {}, "outputs": [], "source": [ "UnitQuaternion.Rx(0.3)\n", "UnitQuaternion.AngVec(0.3, [1, 0, 0])" - ], - "id": "5887c919" + ] }, { "cell_type": "code", "execution_count": null, + "id": "a8c9c5b0", "metadata": {}, "outputs": [], "source": [ "R = SE3.Rx(np.linspace(0, pi / 2, num=100))\n", "len(R)" - ], - "id": "a8c9c5b0" + ] }, { "cell_type": "markdown", + "id": "73993f11", "metadata": {}, "source": [ "# IV. ROBOTICS TOOLBOX\n", "## A. Robot models" - ], - "id": "73993f11" + ] }, { "cell_type": "code", "execution_count": null, + "id": "e8ce61e1", "metadata": {}, "outputs": [], "source": [ @@ -221,12 +222,12 @@ "a2 = 0.360\n", "d4 = 0.380\n", "d6 = 0.065;" - ], - "id": "e8ce61e1" + ] }, { "cell_type": "code", "execution_count": null, + "id": "95745a32", "metadata": {}, "outputs": [], "source": [ @@ -238,72 +239,72 @@ " ],\n", " name=\"my IRB140\",\n", ")" - ], - "id": "95745a32" + ] }, { "cell_type": "code", "execution_count": null, + "id": "a350f331", "metadata": {}, "outputs": [], "source": [ "puma = models.DH.Puma560()" - ], - "id": "a350f331" + ] }, { "cell_type": "code", "execution_count": null, + "id": "d202e6f5", "metadata": {}, "outputs": [], "source": [ "T = puma.fkine([0.1, 0.2, 0.3, 0.4, 0.5, 0.6])" - ], - "id": "d202e6f5" + ] }, { "cell_type": "code", "execution_count": null, + "id": "efc6d9ee", "metadata": {}, "outputs": [], "source": [ "sol = puma.ikine_LM(T)" - ], - "id": "efc6d9ee" + ] }, { "cell_type": "code", "execution_count": null, + "id": "df8baf59", "metadata": {}, "outputs": [], "source": [ "puma.plot(sol.q);" - ], - "id": "df8baf59" + ] }, { "cell_type": "code", "execution_count": null, + "id": "9dc57bf5", "metadata": {}, "outputs": [], "source": [ "puma.ikine_a(T, config=\"lun\")" - ], - "id": "9dc57bf5" + ] }, { "cell_type": "code", "execution_count": null, + "id": "98410a92", "metadata": {}, "outputs": [], "source": [ "from roboticstoolbox import ET" - ], - "id": "98410a92" + ] }, { "cell_type": "code", "execution_count": null, + "id": "e32ae17a", "metadata": {}, "outputs": [], "source": [ @@ -314,12 +315,12 @@ "l4 = 0.0203\n", "l5 = 0.0837\n", "l6 = 0.4318;" - ], - "id": "e32ae17a" + ] }, { "cell_type": "code", "execution_count": null, + "id": "ed5d45bc", "metadata": {}, "outputs": [], "source": [ @@ -337,23 +338,23 @@ " * ET.Ry()\n", " * ET.Rz()\n", ")" - ], - "id": "ed5d45bc" + ] }, { "cell_type": "code", "execution_count": null, + "id": "7fedc2ec", "metadata": {}, "outputs": [], "source": [ "robot = Robot(e)\n", "print(robot)" - ], - "id": "7fedc2ec" + ] }, { "cell_type": "code", "execution_count": null, + "id": "477c2901", "metadata": { "lines_to_next_cell": 2 }, @@ -361,82 +362,82 @@ "source": [ "panda = models.URDF.Panda()\n", "print(panda)" - ], - "id": "477c2901" + ] }, { "cell_type": "markdown", + "id": "796c2401", "metadata": {}, "source": [ "## B. Trajectories" - ], - "id": "796c2401" + ] }, { "cell_type": "code", "execution_count": null, + "id": "f0c3fa38", "metadata": {}, "outputs": [], "source": [ "traj = jtraj(puma.qz, puma.qr, 100)\n", "traj.qplot()" - ], - "id": "f0c3fa38" + ] }, { "cell_type": "code", "execution_count": null, + "id": "b275c405", "metadata": {}, "outputs": [], "source": [ "t = np.arange(0, 2, 0.010)\n", "T0 = SE3(0.6, -0.5, 0.3)" - ], - "id": "b275c405" + ] }, { "cell_type": "code", "execution_count": null, + "id": "6b690343", "metadata": {}, "outputs": [], "source": [ "T1 = SE3(0.4, 0.5, 0.2)" - ], - "id": "6b690343" + ] }, { "cell_type": "code", "execution_count": null, + "id": "127e2a37", "metadata": {}, "outputs": [], "source": [ "Ts = ctraj(T0, T1, t)\n", "len(Ts)" - ], - "id": "127e2a37" + ] }, { "cell_type": "code", "execution_count": null, + "id": "b04aa6d3", "metadata": {}, "outputs": [], "source": [ "sol = puma.ikine_LM(Ts)\n", "sol.q.shape" - ], - "id": "b04aa6d3" + ] }, { "cell_type": "markdown", + "id": "71fd6549", "metadata": {}, "source": [ "## C. Symbolic manipulation" - ], - "id": "71fd6549" + ] }, { "cell_type": "code", "execution_count": null, + "id": "9dc16ca6", "metadata": {}, "outputs": [], "source": [ @@ -444,224 +445,224 @@ "\n", "phi, theta, psi = sym.symbol(\"φ, ϴ, ψ\")\n", "rpy2r(phi, theta, psi)" - ], - "id": "9dc16ca6" + ] }, { "cell_type": "code", "execution_count": null, + "id": "a333f26b", "metadata": {}, "outputs": [], "source": [ "q = sym.symbol(\"q_:6\") # q = (q_1, q_2, ... q_5)\n", "T = puma.fkine(q);" - ], - "id": "a333f26b" + ] }, { "cell_type": "code", "execution_count": null, + "id": "221ef01e", "metadata": {}, "outputs": [], "source": [ "puma = models.DH.Puma560(symbolic=True)\n", "T = puma.fkine(q)\n", "T.t[0]" - ], - "id": "221ef01e" + ] }, { "cell_type": "code", "execution_count": null, + "id": "c10327f1", "metadata": {}, "outputs": [], "source": [ "puma = models.DH.Puma560(symbolic=False)\n", "J = puma.jacob0(puma.qn)" - ], - "id": "c10327f1" + ] }, { "cell_type": "code", "execution_count": null, + "id": "d26cf89c", "metadata": {}, "outputs": [], "source": [ "J = puma.jacobe(puma.qn)" - ], - "id": "d26cf89c" + ] }, { "cell_type": "markdown", + "id": "3e1af871", "metadata": {}, "source": [ "## D. Differential kinematics" - ], - "id": "3e1af871" + ] }, { "cell_type": "code", "execution_count": null, + "id": "06a7bfae", "metadata": {}, "outputs": [], "source": [ "J = puma.jacob0(puma.qr)" - ], - "id": "06a7bfae" + ] }, { "cell_type": "code", "execution_count": null, + "id": "bfd86d43", "metadata": {}, "outputs": [], "source": [ "np.linalg.matrix_rank(J)" - ], - "id": "bfd86d43" + ] }, { "cell_type": "code", "execution_count": null, + "id": "f19c45e1", "metadata": {}, "outputs": [], "source": [ "jsingu(J)" - ], - "id": "f19c45e1" + ] }, { "cell_type": "code", "execution_count": null, + "id": "bf603029", "metadata": {}, "outputs": [], "source": [ "H = panda.hessian0(panda.qz)\n", "H.shape" - ], - "id": "bf603029" + ] }, { "cell_type": "code", "execution_count": null, + "id": "645ff100", "metadata": {}, "outputs": [], "source": [ "puma.manipulability(puma.qn)" - ], - "id": "645ff100" + ] }, { "cell_type": "code", "execution_count": null, + "id": "7267b88a", "metadata": {}, "outputs": [], "source": [ "puma.manipulability(puma.qn, method=\"asada\")" - ], - "id": "7267b88a" + ] }, { "cell_type": "code", "execution_count": null, + "id": "bb095c57", "metadata": {}, "outputs": [], "source": [ "puma.manipulability(puma.qn, axes=\"trans\")" - ], - "id": "bb095c57" + ] }, { "cell_type": "code", "execution_count": null, + "id": "3ef4e6ec", "metadata": { "lines_to_next_cell": 2 }, "outputs": [], "source": [ "panda.jacobm(panda.qr)" - ], - "id": "3ef4e6ec" + ] }, { "cell_type": "markdown", + "id": "df72eb26", "metadata": {}, "source": [ "## E. Dynamics" - ], - "id": "df72eb26" + ] }, { "cell_type": "code", "execution_count": null, + "id": "dcbd3b3e", "metadata": { "lines_to_next_cell": 2 }, "outputs": [], "source": [ "tau = puma.rne(puma.qn, np.zeros((6,)), np.zeros((6,)))" - ], - "id": "dcbd3b3e" + ] }, { "cell_type": "code", "execution_count": null, + "id": "facc4147", "metadata": { "lines_to_next_cell": 2 }, "outputs": [], "source": [ "J = puma.inertia(puma.qn)" - ], - "id": "facc4147" + ] }, { "cell_type": "code", "execution_count": null, + "id": "4447e527", "metadata": { "lines_to_next_cell": 2 }, "outputs": [], "source": [ "C = puma.coriolis(puma.qn, 0.1 * np.ones((6,)))" - ], - "id": "4447e527" + ] }, { "cell_type": "code", "execution_count": null, + "id": "436a1633", "metadata": { "lines_to_next_cell": 2 }, "outputs": [], "source": [ "g = puma.gravload(puma.qn)" - ], - "id": "436a1633" + ] }, { "cell_type": "code", "execution_count": null, + "id": "9f1cd0c9", "metadata": { "lines_to_next_cell": 2 }, "outputs": [], "source": [ "qdd = puma.accel(puma.qn, tau, np.zeros((6,)))" - ], - "id": "9f1cd0c9" + ] }, { "cell_type": "markdown", + "id": "931b155c", "metadata": {}, "source": [ "# V. NEW CAPABILITY\n", "## B. Collision checking" - ], - "id": "931b155c" + ] }, { "cell_type": "code", "execution_count": null, + "id": "839ce028", "metadata": {}, "outputs": [], "source": [ @@ -670,12 +671,12 @@ "obstacle = Cuboid([1, 1, 1], pose=SE3(1, 0, 0))\n", "iscollision = panda.iscollided(panda.qr, obstacle) # boolean\n", "iscollision = panda.links[0].iscollided(obstacle)" - ], - "id": "839ce028" + ] }, { "cell_type": "code", "execution_count": null, + "id": "4e67c4d9", "metadata": {}, "outputs": [], "source": [ @@ -683,30 +684,30 @@ "print(d, p1, p2)\n", "d, p1, p2 = panda.links[0].closest_point(obstacle)\n", "print(d, p1, p2)" - ], - "id": "4e67c4d9" + ] }, { "cell_type": "markdown", + "id": "c1fcd470", "metadata": {}, "source": [ "## C. Interfaces" - ], - "id": "c1fcd470" + ] }, { "cell_type": "code", "execution_count": null, + "id": "7d052b79", "metadata": {}, "outputs": [], "source": [ "panda.plot(panda.qr);" - ], - "id": "7d052b79" + ] }, { "cell_type": "code", "execution_count": null, + "id": "b3d35544", "metadata": {}, "outputs": [], "source": [ @@ -717,65 +718,17 @@ "backend.add(panda) # add robot to the world\n", "panda.q = panda.qr # update the robot\n", "backend.step() # display the world" - ], - "id": "b3d35544" + ] } ], "metadata": { - "jupytext": { - "cell_metadata_filter": "-all", - "encoding": "# -*- coding: utf-8 -*-", - "notebook_metadata_filter": "-all", - "text_representation": { - "extension": ".py", - "format_name": "light" - } - }, "kernelspec": { "display_name": "Python 3", "language": "python", "name": "python3" }, "language_info": { - "codemirror_mode": { - "name": "ipython", - "version": 3 - }, - "file_extension": ".py", - "mimetype": "text/x-python", - "name": "python", - "nbconvert_exporter": "python", - "pygments_lexer": "ipython3", - "version": "3.8.5" - }, - "varInspector": { - "cols": { - "lenName": 16, - "lenType": 16, - "lenVar": 40 - }, - "kernels_config": { - "python": { - "delete_cmd_postfix": "", - "delete_cmd_prefix": "del ", - "library": "var_list.py", - "varRefreshCmd": "print(var_dic_list())" - }, - "r": { - "delete_cmd_postfix": ") ", - "delete_cmd_prefix": "rm(", - "library": "var_list.r", - "varRefreshCmd": "cat(var_dic_list()) " - } - }, - "types_to_exclude": [ - "module", - "function", - "builtin_function_or_method", - "instance", - "_Feature" - ], - "window_display": false + "name": "python" } }, "nbformat": 4, diff --git a/docs/notebooks/icra2026.ipynb b/docs/notebooks/icra2026.ipynb index 3d0e299d5..7fa18c843 100644 --- a/docs/notebooks/icra2026.ipynb +++ b/docs/notebooks/icra2026.ipynb @@ -2,6 +2,7 @@ "cells": [ { "cell_type": "markdown", + "id": "4b9f8521", "metadata": {}, "source": [ "
\n", @@ -18,12 +19,12 @@ "(c) Peter Corke 2026\n", "\n", "Presented at ICRA 2026 as Keynote Tutorial 3 (the \"lost tutorial\")" - ], - "id": "4b9f8521" + ] }, { "cell_type": "code", "execution_count": null, + "id": "8c2c33ae", "metadata": { "jupyter": { "source_hidden": true @@ -73,8 +74,7 @@ " return colab\n", "\n", "COLAB = await ensure_installed()\n" - ], - "id": "8c2c33ae" + ] }, { "cell_type": "code", @@ -1118,16 +1118,7 @@ "name": "python3" }, "language_info": { - "codemirror_mode": { - "name": "ipython", - "version": 3 - }, - "file_extension": ".py", - "mimetype": "text/x-python", - "name": "python", - "nbconvert_exporter": "python", - "pygments_lexer": "ipython3", - "version": "3.10.16" + "name": "python" } }, "nbformat": 4, diff --git a/docs/notebooks/ik-symbolic.ipynb b/docs/notebooks/ik-symbolic.ipynb index 67c78990d..83e59f74c 100644 --- a/docs/notebooks/ik-symbolic.ipynb +++ b/docs/notebooks/ik-symbolic.ipynb @@ -2,6 +2,7 @@ "cells": [ { "cell_type": "markdown", + "id": "f20c8df0", "metadata": {}, "source": [ "
\n", @@ -16,8 +17,7 @@ "

\n", "\n", "(c) Peter Corke 2026" - ], - "id": "f20c8df0" + ] }, { "cell_type": "markdown", @@ -43,6 +43,7 @@ { "cell_type": "code", "execution_count": null, + "id": "cf4a9e24", "metadata": { "jupyter": { "source_hidden": true @@ -92,8 +93,7 @@ " return colab\n", "\n", "COLAB = await ensure_installed(extras=['ik'])\n" - ], - "id": "cf4a9e24" + ] }, { "cell_type": "code", @@ -790,21 +790,12 @@ ], "metadata": { "kernelspec": { - "display_name": "dev", + "display_name": "Python 3", "language": "python", "name": "python3" }, "language_info": { - "codemirror_mode": { - "name": "ipython", - "version": 3 - }, - "file_extension": ".py", - "mimetype": "text/x-python", - "name": "python", - "nbconvert_exporter": "python", - "pygments_lexer": "ipython3", - "version": "3.12.14" + "name": "python" }, "rtb_bootstrap": { "extras": [ diff --git a/docs/notebooks/ik_benchmark.ipynb b/docs/notebooks/ik_benchmark.ipynb index fcd43ff0a..ffe69585d 100644 --- a/docs/notebooks/ik_benchmark.ipynb +++ b/docs/notebooks/ik_benchmark.ipynb @@ -2,6 +2,7 @@ "cells": [ { "cell_type": "markdown", + "id": "d8cda822", "metadata": {}, "source": [ "
\n", @@ -16,12 +17,12 @@ "

\n", "\n", "(c) Peter Corke 2026" - ], - "id": "d8cda822" + ] }, { "cell_type": "code", "execution_count": null, + "id": "c937d041", "metadata": { "jupyter": { "source_hidden": true @@ -71,12 +72,12 @@ " return colab\n", "\n", "COLAB = await ensure_installed()\n" - ], - "id": "c937d041" + ] }, { "cell_type": "code", "execution_count": null, + "id": "81f01e8a", "metadata": {}, "outputs": [], "source": [ @@ -107,12 +108,12 @@ "import warnings\n", "\n", "warnings.filterwarnings(\"ignore\")" - ], - "id": "81f01e8a" + ] }, { "attachments": {}, "cell_type": "markdown", + "id": "08b20209", "metadata": {}, "source": [ "In this Notebook we benchmark the performance of each numerical IK solver we provide with the toolbox.\n", @@ -133,12 +134,12 @@ "\n", "[3.0 Python Class Based IK Methods](#3)\n", "\n" - ], - "id": "08b20209" + ] }, { "attachments": {}, "cell_type": "markdown", + "id": "ebf33971", "metadata": {}, "source": [ "
\n", @@ -147,24 +148,24 @@ "\n", "### 1.0 Fast IK Methods\n", "---" - ], - "id": "ebf33971" + ] }, { "cell_type": "code", "execution_count": null, + "id": "c48854fb", "metadata": {}, "outputs": [], "source": [ "# Our robot and ETS\n", "robot = rtb.models.Panda()\n", "ets = robot.ets()" - ], - "id": "c48854fb" + ] }, { "cell_type": "code", "execution_count": null, + "id": "4e755bd7", "metadata": {}, "outputs": [], "source": [ @@ -193,12 +194,12 @@ "tol = 1e-6\n", "\n", "joint_limits = False" - ], - "id": "4e755bd7" + ] }, { "cell_type": "code", "execution_count": null, + "id": "17b2b8c8", "metadata": {}, "outputs": [], "source": [ @@ -263,12 +264,12 @@ "]\n", "\n", "times = []" - ], - "id": "17b2b8c8" + ] }, { "cell_type": "code", "execution_count": null, + "id": "6a0ddecb", "metadata": {}, "outputs": [], "source": [ @@ -303,33 +304,17 @@ " )\n", "\n", "table.print()" - ], - "id": "6a0ddecb" + ] } ], "metadata": { "kernelspec": { - "display_name": "rtb", + "display_name": "Python 3", "language": "python", "name": "python3" }, "language_info": { - "codemirror_mode": { - "name": "ipython", - "version": 3 - }, - "file_extension": ".py", - "mimetype": "text/x-python", - "name": "python", - "nbconvert_exporter": "python", - "pygments_lexer": "ipython3", - "version": "3.11.0" - }, - "orig_nbformat": 4, - "vscode": { - "interpreter": { - "hash": "4e8f2602a343ae62ff483b4df7805ab0eb447ccc1a56caf0631d5620d36b21ab" - } + "name": "python" } }, "nbformat": 4, diff --git a/docs/notebooks/index.ipynb b/docs/notebooks/index.ipynb index 08419dbbd..f2fce1f46 100644 --- a/docs/notebooks/index.ipynb +++ b/docs/notebooks/index.ipynb @@ -2,6 +2,7 @@ "cells": [ { "cell_type": "markdown", + "id": "c63145b0", "metadata": {}, "source": [ "
\n", @@ -20,8 +21,7 @@ "Notebooks:\n", "\n", "- [ICRA 2026 tutorial: a tour of the Robotics Toolbox](icra2026.ipynb)\n" - ], - "id": "c63145b0" + ] } ], "metadata": { diff --git a/docs/notebooks/iros2020.ipynb b/docs/notebooks/iros2020.ipynb index f6c236c39..6443f17b3 100644 --- a/docs/notebooks/iros2020.ipynb +++ b/docs/notebooks/iros2020.ipynb @@ -2,6 +2,7 @@ "cells": [ { "cell_type": "markdown", + "id": "42f1fc26", "metadata": {}, "source": [ "
\n", @@ -18,22 +19,22 @@ "(c) Peter Corke 2020, updated 2026\n", "\n", "> **Modernised 2026-10-19** to address API drift: this notebook was written for an earlier release of the Toolbox, and has been updated so that it runs with the current one." - ], - "id": "42f1fc26" + ] }, { "cell_type": "markdown", + "id": "84252cb7", "metadata": {}, "source": [ "This notebook corresponds to my virtual tutorial at the IROS 2020 workshop on Screw Theory and Geometric Robot Dynamics.\n", "\n", "First, import some packages, and set things up just so." - ], - "id": "84252cb7" + ] }, { "cell_type": "code", "execution_count": null, + "id": "c7709dc8", "metadata": { "jupyter": { "source_hidden": true @@ -83,12 +84,12 @@ " return colab\n", "\n", "COLAB = await ensure_installed()\n" - ], - "id": "c7709dc8" + ] }, { "cell_type": "code", "execution_count": null, + "id": "caa1b535", "metadata": {}, "outputs": [], "source": [ @@ -106,201 +107,201 @@ "\n", "%matplotlib ipympl\n", "plt.close(\"all\")" - ], - "id": "caa1b535" + ] }, { "cell_type": "markdown", + "id": "d41c6dbc", "metadata": {}, "source": [ "## Creating and using a simple Denavit-Hartenberg model\n", "First we define a robot, we will choose the classical Puma robot which includes both a kinematic and dynamic model" - ], - "id": "d41c6dbc" + ] }, { "cell_type": "code", "execution_count": null, + "id": "823706e9", "metadata": {}, "outputs": [], "source": [ "puma = rtb.models.DH.Puma560()" - ], - "id": "823706e9" + ] }, { "cell_type": "markdown", + "id": "0b9b5814", "metadata": {}, "source": [ "which has created an object in the local _workspace_ which subclasses the `DHRobot` class which subclasses the top-level `Robot` class. If we print this we get some detailed information about it's kinematics as well as the base and tool transforms, and some predefined joint configurations" - ], - "id": "0b9b5814" + ] }, { "cell_type": "code", "execution_count": null, + "id": "e6532611", "metadata": {}, "outputs": [], "source": [ "print(puma)" - ], - "id": "e6532611" + ] }, { "cell_type": "markdown", + "id": "c4a979a4", "metadata": {}, "source": [ "and we can plot the robot in its zero-joint-angle configuration" - ], - "id": "c4a979a4" + ] }, { "cell_type": "code", "execution_count": null, + "id": "cfe97914", "metadata": {}, "outputs": [], "source": [ "puma.plot(puma.qn);" - ], - "id": "cfe97914" + ] }, { "cell_type": "markdown", + "id": "f1349186", "metadata": {}, "source": [ "For an arbitrary set of joint angles" - ], - "id": "f1349186" + ] }, { "cell_type": "code", "execution_count": null, + "id": "8fededc2", "metadata": {}, "outputs": [], "source": [ "q = [0.1, 0.2, 0.3, 0.4, 0.5, 0.6]" - ], - "id": "8fededc2" + ] }, { "cell_type": "markdown", + "id": "85163e8f", "metadata": {}, "source": [ "we can compute the end-effector pose using forward kinematics, taking into account the base and tool transforms" - ], - "id": "85163e8f" + ] }, { "cell_type": "code", "execution_count": null, + "id": "3b8abe37", "metadata": {}, "outputs": [], "source": [ "T = puma.fkine(q)\n", "T" - ], - "id": "3b8abe37" + ] }, { "cell_type": "markdown", + "id": "19493c65", "metadata": {}, "source": [ "the result is an SE(3) matrix, an instance of an `SE3` object. It is shown with the rotational part in red, the translational part in blue, and the constant part in grey.\n", "\n", "To find the inverse kinematics corresponding to this end-effector pose we solve the inverse kinematics numerically" - ], - "id": "19493c65" + ] }, { "cell_type": "code", "execution_count": null, + "id": "2a7c48ad", "metadata": {}, "outputs": [], "source": [ "q = puma.ikine_LM(T).q\n", "q" - ], - "id": "2a7c48ad" + ] }, { "cell_type": "markdown", + "id": "050ef302", "metadata": {}, "source": [ "_Note: that `ikine_LM` returns an `IKSolution` object, with the joint coordinates in its `q` attribute and the status of the solution in other attributes, which we are ignoring here_\n", "\n", "The manipulability at this particular configuration is" - ], - "id": "050ef302" + ] }, { "cell_type": "code", "execution_count": null, + "id": "2eea6dde", "metadata": {}, "outputs": [], "source": [ "puma.manipulability(q)" - ], - "id": "2eea6dde" + ] }, { "cell_type": "markdown", + "id": "043a9acf", "metadata": {}, "source": [ "and the Jacobian, in the world frame (indicated by the `0` suffix) is" - ], - "id": "043a9acf" + ] }, { "cell_type": "code", "execution_count": null, + "id": "7d510083", "metadata": { "scrolled": false }, "outputs": [], "source": [ "puma.plot(puma.qn, vellipse=True);" - ], - "id": "7d510083" + ] }, { "cell_type": "code", "execution_count": null, + "id": "07222729", "metadata": {}, "outputs": [], "source": [ "puma.jacob0(q)" - ], - "id": "07222729" + ] }, { "cell_type": "markdown", + "id": "8d952da9", "metadata": {}, "source": [ "the Jacobian in the end-effector frame is given by `jacobe`." - ], - "id": "8d952da9" + ] }, { "cell_type": "markdown", + "id": "087795dc", "metadata": {}, "source": [ "## More models\n", "A list of all models currently defined in the Toolbox is given by" - ], - "id": "087795dc" + ] }, { "cell_type": "code", "execution_count": null, + "id": "975e3e96", "metadata": {}, "outputs": [], "source": [ "rtb.models.list()" - ], - "id": "975e3e96" + ] }, { "cell_type": "markdown", + "id": "30d0599f", "metadata": {}, "source": [ "which highlights the three categories of robot models supported by the toolbox:\n", @@ -308,21 +309,21 @@ "1. DH - expressed using standard or modified Denavit-Hartenberg parameters, and may include dynamic parameters, meshs and collision objects\n", "2. ETS - expressed as a string of elementary transforms, does not support dynamic parameters\n", "3. URDF - based on a URDF file with some additional meta-data, and may include dynamic parameters, meshs and collision objects" - ], - "id": "30d0599f" + ] }, { "cell_type": "markdown", + "id": "8cc47510", "metadata": {}, "source": [ "### Elementary transform sequence (ETS) models\n", "We can express the Puma 560 simply and concisely as a string of elementary transforms. First we import the class and then define the key dimensions/offsets of the robot" - ], - "id": "8cc47510" + ] }, { "cell_type": "code", "execution_count": null, + "id": "7a0b425a", "metadata": {}, "outputs": [], "source": [ @@ -334,23 +335,23 @@ "l5 = 0.4318\n", "l4 = -0.0837\n", "l6 = 0.0203" - ], - "id": "7a0b425a" + ] }, { "cell_type": "markdown", + "id": "f3ef7f27", "metadata": {}, "source": [ "Consider the robot in a configuration where it is stretched out vertically, this will be our zero-joint-angle configuration. Then starting at the base we define a series of rigid-body motions all the way to the tip. The motion can be:\n", "\n", "- constant, indicated by an argument to the method, or\n", "- a variable, due to a joint, and this is indicated by no argument to the method" - ], - "id": "f3ef7f27" + ] }, { "cell_type": "code", "execution_count": null, + "id": "75f3a39a", "metadata": {}, "outputs": [], "source": [ @@ -368,470 +369,470 @@ " * ET.Ry()\n", " * ET.Rz()\n", ")" - ], - "id": "75f3a39a" + ] }, { "cell_type": "markdown", + "id": "fe827a2e", "metadata": {}, "source": [ "The overloaded `*` indicates composition of the rigid-body motions. The total number of elementary transforms is" - ], - "id": "fe827a2e" + ] }, { "cell_type": "code", "execution_count": null, + "id": "adef41af", "metadata": {}, "outputs": [], "source": [ "len(e)" - ], - "id": "adef41af" + ] }, { "cell_type": "markdown", + "id": "ed94d528", "metadata": {}, "source": [ "and the elements which are joints are" - ], - "id": "ed94d528" + ] }, { "cell_type": "code", "execution_count": null, + "id": "0d4bc8d2", "metadata": {}, "outputs": [], "source": [ "e.joints()" - ], - "id": "0d4bc8d2" + ] }, { "cell_type": "markdown", + "id": "0472fc55", "metadata": {}, "source": [ "and we can pretty print the transform sequence by" - ], - "id": "0472fc55" + ] }, { "cell_type": "code", "execution_count": null, + "id": "ba3d35dc", "metadata": {}, "outputs": [], "source": [ "print(e)" - ], - "id": "ba3d35dc" + ] }, { "cell_type": "markdown", + "id": "49ef48bb", "metadata": {}, "source": [ "We can also evaluate the transform sequence, by subsituting in values for the joint angles. They are taken from consecutive elemements of the joint-angle vector provide to the `eval` method" - ], - "id": "49ef48bb" + ] }, { "cell_type": "code", "execution_count": null, + "id": "587acb32", "metadata": {}, "outputs": [], "source": [ "e.eval([0, 0, 0, 0, 0, 0])" - ], - "id": "587acb32" + ] }, { "cell_type": "code", "execution_count": null, + "id": "7c8d984e", "metadata": {}, "outputs": [], "source": [ "puma.fkine(puma.qr)" - ], - "id": "7c8d984e" + ] }, { "cell_type": "markdown", + "id": "33adf87c", "metadata": {}, "source": [ "and this is the forward kinematics of a robot described by this elementary transform sequence." - ], - "id": "33adf87c" + ] }, { "cell_type": "markdown", + "id": "5f4f21d4", "metadata": {}, "source": [ "## Robot dynamics\n", "We can define a different joint configuration `q`, as well as the joint velocity `qd` and acceleration `qdd`" - ], - "id": "5f4f21d4" + ] }, { "cell_type": "code", "execution_count": null, + "id": "23d9a236", "metadata": {}, "outputs": [], "source": [ "q = puma.qn\n", "qd = np.zeros((6,))\n", "qdd = np.zeros((6,))" - ], - "id": "23d9a236" + ] }, { "cell_type": "markdown", + "id": "0c7272cc", "metadata": {}, "source": [ "and then compute the inverse dynamics using the recursive Newton-Euler algorithm" - ], - "id": "0c7272cc" + ] }, { "cell_type": "code", "execution_count": null, + "id": "880a2a2d", "metadata": {}, "outputs": [], "source": [ "puma.rne(q, qd, qdd)" - ], - "id": "880a2a2d" + ] }, { "cell_type": "markdown", + "id": "c3108943", "metadata": {}, "source": [ "which is written in C for performance. It's actually the same 20 year C-code used in the MATLAB version of the Toolbox!\n", "\n", "We can use this to compute the manipulator inertia matrix" - ], - "id": "c3108943" + ] }, { "cell_type": "code", "execution_count": null, + "id": "485886bc", "metadata": {}, "outputs": [], "source": [ "puma.inertia(q)" - ], - "id": "485886bc" + ] }, { "cell_type": "markdown", + "id": "780d7706", "metadata": {}, "source": [ "the velocity terms" - ], - "id": "780d7706" + ] }, { "cell_type": "code", "execution_count": null, + "id": "31a6ab42", "metadata": {}, "outputs": [], "source": [ "puma.coriolis(q, [0.1, 0.2, 0.3, 0.1, 0.2, 0.3])" - ], - "id": "31a6ab42" + ] }, { "cell_type": "markdown", + "id": "78ca8306", "metadata": {}, "source": [ "where the second argument is the joint velocuty.\n", "\n", "Finally, the gravity torque is" - ], - "id": "78ca8306" + ] }, { "cell_type": "code", "execution_count": null, + "id": "23e4794d", "metadata": {}, "outputs": [], "source": [ "puma.gravload(q)" - ], - "id": "23e4794d" + ] }, { "cell_type": "markdown", + "id": "d0a28297", "metadata": {}, "source": [ "If we set the joint torques to zero, ie. turn the motors off" - ], - "id": "d0a28297" + ] }, { "cell_type": "code", "execution_count": null, + "id": "3475f70c", "metadata": {}, "outputs": [], "source": [ "tau = np.zeros((6,))" - ], - "id": "3475f70c" + ] }, { "cell_type": "markdown", + "id": "e72fdc09", "metadata": {}, "source": [ "then the resulting joint angle acceleration will be" - ], - "id": "e72fdc09" + ] }, { "cell_type": "code", "execution_count": null, + "id": "2ee705dd", "metadata": {}, "outputs": [], "source": [ "puma.accel(q, qd, tau)" - ], - "id": "2ee705dd" + ] }, { "cell_type": "markdown", + "id": "fb0f1a31", "metadata": {}, "source": [ "which we can integrate over time to obtain a joint angle trajectory" - ], - "id": "fb0f1a31" + ] }, { "cell_type": "code", "execution_count": null, + "id": "666f40b2", "metadata": {}, "outputs": [], "source": [ "tg = puma.nofriction().fdyn(5, puma.qn, dt=0.05)\n", "rtb.tools.plot.xplot(tg.t, tg.q)" - ], - "id": "666f40b2" + ] }, { "cell_type": "markdown", + "id": "964e3465", "metadata": {}, "source": [ "Note: _Coulomb friction is a harsh non-linearity and it causes the numerical integrator to take very small times steps, so the result will take many minutes to compute. To speed things up, at the expense of some modeling fidelity, we set the Coulomb friction to zero, but retain the viscous friction. The `nofriction()` method returns a clone of the robot with its friction parameters modified._" - ], - "id": "964e3465" + ] }, { "cell_type": "markdown", + "id": "8bb71d63", "metadata": {}, "source": [ "# Twists and screws\n", "\n", "## Starting simply\n", "The Toolbox provides functions to convert to and from vectors to skew-symmetric matrices (elements of so(2), so(3)) and augmented skew-symmetric matrices (elements of se(2), se(3))" - ], - "id": "8bb71d63" + ] }, { "cell_type": "code", "execution_count": null, + "id": "1af83003", "metadata": {}, "outputs": [], "source": [ "S = skew([1, 2, 3])\n", "S" - ], - "id": "1af83003" + ] }, { "cell_type": "code", "execution_count": null, + "id": "95d63981", "metadata": {}, "outputs": [], "source": [ "vex(S)" - ], - "id": "95d63981" + ] }, { "cell_type": "code", "execution_count": null, + "id": "dca10a9e", "metadata": {}, "outputs": [], "source": [ "S = skewa([1, 2, 3, 4, 5, 6])\n", "S" - ], - "id": "dca10a9e" + ] }, { "cell_type": "code", "execution_count": null, + "id": "76017aec", "metadata": {}, "outputs": [], "source": [ "vexa(S)" - ], - "id": "76017aec" + ] }, { "cell_type": "markdown", + "id": "264adf0b", "metadata": {}, "source": [ "## 3D twists\n", "We can create a revolute twist by specifying the direction of its line of action and a point" - ], - "id": "264adf0b" + ] }, { "cell_type": "code", "execution_count": null, + "id": "420331bf", "metadata": {}, "outputs": [], "source": [ "Twist3.UnitRevolute([1, 0, 0], [0, 0, 0])" - ], - "id": "420331bf" + ] }, { "cell_type": "markdown", + "id": "02b7149e", "metadata": {}, "source": [ " which is a 6-vector encapsulated in an instance of a `Twist3` object. A prismatic twist has just the direction of its line of action" - ], - "id": "02b7149e" + ] }, { "cell_type": "code", "execution_count": null, + "id": "455a5bc6", "metadata": {}, "outputs": [], "source": [ "Twist3.UnitPrismatic([0, 1, 0])" - ], - "id": "455a5bc6" + ] }, { "cell_type": "markdown", + "id": "d6302fb0", "metadata": {}, "source": [ "We can convert an SE(3) matrix, encapsulated in an instance of an `SE3` object to a 3D twist" - ], - "id": "d6302fb0" + ] }, { "cell_type": "code", "execution_count": null, + "id": "6cca3eb4", "metadata": {}, "outputs": [], "source": [ "T = SE3.Rand()\n", "tw = Twist3(T)\n", "tw" - ], - "id": "6cca3eb4" + ] }, { "cell_type": "markdown", + "id": "d28e730e", "metadata": {}, "source": [ "Twists have a number of properties and methods. The moment is a 3-vector" - ], - "id": "d28e730e" + ] }, { "cell_type": "code", "execution_count": null, + "id": "0d8bf900", "metadata": {}, "outputs": [], "source": [ "tw.v" - ], - "id": "0d8bf900" + ] }, { "cell_type": "markdown", + "id": "e21d9b81", "metadata": {}, "source": [ "as is the direction part" - ], - "id": "e21d9b81" + ] }, { "cell_type": "code", "execution_count": null, + "id": "e333a7b5", "metadata": {}, "outputs": [], "source": [ "tw.w" - ], - "id": "e333a7b5" + ] }, { "cell_type": "markdown", + "id": "e6a42e38", "metadata": {}, "source": [ "A twist also has a pitch" - ], - "id": "e6a42e38" + ] }, { "cell_type": "code", "execution_count": null, + "id": "47de6e4b", "metadata": {}, "outputs": [], "source": [ "tw.pitch" - ], - "id": "47de6e4b" + ] }, { "cell_type": "markdown", + "id": "3bfc9a07", "metadata": {}, "source": [ "and a pole" - ], - "id": "3bfc9a07" + ] }, { "cell_type": "code", "execution_count": null, + "id": "974c6771", "metadata": {}, "outputs": [], "source": [ "tw.pole" - ], - "id": "974c6771" + ] }, { "cell_type": "markdown", + "id": "08449d6d", "metadata": {}, "source": [ "The line of action of the twist is a line in 3D space which can be represented by a Toolbox Plucker object" - ], - "id": "08449d6d" + ] }, { "cell_type": "code", "execution_count": null, + "id": "39f22934", "metadata": {}, "outputs": [], "source": [ "tw.line()" - ], - "id": "39f22934" + ] }, { "cell_type": "markdown", + "id": "28b6c645", "metadata": {}, "source": [ "which we could plot, by first defining a plot volume" - ], - "id": "28b6c645" + ] }, { "cell_type": "code", "execution_count": null, + "id": "f3f444b3", "metadata": {}, "outputs": [], "source": [ @@ -841,334 +842,287 @@ "ax.set_zlim3d(-10, 10)\n", "\n", "tw.line().plot(color=\"red\", linewidth=2)" - ], - "id": "f3f444b3" + ] }, { "cell_type": "markdown", + "id": "e322fbb0", "metadata": {}, "source": [ "## Exponentiation of twists\n", "Importantly, we can exponeniate the twist, ie. compute $T = e^{[S]\\theta}$, where $\\theta$ is the argument" - ], - "id": "e322fbb0" + ] }, { "cell_type": "code", "execution_count": null, + "id": "5b728c95", "metadata": {}, "outputs": [], "source": [ "tw.exp(0)" - ], - "id": "5b728c95" + ] }, { "cell_type": "markdown", + "id": "0e73d2eb", "metadata": {}, "source": [ "which is of course the identity matrix, and" - ], - "id": "0e73d2eb" + ] }, { "cell_type": "code", "execution_count": null, + "id": "1e19870e", "metadata": {}, "outputs": [], "source": [ "tw.exp(1)" - ], - "id": "1e19870e" + ] }, { "cell_type": "markdown", + "id": "cf13c21e", "metadata": {}, "source": [ "All Toolbox objects can contain multiple values, so in this example, we exponentiate the twist with a sequence of values, resulting in a single `SE3` object that contains a sequence of SE(3) values." - ], - "id": "cf13c21e" + ] }, { "cell_type": "code", "execution_count": null, + "id": "3b039f93", "metadata": {}, "outputs": [], "source": [ "T = tw.exp([0, 1])\n", "T" - ], - "id": "3b039f93" + ] }, { "cell_type": "markdown", + "id": "e24981ab", "metadata": {}, "source": [ "and we access them in a familiar Pythonic way" - ], - "id": "e24981ab" + ] }, { "cell_type": "code", "execution_count": null, + "id": "2465a8a7", "metadata": {}, "outputs": [], "source": [ "len(T)" - ], - "id": "2465a8a7" + ] }, { "cell_type": "code", "execution_count": null, + "id": "ef1a9fd2", "metadata": {}, "outputs": [], "source": [ "T[1]" - ], - "id": "ef1a9fd2" + ] }, { "cell_type": "markdown", + "id": "129bd6bd", "metadata": {}, "source": [ "## Product of exponentials" - ], - "id": "129bd6bd" + ] }, { "cell_type": "markdown", + "id": "4b762a6d", "metadata": {}, "source": [ "For the robot model we created earlier, we can extract its product of exponentials representation, a set of six twists and a transformation" - ], - "id": "4b762a6d" + ] }, { "cell_type": "code", "execution_count": null, + "id": "f0806c59", "metadata": {}, "outputs": [], "source": [ "S, T0 = puma.twists()" - ], - "id": "f0806c59" + ] }, { "cell_type": "markdown", + "id": "fa4b8a6d", "metadata": {}, "source": [ "where the transformation for zero-joint-angles is" - ], - "id": "fa4b8a6d" + ] }, { "cell_type": "code", "execution_count": null, + "id": "1fe17d94", "metadata": {}, "outputs": [], "source": [ "T0" - ], - "id": "1fe17d94" + ] }, { "cell_type": "markdown", + "id": "053f469e", "metadata": {}, "source": [ "and the twists are" - ], - "id": "053f469e" + ] }, { "cell_type": "code", "execution_count": null, + "id": "4b17d142", "metadata": {}, "outputs": [], "source": [ "S" - ], - "id": "4b17d142" + ] }, { "cell_type": "markdown", + "id": "46012989", "metadata": {}, "source": [ "For a nominal joint-angle configuration" - ], - "id": "46012989" + ] }, { "cell_type": "code", "execution_count": null, + "id": "f2ff1f05", "metadata": {}, "outputs": [], "source": [ "puma.qn" - ], - "id": "f2ff1f05" + ] }, { "cell_type": "markdown", + "id": "00f9a9c8", "metadata": {}, "source": [ "we can exponentiate the twists" - ], - "id": "00f9a9c8" + ] }, { "cell_type": "code", "execution_count": null, + "id": "aa15acca", "metadata": {}, "outputs": [], "source": [ "S.exp(puma.qn)" - ], - "id": "aa15acca" + ] }, { "cell_type": "markdown", + "id": "3e37280e", "metadata": {}, "source": [ "where the result is again an `SE3` object with multiple values, six in this case.\n", "\n", "The full expression for the forward kinematics is" - ], - "id": "3e37280e" + ] }, { "cell_type": "code", "execution_count": null, + "id": "b738bfac", "metadata": {}, "outputs": [], "source": [ "S.exp(puma.qn).prod() * T0" - ], - "id": "b738bfac" + ] }, { "cell_type": "markdown", + "id": "67a0d5f2", "metadata": {}, "source": [ "which of course is identical to the classical approach " - ], - "id": "67a0d5f2" + ] }, { "cell_type": "code", "execution_count": null, + "id": "07a8cca5", "metadata": {}, "outputs": [], "source": [ "puma.fkine(puma.qn)" - ], - "id": "07a8cca5" + ] }, { "cell_type": "markdown", + "id": "de069bd8", "metadata": {}, "source": [ "Extending an earlier example, the lines of actions of the twists are" - ], - "id": "de069bd8" + ] }, { "cell_type": "code", "execution_count": null, + "id": "05725261", "metadata": {}, "outputs": [], "source": [ "lines = S.line()\n", "lines" - ], - "id": "05725261" + ] }, { "cell_type": "markdown", + "id": "39324a5f", "metadata": {}, "source": [ "Which we can overlay on a plot of the robot in its zero-angle configuration" - ], - "id": "39324a5f" + ] }, { "cell_type": "code", "execution_count": null, + "id": "25fd9ebd", "metadata": { "scrolled": true }, "outputs": [], "source": [ "fig = puma.plot(puma.qz)" - ], - "id": "25fd9ebd" + ] }, { "cell_type": "code", "execution_count": null, + "id": "894bd19e", "metadata": {}, "outputs": [], "source": [ "fig = puma.plot(puma.qz)\n", "lines.plot(\"k--\", linewidth=1.5, bounds=np.r_[-1, 1, -1, 1, -1, 1.5])" - ], - "id": "894bd19e" + ] } ], "metadata": { - "anaconda-cloud": {}, "kernelspec": { "display_name": "Python 3", "language": "python", "name": "python3" }, "language_info": { - "codemirror_mode": { - "name": "ipython", - "version": 3 - }, - "file_extension": ".py", - "mimetype": "text/x-python", - "name": "python", - "nbconvert_exporter": "python", - "pygments_lexer": "ipython3", - "version": "3.8.5" - }, - "varInspector": { - "cols": { - "lenName": 16, - "lenType": 16, - "lenVar": 40 - }, - "kernels_config": { - "python": { - "delete_cmd_postfix": "", - "delete_cmd_prefix": "del ", - "library": "var_list.py", - "varRefreshCmd": "print(var_dic_list())" - }, - "r": { - "delete_cmd_postfix": ") ", - "delete_cmd_prefix": "rm(", - "library": "var_list.r", - "varRefreshCmd": "cat(var_dic_list()) " - } - }, - "position": { - "height": "144px", - "left": "854px", - "right": "20px", - "top": "99px", - "width": "350px" - }, - "types_to_exclude": [ - "module", - "function", - "builtin_function_or_method", - "instance", - "_Feature" - ], - "window_display": false + "name": "python" } }, "nbformat": 4, diff --git a/docs/notebooks/kinematics.ipynb b/docs/notebooks/kinematics.ipynb index 476d3f275..7b59c38b1 100644 --- a/docs/notebooks/kinematics.ipynb +++ b/docs/notebooks/kinematics.ipynb @@ -2,6 +2,7 @@ "cells": [ { "cell_type": "markdown", + "id": "4eb2352d", "metadata": {}, "source": [ "
\n", @@ -18,12 +19,12 @@ "(c) Peter Corke 2026\n", "\n", "Forward and inverse kinematics, a joint-space trajectory and the Jacobian of a Puma 560 robot." - ], - "id": "4eb2352d" + ] }, { "cell_type": "code", "execution_count": null, + "id": "d7d8472b", "metadata": { "jupyter": { "source_hidden": true @@ -73,12 +74,12 @@ " return colab\n", "\n", "COLAB = await ensure_installed()\n" - ], - "id": "d7d8472b" + ] }, { "cell_type": "code", "execution_count": null, + "id": "3f97f1fa", "metadata": {}, "outputs": [], "source": [ @@ -96,118 +97,118 @@ "\n", "%matplotlib ipympl\n", "plt.close(\"all\")" - ], - "id": "3f97f1fa" + ] }, { "cell_type": "markdown", + "id": "ef971274", "metadata": {}, "source": [ "The Toolbox supports models defined using a number of different conventions. We will load a very classical model, a Puma560 robot defined in terms of standard Denavit-Hartenberg parameters" - ], - "id": "ef971274" + ] }, { "cell_type": "code", "execution_count": null, + "id": "acacd545", "metadata": {}, "outputs": [], "source": [ "p560 = rtb.models.DH.Puma560()" - ], - "id": "acacd545" + ] }, { "cell_type": "markdown", + "id": "329070d5", "metadata": {}, "source": [ "Now we can display the simple Denavit-Hartenberg parameter model" - ], - "id": "329070d5" + ] }, { "cell_type": "code", "execution_count": null, + "id": "47e6f7f0", "metadata": {}, "outputs": [], "source": [ "print(p560)" - ], - "id": "47e6f7f0" + ] }, { "cell_type": "markdown", + "id": "bdf3fbed", "metadata": {}, "source": [ "The first table shows the kinematic parameters, and from the column titles we can see clearly that this is expressed in terms of standard Denavit-Hartenberg parameters. The first column shows that the joint variables qi are rotations since they are in the θ column. Joint limits are also shown. Joint flip (motion in the opposite sense) would be indicated by the joint variable being shown as for example like `-q3`, and joint offsets by being shown as for example like `q2 + 45°`.\n", "\n", "The second table shows some named joint configurations. For example `p560.qr` is " - ], - "id": "bdf3fbed" + ] }, { "cell_type": "code", "execution_count": null, + "id": "bfd5ed4b", "metadata": {}, "outputs": [], "source": [ "p560.qr" - ], - "id": "bfd5ed4b" + ] }, { "cell_type": "markdown", + "id": "c021e3fa", "metadata": {}, "source": [ "If the robot had a base or tool transform they would be listed in this table also.\n", "\n", "This object is a subclass of `DHRobot`, equivalent to the `SerialLink` class in the MATLAB version of the Toolbox.\n", "This class has many methods and attributes, and we will explore some of them in this notebook." - ], - "id": "c021e3fa" + ] }, { "cell_type": "markdown", + "id": "953eb52b", "metadata": {}, "source": [ "We can easily display the robot graphically" - ], - "id": "953eb52b" + ] }, { "cell_type": "code", "execution_count": null, + "id": "5bb698bb", "metadata": { "scrolled": false }, "outputs": [], "source": [ "p560.plot(p560.qn);" - ], - "id": "5bb698bb" + ] }, { "cell_type": "markdown", + "id": "dd3a1595", "metadata": {}, "source": [ "where `qn` is one of the named configurations shown above, and has the robot positioned to work above a table top. You can use the mouse to rotate the plot and view the robot from different directions. The grey line is the _shadow_ which is a projection of the robot onto the xy-plane.\n", "\n", "In this particular case the end-effector pose is given by the forward kinematics" - ], - "id": "dd3a1595" + ] }, { "cell_type": "code", "execution_count": null, + "id": "fccd07f1", "metadata": {}, "outputs": [], "source": [ "p560.fkine(p560.qn)" - ], - "id": "fccd07f1" + ] }, { "cell_type": "markdown", + "id": "64933906", "metadata": {}, "source": [ "which is a 4x4 SE(3) matrix displayed in a color coded way with rotation matrix in red, translation vector in blue, and constant elements in grey. This is an instance of an `SE3` object safely encapsulates the SE(3) matrix. This class, and related ones, are implemented by the [Spatial Math Toolbox for Python](https://github.com/petercorke/spatialmath-python).\n", @@ -215,23 +216,23 @@ "You can verify the end-effector position, the blue numbers are from top to bottom the x-, y- and z-coordinates of the end-effector position, match the plot shown above.\n", "\n", "We can manually adjust the joint angles of this robot (click and drag the sliders) to see how the shape of the robot changes and how the end-effector pose changes" - ], - "id": "64933906" + ] }, { "cell_type": "code", "execution_count": null, + "id": "874cb125", "metadata": { "scrolled": false }, "outputs": [], "source": [ "# p560.teach(); # works from console, hangs in Jupyter" - ], - "id": "874cb125" + ] }, { "cell_type": "markdown", + "id": "2e177824", "metadata": {}, "source": [ "An important problem in robotics is _inverse kinematics_, determining the joint angles to put the robot's end effector at a particular pose.\n", @@ -242,186 +243,186 @@ "\n", "1. a pure translation\n", "2. a pure rotation defined in terms of the orientation and approach vectors" - ], - "id": "2e177824" + ] }, { "cell_type": "code", "execution_count": null, + "id": "2f2dd38c", "metadata": {}, "outputs": [], "source": [ "T = SE3(0.5, 0.2, 0.5) * SE3.OA([0, 0, 1], [1, 0, 0])\n", "T" - ], - "id": "2f2dd38c" + ] }, { "cell_type": "markdown", + "id": "9ad7d122", "metadata": {}, "source": [ "Now we can compute the joint angles that results in this pose" - ], - "id": "9ad7d122" + ] }, { "cell_type": "code", "execution_count": null, + "id": "44561647", "metadata": {}, "outputs": [], "source": [ "sol = p560.ikine_LM(T)" - ], - "id": "44561647" + ] }, { "cell_type": "markdown", + "id": "af5040f8", "metadata": {}, "source": [ "which returns the joint coordinates as well as solution status" - ], - "id": "af5040f8" + ] }, { "cell_type": "code", "execution_count": null, + "id": "3439b7da", "metadata": {}, "outputs": [], "source": [ "sol" - ], - "id": "3439b7da" + ] }, { "cell_type": "markdown", + "id": "78f05e6a", "metadata": {}, "source": [ "indicating, in this case, that there is no failure. The joint coordinates are" - ], - "id": "78f05e6a" + ] }, { "cell_type": "code", "execution_count": null, + "id": "dcde0ff3", "metadata": {}, "outputs": [], "source": [ "sol.q" - ], - "id": "dcde0ff3" + ] }, { "cell_type": "markdown", + "id": "e650dd68", "metadata": {}, "source": [ "and we can confirm that this is indeed an inverse kinematic solution by computing the forward kinematics" - ], - "id": "e650dd68" + ] }, { "cell_type": "code", "execution_count": null, + "id": "ef04ecf0", "metadata": {}, "outputs": [], "source": [ "p560.fkine(sol.q)" - ], - "id": "ef04ecf0" + ] }, { "cell_type": "markdown", + "id": "851b1a98", "metadata": {}, "source": [ "which matches the original transform." - ], - "id": "851b1a98" + ] }, { "cell_type": "markdown", + "id": "3374002f", "metadata": {}, "source": [ "A simple trajectory between two joint configuration is" - ], - "id": "3374002f" + ] }, { "cell_type": "code", "execution_count": null, + "id": "b017a430", "metadata": {}, "outputs": [], "source": [ "qt = rtb.tools.trajectory.jtraj(p560.qz, sol.q, 50)" - ], - "id": "b017a430" + ] }, { "cell_type": "markdown", + "id": "d503f249", "metadata": {}, "source": [ "The result is a _namedtuple_ with attributes `q` containing the joint angles, as well as `qd`, `qdd` and `t` which hold the joint velocity, joint accelerations and time respectively. \n", "\n", "The joint angles are a matrix with one column per joint and one row per timestep, and time increasing with row number." - ], - "id": "d503f249" + ] }, { "cell_type": "code", "execution_count": null, + "id": "8ace240a", "metadata": { "scrolled": true }, "outputs": [], "source": [ "qt.q" - ], - "id": "8ace240a" + ] }, { "cell_type": "markdown", + "id": "72b2d59e", "metadata": {}, "source": [ "We can plot this trajectory as a function of time using the convenience method `qplot`" - ], - "id": "72b2d59e" + ] }, { "cell_type": "code", "execution_count": null, + "id": "634d1126", "metadata": {}, "outputs": [], "source": [ "qt.qplot()" - ], - "id": "634d1126" + ] }, { "cell_type": "markdown", + "id": "3b54aa99", "metadata": {}, "source": [ "and then we can animate this" - ], - "id": "3b54aa99" + ] }, { "cell_type": "code", "execution_count": null, + "id": "e18004d7", "metadata": {}, "outputs": [], "source": [ "p560.plot(qt.q, dt=0.1);" - ], - "id": "e18004d7" + ] }, { "cell_type": "markdown", + "id": "adc7bfe2", "metadata": {}, "source": [ "_Note: animation not working in Jupyter..._" - ], - "id": "adc7bfe2" + ] }, { "cell_type": "markdown", + "id": "a05c484c", "metadata": {}, "source": [ "The inverse kinematic solution was found using an iterative numerical procedure. It is quite general but it has several drawbacks:\n", @@ -434,102 +435,101 @@ " 180° about the gripper axis leaves the fingers in the same configuration.\n", "\n", "Most industrial robots have a _spherical wrist_ which means that the last three joint axes intersect at a single point in the middle of the wrist mechanism. We can test for this condition" - ], - "id": "a05c484c" + ] }, { "cell_type": "code", "execution_count": null, + "id": "d7b4b125", "metadata": {}, "outputs": [], "source": [ "p560.isspherical()" - ], - "id": "d7b4b125" + ] }, { "cell_type": "markdown", + "id": "cd7b6def", "metadata": {}, "source": [ "This greatly simplifies things because the last three joints only control orientation and have no effect on the end-effector position. This means that only the first three joints define position $(x_e, y_e, z_e)$. Three joints that control three unknowns is relatively easy to solve for, and analytical solutions (complex trigonmetric equations) can be found, and in fact have been published for most industrial robot manipulators.\n", "\n", "The Puma560 has an analytical solution. We can request the solution with the arm to the left and the elbow up, and the wrist not flipped by using the configuration string `\"lun\"`\n" - ], - "id": "cd7b6def" + ] }, { "cell_type": "code", "execution_count": null, + "id": "ee3adf9e", "metadata": {}, "outputs": [], "source": [ "sol = p560.ikine_a(T, \"lun\")\n", "sol" - ], - "id": "ee3adf9e" + ] }, { "cell_type": "markdown", + "id": "dcbb23c1", "metadata": {}, "source": [ "which is different to the values found earlier, but we can verify it is a valid solution" - ], - "id": "dcbb23c1" + ] }, { "cell_type": "code", "execution_count": null, + "id": "7f989d9c", "metadata": {}, "outputs": [], "source": [ "p560.fkine(sol.q)" - ], - "id": "7f989d9c" + ] }, { "cell_type": "markdown", + "id": "064fb467", "metadata": {}, "source": [ "In fact the solution we found earlier, but didn't explicitly specify, is the right-handed elbow-up configuration" - ], - "id": "064fb467" + ] }, { "cell_type": "code", "execution_count": null, + "id": "dc237281", "metadata": {}, "outputs": [], "source": [ "sol = p560.ikine_a(T, \"run\")\n", "sol.q" - ], - "id": "dc237281" + ] }, { "cell_type": "markdown", + "id": "4d65d29d", "metadata": {}, "source": [ "Other useful functions include the manipulator Jacobian which maps joint velocity to end-effector velocity expressed in the world frame" - ], - "id": "4d65d29d" + ] }, { "cell_type": "code", "execution_count": null, + "id": "0952d140", "metadata": {}, "outputs": [], "source": [ "p560.jacob0(p560.qn)" - ], - "id": "0952d140" + ] }, { "cell_type": "code", "execution_count": null, + "id": "1d5bea0e", "metadata": {}, "outputs": [], - "source": [], - "id": "1d5bea0e" + "source": [] } ], "metadata": { @@ -539,58 +539,7 @@ "name": "python3" }, "language_info": { - "codemirror_mode": { - "name": "ipython", - "version": 3 - }, - "file_extension": ".py", - "mimetype": "text/x-python", - "name": "python", - "nbconvert_exporter": "python", - "pygments_lexer": "ipython3", - "version": "3.8.5" - }, - "toc": { - "base_numbering": 1, - "nav_menu": {}, - "number_sections": true, - "sideBar": true, - "skip_h1_title": false, - "title_cell": "Table of Contents", - "title_sidebar": "Contents", - "toc_cell": false, - "toc_position": {}, - "toc_section_display": true, - "toc_window_display": false - }, - "varInspector": { - "cols": { - "lenName": 16, - "lenType": 16, - "lenVar": 40 - }, - "kernels_config": { - "python": { - "delete_cmd_postfix": "", - "delete_cmd_prefix": "del ", - "library": "var_list.py", - "varRefreshCmd": "print(var_dic_list())" - }, - "r": { - "delete_cmd_postfix": ") ", - "delete_cmd_prefix": "rm(", - "library": "var_list.r", - "varRefreshCmd": "cat(var_dic_list()) " - } - }, - "types_to_exclude": [ - "module", - "function", - "builtin_function_or_method", - "instance", - "_Feature" - ], - "window_display": false + "name": "python" } }, "nbformat": 4, diff --git a/docs/notebooks/symbolic.ipynb b/docs/notebooks/symbolic.ipynb index c008ca4d3..b509a5a47 100644 --- a/docs/notebooks/symbolic.ipynb +++ b/docs/notebooks/symbolic.ipynb @@ -2,6 +2,7 @@ "cells": [ { "cell_type": "markdown", + "id": "0e39b490", "metadata": {}, "source": [ "
\n", @@ -20,12 +21,12 @@ "Forward kinematics and inverse dynamics of a Puma 560 robot with symbolic joint variables, and generating code from the results. The dynamics section takes a few minutes to run in full.\n", "\n", "> **Modernised 2026-10-19** to address API drift: this notebook was written for an earlier release of the Toolbox, and has been updated so that it runs with the current one." - ], - "id": "0e39b490" + ] }, { "cell_type": "code", "execution_count": null, + "id": "8fb6aeb6", "metadata": { "jupyter": { "source_hidden": true @@ -75,12 +76,12 @@ " return colab\n", "\n", "COLAB = await ensure_installed(extras=['ik'])\n" - ], - "id": "8fb6aeb6" + ] }, { "cell_type": "code", "execution_count": null, + "id": "3c085bb6", "metadata": {}, "outputs": [], "source": [ @@ -92,393 +93,393 @@ "import spatialmath.base.symbolic as sym\n", "\n", "%matplotlib ipympl" - ], - "id": "3c085bb6" + ] }, { "cell_type": "markdown", + "id": "70cda885", "metadata": {}, "source": [ "We use the Spatial Math Toolbox wrapper of SymPy which is `spatialmath.base.symbolic` which is imported above.\n", "\n", "## Creating a symbolic variable\n", "We will first create a symbolic variable" - ], - "id": "70cda885" + ] }, { "cell_type": "code", "execution_count": null, + "id": "1a5df959", "metadata": {}, "outputs": [], "source": [ "theta = sym.symbol('theta')" - ], - "id": "1a5df959" + ] }, { "cell_type": "code", "execution_count": null, + "id": "51ddfd57", "metadata": {}, "outputs": [], "source": [ "theta" - ], - "id": "51ddfd57" + ] }, { "cell_type": "markdown", + "id": "1c08445e", "metadata": {}, "source": [ "which displays as a Greek letter, it has the type" - ], - "id": "1c08445e" + ] }, { "cell_type": "code", "execution_count": null, + "id": "130a18ae", "metadata": {}, "outputs": [], "source": [ "type(theta)" - ], - "id": "130a18ae" + ] }, { "cell_type": "markdown", + "id": "f46e3050", "metadata": {}, "source": [ "The function specifies that the symbolic variables are real valued by default, which will simplify subsequent simplification steps" - ], - "id": "f46e3050" + ] }, { "cell_type": "code", "execution_count": null, + "id": "c1f363bf", "metadata": {}, "outputs": [], "source": [ "theta.is_real" - ], - "id": "c1f363bf" + ] }, { "cell_type": "markdown", + "id": "7d03e967", "metadata": {}, "source": [ "We can test if a variable is symbolic" - ], - "id": "7d03e967" + ] }, { "cell_type": "code", "execution_count": null, + "id": "c19eb5ac", "metadata": {}, "outputs": [], "source": [ "sym.issymbol(theta)" - ], - "id": "c19eb5ac" + ] }, { "cell_type": "code", "execution_count": null, + "id": "b4aa94d7", "metadata": {}, "outputs": [], "source": [ "sym.issymbol(3.7)" - ], - "id": "b4aa94d7" + ] }, { "cell_type": "markdown", + "id": "5016dfa9", "metadata": {}, "source": [ "## Symbolics with the Spatial Math Toolbox\n", "Many Spatial Toolbox functions handle symbolic variables" - ], - "id": "5016dfa9" + ] }, { "cell_type": "code", "execution_count": null, + "id": "e612eba7", "metadata": {}, "outputs": [], "source": [ "R = rot2(theta)\n", "R" - ], - "id": "e612eba7" + ] }, { "cell_type": "markdown", + "id": "f34c6ea9", "metadata": {}, "source": [ "and return a NumPy array with symbolic values. \n", "\n", "The 3D case is similar" - ], - "id": "f34c6ea9" + ] }, { "cell_type": "code", "execution_count": null, + "id": "f9e71c5f", "metadata": {}, "outputs": [], "source": [ "R = rotx(theta)\n", "R" - ], - "id": "f9e71c5f" + ] }, { "cell_type": "code", "execution_count": null, + "id": "be3f7dff", "metadata": {}, "outputs": [], "source": [ "T = trotx(theta)\n", "T" - ], - "id": "be3f7dff" + ] }, { "cell_type": "markdown", + "id": "3e169bc8", "metadata": {}, "source": [ "The elements of this NumPy array are all objects, as indicated by the `dtype`. However when we index the elements, they will be converted back to numeric types if possible" - ], - "id": "3e169bc8" + ] }, { "cell_type": "code", "execution_count": null, + "id": "a7caa2fd", "metadata": {}, "outputs": [], "source": [ "type(T[0,0])" - ], - "id": "a7caa2fd" + ] }, { "cell_type": "code", "execution_count": null, + "id": "a749c06d", "metadata": {}, "outputs": [], "source": [ "type(T[1,1])" - ], - "id": "a749c06d" + ] }, { "cell_type": "markdown", + "id": "3df2affa", "metadata": {}, "source": [ "We can perform arithmetic on such matrices, for example" - ], - "id": "3df2affa" + ] }, { "cell_type": "code", "execution_count": null, + "id": "b0ec7923", "metadata": {}, "outputs": [], "source": [ "T @ T" - ], - "id": "b0ec7923" + ] }, { "cell_type": "markdown", + "id": "d2567a90", "metadata": {}, "source": [ "## Pose classes\n", "The symbolic capability extends to the pose classes" - ], - "id": "d2567a90" + ] }, { "cell_type": "code", "execution_count": null, + "id": "2707cd5c", "metadata": {}, "outputs": [], "source": [ "T = SE3.Rx(theta)\n", "T" - ], - "id": "2707cd5c" + ] }, { "cell_type": "code", "execution_count": null, + "id": "a5ce3099", "metadata": {}, "outputs": [], "source": [ "T2 = T * T\n", "T2" - ], - "id": "a5ce3099" + ] }, { "cell_type": "markdown", + "id": "4a06bc35", "metadata": {}, "source": [ "but the colored layout is problematic." - ], - "id": "4a06bc35" + ] }, { "cell_type": "markdown", + "id": "bb8a15e0", "metadata": {}, "source": [ "## Robot forward kinematics" - ], - "id": "bb8a15e0" + ] }, { "cell_type": "markdown", + "id": "e18a7cd2", "metadata": {}, "source": [ "We will create a symbolic version of the robot model" - ], - "id": "e18a7cd2" + ] }, { "cell_type": "code", "execution_count": null, + "id": "9408a564", "metadata": {}, "outputs": [], "source": [ "puma = rtb.models.DH.Puma560(symbolic=True)" - ], - "id": "9408a564" + ] }, { "cell_type": "code", "execution_count": null, + "id": "c9759f40", "metadata": {}, "outputs": [], "source": [ "print(puma)" - ], - "id": "c9759f40" + ] }, { "cell_type": "markdown", + "id": "ffa2df67", "metadata": {}, "source": [ "We see that the $\\alpha_j$ values are now given in radians and are colored red. This means the value is a symbolic expression, for the first link it is $\\pi/2$ which is a precise value, compared to the non-symbolic case which is a floating point number 1.5707963267948966 that only approximates $\\pi/2$." - ], - "id": "ffa2df67" + ] }, { "cell_type": "markdown", + "id": "7cb9952d", "metadata": {}, "source": [ "The next thing we need to do is create a vector of joint coordinates which are all symbols" - ], - "id": "7cb9952d" + ] }, { "cell_type": "code", "execution_count": null, + "id": "61352f75", "metadata": {}, "outputs": [], "source": [ "q = sym.symbol('q_:6')\n", "q" - ], - "id": "61352f75" + ] }, { "cell_type": "markdown", + "id": "e2349534", "metadata": {}, "source": [ "We use the underscore, because the value of the symbol is pretty printed by SymPy as a subscript (just as it is with LaTeX math notation)" - ], - "id": "e2349534" + ] }, { "cell_type": "code", "execution_count": null, + "id": "86f820c2", "metadata": {}, "outputs": [], "source": [ "q[0]" - ], - "id": "86f820c2" + ] }, { "cell_type": "markdown", + "id": "55f7574e", "metadata": {}, "source": [ "We are now set to compute the forward kinematics which will be a matrix whose elements will be complicated expressions of symbolic joint variables and kinematic parameters" - ], - "id": "55f7574e" + ] }, { "cell_type": "code", "execution_count": null, + "id": "d33f041b", "metadata": {}, "outputs": [], "source": [ "T = puma.fkine(q)\n", "T" - ], - "id": "d33f041b" + ] }, { "cell_type": "markdown", + "id": "2222d8f6", "metadata": {}, "source": [ "The color coding helps us identify the rotational and translational parts, but the format is not very readable. We can display the underlying NumPy array" - ], - "id": "2222d8f6" + ] }, { "cell_type": "code", "execution_count": null, + "id": "19ff6ef3", "metadata": {}, "outputs": [], "source": [ "T.A" - ], - "id": "19ff6ef3" + ] }, { "cell_type": "markdown", + "id": "88827405", "metadata": {}, "source": [ "which is not a lot better.\n", "\n", "As this stage it is far better to convert the result to a SymPy matrix" - ], - "id": "88827405" + ] }, { "cell_type": "code", "execution_count": null, + "id": "5a8f1894", "metadata": {}, "outputs": [], "source": [ "from sympy import Matrix\n", "Matrix(T.A)" - ], - "id": "5a8f1894" + ] }, { "cell_type": "markdown", + "id": "ab43bcd3", "metadata": {}, "source": [ "which is decently pretty printed by SymPy.\n", "\n", "Often after a round of symbolic calculations there are simplifications that can be achieved. We can symbolically simplify each element of the `SE3` object by" - ], - "id": "ab43bcd3" + ] }, { "cell_type": "code", "execution_count": null, + "id": "ca1bad2d", "metadata": {}, "outputs": [], "source": [ @@ -486,144 +487,144 @@ "\n", "M = Matrix(Ts.A)\n", "M" - ], - "id": "ca1bad2d" + ] }, { "cell_type": "markdown", + "id": "ca0ab88f", "metadata": {}, "source": [ "which is more compact (it takes a few seconds to compute). We can see that a trigometric _sum of angles_ substition has been performed, there are instances of sine and cosine of $q_1 + q_2$, the shoulder and elbow joints. This is to be expected since these joints are adjacent and have with parallel axes.\n", "\n", "We can _slice_ the end-effector translationfrom the SymPy matrix" - ], - "id": "ca0ab88f" + ] }, { "cell_type": "code", "execution_count": null, + "id": "ab687736", "metadata": {}, "outputs": [], "source": [ "M[:3,3]" - ], - "id": "ab687736" + ] }, { "cell_type": "markdown", + "id": "a659ed23", "metadata": {}, "source": [ "The floating point constants here have been inherited from the kinematic model we imported at [this step](#Robot-forward-kinematics). It would be possible to replace the non-zero kinematic constants $a_i$ and $d_i$ with symbols created using `sym.symbol` as shown in [this section](#Creating-a-symbolic-variable)." - ], - "id": "a659ed23" + ] }, { "cell_type": "markdown", + "id": "66ac5b98", "metadata": {}, "source": [ "## Code generation\n", "We can now use some of SymPy's superpowers to turn our forward kinematic expression into code" - ], - "id": "66ac5b98" + ] }, { "cell_type": "code", "execution_count": null, + "id": "3a0312f9", "metadata": {}, "outputs": [], "source": [ "from sympy import ccode, pycode, octave_code\n", "print(ccode(M, assign_to=\"T\"))" - ], - "id": "3a0312f9" + ] }, { "cell_type": "markdown", + "id": "8228338a", "metadata": {}, "source": [ "which is pure C code that does not require any linear algebra package to compute. We simply need to define the values of `q_0` to `q_5` in order to determine the end-effector pose. The result is computed using symbolically simplified expressions. The code is not optimized, but we could expect the compiler to perform some additional simplification.\n", "\n", "The equivalent MATLAB code is" - ], - "id": "8228338a" + ] }, { "cell_type": "code", "execution_count": null, + "id": "22aaf482", "metadata": {}, "outputs": [], "source": [ "octave_code(M)" - ], - "id": "22aaf482" + ] }, { "cell_type": "markdown", + "id": "19de3fd2", "metadata": {}, "source": [ "We can also output Python code" - ], - "id": "19de3fd2" + ] }, { "cell_type": "code", "execution_count": null, + "id": "9e03efae", "metadata": {}, "outputs": [], "source": [ "print(pycode(M))" - ], - "id": "9e03efae" + ] }, { "cell_type": "markdown", + "id": "d8a37469", "metadata": {}, "source": [ "Which constructs an instance of a SymPy `ImmutableDenseMatrix` which we can turn into a function" - ], - "id": "d8a37469" + ] }, { "cell_type": "code", "execution_count": null, + "id": "22e110f7", "metadata": {}, "outputs": [], "source": [ "from sympy import lambdify\n", "T_func = lambdify(q, M, modules='numpy')" - ], - "id": "22e110f7" + ] }, { "cell_type": "markdown", + "id": "545c42a8", "metadata": {}, "source": [ "If we pass in the zero joint angles we get the familiar forward kinematic solution result" - ], - "id": "545c42a8" + ] }, { "cell_type": "code", "execution_count": null, + "id": "da379113", "metadata": {}, "outputs": [], "source": [ "T_func(0, 0, 0, 0, 0, 0)" - ], - "id": "da379113" + ] }, { "cell_type": "markdown", + "id": "2fed14fe", "metadata": {}, "source": [ "SymPy also supports printing code in C++ (`cxxcode`), Fortran (`fcode`), JavaScript (`jscode`), Julia (`julia_code`), Rust (`rust_code`), Mathematica (`mathematica_code`) and Octave/MATLAB (`octave_code`).\n", "\n", "The [SymPy autowrap](https://docs.sympy.org/latest/modules/codegen.html#autowrap) capability automatically generates code, writes it to disk, compiles it, and imports it into the current session. It creates a wrapper using Cython and creates a numerical function." - ], - "id": "2fed14fe" + ] }, { "cell_type": "markdown", + "id": "84ec9a92", "metadata": {}, "source": [ "## Robot dynamics\n", @@ -631,476 +632,476 @@ "We can also compute the dynamics symbolically. To do this we must use the Python version of the inverse dynamics rather than the default efficient C-code implementation.\n", "\n", "We need to setup the problem. Firstly we make the gravitational constant a symbol" - ], - "id": "84ec9a92" + ] }, { "cell_type": "code", "execution_count": null, + "id": "c2c2765d", "metadata": {}, "outputs": [], "source": [ "g = sym.symbol('g')\n", "puma.gravity = [0, 0, -g]" - ], - "id": "c2c2765d" + ] }, { "cell_type": "markdown", + "id": "e1fa5bf1", "metadata": {}, "source": [ "Next we create symbolic vectors for joint velocity and acceleration, just as we did earlier for joint coordinates" - ], - "id": "e1fa5bf1" + ] }, { "cell_type": "code", "execution_count": null, + "id": "7a855919", "metadata": {}, "outputs": [], "source": [ "qd = sym.symbol('qd_:6')\n", "qd" - ], - "id": "7a855919" + ] }, { "cell_type": "code", "execution_count": null, + "id": "52774649", "metadata": {}, "outputs": [], "source": [ "qdd = sym.symbol('qdd_:6')\n", "qdd" - ], - "id": "52774649" + ] }, { "cell_type": "markdown", + "id": "5c967a12", "metadata": {}, "source": [ "Now we compute the inverse dynamics as a function of symbolic joint coordinates, velocities, accelerations and gravity as well as a lot of numerical kinematic and dynamic parameters.\n", "\n", "This takes a second or so. The result is impressive: each torque is an expression with thousands of terms. Note that `gravity` is the acceleration due to gravity, so for the usual orientation of the Puma it is `[0, 0, -g]`." - ], - "id": "5c967a12" + ] }, { "cell_type": "code", "execution_count": null, + "id": "b6db73a3", "metadata": { "scrolled": true }, "outputs": [], "source": [ "%time tau = puma.rne_python(q, qd, qdd)" - ], - "id": "b6db73a3" + ] }, { "cell_type": "code", "execution_count": null, + "id": "051cec6a", "metadata": {}, "outputs": [], "source": [ "from sympy import trigsimp, simplify" - ], - "id": "051cec6a" + ] }, { "cell_type": "code", "execution_count": null, + "id": "effe62d5", "metadata": {}, "outputs": [], "source": [ "%time z = simplify(tau[5])" - ], - "id": "effe62d5" + ] }, { "cell_type": "markdown", + "id": "3d6e6daa", "metadata": {}, "source": [ "SymPy's `simplify` can reduce these expressions a lot, but it takes longer the more terms there are. It took about a second for the last joint, 15 seconds for $\\tau_3$, 50 seconds for $\\tau_4$ and several minutes for $\\tau_2$, which we will not wait for." - ], - "id": "3d6e6daa" + ] }, { "cell_type": "markdown", + "id": "eade206a", "metadata": {}, "source": [ "The result `tau` is a NumPy array, as it is for the numeric case, but with `dtype=object` since each element is a SymPy expression" - ], - "id": "eade206a" + ] }, { "cell_type": "code", "execution_count": null, + "id": "00437d19", "metadata": {}, "outputs": [], "source": [ "type(tau)" - ], - "id": "00437d19" + ] }, { "cell_type": "markdown", + "id": "43780f20", "metadata": {}, "source": [ "and it has the expected shape" - ], - "id": "43780f20" + ] }, { "cell_type": "code", "execution_count": null, + "id": "00d293ec", "metadata": {}, "outputs": [], "source": [ "tau.shape" - ], - "id": "00d293ec" + ] }, { "cell_type": "markdown", + "id": "d4594129", "metadata": {}, "source": [ "The torque on the first joint $\\tau_0$ is" - ], - "id": "d4594129" + ] }, { "cell_type": "code", "execution_count": null, + "id": "db07968d", "metadata": {}, "outputs": [], "source": [ "tau[0]" - ], - "id": "db07968d" + ] }, { "cell_type": "markdown", + "id": "53b9c432", "metadata": {}, "source": [ "which is a complicated expression. To make it easier to work with, we will expand it" - ], - "id": "53b9c432" + ] }, { "cell_type": "code", "execution_count": null, + "id": "c5248007", "metadata": {}, "outputs": [], "source": [ "tau_0 = tau[0].expand()\n", "tau_0" - ], - "id": "c5248007" + ] }, { "cell_type": "markdown", + "id": "6297390b", "metadata": {}, "source": [ "to form a multinomial, a sum of products of trigonometric functions. The number of product terms is" - ], - "id": "6297390b" + ] }, { "cell_type": "code", "execution_count": null, + "id": "1a7fbeeb", "metadata": {}, "outputs": [], "source": [ "len(tau_0.args)" - ], - "id": "1a7fbeeb" + ] }, { "cell_type": "markdown", + "id": "87bf7dec", "metadata": {}, "source": [ "With the expression in this form we can find all the terms that contain $\\ddot{q}_0$" - ], - "id": "87bf7dec" + ] }, { "cell_type": "code", "execution_count": null, + "id": "ae97b209", "metadata": {}, "outputs": [], "source": [ "m = tau_0.coeff(qdd[0]).args\n", "m" - ], - "id": "ae97b209" + ] }, { "cell_type": "markdown", + "id": "9e27d63d", "metadata": {}, "source": [ "so we can write our torque expression as $\\tau_0 = m \\ddot{q}_0 + \\cdots$ which means that $m$ must be element $\\textbf{M}_{00}$ of the manipulator inertia matrix. We see it has contributions due to the centre of mass of the arm which depends on the configuration of joints 1 to 5, and motor inertia. \n", "\n", "Similarly " - ], - "id": "9e27d63d" + ] }, { "cell_type": "code", "execution_count": null, + "id": "eaf3b9a3", "metadata": {}, "outputs": [], "source": [ "m = tau_0.coeff(qdd[1]).args\n", "m" - ], - "id": "eaf3b9a3" + ] }, { "cell_type": "markdown", + "id": "d7594f8b", "metadata": {}, "source": [ "must be the element $\\textbf{M}_{01}$, an off-diagonal element of the inertia matrix. In this way we can generate expressions for each element of the manipulator inertia matrix. Remember that this matrix is symmetric so we only need to compute half of the off-diagnonal terms.\n", "\n", "In a similar way we can find elements of the Coriolis and centripetal matrix. Terms containing $\\dot{q}_0$ are" - ], - "id": "d7594f8b" + ] }, { "cell_type": "code", "execution_count": null, + "id": "e66008fa", "metadata": {}, "outputs": [], "source": [ "c = tau_0.coeff(qd[0]).args\n", "c" - ], - "id": "e66008fa" + ] }, { "cell_type": "markdown", + "id": "a23fb8be", "metadata": {}, "source": [ "so we can write our torque expression as $\\tau_0 = c \\dot{q}_0 + \\cdots$\n", "A subset of these terms still contain $\\dot{q}_0$" - ], - "id": "a23fb8be" + ] }, { "cell_type": "code", "execution_count": null, + "id": "6d0e77ea", "metadata": {}, "outputs": [], "source": [ "C = tau_0.coeff(qd[0], 2).args\n", "C" - ], - "id": "6d0e77ea" + ] }, { "cell_type": "markdown", + "id": "4e89d40c", "metadata": {}, "source": [ "ie. they originally contained $\\dot{q}_0^2$. The squared velocity terms belong on the diagonal of the Coriolis matrix and represent centripetal torques. This particular expression is the element $\\textbf{C}_{00}$.\n", "\n", "The off-diagonal terms are coefficients of $\\dot{q}_i \\dot{q}_j$ and we can find $\\dot{q}_0 \\dot{q}_1$ by" - ], - "id": "4e89d40c" + ] }, { "cell_type": "code", + "execution_count": null, + "id": "fbba5177", "metadata": {}, + "outputs": [], "source": [ "c = tau_0.coeff(qd[0]).coeff(qd[1]).args\n", "c" - ], - "execution_count": null, - "outputs": [], - "id": "fbba5177" + ] }, { "cell_type": "markdown", + "id": "c024f72d", "metadata": {}, "source": [ "We add $C/2$ to both $C_{01}$ and $C_{10}$ and repeat for all the off-diagonal terms. $C_{ij} = \\Sum_{i=0}^n \\Sum_{j=0}^n c\n", "\n", "The elements of this matrix map velocities to force and therefore have the same dimensions as viscous friction. In fact the motor friction parameter $B_m$ will be mixed up in these expressions, so we should first set $B_m = 0$ for all joints before proceeding." - ], - "id": "c024f72d" + ] }, { "cell_type": "markdown", + "id": "dd52ac06", "metadata": {}, "source": [ "SymPy provides tools that allow us to pull apart the expression's parse tree. The top node is given by `.func` which in this case is" - ], - "id": "dd52ac06" + ] }, { "cell_type": "code", "execution_count": null, + "id": "66950865", "metadata": {}, "outputs": [], "source": [ "tau_0.func" - ], - "id": "66950865" + ] }, { "cell_type": "markdown", + "id": "0dd4dc63", "metadata": {}, "source": [ "indicating an addition node, and its arguments are" - ], - "id": "0dd4dc63" + ] }, { "cell_type": "code", "execution_count": null, + "id": "34645872", "metadata": { "scrolled": true }, "outputs": [], "source": [ "tau_0.args" - ], - "id": "34645872" + ] }, { "cell_type": "markdown", + "id": "8772b5fd", "metadata": {}, "source": [ "expressed as a tuple. A term, say the first one, can be further decomposed" - ], - "id": "8772b5fd" + ] }, { "cell_type": "code", "execution_count": null, + "id": "ea341725", "metadata": {}, "outputs": [], "source": [ "tau_0.args[0].func" - ], - "id": "ea341725" + ] }, { "cell_type": "code", "execution_count": null, + "id": "dd92407d", "metadata": {}, "outputs": [], "source": [ "tau_0.args[0].args" - ], - "id": "dd92407d" + ] }, { "cell_type": "markdown", + "id": "40e07b83", "metadata": {}, "source": [ "which is we see here is a product of three terms. There is a standard ordering of the arguments and it looks like constants are always first." - ], - "id": "40e07b83" + ] }, { "cell_type": "markdown", + "id": "31e6ee9d", "metadata": {}, "source": [ "We can see that many of the terms have very small coefficients, they literally don't add much to the result, so we can cull them out, or rather, we can select those with significant coefficients" - ], - "id": "31e6ee9d" + ] }, { "cell_type": "code", "execution_count": null, + "id": "b3276582", "metadata": {}, "outputs": [], "source": [ "signif_terms = [t for t in tau_0.args if abs(t.args[0]) > 1e-2]\n", "signif_terms" - ], - "id": "b3276582" + ] }, { "cell_type": "markdown", + "id": "37cb8924", "metadata": {}, "source": [ "and there are now only" - ], - "id": "37cb8924" + ] }, { "cell_type": "code", "execution_count": null, + "id": "7ddacfaa", "metadata": {}, "outputs": [], "source": [ "len(signif_terms)" - ], - "id": "7ddacfaa" + ] }, { "cell_type": "markdown", + "id": "a8286e0d", "metadata": {}, "source": [ "product terms remaining, just 25% of the number we started with.\n", "\n", "We can reconstruct the expression with just the significant terms" - ], - "id": "a8286e0d" + ] }, { "cell_type": "code", "execution_count": null, + "id": "7180d48f", "metadata": {}, "outputs": [], "source": [ "tau_0s = tau_0.func(*signif_terms)\n", "tau_0s" - ], - "id": "7180d48f" + ] }, { "cell_type": "markdown", + "id": "e28872d0", "metadata": {}, "source": [ "and the top-level node function, `tau_0.func` (which is `Add`) acts as a constructor to which we pass the significant terms.\n", "\n", "We can generate executable code in C to compute the torque on joint 0 which could be used in a feed-forward controller for example. " - ], - "id": "e28872d0" + ] }, { "cell_type": "code", "execution_count": null, + "id": "5e7f04f5", "metadata": {}, "outputs": [], "source": [ "print(ccode(tau_0s, assign_to=\"tau0\"))" - ], - "id": "5e7f04f5" + ] }, { "cell_type": "markdown", + "id": "6a829be4", "metadata": {}, "source": [ "We can do this for any of the expressions we generated above, ie. for elements of the inertia matrix or the velocity matrix.\n", "\n", "The gravity vector is obtained when $\\dot{q}_j =0$ and $\\ddot{q}_j =0$" - ], - "id": "6a829be4" + ] }, { "cell_type": "markdown", + "id": "6f6fb8b9", "metadata": {}, "source": [ "To set these symbolic values to zero we define a substituion dictionary where a key is the symbols to be substituted and the value is its new (substituted) value." - ], - "id": "6f6fb8b9" + ] }, { "cell_type": "code", "execution_count": null, + "id": "18037d42", "metadata": {}, "outputs": [], "source": [ @@ -1108,23 +1109,23 @@ "for j in range(puma.n):\n", " subsdict[qd[j]] = 0\n", " subsdict[qdd[j]] = 0" - ], - "id": "18037d42" + ] }, { "cell_type": "code", "execution_count": null, + "id": "84832fe9", "metadata": {}, "outputs": [], "source": [ "tau_g = [t.subs(subsdict) for t in tau]\n", "tau_g" - ], - "id": "84832fe9" + ] }, { "cell_type": "code", "execution_count": null, + "id": "02ee0c13", "metadata": {}, "outputs": [], "source": [ @@ -1137,40 +1138,39 @@ " subsdict[sym.cos(q[j])] = C\n", " sqdict[S**2] = 1 - C**2\n", "subsdict" - ], - "id": "02ee0c13" + ] }, { "cell_type": "code", "execution_count": null, + "id": "0889139e", "metadata": {}, "outputs": [], "source": [ "sqdict" - ], - "id": "0889139e" + ] }, { "cell_type": "code", "execution_count": null, + "id": "9c4271d7", "metadata": {}, "outputs": [], "source": [ "t1 = tau[1].subs(subsdict)" - ], - "id": "9c4271d7" + ] }, { "cell_type": "code", "execution_count": null, + "id": "2742b4c0", "metadata": { "scrolled": true }, "outputs": [], "source": [ "t1.expand().subs(sqdict)" - ], - "id": "2742b4c0" + ] } ], "metadata": { @@ -1180,58 +1180,7 @@ "name": "python3" }, "language_info": { - "codemirror_mode": { - "name": "ipython", - "version": 3 - }, - "file_extension": ".py", - "mimetype": "text/x-python", - "name": "python", - "nbconvert_exporter": "python", - "pygments_lexer": "ipython3", - "version": "3.8.3" - }, - "toc": { - "base_numbering": 1, - "nav_menu": {}, - "number_sections": true, - "sideBar": true, - "skip_h1_title": false, - "title_cell": "Table of Contents", - "title_sidebar": "Contents", - "toc_cell": false, - "toc_position": {}, - "toc_section_display": true, - "toc_window_display": false - }, - "varInspector": { - "cols": { - "lenName": 16, - "lenType": 16, - "lenVar": 40 - }, - "kernels_config": { - "python": { - "delete_cmd_postfix": "", - "delete_cmd_prefix": "del ", - "library": "var_list.py", - "varRefreshCmd": "print(var_dic_list())" - }, - "r": { - "delete_cmd_postfix": ") ", - "delete_cmd_prefix": "rm(", - "library": "var_list.r", - "varRefreshCmd": "cat(var_dic_list()) " - } - }, - "types_to_exclude": [ - "module", - "function", - "builtin_function_or_method", - "instance", - "_Feature" - ], - "window_display": false + "name": "python" }, "rtb_bootstrap": { "extras": [ diff --git a/docs/notebooks/sync_bootstrap.py b/docs/notebooks/sync_bootstrap.py index 94b4f9636..2e31e54ed 100644 --- a/docs/notebooks/sync_bootstrap.py +++ b/docs/notebooks/sync_bootstrap.py @@ -139,7 +139,7 @@ def process_notebook(path: Path, template: str, fix: bool) -> bool: if changed and fix: with path.open("w", encoding="utf-8") as f: - json.dump(nb, f, indent=1, ensure_ascii=False) + json.dump(nb, f, indent=1, ensure_ascii=False, sort_keys=True) f.write("\n") return changed diff --git a/docs/notebooks/two-link-dynamics.ipynb b/docs/notebooks/two-link-dynamics.ipynb index 6d44d68f4..0a38cd268 100644 --- a/docs/notebooks/two-link-dynamics.ipynb +++ b/docs/notebooks/two-link-dynamics.ipynb @@ -2,6 +2,7 @@ "cells": [ { "cell_type": "markdown", + "id": "59e40ae3", "metadata": {}, "source": [ "
\n", @@ -20,11 +21,11 @@ "The gravity and velocity torques of the two-link planar arm in Spong, Hutchinson and Vidyasagar, computed by the Toolbox from a DH model and from an ETS model, and compared with the analytic Euler-Lagrange equations.\n", "\n", "> **Modernised 2026-10-19** to address API drift: this notebook was written for an earlier release of the Toolbox, and has been updated so that it runs with the current one." - ], - "id": "59e40ae3" + ] }, { "cell_type": "markdown", + "id": "1181a25a", "metadata": {}, "source": [ "![Spong book](figs/spong-2link.png)\n", @@ -34,12 +35,12 @@ "Robot Modeling and Control, Spong, Vidyasagar, Hutchinson, Wiley 2006\n", "\n", "We will choose the parameters to be" - ], - "id": "1181a25a" + ] }, { "cell_type": "code", "execution_count": null, + "id": "44ef7c64", "metadata": { "jupyter": { "source_hidden": true @@ -89,12 +90,12 @@ " return colab\n", "\n", "COLAB = await ensure_installed()\n" - ], - "id": "44ef7c64" + ] }, { "cell_type": "code", "execution_count": null, + "id": "fff04355", "metadata": {}, "outputs": [], "source": [ @@ -125,71 +126,71 @@ "\n", "# global parameters\n", "g = 9.81" - ], - "id": "fff04355" + ] }, { "cell_type": "markdown", + "id": "97dca5bc", "metadata": {}, "source": [ "## Denavit-Hartenberg notation" - ], - "id": "97dca5bc" + ] }, { "cell_type": "code", "execution_count": null, + "id": "5bc9a4b1", "metadata": {}, "outputs": [], "source": [ "L1 = RevoluteDH(a=l1, m=m1, r=[-lc1, 0, 0])\n", "L2 = RevoluteDH(a=l2, m=m2, r=[-lc2, 0, 0])\n", "robot = DHRobot([L1, L2], gravity=[0, -g, 0])" - ], - "id": "5bc9a4b1" + ] }, { "cell_type": "markdown", + "id": "90faa0b7", "metadata": {}, "source": [ "The DH notation only allows rotation about the z-axis, which is out of the page in the Figure above. The `gravity` argument is the acceleration due to gravity, a vector in the world frame. Gravity acts downward, in the $-y$ direction of the figure, so we set it to `[0, -g, 0]` (the default is `[0, 0, -9.81]`). The gravity load is the torque the joints must exert to hold the arm still against gravity, so it is positive for an arm stretched out horizontally." - ], - "id": "90faa0b7" + ] }, { "cell_type": "code", "execution_count": null, + "id": "54518096", "metadata": {}, "outputs": [], "source": [ "print(robot)\n", "print(robot.dynamics())" - ], - "id": "54518096" + ] }, { "cell_type": "code", "execution_count": null, + "id": "9ac9cf16", "metadata": {}, "outputs": [], "source": [ "for q in qq:\n", " tau = robot.gravload(q)\n", " print(tau)" - ], - "id": "9ac9cf16" + ] }, { "cell_type": "markdown", + "id": "23cb99c3", "metadata": {}, "source": [ "Using equations (7.85) and (7.86) we can write an expression for gravity torque derived analytically using Euler-Lagrange (rather than Newton-Euler) approach" - ], - "id": "23cb99c3" + ] }, { "cell_type": "code", "execution_count": null, + "id": "7a06438a", "metadata": {}, "outputs": [], "source": [ @@ -198,12 +199,12 @@ " (m1 * lc1 + m2 * l1) * g * cos(q[0]) + m2 * lc2 * g * cos(q[0] + q[1]),\n", " m2 * lc2 * g * cos(q[0] + q[1]),\n", " ]" - ], - "id": "7a06438a" + ] }, { "cell_type": "code", "execution_count": null, + "id": "353ac8f8", "metadata": {}, "outputs": [], "source": [ @@ -211,47 +212,47 @@ " tau_rne = robot.gravload(q)\n", " tau_el = EL_grav(q)\n", " print(tau_rne, \"vs\", tau_el)" - ], - "id": "353ac8f8" + ] }, { "cell_type": "markdown", + "id": "df4b8940", "metadata": {}, "source": [ "In fact we could use symbolic values of the constants above, repeat the above process, and generate the symbolic equations for gravity load." - ], - "id": "df4b8940" + ] }, { "cell_type": "markdown", + "id": "fc0719d4", "metadata": {}, "source": [ "Now we can explore the velocity coupling terms. To isolate them we will set gravity and joint acceleration to zero" - ], - "id": "fc0719d4" + ] }, { "cell_type": "code", "execution_count": null, + "id": "934c0ed1", "metadata": {}, "outputs": [], "source": [ "robot.gravity = [0, 0, 0]\n", "robot.rne(q2, [1, 1], z)" - ], - "id": "934c0ed1" + ] }, { "cell_type": "markdown", + "id": "a4f17632", "metadata": {}, "source": [ "Using equations on page 261 and (7.87) we can write the velocity dependent terms as" - ], - "id": "a4f17632" + ] }, { "cell_type": "code", "execution_count": null, + "id": "a8398927", "metadata": {}, "outputs": [], "source": [ @@ -267,12 +268,12 @@ " c121 * qd[0] * qd[1] + c211 * qd[1] * qd[0] + c221 * qd[1] ** 2,\n", " c112 * qd[0] ** 2,\n", " ]" - ], - "id": "a8398927" + ] }, { "cell_type": "code", "execution_count": null, + "id": "a59f1388", "metadata": {}, "outputs": [], "source": [ @@ -281,12 +282,12 @@ " tau_rne = robot.rne(q, qd, z)\n", " tau_el = EL_velocity(q, qd)\n", " print(tau_rne, \"vs\", tau_el)" - ], - "id": "a59f1388" + ] }, { "cell_type": "code", "execution_count": null, + "id": "3a4abe52", "metadata": {}, "outputs": [], "source": [ @@ -312,28 +313,28 @@ "q = symbol(\"q_:2\")\n", "qd = symbol(\"qd_:2\")\n", "robot.rne_python(q, qd, [0, 0])" - ], - "id": "3a4abe52" + ] }, { "cell_type": "markdown", + "id": "c90a3625", "metadata": {}, "source": [ "## ETS notation" - ], - "id": "c90a3625" + ] }, { "cell_type": "markdown", + "id": "6a72b4cf", "metadata": {}, "source": [ "We will redefine the parameters of the model to numerical values" - ], - "id": "6a72b4cf" + ] }, { "cell_type": "code", "execution_count": null, + "id": "7ceead6a", "metadata": {}, "outputs": [], "source": [ @@ -357,24 +358,24 @@ "\n", "# global parameters\n", "g = 9.81" - ], - "id": "7ceead6a" + ] }, { "cell_type": "code", "execution_count": null, + "id": "04fbffa0", "metadata": {}, "outputs": [], "source": [ "L1 = Link(ets=ET.Rz(), m=1, r=[0.5, 0, 0], name=\"L1\")\n", "L2 = Link(ets=ET.tx(1) * ET.Rz(), m=1, r=[0.5, 0, 0], parent=L1, name=\"L2\")\n", "robot = Robot([L1, L2], gravity=[0, -g, 0])" - ], - "id": "04fbffa0" + ] }, { "cell_type": "code", "execution_count": null, + "id": "dbeb857e", "metadata": {}, "outputs": [], "source": [ @@ -382,8 +383,7 @@ " tau_rne = robot.gravload(q)\n", " tau_el = EL_grav(q)\n", " print(tau_rne, \"vs\", tau_el)" - ], - "id": "dbeb857e" + ] } ], "metadata": { @@ -393,45 +393,7 @@ "name": "python3" }, "language_info": { - "codemirror_mode": { - "name": "ipython", - "version": 3 - }, - "file_extension": ".py", - "mimetype": "text/x-python", - "name": "python", - "nbconvert_exporter": "python", - "pygments_lexer": "ipython3", - "version": "3.8.5" - }, - "varInspector": { - "cols": { - "lenName": 16, - "lenType": 16, - "lenVar": 40 - }, - "kernels_config": { - "python": { - "delete_cmd_postfix": "", - "delete_cmd_prefix": "del ", - "library": "var_list.py", - "varRefreshCmd": "print(var_dic_list())" - }, - "r": { - "delete_cmd_postfix": ") ", - "delete_cmd_prefix": "rm(", - "library": "var_list.r", - "varRefreshCmd": "cat(var_dic_list()) " - } - }, - "types_to_exclude": [ - "module", - "function", - "builtin_function_or_method", - "instance", - "_Feature" - ], - "window_display": false + "name": "python" } }, "nbformat": 4, diff --git a/tests/test_notebook_bootstrap.py b/tests/test_notebook_bootstrap.py index 8092a8ccd..8c00d058a 100644 --- a/tests/test_notebook_bootstrap.py +++ b/tests/test_notebook_bootstrap.py @@ -12,6 +12,8 @@ import sys from pathlib import Path +import pytest + NOTEBOOKS_DIR = Path(__file__).resolve().parent.parent / "docs" / "notebooks" @@ -265,3 +267,43 @@ def test_markdown_cells_get_ids_too(tmp_path): assert sync_bootstrap.process_notebook(nb_path, template="", fix=True) assert "id" in json.loads(nb_path.read_text(encoding="utf-8"))["cells"][0] + + +def test_editor_metadata_is_normalised(tmp_path): + nb_path = tmp_path / "demo.ipynb" + nb = { + "cells": [_code_cell("x = 1\n")], + "metadata": { + "kernelspec": {"display_name": "dev", "language": "python", "name": "python3"}, + "language_info": {"name": "python", "version": "3.12.14"}, + "vscode": {"interpreter": {"hash": "abc"}}, + "varInspector": {"window_display": False}, + "rtb_bootstrap": {"extras": ["ik"]}, + }, + "nbformat": 4, + "nbformat_minor": 5, + } + nb_path.write_text(json.dumps(nb), encoding="utf-8") + + assert clear_outputs.process_notebook(nb_path, fix=False) # reported, not written + assert clear_outputs.process_notebook(nb_path, fix=True) + + out = json.loads(nb_path.read_text(encoding="utf-8")) + assert out["metadata"] == { + "kernelspec": {"display_name": "Python 3", "language": "python", "name": "python3"}, + "language_info": {"name": "python"}, + "rtb_bootstrap": {"extras": ["ik"]}, # ours, kept + } + assert not clear_outputs.process_notebook(nb_path, fix=True) + + +def test_notebooks_are_written_as_nbformat_writes_them(tmp_path): + # so that opening and saving a notebook in VS Code or Jupyter changes nothing + nbformat = pytest.importorskip("nbformat") + + nb_path = tmp_path / "demo.ipynb" + _write_notebook(nb_path, [_code_cell("x = 1\n")]) + clear_outputs.process_notebook(nb_path, fix=True) + + text = nb_path.read_text(encoding="utf-8") + assert text == nbformat.writes(nbformat.reads(text, as_version=4)) + "\n"