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_create_relative_frame_and_transform(self): km = ArticulationModel() p1 = translation3(1, -3, 5) axis1 = vector3(1, -2, 5) axis1 /= norm(axis1) p2 = rotation3_axis_angle(axis1, 1.67) p3 = translation3(-7, 1, 0) axis2 = vector3(5, 0, 3) axis2 /= norm(axis2) p4 = rotation3_axis_angle(axis2, -0.6) p5 = translation3(0, 0, 4) p6 = frame3_rpy(1.2, -0.3, 0.67, [1, 2, 3]) f_a = Frame('world', p1, p1) f_b = Frame('a', f_a.pose * p2, p2) f_c = Frame('b', f_b.pose * p3, p3) f_c2 = Frame('b', f_b.pose * p6, p6) f_d = Frame('c', f_c.pose * p4, p4) f_e = Frame('b', f_b.pose * p5, p5) f_f = Frame('world', p6, p6) f_g = Frame('lol', se.eye(4)) km.apply_operation('create a', CreateComplexObject(Path('a'), f_a)) km.apply_operation('create b', CreateRelativeFrame(Path('b'), f_b)) km.apply_operation('create c', CreateRelativeFrame(Path('c'), f_c)) km.apply_operation('create d', CreateRelativeFrame(Path('d'), f_d)) km.apply_operation('create e', CreateRelativeFrame(Path('e'), f_e)) km.apply_operation('create f', CreateComplexObject(Path('f'), f_f)) km.apply_operation('create g', CreateComplexObject(Path('g'), f_g)) d_in_b = fk_a_in_b(km, f_d, f_b) b_in_d = fk_a_in_b(km, f_b, f_d) d_in_e = fk_a_in_b(km, f_d, f_e) d_in_f = fk_a_in_b(km, f_d, f_f) km.apply_operation( 'tf d to e', CreateRelativeTransform(Path('d_to_e'), Path('d'), Path('e'))) self.assertTrue(km.has_data('d_to_e')) tf = km.get_data('d_to_e') self.assertEquals(tf.from_frame, 'd') self.assertEquals(tf.to_frame, 'e') self.assertEquals(tf.pose, d_in_e) km.apply_operation('create c', CreateRelativeFrame(Path('c'), f_c2)) km.clean_structure() tf = km.get_data('d_to_e') self.assertEquals( tf.pose, inverse_frame(f_e.to_parent) * f_c2.to_parent * f_d.to_parent)
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 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))
urdf_model = URDF.from_xml_string(urdf_str) # ROBOT STATE PUBLISHER # sp_p = subprocess.Popen([pub_path, # '__name:={}_state_publisher'.format(urdf_model.name), # 'robot_description:=/robot_description', # '_tf_prefix:={}'.format(urdf_model.name), # 'joint_states:=/joint_states']) # KINEMATIC MODEL km = ArticulationModel() load_urdf(km, Path('fetch'), urdf_model) km.clean_structure() km.apply_operation_before('create world', 'create fetch', CreateComplexObject(Path('world'), Frame(''))) roomba_op = create_roomba_joint_with_symbols( Path('world/pose'), Path('fetch/links/base_link/pose'), Path('fetch/joints/to_world'), vector3(0, 0, 1), vector3(1, 0, 0), 1.0, 0.6, Path('fetch')) km.apply_operation_after('connect world base_link', 'create fetch/base_link', roomba_op) km.clean_structure() with open(res_pkg_path('package://kineverse/test/fetch.json'), 'w') as fetch_json: start = Time.now() print('Starting to write JSON') json.dump(km.get_data('fetch'), fetch_json) #, sort_keys=True, indent=' ')
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)