def _create_frames_expressions(self):
for joint_name, urdf_joint in self._urdf_robot.joint_map.items():
if self.is_joint_controllable(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 = spw.translation3(*xyz) * spw.rotation_matrix_from_rpy(*rpy)
else:
joint_frame = spw.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_rotational_joint(joint_name):
joint_frame *= spw.rotation_matrix_from_axis_angle(spw.vector3(*urdf_joint.axis), joint_symbol)
elif self.is_translational_joint(joint_name):
joint_frame *= spw.translation3(*(spw.point3(*urdf_joint.axis) * joint_symbol)[:3])
self._joint_to_frame[joint_name] = joint_frame
def get_expression(self):
return sw.point3(self.x, self.y, self.z)
def get_position(self):
return sw.point3(self.x, self.y, self.z)