def __init__(self, world):
self.cameraToWorldFrame = identity()
self.focalPointInWorldFrame = identity()
self.comHeight = 0 # MM cause that's the way they do it.
self.cameraToHorizonFrame = identity()
self.horizonSlope = 0.0
self.horizonLeft = [0.0, 0.0]
self.horizonRight = [0.0, 0.0]
self._inclination_vars = [
"Device/SubDeviceList/InertialSensor/AngleX/Sensor/Value",
"Device/SubDeviceList/InertialSensor/AngleY/Sensor/Value",
]
self._inclination = [0.0, 0.0]
self.updateTransforms([0.0] * 26, [0.0, 0.0]) # init stuff
self._connecting_to_webots = burst.connecting_to_webots()
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