This notebook is part of the clifford documentation: https://clifford.readthedocs.io/.

Application to Robotic Manipulators

This notebook is intended to expand upon the ideas in part of the presentation Robots, Ganja & Screw Theory

Serial manipulator

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.

We're going to define our class piecewise as we go along here. To aid that, we'll write a simple base class to let us do just that. In your own code, there's no need to do this.



In [ ]:

    
class AddMethodsAsWeGo:
    @classmethod
    def _add_method(cls, m):
        if isinstance(m, property):
            name = (m.fget or m.fset).__name__
        else:
            name = m.__name__
        setattr(cls, name, m)

Let's start by defining some names for the links, and a place to store our parameters:



In [ ]:

    
from enum import Enum

class Links(Enum):
    BASE = 'b'
    SHOULDER = 's'
    UPPER = 'u'
    ELBOW = 'e'
    FOREARM = 'f'
    ENDPOINT = 'n'
    
    
class SerialRobot(AddMethodsAsWeGo):
    def __init__(self, rho, l):
        self.l = l
        self.rho = rho
        self._thetas = (0, 0, 0)

    @property
    def thetas(self):
        return self._thetas

Forward kinematics

(slides)

As a reminder, we can construct rotation and translation motors as: $$ \begin{align} T(a) &= \exp \left(\frac{1}{2} n_{\infty} \wedge a \right) \\ &= 1 + \frac{1}{2}n_{\infty} \wedge a \\ R(\theta, \hat B) &= \exp (\frac{1}{2} \theta \hat B) \\ &= \cos \frac{\theta}{2} + \sin \frac{\theta}{2} \hat B \end{align} $$

Applying these to our geometry, we get

$$ \begin{align} R_{\text{base} \gets \text{shoulder}} &= R(\theta_0, e_1 \wedge e_3) \\ R_{\text{shoulder} \gets \text{upper arm}} &= R(\theta_1, e_1 \wedge e_2) \\ R_{\text{upper arm} \gets \text{elbow}} &= T(\rho e_1) \\ R_{\text{elbow} \gets \text{forearm}} &= R(\theta_2, e_1 \wedge e_2) \\ R_{\text{forearm} \gets \text{endpoint}} &= T(-l e_1)\\ \end{align} $$

From which we can get the overall rotor to the frame of the endpoint, and the positions $X$ and $Y$:

$$ \begin{align} R_{\text{base} \gets \text{elbow}} &= R_{\text{base} \gets \text{shoulder}} R_{\text{shoulder} \gets \text{upper arm}} R_{\text{upper arm} \gets \text{elbow}} \\ X &= R_{\text{base} \gets \text{elbow}} n_0 \tilde R_{\text{base} \gets \text{elbow}} \\ R_{\text{base} \gets \text{endpoint}} &= 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}} \\ Y &= R_{\text{base} \gets \text{endpoint}} n_0 \tilde R_{\text{base} \gets \text{endpoint}} \\ \end{align} $$

We can write this as:



In [ ]:

    
from clifford.g3c import *
from clifford.tools.g3c import generate_translation_rotor, apply_rotor
from clifford.tools.g3 import generate_rotation_rotor

def _update_chain(rotors, a, b, c):
    rotors[a, c] = rotors[a, b] * rotors[b, c]

@SerialRobot._add_method
@SerialRobot.thetas.setter
def thetas(self, value):
    theta0, theta1, theta2 = self._thetas = value
    # shorthands for brevity
    R = generate_rotation_rotor
    T = generate_translation_rotor

    rotors = {}
    rotors[Links.BASE, Links.SHOULDER] = R(theta0, e1, e3)
    rotors[Links.SHOULDER, Links.UPPER] = R(theta1, e1, e2)
    rotors[Links.UPPER, Links.ELBOW] = T(self.rho * e1)
    rotors[Links.ELBOW, Links.FOREARM] = R(theta2, e1, e2)
    rotors[Links.FOREARM, Links.ENDPOINT] = T(-self.l * e1)

    _update_chain(rotors, Links.BASE, Links.SHOULDER, Links.UPPER)
    _update_chain(rotors, Links.BASE, Links.UPPER, Links.ELBOW)
    _update_chain(rotors, Links.BASE, Links.ELBOW, Links.FOREARM)
    _update_chain(rotors, Links.BASE, Links.FOREARM, Links.ENDPOINT)
    self.rotors = rotors

