def test_get_controlled_leaf_joints(self, test_folder, delete_test_folder):
r = self.cls(pr2_urdf(), path_to_data_folder=test_folder)
r.controlled_joints = [u'torso_lift_joint',
u'r_upper_arm_roll_joint',
u'r_shoulder_pan_joint',
u'r_shoulder_lift_joint',
u'r_forearm_roll_joint',
u'r_elbow_flex_joint',
u'r_wrist_flex_joint',
u'r_wrist_roll_joint',
u'l_upper_arm_roll_joint',
u'l_shoulder_pan_joint',
u'l_shoulder_lift_joint',
u'l_forearm_roll_joint',
u'l_elbow_flex_joint',
u'l_wrist_flex_joint',
u'l_wrist_roll_joint',
u'head_pan_joint',
u'head_tilt_joint',
u'odom_x_joint',
u'odom_y_joint',
u'odom_z_joint']
r = r.get_controlled_leaf_joints()
assert r == {
'l_wrist_roll_joint', 'r_wrist_roll_joint', 'odom_z_joint', 'l_forearm_roll_joint', 'torso_lift_joint',
'head_tilt_joint', 'r_forearm_roll_joint'
}
def test_get_directly_controllable_collision_links(self, test_folder, delete_test_folder):
r = self.cls(pr2_urdf(), path_to_data_folder=test_folder)
r.controlled_joints = [u'torso_lift_joint',
u'r_upper_arm_roll_joint',
u'r_shoulder_pan_joint',
u'r_shoulder_lift_joint',
u'r_forearm_roll_joint',
u'r_elbow_flex_joint',
u'r_wrist_flex_joint',
u'r_wrist_roll_joint',
u'l_upper_arm_roll_joint',
u'l_shoulder_pan_joint',
u'l_shoulder_lift_joint',
u'l_forearm_roll_joint',
u'l_elbow_flex_joint',
u'l_wrist_flex_joint',
u'l_wrist_roll_joint',
u'head_pan_joint',
u'head_tilt_joint',
u'odom_x_joint',
u'odom_y_joint',
u'odom_z_joint']
result = r.get_directly_controllable_collision_links(u'odom_x_joint')
assert result == []
result = r.get_directly_controllable_collision_links(u'odom_y_joint')
assert result == []
result = r.get_directly_controllable_collision_links(u'odom_z_joint')
assert result == [u'base_link']
result = r.get_directly_controllable_collision_links(u'l_elbow_flex_joint')
assert result == [u'l_elbow_flex_link']
result = r.get_directly_controllable_collision_links(u'r_wrist_roll_joint')
assert result == [u'r_wrist_roll_link']
result = r.get_directly_controllable_collision_links(u'br_caster_l_wheel_joint')
assert result == []
def test_detach_at_link(self, function_setup):
parsed_pr2 = self.cls(pr2_urdf())
try:
parsed_pr2.detach_sub_tree(u'torso_lift_link')
assert False, u'didnt get expected exception'
except KeyError:
assert True
def test_detach_non_existing_object(self, function_setup):
parsed_pr2 = self.cls(pr2_urdf())
try:
parsed_pr2.detach_sub_tree(u'muh')
assert False, u'didnt get expected exception'
except KeyError:
assert True
def test_reset1(self, function_setup):
parsed_pr2 = self.cls(pr2_urdf())
links_before = set(parsed_pr2.get_link_names())
joints_before = set(parsed_pr2.get_joint_names())
parsed_pr2.detach_sub_tree(u'l_shoulder_pan_joint')
parsed_pr2.reset()
assert set(parsed_pr2.get_link_names()) == links_before
assert set(parsed_pr2.get_joint_names()) == joints_before
def test_urdf_from_file(self, function_setup):
"""
:type parsed_pr2: tested_class
"""
parsed_pr2 = self.cls(pr2_urdf())
assert len(parsed_pr2.get_joint_names()) == 96
assert len(parsed_pr2.get_link_names()) == 97
assert parsed_pr2.get_name() == u'pr2'
def test_god_map_with_world(self):
gm = GodMap()
w = World()
r = WorldObject(pr2_urdf())
w.add_robot(r, PoseStamped(), [], 0, KeyDefaultDict(lambda key: 0),
False)
gm.safe_set_data([u'world'], w)
assert r == gm.safe_get_data([u'world', u'robot'])
def make_world_with_pr2(self, path_to_data_folder=None):
"""
:rtype: World
"""
w = self.world_cls(path_to_data_folder=path_to_data_folder)
r = self.cls(pr2_urdf())
w.add_robot(r, None, r.controlled_joints, None, None, None,
path_to_data_folder is not None, [], [], 'llvm', 0)
return w
def test_add_robot(self, function_setup):
empty_world = self.world_cls()
assert len(empty_world.get_objects()) == 0
assert not empty_world.has_robot()
pr2 = self.cls(pr2_urdf())
empty_world.add_robot(pr2, None, pr2.controlled_joints, None, None,
None, False, [], [], 'llvm', 0)
assert empty_world.has_robot()
assert pr2 == empty_world.robot
return empty_world
def test_base_pose2(self, function_setup):
parsed_pr2 = self.cls(pr2_urdf())
p = Pose()
p.orientation.w = 10
parsed_pr2.base_pose = p
orientation = parsed_pr2.base_pose.orientation
orientation_vector = [
orientation.x, orientation.y, orientation.z, orientation.w
]
assert np.linalg.norm(orientation_vector) == 1
def test_get_chain_attached(self, function_setup):
parsed_pr2 = self.cls(pr2_urdf())
box = self.cls.from_world_body(make_world_body_box())
p = Pose()
p.position = Point(0, 0, 0.1)
p.orientation = Quaternion(1, 0, 0, 0)
parsed_pr2.attach_urdf_object(box, u'l_gripper_tool_frame', p)
tip = u'l_gripper_tool_frame'
root = box.get_name()
chain = parsed_pr2.get_joint_names_from_chain(root, tip)
assert chain == [box.get_name()]
def test_god_map_with_world(self):
gm = GodMap()
w = World()
r = WorldObject(pr2_urdf())
w.add_robot(robot=r,
base_pose=PoseStamped(),
controlled_joints=[],
ignored_pairs=set(),
added_pairs=set())
gm.set_data([u'world'], w)
gm_robot = gm.get_data(identifier.robot)
assert 'pr2' == gm_robot.get_name()
def test_attach_twice(self, function_setup):
parsed_pr2 = self.cls(pr2_urdf())
box = self.cls.from_world_body(make_world_body_box())
p = Pose()
p.position = Point(0, 0, 0)
p.orientation = Quaternion(0, 0, 0, 1)
parsed_pr2.attach_urdf_object(box, u'l_gripper_tool_frame', p)
try:
parsed_pr2.attach_urdf_object(box, u'l_gripper_tool_frame', p)
assert False, u'didnt get expected exception'
except DuplicateNameException as e:
assert True
def test_attach_urdf_object3(self, function_setup):
parsed_donbot = self.cls(donbot_urdf())
pr2 = make_urdf_world_body(u'pr2', pr2_urdf())
pr2_obj = self.cls.from_world_body(pr2)
p = Pose()
p.position = Point(0, 0, 0)
p.orientation = Quaternion(0, 0, 0, 1)
try:
parsed_donbot.attach_urdf_object(pr2_obj, u'eef', p)
assert False, u'expected exception'
except Exception:
assert True
def test_get_chain3(self, function_setup):
parsed_pr2 = self.cls(pr2_urdf())
tip = parsed_pr2.get_root()
root = u'l_gripper_tool_frame'
chain = parsed_pr2.get_joint_names_from_chain(root, tip)
assert chain == [
u'l_gripper_tool_joint', u'l_gripper_palm_joint',
u'l_force_torque_joint', u'l_force_torque_adapter_joint',
u'l_wrist_roll_joint', u'l_wrist_flex_joint', u'l_forearm_joint',
u'l_forearm_roll_joint', u'l_elbow_flex_joint',
u'l_upper_arm_joint', u'l_upper_arm_roll_joint',
u'l_shoulder_lift_joint', u'l_shoulder_pan_joint',
u'torso_lift_joint', u'base_footprint_joint'
]
def test_attach_detach(self, function_setup):
parsed_pr2 = self.cls(pr2_urdf())
box = self.cls.from_world_body(make_world_body_box())
p = Pose()
p.position = Point(0.0, 0.0, 0.0)
p.orientation = Quaternion(0.0, 0.0, 0.0, 1.0)
original_parsed_pr2 = deepcopy(parsed_pr2)
# original_urdf = parsed_pr2.get_urdf_str()
parsed_pr2.attach_urdf_object(box, u'l_gripper_tool_frame', p)
parsed_pr2.detach_sub_tree(u'box')
assert set(original_parsed_pr2.get_link_names()) == set(
parsed_pr2.get_link_names())
assert set(original_parsed_pr2.get_joint_names()) == set(
parsed_pr2.get_joint_names())
def test_add_robot(self, function_setup):
empty_world = self.world_cls()
assert len(empty_world.get_objects()) == 0
assert not empty_world.has_robot()
# extracting the urdf turns integers into floats
pr2 = self.cls(self.cls(pr2_urdf()).get_urdf_str())
empty_world.add_robot(robot=pr2,
base_pose=None,
controlled_joints=pr2.controlled_joints,
ignored_pairs=[],
added_pairs=[])
assert empty_world.has_robot()
assert pr2.get_urdf_str() == empty_world.robot.get_urdf_str()
return empty_world
def test_reset2(self, function_setup):
parsed_pr2 = self.cls(pr2_urdf())
links_before = set(parsed_pr2.get_link_names())
joints_before = set(parsed_pr2.get_joint_names())
box = self.cls.from_world_body(make_world_body_box())
p = Pose()
p.position = Point(0, 0, 0)
p.orientation = Quaternion(0, 0, 0, 1)
parsed_pr2.attach_urdf_object(box, u'l_gripper_tool_frame', p)
parsed_pr2.reset()
assert set(parsed_pr2.get_link_names()) == links_before
assert set(parsed_pr2.get_joint_names()) == joints_before
def test_check_collisions2(self, test_folder):
w = self.make_world_with_pr2()
pr22 = self.cls(pr2_urdf())
pr22.set_name('pr22')
w.add_object(pr22)
base_pose = Pose()
base_pose.position.x = 0.05
base_pose.orientation.w = 1
w.set_object_pose('pr22', base_pose)
pr23 = self.cls(pr2_urdf())
pr23.set_name('pr23')
w.add_object(pr23)
base_pose = Pose()
base_pose.position.y = 0.05
base_pose.orientation.w = 1
w.set_object_pose('pr23', base_pose)
min_dist = defaultdict(lambda: {u'zero_weight_distance': 0.1})
cut_off_distances = w.collision_goals_to_collision_matrix([], min_dist)
for i in range(160):
assert len(w.check_collisions(cut_off_distances)) == 90
def test_check_collisions(self, test_folder):
w = self.make_world_with_pr2()
pr22 = self.cls(pr2_urdf())
pr22.set_name('pr22')
w.add_object(pr22)
base_pose = Pose()
base_pose.position.x = 10
base_pose.orientation.w = 1
w.set_object_pose('pr22', base_pose)
robot_links = pr22.get_link_names()
cut_off_distances = {(link1, 'pr22', link2): 0.1
for link1, link2 in product(robot_links, repeat=2)
}
assert len(w.check_collisions(cut_off_distances).all_collisions) == 0
def test_attach_urdf_object1(self, function_setup):
parsed_pr2 = self.cls(pr2_urdf())
num_of_links_before = len(parsed_pr2.get_link_names())
num_of_joints_before = len(parsed_pr2.get_joint_names())
link_chain_before = len(
parsed_pr2.get_links_from_sub_tree(u'torso_lift_joint'))
box = self.cls.from_world_body(make_world_body_box())
p = Pose()
p.position = Point(0, 0, 0)
p.orientation = Quaternion(0, 0, 0, 1)
parsed_pr2.attach_urdf_object(box, u'l_gripper_tool_frame', p)
assert box.get_name() in parsed_pr2.get_link_names()
assert len(parsed_pr2.get_link_names()) == num_of_links_before + 1
assert len(parsed_pr2.get_joint_names()) == num_of_joints_before + 1
assert len(parsed_pr2.get_links_from_sub_tree(
u'torso_lift_joint')) == link_chain_before + 1
def test_god_map_with_world(self):
gm = GodMap()
w = World()
r = WorldObject(pr2_urdf())
w.add_robot(robot=r,
base_pose=PoseStamped(),
controlled_joints=[],
joint_vel_limit=KeyDefaultDict(lambda key: 0),
joint_acc_limit=KeyDefaultDict(lambda key: 0),
joint_weights=KeyDefaultDict(lambda key: 0),
calc_self_collision_matrix=False,
ignored_pairs=set(),
added_pairs=set(),
symengine_backend='llvm',
symengine_opt_level=0)
gm.safe_set_data([u'world'], w)
gm_robot = gm.safe_get_data(identifier.robot)
assert 'pr2' == gm_robot.get_name()
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_check_collisions3(self, test_folder):
w = self.make_world_with_pr2()
pr22 = self.cls(pr2_urdf())
pr22.set_name('pr22')
w.add_object(pr22)
base_pose = Pose()
base_pose.position.x = 1.5
base_pose.orientation.w = 1
w.set_object_pose('pr22', base_pose)
min_dist = defaultdict(lambda: {u'zero_weight_distance': 0.1})
cut_off_distances = w.collision_goals_to_collision_matrix([], min_dist)
robot_links = pr22.get_link_names()
cut_off_distances.update({
(link1, 'pr22', link2): 0.1
for link1, link2 in product(robot_links, repeat=2)
})
for i in range(160):
assert len(w.check_collisions(cut_off_distances)) == 36
def test_get_movable_parent_joint(self, function_setup):
parsed_pr2 = self.cls(pr2_urdf())
assert parsed_pr2.get_controlled_parent_joint(
u'l_gripper_tool_frame') == u'l_wrist_roll_joint'
def test_get_parent_joint_of_joint(self, function_setup):
parsed_pr2 = self.cls(pr2_urdf())
assert parsed_pr2.get_parent_joint_of_joint(
u'l_gripper_tool_joint') == u'l_gripper_palm_joint'
def test_get_non_base_movement_root2(self, function_setup):
parsed_pr2 = self.cls(pr2_urdf())
assert parsed_pr2.get_non_base_movement_root() == u'base_footprint'
def test_get_first_link_with_collision(self, function_setup):
parsed_pr2 = self.cls(pr2_urdf())
assert parsed_pr2.get_first_link_with_collision() == u'base_link'
def test_get_parent_link_name(self, function_setup):
parsed_pr2 = self.cls(pr2_urdf())
assert parsed_pr2.get_parent_link_of_link(
u'l_gripper_tool_frame') == u'l_gripper_palm_link'
def test_from_world_body_urdf(self, function_setup):
wb = make_urdf_world_body(u'pr2', pr2_urdf())
urdf_obj = self.cls.from_world_body(wb)
assert len(urdf_obj.get_joint_names()) == 96
assert len(urdf_obj.get_link_names()) == 97