moveit2
The MoveIt Motion Planning Framework for ROS 2.
moveit_configs_builder.py
Go to the documentation of this file.
1 """Simplify loading moveit config parameters.
2 
3 This module provides builder-pattern based class to simplify loading moveit related parameters found in
4 robot_moveit_config package generated by moveit setup assistant.
5 
6 By default it expects the following structure for the moveit configs package
7 
8 robot_name_moveit_config/
9  .setup_assistant -> Used to retrieve information about the SRDF file and
10  the URDF file used when generating the package
11  config/
12  kinematics.yaml -> IK solver's parameters
13  joint_limits.yaml -> Overriding position/velocity/acceleration limits from the URDF file
14  moveit_cpp.yaml -> MoveItCpp related parameters
15  *_planning.yaml -> planning pipelines parameters
16  pilz_cartesian_limits.yaml -> Pilz planner parameters
17  moveit_controllers.yaml -> trajectory execution manager's parameters
18  ...
19 
20 Example:
21  moveit_configs = MoveItConfigsBuilder("robot_name").to_moveit_configs()
22  ...
23  moveit_configs.package_path
24  moveit_configs.robot_description
25  moveit_configs.robot_description_semantic
26  moveit_configs.robot_description_kinematics
27  moveit_configs.planning_pipelines
28  moveit_configs.trajectory_execution
29  moveit_configs.planning_scene_monitor
30  moveit_configs.sensors_3d
31  moveit_configs.move_group_capabilities
32  moveit_configs.joint_limits
33  moveit_configs.moveit_cpp
34  moveit_configs.pilz_cartesian_limits
35  # Or to get all the parameters as a dictionary
36  moveit_configs.to_dict()
37 
38 Each function in MoveItConfigsBuilder has a file_path as an argument which is used to override the default
39 path for the file
40 
41 Example:
42  moveit_configs = MoveItConfigsBuilder("robot_name")
43  # Relative to robot_name_moveit_configs
44  .robot_description_semantic(Path("my_config") / "my_file.srdf")
45  .to_moveit_configs()
46  # Or
47  moveit_configs = MoveItConfigsBuilder("robot_name")
48  # Absolute path to robot_name_moveit_config
49  .robot_description_semantic(Path.home() / "my_config" / "new_file.srdf")
50  .to_moveit_configs()
51 """
52 
53 from pathlib import Path
54 from typing import Optional, List, Dict
55 import logging
56 import re
57 from dataclasses import dataclass, field
58 from ament_index_python.packages import get_package_share_directory
59 
60 from launch_param_builder import ParameterBuilder, load_yaml, load_xacro
61 from launch_param_builder.utils import ParameterBuilderFileNotFoundError
62 from moveit_configs_utils.substitutions import Xacro
63 from launch.some_substitutions_type import SomeSubstitutionsType
64 from launch_ros.parameter_descriptions import ParameterValue
65 
66 moveit_configs_utils_path = Path(get_package_share_directory("moveit_configs_utils"))
67 
68 
69 def get_pattern_matches(folder, pattern):
70  """Given all the files in the folder, find those that match the pattern.
71 
72  If there are groups defined, the groups are returned. Otherwise the path to the matches are returned.
73  """
74  matches = []
75  if not folder.exists():
76  return matches
77  for child in folder.iterdir():
78  if not child.is_file():
79  continue
80  m = pattern.search(child.name)
81  if m:
82  groups = m.groups()
83  if groups:
84  matches.append(groups[0])
85  else:
86  matches.append(child)
87  return matches
88 
89 
90 @dataclass(slots=True)
92  """Class containing MoveIt related parameters."""
93 
94  # A pathlib Path to the moveit config package
95  package_path: Optional[str] = None
96  # A dictionary that has the contents of the URDF file.
97  robot_description: Dict = field(default_factory=dict)
98  # A dictionary that has the contents of the SRDF file.
99  robot_description_semantic: Dict = field(default_factory=dict)
100  # A dictionary IK solver specific parameters.
101  robot_description_kinematics: Dict = field(default_factory=dict)
102  # A dictionary that contains the planning pipelines parameters.
103  planning_pipelines: Dict = field(default_factory=dict)
104  # A dictionary contains parameters for trajectory execution & moveit controller managers.
105  trajectory_execution: Dict = field(default_factory=dict)
106  # A dictionary that has the planning scene monitor's parameters.
107  planning_scene_monitor: Dict = field(default_factory=dict)
108  # A dictionary that has the sensor 3d configuration parameters.
109  sensors_3d: Dict = field(default_factory=dict)
110  # A dictionary containing move_group's non-default capabilities.
111  move_group_capabilities: Dict = field(
112  default_factory=lambda: {"capabilities": "", "disable_capabilities": ""}
113  )
114  # A dictionary containing the overridden position/velocity/acceleration limits.
115  joint_limits: Dict = field(default_factory=dict)
116  # A dictionary containing MoveItCpp related parameters.
117  moveit_cpp: Dict = field(default_factory=dict)
118  # A dictionary containing the cartesian limits for the Pilz planner.
119  pilz_cartesian_limits: Dict = field(default_factory=dict)
120 
121  def to_dict(self):
122  parameters = {}
123  parameters.update(self.robot_description)
124  parameters.update(self.robot_description_semantic)
125  parameters.update(self.robot_description_kinematics)
126  parameters.update(self.planning_pipelines)
127  parameters.update(self.trajectory_execution)
128  parameters.update(self.planning_scene_monitor)
129  parameters.update(self.sensors_3d)
130  parameters.update(self.joint_limits)
131  parameters.update(self.moveit_cpp)
132  # Update robot_description_planning with pilz cartesian limits
133  if self.pilz_cartesian_limits:
134  parameters["robot_description_planning"].update(
135  self.pilz_cartesian_limits["robot_description_planning"]
136  )
137  return parameters
138 
139 
140 class MoveItConfigsBuilder(ParameterBuilder):
141  __moveit_configs: MoveItConfigs
142  __robot_name: str
143  __urdf_package: Path
144  # Relative path of the URDF file w.r.t. __urdf_package
145  __urdf_file_path: Path
146  # Relative path of the SRDF file w.r.t. robot_name_moveit_config
147  __srdf_file_path: Path
148  # String specify the parameter name that the robot description will be loaded to, it will also be used as a prefix
149  # for "_planning", "_semantic", and "_kinematics"
150  __robot_description: str
151  __config_dir_path = Path("config")
152 
153  # Look-up for robot_name_moveit_config package
154  def __init__(
155  self,
156  robot_name: str,
157  robot_description="robot_description",
158  package_name: Optional[str] = None,
159  ):
160  super().__init__(package_name or (robot_name + "_moveit_config"))
161  self.__moveit_configs__moveit_configs = MoveItConfigs(package_path=self._package_path)
162  self.__robot_name__robot_name = robot_name
163  setup_assistant_file = self._package_path / ".setup_assistant"
164 
165  self.__urdf_package__urdf_package = None
166  self.__urdf_file_path__urdf_file_path = None
167  self.__urdf_xacro_args__urdf_xacro_args = None
168  self.__srdf_file_path__srdf_file_path = None
169 
170  modified_urdf_path = Path("config") / (self.__robot_name__robot_name + ".urdf.xacro")
171  if (self._package_path / modified_urdf_path).exists():
172  self.__urdf_package__urdf_package = self._package_path
173  self.__urdf_file_path__urdf_file_path = modified_urdf_path
174 
175  if setup_assistant_file.exists():
176  setup_assistant_yaml = load_yaml(setup_assistant_file)
177  config = setup_assistant_yaml.get("moveit_setup_assistant_config", {})
178 
179  if urdf_config := config.get("urdf", config.get("URDF")):
180  if self.__urdf_package__urdf_package is None:
181  self.__urdf_package__urdf_package = Path(
182  get_package_share_directory(urdf_config["package"])
183  )
184  self.__urdf_file_path__urdf_file_path = Path(urdf_config["relative_path"])
185 
186  if xacro_args := urdf_config.get("xacro_args"):
187  self.__urdf_xacro_args__urdf_xacro_args = dict(
188  arg.split(":=") for arg in xacro_args.split(" ") if arg
189  )
190 
191  if srdf_config := config.get("srdf", config.get("SRDF")):
192  self.__srdf_file_path__srdf_file_path = Path(srdf_config["relative_path"])
193 
194  if not self.__urdf_package__urdf_package or not self.__urdf_file_path__urdf_file_path:
195  logging.warning(
196  f"\x1b[33;21mCannot infer URDF from `{self._package_path}`. -- using config/{robot_name}.urdf\x1b[0m"
197  )
198  self.__urdf_package__urdf_package = self._package_path
199  self.__urdf_file_path__urdf_file_path = self.__config_dir_path__config_dir_path / (
200  self.__robot_name__robot_name + ".urdf"
201  )
202 
203  if not self.__srdf_file_path__srdf_file_path:
204  logging.warning(
205  f"\x1b[33;21mCannot infer SRDF from `{self._package_path}`. -- using config/{robot_name}.srdf\x1b[0m"
206  )
207  self.__srdf_file_path__srdf_file_path = self.__config_dir_path__config_dir_path / (
208  self.__robot_name__robot_name + ".srdf"
209  )
210 
211  self.__robot_description__robot_description = robot_description
212 
214  self,
215  file_path: Optional[str] = None,
216  mappings: dict[SomeSubstitutionsType, SomeSubstitutionsType] = None,
217  ):
218  """Load robot description.
219 
220  :param file_path: Absolute or relative path to the URDF file (w.r.t. robot_name_moveit_config).
221  :param mappings: mappings to be passed when loading the xacro file.
222  :return: Instance of MoveItConfigsBuilder with robot_description loaded.
223  """
224  if file_path is None:
225  robot_description_file_path = self.__urdf_package__urdf_package / self.__urdf_file_path__urdf_file_path
226  else:
227  robot_description_file_path = self._package_path / file_path
228  if (mappings is None) or all(
229  (isinstance(key, str) and isinstance(value, str))
230  for key, value in mappings.items()
231  ):
232  try:
233  self.__moveit_configs__moveit_configs.robot_description = {
234  self.__robot_description__robot_description: load_xacro(
235  robot_description_file_path,
236  mappings=mappings or self.__urdf_xacro_args__urdf_xacro_args,
237  )
238  }
239  except ParameterBuilderFileNotFoundError as e:
240  logging.warning(f"\x1b[33;21m{e}\x1b[0m")
241  logging.warning(
242  f"\x1b[33;21mThe robot description will be loaded from /robot_description topic \x1b[0m"
243  )
244 
245  else:
246  self.__moveit_configs__moveit_configs.robot_description = {
247  self.__robot_description__robot_description: ParameterValue(
248  Xacro(str(robot_description_file_path), mappings=mappings),
249  value_type=str,
250  )
251  }
252  return self
253 
255  self,
256  file_path: Optional[str] = None,
257  mappings: dict[SomeSubstitutionsType, SomeSubstitutionsType] = None,
258  ):
259  """Load semantic robot description.
260 
261  :param file_path: Absolute or relative path to the SRDF file (w.r.t. robot_name_moveit_config).
262  :param mappings: mappings to be passed when loading the xacro file.
263  :return: Instance of MoveItConfigsBuilder with robot_description_semantic loaded.
264  """
265 
266  if (mappings is None) or all(
267  (isinstance(key, str) and isinstance(value, str))
268  for key, value in mappings.items()
269  ):
270  self.__moveit_configs__moveit_configs.robot_description_semantic = {
271  self.__robot_description__robot_description
272  + "_semantic": load_xacro(
273  self._package_path / (file_path or self.__srdf_file_path__srdf_file_path),
274  mappings=mappings,
275  )
276  }
277  else:
278  self.__moveit_configs__moveit_configs.robot_description_semantic = {
279  self.__robot_description__robot_description
280  + "_semantic": ParameterValue(
281  Xacro(
282  str(self._package_path / (file_path or self.__srdf_file_path__srdf_file_path)),
283  mappings=mappings,
284  ),
285  value_type=str,
286  )
287  }
288  return self
289 
290  def robot_description_kinematics(self, file_path: Optional[str] = None):
291  """Load IK solver parameters.
292 
293  :param file_path: Absolute or relative path to the kinematics yaml file (w.r.t. robot_name_moveit_config).
294  :return: Instance of MoveItConfigsBuilder with robot_description_kinematics loaded.
295  """
296  self.__moveit_configs__moveit_configs.robot_description_kinematics = {
297  self.__robot_description__robot_description
298  + "_kinematics": load_yaml(
299  self._package_path
300  / (file_path or self.__config_dir_path__config_dir_path / "kinematics.yaml")
301  )
302  }
303  return self
304 
305  def joint_limits(self, file_path: Optional[str] = None):
306  """Load joint limits overrides.
307 
308  :param file_path: Absolute or relative path to the joint limits yaml file (w.r.t. robot_name_moveit_config).
309  :return: Instance of MoveItConfigsBuilder with robot_description_planning loaded.
310  """
311  self.__moveit_configs__moveit_configs.joint_limits = {
312  self.__robot_description__robot_description
313  + "_planning": load_yaml(
314  self._package_path
315  / (file_path or self.__config_dir_path__config_dir_path / "joint_limits.yaml")
316  )
317  }
318  return self
319 
320  def moveit_cpp(self, file_path: Optional[str] = None):
321  """Load MoveItCpp parameters.
322 
323  :param file_path: Absolute or relative path to the MoveItCpp yaml file (w.r.t. robot_name_moveit_config).
324  :return: Instance of MoveItConfigsBuilder with moveit_cpp loaded.
325  """
326  self.__moveit_configs__moveit_configs.moveit_cpp = load_yaml(
327  self._package_path
328  / (file_path or self.__config_dir_path__config_dir_path / "moveit_cpp.yaml")
329  )
330  return self
331 
333  self,
334  file_path: Optional[str] = None,
335  moveit_manage_controllers: bool = True,
336  ):
337  """Load trajectory execution and moveit controller managers' parameters
338 
339  :param file_path: Absolute or relative path to the controllers yaml file (w.r.t. robot_name_moveit_config).
340  :param moveit_manage_controllers: Whether trajectory execution manager is allowed to switch controllers' states.
341  :return: Instance of MoveItConfigsBuilder with trajectory_execution loaded.
342  """
343  self.__moveit_configs__moveit_configs.trajectory_execution = {
344  "moveit_manage_controllers": moveit_manage_controllers,
345  }
346 
347  # Find the most likely controller params as needed
348  if file_path is None:
349  config_folder = self._package_path / self.__config_dir_path__config_dir_path
350  controller_pattern = re.compile("^(.*)_controllers.yaml$")
351  possible_names = get_pattern_matches(config_folder, controller_pattern)
352  if not possible_names:
353  # Warn the user instead of raising exception
354  logging.warning(
355  "\x1b[33;20mtrajectory_execution: `Parameter file_path is undefined "
356  f"and no matches for {config_folder}/*_controllers.yaml\x1b[0m"
357  )
358  else:
359  chosen_name = None
360  if len(possible_names) == 1:
361  chosen_name = possible_names[0]
362  else:
363  # Try a couple other common names, in order of precedence
364  for name in ["moveit", "moveit2", self.__robot_name__robot_name]:
365  if name in possible_names:
366  chosen_name = name
367  break
368  else:
369  option_str = "\n - ".join(
370  name + "_controllers.yaml" for name in possible_names
371  )
372  raise RuntimeError(
373  "trajectory_execution: "
374  f"Unable to guess which parameter file to load. Options:\n - {option_str}"
375  )
376  file_path = config_folder / (chosen_name + "_controllers.yaml")
377 
378  else:
379  file_path = self._package_path / file_path
380 
381  if file_path:
382  self.__moveit_configs__moveit_configs.trajectory_execution.update(load_yaml(file_path))
383  return self
384 
386  self,
387  publish_planning_scene: bool = True,
388  publish_geometry_updates: bool = True,
389  publish_state_updates: bool = True,
390  publish_transforms_updates: bool = True,
391  publish_robot_description: bool = False,
392  publish_robot_description_semantic: bool = False,
393  ):
394  self.__moveit_configs__moveit_configs.planning_scene_monitor = {
395  # TODO: Fix parameter namespace upstream -- see planning_scene_monitor.cpp:262
396  # "planning_scene_monitor": {
397  "publish_planning_scene": publish_planning_scene,
398  "publish_geometry_updates": publish_geometry_updates,
399  "publish_state_updates": publish_state_updates,
400  "publish_transforms_updates": publish_transforms_updates,
401  "publish_robot_description": publish_robot_description,
402  "publish_robot_description_semantic": publish_robot_description_semantic,
403  # }
404  }
405  return self
406 
407  def sensors_3d(self, file_path: Optional[str] = None):
408  """Load sensors_3d parameters.
409 
410  :param file_path: Absolute or relative path to the sensors_3d yaml file (w.r.t. robot_name_moveit_config).
411  :return: Instance of MoveItConfigsBuilder with robot_description_planning loaded.
412  """
413  sensors_path = self._package_path / (
414  file_path or self.__config_dir_path__config_dir_path / "sensors_3d.yaml"
415  )
416  if sensors_path.exists():
417  sensors_data = load_yaml(sensors_path)
418  # TODO(mikeferguson): remove the second part of this check once
419  # https://github.com/ros-planning/moveit_resources/pull/141 has made through buildfarm
420  if len(sensors_data["sensors"]) > 0 and sensors_data["sensors"][0]:
421  self.__moveit_configs__moveit_configs.sensors_3d = sensors_data
422  return self
423 
425  self,
426  default_planning_pipeline: str = None,
427  pipelines: List[str] = None,
428  load_all: bool = True,
429  ):
430  """Load planning pipelines parameters.
431 
432  :param default_planning_pipeline: Name of the default planning pipeline.
433  :param pipelines: List of the planning pipelines to be loaded.
434  :param load_all: Only used if pipelines is None.
435  If true, loads all pipelines defined in config package AND this package.
436  If false, only loads the pipelines defined in config package.
437  :return: Instance of MoveItConfigsBuilder with planning_pipelines loaded.
438  """
439  config_folder = self._package_path / self.__config_dir_path__config_dir_path
440  default_folder = moveit_configs_utils_path / "default_configs"
441 
442  # If no pipelines are specified, search by filename
443  if pipelines is None:
444  planning_pattern = re.compile("^(.*)_planning.yaml$")
445  pipelines = get_pattern_matches(config_folder, planning_pattern)
446  if load_all:
447  for pipeline in get_pattern_matches(default_folder, planning_pattern):
448  if pipeline not in pipelines:
449  pipelines.append(pipeline)
450 
451  # Define default pipeline as needed
452  if not default_planning_pipeline:
453  if "ompl" in pipelines:
454  default_planning_pipeline = "ompl"
455  else:
456  default_planning_pipeline = pipelines[0]
457 
458  if default_planning_pipeline not in pipelines:
459  raise RuntimeError(
460  f"default_planning_pipeline: `{default_planning_pipeline}` doesn't name any of the input pipelines "
461  f"`{','.join(pipelines)}`"
462  )
463  self.__moveit_configs__moveit_configs.planning_pipelines = {
464  "planning_pipelines": pipelines,
465  "default_planning_pipeline": default_planning_pipeline,
466  }
467  for pipeline in pipelines:
468  parameter_file = config_folder / (pipeline + "_planning.yaml")
469  if not parameter_file.exists():
470  parameter_file = default_folder / (pipeline + "_planning.yaml")
471  self.__moveit_configs__moveit_configs.planning_pipelines[pipeline] = load_yaml(
472  parameter_file
473  )
474 
475  # Special rule to add ompl planner_configs
476  if "ompl" in self.__moveit_configs__moveit_configs.planning_pipelines:
477  ompl_config = self.__moveit_configs__moveit_configs.planning_pipelines["ompl"]
478  if "planner_configs" not in ompl_config:
479  ompl_config.update(load_yaml(default_folder / "ompl_defaults.yaml"))
480 
481  return self
482 
483  def pilz_cartesian_limits(self, file_path: Optional[str] = None):
484  """Load cartesian limits.
485 
486  :param file_path: Absolute or relative path to the cartesian limits file (w.r.t. robot_name_moveit_config).
487  :return: Instance of MoveItConfigsBuilder with pilz_cartesian_limits loaded.
488  """
489  deprecated_path = self._package_path / (
490  self.__config_dir_path__config_dir_path / "cartesian_limits.yaml"
491  )
492  if deprecated_path.exists():
493  logging.warning(
494  f"\x1b[33;21mcartesian_limits.yaml is deprecated, please rename to pilz_cartesian_limits.yaml\x1b[0m"
495  )
496 
497  self.__moveit_configs__moveit_configs.pilz_cartesian_limits = {
498  self.__robot_description__robot_description
499  + "_planning": load_yaml(
500  self._package_path
501  / (file_path or self.__config_dir_path__config_dir_path / "pilz_cartesian_limits.yaml")
502  )
503  }
504  return self
505 
506  def to_moveit_configs(self):
507  """Get MoveIt configs from robot_name_moveit_config.
508 
509  :return: An MoveItConfigs instance with all parameters loaded.
510  """
511  if not self.__moveit_configs__moveit_configs.robot_description:
512  self.robot_descriptionrobot_description()
513  if not self.__moveit_configs__moveit_configs.robot_description_semantic:
514  self.robot_description_semanticrobot_description_semantic()
515  if not self.__moveit_configs__moveit_configs.robot_description_kinematics:
516  self.robot_description_kinematicsrobot_description_kinematics()
517  if not self.__moveit_configs__moveit_configs.planning_pipelines:
518  self.planning_pipelinesplanning_pipelines()
519  if not self.__moveit_configs__moveit_configs.trajectory_execution:
520  self.trajectory_executiontrajectory_execution()
521  if not self.__moveit_configs__moveit_configs.planning_scene_monitor:
522  self.planning_scene_monitorplanning_scene_monitor()
523  if not self.__moveit_configs__moveit_configs.sensors_3d:
524  self.sensors_3dsensors_3d()
525  if not self.__moveit_configs__moveit_configs.joint_limits:
526  self.joint_limitsjoint_limits()
527  # TODO(JafarAbdi): We should have a default moveit_cpp.yaml as port of a moveit config package
528  # if not self.__moveit_configs.moveit_cpp:
529  # self.moveit_cpp()
530  if "pilz_industrial_motion_planner" in self.__moveit_configs__moveit_configs.planning_pipelines:
531  if not self.__moveit_configs__moveit_configs.pilz_cartesian_limits:
532  self.pilz_cartesian_limitspilz_cartesian_limits()
533  return self.__moveit_configs__moveit_configs
534 
535  def to_dict(self, include_moveit_configs: bool = True):
536  """Get loaded parameters from robot_name_moveit_config as a dictionary.
537 
538  :param include_moveit_configs: Whether to include the MoveIt config parameters or
539  only the ones from ParameterBuilder
540  :return: Dictionary with all parameters loaded.
541  """
542  parameters = self._parameters
543  if include_moveit_configs:
544  parameters.update(self.to_moveit_configsto_moveit_configs().to_dict())
545  return parameters
def __init__(self, str robot_name, robot_description="robot_description", Optional[str] package_name=None)
def planning_scene_monitor(self, bool publish_planning_scene=True, bool publish_geometry_updates=True, bool publish_state_updates=True, bool publish_transforms_updates=True, bool publish_robot_description=False, bool publish_robot_description_semantic=False)
def trajectory_execution(self, Optional[str] file_path=None, bool moveit_manage_controllers=True)
def robot_description(self, Optional[str] file_path=None, dict[SomeSubstitutionsType, SomeSubstitutionsType] mappings=None)
def planning_pipelines(self, str default_planning_pipeline=None, List[str] pipelines=None, bool load_all=True)
def robot_description_semantic(self, Optional[str] file_path=None, dict[SomeSubstitutionsType, SomeSubstitutionsType] mappings=None)
def load_yaml(package_name, file_path)