def test_model_eq(self):
urdf_model = URDF.from_xml_file(res_pkg_path('package://kineverse/urdf/testbot.urdf'))
ks1 = ArticulationModel()
ks2 = ArticulationModel()
load_urdf(ks1, Path(urdf_model.name), urdf_model)
load_urdf(ks2, Path(urdf_model.name), urdf_model)
self.assertEquals(ks1, ks2)
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_fk_a_in_b(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_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.set_data('a', f_a)
km.set_data('b', f_b)
km.set_data('c', f_c)
km.set_data('d', f_d)
km.set_data('e', f_e)
km.set_data('f', f_f)
km.set_data('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)
self.assertEquals(d_in_b, f_c.to_parent * f_d.to_parent)
self.assertEquals(b_in_d, inverse_frame(f_c.to_parent * f_d.to_parent))
self.assertEquals(
d_in_e,
inverse_frame(f_e.to_parent) * f_c.to_parent * f_d.to_parent)
self.assertEquals(d_in_f, inverse_frame(f_f.pose) * f_d.pose)
with self.assertRaises(Exception):
fk_a_in_b(km, f_d, f_g)
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_collect_chain(self):
km = ArticulationModel()
f_a = Frame('world', None)
f_b = Frame('a', None)
f_c = Frame('b', None)
f_d = Frame('c', None)
f_e = Frame('b', None)
km.set_data('a', f_a)
km.set_data('b', f_b)
km.set_data('c', f_c)
km.set_data('d', f_d)
km.set_data('e', f_e)
chain = collect_chain(km, f_d)
self.assertEquals(chain, [f_a, f_b, f_c, f_d])
traj_pup = rospy.Publisher('/fetch/commands/joint_trajectory',
JointTrajectoryMsg,
queue_size=1)
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'),
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 test_load(self):
urdf_model = URDF.from_xml_file(res_pkg_path('package://kineverse/urdf/testbot.urdf'))
ks = ArticulationModel()
load_urdf(ks, Path(urdf_model.name), urdf_model)
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_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)