@SerialRobot._add_method
@property
def y_pos(self):
    return apply_rotor(eo, self.rotors[Links.BASE, Links.ENDPOINT])


@SerialRobot._add_method
@property
def x_pos(self):
    return apply_rotor(eo, self.rotors[Links.BASE, Links.ELBOW])

Let's write a renderer so we can check this all works



In [ ]:

    
from pyganja import GanjaScene

def add_rotor(sc: GanjaScene, r, *, label=None, color=None, scale=0.1):
    """ show how a rotor transforms the axes at the origin """
    y = apply_rotor(eo, r)
    y_frame = [
        apply_rotor(d, r)
        for d in [up(scale*e1), up(scale*e2), up(scale*e3)]
    ]
    sc.add_object(y, label=label, color=color)
    sc.add_facet([y, y_frame[0]], color=(255, 0, 0))
    sc.add_facet([y, y_frame[1]], color=(0, 255, 0))
    sc.add_facet([y, y_frame[2]], color=(0, 0, 255))
    

@SerialRobot._add_method
def to_scene(self):
    sc = GanjaScene()
    axis_scale = 0.1
    link_scale = 0.05
    arm_color = (192, 192, 192)

    base_obj = (up(0.2*e1)^up(0.2*e3)^up(-0.2*e1)).normal()
    sc.add_object(base_obj, color=0)

    shoulder_axis = [
        apply_rotor(p, self.rotors[Links.BASE, Links.UPPER])
        for p in [up(axis_scale*e3), up(-axis_scale*e3)]
    ]
    sc.add_facet(shoulder_axis, color=(0, 0, 128))
    shoulder_angle = [
        apply_rotor(eo, self.rotors[Links.BASE, Links.SHOULDER]),
        apply_rotor(up(axis_scale*e1), self.rotors[Links.BASE, Links.SHOULDER]),
        apply_rotor(up(axis_scale*e1), self.rotors[Links.BASE, Links.UPPER]),
    ]
    sc.add_facet(shoulder_angle, color=(0, 0, 128))

    upper_arm_points = [
        apply_rotor(up(link_scale*e3), self.rotors[Links.BASE, Links.UPPER]),
        apply_rotor(up(-link_scale*e3), self.rotors[Links.BASE, Links.UPPER]),
        apply_rotor(up(link_scale*e3), self.rotors[Links.BASE, Links.ELBOW]),
        apply_rotor(up(-link_scale*e3), self.rotors[Links.BASE, Links.ELBOW])
    ]
    sc.add_facet(upper_arm_points[:3], color=arm_color)
    sc.add_facet(upper_arm_points[1:], color=arm_color)

    elbow_axis = [
        apply_rotor(p, self.rotors[Links.BASE, Links.ELBOW])
        for p in [up(axis_scale*e3), up(-axis_scale*e3)]
    ]
    sc.add_facet(elbow_axis, color=(0, 0, 128))

    forearm_points = [
        apply_rotor(up(link_scale*e3), self.rotors[Links.BASE, Links.FOREARM]),
        apply_rotor(up(-link_scale*e3), self.rotors[Links.BASE, Links.FOREARM]),
        apply_rotor(up(link_scale*e3), self.rotors[Links.BASE, Links.ENDPOINT]),
        apply_rotor(up(-link_scale*e3), self.rotors[Links.BASE, Links.ENDPOINT])
    ]
    sc.add_facet(forearm_points[:3], color=arm_color)
    sc.add_facet(forearm_points[1:], color=arm_color)

    add_rotor(sc, self.rotors[Links.BASE, Links.ELBOW], label='x', color=(128, 128, 128))
    add_rotor(sc, self.rotors[Links.BASE, Links.ENDPOINT], label='y', color=(128, 128, 128))

    return sc

