Looking for orientation in forward kinematics for serial robot manipulator with CGA

Conformal GA
1

Hi, I am trying to do foward kinematic for serial robot with CGA. I have been successful with constructing a compound Rotor that chain all the translation and rotation of every links, and getting the final position of the end-effector by applying such rotor to the null basis ‘no’. However, I’m having trouble finding the orientation of the end-effector, which can be easily extracted from the rotation matrix of the matrix algebra counter-part.

Here is the code using clifford library for more context:

from clifford.g3c import *

import clifford as cf

import numpy as np

import time

import math

import sympy as sp

def CGA_fkine_kuka7(q):
    ni = e4 + e5
    no = 0.5*(e5-e4)

    Rz0 = math.e**(-0.5*q[0]*e12)
    Tz0 = 1 - 0.5*0.34*(e3^ni);
    Rx0 = math.e**(-0.5*(-np.pi/2)*e23)

    Rz1 = math.e**(-0.5*q[1]*e12)
    Rx1 = math.e**(-0.5*(np.pi/2)*e23)

    Rz2 = math.e**(-0.5*q[2]*e12)
    Tz2 = 1 - 0.5*0.4*(e3^ni);
    Rx2 = math.e**(-0.5*(np.pi/2)*e23)

    Rz3 = math.e**(-0.5*q[3]*e12)
    Rx3 = math.e**(-0.5*(-np.pi/2)*e23)

    Rz4 = math.e**(-0.5*q[4]*e12)
    Tz4 = 1 - 0.5*0.4*(e3^ni);
    Rx4 = math.e**(-0.5*(-np.pi/2)*e23)

    Rz5 = math.e**(-0.5*q[5]*e12)
    Rx5 = math.e**(-0.5*(np.pi/2)*e23)

    Rz6 = math.e**(-0.5*q[6]*e12)
    Tz6 = 1 - 0.5*0.126*(e3^ni);

    R = Rz0*Tz0*Rx0*Rz1*Rx1*Rz2*Tz2*Rx2*Rz3*Rx3*Rz4*Tz4*Rx4*Rz5*Rx5*Rz6*Tz6

    Pos = R*no*~R

    return Pos, R
2

The rotation rotor is obtained from a Euclidean versor V by R = -no . (V ni).
Once you have the rotor, axis and angle are easily obtained by the standard bivector logarithm from R_3 (or by the equivalent quaternion code).

1 Like