1 """Simplify loading moveit config parameters.
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.
6 By default it expects the following structure for the moveit configs package
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
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()
38 Each 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")
53 from pathlib
import Path
54 from typing
import Optional, List, Dict
57 from dataclasses
import dataclass, field
58 from ament_index_python.packages
import get_package_share_directory
60 from launch_param_builder
import ParameterBuilder, load_yaml, load_xacro
61 from launch_param_builder.utils
import ParameterBuilderFileNotFoundError
63 from launch.some_substitutions_type
import SomeSubstitutionsType
64 from launch_ros.parameter_descriptions
import ParameterValue
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])
90 @dataclass(slots=True)
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(
112 default_factory=
lambda: {
"capabilities":
"",
"disable_capabilities":
""}
115 joint_limits: Dict = field(default_factory=dict)
117 moveit_cpp: Dict = field(default_factory=dict)
119 pilz_cartesian_limits: Dict = field(default_factory=dict)
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)
133 if self.pilz_cartesian_limits:
134 parameters[
"robot_description_planning"].update(
135 self.pilz_cartesian_limits[
"robot_description_planning"]
141 __moveit_configs: MoveItConfigs
145 __urdf_file_path: Path
147 __srdf_file_path: Path
150 __robot_description: str
151 __config_dir_path = Path(
"config")
157 robot_description=
"robot_description",
158 package_name: Optional[str] =
None,
160 super().
__init__(package_name
or (robot_name +
"_moveit_config"))
163 setup_assistant_file = self._package_path /
".setup_assistant"
170 modified_urdf_path = Path(
"config") / (self.
__robot_name__robot_name +
".urdf.xacro")
171 if (self._package_path / modified_urdf_path).exists():
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", {})
179 if urdf_config := config.get(
"urdf", config.get(
"URDF")):
186 if xacro_args := urdf_config.get(
"xacro_args"):
188 arg.split(
":=")
for arg
in xacro_args.split(
" ")
if arg
191 if srdf_config := config.get(
"srdf", config.get(
"SRDF")):
196 f
"\x1b[33;21mCannot infer URDF from `{self._package_path}`. -- using config/{robot_name}.urdf\x1b[0m"
205 f
"\x1b[33;21mCannot infer SRDF from `{self._package_path}`. -- using config/{robot_name}.srdf\x1b[0m"
215 file_path: Optional[str] =
None,
216 mappings: dict[SomeSubstitutionsType, SomeSubstitutionsType] =
None,
218 """Load robot description.
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.
224 if file_path
is None:
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()
235 robot_description_file_path,
239 except ParameterBuilderFileNotFoundError
as e:
240 logging.warning(f
"\x1b[33;21m{e}\x1b[0m")
242 f
"\x1b[33;21mThe robot description will be loaded from /robot_description topic \x1b[0m"
248 Xacro(str(robot_description_file_path), mappings=mappings),
256 file_path: Optional[str] =
None,
257 mappings: dict[SomeSubstitutionsType, SomeSubstitutionsType] =
None,
259 """Load semantic robot description.
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.
266 if (mappings
is None)
or all(
267 (isinstance(key, str)
and isinstance(value, str))
268 for key, value
in mappings.items()
272 +
"_semantic": load_xacro(
280 +
"_semantic": ParameterValue(
282 str(self._package_path / (file_path
or self.
__srdf_file_path__srdf_file_path)),
291 """Load IK solver parameters.
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.
306 """Load joint limits overrides.
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.
321 """Load MoveItCpp parameters.
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.
334 file_path: Optional[str] =
None,
335 moveit_manage_controllers: bool =
True,
337 """Load trajectory execution and moveit controller managers' parameters
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.
344 "moveit_manage_controllers": moveit_manage_controllers,
348 if file_path
is None:
350 controller_pattern = re.compile(
"^(.*)_controllers.yaml$")
352 if not possible_names:
355 "\x1b[33;20mtrajectory_execution: `Parameter file_path is undefined "
356 f
"and no matches for {config_folder}/*_controllers.yaml\x1b[0m"
360 if len(possible_names) == 1:
361 chosen_name = possible_names[0]
364 for name
in [
"moveit",
"moveit2", self.
__robot_name__robot_name]:
365 if name
in possible_names:
369 option_str =
"\n - ".join(
370 name +
"_controllers.yaml" for name
in possible_names
373 "trajectory_execution: "
374 f
"Unable to guess which parameter file to load. Options:\n - {option_str}"
376 file_path = config_folder / (chosen_name +
"_controllers.yaml")
379 file_path = self._package_path / file_path
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,
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,
408 """Load sensors_3d parameters.
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.
413 sensors_path = self._package_path / (
416 if sensors_path.exists():
420 if len(sensors_data[
"sensors"]) > 0
and sensors_data[
"sensors"][0]:
426 default_planning_pipeline: str =
None,
427 pipelines: List[str] =
None,
428 load_all: bool =
True,
430 """Load planning pipelines parameters.
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.
440 default_folder = moveit_configs_utils_path /
"default_configs"
443 if pipelines
is None:
444 planning_pattern = re.compile(
"^(.*)_planning.yaml$")
448 if pipeline
not in pipelines:
449 pipelines.append(pipeline)
452 if not default_planning_pipeline:
453 if "ompl" in pipelines:
454 default_planning_pipeline =
"ompl"
456 default_planning_pipeline = pipelines[0]
458 if default_planning_pipeline
not in pipelines:
460 f
"default_planning_pipeline: `{default_planning_pipeline}` doesn't name any of the input pipelines "
461 f
"`{','.join(pipelines)}`"
464 "planning_pipelines": pipelines,
465 "default_planning_pipeline": default_planning_pipeline,
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")
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"))
484 """Load cartesian limits.
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.
489 deprecated_path = self._package_path / (
492 if deprecated_path.exists():
494 f
"\x1b[33;21mcartesian_limits.yaml is deprecated, please rename to pilz_cartesian_limits.yaml\x1b[0m"
501 / (file_path
or self.
__config_dir_path__config_dir_path /
"pilz_cartesian_limits.yaml")
507 """Get MoveIt configs from robot_name_moveit_config.
509 :return: An MoveItConfigs instance with all parameters loaded.
530 if "pilz_industrial_motion_planner" in self.
__moveit_configs__moveit_configs.planning_pipelines:
535 def to_dict(self, include_moveit_configs: bool =
True):
536 """Get loaded parameters from robot_name_moveit_config as a dictionary.
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.
542 parameters = self._parameters
543 if include_moveit_configs:
def to_moveit_configs(self)
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 pilz_cartesian_limits(self, Optional[str] file_path=None)
def joint_limits(self, Optional[str] file_path=None)
def sensors_3d(self, Optional[str] file_path=None)
def to_dict(self, bool include_moveit_configs=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 robot_description_kinematics(self, Optional[str] file_path=None)
def moveit_cpp(self, Optional[str] file_path=None)
def load_yaml(package_name, file_path)
def get_package_share_directory(pkg_name)
def get_pattern_matches(folder, pattern)