def add_chain_joints(self, root_link, tip_link):
"""
Returns a dict with joint names as keys and sympy symbols
as values for all 1-dof movable robot joints in URDF between
ROOT_LINK and TIP_LINK.
:param root_link: str, denoting the root of the kin. chain
:param tip_link: str, denoting the tip of the kin. chain
:return: dict{str, sympy.Symbol}, with symbols for all joints in chain
"""
jointsAndLinks = self.urdf_robot.get_chain(root_link, tip_link, True, True, True)
parentFrame = self.frames[root_link]
for i in range(1, len(jointsAndLinks), 2):
joint_name = jointsAndLinks[i]
link_name = jointsAndLinks[i + 1]
joint = self.urdf_robot.joint_map[joint_name]
rpy = (0,0,0)
xyz = (0,0,0)
axis = (1,0,0)
if joint_name not in self._joints:
if joint.origin is not None:
rpy = joint.origin.rpy if joint.origin.rpy is not None else (0,0,0)
xyz = joint.origin.xyz if joint.origin.xyz is not None else (0,0,0)
if joint.axis != None:
axis = (joint.axis[0], joint.axis[1], joint.axis[2])
offset_frame = spw.frame3_rpy(*rpy, loc=spw.point3(*xyz))
if joint.type == 'revolute' or joint.type == 'continuous':
self._joints[joint_name] = Joint(spw.Symbol(joint_name),
joint.limit.velocity,
joint.limit.lower,
joint.limit.upper,
joint.type == 'continuous')
self.frames[link_name] = parentFrame * offset_frame * spw.frame3_axis_angle(spw.vec3(*axis),
spw.Symbol(joint_name),
spw.point3(0,0,0))
elif joint.type == 'prismatic':
self._joints[joint_name] = Joint(spw.Symbol(joint_name),
joint.limit.velocity,
joint.limit.lower,
joint.limit.upper,
False)
self.frames[link_name] = parentFrame * spw.frame3_rpy(*rpy,
loc=spw.point3(*xyz) + spw.vec3(
*axis) * spw.Symbol(joint_name))
elif joint.type == 'fixed':
self.frames[link_name] = parentFrame * spw.frame3_rpy(*rpy, loc=spw.point3(*xyz))
else:
raise Exception('Joint type "' + joint.type + '" is not supported by urdf parser.')
parentFrame = self.frames[link_name]
def get_expression(self):
return spw.point3(*self._symbol_map.values())
def get_position(self):
return spw.point3(*self._symbol_map.values()[4:])
def get_expression(self):
return spw.frame3_quaternion(
*(self._symbol_map.values()[:4] +
[spw.point3(*self._symbol_map.values()[4:])]))
def test_point3(self, v):
r1 = spw.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 spw.frame3_rpy(*(self._symbol_map.values()[:3] +
[spw.point3(*self._symbol_map.values()[3:])]))