def _create_frames_expressions(self):
for joint_name, urdf_joint in self._urdf_robot.joint_map.items():
if self.is_joint_movable(joint_name):
joint_symbol = self.get_joint_position_symbol(joint_name)
if self.is_joint_mimic(joint_name):
multiplier = 1 if urdf_joint.mimic.multiplier is None else urdf_joint.mimic.multiplier
offset = 0 if urdf_joint.mimic.offset is None else urdf_joint.mimic.offset
joint_symbol = self.get_joint_position_symbol(
urdf_joint.mimic.joint) * multiplier + offset
if self.is_joint_type_supported(joint_name):
if urdf_joint.origin is not None:
xyz = urdf_joint.origin.xyz if urdf_joint.origin.xyz is not None else [
0, 0, 0
]
rpy = urdf_joint.origin.rpy if urdf_joint.origin.rpy is not None else [
0, 0, 0
]
joint_frame = w.dot(w.translation3(*xyz),
w.rotation_matrix_from_rpy(*rpy))
else:
joint_frame = w.eye(4)
else:
# TODO more specific exception
raise TypeError(
u'Joint type "{}" is not supported by urdfs parser.'.
format(urdf_joint.type))
if self.is_joint_rotational(joint_name):
joint_frame = w.dot(
joint_frame,
w.rotation_matrix_from_axis_angle(
w.vector3(*urdf_joint.axis), joint_symbol))
elif self.is_joint_prismatic(joint_name):
translation_axis = (w.point3(*urdf_joint.axis) * joint_symbol)
joint_frame = w.dot(
joint_frame,
w.translation3(translation_axis[0], translation_axis[1],
translation_axis[2]))
self._joint_to_frame[joint_name] = joint_frame
def test_point3(self, v):
r1 = w.point3(*v)
self.assertEqual(r1[0], v[0])
self.assertEqual(r1[1], v[1])
self.assertEqual(r1[2], v[2])
self.assertEqual(r1[3], 1)
def get_expression(self):
return w.point3(self.x, self.y, self.z)
def get_position(self):
return w.point3(self.x, self.y, self.z)