def test_safe_load_collision_matrix(self, test_folder, delete_test_folder):
r = self.cls(donbot_urdf(), path_to_data_folder=test_folder)
r.init_self_collision_matrix()
scm = r.get_self_collision_matrix()
r.safe_self_collision_matrix(test_folder)
r.load_self_collision_matrix(test_folder)
assert scm == r.get_self_collision_matrix()
def test_get_chain_2(self, function_setup):
parsed_donbot = self.cls(donbot_urdf())
chain = parsed_donbot.get_chain('base_link', 'plate')
assert chain == [
'base_link', 'base_footprint_joint', 'base_footprint',
'plate_joint', 'plate'
]
def test_safe_load_collision_matrix(self, test_folder, delete_test_folder):
r = self.cls(donbot_urdf(), path_to_data_folder=test_folder)
r.init_self_collision_matrix()
expected = r.get_self_collision_matrix()
r.safe_self_collision_matrix(test_folder)
assert r.load_self_collision_matrix(test_folder)
actual = r.get_self_collision_matrix()
assert expected == actual
def test_get_chain_fixed_joints2(self, function_setup):
parsed_donbot = self.cls(donbot_urdf())
chain = parsed_donbot.get_chain('odom',
'gripper_gripper_right_link',
joints=True,
fixed=False)
assert chain == parsed_donbot._urdf_robot.get_chain(
'odom', 'gripper_gripper_right_link', joints=True, fixed=False)
def make_world_with_donbot(self, path_to_data_folder=None):
"""
:rtype: World
"""
w = self.world_cls(path_to_data_folder=path_to_data_folder)
r = self.cls(donbot_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_get_chain_fixed_joints(self, function_setup):
parsed_donbot = self.cls(donbot_urdf())
chain = parsed_donbot.get_chain('odom',
'odom_x_frame',
joints=False,
fixed=False)
assert chain == parsed_donbot._urdf_robot.get_chain('odom',
'odom_x_frame',
joints=False,
fixed=False)
def test_detach_object2(self, test_folder):
r = self.cls(donbot_urdf(), path_to_data_folder=test_folder)
r.init_self_collision_matrix()
scm = r.get_self_collision_matrix()
box = WorldObject.from_world_body(make_world_body_box())
p = Pose()
p.position = Point(0, 0, 0)
p.orientation = Quaternion(0, 0, 0, 1)
r.attach_urdf_object(box, u'gripper_tool_frame', p)
assert len(scm) < len(r.get_self_collision_matrix())
r.detach_sub_tree(box.get_name())
assert scm.symmetric_difference(r.get_self_collision_matrix()) == set()
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_donbot_fk1(self, parsed_donbot, js):
kdl = KDL(donbot_urdf())
root = u'base_footprint'
tips = [u'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_donbot.joint_state = mjs
symengine_fk = parsed_donbot.get_fk_pose(root, tip).pose
compare_poses(kdl_fk, symengine_fk)
def test_reset_collision_matrix(self, test_folder):
r = self.cls(donbot_urdf(), path_to_data_folder=test_folder)
r.update_self_collision_matrix()
scm = r.get_self_collision_matrix()
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)
r.attach_urdf_object(box, u'gripper_tool_frame', p)
assert scm.symmetric_difference(r.get_self_collision_matrix()) != set()
r.reset()
assert scm.symmetric_difference(r.get_self_collision_matrix()) == set()
def test_get_controlled_leaf_joints2(self, test_folder, delete_test_folder):
r = self.cls(donbot_urdf(), path_to_data_folder=test_folder)
r.controlled_joints = [u'ur5_shoulder_pan_joint',
u'ur5_shoulder_lift_joint',
u'ur5_elbow_joint',
u'ur5_wrist_1_joint',
u'ur5_wrist_2_joint',
u'ur5_wrist_3_joint',
u'odom_x_joint',
u'odom_y_joint',
u'odom_z_joint']
r = r.get_controlled_leaf_joints()
assert r == {
'odom_z_joint', 'ur5_wrist_3_joint'
}
def test_safe_load_collision_matrix2(self, test_folder, delete_test_folder):
r = self.cls(donbot_urdf(), path_to_data_folder=test_folder)
r.init_self_collision_matrix()
scm = r.get_self_collision_matrix()
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)
r.attach_urdf_object(box, u'gripper_tool_frame', p)
r.update_self_collision_matrix()
scm_with_obj = r.get_self_collision_matrix()
r.detach_sub_tree(box.get_name())
r.load_self_collision_matrix(test_folder)
assert scm == r.get_self_collision_matrix()
r.attach_urdf_object(box, u'gripper_tool_frame', p)
r.load_self_collision_matrix(test_folder)
assert scm_with_obj == r.get_self_collision_matrix()
def test_attach_urdf_object2(self, function_setup):
parsed_base_bot = self.cls(base_bot_urdf())
links_before = set(parsed_base_bot.get_link_names())
joints_before = set(parsed_base_bot.get_joint_names())
donbot = make_urdf_world_body(u'mustafa', donbot_urdf())
donbot_obj = self.cls.from_world_body(donbot)
p = Pose()
p.position = Point(0, 0, 0)
p.orientation = Quaternion(0, 0, 0, 1)
parsed_base_bot.attach_urdf_object(donbot_obj, u'eef', p)
assert donbot_obj.get_root() in parsed_base_bot.get_link_names()
assert set(parsed_base_bot.get_link_names()).difference(
links_before.union(set(donbot_obj.get_link_names()))) == set()
assert set(parsed_base_bot.get_joint_names()).difference(
joints_before.union(set(
donbot_obj.get_joint_names()))) == {u'mustafa'}
links = [l.name for l in parsed_base_bot.get_urdf_robot().links]
assert len(links) == len(set(links))
joints = [j.name for j in parsed_base_bot.get_urdf_robot().joints]
assert len(joints) == len(set(joints))
def test_attach_urdf_object1_2(self, test_folder):
parsed_pr2 = self.cls(donbot_urdf(), path_to_data_folder=test_folder)
parsed_pr2.init_self_collision_matrix()
scm = parsed_pr2.get_self_collision_matrix()
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'ur5_shoulder_pan_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'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'ur5_shoulder_pan_joint')) == link_chain_before + 1
assert scm.difference(parsed_pr2.get_self_collision_matrix()) == set()
assert len(scm) < len(parsed_pr2.get_self_collision_matrix())
def test_get_chain_1(self, function_setup):
parsed_donbot = self.cls(donbot_urdf())
chain = parsed_donbot.get_chain('odom', 'gripper_tool_frame')
assert chain == parsed_donbot._urdf_robot.get_chain(
'odom', 'gripper_tool_frame')
def parsed_donbot(function_setup):
"""
:rtype: Robot
"""
return Robot(donbot_urdf())
def test_get_sub_tree_link_names_with_collision_donbot(
self, function_setup):
parsed_donbot = self.cls(donbot_urdf())
expected = {
u'ur5_wrist_3_joint': {
u'ur5_wrist_3_link', u'ur5_ee_link', u'gripper_base_link',
u'gripper_gripper_left_link', u'gripper_finger_left_link',
u'gripper_gripper_right_link', u'gripper_finger_right_link'
},
u'ur5_elbow_joint': {
u'ur5_forearm_link', u'ur5_wrist_1_link', u'ur5_wrist_2_link',
u'ur5_wrist_3_link', u'ur5_ee_link', u'gripper_base_link',
u'gripper_gripper_left_link', u'gripper_finger_left_link',
u'gripper_gripper_right_link', u'gripper_finger_right_link'
},
u'ur5_wrist_1_joint': {
u'ur5_wrist_1_link', u'ur5_wrist_2_link', u'ur5_wrist_3_link',
u'ur5_ee_link', u'gripper_base_link',
u'gripper_gripper_left_link', u'gripper_finger_left_link',
u'gripper_gripper_right_link', u'gripper_finger_right_link'
},
u'odom_z_joint': {
u'base_link', u'plate', u'ur5_base_link', u'ur5_shoulder_link',
u'ur5_upper_arm_link', u'ur5_forearm_link',
u'ur5_wrist_1_link', u'ur5_wrist_2_link', u'ur5_wrist_3_link',
u'ur5_ee_link', u'gripper_base_link',
u'gripper_gripper_left_link', u'gripper_finger_left_link',
u'gripper_gripper_right_link', u'gripper_finger_right_link'
},
u'ur5_shoulder_lift_joint': {
u'ur5_upper_arm_link', u'ur5_forearm_link',
u'ur5_wrist_1_link', u'ur5_wrist_2_link', u'ur5_wrist_3_link',
u'ur5_ee_link', u'gripper_base_link',
u'gripper_gripper_left_link', u'gripper_finger_left_link',
u'gripper_gripper_right_link', u'gripper_finger_right_link'
},
u'odom_y_joint': {
u'base_link', u'plate', u'ur5_base_link', u'ur5_shoulder_link',
u'ur5_upper_arm_link', u'ur5_forearm_link',
u'ur5_wrist_1_link', u'ur5_wrist_2_link', u'ur5_wrist_3_link',
u'ur5_ee_link', u'gripper_base_link',
u'gripper_gripper_left_link', u'gripper_finger_left_link',
u'gripper_gripper_right_link', u'gripper_finger_right_link'
},
u'ur5_wrist_2_joint': {
u'ur5_wrist_2_link', u'ur5_wrist_3_link', u'ur5_ee_link',
u'gripper_base_link', u'gripper_gripper_left_link',
u'gripper_finger_left_link', u'gripper_gripper_right_link',
u'gripper_finger_right_link'
},
u'odom_x_joint': {
u'base_link', u'plate', u'ur5_base_link', u'ur5_shoulder_link',
u'ur5_upper_arm_link', u'ur5_forearm_link',
u'ur5_wrist_1_link', u'ur5_wrist_2_link', u'ur5_wrist_3_link',
u'ur5_ee_link', u'gripper_base_link',
u'gripper_gripper_left_link', u'gripper_finger_left_link',
u'gripper_gripper_right_link', u'gripper_finger_right_link'
},
u'ur5_shoulder_pan_joint': {
u'ur5_shoulder_link', u'ur5_upper_arm_link',
u'ur5_forearm_link', u'ur5_wrist_1_link', u'ur5_wrist_2_link',
u'ur5_wrist_3_link', u'ur5_ee_link', u'gripper_base_link',
u'gripper_gripper_left_link', u'gripper_finger_left_link',
u'gripper_gripper_right_link', u'gripper_finger_right_link'
},
u'gripper_joint':
{u'gripper_gripper_right_link', u'gripper_finger_right_link'}
}
for joint in parsed_donbot.get_joint_names_controllable():
assert set(
parsed_donbot.get_sub_tree_link_names_with_collision(
joint)).difference(expected[joint]) == set()
def test_safe_load_collision_matrix(self, test_folder, delete_test_folder):
r = self.cls(donbot_urdf(), path_to_data_folder=test_folder)
scm = r.get_self_collision_matrix()
assert len(scm) == 0
def make_world_with_donbot(self, path_to_data_folder=None):
"""
:rtype: World
"""
return self.make_world_with_robot(donbot_urdf(), path_to_data_folder)
def test_get_non_base_movement_root(self, function_setup):
parsed_donbot = self.cls(donbot_urdf())
assert parsed_donbot.get_non_base_movement_root() == u'base_footprint'
class TestSymengineController(object):
pr2_joint_limits = Robot(pr2_urdf()).get_all_joint_limits()
donbot_joint_limits = Robot(donbot_urdf()).get_all_joint_limits()
boxy_joint_limits = Robot(boxy_urdf()).get_all_joint_limits()
def test_constraints_pr2(self, parsed_pr2):
assert len(parsed_pr2.hard_constraints) == 26
assert len(parsed_pr2.joint_constraints) == 45
def test_constraints_donbot(self, parsed_donbot):
assert len(parsed_donbot.hard_constraints) == 9
assert len(parsed_donbot.joint_constraints) == 10
def test_constraints_boxy(self, parsed_boxy):
assert len(parsed_boxy.hard_constraints) == 26
assert len(parsed_boxy.joint_constraints) == 26
@given(rnd_joint_state(pr2_joint_limits))
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)
@given(rnd_joint_state(donbot_joint_limits))
def test_donbot_fk1(self, parsed_donbot, js):
kdl = KDL(donbot_urdf())
root = u'base_footprint'
tips = [u'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_donbot.joint_state = mjs
symengine_fk = parsed_donbot.get_fk_pose(root, tip).pose
compare_poses(kdl_fk, symengine_fk)
@given(rnd_joint_state(boxy_joint_limits))
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_get_controllable_joint_names_pr2(self, parsed_pr2):
expected = {u'l_shoulder_pan_joint', u'br_caster_l_wheel_joint', u'r_gripper_l_finger_tip_joint',
u'r_elbow_flex_joint', u'torso_lift_joint', u'r_gripper_l_finger_joint', u'r_forearm_roll_joint',
u'l_gripper_r_finger_tip_joint', u'r_shoulder_lift_joint', u'fl_caster_rotation_joint',
u'l_gripper_motor_screw_joint', u'r_wrist_roll_joint', u'r_gripper_motor_slider_joint',
u'l_forearm_roll_joint', u'r_gripper_joint', u'bl_caster_rotation_joint',
u'fl_caster_r_wheel_joint', u'l_shoulder_lift_joint', u'head_pan_joint', u'head_tilt_joint',
u'fr_caster_l_wheel_joint', u'fl_caster_l_wheel_joint', u'l_gripper_motor_slider_joint',
u'br_caster_r_wheel_joint', u'r_gripper_motor_screw_joint', u'r_upper_arm_roll_joint',
u'fr_caster_rotation_joint', u'torso_lift_motor_screw_joint', u'bl_caster_l_wheel_joint',
u'r_wrist_flex_joint', u'r_gripper_r_finger_tip_joint', u'l_elbow_flex_joint',
u'laser_tilt_mount_joint', u'r_shoulder_pan_joint', u'fr_caster_r_wheel_joint',
u'l_wrist_roll_joint', u'r_gripper_r_finger_joint', u'bl_caster_r_wheel_joint', u'l_gripper_joint',
u'l_gripper_l_finger_tip_joint', u'br_caster_rotation_joint', u'l_gripper_l_finger_joint',
u'l_wrist_flex_joint', u'l_upper_arm_roll_joint', u'l_gripper_r_finger_joint'}
assert set(parsed_pr2.get_joint_names_controllable()).difference(expected) == set()
def test_get_joint_names_donbot(self, parsed_donbot):
expected = {u'ur5_wrist_3_joint', u'ur5_elbow_joint', u'ur5_wrist_1_joint', u'odom_z_joint',
u'ur5_shoulder_lift_joint', u'odom_y_joint', u'ur5_wrist_2_joint', u'odom_x_joint',
u'ur5_shoulder_pan_joint', u'gripper_joint'}
assert set(parsed_donbot.get_joint_names_controllable()).difference(expected) == set()
def test_get_joint_names_boxy(self, parsed_boxy):
expected = {u'left_arm_2_joint', u'neck_joint_end', u'neck_wrist_1_joint', u'right_arm_2_joint',
u'right_arm_4_joint', u'neck_wrist_3_joint', u'right_arm_3_joint',
u'right_gripper_base_gripper_right_joint', u'left_gripper_base_gripper_right_joint',
u'left_arm_0_joint', u'right_gripper_base_gripper_left_joint', u'left_arm_4_joint',
u'left_arm_6_joint', u'right_arm_1_joint', u'left_arm_1_joint', u'neck_wrist_2_joint',
u'triangle_base_joint', u'neck_elbow_joint', u'right_arm_5_joint', u'left_arm_3_joint',
u'neck_shoulder_pan_joint', u'right_arm_0_joint', u'neck_shoulder_lift_joint', u'left_arm_5_joint',
u'left_gripper_base_gripper_left_joint', u'right_arm_6_joint'}
assert set(parsed_boxy.get_joint_names_controllable()).difference(expected) == set()
