Adding RPBT config (moveit#43)

* adding RPBT config * removing roslaunch check becasue of removed dependencies * adding pilz pipeline config for panda * add remaining prbt-related packages * prbt_ikfast_manipulaor_plugin * prbt_support * prbt_pg70_support * drop no longer needed dependency * add remaining prbt-related packages * prbt_ikfast_manipulaor_plugin * prbt_support * prbt_pg70_support * add argument to enable last point execution to speed up test time by skipping trajectory in fake execution * last point execution for pg70 gripper * drop system_info since we only need urdf model for testing in simulation to get rid of dependency to pilz_testutils * actually use pg70 package inside from moveit_resources instead of schunk_description * drop depend on prbt_hardware_support not needed in simulation * add execution type for panda config to allow skipping execution and jump to last point in fake execution * fixup 4bc2ce drop system_info * fix roslaunch and CMakeLists checks * also adding capabilities here * drop more dependencies * using pg70 support from moveit_resources * add execution_type to gripper * drop Werror as suggested in the review by v4hn * update deprecated macro to INSTANTIATE_TEST_SUITE_P * Revert "update deprecated macro to INSTANTIATE_TEST_SUITE_P" This reverts commit 1a467cc. * update version numbers to be consitant across the meta-package * version increase fo pilz packages * removing patch files * tiny fix * update panda config from templates re-generated from MSA with adapted templates see moveit 99c059f * update fanuc package from MSA with updated templates, see moveit commit a4527b * drop remapping to joint_states_desired for easier include of move_group.launch into tests. This brings the panda_moveit_config closer to the templates in MSA. Co-authored-by: Joachim Schleicher <J.Schleicher@pilz.de>
sjahr · Sep 27, 2021 · 146ed9c · 146ed9c
1 parent ca3f793
commit 146ed9c
Show file tree

Hide file tree

Showing 115 changed files with 29,302 additions and 57 deletions.
diff --git a/fanuc_moveit_config/config/cartesian_limits.yaml b/fanuc_moveit_config/config/cartesian_limits.yaml
@@ -0,0 +1,5 @@
+cartesian_limits:
+  max_trans_vel: 1
+  max_trans_acc: 2.25
+  max_trans_dec: -5
+  max_rot_vel: 1.57
diff --git a/fanuc_moveit_config/config/fake_controllers.yaml b/fanuc_moveit_config/config/fake_controllers.yaml
@@ -1,5 +1,6 @@
 controller_list:
   - name: fake_manipulator_controller
+    type: $(arg execution_type)
     joints:
       - joint_1
       - joint_2

diff --git a/fanuc_moveit_config/launch/chomp_planning_pipeline.launch.xml b/fanuc_moveit_config/launch/chomp_planning_pipeline.launch.xml
@@ -1,10 +1,30 @@
 <launch>
-   <!-- CHOMP Plugin for MoveIt -->
-   <arg name="planning_plugin" value="chomp_interface/CHOMPPlanner" />
-   <arg name="start_state_max_bounds_error" value="0.1" />
+  <!-- CHOMP Plugin for MoveIt -->
+  <arg name="planning_plugin" value="chomp_interface/CHOMPPlanner" />
 
-   <param name="planning_plugin" value="$(arg planning_plugin)" />
-   <param name="start_state_max_bounds_error" value="$(arg start_state_max_bounds_error)" />
+  <!-- define capabilites that are loaded on start (space seperated) -->
+  <arg name="capabilities" default=""/>
 
-   <rosparam command="load" file="$(find moveit_resources_fanuc_moveit_config)/config/chomp_planning.yaml" />
+  <!-- inhibit capabilites (space seperated) -->
+  <arg name="disable_capabilities" default=""/>
+
+  <arg name="start_state_max_bounds_error" value="0.1" />
+  <!-- The request adapters (plugins) used when planning.
+       ORDER MATTERS -->
+  <arg name="planning_adapters"
+       value="default_planner_request_adapters/AddTimeParameterization
+              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"
+              />
+
+  <param name="planning_plugin" value="$(arg planning_plugin)" />
+  <param name="request_adapters" value="$(arg planning_adapters)" />
+  <param name="start_state_max_bounds_error" value="$(arg start_state_max_bounds_error)" />
+  <param name="capabilities" value="$(arg capabilities)" />
+  <param name="disable_capabilities" value="$(arg disable_capabilities)" />
+
+  <rosparam command="load" file="$(find moveit_resources_fanuc_moveit_config)/config/chomp_planning.yaml" />
 </launch>
diff --git a/fanuc_moveit_config/launch/fake_moveit_controller_manager.launch.xml b/fanuc_moveit_config/launch/fake_moveit_controller_manager.launch.xml
@@ -1,9 +1,12 @@
 <launch>
 
+  <!-- execute the trajectory in 'interpolate' mode or jump to goal position in 'last point' mode -->
+  <arg name="execution_type" default="interpolate" />
+
   <!-- Set the param that trajectory_execution_manager needs to find the controller plugin -->
   <param name="moveit_controller_manager" value="moveit_fake_controller_manager/MoveItFakeControllerManager"/>
 
   <!-- The rest of the params are specific to this plugin -->
-  <rosparam file="$(find moveit_resources_fanuc_moveit_config)/config/fake_controllers.yaml"/>
+  <rosparam subst_value="true" file="$(find moveit_resources_fanuc_moveit_config)/config/fake_controllers.yaml"/>
 
 </launch>
diff --git a/fanuc_moveit_config/launch/move_group.launch b/fanuc_moveit_config/launch/move_group.launch
@@ -15,6 +15,7 @@
   <arg name="pipeline" default="ompl" />
   <arg name="allow_trajectory_execution" default="true"/>
   <arg name="fake_execution" default="false"/>
+  <arg name="execution_type" default="interpolate"/> <!-- set to 'last point' to skip intermediate trajectory in fake execution -->
   <arg name="max_safe_path_cost" default="1"/>
   <arg name="jiggle_fraction" default="0.05" />
   <arg name="publish_monitored_planning_scene" default="true"/>
@@ -46,13 +47,16 @@
   <!-- Planning Functionality -->
   <include ns="move_group" file="$(find moveit_resources_fanuc_moveit_config)/launch/planning_pipeline.launch.xml">
     <arg name="pipeline" value="$(arg pipeline)" />
+    <param name="capabilities" value="$(arg capabilities)"/>
+    <param name="disable_capabilities" value="$(arg disable_capabilities)"/>
   </include>
 
   <!-- Trajectory Execution Functionality -->
   <include ns="move_group" file="$(find moveit_resources_fanuc_moveit_config)/launch/trajectory_execution.launch.xml" if="$(arg allow_trajectory_execution)">
     <arg name="moveit_manage_controllers" value="true" />
     <arg name="moveit_controller_manager" value="fanuc" unless="$(arg fake_execution)"/>
     <arg name="moveit_controller_manager" value="fake" if="$(arg fake_execution)"/>
+    <arg name="execution_type" value="$(arg execution_type)" />
   </include>
 
   <!-- Sensors Functionality -->
@@ -68,8 +72,6 @@
     <param name="allow_trajectory_execution" value="$(arg allow_trajectory_execution)"/>
     <param name="max_safe_path_cost" value="$(arg max_safe_path_cost)"/>
     <param name="jiggle_fraction" value="$(arg jiggle_fraction)" />
-    <param name="capabilities" value="$(arg capabilities)"/>
-    <param name="disable_capabilities" value="$(arg disable_capabilities)"/>
 
 
     <!-- Publish the planning scene of the physical robot so that rviz plugin can know actual robot -->

diff --git a/fanuc_moveit_config/launch/ompl_planning_pipeline.launch.xml b/fanuc_moveit_config/launch/ompl_planning_pipeline.launch.xml
@@ -3,6 +3,12 @@
   <!-- OMPL Plugin for MoveIt -->
   <arg name="planning_plugin" value="ompl_interface/OMPLPlanner" />
 
+  <!-- define capabilites that are loaded on start (space seperated) -->
+  <arg name="capabilities" default=""/>
+
+  <!-- inhibit capabilites (space seperated) -->
+  <arg name="disable_capabilities" default=""/>
+
   <!-- The request adapters (plugins) used when planning with OMPL.
        ORDER MATTERS -->
   <arg name="planning_adapters"
@@ -19,6 +25,8 @@
   <param name="planning_plugin" value="$(arg planning_plugin)" />
   <param name="request_adapters" value="$(arg planning_adapters)" />
   <param name="start_state_max_bounds_error" value="$(arg start_state_max_bounds_error)" />
+  <param name="capabilities" value="$(arg capabilities)" />
+  <param name="disable_capabilities" value="$(arg disable_capabilities)" />
 
   <rosparam command="load" file="$(find moveit_resources_fanuc_moveit_config)/config/ompl_planning.yaml"/>
 

diff --git a/fanuc_moveit_config/launch/pilz_industrial_motion_planner_planning_pipeline.launch.xml b/fanuc_moveit_config/launch/pilz_industrial_motion_planner_planning_pipeline.launch.xml
@@ -0,0 +1,28 @@
+<launch>
+
+  <!-- Pilz Command Planner Plugin for MoveIt -->
+  <arg name="planning_plugin" value="pilz_industrial_motion_planner::CommandPlanner" />
+
+  <!-- The request adapters (plugins) used when planning.
+       ORDER MATTERS -->
+  <arg name="planning_adapters" value="" />
+
+  <!-- define capabilites that are loaded on start (space seperated) -->
+  <arg name="capabilities" default=""/>
+
+  <!-- inhibit capabilites (space seperated) -->
+  <arg name="disable_capabilities" default=""/>
+
+  <arg name="start_state_max_bounds_error" value="0.1" />
+
+  <param name="planning_plugin" value="$(arg planning_plugin)" />
+  <param name="request_adapters" value="$(arg planning_adapters)" />
+  <param name="start_state_max_bounds_error" value="$(arg start_state_max_bounds_error)" />
+
+  <!-- MoveGroup capabilities to load, append sequence capability -->
+  <param name="capabilities" value="$(arg capabilities)
+                                    pilz_industrial_motion_planner/MoveGroupSequenceAction
+                                    pilz_industrial_motion_planner/MoveGroupSequenceService
+                                   " />
+  <param name="disable_capabilities" value="$(arg disable_capabilities)" />
+</launch>
diff --git a/fanuc_moveit_config/launch/planning_context.launch b/fanuc_moveit_config/launch/planning_context.launch
@@ -14,6 +14,7 @@
   <!-- Load updated joint limits (override information from URDF) -->
   <group ns="$(arg robot_description)_planning">
     <rosparam command="load" file="$(find moveit_resources_fanuc_moveit_config)/config/joint_limits.yaml"/>
+    <rosparam command="load" file="$(find moveit_resources_fanuc_moveit_config)/config/cartesian_limits.yaml"/>
   </group>
 
   <!-- Load default settings for kinematics; these settings are overridden by settings in a node's namespace -->

diff --git a/fanuc_moveit_config/launch/planning_pipeline.launch.xml b/fanuc_moveit_config/launch/planning_pipeline.launch.xml
@@ -5,6 +5,15 @@
 
   <arg name="pipeline" default="ompl" />
 
-  <include file="$(find moveit_resources_fanuc_moveit_config)/launch/$(arg pipeline)_planning_pipeline.launch.xml" />
+  <!-- define capabilites that are loaded on start (space seperated) -->
+  <arg name="capabilities" default=""/>
+
+  <!-- inhibit capabilites (space seperated) -->
+  <arg name="disable_capabilities" default=""/>
+
+  <include file="$(find moveit_resources_fanuc_moveit_config)/launch/$(arg pipeline)_planning_pipeline.launch.xml">
+    <arg name="capabilities" value="$(arg capabilities)"/>
+    <arg name="disable_capabilities" value="$(arg disable_capabilities)"/>
+  </include>
 
 </launch>
diff --git a/fanuc_moveit_config/launch/trajectory_execution.launch.xml b/fanuc_moveit_config/launch/trajectory_execution.launch.xml
@@ -2,6 +2,8 @@
 
   <!-- This file makes it easy to include the settings for trajectory execution  -->
 
+  <arg name="execution_type" default="interpolate" />
+
   <!-- Flag indicating whether MoveIt is allowed to load/unload  or switch controllers -->
   <arg name="moveit_manage_controllers" default="true"/>
   <param name="moveit_manage_controllers" value="$(arg moveit_manage_controllers)"/>
@@ -15,6 +17,8 @@
 
   <!-- Load the robot specific controller manager; this sets the moveit_controller_manager ROS parameter -->
   <arg name="moveit_controller_manager" default="fanuc" />
-  <include file="$(find moveit_resources_fanuc_moveit_config)/launch/$(arg moveit_controller_manager)_moveit_controller_manager.launch.xml" />
+  <include file="$(find moveit_resources_fanuc_moveit_config)/launch/$(arg moveit_controller_manager)_moveit_controller_manager.launch.xml">
+    <arg name="execution_type" value="$(arg execution_type)" />
+  </include>
 
 </launch>
diff --git a/panda_moveit_config/config/cartesian_limits.yaml b/panda_moveit_config/config/cartesian_limits.yaml
@@ -0,0 +1,5 @@
+cartesian_limits:
+  max_trans_vel: 1
+  max_trans_acc: 2.25
+  max_trans_dec: -5
+  max_rot_vel: 1.57
diff --git a/panda_moveit_config/launch/chomp_planning_pipeline.launch.xml b/panda_moveit_config/launch/chomp_planning_pipeline.launch.xml
@@ -1,17 +1,30 @@
 <launch>
-   <!-- CHOMP Plugin for MoveIt! -->
-   <arg name="planning_plugin" value="chomp_interface/CHOMPPlanner" />
-   <arg name="start_state_max_bounds_error" value="0.1" />
+  <!-- CHOMP Plugin for MoveIt -->
+  <arg name="planning_plugin" value="chomp_interface/CHOMPPlanner" />
 
-   <param name="planning_plugin" value="$(arg planning_plugin)" />
-   <param name="request_adapters" value="
-       default_planner_request_adapters/AddTimeParameterization
-       default_planner_request_adapters/FixWorkspaceBounds
-       default_planner_request_adapters/FixStartStateBounds
-       default_planner_request_adapters/FixStartStateCollision
-       default_planner_request_adapters/FixStartStatePathConstraints"
-       />
-   <param name="start_state_max_bounds_error" value="$(arg start_state_max_bounds_error)" />
+  <!-- define capabilites that are loaded on start (space seperated) -->
+  <arg name="capabilities" default=""/>
 
-   <rosparam command="load" file="$(find moveit_resources_panda_moveit_config)/config/chomp_planning.yaml" />
+  <!-- inhibit capabilites (space seperated) -->
+  <arg name="disable_capabilities" default=""/>
+
+  <arg name="start_state_max_bounds_error" value="0.1" />
+  <!-- The request adapters (plugins) used when planning.
+       ORDER MATTERS -->
+  <arg name="planning_adapters"
+       value="default_planner_request_adapters/AddTimeParameterization
+              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"
+              />
+
+  <param name="planning_plugin" value="$(arg planning_plugin)" />
+  <param name="request_adapters" value="$(arg planning_adapters)" />
+  <param name="start_state_max_bounds_error" value="$(arg start_state_max_bounds_error)" />
+  <param name="capabilities" value="$(arg capabilities)" />
+  <param name="disable_capabilities" value="$(arg disable_capabilities)" />
+
+  <rosparam command="load" file="$(find moveit_resources_panda_moveit_config)/config/chomp_planning.yaml" />
 </launch>
diff --git a/panda_moveit_config/launch/fake_moveit_controller_manager.launch.xml b/panda_moveit_config/launch/fake_moveit_controller_manager.launch.xml
@@ -1,9 +1,12 @@
 <launch>
 
+  <!-- execute the trajectory in 'interpolate' mode or jump to goal position in 'last point' mode -->
+  <arg name="execution_type" default="interpolate" />
+
   <!-- Set the param that trajectory_execution_manager needs to find the controller plugin -->
   <param name="moveit_controller_manager" value="moveit_fake_controller_manager/MoveItFakeControllerManager"/>
 
   <!-- The rest of the params are specific to this plugin -->
-  <rosparam file="$(find moveit_resources_panda_moveit_config)/config/fake_controllers.yaml"/>
+  <rosparam subst_value="true" file="$(find moveit_resources_panda_moveit_config)/config/fake_controllers.yaml"/>
 
 </launch>
diff --git a/panda_moveit_config/launch/move_group.launch b/panda_moveit_config/launch/move_group.launch
@@ -6,8 +6,6 @@
     <arg name="load_gripper" value="$(arg load_gripper)" />
   </include>
 
-  <arg name="pipeline" default="ompl" />
-
   <!-- GDB Debug Option -->
   <arg name="debug" default="false" />
   <arg unless="$(arg debug)" name="launch_prefix" value="" />
@@ -20,15 +18,43 @@
   <arg     if="$(arg info)" name="command_args" value="--debug" />
 
   <!-- move_group settings -->
+  <arg name="pipeline" default="ompl" />
   <arg name="allow_trajectory_execution" default="true"/>
   <arg name="fake_execution" default="false"/>
+  <arg name="execution_type" default="interpolate"/> <!-- set to 'last point' to skip intermediate trajectory in fake execution -->
   <arg name="max_safe_path_cost" default="1"/>
   <arg name="jiggle_fraction" default="0.05" />
   <arg name="publish_monitored_planning_scene" default="true"/>
 
+  <arg name="capabilities" default=""/>
+  <arg name="disable_capabilities" default=""/>
+  <!-- load these non-default MoveGroup capabilities (space seperated) -->
+  <!--
+  <arg name="capabilities" value="
+                a_package/AwsomeMotionPlanningCapability
+                another_package/GraspPlanningPipeline
+                " />
+  -->
+
+  <!-- inhibit these default MoveGroup capabilities (space seperated) -->
+  <!--
+  <arg name="disable_capabilities" value="
+                move_group/MoveGroupKinematicsService
+                move_group/ClearOctomapService
+                " />
+  -->
+
+  <arg name="load_robot_description" default="true" />
+  <!-- load URDF, SRDF and joint_limits configuration -->
+  <include file="$(find moveit_resources_panda_moveit_config)/launch/planning_context.launch">
+    <arg name="load_robot_description" value="$(arg load_robot_description)" />
+  </include>
+
   <!-- Planning Functionality -->
   <include ns="move_group" file="$(find moveit_resources_panda_moveit_config)/launch/planning_pipeline.launch.xml">
     <arg name="pipeline" value="$(arg pipeline)" />
+    <param name="capabilities" value="$(arg capabilities)"/>
+    <param name="disable_capabilities" value="$(arg disable_capabilities)"/>
   </include>
 
   <!-- Trajectory Execution Functionality -->
@@ -37,6 +63,7 @@
     <arg name="moveit_controller_manager" value="panda" if="$(eval not arg('fake_execution') and not arg('load_gripper'))"/>
     <arg name="moveit_controller_manager" value="panda_gripper" if="$(eval not arg('fake_execution') and arg('load_gripper'))"/>
     <arg name="moveit_controller_manager" value="fake" if="$(arg fake_execution)"/>
+    <arg name="execution_type" value="$(arg execution_type)" />
   </include>
 
   <!-- Sensors Functionality -->
@@ -53,29 +80,13 @@
     <param name="max_safe_path_cost" value="$(arg max_safe_path_cost)"/>
     <param name="jiggle_fraction" value="$(arg jiggle_fraction)" />
 
-    <!-- load these non-default MoveGroup capabilities -->
-    <!--
-    <param name="capabilities" value="
-                  a_package/AwsomeMotionPlanningCapability
-                  another_package/GraspPlanningPipeline
-                  " />
-    -->
-
-    <!-- inhibit these default MoveGroup capabilities -->
-    <!--
-    <param name="disable_capabilities" value="
-                  move_group/MoveGroupKinematicsService
-                  move_group/ClearOctomapService
-                  " />
-    -->
 
     <!-- Publish the planning scene of the physical robot so that rviz plugin can know actual robot -->
     <param name="planning_scene_monitor/publish_planning_scene" value="$(arg publish_monitored_planning_scene)" />
     <param name="planning_scene_monitor/publish_geometry_updates" value="$(arg publish_monitored_planning_scene)" />
     <param name="planning_scene_monitor/publish_state_updates" value="$(arg publish_monitored_planning_scene)" />
     <param name="planning_scene_monitor/publish_transforms_updates" value="$(arg publish_monitored_planning_scene)" />
 
-    <remap from="/joint_states" to="/joint_states_desired" />
   </node>
 
 </launch>
diff --git a/panda_moveit_config/launch/ompl_planning_pipeline.launch.xml b/panda_moveit_config/launch/ompl_planning_pipeline.launch.xml
@@ -1,23 +1,32 @@
 <launch>
 
-  <!-- OMPL Plugin for MoveIt! -->
+  <!-- OMPL Plugin for MoveIt -->
   <arg name="planning_plugin" value="ompl_interface/OMPLPlanner" />
 
-  <!-- The request adapters (plugins) used when planning with OMPL. 
+  <!-- define capabilites that are loaded on start (space seperated) -->
+  <arg name="capabilities" default=""/>
+
+  <!-- inhibit capabilites (space seperated) -->
+  <arg name="disable_capabilities" default=""/>
+
+  <!-- The request adapters (plugins) used when planning with OMPL.
        ORDER MATTERS -->
-  <arg name="planning_adapters" default="
-       default_planner_request_adapters/AddTimeParameterization
-       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"
-       />
+  <arg name="planning_adapters"
+       value="default_planner_request_adapters/AddTimeParameterization
+              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"
+              />
+
   <arg name="start_state_max_bounds_error" value="0.1" />
 
   <param name="planning_plugin" value="$(arg planning_plugin)" />
   <param name="request_adapters" value="$(arg planning_adapters)" />
   <param name="start_state_max_bounds_error" value="$(arg start_state_max_bounds_error)" />
+  <param name="capabilities" value="$(arg capabilities)" />
+  <param name="disable_capabilities" value="$(arg disable_capabilities)" />
 
   <rosparam command="load" file="$(find moveit_resources_panda_moveit_config)/config/ompl_planning.yaml"/>