Fixed the wrong type for time parameterization in ompl (#55) (#65)

* Fixed the wrong type for time parameterization in ompl * Updated to use the MoveItConfigsBuilder * Optimized the imports. * Update abb_bringup/launch/abb_moveit.launch.py Removed hard coded config package * Update abb_bringup/launch/abb_moveit.launch.py added argument names for clarity. * Update package.xml Added moveit_configs_utils dependency to package.xml * Formatting adn also publishing robot_description and semantic * Fixed formatting to pass format test per requeset by Yadunund. * Used pre-commit to fix formatting locally. * Removed planning pipeline limits to ompl planner, also added pilz_cartesian_limits yaml. --------- Co-authored-by: Jacob Odle <[email protected]>
PickNikRobotics · Apr 19, 2024 · 0b4b29e · 0b4b29e
1 parent 51c2065
commit 0b4b29e
Show file tree

Hide file tree

Showing 4 changed files with 76 additions and 87 deletions.
diff --git 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,15 +142,14 @@ 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]
     return nodes_to_start
 
 
 def generate_launch_description():
-
     declared_arguments = []
 
     # TODO(andyz): add other options

diff --git a/abb_bringup/package.xml b/abb_bringup/package.xml
@@ -33,6 +33,7 @@
   <exec_depend>rviz2</exec_depend>
   <exec_depend>urdf</exec_depend>
   <exec_depend>xacro</exec_depend>
+  <exec_depend>moveit_configs_utils</exec_depend>
 
   <export>
     <build_type>ament_cmake</build_type>

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
@@ -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
@@ -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