From 8fdfdc023355ebb436992f8a0335924622a733f6 Mon Sep 17 00:00:00 2001
From: Robert Haschke <rhaschke@techfak.uni-bielefeld.de>
Date: Fri, 10 Nov 2023 01:10:52 +0100
Subject: [PATCH] Add ResolveConstraintFrames to all planning pipelines

---
 .../launch/ompl-chomp_planning_pipeline.launch.xml    |  1 +
 ...strial_motion_planner_planning_pipeline.launch.xml |  2 +-
 .../launch/stomp_planning_pipeline.launch.xml         |  1 +
 ...strial_motion_planner_planning_pipeline.launch.xml |  2 +-
 .../launch/lerp_planning_pipeline.launch.xml          |  1 +
 .../launch/ompl-chomp_planning_pipeline.launch.xml    |  1 +
 ...strial_motion_planner_planning_pipeline.launch.xml |  2 +-
 .../launch/stomp_planning_pipeline.launch.xml         |  1 +
 .../launch/trajopt_planning_pipeline.launch.xml       |  1 +
 .../launch/ompl_planning_pipeline.launch.xml          | 11 ++++++-----
 ...strial_motion_planner_planning_pipeline.launch.xml |  2 +-
 11 files changed, 16 insertions(+), 9 deletions(-)

diff --git a/dual_panda_moveit_config/launch/ompl-chomp_planning_pipeline.launch.xml b/dual_panda_moveit_config/launch/ompl-chomp_planning_pipeline.launch.xml
index b0789924..bed09bd0 100644
--- a/dual_panda_moveit_config/launch/ompl-chomp_planning_pipeline.launch.xml
+++ b/dual_panda_moveit_config/launch/ompl-chomp_planning_pipeline.launch.xml
@@ -3,6 +3,7 @@
   <include file="$(find panda_moveit_config)/launch/ompl_planning_pipeline.launch.xml">
     <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
diff --git a/dual_panda_moveit_config/launch/pilz_industrial_motion_planner_planning_pipeline.launch.xml b/dual_panda_moveit_config/launch/pilz_industrial_motion_planner_planning_pipeline.launch.xml
index c7c4cf50..b2fb1083 100644
--- a/dual_panda_moveit_config/launch/pilz_industrial_motion_planner_planning_pipeline.launch.xml
+++ b/dual_panda_moveit_config/launch/pilz_industrial_motion_planner_planning_pipeline.launch.xml
@@ -1,7 +1,7 @@
 <launch>
 
   <!-- The request adapters (plugins) used when planning. ORDER MATTERS! -->
-  <arg name="planning_adapters" default="" />
+  <arg name="planning_adapters" default="default_planner_request_adapters/ResolveConstraintFrames"/>
 
   <param name="planning_plugin" value="pilz_industrial_motion_planner::CommandPlanner" />
   <param name="request_adapters" value="$(arg planning_adapters)" />
diff --git a/dual_panda_moveit_config/launch/stomp_planning_pipeline.launch.xml b/dual_panda_moveit_config/launch/stomp_planning_pipeline.launch.xml
index 30cad6c4..f70a7ca8 100644
--- a/dual_panda_moveit_config/launch/stomp_planning_pipeline.launch.xml
+++ b/dual_panda_moveit_config/launch/stomp_planning_pipeline.launch.xml
@@ -7,6 +7,7 @@
   <!-- The request adapters (plugins) used when planning. 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
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
index 10f0f1f5..f0ca9764 100644
--- 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
@@ -5,7 +5,7 @@
 
   <!-- The request adapters (plugins) used when planning.
        ORDER MATTERS -->
-  <arg name="planning_adapters" value="" />
+  <arg name="planning_adapters" value="default_planner_request_adapters/ResolveConstraintFrames" />
 
   <arg name="start_state_max_bounds_error" value="0.1" />
 
diff --git a/panda_moveit_config/launch/lerp_planning_pipeline.launch.xml b/panda_moveit_config/launch/lerp_planning_pipeline.launch.xml
index fc46c962..64efaa77 100644
--- a/panda_moveit_config/launch/lerp_planning_pipeline.launch.xml
+++ b/panda_moveit_config/launch/lerp_planning_pipeline.launch.xml
@@ -6,6 +6,7 @@
   <!-- The request adapters (plugins) used when planning with LERP. 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
