def test_model_reform(self): ks = ArticulationModel() a = Position('a') b = Position('b') c = Position('c') parent_pose_a = frame3_axis_angle(vector3(0,1,0), a, point3(0, b, 5)) parent_pose_b = frame3_axis_angle(vector3(1,0,0), b, point3(7* b, 0, 5)) joint_transform = translation3(7, -5, 33) axis = vector3(1, -3, 7) axis = axis position = c child_pose_a = parent_pose_a * joint_transform * translation3(*(axis[:,:3] * position)) child_pose_b = parent_pose_b * joint_transform * translation3(*(axis[:,:3] * position)) ks.apply_operation('create parent', CreateComplexObject(Path('parent'), KinematicLink('', parent_pose_a))) 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_a) ks.apply_operation('create parent', CreateComplexObject(Path('parent'), KinematicLink('', parent_pose_b))) ks.clean_structure() self.assertTrue(ks.has_data('fixed_joint')) self.assertEquals(ks.get_data('child/pose'), child_pose_b)
def test_double_reload(self): km = ArticulationModel() urdf_model = URDF.from_xml_file(res_pkg_path('package://kineverse/urdf/testbot.urdf')) load_urdf(km, Path(urdf_model.name), urdf_model) km.clean_structure() eef_frame_1 = km.get_data(Path(urdf_model.name) + Path('links/gripper_link/pose')) load_urdf(km, Path(urdf_model.name), urdf_model) km.clean_structure() eef_frame_2 = km.get_data(Path(urdf_model.name) + Path('links/gripper_link/pose')) self.assertEquals(eef_frame_1, eef_frame_2)
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)
tf_broadcaster = tf.TransformBroadcaster() 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')