From 4206959d4fdbf1079ab2d531d1a5e576c62a72f6 Mon Sep 17 00:00:00 2001
From: Rick Staa <rick.staa@outlook.com>
Date: Wed, 13 Oct 2021 20:46:11 +0200
Subject: [PATCH] Add CHOMP config files

* Add demo_chomp.launch
* Restore original 'chomp_planning.yaml' config
* Add CHOMP post-processing example, using OMPL+CHOMP
---
 config/chomp_planning.yaml                    |  6 +++---
 launch/demo_chomp.launch                      |  5 +++++
 .../ompl-chomp_planning_pipeline.launch.xml   | 19 +++++++++++++++++++
 3 files changed, 27 insertions(+), 3 deletions(-)
 create mode 100644 launch/demo_chomp.launch
 create mode 100644 launch/ompl-chomp_planning_pipeline.launch.xml

diff --git a/config/chomp_planning.yaml b/config/chomp_planning.yaml
index c14a673..eb9c912 100644
--- a/config/chomp_planning.yaml
+++ b/config/chomp_planning.yaml
@@ -7,12 +7,12 @@ learning_rate: 0.01
 smoothness_cost_velocity: 0.0
 smoothness_cost_acceleration: 1.0
 smoothness_cost_jerk: 0.0
-ridge_factor: 0.01
+ridge_factor: 0.0
 use_pseudo_inverse: false
 pseudo_inverse_ridge_factor: 1e-4
 joint_update_limit: 0.1
 collision_clearance: 0.2
 collision_threshold: 0.07
 use_stochastic_descent: true
-enable_failure_recovery: true
-max_recovery_attempts: 5
\ No newline at end of file
+enable_failure_recovery: false
+max_recovery_attempts: 5
diff --git a/launch/demo_chomp.launch b/launch/demo_chomp.launch
new file mode 100644
index 0000000..a3051a8
--- /dev/null
+++ b/launch/demo_chomp.launch
@@ -0,0 +1,5 @@
+<launch>
+  <include file="$(dirname)/demo.launch">
+    <arg name="pipeline" value="chomp"/>
+  </include>
+</launch>
diff --git a/launch/ompl-chomp_planning_pipeline.launch.xml b/launch/ompl-chomp_planning_pipeline.launch.xml
new file mode 100644
index 0000000..178d2be
--- /dev/null
+++ b/launch/ompl-chomp_planning_pipeline.launch.xml
@@ -0,0 +1,19 @@
+<launch>
+  <!-- load OMPL planning pipeline, but add the CHOMP planning adapter. -->
+  <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/FixWorkspaceBounds
+       default_planner_request_adapters/FixStartStateBounds
+       default_planner_request_adapters/FixStartStateCollision
+       default_planner_request_adapters/FixStartStatePathConstraints
+       chomp/OptimizerAdapter"
+       />
+  </include>
+
+  <!-- load chomp config -->
+  <rosparam command="load" file="$(find panda_moveit_config)/config/chomp_planning.yaml"/>
+
+  <!-- override trajectory_initialization_method -->
+  <param name="trajectory_initialization_method" value="fillTrajectory"/>
+</launch>
-- 
GitLab