We can now instantiate our robot



In [ ]:

    
serial_robot = SerialRobot(rho=1, l=0.5)

Choose a trajectory



In [ ]:

    
import math
theta_traj = [
    (math.pi/6 + i*math.pi/12, math.pi/3 - math.pi/12*i, 3*math.pi/4)
    for i in range(3)
]

And plot the robot in each state, using ipywidgets (docs) to let us plot ganja side-by-side. Unfortunately, pyganja provides no mechanism to animate these plots from python.

This will not render side-by-side in the online clifford documentation, but will in a local notebook.



In [ ]:

    
import ipywidgets
from IPython.display import Latex, display
from pyganja import draw

outputs = [
    ipywidgets.Output(layout=ipywidgets.Layout(flex='1'))
    for i in range(len(theta_traj))
]
for output, thetas in zip(outputs, theta_traj):
    with output:
        # interesting part here - run the forward kinematics, print the angles we used
        serial_robot.thetas = thetas
        display(Latex(r"$\theta_i = {:.2f}, {:.2f}, {:.2f}$".format(*thetas)))
        draw(serial_robot.to_scene(), scale=1.5)
ipywidgets.HBox(outputs)

Inverse kinematics

(slides)

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. The inverse kinematics of a serial manipulator is where CGA provide some nice tricks.

There are three facts we know about the position $X$, each of which describes a constraint surface

$X$ must lie on a sphere with radius $l$ centered at $Y$, which can be written $$S^* = Y - \frac{1}{2}l^2n_\infty$$
$X$ must lie on a sphere with radius $\rho$ centered at $n_o$, which can be written $$S_\text{base}^* = n_0 - \frac{1}{2}\rho^2n_\infty$$
$X$ must lie on a plane through $n_o$, $e_3$, and $Y$, which can be written $$\Pi = n_0\wedge \operatorname{up}(e_3)\wedge Y\wedge n_\infty$$

Note that $\Pi = 0$ is possible iff $Y = \operatorname{up}(ke_3)$.

For $X$ to satisfy all three constraints. we have

\begin{align} S \wedge X = S_\text{base} \wedge X = \Pi \wedge X &= 0 \\ X \wedge (\underbrace{S \vee S_\text{base} \vee \Pi}_P) &= 0 \quad\text{If $\Pi \ne 0$} \\ X \wedge (\underbrace{S \vee S_\text{base}}_C) &= 0 \quad\text{otherwise} \\ \end{align}

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.



In [ ]:

    
@SerialRobot._add_method
def _get_x_constraints_for(self, Y):
    """ Get the space containing all possible elbow positions """
    # strictly should be undual, but we don't have that in clifford
    S = (Y - 0.5*self.l**2*einf).dual()
    S_base = (eo - 0.5*self.rho**2*einf).dual()
    Pi = eo ^ up(e2) ^ Y ^ einf
    return S, S_base, Pi

@SerialRobot._add_method
def _get_x_positions_for(self, Y):
    """ Get the space containing all possible elbow positions """
    S, S_base, Pi = self._get_x_constraints_for(Y)
    if Pi == 0:
        # any solution on the circle is OK
        return S & S_base
    else:
        # there are just two solutions
        return S & S_base & Pi

From the pointpair $P$ we can extract the two possible $X$ locations with: $$ X = \left[1 \pm \frac{P}{\sqrt{P\tilde{P}}}\right](P\cdot n_\infty) $$

To be considered a full solution to the inverse kinematics problem, we need to produce the angles $\theta_0, \theta_1, \theta_2$. We can do this as follows



