def fk_a_in_b(ks, frame_a, frame_b):
tf_chain_a = collect_chain(ks, frame_a)
tf_chain_b = collect_chain(ks, frame_b)
if tf_chain_a[0] != tf_chain_b[0] and tf_chain_a[0].parent != tf_chain_b[0].parent:
raise Exception('Frames "{}" and "{}" have no common root!'.format(frame_a, frame_b))
if frame_b in tf_chain_a:
out = se.eye(4)
for x in range(tf_chain_a.index(frame_b) + 1, len(tf_chain_a)):
out *= tf_chain_a[x].to_parent
return out
elif frame_a in tf_chain_b:
out = se.eye(4)
for x in range(tf_chain_b.index(frame_a) + 1, len(tf_chain_b)):
out *= tf_chain_b[x].to_parent
return inverse_frame(out)
else:
x = 0
while tf_chain_a[x] == tf_chain_b[x]:
x += 1
a_in_root = se.eye(4)
for y in range(x, len(tf_chain_a)):
a_in_root *= tf_chain_a[y].to_parent
b_in_root = se.eye(4)
for y in range(x, len(tf_chain_b)):
b_in_root *= tf_chain_b[y].to_parent
return inverse_frame(b_in_root) * a_in_root
def load_urdf(ks, prefix, urdf, reference_frame='world'):
if type(prefix) == str:
prefix = Path(prefix)
ks.apply_operation('create {}'.format(str(prefix)), CreateComplexObject(prefix, URDFRobot(urdf.name)))
for u_link in urdf.links:
link_path = prefix + Path(['links', u_link.name])
collision = None
geometry = None
inertial = InertialData(link_path, se.eye(4))
if hasattr(u_link, 'collisions') and u_link.collisions is not None and len(u_link.collisions) > 0:
collision = {}
for x, c in enumerate(u_link.collisions):
collision[x] = Geometry(str(link_path), urdf_origin_to_transform(c.origin), '')
urdf_to_geometry(c.geometry, collision[x])
elif u_link.collision is not None:
collision = {'0': Geometry(str(link_path), urdf_origin_to_transform(u_link.collision.origin), '')}
urdf_to_geometry(u_link.collision.geometry, collision.values()[0])
if hasattr(u_link, 'visuals') and u_link.visuals is not None and len(u_link.visuals) > 0:
geometry = {}
for x, v in enumerate(u_link.visuals):
geometry[x] = Geometry(str(link_path), urdf_origin_to_transform(v.origin), '')
urdf_to_geometry(v.geometry, geometry[x])
elif u_link.visual is not None:
geometry = {'0': Geometry(str(link_path), urdf_origin_to_transform(u_link.visual.origin), '')}
urdf_to_geometry(u_link.visual.geometry, geometry.values()[0])
if u_link.inertial is not None:
if u_link.inertial.origin is not None:
inertial.pose = urdf_origin_to_transform(u_link.inertial.origin)
inertial.to_parent = inertial.pose
inertial.mass = u_link.inertial.mass
uin = u_link.inertial.inertia # Just for readability
inertial.inertia_matrix = matrix_wrapper([[uin.ixx, uin.ixy, uin.ixz],
[uin.ixy, uin.iyy, uin.iyz],
[uin.ixz, uin.iyz, uin.izz]])
ks.apply_operation('create {}'.format(str(prefix + Path(u_link.name))),
CreateComplexObject(link_path,
KinematicLink(reference_frame, urdf_origin_to_transform(u_link.origin), None, geometry, collision, inertial)))
links_left = [urdf.get_root()]
joint_set = set()
while len(links_left) > 0:
n_plink = links_left[0]
links_left = links_left[1:]
u_children = urdf.child_map[n_plink]
for n_joint, n_link in u_children:
# Create transformation operation modeling joint
u_joint = urdf.joint_map[n_joint]
if u_joint.mimic is not None:
multiplier = u_joint.mimic.multiplier
offset = u_joint.mimic.offset
position = prefix + ('joints', u_joint.mimic.joint, 'position')
else:
position = create_pos((prefix + (u_joint.name, )).to_symbol())
multiplier = None
offset = None
lower_limit = -1e9
upper_limit = 1e9
vel_limit = 1e9
if u_joint.limit is not None:
lower_limit = u_joint.limit.lower if u_joint.limit.lower is not None else -1e9
upper_limit = u_joint.limit.upper if u_joint.limit.upper is not None else 1e9
vel_limit = u_joint.limit.velocity
if u_joint.type == 'fixed':
op = SetFixedJoint(prefix + ('links', u_joint.parent, 'pose'),
prefix + ('links', u_joint.child, 'pose'),
prefix + ('joints', u_joint.name),
urdf_origin_to_transform(u_joint.origin))
elif u_joint.type == 'prismatic':
op = SetPrismaticJoint(prefix + ('links', u_joint.parent, 'pose'),
prefix + ('links', u_joint.child, 'pose'),
prefix + ('joints', u_joint.name),
urdf_origin_to_transform(u_joint.origin),
urdf_axis_to_vector(u_joint.axis),
position,
lower_limit,
upper_limit,
vel_limit,
multiplier,
offset)
elif u_joint.type == 'continuous':
op = SetContinuousJoint(prefix + ('links', u_joint.parent, 'pose'),
prefix + ('links', u_joint.child, 'pose'),
prefix + ('joints', u_joint.name),
urdf_origin_to_transform(u_joint.origin),
urdf_axis_to_vector(u_joint.axis),
position,
vel_limit,
multiplier,
offset)
elif u_joint.type == 'revolute':
op = SetRevoluteJoint(prefix + ('links', u_joint.parent, 'pose'),
prefix + ('links', u_joint.child, 'pose'),
prefix + ('joints', u_joint.name),
urdf_origin_to_transform(u_joint.origin),
urdf_axis_to_vector(u_joint.axis),
position,
lower_limit,
upper_limit,
vel_limit,
multiplier,
offset)
else:
raise Exception('Joint type "{}" is currently not covered.'.format(u_joint.type))
ks.apply_operation('connect {} {}'.format(str(prefix + ('links', u_joint.parent)), str(prefix + ('links', u_joint.child))), op)
joint_set.add(n_joint)
if n_link in urdf.child_map:
links_left.append(n_link)
def urdf_origin_to_transform(origin):
if origin is not None:
xyz = origin.xyz if origin.xyz is not None else [0.0,0.0,0.0]
rpy = origin.rpy if origin.rpy is not None else [0.0,0.0,0.0]
return translation3(*xyz) * rotation3_rpy(*rpy)
return se.eye(4)
def test_revolute_and_continuous_joint(self):
ks = ArticulationModel()
a = Position('a')
b = Position('b')
c = Position('c')
parent_pose = frame3_axis_angle(vector3(0,1,0), a, point3(0, b, 5))
joint_transform = translation3(7, -5, 33)
axis = vector3(1, -3, 7)
axis = axis / norm(axis)
position = c
child_pose = parent_pose * joint_transform * rotation3_axis_angle(axis, position)
ks.apply_operation('create parent', CreateComplexObject(Path('parent'), KinematicLink('', parent_pose)))
ks.apply_operation('create child', CreateComplexObject(Path('child'), KinematicLink('', se.eye(4))))
self.assertTrue(ks.has_data('parent/pose'))
self.assertTrue(ks.has_data('child/pose'))
ks.apply_operation('connect parent child',
SetRevoluteJoint(Path('parent/pose'),
Path('child/pose'),
Path('fixed_joint'),
joint_transform,
axis,
position,
-1, 2, 0.5))
self.assertTrue(ks.has_data('fixed_joint'))
self.assertEquals(ks.get_data('child/pose'), child_pose)
ks.remove_operation('connect parent child')
ks.apply_operation('connect parent child',
SetContinuousJoint(Path('parent/pose'),
Path('child/pose'),
Path('fixed_joint'),
joint_transform,
axis,
position, 0.5))
def test_prismatic_joint(self):
ks = ArticulationModel()
a = Position('a')
b = Position('b')
c = Position('c')
parent_pose = frame3_axis_angle(vector3(0,1,0), a, point3(0, b, 5))
joint_transform = translation3(7, -5, 33)
axis = vector3(1, -3, 7)
position = c
child_pose = parent_pose * joint_transform * translation3(*(axis[:,:3] * position))
ks.apply_operation('create parent', CreateComplexObject(Path('parent'), KinematicLink('', parent_pose)))
ks.apply_operation('create child', CreateComplexObject(Path('child'), KinematicLink('', se.eye(4))))
self.assertTrue(ks.has_data('parent/pose'))
self.assertTrue(ks.has_data('child/pose'))
ks.apply_operation('connect parent child',
SetPrismaticJoint(Path('parent/pose'),
Path('child/pose'),
Path('fixed_joint'),
joint_transform,
axis,
position,
-1, 2, 0.5))
self.assertTrue(ks.has_data('fixed_joint'))
self.assertEquals(ks.get_data('child/pose'), child_pose)
def test_fixed_joint(self):
ks = ArticulationModel()
a = Position('a')
b = Position('b')
parent_pose = frame3_axis_angle(vector3(0,1,0), a, point3(0, b, 5))
joint_transform = translation3(7, -5, 33)
child_pose = parent_pose * joint_transform
ks.apply_operation('create parent', CreateComplexObject(Path('parent'), KinematicLink('', parent_pose)))
ks.apply_operation('create child', CreateComplexObject(Path('child'), KinematicLink('', se.eye(4))))
self.assertTrue(ks.has_data('parent/pose'))
self.assertTrue(ks.has_data('child/pose'))
ks.apply_operation('connect parent child', SetFixedJoint(Path('parent/pose'), Path('child/pose'), Path('fixed_joint'), joint_transform))
self.assertTrue(ks.has_data('fixed_joint'))
self.assertEquals(ks.get_data('child/pose'), child_pose)
def __init__(self, parent_path, pose=None, to_parent=None):
self.parent = parent_path
self.pose = pose if pose is not None else se.eye(4)
self.to_parent = to_parent if to_parent is not None else self.pose