def local_trans(self, joint_name, joint_angle):
"""calculate local transformation of one joint
:param str joint_name: the name of joint
:param float joint_angle: the angle of joint in radians
:return: transformation
:rtype: 4x4 matrix
"""
T = identity(4)
s = np.sin(joint_angle)
c = np.cos(joint_angle)
# transformation
for trafo in (matrix for matrix, joints in JOINTS.iteritems() if joint_name in joints):
T = np.dot(T, trafo(s, c))
# offset
x, y, z = OFFSET[joint_name]
T[3, 0] = x
T[3, 1] = y
T[3, 2] = z
return T
def set_joint_axis(self, jacobi, effector_chain):
for i, joint in enumerate(effector_chain):
for trafo in (matrix for matrix, joints in JOINTS.iteritems() if joint in joints):
jacobi[self.get_angle_index(trafo), i] = 1
return jacobi