In [ ]:

    
@SerialRobot._add_method
@SerialRobot.y_pos.setter
def y_pos(self, Y):
    R = generate_rotation_rotor
    T = generate_translation_rotor
    
    rotors = {}
    rotors[Links.UPPER, Links.ELBOW] = T(self.rho * e1)
    rotors[Links.FOREARM, Links.ENDPOINT] = T(-self.l * e1)

    x_options = self._get_x_positions_for(Y)
    if x_options.grades == {3}:
        # no need to adjust the base angle
        theta_0 = self.thetas[0]
        rotors[Links.BASE, Links.SHOULDER] = self.rotors[Links.BASE, Links.SHOULDER]
        # remove the rotation from x, intersect it with the plane of the links
        x_options = x_options & (eo ^ up(e3) ^ up(e1) ^ einf)
    else:
        y_down = down(Y)
        theta0 = math.atan2(y_down[(3,)], y_down[(1,)])
        rotors[Links.BASE, Links.SHOULDER] = R(theta0, e1, e3)
        
        # remove the first rotor from x
        x_options = apply_rotor(x_options, ~rotors[Links.BASE, Links.SHOULDER])
    
    # project out one end of the point-pair
    x = (1 - x_options.normal()) * (x_options | einf)
    
    x_down = down(x)
    theta1 = math.atan2(x_down[(2,)], x_down[(1,)])
    rotors[Links.SHOULDER, Links.UPPER] = R(theta1, e1, e2)
    
    _update_chain(rotors, Links.BASE, Links.SHOULDER, Links.UPPER)
    _update_chain(rotors, Links.BASE, Links.UPPER, Links.ELBOW)
    
    # remove the second rotor
    Y = apply_rotor(Y, ~rotors[Links.BASE, Links.ELBOW])
    y_down = down(Y)
    
    theta2 = math.atan2(-y_down[(2,)], -y_down[(1,)])
    rotors[Links.ELBOW, Links.FOREARM] = R(theta2, e1, e2)
    _update_chain(rotors, Links.BASE, Links.ELBOW, Links.FOREARM)
    _update_chain(rotors, Links.BASE, Links.FOREARM, Links.ENDPOINT)
    
    self._thetas = (theta0, theta1, theta2)
    self.rotors = rotors

Define a trajectory again, this time with a scene to render it:



In [ ]:

    
y_traj = [
    up(0.3*e3 + 0.8*e2 - 0.25*e1),
    up(0.6*e3 + 0.8*e2),
    up(0.9*e3 + 0.8*e2 + 0.25*e1)
]

expected_scene = GanjaScene()
expected_scene.add_facet(y_traj[0:2], color=(255, 128, 128))
expected_scene.add_facet(y_traj[1:3], color=(255, 128, 128))

And we can run the inverse kinematics by setting serial_robot.y_pos:



In [ ]:

    
outputs = [
    ipywidgets.Output(layout=ipywidgets.Layout(flex='1'))
    for i in range(len(y_traj))
]
first = True
for output, y in zip(outputs, y_traj):
    with output:
        # interesting part here - run the reverse kinematics, print the angles we used
        serial_robot.y_pos = y
        display(Latex(r"$\theta_i = {:.2f}, {:.2f}, {:.2f}$".format(*serial_robot.thetas)))
        sc = serial_robot.to_scene()
        
        # Show the spheres we used to construct the solution
        sc += expected_scene
        if first:
            extra_scene = GanjaScene()
            S, S_base, Pi = serial_robot._get_x_constraints_for(y)
            extra_scene.add_object(S_base, label='S_base', color=(255, 255, 128))
            extra_scene.add_object(S, label='S', color=(255, 128, 128))
            extra_scene.add_object(Pi, label='Pi', color=(128, 255, 192, 128))
            sc += extra_scene
        draw(sc, scale=1.5)
    first = False
ipywidgets.HBox(outputs)

Parallel manipulators

For now, refer to the presentation

(slides)

Inverse kinematics

(slides)

For now, refer to the presentation

Forward kinematics

(slides)

For now, refer to the presentation