diff --git a/abb_bringup/launch/abb_moveit.launch.py b/abb_bringup/launch/abb_moveit.launch.py
index 5adb199..53ffa76 100644
--- a/abb_bringup/launch/abb_moveit.launch.py
+++ b/abb_bringup/launch/abb_moveit.launch.py
@@ -1,17 +1,15 @@
+import os
+
+import yaml
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.actions import OpaqueFunction
from launch.substitutions import (
- Command,
- FindExecutable,
LaunchConfiguration,
- PathJoinSubstitution,
)
from launch_ros.actions import Node
-from launch_ros.substitutions import FindPackageShare
-import os
-import yaml
+from moveit_configs_utils import MoveItConfigsBuilder
def load_yaml(package_name, file_path):
@@ -32,94 +30,84 @@ def launch_setup(context, *args, **kwargs):
moveit_config_package = LaunchConfiguration("moveit_config_package")
moveit_config_file = LaunchConfiguration("moveit_config_file")
- # Planning context
- robot_description_content = Command(
- [
- PathJoinSubstitution([FindExecutable(name="xacro")]),
- " ",
- PathJoinSubstitution(
- [FindPackageShare(support_package), "urdf", robot_xacro_file]
- ),
- ]
- )
- robot_description = {"robot_description": robot_description_content}
-
- robot_description_semantic_content = Command(
- [
- PathJoinSubstitution([FindExecutable(name="xacro")]),
- " ",
- PathJoinSubstitution(
- [FindPackageShare(moveit_config_package), "config", moveit_config_file]
- ),
- ]
- )
- robot_description_semantic = {
- "robot_description_semantic": robot_description_semantic_content.perform(
- context
+ # MoveIt configuration
+ moveit_config = (
+ MoveItConfigsBuilder(
+ "abb_bringup", package_name=f"{moveit_config_package.perform(context)}"
)
- }
-
- kinematics_yaml = load_yaml(
- "abb_irb1200_5_90_moveit_config", "config/kinematics.yaml"
- )
-
- joint_limits_yaml = {
- "robot_description_planning": load_yaml(
- moveit_config_package.perform(context), "config/joint_limits.yaml"
+ .robot_description(
+ file_path=os.path.join(
+ get_package_share_directory(f"{support_package.perform(context)}"),
+ "urdf",
+ f"{robot_xacro_file.perform(context)}",
+ )
)
- }
-
- # Planning Functionality
- ompl_planning_pipeline_config = {
- "move_group": {
- "planning_plugin": "ompl_interface/OMPLPlanner",
- "request_adapters": """default_planner_request_adapters/AddTimeOptimalParameterization default_planner_request_adapters/ResolveConstraintFrames default_planner_request_adapters/FixWorkspaceBounds default_planner_request_adapters/FixStartStateBounds default_planner_request_adapters/FixStartStateCollision default_planner_request_adapters/FixStartStatePathConstraints""",
- "start_state_max_bounds_error": 0.1,
- }
- }
- ompl_planning_yaml = load_yaml(
- "abb_irb1200_5_90_moveit_config", "config/ompl_planning.yaml"
+ .robot_description_semantic(
+ file_path=os.path.join(
+ get_package_share_directory(
+ f"{moveit_config_package.perform(context)}"
+ ),
+ "config",
+ f"{moveit_config_file.perform(context)}",
+ )
+ )
+ .planning_pipelines()
+ .robot_description_kinematics(
+ file_path=os.path.join(
+ get_package_share_directory(
+ f"{moveit_config_package.perform(context)}"
+ ),
+ "config",
+ "kinematics.yaml",
+ )
+ )
+ .trajectory_execution(
+ file_path=os.path.join(
+ get_package_share_directory(
+ f"{moveit_config_package.perform(context)}"
+ ),
+ "config",
+ "moveit_controllers.yaml",
+ )
+ )
+ .planning_scene_monitor(
+ publish_planning_scene=True,
+ publish_geometry_updates=True,
+ publish_state_updates=True,
+ publish_transforms_updates=True,
+ publish_robot_description=True,
+ publish_robot_description_semantic=True,
+ )
+ .joint_limits(
+ file_path=os.path.join(
+ get_package_share_directory(
+ f"{moveit_config_package.perform(context)}"
+ ),
+ "config",
+ "joint_limits.yaml",
+ )
+ )
+ .to_moveit_configs()
)
- ompl_planning_pipeline_config["move_group"].update(ompl_planning_yaml)
- # Trajectory Execution Functionality
- moveit_simple_controllers_yaml = load_yaml(
- "abb_irb1200_5_90_moveit_config", "config/moveit_controllers.yaml"
- )
+ # MoveIt controllers
moveit_controllers = {
- "moveit_simple_controller_manager": moveit_simple_controllers_yaml,
+ "moveit_simple_controller_manager": load_yaml(
+ f"{moveit_config_package.perform(context)}",
+ "config/moveit_controllers.yaml",
+ ),
"moveit_controller_manager": "moveit_simple_controller_manager/MoveItSimpleControllerManager",
}
- trajectory_execution = {
- # MoveIt does not handle controller switching automatically
- "moveit_manage_controllers": False,
- "trajectory_execution.allowed_execution_duration_scaling": 1.2,
- "trajectory_execution.allowed_goal_duration_margin": 0.5,
- "trajectory_execution.allowed_start_tolerance": 0.01,
- }
-
- planning_scene_monitor_parameters = {
- "publish_planning_scene": True,
- "publish_geometry_updates": True,
- "publish_state_updates": True,
- "publish_transforms_updates": True,
- }
-
# Start the actual move_group node/action server
move_group_node = Node(
package="moveit_ros_move_group",
executable="move_group",
output="screen",
parameters=[
- robot_description,
- robot_description_semantic,
- kinematics_yaml,
- ompl_planning_pipeline_config,
- trajectory_execution,
+ moveit_config.trajectory_execution,
moveit_controllers,
- planning_scene_monitor_parameters,
- joint_limits_yaml,
+ moveit_config.to_dict(),
],
)
@@ -135,11 +123,7 @@ def launch_setup(context, *args, **kwargs):
output="log",
arguments=["-d", rviz_config],
parameters=[
- robot_description,
- robot_description_semantic,
- ompl_planning_pipeline_config,
- kinematics_yaml,
- joint_limits_yaml,
+ moveit_config.to_dict(),
],
)
@@ -158,7 +142,7 @@ def launch_setup(context, *args, **kwargs):
executable="robot_state_publisher",
name="robot_state_publisher",
output="both",
- parameters=[robot_description],
+ parameters=[moveit_config.robot_description],
)
nodes_to_start = [move_group_node, rviz_node, static_tf_node, robot_state_pub_node]
@@ -166,7 +150,6 @@ def launch_setup(context, *args, **kwargs):
def generate_launch_description():
-
declared_arguments = []
# TODO(andyz): add other options
diff --git a/abb_bringup/package.xml b/abb_bringup/package.xml
index 9ad87b7..0598625 100644
--- a/abb_bringup/package.xml
+++ b/abb_bringup/package.xml
@@ -33,6 +33,7 @@
rviz2
urdf
xacro
+ moveit_configs_utils
ament_cmake
diff --git a/robot_specific_config/abb_irb1200_5_90_moveit_config/config/ompl_planning.yaml b/robot_specific_config/abb_irb1200_5_90_moveit_config/config/ompl_planning.yaml
index f4c8d82..1581cff 100644
--- a/robot_specific_config/abb_irb1200_5_90_moveit_config/config/ompl_planning.yaml
+++ b/robot_specific_config/abb_irb1200_5_90_moveit_config/config/ompl_planning.yaml
@@ -1,6 +1,6 @@
planning_plugin: ompl_interface/OMPLPlanner
request_adapters: >-
- default_planner_request_adapters/AddTimeParameterization
+ default_planner_request_adapters/AddTimeOptimalParameterization
default_planner_request_adapters/ResolveConstraintFrames
default_planner_request_adapters/FixWorkspaceBounds
default_planner_request_adapters/FixStartStateBounds
diff --git a/robot_specific_config/abb_irb1200_5_90_moveit_config/config/pilz_cartesian_limits.yaml b/robot_specific_config/abb_irb1200_5_90_moveit_config/config/pilz_cartesian_limits.yaml
new file mode 100644
index 0000000..c1e8d32
--- /dev/null
+++ b/robot_specific_config/abb_irb1200_5_90_moveit_config/config/pilz_cartesian_limits.yaml
@@ -0,0 +1,5 @@
+cartesian_limits:
+ max_trans_vel: 1.0
+ max_trans_acc: 2.25
+ max_trans_dec: -5.0
+ max_rot_vel: 1.57