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,