def test_attach_detach_existing_obj_to_robot1(self, function_setup):
obj_name = u'box'
world_with_pr2 = self.make_world_with_pr2()
box = self.cls.from_world_body(make_world_body_box(name=obj_name))
world_with_pr2.add_object(box)
links_before = set(world_with_pr2.robot.get_link_names())
joints_before = set(world_with_pr2.robot.get_joint_names())
p = Pose()
p.orientation.w = 1
world_with_pr2.attach_existing_obj_to_robot(obj_name,
u'l_gripper_tool_frame', p)
assert obj_name not in world_with_pr2.get_object_names()
assert set(world_with_pr2.robot.get_link_names()).difference(
links_before) == {obj_name}
assert set(world_with_pr2.robot.get_joint_names()).difference(
joints_before) == {obj_name}
world_with_pr2.detach(obj_name)
assert set(world_with_pr2.robot.get_link_names()).symmetric_difference(
links_before) == set()
assert set(
world_with_pr2.robot.get_joint_names()).symmetric_difference(
joints_before) == set()
assert obj_name in world_with_pr2.get_object_names()
compare_poses(
world_with_pr2.robot.get_fk_pose(world_with_pr2.robot.get_root(),
u'l_gripper_tool_frame').pose,
world_with_pr2.get_object(obj_name).base_pose)
return world_with_pr2
def test_boxy_fk1(self, parsed_boxy, js):
kdl = KDL(boxy_urdf())
root = u'base_footprint'
tips = [u'left_gripper_tool_frame', u'right_gripper_tool_frame']
for tip in tips:
kdl_r = kdl.get_robot(root, tip)
kdl_fk = kdl_to_pose(kdl_r.fk(js))
mjs = {}
for joint_name, position in js.items():
mjs[joint_name] = SingleJointState(joint_name, position)
parsed_boxy.joint_state = mjs
symengine_fk = parsed_boxy.get_fk_pose(root, tip).pose
compare_poses(kdl_fk, symengine_fk)
def test_pr2_fk1(self, parsed_pr2, js):
"""
:type parsed_pr2: Robot
:type js:
:return:
"""
kdl = KDL(pr2_urdf())
root = u'base_link'
tips = [u'l_gripper_tool_frame', u'r_gripper_tool_frame']
for tip in tips:
kdl_r = kdl.get_robot(root, tip)
kdl_fk = kdl_to_pose(kdl_r.fk(js))
mjs = {}
for joint_name, position in js.items():
mjs[joint_name] = SingleJointState(joint_name, position)
parsed_pr2.joint_state = mjs
symengine_fk = parsed_pr2.get_fk_pose(root, tip).pose
compare_poses(kdl_fk, symengine_fk)
def test_avoid_self_collision2(self, self_collision_pose):
self_collision_pose.send_and_check_goal()
map_T_root = lookup_pose(u'map', u'base_footprint')
expected_pose = Pose()
expected_pose.orientation.w = 1
compare_poses(map_T_root.pose, expected_pose)