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_add_fn_call(self): ks = ArticulationModel() ks.apply_operation('create my_var', CreateSingleValue('my_var', 5)) ks.apply_operation('create vec_a', CreateSingleValue('vec_a', vector3(1,0,0))) ks.apply_operation('create vec_b', CreateSingleValue('vec_b', vector3(0,1,0))) with self.assertRaises(Exception): op = CallFunctionOperator('sin_of_my_var', sin) op = CallFunctionOperator('sin_of_my_var', sin, Path('my_var')) self.assertIn('expr', op.args_paths) self.assertEquals(op.args_paths['expr'], 'my_var') ks.apply_operation('compute sin of my_var', op) self.assertTrue(ks.has_data('sin_of_my_var')) self.assertEquals(ks.get_data('sin_of_my_var'), sin(5)) ks.remove_operation('compute sin of my_var') self.assertFalse(ks.has_data('sin_of_my_var')) with self.assertRaises(Exception): op = CallFunctionOperator('cross_of_a_b', cross, Path('vec_a')) op = CallFunctionOperator('cross_of_a_b', cross, Path('vec_a'), Path('vec_b')) self.assertIn('u', op.args_paths) self.assertEquals(op.args_paths['u'], 'vec_a') self.assertIn('v', op.args_paths) self.assertEquals(op.args_paths['v'], 'vec_b') ks.apply_operation('compute cross of vectors', op) self.assertTrue(ks.has_data('cross_of_a_b')) self.assertEquals(ks.get_data('cross_of_a_b'), vector3(0,0,1)) ks.remove_operation('compute cross of vectors') self.assertFalse(ks.has_data('cross_of_a_b'))
def test_add_object(self): ks = ArticulationModel() obj = bb(some_str='lol', some_scalar=7.5, some_subobj=bb(x=4, y=5, z=10)) op = CreateComplexObject(Path('my_obj'), obj) self.assertIn('path/some_str', op.mod_paths) self.assertIn('path/some_scalar', op.mod_paths) self.assertIn('path/some_subobj', op.mod_paths) self.assertIn('path/some_subobj/x', op.mod_paths) self.assertIn('path/some_subobj/y', op.mod_paths) self.assertIn('path/some_subobj/z', op.mod_paths) self.assertIn('path', op._root_set) self.assertEquals(op.mod_paths['path/some_str'], ('my_obj','some_str')) self.assertEquals(op.mod_paths['path/some_scalar'], ('my_obj','some_scalar')) self.assertEquals(op.mod_paths['path/some_subobj'], ('my_obj','some_subobj')) self.assertEquals(op.mod_paths['path/some_subobj/x'],('my_obj','some_subobj','x')) self.assertEquals(op.mod_paths['path/some_subobj/y'],('my_obj','some_subobj','y')) self.assertEquals(op.mod_paths['path/some_subobj/z'],('my_obj','some_subobj','z')) self.assertEquals(op._root_set['path'], ('my_obj')) ks.apply_operation('create my_obj', op) self.assertTrue(ks.has_data('my_obj')) self.assertEquals(ks.get_data('my_obj/some_str'), 'lol') self.assertEquals(ks.get_data('my_obj/some_scalar'), 7.5) self.assertEquals(ks.get_data('my_obj/some_subobj/x'), 4) self.assertEquals(ks.get_data('my_obj/some_subobj/y'), 5) self.assertEquals(ks.get_data('my_obj/some_subobj/z'), 10) ks.remove_operation('create my_obj') self.assertFalse(ks.has_data('my_obj')) self.assertFalse(ks.has_data('my_obj/some_scalar')) self.assertFalse(ks.has_data('my_obj/some_subobj')) self.assertFalse(ks.has_data('my_obj/some_subobj/x')) self.assertFalse(ks.has_data('my_obj/some_subobj/y')) self.assertFalse(ks.has_data('my_obj/some_subobj/z'))
def test_add_single(self): ks = ArticulationModel() p = Path('my_var') op = CreateSingleValue(p, 5) ks.apply_operation('create my_var', op) self.assertTrue(ks.has_data(p)) self.assertEquals(ks.get_data(p), 5) ks.remove_operation('create my_var') self.assertFalse(ks.has_data(p))
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_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_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))