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

@@ -4,15 +4,25 @@ max_iterations_after_collision_free: 5
 smoothness_cost_weight: 0.1
 obstacle_cost_weight: 1.0
 learning_rate: 0.01
+animate_path: true
+add_randomness: false
 smoothness_cost_velocity: 0.0
 smoothness_cost_acceleration: 1.0
 smoothness_cost_jerk: 0.0
-ridge_factor: 0.01
+hmc_discretization: 0.01
+hmc_stochasticity: 0.01
+hmc_annealing_factor: 0.99
+use_hamiltonian_monte_carlo: false
+ridge_factor: 0.0
 use_pseudo_inverse: false
 pseudo_inverse_ridge_factor: 1e-4
+animate_endeffector: false
+animate_endeffector_segment: "panda_rightfinger"
 joint_update_limit: 0.1
 collision_clearance: 0.2
 collision_threshold: 0.07
+random_jump_amount: 1.0
 use_stochastic_descent: true
-enable_failure_recovery: true
-max_recovery_attempts: 5
+enable_failure_recovery: false
+max_recovery_attempts: 5
+trajectory_initialization_method: "quintic-spline"

launch/demo_chomp.launch

@@ -0,0 +1,5 @@
+<launch>
+  <include file="$(dirname)/demo.launch">
+    <arg name="pipeline" value="chomp"/>
+  </include>
+</launch>

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>