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')