generate_moveit_config.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 # Software License Agreement (BSD License)
3 #
4 # Copyright (c) 2015, CITEC, Bielefeld University
5 # All rights reserved.
6 #
7 # Redistribution and use in source and binary forms, with or without
8 # modification, are permitted provided that the following conditions
9 # are met:
10 #
11 # * Redistributions of source code must retain the above copyright
12 # notice, this list of conditions and the following disclaimer.
13 # * Redistributions in binary form must reproduce the above
14 # copyright notice, this list of conditions and the following
15 # disclaimer in the documentation and/or other materials provided
16 # with the distribution.
17 #
18 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
19 # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
20 # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
21 # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
22 # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
23 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
24 # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
25 # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
26 # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
27 # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
28 # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
29 # POSSIBILITY OF SUCH DAMAGE.
30 
31 # Author: Guillaume Walck <gwalck@techfak.uni-bielefeld.de>
32 # Author: Shadow Robot Software Team <software@shadowrobot.com>
33 
34 
35 import argparse
36 import yaml
37 import re
38 
39 import rospy
40 import rospkg
41 import rosparam
42 from srdfdom.srdf import SRDF
43 from copy import deepcopy
44 
45 from urdf_parser_py.urdf import URDF
46 import generate_robot_srdf
47 import sr_moveit_hand_config.generate_moveit_config as hand_config
48 
49 
50 def yaml_reindent(in_str, numspaces):
51  s_indent = "\n".join((numspaces * " ") + i for i in in_str.splitlines())
52  return s_indent
53 
54 
55 def upload_output_params(upload_str, output_path=None, upload=True, ns_=None):
56  if upload:
57  paramlist = rosparam.load_str(upload_str, "generated",
58  default_namespace=ns_)
59  for params, namespace in paramlist:
60  rosparam.upload_params(namespace, params)
61  if output_path is not None:
62  file_writer = open(output_path, "wb")
63  file_writer.write(upload_str)
64  file_writer.close()
65 
66 
67 def generate_fake_controllers(robot, robot_config, output_path=None, ns_=None):
68  output_str = "controller_list:\n"
69  for manipulator in robot_config.manipulators:
70  if manipulator.has_arm:
71  # Read arm srdf
72  arm_yaml_path = manipulator.arm.moveit_path + "/" + "fake_controllers.yaml"
73  with open(arm_yaml_path, 'r') as stream:
74  arm_yamldoc = yaml.load(stream)
75 
76  output_str += " - name: fake_" + manipulator.arm.prefix + "controller" + "\n"
77  output_str += " joints:\n"
78  for joint in arm_yamldoc["controller_list"][0]["joints"]:
79  output_str += " - " + manipulator.arm.prefix + joint + "\n"
80  if manipulator.has_hand and not manipulator.hand.is_lite:
81  output_str += " - " + manipulator.hand.prefix + "WRJ2" + "\n"
82  output_str += " - " + manipulator.hand.prefix + "WRJ1" + "\n"
83 
84  if manipulator.has_hand:
85  sh_group = None
86  for group in robot.groups:
87  if group.name == manipulator.hand.internal_name:
88  sh_group = group
89  break
90  output_str += " - name: fake_" + manipulator.hand.prefix + "controller\n"
91  output_str += " joints:\n"
92  if len(group.joints) == 0:
93  output_str += " []\n"
94  else:
95  for joint in group.joints:
96  if joint.name[-3:] != "tip":
97  if manipulator.has_arm:
98  if joint.name[len(manipulator.hand.prefix):] not in ["WRJ1", "WRJ2"]:
99  output_str += " - " + joint.name + "\n"
100  else:
101  output_str += " - " + joint.name + "\n"
102 
103  # load on param server or output to file
104  upload_output_params(output_str, output_path, ns_)
105 
106 
107 def generate_real_controllers(robot, robot_config, output_path=None, ns_=None):
108  output_str = "controller_list:\n"
109  for manipulator in robot_config.manipulators:
110  if manipulator.has_arm:
111  # Read arm srdf
112  arm_yaml_path = manipulator.arm.moveit_path + "/" + "controllers.yaml"
113  with open(arm_yaml_path, 'r') as stream:
114  arm_yamldoc = yaml.load(stream)
115 
116  output_str += " - name: " + manipulator.arm.prefix + "trajectory_controller\n"
117  output_str += " action_ns: follow_joint_trajectory\n"
118  output_str += " type: FollowJointTrajectory\n"
119  output_str += " default: true\n"
120  output_str += " joints:\n"
121  for joint in arm_yamldoc["controller_list"][0]["joints"]:
122  output_str += " - " + manipulator.arm.prefix + joint + "\n"
123  if manipulator.has_hand and not manipulator.hand.is_lite:
124  output_str += " - " + manipulator.hand.prefix + "WRJ2" + "\n"
125  output_str += " - " + manipulator.hand.prefix + "WRJ1" + "\n"
126 
127  if manipulator.has_hand:
128  sh_group = None
129  for group in robot.groups:
130  if group.name == manipulator.hand.internal_name:
131  sh_group = group
132  break
133  controller_name = " - name: " + manipulator.hand.prefix + "trajectory_controller\n"
134  output_str += controller_name
135  output_str += " action_ns: follow_joint_trajectory\n"
136  output_str += " type: FollowJointTrajectory\n"
137  output_str += " default: true\n"
138  output_str += " joints:\n"
139  if len(group.joints) == 0:
140  output_str += " []\n"
141  else:
142  for joint in group.joints:
143  if joint.name[-3:] != "tip":
144  if manipulator.has_arm:
145  if joint.name[len(manipulator.hand.prefix):] not in ["WRJ1", "WRJ2"]:
146  output_str += " - " + joint.name + "\n"
147  else:
148  output_str += " - " + joint.name + "\n"
149 
150  # load on param server or output to file
151  upload_output_params(output_str, output_path, ns_)
152 
153 
154 def generate_ompl_planning(robot, robot_config, hand_template_path="ompl_planning_template.yaml",
155  output_path=None, ns_=None):
156  with open(hand_template_path, 'r') as stream:
157  hand_yamldoc = yaml.load(stream)
158  output_str = ""
159  output_str += "planner_configs:\n"
160  output_str += yaml_reindent(yaml.dump(hand_yamldoc["planner_configs"],
161  default_flow_style=False, allow_unicode=True), 2)
162  output_str += "\n"
163 
164  for manipulator in robot_config.manipulators:
165  if manipulator.has_arm:
166  arm_yaml_path = manipulator.arm.moveit_path + "/" + "ompl_planning.yaml"
167  with open(arm_yaml_path, 'r') as stream:
168  arm_yamldoc = yaml.load(stream)
169  if manipulator.arm.extra_groups_config_path:
170  arm_yaml_extra_groups_path = (manipulator.arm.extra_groups_config_path + "/" +
171  "ompl_planning_extra_groups.yaml")
172  with open(arm_yaml_extra_groups_path, 'r') as stream:
173  arm_yamldoc_extra_groups = yaml.load(stream)
174  prefix = manipulator.arm.prefix
175  for group in robot.groups:
176  group_name = group.name
177  if group_name == manipulator.arm.internal_name:
178  group_name = manipulator.arm.main_group
179  group_prefix = prefix
180  elif manipulator.arm.internal_name in group_name:
181  group_prefix, group_name = group_name.split("_", 1)
182  group_prefix = prefix
183  else:
184  group_name = group.name[len(prefix):]
185  group_prefix = group.name[:len(prefix)]
186 
187  if group_name in arm_yamldoc and group_prefix == prefix:
188  output_str += group.name + ":\n"
189  group_config = arm_yamldoc[group_name]
190  # prepend prefix on projection_evaluator
191  if prefix:
192  if "projection_evaluator" in group_config:
193  proj_eval = group_config["projection_evaluator"]
194  proj_eval.strip()
195  proj_eval_striped = re.split('\W+', proj_eval)
196  joints = [word for word in proj_eval_striped if word not in ["joints", ""]]
197  proj_eval_new = "joints("
198  for joint in joints:
199  proj_eval_new = proj_eval_new + prefix + joint + ","
200  proj_eval_new = proj_eval_new[:-1] + ")"
201  group_config["projection_evaluator"] = proj_eval_new
202  group_dump = yaml.dump(group_config,
203  default_flow_style=False,
204  allow_unicode=True)
205  output_str += yaml_reindent(group_dump, 2)
206  output_str += "\n"
207  elif group_name in arm_yamldoc_extra_groups:
208  output_str += group.name + ":\n"
209  group_config = arm_yamldoc_extra_groups[group_name]
210  # prepend prefix on projection_evaluator
211  if prefix:
212  if "projection_evaluator" in group_config:
213  proj_eval = group_config["projection_evaluator"]
214  proj_eval.strip()
215  proj_eval_striped = re.split('\W+', proj_eval)
216  joints = [word for word in proj_eval_striped if word not in ["joints", ""]]
217  proj_eval_new = "joints("
218  for joint in joints:
219  proj_eval_new = proj_eval_new + prefix + joint + ","
220  proj_eval_new = proj_eval_new[:-1] + ")"
221  group_config["projection_evaluator"] = proj_eval_new
222  group_dump = yaml.dump(group_config,
223  default_flow_style=False,
224  allow_unicode=True)
225  output_str += yaml_reindent(group_dump, 2)
226  output_str += "\n"
227 
228  if manipulator.has_hand:
229  with open(hand_template_path, 'r') as stream:
230  hand_yamldoc = yaml.load(stream)
231  prefix = manipulator.hand.prefix
232  if prefix:
233  proj_eval_re = re.compile(r'joints\(([TFMRLW][FHR]J[0-5]),([TFMRLW][FHR]J[0-5])\)')
234  for group in robot.groups:
235  group_name = group.name
236  if group_name == manipulator.hand.internal_name:
237  group_name = "hand"
238  group_prefix = prefix
239  else:
240  group_name = group.name[len(prefix):]
241  group_prefix = group.name[:len(prefix)]
242  if group_name in hand_yamldoc and group_prefix == prefix:
243  output_str += group.name + ":\n"
244  group_config = hand_yamldoc[group_name]
245  # prepend prefix on projection_evaluator
246  if prefix:
247  if "projection_evaluator" in group_config:
248  proj_eval = group_config["projection_evaluator"]
249  proj_eval.strip()
250  proj_eval_new = proj_eval_re.sub(r'joints(' +
251  prefix +
252  r'\g<1>,' +
253  prefix +
254  r'\g<2>)',
255  proj_eval)
256  group_config["projection_evaluator"] = proj_eval_new
257  group_dump = yaml.dump(group_config,
258  default_flow_style=False,
259  allow_unicode=True)
260  output_str += yaml_reindent(group_dump, 2)
261  output_str += "\n"
262  stream.close()
263 
264  # load on param server or output to file
265  upload_output_params(output_str, output_path, ns_)
266 
267 
268 def generate_kinematics(robot, robot_config, hand_template_path="kinematics_template.yaml",
269  output_path=None, ns_=None):
270  output_str = ""
271  while not rospy.has_param('/robot_description'):
272  rospy.sleep(0.5)
273  rospy.loginfo("waiting for robot_description")
274  urdf_str = rospy.get_param('/robot_description')
275  robot_urdf = URDF.from_xml_string(urdf_str)
276 
277  for manipulator in robot_config.manipulators:
278  if manipulator.has_arm:
279  arm_yaml_path = manipulator.arm.moveit_path + "/" + "kinematics.yaml"
280  with open(arm_yaml_path, 'r') as stream:
281  arm_yamldoc = yaml.load(stream)
282  if manipulator.arm.extra_groups_config_path:
283  arm_yaml_extra_groups_path = (manipulator.arm.extra_groups_config_path + "/" +
284  "kinematics_extra_groups.yaml")
285  with open(arm_yaml_extra_groups_path, 'r') as stream:
286  arm_yamldoc_extra_groups = yaml.load(stream)
287  prefix = manipulator.arm.prefix
288  for group in robot.groups:
289  group_name = group.name
290  if group_name == manipulator.arm.internal_name:
291  group_name = manipulator.arm.main_group
292  group_prefix = prefix
293  elif manipulator.arm.internal_name in group_name:
294  group_prefix, group_name = group_name.split("_", 1)
295  group_prefix = prefix
296  else:
297  group_name = group.name[len(prefix):]
298  group_prefix = group.name[:len(prefix)]
299 
300  if group_name in arm_yamldoc and group_prefix == prefix:
301  kinematics_config = arm_yamldoc[group_name]
302  if prefix:
303  if "tip_name" in kinematics_config:
304  tip_name = kinematics_config["tip_name"]
305  kinematics_config["tip_name"] = prefix + tip_name
306  if "root_name" in kinematics_config:
307  root_name = kinematics_config["root_name"]
308  kinematics_config["root_name"] = prefix + root_name
309 
310  output_str += group.name + ":\n"
311  output_str += yaml_reindent(yaml.dump(kinematics_config,
312  default_flow_style=False,
313  allow_unicode=True), 2)
314  output_str += "\n"
315  elif group_name in arm_yamldoc_extra_groups:
316  kinematics_config = arm_yamldoc_extra_groups[group_name]
317  if prefix:
318  if "tip_name" in kinematics_config:
319  tip_name = kinematics_config["tip_name"]
320  kinematics_config["tip_name"] = prefix + tip_name
321  if "root_name" in kinematics_config:
322  root_name = kinematics_config["root_name"]
323  kinematics_config["root_name"] = prefix + root_name
324 
325  output_str += group.name + ":\n"
326  output_str += yaml_reindent(yaml.dump(kinematics_config,
327  default_flow_style=False,
328  allow_unicode=True), 2)
329  output_str += "\n"
330 
331  if manipulator.has_hand:
332  # open hand template files
333  with open(hand_template_path, 'r') as stream:
334  hand_yamldoc = yaml.load(stream)
335 
336  if 'kinematics_template' in hand_template_path:
337  default_solver_for_fixed_joint = "trac_ik"
338  fixed_joint_template_path = rospkg.RosPack().get_path(
339  'sr_moveit_hand_config') + "/config/kinematics_" + default_solver_for_fixed_joint + "_template.yaml"
340  with open(fixed_joint_template_path, 'r') as stream:
341  hand_yamldoc_fixed_joint = yaml.load(stream)
342  else:
343  hand_yamldoc_fixed_joint = deepcopy(hand_yamldoc)
344 
345  prefix = manipulator.hand.prefix
346  finger_prefixes = ["FF", "MF", "RF", "LF", "TH"]
347 
348  # Find in any finger has a fix joint apart from the tip as it needs to use a different kinematics
349  is_fixed = {"first_finger": False, "middle_finger": False, "ring_finger": False,
350  "little_finger": False, "thumb": False}
351  finger_with_fixed_joint = [False, False, False, False, False]
352  for joint in robot_urdf.joints:
353  joint_name = joint.name[len(prefix):]
354  for index, finger_prefix in enumerate(finger_prefixes):
355  if joint_name[0:2].upper() == finger_prefix and joint_name[-3:] != "tip" and joint.type == "fixed":
356  finger_with_fixed_joint[index] = True
357  is_fixed['first_finger'] = finger_with_fixed_joint[0]
358  is_fixed['middle_finger'] = finger_with_fixed_joint[1]
359  is_fixed['ring_finger'] = finger_with_fixed_joint[2]
360  is_fixed['little_finger'] = finger_with_fixed_joint[3]
361  is_fixed['thumb'] = finger_with_fixed_joint[4]
362 
363  for group in robot.groups:
364  kinematics_config = None
365  group_name = group.name
366  if group_name == manipulator.hand.internal_name:
367  group_name = "hand"
368  group_prefix = prefix
369  else:
370  group_name = group.name[len(prefix):]
371  group_prefix = group.name[:len(prefix)]
372 
373  if group_name in hand_yamldoc and group_prefix == prefix:
374  if is_fixed.get(group_name):
375  kinematics_config = hand_yamldoc_fixed_joint[group_name]
376  else:
377  kinematics_config = hand_yamldoc[group_name]
378 
379  if kinematics_config is not None:
380  if prefix:
381  if "tip_name" in kinematics_config:
382  tip_name = kinematics_config["tip_name"]
383  kinematics_config["tip_name"] = prefix + tip_name
384  if "root_name" in kinematics_config:
385  root_name = kinematics_config["root_name"]
386  kinematics_config["root_name"] = prefix + root_name
387 
388  output_str += group.name + ":\n"
389  output_str += yaml_reindent(yaml.dump(kinematics_config,
390  default_flow_style=False,
391  allow_unicode=True), 2)
392  output_str += "\n"
393  # load on param server or output to file
394  upload_output_params(output_str, output_path, ns_)
395 
396 
397 def generate_joint_limits(robot, robot_config, hand_template_path="joint_limits_template.yaml",
398  output_path=None, ns_=None):
399  output_str = ""
400  output_str += "joint_limits:\n"
401  for manipulator in robot_config.manipulators:
402  if manipulator.has_arm:
403  # Read arm srdf
404  arm_yaml_path = manipulator.arm.moveit_path + "/" + "joint_limits.yaml"
405  with open(arm_yaml_path, 'r') as stream:
406  arm_yamldoc = yaml.load(stream)
407  for joint in arm_yamldoc["joint_limits"]:
408  joint_limits_config = arm_yamldoc["joint_limits"][joint]
409  output_str += " " + manipulator.arm.prefix + joint + ":\n"
410  joint_limits_dump = yaml.dump(
411  joint_limits_config,
412  default_flow_style=False,
413  allow_unicode=True)
414  output_str += yaml_reindent(joint_limits_dump, 4)
415  output_str += "\n"
416  if manipulator.has_hand:
417  with open(hand_template_path, 'r') as stream:
418  hand_yamldoc = yaml.load(stream)
419  group_name = manipulator.hand.internal_name
420  if group_name is not None:
421  # for each joint in full hand group
422  for joint in robot.group_map[group_name].joints:
423  joint_name = joint.name[-4:]
424  if joint_name in hand_yamldoc["joint_limits"]:
425  joint_limits_config = hand_yamldoc["joint_limits"][joint_name]
426  output_str += " " + joint.name + ":\n"
427  joint_limits_dump = yaml.dump(
428  joint_limits_config,
429  default_flow_style=False,
430  allow_unicode=True)
431  output_str += yaml_reindent(joint_limits_dump, 4)
432  output_str += "\n"
433  stream.close()
434 
435  # load on param server or output to file
436  upload_output_params(output_str, output_path, ns_)
437 
438 if __name__ == '__main__':
439 
440  PARSER = argparse.ArgumentParser(usage='Load an SRDF file')
441  PARSER.add_argument('file', type=argparse.FileType('r'), nargs='?',
442  default=None,
443  help='File to load. Use - for stdin')
444  ARGS = PARSER.parse_args()
445 
446  if ARGS.file is not None:
447 
448  ROBOT = SRDF.from_xml_string(ARGS.file.read())
449  generate_fake_controllers(ROBOT,
450  output_path="fake_controllers.yaml")
451  generate_real_controllers(ROBOT,
452  output_path="controllers.yaml")
453  generate_ompl_planning(ROBOT,
454  "ompl_planning_template.yaml",
455  output_path="ompl_planning.yaml")
456  generate_kinematics(ROBOT,
457  "kinematics_template.yaml",
458  output_path="kinematics.yaml")
459  generate_joint_limits(ROBOT,
460  "joint_limits_template.yaml",
461  output_path="joint_limits.yaml")
462  else:
463  rospy.logerr("No SRDF file provided")
generate_moveit_config.generate_kinematics
def generate_kinematics(robot, robot_config, hand_template_path="kinematics_template.yaml", output_path=None, ns_=None)
Definition: generate_moveit_config.py:269
generate_moveit_config.upload_output_params
def upload_output_params(upload_str, output_path=None, upload=True, ns_=None)
Definition: generate_moveit_config.py:55
generate_moveit_config.generate_ompl_planning
def generate_ompl_planning(robot, robot_config, hand_template_path="ompl_planning_template.yaml", output_path=None, ns_=None)
Definition: generate_moveit_config.py:155
urdf_parser_py::urdf
generate_moveit_config.generate_real_controllers
def generate_real_controllers(robot, robot_config, output_path=None, ns_=None)
Definition: generate_moveit_config.py:107
generate_moveit_config.generate_fake_controllers
def generate_fake_controllers(robot, robot_config, output_path=None, ns_=None)
Definition: generate_moveit_config.py:67
generate_moveit_config.yaml_reindent
def yaml_reindent(in_str, numspaces)
Definition: generate_moveit_config.py:50
generate_moveit_config.generate_joint_limits
def generate_joint_limits(robot, robot_config, hand_template_path="joint_limits_template.yaml", output_path=None, ns_=None)
Definition: generate_moveit_config.py:398

sr_multi_moveit_config
Author(s): MoveIt Setup Assistant
autogenerated on Wed Oct 14 2020 04:05:22