diff --git a/panda_moveit_config/launch/ompl-chomp_planning_pipeline.launch.xml b/panda_moveit_config/launch/ompl-chomp_planning_pipeline.launch.xml
index 23f89582..d7a349ef 100644
--- a/panda_moveit_config/launch/ompl-chomp_planning_pipeline.launch.xml
+++ b/panda_moveit_config/launch/ompl-chomp_planning_pipeline.launch.xml
@@ -3,6 +3,7 @@
   <include file="$(dirname)/ompl_planning_pipeline.launch.xml">
     <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
diff --git a/panda_moveit_config/launch/pilz_industrial_motion_planner_planning_pipeline.launch.xml b/panda_moveit_config/launch/pilz_industrial_motion_planner_planning_pipeline.launch.xml
index c7c4cf50..6934027f 100644
--- a/panda_moveit_config/launch/pilz_industrial_motion_planner_planning_pipeline.launch.xml
+++ b/panda_moveit_config/launch/pilz_industrial_motion_planner_planning_pipeline.launch.xml
@@ -1,7 +1,7 @@
 <launch>
 
   <!-- The request adapters (plugins) used when planning. ORDER MATTERS! -->
-  <arg name="planning_adapters" default="" />
+  <arg name="planning_adapters" default="default_planner_request_adapters/ResolveConstraintFrames" />
 
   <param name="planning_plugin" value="pilz_industrial_motion_planner::CommandPlanner" />
   <param name="request_adapters" value="$(arg planning_adapters)" />
diff --git a/panda_moveit_config/launch/stomp_planning_pipeline.launch.xml b/panda_moveit_config/launch/stomp_planning_pipeline.launch.xml
index db7647c2..d36db43e 100644
--- a/panda_moveit_config/launch/stomp_planning_pipeline.launch.xml
+++ b/panda_moveit_config/launch/stomp_planning_pipeline.launch.xml
@@ -6,6 +6,7 @@
   <!-- The request adapters (plugins) 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
diff --git a/panda_moveit_config/launch/trajopt_planning_pipeline.launch.xml b/panda_moveit_config/launch/trajopt_planning_pipeline.launch.xml
index 8a328b6c..4aed7a9e 100644
--- a/panda_moveit_config/launch/trajopt_planning_pipeline.launch.xml
+++ b/panda_moveit_config/launch/trajopt_planning_pipeline.launch.xml
@@ -6,6 +6,7 @@
   <!-- The request adapters (plugins) used when planning with TrajOpt. 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
diff --git a/prbt_moveit_config/launch/ompl_planning_pipeline.launch.xml b/prbt_moveit_config/launch/ompl_planning_pipeline.launch.xml
index 2ab26f57..3f01f413 100644
--- a/prbt_moveit_config/launch/ompl_planning_pipeline.launch.xml
+++ b/prbt_moveit_config/launch/ompl_planning_pipeline.launch.xml
@@ -3,13 +3,14 @@
   <!-- OMPL Plugin for MoveIt! -->
   <arg name="planning_plugin" value="ompl_interface/OMPLPlanner" />
 
-  <!-- The request adapters (plugins) used when planning with OMPL. 
+  <!-- The request adapters (plugins) used when planning with OMPL.
        ORDER MATTERS -->
   <arg name="planning_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" />
+            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" />
 
diff --git a/prbt_moveit_config/launch/pilz_industrial_motion_planner_planning_pipeline.launch.xml b/prbt_moveit_config/launch/pilz_industrial_motion_planner_planning_pipeline.launch.xml
index 439e3ef0..ecb1861e 100644
--- a/prbt_moveit_config/launch/pilz_industrial_motion_planner_planning_pipeline.launch.xml
+++ b/prbt_moveit_config/launch/pilz_industrial_motion_planner_planning_pipeline.launch.xml
@@ -3,7 +3,7 @@
   <!-- Pilz Command Planner Plugin for MoveIt -->
   <arg name="planning_plugin" value="pilz_industrial_motion_planner::CommandPlanner" />
 
-  <arg name="planning_adapters" value="" />
+  <arg name="planning_adapters" value="default_planner_request_adapters/ResolveConstraintFrames"/>
 
   <arg name="start_state_max_bounds_error" value="0.1" />