def updateTransforms(self, bodyAngles, inclinationAngles, supportLegChain=LLEG_CHAIN, debug=False):
"""
TOOD: get supportFoot from ALMotion (if it is our own walk engine should
be easier)
Calculate all forward transformation matrices from the center of mass to each
end effector. For the head this is the focal point of the camera. We also
calculate the transformation from the camera frame to the world frame.
Then we calculate horizon and camera height which is necessary for the
calculation of pix estimates.
Input:
bodyAngles, inclinationAngles - angles of body joints and accelerometer calculated
inclination.
supportLegChain - which leg is on the ground.
"""
self._bodyAngles = bodyAngles
self._inclination = inclinationAngles
if debug:
import pdb
pdb.set_trace()
# Make up bogus values
HEAD_START, LLEG_START, RLEG_START = 0, 8, 14
headAngles = bodyAngles[HEAD_START:2]
lLegAngles = bodyAngles[LLEG_START : LLEG_START + 6]
rLegAngles = bodyAngles[RLEG_START : RLEG_START + 6]
origin = vector4D(0.0, 0.0, 0.0)
cameraToBodyTransform = calculateForwardTransform(HEAD_CHAIN, headAngles)
leg_angles = (supportLegChain == LLEG_CHAIN and lLegAngles) or rLegAngles
supportLegToBodyTransform = calculateForwardTransform(supportLegChain, leg_angles)
supportLegLocation = dot(supportLegToBodyTransform, origin)
# At this time we trust inertial
bodyInclinationX, bodyInclinationY = inclinationAngles
bodyToWorldTransform = dot(rotation4D(X_AXIS, bodyInclinationX), rotation4D(Y_AXIS, bodyInclinationY))
torsoLocationInLegFrame = dot(bodyToWorldTransform, supportLegLocation)
# get the Z component of the location
self.comHeight = -torsoLocationInLegFrame[Z]
self.cameraToWorldFrame = cameraToWorldFrame = dot(bodyToWorldTransform, cameraToBodyTransform)
self.calcImageHorizonLine()
self.focalPointInWorldFrame = [cameraToWorldFrame[X, 3], cameraToWorldFrame[Y, 3], cameraToWorldFrame[Z, 3]]
return self.focalPointInWorldFrame
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
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,
]
# Base transforms to get from body center to beg. of chain
HEAD_END_TRANSFORMS = [
rotation4D(X_AXIS, pi / 2),
rotation4D(Y_AXIS, pi / 2),
translation4D(CAMERA_OFF_X, 0, CAMERA_OFF_Z),
rotation4D(Y_AXIS, CAMERA_PITCH_ANGLE),
]
LEFT_ARM_END_TRANSFORMS = [rotation4D(Z_AXIS, -pi / 2), translation4D(LOWER_ARM_LENGTH, 0.0, 0.0)]
LEFT_LEG_END_TRANSFORMS = [rotation4D(Z_AXIS, pi), rotation4D(Y_AXIS, -pi / 2), translation4D(0.0, 0.0, -FOOT_HEIGHT)]
RIGHT_LEG_END_TRANSFORMS = [rotation4D(Z_AXIS, pi), rotation4D(Y_AXIS, -pi / 2), translation4D(0.0, 0.0, -FOOT_HEIGHT)]
RIGHT_ARM_END_TRANSFORMS = [rotation4D(Z_AXIS, -pi / 2), translation4D(LOWER_ARM_LENGTH, 0.0, 0.0)]
END_TRANSFORMS = [