{ "cells": [ { "cell_type": "markdown", "metadata": { "nbsphinx": "hidden" }, "source": [ "This notebook is part of the `clifford` documentation: https://clifford.readthedocs.io/." ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "# Application to Robotic Manipulators\n", "\n", "This notebook is intended to expand upon the ideas in part of the presentation [Robots, Ganja & Screw Theory](https://slides.com/hugohadfield/game2020)\n", "\n", "## Serial manipulator\n", "\n", "\n", "\n", "[(slides)](https://slides.com/hugohadfield/game2020#/serial)\n", "\n", "Let's consider a 2-link 3 DOF arm. We'll model the links within the robot with rotors, which transform to the coordinate frame of the end of each link. This is very similar to the approach that would classically be taken with 4×4 matrices.\n", "\n", "We're going to define our class piecewise as we go along here.\n", "To aid that, we'll write a simple base class to let us do just that.\n", "In your own code, there's no need to do this." ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "class AddMethodsAsWeGo:\n", " @classmethod\n", " def _add_method(cls, m):\n", " if isinstance(m, property):\n", " name = (m.fget or m.fset).__name__\n", " else:\n", " name = m.__name__\n", " setattr(cls, name, m)" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "Let's start by defining some names for the links, and a place to store our parameters:" ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "from enum import Enum\n", "\n", "class Links(Enum):\n", " BASE = 'b'\n", " SHOULDER = 's'\n", " UPPER = 'u'\n", " ELBOW = 'e'\n", " FOREARM = 'f'\n", " ENDPOINT = 'n'\n", " \n", " \n", "class SerialRobot(AddMethodsAsWeGo):\n", " def __init__(self, rho, l):\n", " self.l = l\n", " self.rho = rho\n", " self._thetas = (0, 0, 0)\n", "\n", " @property\n", " def thetas(self):\n", " return self._thetas" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "### Forward kinematics\n", "[(slides)](https://slides.com/hugohadfield/game2020#/serial-forward-rotors)\n", "\n", "As a reminder, we can construct rotation and translation motors as:\n", "$$\n", "\\begin{align}\n", "T(a)\n", " &= \\exp \\left(\\frac{1}{2} n_{\\infty} \\wedge a \\right) \\\\\n", " &= 1 + \\frac{1}{2}n_{\\infty} \\wedge a \\\\\n", "R(\\theta, \\hat B)\n", " &= \\exp (\\frac{1}{2} \\theta \\hat B) \\\\\n", " &= \\cos \\frac{\\theta}{2} + \\sin \\frac{\\theta}{2} \\hat B\n", "\\end{align}\n", "$$\n", "\n", "Applying these to our geometry, we get\n", "\n", "$$\n", "\\begin{align}\n", "R_{\\text{base} \\gets \\text{shoulder}} &= R(\\theta_0, e_1 \\wedge e_3) \\\\\n", "R_{\\text{shoulder} \\gets \\text{upper arm}} &= R(\\theta_1, e_1 \\wedge e_2) \\\\\n", "R_{\\text{upper arm} \\gets \\text{elbow}} &= T(\\rho e_1) \\\\\n", "R_{\\text{elbow} \\gets \\text{forearm}} &= R(\\theta_2, e_1 \\wedge e_2) \\\\\n", "R_{\\text{forearm} \\gets \\text{endpoint}} &= T(-l e_1)\\\\\n", "\\end{align}\n", "$$\n", "\n", "From which we can get the overall rotor to the frame of the endpoint, and the positions $X$ and $Y$:\n", "\n", "$$\n", "\\begin{align}\n", "R_{\\text{base} \\gets \\text{elbow}}\n", " &= R_{\\text{base} \\gets \\text{shoulder}} R_{\\text{shoulder} \\gets \\text{upper arm}} R_{\\text{upper arm} \\gets \\text{elbow}} \\\\\n", "X &= R_{\\text{base} \\gets \\text{elbow}} n_0 \\tilde R_{\\text{base} \\gets \\text{elbow}} \\\\\n", "R_{\\text{base} \\gets \\text{endpoint}}\n", " &= R_{\\text{base} \\gets \\text{shoulder}} R_{\\text{shoulder} \\gets \\text{upper arm}} R_{\\text{upper arm} \\gets \\text{elbow}} R_{\\text{elbow} \\gets \\text{forearm}} R_{\\text{forearm} \\gets \\text{endpoint}} \\\\\n", "Y &= R_{\\text{base} \\gets \\text{endpoint}} n_0 \\tilde R_{\\text{base} \\gets \\text{endpoint}} \\\\\n", "\\end{align}\n", "$$\n", "\n", "We can write this as:" ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "from clifford.g3c import *\n", "from clifford.tools.g3c import generate_translation_rotor, apply_rotor\n", "from clifford.tools.g3 import generate_rotation_rotor\n", "\n", "def _update_chain(rotors, a, b, c):\n", " rotors[a, c] = rotors[a, b] * rotors[b, c]\n", "\n", "@SerialRobot._add_method\n", "@SerialRobot.thetas.setter\n", "def thetas(self, value):\n", " theta0, theta1, theta2 = self._thetas = value\n", " # shorthands for brevity\n", " R = generate_rotation_rotor\n", " T = generate_translation_rotor\n", "\n", " rotors = {}\n", " rotors[Links.BASE, Links.SHOULDER] = R(theta0, e1, e3)\n", " rotors[Links.SHOULDER, Links.UPPER] = R(theta1, e1, e2)\n", " rotors[Links.UPPER, Links.ELBOW] = T(self.rho * e1)\n", " rotors[Links.ELBOW, Links.FOREARM] = R(theta2, e1, e2)\n", " rotors[Links.FOREARM, Links.ENDPOINT] = T(-self.l * e1)\n", "\n", " _update_chain(rotors, Links.BASE, Links.SHOULDER, Links.UPPER)\n", " _update_chain(rotors, Links.BASE, Links.UPPER, Links.ELBOW)\n", " _update_chain(rotors, Links.BASE, Links.ELBOW, Links.FOREARM)\n", " _update_chain(rotors, Links.BASE, Links.FOREARM, Links.ENDPOINT)\n", " self.rotors = rotors\n", "\n", "@SerialRobot._add_method\n", "@property\n", "def y_pos(self):\n", " return apply_rotor(eo, self.rotors[Links.BASE, Links.ENDPOINT])\n", "\n", "\n", "@SerialRobot._add_method\n", "@property\n", "def x_pos(self):\n", " return apply_rotor(eo, self.rotors[Links.BASE, Links.ELBOW])" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "Let's write a renderer so we can check this all works" ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "from pyganja import GanjaScene\n", "\n", "def add_rotor(sc: GanjaScene, r, *, label=None, color=None, scale=0.1):\n", " \"\"\" show how a rotor transforms the axes at the origin \"\"\"\n", " y = apply_rotor(eo, r)\n", " y_frame = [\n", " apply_rotor(d, r)\n", " for d in [up(scale*e1), up(scale*e2), up(scale*e3)]\n", " ]\n", " sc.add_object(y, label=label, color=color)\n", " sc.add_facet([y, y_frame[0]], color=(255, 0, 0))\n", " sc.add_facet([y, y_frame[1]], color=(0, 255, 0))\n", " sc.add_facet([y, y_frame[2]], color=(0, 0, 255))\n", " \n", "\n", "@SerialRobot._add_method\n", "def to_scene(self):\n", " sc = GanjaScene()\n", " axis_scale = 0.1\n", " link_scale = 0.05\n", " arm_color = (192, 192, 192)\n", "\n", " base_obj = (up(0.2*e1)^up(0.2*e3)^up(-0.2*e1)).normal()\n", " sc.add_object(base_obj, color=0)\n", "\n", " shoulder_axis = [\n", " apply_rotor(p, self.rotors[Links.BASE, Links.UPPER])\n", " for p in [up(axis_scale*e3), up(-axis_scale*e3)]\n", " ]\n", " sc.add_facet(shoulder_axis, color=(0, 0, 128))\n", " shoulder_angle = [\n", " apply_rotor(eo, self.rotors[Links.BASE, Links.SHOULDER]),\n", " apply_rotor(up(axis_scale*e1), self.rotors[Links.BASE, Links.SHOULDER]),\n", " apply_rotor(up(axis_scale*e1), self.rotors[Links.BASE, Links.UPPER]),\n", " ]\n", " sc.add_facet(shoulder_angle, color=(0, 0, 128))\n", "\n", " upper_arm_points = [\n", " apply_rotor(up(link_scale*e3), self.rotors[Links.BASE, Links.UPPER]),\n", " apply_rotor(up(-link_scale*e3), self.rotors[Links.BASE, Links.UPPER]),\n", " apply_rotor(up(link_scale*e3), self.rotors[Links.BASE, Links.ELBOW]),\n", " apply_rotor(up(-link_scale*e3), self.rotors[Links.BASE, Links.ELBOW])\n", " ]\n", " sc.add_facet(upper_arm_points[:3], color=arm_color)\n", " sc.add_facet(upper_arm_points[1:], color=arm_color)\n", "\n", " elbow_axis = [\n", " apply_rotor(p, self.rotors[Links.BASE, Links.ELBOW])\n", " for p in [up(axis_scale*e3), up(-axis_scale*e3)]\n", " ]\n", " sc.add_facet(elbow_axis, color=(0, 0, 128))\n", "\n", " forearm_points = [\n", " apply_rotor(up(link_scale*e3), self.rotors[Links.BASE, Links.FOREARM]),\n", " apply_rotor(up(-link_scale*e3), self.rotors[Links.BASE, Links.FOREARM]),\n", " apply_rotor(up(link_scale*e3), self.rotors[Links.BASE, Links.ENDPOINT]),\n", " apply_rotor(up(-link_scale*e3), self.rotors[Links.BASE, Links.ENDPOINT])\n", " ]\n", " sc.add_facet(forearm_points[:3], color=arm_color)\n", " sc.add_facet(forearm_points[1:], color=arm_color)\n", "\n", " add_rotor(sc, self.rotors[Links.BASE, Links.ELBOW], label='x', color=(128, 128, 128))\n", " add_rotor(sc, self.rotors[Links.BASE, Links.ENDPOINT], label='y', color=(128, 128, 128))\n", "\n", " return sc" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "We can now instantiate our robot" ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "serial_robot = SerialRobot(rho=1, l=0.5)" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "Choose a trajectory" ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "import math\n", "theta_traj = [\n", " (math.pi/6 + i*math.pi/12, math.pi/3 - math.pi/12*i, 3*math.pi/4)\n", " for i in range(3)\n", "]" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "And plot the robot in each state, using `ipywidgets` ([docs](https://ipywidgets.readthedocs.io/)) to let us plot ganja side-by-side.\n", "Unfortunately, `pyganja` provides no mechanism to animate these plots from python.\n", "\n", "
\n", "This will not render side-by-side in the online clifford documentation, but will in a local notebook. \n", "
" ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "import ipywidgets\n", "from IPython.display import Latex, display\n", "from pyganja import draw\n", "\n", "outputs = [\n", " ipywidgets.Output(layout=ipywidgets.Layout(flex='1'))\n", " for i in range(len(theta_traj))\n", "]\n", "for output, thetas in zip(outputs, theta_traj):\n", " with output:\n", " # interesting part here - run the forward kinematics, print the angles we used\n", " serial_robot.thetas = thetas\n", " display(Latex(r\"$\\theta_i = {:.2f}, {:.2f}, {:.2f}$\".format(*thetas)))\n", " draw(serial_robot.to_scene(), scale=1.5)\n", "ipywidgets.HBox(outputs)" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "### Inverse kinematics\n", "\n", "[(slides)](https://slides.com/hugohadfield/game2020#/serial-reverse)\n", "\n", "\n", "For the forward kinematics, we didn't actually need conformal geometric algebra at all—PGA would have done just fine, as all we needed were rotations and translations.\n", "The inverse kinematics of a serial manipulator is where CGA provide some nice tricks.\n", "\n", "There are three facts we know about the position $X$, each of which describes a constraint surface\n", "\n", "* $X$ must lie on a sphere with radius $l$ centered at $Y$, which can be written\n", " $$S^* = Y - \\frac{1}{2}l^2n_\\infty$$\n", " \n", "* $X$ must lie on a sphere with radius $\\rho$ centered at $n_o$, which can be written\n", " $$S_\\text{base}^* = n_0 - \\frac{1}{2}\\rho^2n_\\infty$$\n", " \n", "* $X$ must lie on a plane through $n_o$, $e_3$, and $Y$, which can be written\n", " $$\\Pi = n_0\\wedge \\operatorname{up}(e_3)\\wedge Y\\wedge n_\\infty$$\n", " \n", " Note that $\\Pi = 0$ is possible iff $Y = \\operatorname{up}(ke_3)$.\n", "\n", "For $X$ to satisfy all three constraints. we have\n", "\n", "\\begin{align}\n", "S \\wedge X = S_\\text{base} \\wedge X = \\Pi \\wedge X &= 0 \\\\\n", "X \\wedge (\\underbrace{S \\vee S_\\text{base} \\vee \\Pi}_P) &= 0 \\quad\\text{If $\\Pi \\ne 0$} \\\\\n", "X \\wedge (\\underbrace{S \\vee S_\\text{base}}_C) &= 0 \\quad\\text{otherwise} \\\\\n", "\\end{align}\n", "\n", "By looking at the grade of the term labelled $P$, we conclude it must be a point-pair—which tells us $X$ must lie in one of two locations. Similarly, $C$ must be a circle." ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "@SerialRobot._add_method\n", "def _get_x_constraints_for(self, Y):\n", " \"\"\" Get the space containing all possible elbow positions \"\"\"\n", " # strictly should be undual, but we don't have that in clifford\n", " S = (Y - 0.5*self.l**2*einf).dual()\n", " S_base = (eo - 0.5*self.rho**2*einf).dual()\n", " Pi = eo ^ up(e2) ^ Y ^ einf\n", " return S, S_base, Pi\n", "\n", "@SerialRobot._add_method\n", "def _get_x_positions_for(self, Y):\n", " \"\"\" Get the space containing all possible elbow positions \"\"\"\n", " S, S_base, Pi = self._get_x_constraints_for(Y)\n", " if Pi == 0:\n", " # any solution on the circle is OK\n", " return S & S_base\n", " else:\n", " # there are just two solutions\n", " return S & S_base & Pi" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "From the pointpair $P$ we can extract the two possible $X$ locations with:\n", "$$\n", "X = \\left[1 \\pm \\frac{P}{\\sqrt{P\\tilde{P}}}\\right](P\\cdot n_\\infty)\n", "$$\n", "\n", "To be considered a full solution to the inverse kinematics problem, we need to produce the angles $\\theta_0, \\theta_1, \\theta_2$.\n", "We can do this as follows" ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "@SerialRobot._add_method\n", "@SerialRobot.y_pos.setter\n", "def y_pos(self, Y):\n", " R = generate_rotation_rotor\n", " T = generate_translation_rotor\n", " \n", " rotors = {}\n", " rotors[Links.UPPER, Links.ELBOW] = T(self.rho * e1)\n", " rotors[Links.FOREARM, Links.ENDPOINT] = T(-self.l * e1)\n", "\n", " x_options = self._get_x_positions_for(Y)\n", " if x_options.grades == {3}:\n", " # no need to adjust the base angle\n", " theta_0 = self.thetas[0]\n", " rotors[Links.BASE, Links.SHOULDER] = self.rotors[Links.BASE, Links.SHOULDER]\n", " # remove the rotation from x, intersect it with the plane of the links\n", " x_options = x_options & (eo ^ up(e3) ^ up(e1) ^ einf)\n", " else:\n", " y_down = down(Y)\n", " theta0 = math.atan2(y_down[(3,)], y_down[(1,)])\n", " rotors[Links.BASE, Links.SHOULDER] = R(theta0, e1, e3)\n", " \n", " # remove the first rotor from x\n", " x_options = apply_rotor(x_options, ~rotors[Links.BASE, Links.SHOULDER])\n", " \n", " # project out one end of the point-pair\n", " x = (1 - x_options.normal()) * (x_options | einf)\n", " \n", " x_down = down(x)\n", " theta1 = math.atan2(x_down[(2,)], x_down[(1,)])\n", " rotors[Links.SHOULDER, Links.UPPER] = R(theta1, e1, e2)\n", " \n", " _update_chain(rotors, Links.BASE, Links.SHOULDER, Links.UPPER)\n", " _update_chain(rotors, Links.BASE, Links.UPPER, Links.ELBOW)\n", " \n", " # remove the second rotor\n", " Y = apply_rotor(Y, ~rotors[Links.BASE, Links.ELBOW])\n", " y_down = down(Y)\n", " \n", " theta2 = math.atan2(-y_down[(2,)], -y_down[(1,)])\n", " rotors[Links.ELBOW, Links.FOREARM] = R(theta2, e1, e2)\n", " _update_chain(rotors, Links.BASE, Links.ELBOW, Links.FOREARM)\n", " _update_chain(rotors, Links.BASE, Links.FOREARM, Links.ENDPOINT)\n", " \n", " self._thetas = (theta0, theta1, theta2)\n", " self.rotors = rotors" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "Define a trajectory again, this time with a scene to render it:" ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "y_traj = [\n", " up(0.3*e3 + 0.8*e2 - 0.25*e1),\n", " up(0.6*e3 + 0.8*e2),\n", " up(0.9*e3 + 0.8*e2 + 0.25*e1)\n", "]\n", "\n", "expected_scene = GanjaScene()\n", "expected_scene.add_facet(y_traj[0:2], color=(255, 128, 128))\n", "expected_scene.add_facet(y_traj[1:3], color=(255, 128, 128))" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "And we can run the inverse kinematics by setting `serial_robot.y_pos`:" ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "outputs = [\n", " ipywidgets.Output(layout=ipywidgets.Layout(flex='1'))\n", " for i in range(len(y_traj))\n", "]\n", "first = True\n", "for output, y in zip(outputs, y_traj):\n", " with output:\n", " # interesting part here - run the reverse kinematics, print the angles we used\n", " serial_robot.y_pos = y\n", " display(Latex(r\"$\\theta_i = {:.2f}, {:.2f}, {:.2f}$\".format(*serial_robot.thetas)))\n", " sc = serial_robot.to_scene()\n", " \n", " # Show the spheres we used to construct the solution\n", " sc += expected_scene\n", " if first:\n", " extra_scene = GanjaScene()\n", " S, S_base, Pi = serial_robot._get_x_constraints_for(y)\n", " extra_scene.add_object(S_base, label='S_base', color=(255, 255, 128))\n", " extra_scene.add_object(S, label='S', color=(255, 128, 128))\n", " extra_scene.add_object(Pi, label='Pi', color=(128, 255, 192, 128))\n", " sc += extra_scene\n", " draw(sc, scale=1.5)\n", " first = False\n", "ipywidgets.HBox(outputs)" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "## Parallel manipulators\n", "\n", "For now, refer to the presentation\n", "\n", "\n", "[(slides)](https://slides.com/hugohadfield/game2020#/parallel)" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "### Inverse kinematics\n", "\n", "[(slides)](https://slides.com/hugohadfield/game2020#/agile-3dof-inverse)\n", "\n", "For now, refer to the presentation\n", "\n" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "### Forward kinematics\n", "\n", "[(slides)](https://slides.com/hugohadfield/game2020#/agile-2dof-forward)\n", "\n", "For now, refer to the presentation" ] } ], "metadata": { "kernelspec": { "display_name": "Python 3", "language": "python", "name": "python3" }, "language_info": { "codemirror_mode": { "name": "ipython", "version": 3 }, "file_extension": ".py", "mimetype": "text/x-python", "name": "python", "nbconvert_exporter": "python", "pygments_lexer": "ipython3", "version": "3.8.0" } }, "nbformat": 4, "nbformat_minor": 4 }