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

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*e12)
Tz0 = 1 - 0.5*0.34*(e3^ni);
Rx0 = math.e**(-0.5*(-np.pi/2)*e23)

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

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

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

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

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

Rz6 = math.e**(-0.5*q*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
``````

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