adding support for CHOMP

config/chomp_planning.yaml

+planning_time_limit: 10.0
+max_iterations: 200
+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
+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_clearence: 0.2
+collision_threshold: 0.07
+random_jump_amount: 1.0
+use_stochastic_descent: true

launch/chomp_planning_pipeline.launch.xml

+<launch>
+
+   <!-- CHOMP Plugin for MoveIt! -->
+   <arg name="planning_plugin" value="chomp_interface/CHOMPPlanner" />
+
+   <arg name="start_state_max_bounds_error" value="0.1" />
+
+   <param name="planning_plugin" value="$(arg planning_plugin)" />
+   <param name="start_state_max_bounds_error" value="$(arg start_state_max_bounds_error)" />
+
+   <param name="collision_detector" value="Hybrid" />
+
+   <rosparam command="load" file="$(find panda_moveit_config)/config/chomp_planning.yaml" />
+
+</launch>

launch/demo_chomp.launch

+<launch>
+
+  <!-- By default, we do not start a database (it can be large) -->
+  <arg name="db" default="false" />
+  <!-- Allow user to specify database location -->
+  <arg name="db_path" default="$(find panda_moveit_config)/default_warehouse_mongo_db" />
+
+  <!-- By default, we are not in debug mode -->
+  <arg name="debug" default="false" />
+
+  <!--
+  By default, hide joint_state_publisher's GUI
+
+  MoveIt!'s "demo" mode replaces the real robot driver with the joint_state_publisher.
+  The latter one maintains and publishes the current joint configuration of the simulated robot.
+  It also provides a GUI to move the simulated robot around "manually".
+  This corresponds to moving around the real robot without the use of MoveIt.
+  -->
+  <arg name="use_gui" default="false" />
+
+  <!-- Load the URDF, SRDF and other .yaml configuration files on the param server -->
+  <include file="$(find panda_moveit_config)/launch/planning_context.launch">
+    <arg name="load_robot_description" value="true"/>
+  </include>
+
+  <!-- If needed, broadcast static tf for robot root -->
+  
+
+  <!-- We do not have a robot connected, so publish fake joint states -->
+  <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher">
+    <param name="/use_gui" value="$(arg use_gui)"/>
+    <rosparam param="/source_list">[/move_group/fake_controller_joint_states]</rosparam>
+  </node>
+
+  <!-- Given the published joint states, publish tf for the robot links -->
+  <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" respawn="true" output="screen" />
+
+  <!-- Run the main MoveIt executable without trajectory execution (we do not have controllers configured by default) -->
+  <include file="$(find panda_moveit_config)/launch/move_group.launch">
+    <arg name="allow_trajectory_execution" value="true"/>
+    <arg name="fake_execution" value="true"/>
+    <arg name="info" value="true"/>
+    <arg name="debug" value="$(arg debug)"/>
+    <arg name="planner" value="chomp" />
+  </include>
+
+  <!-- Run Rviz and load the default config to see the state of the move_group node -->
+  <include file="$(find panda_moveit_config)/launch/moveit_rviz.launch">
+    <arg name="config" value="true"/>
+    <arg name="debug" value="$(arg debug)"/>
+  </include>
+
+  <!-- If database loading was enabled, start mongodb as well -->
+  <include file="$(find panda_moveit_config)/launch/default_warehouse_db.launch" if="$(arg db)">
+    <arg name="moveit_warehouse_database_path" value="$(arg db_path)"/>
+  </include>
+
+</launch>

launch/move_group.launch

@@ -2,6 +2,8 @@
 
   <include file="$(find panda_moveit_config)/launch/planning_context.launch" />
 
+  <arg name="planner" default="ompl" />
+
   <!-- GDB Debug Option -->
   <arg name="debug" default="false" />
   <arg unless="$(arg debug)" name="launch_prefix" value="" />
@@ -22,7 +24,7 @@
 
   <!-- Planning Functionality -->
   <include ns="move_group" file="$(find panda_moveit_config)/launch/planning_pipeline.launch.xml">
-    <arg name="pipeline" value="ompl" />
+    <arg name="pipeline" value="$(arg planner)" />
   </include>
 
   <!-- Trajectory Execution Functionality -->

package.xml

@@ -20,6 +20,7 @@
   <run_depend>moveit_fake_controller_manager</run_depend>
   <run_depend>moveit_kinematics</run_depend>
   <run_depend>moveit_planners_ompl</run_depend>
+  <run_depend>moveit_planners_chomp</run_depend>
   <run_depend>moveit_ros_visualization</run_depend>
   <run_depend>joint_state_publisher</run_depend>
   <run_depend>robot_state_publisher</run_depend>