def test_complex_srdf(self): datadir = rospkg.RosPack().get_path('srdfdom') + "/test/resources/" stream = open(datadir + 'pr2_desc.3.srdf', 'r') robot = SRDF.from_xml_string(stream.read()) stream.close() self.assertTrue(len(robot.virtual_joints) == 1) self.assertTrue(len(robot.groups) == 7) self.assertTrue(len(robot.group_states) == 2) self.assertTrue(len(robot.disable_collisionss) == 2) self.assertTrue(robot.disable_collisionss[0].reason == "adjacent") self.assertTrue(len(robot.end_effectors) == 2) self.assertTrue(robot.virtual_joints[0].name == "world_joint") self.assertTrue(robot.virtual_joints[0].type == "planar") for group in robot.groups: if (group.name == "left_arm" or group.name == "right_arm"): self.assertTrue(len(group.chains) == 1) if group.name == "arms": self.assertTrue(len(group.subgroups) == 2) if group.name == "base": self.assertTrue(len(group.joints) == 1) if (group.name == "l_end_effector" or group.name == "r_end_effector"): self.assertTrue(len(group.links) == 1) self.assertTrue(len(group.joints) == 9) if group.name == "whole_body": self.assertTrue(len(group.joints) == 1) self.assertTrue(len(group.subgroups) == 2) index = 0 if robot.group_states[0].group != "arms": index = 1 self.assertTrue(robot.group_states[index].group == "arms") self.assertTrue(robot.group_states[index].name == "tuck_arms") self.assertTrue(robot.group_states[1 - index].group == "base") self.assertTrue(robot.group_states[1 - index].name == "home") v = next((joint.value for joint in robot.group_states[index].joints if joint.name == "l_shoulder_pan_joint"), None) self.assertTrue(len(v) == 1) self.assertTrue(v[0] == 0.2) w = next((joint.value for joint in robot.group_states[1 - index].joints if joint.name == "world_joint"), None) self.assertTrue(len(w) == 3) self.assertTrue(w[0] == 0.4) self.assertTrue(w[1] == 0) self.assertTrue(w[2] == -1) index = 0 if (robot.end_effectors[0].name[0] == 'r') else 1 self.assertTrue(robot.end_effectors[index].name == 'r_end_effector') self.assertTrue(robot.end_effectors[index].group == 'r_end_effector') self.assertTrue( robot.end_effectors[index].parent_link == 'r_wrist_roll_link')
def test_simple_srdf(self): datadir = rospkg.RosPack().get_path('srdfdom') + "/test/resources/" stream = open(datadir + 'pr2_desc.1.srdf', 'r') robot = SRDF.from_xml_string(stream.read()) stream.close() self.assertTrue(len(robot.virtual_joints) == 0) self.assertTrue(len(robot.groups) == 0) self.assertTrue(len(robot.group_states) == 0) self.assertTrue(len(robot.disable_collisionss) == 0) self.assertTrue(len(robot.end_effectors) == 0) stream = open(datadir + 'pr2_desc.2.srdf', 'r') robot = SRDF.from_xml_string(stream.read()) stream.close() self.assertTrue(len(robot.virtual_joints) == 1) self.assertTrue(len(robot.groups) == 1) self.assertTrue(len(robot.group_states) == 0) self.assertTrue(len(robot.disable_collisionss) == 0) self.assertTrue(len(robot.end_effectors) == 0)
def test_full_srdf(self): srdf_data = ''' <robot name="myrobot"> <group name="body"> <joint name="J1" /> <joint name="J2" /> <joint name="J3" /> <chain base_link="robot_base" tip_link="robot_tip" /> <group name="arm" /> </group> <group_state name="zero" group="body"> <joint name="J1" value="0" /> <joint name="J2" value="0" /> <joint name="J3" value="0" /> </group_state> <end_effector name="tip_ee" parent_link="tip" group="arm" parent_group="body" /> <end_effector name="othertip_ee" parent_link="othertip" group="arm" /> <virtual_joint name="virtual_joint" type="floating" parent_frame="body_frame" child_link="arm" /> <disable_collisions link1="link1" link2="link3" /> <disable_collisions reason="Adjacent" link1="link1" link2="link2" /> <link_sphere_approximation link="link1" /> <link_sphere_approximation link="link2" > <sphere center="1.0 2.0 3.0" radius="1.0" /> <sphere center="1.0 2.0 4.0" radius="2.0" /> </link_sphere_approximation> </robot> ''' expected = ''' <robot name="myrobot"> <group name="body"> <joint name="J1" /> <joint name="J2" /> <joint name="J3" /> <chain base_link="robot_base" tip_link="robot_tip"/> <group name="arm" /> </group> <group_state name="zero" group="body"> <joint name="J1" value="0" /> <joint name="J2" value="0" /> <joint name="J3" value="0" /> </group_state> <end_effector group="arm" name="tip_ee" parent_group="body" parent_link="tip"/> <end_effector name="othertip_ee" parent_link="othertip" group="arm" /> <virtual_joint child_link="arm" name="virtual_joint" parent_frame="body_frame" type="floating" /> <disable_collisions link1="link1" link2="link3" /> <disable_collisions link1="link1" link2="link2" reason="Adjacent" /> <link_sphere_approximation link="link1" /> <link_sphere_approximation link="link2" > <sphere center="1.0 2.0 3.0" radius="1.0" /> <sphere center="1.0 2.0 4.0" radius="2.0" /> </link_sphere_approximation> </robot> ''' robot = SRDF.from_xml_string(srdf_data) self.assertTrue(xml_matches(robot.to_xml_string(), expected))
def load_srdf(self, srdf_file): stream = open(srdf_file, 'r') self.srdf = SRDF.from_xml_string(stream.read()) for gs in self.srdf.group_states: joint_map = OrderedDict() for joint in gs.joints: joint_map[joint.name] = joint.value[0] self.group_states_map[gs.name, gs.group] = joint_map self.joint_states_map[gs.name] = joint_map self.group_map[gs.group] = joint_map.keys()
def load_srdf(self): srdf_file = home + "/catkin_ws/src/robot_descriptions/kuka_husky_description/moveit_config/config/kuka_husky.srdf" stream = open(srdf_file, 'r') srdf = SRDF.from_xml_string(stream.read()) ignored_collisions = DefaultOrderedDict(bool) for collision in srdf.disable_collisionss: ignored_collisions[collision.link1, collision.link2] = True ignored_collisions[collision.link2, collision.link1] = True # print ignored_collisions self.planner.world.ignored_collisions = ignored_collisions
# load on param server or output to file upload_output_params(output_str, output_path, ns_) if __name__ == '__main__': PARSER = argparse.ArgumentParser(usage='Load an SRDF file') PARSER.add_argument('file', type=argparse.FileType('r'), nargs='?', default=None, help='File to load. Use - for stdin') ARGS = PARSER.parse_args() if ARGS.file is not None: ROBOT = SRDF.from_xml_string(ARGS.file.read()) generate_fake_controllers(ROBOT, output_path="fake_controllers.yaml") generate_real_controllers(ROBOT, output_path="controllers.yaml") generate_ompl_planning(ROBOT, "ompl_planning_template.yaml", output_path="ompl_planning.yaml") generate_kinematics(ROBOT, "kinematics_template.yaml", output_path="kinematics.yaml") generate_joint_limits(ROBOT, "joint_limits_template.yaml", output_path="joint_limits.yaml") else: rospy.logerr("No file SRDF provided")
rospy.init_node('moveit_config_generator', anonymous=True) if command in [ 'fake_controllers', 'real_controllers', 'ompl_planning', 'kinematics', 'joint_limits' ]: NS = rospy.get_namespace() # wait for parameters while (not rospy.search_param('robot_description_semantic') and not rospy.is_shutdown()): time.sleep(0.5) rospy.loginfo("waiting for robot_description_semantic") # load the srdf from the parameter server full_param_name = rospy.search_param('robot_description_semantic') srdf_str = rospy.get_param(full_param_name) # parse it robot = SRDF.from_xml_string(srdf_str) sh_config_path = rospkg.RosPack().get_path( 'sr_moveit_hand_config') + "/config/" with open(robot_config_file, "r") as stream: yamldoc = yaml.safe_load(stream) robot_config = generate_robot_srdf.Robot() robot_config.set_parameters(yamldoc) output_path = None # generate the desired yaml and load it. if command == "fake_controllers": if save_file: output_path = ( rospkg.RosPack().get_path('sr_multi_moveit_config') + "/config/" + "fake_controllers.yaml")