def _calc_mat(self, node):
mat = mat4.identity()
channels = node.channels
param_offset = self._bone.get_param_offset(node)
for i, channel in enumerate(channels):
mat *= self._mat_funcs[channel](self._frame[param_offset + i])
return mat
def _calc_mat(self, node):
mat = mat4.identity()
channels = node.channels
param_offset = self._bone.get_param_offset(node)
for i, channel in enumerate(channels):
mat *= self._mat_funcs[channel](self._frame[param_offset + i])
return mat
def __init__(self, bone, frame):
self._matrixes_global = []
self._matrixes_local = []
self._positions = []
self.__last_matrix = mat4.identity()
self._bone = bone
self._frame = frame
self._process_node(bone.root)
def __init__(self, bone, frame):
self._matrixes_global = []
self._matrixes_local = []
self._positions = []
self.__last_matrix = mat4.identity()
self._bone = bone
self._frame = frame
self._process_node(bone.root)
def test_calc_matrix(self):
n1 = Node()
n1.channels = ["Xposition", "Yposition", "Zposition"]
n2 = Node()
n2.channels = []
n3 = Node()
n3.channels = ["Xrotation", "Yrotation", "Zrotation"]
n1.children.append(n2)
n2.children.append(n3)
bone = BVHToolkit.Bone(n1)
pose = BVHToolkit.Pose(bone, [10, 20, 30, 90, 90, 90])
m1 = mat4.translation((10, 20, 30))
m2 = mat4.rotation(math.pi / 2, (1, 0, 0)) * \
mat4.rotation(math.pi / 2, (0, 1, 0)) * \
mat4.rotation(math.pi / 2, (0, 0, 1))
self.assertMat4Equal(pose._calc_mat(n1), m1)
self.assertMat4Equal(pose._calc_mat(n2), mat4.identity())
self.assertMat4Equal(pose._calc_mat(n3), m2)
def test_calc_matrix(self):
n1 = Node()
n1.channels = ["Xposition", "Yposition", "Zposition"]
n2 = Node()
n2.channels = []
n3 = Node()
n3.channels = ["Xrotation", "Yrotation", "Zrotation"]
n1.children.append(n2)
n2.children.append(n3)
bone = BVHToolkit.Bone(n1)
pose = BVHToolkit.Pose(bone, [10, 20, 30, 90, 90, 90])
m1 = mat4.translation((10, 20, 30))
m2 = (
mat4.rotation(math.pi / 2, (1, 0, 0))
* mat4.rotation(math.pi / 2, (0, 1, 0))
* mat4.rotation(math.pi / 2, (0, 0, 1))
)
self.assertMat4Equal(pose._calc_mat(n1), m1)
self.assertMat4Equal(pose._calc_mat(n2), mat4.identity())
self.assertMat4Equal(pose._calc_mat(n3), m2)
def getTransform(self, name, space, useSensorValues, joints=None):
# Utilize cache if available
if name in self.__cache[space]:
return self.__cache[space][name]
if not joints:
joints = dict(zip(self.joints, self.nao.angles))
mat = mat4.identity()
larm = ['LShoulderPitch', 'LShoulderRoll', 'LElbowYaw', 'LElbowRoll', 'LWristYaw', 'LArm']
rarm = ['RShoulderPitch', 'RShoulderRoll', 'RElbowYaw', 'RElbowRoll', 'RWristYaw', 'RArm']
lleg = ['LHipYawPitch', 'LHipRoll', 'LHipPitch', 'LKneePitch', 'LAnklePitch', 'LAnkleRoll', 'LLeg']
rleg = ['RHipYawPitch', 'RHipRoll', 'RHipPitch', 'RKneePitch', 'RAnklePitch', 'RAnkleRoll', 'RLeg']
if name in larm:
path = larm[:larm.index(name) + 1]
for joint in reversed(path):
if joint == "LShoulderPitch":
offset = vec3(0, self.geometry['ShoulderOffsetY'], self.geometry['ShoulderOffsetZ'])
mat.rotate(-joints['LShoulderPitch'], AXIS_Y)
mat.translate(-offset)
elif joint == "LShoulderRoll":
mat.rotate(-joints['LShoulderRoll'], AXIS_Z)
elif joint == "LElbowYaw":
mat.rotate(-joints['LElbowYaw'], AXIS_X)
offset = vec3(self.geometry['UpperArmLength'], self.geometry['ElbowOffsetY'], 0)
mat.translate(-offset)
elif joint == "LElbowRoll":
mat.rotate(-joints['LElbowRoll'], AXIS_Z)
elif joint == "LWristYaw":
mat.rotate(-joints['LWristYaw'], AXIS_X)
offset = vec3(self.geometry['LowerArmLength'], 0, 0)
mat.translate(-offset)
elif joint == "LHand" or name == "LArm":
offset = vec3(self.geometry['HandOffsetX'], 0, -self.geometry['HandOffsetZ'])
mat.translate(-offset)
elif name in rarm:
path = rarm[:rarm.index(name) + 1]
for joint in reversed(path):
if joint == "RShoulderPitch":
offset = vec3(0, -self.geometry['ShoulderOffsetY'], self.geometry['ShoulderOffsetZ'])
mat.rotate(-joints['RShoulderPitch'], AXIS_Y)
mat.translate(-offset)
elif joint == "RShoulderRoll":
mat.rotate(-joints['RShoulderRoll'], AXIS_Z)
elif joint == "RElbowYaw":
mat.rotate(-joints['RElbowYaw'], AXIS_X)
offset = vec3(self.geometry['UpperArmLength'], -self.geometry['ElbowOffsetY'], 0)
mat.translate(-offset)
elif joint == "RElbowRoll":
mat.rotate(-joints['RElbowRoll'], AXIS_Z)
elif joint == "RWristYaw":
mat.rotate(-joints['RWristYaw'], AXIS_X)
offset = vec3(self.geometry['LowerArmLength'], 0, 0)
mat.translate(-offset)
elif joint == "RHand" or name == "RArm":
offset = vec3(self.geometry['HandOffsetX'], 0, -self.geometry['HandOffsetZ'])
mat.translate(-offset)
elif name == "Torso":
pass # No translation
elif name == "Head":
offset = vec3(0, 0, self.geometry['NeckOffsetZ'])
mat.rotate(-joints['HeadPitch'], AXIS_Y)
mat.rotate(-joints['HeadYaw'], AXIS_Z)
mat.translate(-offset)
elif name == "CameraTop":
offset = vec3(self.geometry['CameraTopOffsetX'],
self.geometry['CameraTopOffsetY'],
self.geometry['CameraTopOffsetZ'])
mat.translate(-offset)
mat *= self.getTransform("Head", SPACE_TORSO, useSensorValues, joints)
elif name == "CameraBottom":
mat.rotate(-self.geometry['CameraBottomPitch'], AXIS_Y)
offset = vec3(self.geometry['CameraBottomOffsetX'],
self.geometry['CameraBottomOffsetY'],
self.geometry['CameraBottomOffsetZ'])
mat.translate(-offset)
mat *= self.getTransform("Head", SPACE_TORSO, useSensorValues, joints)
elif name in lleg:
path = lleg[:lleg.index(name) + 1]
for joint in reversed(path):
if joint == "LHipYawPitch":
hipYP = vec3(0, 1, -1).normalize()
mat.rotate(-joints['LHipYawPitch'], hipYP)
offset = vec3(0, self.geometry['HipOffsetY'], -self.geometry['HipOffsetZ'])
mat.translate(-offset)
elif joint == "LHipRoll":
mat.rotate(-joints['LHipRoll'], AXIS_X)
elif joint == "LHipPitch":
mat.rotate(-joints['LHipPitch'], AXIS_Y)
elif joint == "LKneePitch":
mat.rotate(-joints['LKneePitch'], AXIS_Y)
offset = vec3(0, 0, -self.geometry['ThighLength'])
mat.translate(-offset)
elif joint == "LAnklePitch":
mat.rotate(-joints['LAnklePitch'], AXIS_Y)
offset = vec3(0, 0, -self.geometry['TibiaLength'])
mat.translate(-offset)
elif joint == "LAnkleRoll":
mat.rotate(-joints['LAnkleRoll'], AXIS_X)
elif joint == "LLeg":
offset = vec3(0, 0, -self.geometry['FootHeight'])
mat.translate(-offset)
elif name in rleg:
path = rleg[:rleg.index(name) + 1]
for joint in reversed(path):
if joint == "RHipYawPitch":
hipYP = vec3(0, -1, -1).normalize()
mat.rotate(-joints['RHipYawPitch'], hipYP)
offset = vec3(0, -self.geometry['HipOffsetY'], -self.geometry['HipOffsetZ'])
mat.translate(-offset)
elif joint == "RHipRoll":
mat.rotate(-joints['RHipRoll'], AXIS_X)
elif joint == "RHipPitch":
mat.rotate(-joints['RHipPitch'], AXIS_Y)
elif joint == "RKneePitch":
mat.rotate(-joints['RKneePitch'], AXIS_Y)
offset = vec3(0, 0, -self.geometry['ThighLength'])
mat.translate(-offset)
elif joint == "RAnklePitch":
mat.rotate(-joints['RAnklePitch'], AXIS_Y)
offset = vec3(0, 0, -self.geometry['TibiaLength'])
mat.translate(-offset)
elif joint == "RAnkleRoll":
mat.rotate(-joints['RAnkleRoll'], AXIS_X)
elif joint == "RLeg":
offset = vec3(0, 0, -self.geometry['FootHeight'])
mat.translate(-offset)
else:
print "Unknown effector : `%s'" % name
self.__cache[SPACE_TORSO][name] = mat4(mat)
if space == SPACE_TORSO:
return mat
# SPACE_NAO: Need the position of the torso in Nao Space. The origin is
# located at the average of the positions of the two feet.
#
# CAUTION: this doesn't fully work. Origin might be calculated correctly
# calculated but the orientation is defined such that the X-axis must
# always look forward so with complicated leg configurations, the
# coordinate system might not be the exact average between the two
# orientations. Or something like that. I don't know, I give up ;-)
lleg = self.getPosition("LLeg", SPACE_TORSO, True)
rleg = self.getPosition("RLeg", SPACE_TORSO, True)
lpos = vec3(lleg[:3])
avg = list(numpy.average([lleg, rleg], 0))
offset = vec3(avg[:3])
# Setup transformation matrix
change = mat4.identity()
change.setMat3(mat3.fromEulerZYX(avg[5], avg[4], avg[3]))
mat.translate(offset)
mat *= change
self.__cache[SPACE_NAO][name] = mat4(mat)
if space == SPACE_NAO:
return mat
# SPACE_WORLD: The last option. This is the same as SPACE_NAO when
# naoqi starts, but when the NAO moves about, the origin is left behind.
#
# CAUTION: behavior not identical to naoqi (yet). Don't know what the
# problem is, but the rotation and position reported by a (simulated)
# naoqi are different from the values calculated here, although they do
# seem to make sense... Don't known, don't care, give up (for now).
# Probably naoqi has an estimate of the error when performing walks and
# incorporates that in the resulting odometry.
offset = vec3(self.nao.position)
mat.rotate(-self.nao.orientation, AXIS_Z)
mat.translate(-offset)
self.__cache[SPACE_WORLD][name] = mat4(mat)
return mat
def getTransform(self, name, space, useSensorValues, joints=None):
# Utilize cache if available
if name in self.__cache[space]:
return self.__cache[space][name]
if not joints:
joints = dict(zip(self.joints, self.nao.angles))
mat = mat4.identity()
larm = [
'LShoulderPitch', 'LShoulderRoll', 'LElbowYaw', 'LElbowRoll',
'LWristYaw', 'LArm'
]
rarm = [
'RShoulderPitch', 'RShoulderRoll', 'RElbowYaw', 'RElbowRoll',
'RWristYaw', 'RArm'
]
lleg = [
'LHipYawPitch', 'LHipRoll', 'LHipPitch', 'LKneePitch',
'LAnklePitch', 'LAnkleRoll', 'LLeg'
]
rleg = [
'RHipYawPitch', 'RHipRoll', 'RHipPitch', 'RKneePitch',
'RAnklePitch', 'RAnkleRoll', 'RLeg'
]
if name in larm:
path = larm[:larm.index(name) + 1]
for joint in reversed(path):
if joint == "LShoulderPitch":
offset = vec3(0, self.geometry['ShoulderOffsetY'],
self.geometry['ShoulderOffsetZ'])
mat.rotate(-joints['LShoulderPitch'], AXIS_Y)
mat.translate(-offset)
elif joint == "LShoulderRoll":
mat.rotate(-joints['LShoulderRoll'], AXIS_Z)
elif joint == "LElbowYaw":
mat.rotate(-joints['LElbowYaw'], AXIS_X)
offset = vec3(self.geometry['UpperArmLength'],
self.geometry['ElbowOffsetY'], 0)
mat.translate(-offset)
elif joint == "LElbowRoll":
mat.rotate(-joints['LElbowRoll'], AXIS_Z)
elif joint == "LWristYaw":
mat.rotate(-joints['LWristYaw'], AXIS_X)
offset = vec3(self.geometry['LowerArmLength'], 0, 0)
mat.translate(-offset)
elif joint == "LHand" or name == "LArm":
offset = vec3(self.geometry['HandOffsetX'], 0,
-self.geometry['HandOffsetZ'])
mat.translate(-offset)
elif name in rarm:
path = rarm[:rarm.index(name) + 1]
for joint in reversed(path):
if joint == "RShoulderPitch":
offset = vec3(0, -self.geometry['ShoulderOffsetY'],
self.geometry['ShoulderOffsetZ'])
mat.rotate(-joints['RShoulderPitch'], AXIS_Y)
mat.translate(-offset)
elif joint == "RShoulderRoll":
mat.rotate(-joints['RShoulderRoll'], AXIS_Z)
elif joint == "RElbowYaw":
mat.rotate(-joints['RElbowYaw'], AXIS_X)
offset = vec3(self.geometry['UpperArmLength'],
-self.geometry['ElbowOffsetY'], 0)
mat.translate(-offset)
elif joint == "RElbowRoll":
mat.rotate(-joints['RElbowRoll'], AXIS_Z)
elif joint == "RWristYaw":
mat.rotate(-joints['RWristYaw'], AXIS_X)
offset = vec3(self.geometry['LowerArmLength'], 0, 0)
mat.translate(-offset)
elif joint == "RHand" or name == "RArm":
offset = vec3(self.geometry['HandOffsetX'], 0,
-self.geometry['HandOffsetZ'])
mat.translate(-offset)
elif name == "Torso":
pass # No translation
elif name == "Head":
offset = vec3(0, 0, self.geometry['NeckOffsetZ'])
mat.rotate(-joints['HeadPitch'], AXIS_Y)
mat.rotate(-joints['HeadYaw'], AXIS_Z)
mat.translate(-offset)
elif name == "CameraTop":
offset = vec3(self.geometry['CameraTopOffsetX'],
self.geometry['CameraTopOffsetY'],
self.geometry['CameraTopOffsetZ'])
mat.translate(-offset)
mat *= self.getTransform("Head", SPACE_TORSO, useSensorValues,
joints)
elif name == "CameraBottom":
mat.rotate(-self.geometry['CameraBottomPitch'], AXIS_Y)
offset = vec3(self.geometry['CameraBottomOffsetX'],
self.geometry['CameraBottomOffsetY'],
self.geometry['CameraBottomOffsetZ'])
mat.translate(-offset)
mat *= self.getTransform("Head", SPACE_TORSO, useSensorValues,
joints)
elif name in lleg:
path = lleg[:lleg.index(name) + 1]
for joint in reversed(path):
if joint == "LHipYawPitch":
hipYP = vec3(0, 1, -1).normalize()
mat.rotate(-joints['LHipYawPitch'], hipYP)
offset = vec3(0, self.geometry['HipOffsetY'],
-self.geometry['HipOffsetZ'])
mat.translate(-offset)
elif joint == "LHipRoll":
mat.rotate(-joints['LHipRoll'], AXIS_X)
elif joint == "LHipPitch":
mat.rotate(-joints['LHipPitch'], AXIS_Y)
elif joint == "LKneePitch":
mat.rotate(-joints['LKneePitch'], AXIS_Y)
offset = vec3(0, 0, -self.geometry['ThighLength'])
mat.translate(-offset)
elif joint == "LAnklePitch":
mat.rotate(-joints['LAnklePitch'], AXIS_Y)
offset = vec3(0, 0, -self.geometry['TibiaLength'])
mat.translate(-offset)
elif joint == "LAnkleRoll":
mat.rotate(-joints['LAnkleRoll'], AXIS_X)
elif joint == "LLeg":
offset = vec3(0, 0, -self.geometry['FootHeight'])
mat.translate(-offset)
elif name in rleg:
path = rleg[:rleg.index(name) + 1]
for joint in reversed(path):
if joint == "RHipYawPitch":
hipYP = vec3(0, -1, -1).normalize()
mat.rotate(-joints['RHipYawPitch'], hipYP)
offset = vec3(0, -self.geometry['HipOffsetY'],
-self.geometry['HipOffsetZ'])
mat.translate(-offset)
elif joint == "RHipRoll":
mat.rotate(-joints['RHipRoll'], AXIS_X)
elif joint == "RHipPitch":
mat.rotate(-joints['RHipPitch'], AXIS_Y)
elif joint == "RKneePitch":
mat.rotate(-joints['RKneePitch'], AXIS_Y)
offset = vec3(0, 0, -self.geometry['ThighLength'])
mat.translate(-offset)
elif joint == "RAnklePitch":
mat.rotate(-joints['RAnklePitch'], AXIS_Y)
offset = vec3(0, 0, -self.geometry['TibiaLength'])
mat.translate(-offset)
elif joint == "RAnkleRoll":
mat.rotate(-joints['RAnkleRoll'], AXIS_X)
elif joint == "RLeg":
offset = vec3(0, 0, -self.geometry['FootHeight'])
mat.translate(-offset)
else:
print "Unknown effector : `%s'" % name
self.__cache[SPACE_TORSO][name] = mat4(mat)
if space == SPACE_TORSO:
return mat
# SPACE_NAO: Need the position of the torso in Nao Space. The origin is
# located at the average of the positions of the two feet.
#
# CAUTION: this doesn't fully work. Origin might be calculated correctly
# calculated but the orientation is defined such that the X-axis must
# always look forward so with complicated leg configurations, the
# coordinate system might not be the exact average between the two
# orientations. Or something like that. I don't know, I give up ;-)
lleg = self.getPosition("LLeg", SPACE_TORSO, True)
rleg = self.getPosition("RLeg", SPACE_TORSO, True)
lpos = vec3(lleg[:3])
avg = list(numpy.average([lleg, rleg], 0))
offset = vec3(avg[:3])
# Setup transformation matrix
change = mat4.identity()
change.setMat3(mat3.fromEulerZYX(avg[5], avg[4], avg[3]))
mat.translate(offset)
mat *= change
self.__cache[SPACE_NAO][name] = mat4(mat)
if space == SPACE_NAO:
return mat
# SPACE_WORLD: The last option. This is the same as SPACE_NAO when
# naoqi starts, but when the NAO moves about, the origin is left behind.
#
# CAUTION: behavior not identical to naoqi (yet). Don't know what the
# problem is, but the rotation and position reported by a (simulated)
# naoqi are different from the values calculated here, although they do
# seem to make sense... Don't known, don't care, give up (for now).
# Probably naoqi has an estimate of the error when performing walks and
# incorporates that in the resulting odometry.
offset = vec3(self.nao.position)
mat.rotate(-self.nao.orientation, AXIS_Z)
mat.translate(-offset)
self.__cache[SPACE_WORLD][name] = mat4(mat)
return mat
