# Application to Robotic Manipulators¶

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

## Serial manipulator¶

(slides)

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.

[1]:

class AddMethodsAsWeGo:
@classmethod
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:

[2]:

from enum import Enum

BASE = 'b'
SHOULDER = 's'
UPPER = 'u'
ELBOW = 'e'
FOREARM = 'f'
ENDPOINT = 'n'

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{split}\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}\end{split}

Applying these to our geometry, we get

\begin{split}\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}\end{split}

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

\begin{split}\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}\end{split}

We can write this as:

[3]:

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.thetas.setter
def thetas(self, value):
theta0, theta1, theta2 = self._thetas = value
# shorthands for brevity
R = generate_rotation_rotor
T = generate_translation_rotor

rotors = {}

self.rotors = rotors

@property
def y_pos(self):

@property
def x_pos(self):

/home/docs/checkouts/readthedocs.org/user_builds/clifford/envs/v1.4.0/lib/python3.8/site-packages/pyganja/__init__.py:2: UserWarning: Failed to import cef_gui, cef functions will be unavailable
from .script_api import *


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

[4]:

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)]
]

def to_scene(self):
sc = GanjaScene()
axis_scale = 0.1
arm_color = (192, 192, 192)

base_obj = (up(0.2*e1)^up(0.2*e3)^up(-0.2*e1)).normal()

shoulder_axis = [
for p in [up(axis_scale*e3), up(-axis_scale*e3)]
]
shoulder_angle = [
]

upper_arm_points = [
]

elbow_axis = [
for p in [up(axis_scale*e3), up(-axis_scale*e3)]
]

forearm_points = [
]

return sc


We can now instantiate our robot

[5]:

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


Choose a trajectory

[6]:

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.

[7]:

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.

[8]:

@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

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

[9]:

@SerialRobot._add_method
@SerialRobot.y_pos.setter
def y_pos(self, Y):
R = generate_rotation_rotor
T = generate_translation_rotor

rotors = {}

x_options = self._get_x_positions_for(Y)
# no need to adjust the base angle
theta_0 = self.thetas[0]
# 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,)])

# remove the first rotor from x

# 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,)])

# remove the second rotor
y_down = down(Y)

theta2 = math.atan2(-y_down[(2,)], -y_down[(1,)])

self._thetas = (theta0, theta1, theta2)
self.rotors = rotors


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

[10]:

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()


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

[11]:

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(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