def get_group_state_values(group_state_name):
""" Function to parse the SRDF to obtain the
joint configuration and names for a given group state
Parameters
----------
group_state_name : str
valid group state name from the SRDF for the given group
Returns
-------
tuple
Returns a tuple of two lists - one containing the joint
configuration values and the other containing the joint names
"""
robot = SRDF.from_parameter_server()
joint_values = [
joint.value[0]
for joint in robot.group_state_map[group_state_name].joints
]
joint_names = [
joint.name for joint in robot.group_state_map[group_state_name].joints
]
rospy.loginfo("Joint names {}".format(joint_names))
rospy.loginfo("Joint values {}".format(joint_values))
return joint_values, joint_names
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
def __init__(self, context):
super(MoveitCommanderWidget, self).__init__()
self.setObjectName('MoveitCommanderWidget')
self.robot = SRDF.from_parameter_server()
self.groups = self.get_groups()
self.commanders = [
moveit_commander.MoveGroupCommander(group) for group in self.groups
]
# list of QLineEdit fields used to display joint values
self.text_joint_values = []
self.MAX_COLUMNS = 4
self.tab_widget = QTabWidget()
for g in self.groups:
frame = self.setup_group_frame(g)
self.tab_widget.addTab(frame, g)
widget_layout = QVBoxLayout()
widget_layout.addWidget(self.tab_widget)
self.setLayout(widget_layout)
# 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")
group_state_joint_angles = []
for x in range(6):
group_state_joint_angles.append(
float(
str(robot.group_states[statedict[state_name]].joints[x].value)
[1:-1]))
return group_state_joint_angles
if __name__ == "__main__":
# Create a node
rospy.init_node("mobmanip")
#Parse the SRDF from the parameter server
robot = SRDF.from_parameter_server()
# start moveit part
group = moveit_commander.MoveGroupCommander("manipulator")
group.set_planner_id("RRTConnectkConfigDefault")
display_trajectory_publisher = rospy.Publisher(
'/move_group/display_planned_path',
moveit_msgs.msg.DisplayTrajectory,
queue_size=20)
# Make sure sim time is working
while not rospy.Time.now():
pass
# Setup clients
move_base = MoveBaseClient()
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")
