import numpy as np
class KinematicTreeNode:
def __init__(self): # Constructor
self.translation = None
self.rotation = None
self.child = None
def RotZ(theta):
c = np.cos(theta)
s = np.sin(theta)
return np.array([[c, -s],[s, c]])
def EndEffectorPos(X):
if X.child == None:
return X.translation
else:
return X.translation + np.matmul(RotZ(X.rotation),EndEffectorPos(X.child))
base = KinematicTreeNode()
L1 = KinematicTreeNode()
L2 = KinematicTreeNode()
L3 = KinematicTreeNode()
L4 = KinematicTreeNode()
base.translation = np.array([[0],[0]]) # translation to the joint
base.rotation = -np.pi/2 # rotation of the joint
L1.translation = np.array([[1],[0]])
L1.rotation = 0.0
L2.translation = np.array([[1],[0]])
L2.rotation = np.pi/2
L3.translation = np.array([[1],[0]])
L3.rotation = 0
L4.translation = np.array([[1],[0]])
L4.rotation = 0
base.child = L1
L1.child = L2
L2.child = L3
L3.child = L4
L4.child = None
EndEffectorPos(base)