def calculateForwardTransform(id, angles):
"""
id - chain id
angles - [float]
"""
fullTransform = identity()
# Do base transforms
for baseTransform in BASE_TRANSFORMS[id]:
fullTransform = dot(fullTransform, baseTransform)
# Do mDH transforms
numTransforms = NUM_JOINTS_CHAIN[id]
for angle, (alpha, l, theta, d) in zip(angles, MDH_PARAMS[id]):
# Right before we do a transformation, we are in the correct
# coordinate frame and we need to store it, so we know where all the
# links of a chain are. We only need to do this if the transformation
# gives us a new link
# length L - movement along the X(i-1) axis
if l != 0:
transX = translation4D(l, 0.0, 0.0)
fullTransform = dot(fullTransform, transX)
# twist: - rotate about the X(i-1) axis
if alpha != 0:
rotX = rotation4D(X_AXIS, alpha)
fullTransform = dot(fullTransform, rotX)
# theta - rotate about the Z(i) axis
if theta + angle != 0:
rotZ = rotation4D(Z_AXIS, theta + angle)
fullTransform = dot(fullTransform, rotZ)
# offset D movement along the Z(i) axis
if d != 0:
transZ = translation4D(0.0, 0.0, d)
fullTransform = dot(fullTransform, transZ)
# Do the end transforms
for endTransform in END_TRANSFORMS[id]:
fullTransform = dot(fullTransform, endTransform)
return fullTransform
[0.0, -THIGH_LENGTH, 0.0, 0.0],
[0.0, -TIBIA_LENGTH, 0.0, 0.0],
[-pi / 2, 0.0, 0.0, 0.0],
]
RIGHT_ARM_MDH_PARAMS = [
[-pi / 2, 0.0, 0.0, 0.0],
[pi / 2, 0.0, pi / 2, 0.0],
[pi / 2, 0.0, 0.0, UPPER_ARM_LENGTH],
[-pi / 2, 0.0, 0.0, 0.0],
]
MDH_PARAMS = [HEAD_MDH_PARAMS, LEFT_ARM_MDH_PARAMS, LEFT_LEG_MDH_PARAMS, RIGHT_LEG_MDH_PARAMS, RIGHT_ARM_MDH_PARAMS]
# Base transforms to get from body center to beg. of chain
HEAD_BASE_TRANSFORMS = [translation4D(0.0, 0.0, NECK_OFFSET_Z)]
LEFT_ARM_BASE_TRANSFORMS = [translation4D(0.0, SHOULDER_OFFSET_Y, SHOULDER_OFFSET_Z)]
LEFT_LEG_BASE_TRANSFORMS = [translation4D(0.0, HIP_OFFSET_Y, -HIP_OFFSET_Z)]
RIGHT_LEG_BASE_TRANSFORMS = [translation4D(0.0, -HIP_OFFSET_Y, -HIP_OFFSET_Z)]
RIGHT_ARM_BASE_TRANSFORMS = [translation4D(0.0, -SHOULDER_OFFSET_Y, SHOULDER_OFFSET_Z)]
BASE_TRANSFORMS = [
HEAD_BASE_TRANSFORMS,
LEFT_ARM_BASE_TRANSFORMS,
LEFT_LEG_BASE_TRANSFORMS,
RIGHT_LEG_BASE_TRANSFORMS,
RIGHT_ARM_BASE_TRANSFORMS,
