1"""Simplify loading moveit config parameters.
3This module provides builder-pattern based class to simplify loading moveit related parameters found in
4robot_moveit_config package generated by moveit setup assistant.
6By default it expects the following structure for the moveit configs package
8robot_name_moveit_config/
9 .setup_assistant -> Used to retrieve information about the SRDF file and
10 the URDF file used when generating the package
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
21 moveit_configs = MoveItConfigsBuilder("robot_name").to_moveit_configs()
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()
38Each function in MoveItConfigsBuilder has a file_path as an argument which is used to override the default
42 moveit_configs = MoveItConfigsBuilder("robot_name")
43 # Relative to robot_name_moveit_configs
44 .robot_description_semantic(Path("my_config") / "my_file.srdf")
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")
53from pathlib
import Path
54from typing
import Optional, List, Dict
57from dataclasses
import dataclass, field
58from ament_index_python.packages
import get_package_share_directory
60from launch_param_builder
import ParameterBuilder, load_yaml, load_xacro
61from launch_param_builder.utils
import ParameterBuilderFileNotFoundError
63from launch.some_substitutions_type
import SomeSubstitutionsType
64from launch_ros.parameter_descriptions
import ParameterValue
66moveit_configs_utils_path = Path(get_package_share_directory(
"moveit_configs_utils"))
70 """Given all the files in the folder, find those that match the pattern.
72 If there are groups defined, the groups are returned. Otherwise the path to the matches are returned.
75 if not folder.exists():
77 for child
in folder.iterdir():
78 if not child.is_file():
80 m = pattern.search(child.name)
84 matches.append(groups[0])
92 """Class containing MoveIt related parameters."""
95 package_path: Optional[str] =
None
97 robot_description: Dict = field(default_factory=dict)
99 robot_description_semantic: Dict = field(default_factory=dict)
101 robot_description_kinematics: Dict = field(default_factory=dict)
103 planning_pipelines: Dict = field(default_factory=dict)
105 trajectory_execution: Dict = field(default_factory=dict)
107 planning_scene_monitor: Dict = field(default_factory=dict)
109 sensors_3d: Dict = field(default_factory=dict)
111 move_group_capabilities: Dict = field(default_factory=dict)
113 joint_limits: Dict = field(default_factory=dict)
115 moveit_cpp: Dict = field(default_factory=dict)
117 pilz_cartesian_limits: Dict = field(default_factory=dict)
132 parameters[
"robot_description_planning"].update(
139 __moveit_configs: MoveItConfigs
143 __urdf_file_path: Path
145 __srdf_file_path: Path
148 __robot_description: str
149 __config_dir_path = Path(
"config")
155 robot_description=
"robot_description",
156 package_name: Optional[str] =
None,
158 super().
__init__(package_name
or (robot_name +
"_moveit_config"))
161 setup_assistant_file = self._package_path /
".setup_assistant"
169 if (self._package_path / modified_urdf_path).exists():
173 if setup_assistant_file.exists():
174 setup_assistant_yaml = load_yaml(setup_assistant_file)
175 config = setup_assistant_yaml.get(
"moveit_setup_assistant_config", {})
177 if urdf_config := config.get(
"urdf", config.get(
"URDF")):
180 get_package_share_directory(urdf_config[
"package"])
184 if xacro_args := urdf_config.get(
"xacro_args"):
186 arg.split(
":=")
for arg
in xacro_args.split(
" ")
if arg
189 if srdf_config := config.get(
"srdf", config.get(
"SRDF")):
194 f
"\x1b[33;21mCannot infer URDF from `{self._package_path}`. -- using config/{robot_name}.urdf\x1b[0m"
203 f
"\x1b[33;21mCannot infer SRDF from `{self._package_path}`. -- using config/{robot_name}.srdf\x1b[0m"
213 file_path: Optional[str] =
None,
214 mappings: dict[SomeSubstitutionsType, SomeSubstitutionsType] =
None,
216 """Load robot description.
218 :param file_path: Absolute or relative path to the URDF file (w.r.t. robot_name_moveit_config).
219 :param mappings: mappings to be passed when loading the xacro file.
220 :return: Instance of MoveItConfigsBuilder with robot_description loaded.
222 if file_path
is None:
225 robot_description_file_path = self._package_path / file_path
226 if (mappings
is None)
or all(
227 (isinstance(key, str)
and isinstance(value, str))
228 for key, value
in mappings.items()
233 robot_description_file_path,
237 except ParameterBuilderFileNotFoundError
as e:
238 logging.warning(f
"\x1b[33;21m{e}\x1b[0m")
240 f
"\x1b[33;21mThe robot description will be loaded from /robot_description topic \x1b[0m"
246 Xacro(str(robot_description_file_path), mappings=mappings),
254 file_path: Optional[str] =
None,
255 mappings: dict[SomeSubstitutionsType, SomeSubstitutionsType] =
None,
257 """Load semantic robot description.
259 :param file_path: Absolute or relative path to the SRDF file (w.r.t. robot_name_moveit_config).
260 :param mappings: mappings to be passed when loading the xacro file.
261 :return: Instance of MoveItConfigsBuilder with robot_description_semantic loaded.
264 if (mappings
is None)
or all(
265 (isinstance(key, str)
and isinstance(value, str))
266 for key, value
in mappings.items()
270 +
"_semantic": load_xacro(
278 +
"_semantic": ParameterValue(
289 """Load IK solver parameters.
291 :param file_path: Absolute or relative path to the kinematics yaml file (w.r.t. robot_name_moveit_config).
292 :return: Instance of MoveItConfigsBuilder with robot_description_kinematics loaded.
296 +
"_kinematics": load_yaml(
304 """Load joint limits overrides.
306 :param file_path: Absolute or relative path to the joint limits yaml file (w.r.t. robot_name_moveit_config).
307 :return: Instance of MoveItConfigsBuilder with robot_description_planning loaded.
311 +
"_planning": load_yaml(
319 """Load MoveItCpp parameters.
321 :param file_path: Absolute or relative path to the MoveItCpp yaml file (w.r.t. robot_name_moveit_config).
322 :return: Instance of MoveItConfigsBuilder with moveit_cpp loaded.
332 file_path: Optional[str] =
None,
333 moveit_manage_controllers: bool =
True,
335 """Load trajectory execution and moveit controller managers' parameters
337 :param file_path: Absolute or relative path to the controllers yaml file (w.r.t. robot_name_moveit_config).
338 :param moveit_manage_controllers: Whether trajectory execution manager is allowed to switch controllers' states.
339 :return: Instance of MoveItConfigsBuilder with trajectory_execution loaded.
342 "moveit_manage_controllers": moveit_manage_controllers,
346 if file_path
is None:
348 controller_pattern = re.compile(
"^(.*)_controllers.yaml$")
350 if not possible_names:
353 "\x1b[33;20mtrajectory_execution: `Parameter file_path is undefined "
354 f
"and no matches for {config_folder}/*_controllers.yaml\x1b[0m"
358 if len(possible_names) == 1:
359 chosen_name = possible_names[0]
363 if name
in possible_names:
367 option_str =
"\n - ".join(
368 name +
"_controllers.yaml" for name
in possible_names
371 "trajectory_execution: "
372 f
"Unable to guess which parameter file to load. Options:\n - {option_str}"
374 file_path = config_folder / (chosen_name +
"_controllers.yaml")
377 file_path = self._package_path / file_path
385 publish_planning_scene: bool =
True,
386 publish_geometry_updates: bool =
True,
387 publish_state_updates: bool =
True,
388 publish_transforms_updates: bool =
True,
389 publish_robot_description: bool =
False,
390 publish_robot_description_semantic: bool =
False,
395 "publish_planning_scene": publish_planning_scene,
396 "publish_geometry_updates": publish_geometry_updates,
397 "publish_state_updates": publish_state_updates,
398 "publish_transforms_updates": publish_transforms_updates,
399 "publish_robot_description": publish_robot_description,
400 "publish_robot_description_semantic": publish_robot_description_semantic,
406 """Load sensors_3d parameters.
408 :param file_path: Absolute or relative path to the sensors_3d yaml file (w.r.t. robot_name_moveit_config).
409 :return: Instance of MoveItConfigsBuilder with robot_description_planning loaded.
411 sensors_path = self._package_path / (
414 if sensors_path.exists():
415 sensors_data = load_yaml(sensors_path)
418 if len(sensors_data[
"sensors"]) > 0
and sensors_data[
"sensors"][0]:
424 default_planning_pipeline: str =
None,
425 pipelines: List[str] =
None,
426 load_all: bool =
True,
428 """Load planning pipelines parameters.
430 :param default_planning_pipeline: Name of the default planning pipeline.
431 :param pipelines: List of the planning pipelines to be loaded.
432 :param load_all: Only used if pipelines is None.
433 If true, loads all pipelines defined in config package AND this package.
434 If false, only loads the pipelines defined in config package.
435 :return: Instance of MoveItConfigsBuilder with planning_pipelines loaded.
438 default_folder = moveit_configs_utils_path /
"default_configs"
441 if pipelines
is None:
442 planning_pattern = re.compile(
"^(.*)_planning.yaml$")
446 if pipeline
not in pipelines:
447 pipelines.append(pipeline)
450 if not default_planning_pipeline:
451 if "ompl" in pipelines:
452 default_planning_pipeline =
"ompl"
454 default_planning_pipeline = pipelines[0]
456 if default_planning_pipeline
not in pipelines:
458 f
"default_planning_pipeline: `{default_planning_pipeline}` doesn't name any of the input pipelines "
459 f
"`{','.join(pipelines)}`"
462 "planning_pipelines": pipelines,
463 "default_planning_pipeline": default_planning_pipeline,
465 for pipeline
in pipelines:
466 parameter_file = config_folder / (pipeline +
"_planning.yaml")
467 if not parameter_file.exists():
468 parameter_file = default_folder / (pipeline +
"_planning.yaml")
476 if "planner_configs" not in ompl_config:
477 ompl_config.update(load_yaml(default_folder /
"ompl_defaults.yaml"))
482 """Load cartesian limits.
484 :param file_path: Absolute or relative path to the cartesian limits file (w.r.t. robot_name_moveit_config).
485 :return: Instance of MoveItConfigsBuilder with pilz_cartesian_limits loaded.
487 deprecated_path = self._package_path / (
490 if deprecated_path.exists():
492 f
"\x1b[33;21mcartesian_limits.yaml is deprecated, please rename to pilz_cartesian_limits.yaml\x1b[0m"
497 +
"_planning": load_yaml(
505 """Get MoveIt configs from robot_name_moveit_config.
507 :return: An MoveItConfigs instance with all parameters loaded.
533 def to_dict(self, include_moveit_configs: bool =
True):
534 """Get loaded parameters from robot_name_moveit_config as a dictionary.
536 :param include_moveit_configs: Whether to include the MoveIt config parameters or
537 only the ones from ParameterBuilder
538 :return: Dictionary with all parameters loaded.
540 parameters = self._parameters
541 if include_moveit_configs:
robot_description_kinematics(self, Optional[str] file_path=None)
sensors_3d(self, Optional[str] file_path=None)
trajectory_execution(self, Optional[str] file_path=None, bool moveit_manage_controllers=True)
__init__(self, str robot_name, robot_description="robot_description", Optional[str] package_name=None)
to_dict(self, bool include_moveit_configs=True)
robot_description_semantic(self, Optional[str] file_path=None, dict[SomeSubstitutionsType, SomeSubstitutionsType] mappings=None)
MoveItConfigs __moveit_configs
joint_limits(self, Optional[str] file_path=None)
pilz_cartesian_limits(self, Optional[str] file_path=None)
planning_pipelines(self, str default_planning_pipeline=None, List[str] pipelines=None, bool load_all=True)
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)
robot_description(self, Optional[str] file_path=None, dict[SomeSubstitutionsType, SomeSubstitutionsType] mappings=None)
robot_description_semantic
Dict robot_description_kinematics
Dict pilz_cartesian_limits
Dict planning_scene_monitor
Dict robot_description_semantic
Dict trajectory_execution
robot_description_kinematics
get_pattern_matches(folder, pattern)