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_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 track(self, model_path, alias):
with self.lock:
if type(model_path) is not str:
model_path = str(model_path)
start_tick = len(self.tracked_poses) == 0 and self._use_timer
if model_path not in self.tracked_poses:
syms = [
Position(Path(model_path) + (x, ))
for x in ['x', 'y', 'z', 'qx', 'qy', 'qz', 'qw']
]
def rf(msg, state):
state[syms[0]] = msg.position.x
state[syms[1]] = msg.position.y
state[syms[2]] = msg.position.z
state[syms[3]] = msg.orientation.x
state[syms[4]] = msg.orientation.y
state[syms[5]] = msg.orientation.z
state[syms[6]] = msg.orientation.w
def process_model_update(data):
self._generate_pose_constraints(model_path, data)
axis = vector3(*syms[:3])
self.aliases[alias] = model_path
self.tracked_poses[model_path] = TrackerEntry(
frame3_quaternion(*syms), rf, process_model_update)
self._unintialized_poses.add(model_path)
if type(self.km_client) == ModelClient:
self.km_client.register_on_model_changed(
Path(model_path), process_model_update)
else:
process_model_update(self.km_client.get_data(model_path))
if start_tick:
self.timer = rospy.Timer(rospy.Duration(1.0 / 50),
self.cb_tick)
mesh='package://kineverse_experiment_world/urdf/faucet_head.obj')
rb_head = RigidBody(Path('world'),
cm.eye(4),
geometry={0: geom_head},
collision={0: geom_head})
geom_base = Geometry(
Path('base'),
cm.eye(4),
'mesh',
mesh='package://kineverse_experiment_world/urdf/faucet_base.obj')
rb_base = RigidBody(Path('world'),
cm.eye(4),
geometry={0: geom_base},
collision={0: geom_base})
km.apply_operation('create base', OP_CCO(Path('base'), rb_base))
km.apply_operation('create head', OP_CCO(Path('head'), rb_head))
km.apply_operation(
'connect base head',
OP_SBJ(Path('base/pose'), Path('head/pose'), Path('ball_joint'),
translation3(0.006, 0, 0.118), Position('axis_x'),
Position('axis_y'), Position('axis_z'), 0.2, 1.2))
km.clean_structure()
km.dispatch_events()
with open('faucet.json', 'w') as f:
km.save_to_file(f)
def P(n, x, i):
return Position('{}_{}_{}'.format(n, x, i))