CHOMP motion planner cleanup of unused parameters and code + addition…

… of trajectory initialization methods (linear, cubic, quintic-spline) (moveit#960)

* added options for trajectory initialization using (linear, cubic) interpolation in addition to quintic-spline, this can be specified in the chomp_planning.yaml file as a string parameter

* removed/cleaned redundant commented code from chomp_optimizer.cpp file

* commented print statements

* removed unused CHOMP parameters code

* applied clang-format and addressed comments

* applied clang-format and addressed comments

* added PR suggested revies/changes

* removed usused 2 lines

* generating chomp_planning.yaml and chomp_planning_pipeline.launch.xml from the moveit setup assistant

* restored CHOMP, hamiltonian monte carlo (HMC) based code and commented it

* applied clang format

moveit_planners/chomp/chomp_interface/src/chomp_interface.cpp

@@ -51,25 +51,24 @@ void CHOMPInterface::loadParams()
   nh_.param("smoothness_cost_weight", params_.smoothness_cost_weight_, 0.1);
   nh_.param("obstacle_cost_weight", params_.obstacle_cost_weight_, 1.0);
   nh_.param("learning_rate", params_.learning_rate_, 0.01);
-  nh_.param("animate_path", params_.animate_path_, true);
-  nh_.param("add_randomness", params_.add_randomness_, false);
+
+  // nh_.param("add_randomness", params_.add_randomness_, false);
   nh_.param("smoothness_cost_velocity", params_.smoothness_cost_velocity_, 0.0);
   nh_.param("smoothness_cost_acceleration", params_.smoothness_cost_acceleration_, 1.0);
   nh_.param("smoothness_cost_jerk", params_.smoothness_cost_jerk_, 0.0);
-  nh_.param("hmc_discretization", params_.hmc_discretization_, 0.01);
-  nh_.param("hmc_stochasticity", params_.hmc_stochasticity_, 0.01);
-  nh_.param("hmc_annealing_factor", params_.hmc_annealing_factor_, 0.99);
-  nh_.param("use_hamiltonian_monte_carlo", params_.use_hamiltonian_monte_carlo_, false);
+  // nh_.param("hmc_discretization", params_.hmc_discretization_, 0.01);
+  // nh_.param("hmc_stochasticity", params_.hmc_stochasticity_, 0.01);
+  // nh_.param("hmc_annealing_factor", params_.hmc_annealing_factor_, 0.99);
+  // nh_.param("use_hamiltonian_monte_carlo", params_.use_hamiltonian_monte_carlo_, false);
   nh_.param("ridge_factor", params_.ridge_factor_, 0.0);
   nh_.param("use_pseudo_inverse", params_.use_pseudo_inverse_, false);
   nh_.param("pseudo_inverse_ridge_factor", params_.pseudo_inverse_ridge_factor_, 1e-4);
-  nh_.param("animate_endeffector", params_.animate_endeffector_, false);
-  nh_.param("animate_endeffector_segment", params_.animate_endeffector_segment_, std::string("r_gripper_tool_frame"));
+
   nh_.param("joint_update_limit", params_.joint_update_limit_, 0.1);
   nh_.param("collision_clearence", params_.min_clearence_, 0.2);
   nh_.param("collision_threshold", params_.collision_threshold_, 0.07);
-  nh_.param("random_jump_amount", params_.random_jump_amount_, 1.0);
+  // nh_.param("random_jump_amount", params_.random_jump_amount_, 1.0);
   nh_.param("use_stochastic_descent", params_.use_stochastic_descent_, true);
-  // filter_mode_ = false;
+  nh_.param("interpolation_method", params_.trajectory_initialization_method_, std::string("quintic-spline"));
 }
 }

moveit_planners/chomp/chomp_motion_planner/include/chomp_motion_planner/chomp_parameters.h

@@ -53,29 +53,29 @@ class ChompParameters
   int getMaxIterationsAfterCollisionFree() const;
   double getSmoothnessCostWeight() const;
   double getObstacleCostWeight() const;
-  bool getAnimatePath() const;
+
   double getLearningRate() const;
   double getSmoothnessCostVelocity() const;
   double getSmoothnessCostAcceleration() const;
   double getSmoothnessCostJerk() const;
-  bool getAddRandomness() const;
-  bool getUseHamiltonianMonteCarlo() const;
-  double getHmcDiscretization() const;
-  double getHmcStochasticity() const;
-  double getHmcAnnealingFactor() const;
+  // bool getAddRandomness() const;
+  // bool getUseHamiltonianMonteCarlo() const;
+  // double getHmcDiscretization() const;
+  // double getHmcStochasticity() const;
+  // double getHmcAnnealingFactor() const;
   double getRidgeFactor() const;
   bool getUsePseudoInverse() const;
   double getPseudoInverseRidgeFactor() const;
-  bool getAnimateEndeffector() const;
-  std::string getAnimateEndeffectorSegment() const;
+
   double getJointUpdateLimit() const;
   double getMinClearence() const;
   double getCollisionThreshold() const;
   bool getFilterMode() const;
   void setFilterMode(bool mode);
-  double getRandomJumpAmount() const;
-  void setRandomJumpAmount(double amount);
+  // double getRandomJumpAmount() const;
+  // void setRandomJumpAmount(double amount);
   bool getUseStochasticDescent() const;
+  std::string getTrajectoryInitializationMethod() const;
 
 public:
   double planning_time_limit_;
@@ -84,30 +84,32 @@ class ChompParameters
   double smoothness_cost_weight_;
   double obstacle_cost_weight_;
   double learning_rate_;
-  bool animate_path_;
+
   double smoothness_cost_velocity_;
   double smoothness_cost_acceleration_;
   double smoothness_cost_jerk_;
-  bool add_randomness_;
-  bool use_hamiltonian_monte_carlo_;
+  // bool add_randomness_;
+  // bool use_hamiltonian_monte_carlo_;
   bool use_stochastic_descent_;
-  double hmc_stochasticity_;
-  double hmc_discretization_;
-  double hmc_annealing_factor_;
+
+  // double hmc_stochasticity_;
+  // double hmc_discretization_;
+  // double hmc_annealing_factor_;
   double ridge_factor_;
   bool use_pseudo_inverse_;
   double pseudo_inverse_ridge_factor_;
-  bool animate_endeffector_;
-  std::string animate_endeffector_segment_;
+
   double joint_update_limit_;
   double min_clearence_;
   double collision_threshold_;
   bool filter_mode_;
-  double random_jump_amount_;
+  // double random_jump_amount_;
+  std::string trajectory_initialization_method_;
 };
 
 /////////////////////// inline functions follow ////////////////////////
 
+/*
 inline double ChompParameters::getRandomJumpAmount() const
 {
   return random_jump_amount_;
@@ -117,6 +119,7 @@ inline void ChompParameters::setRandomJumpAmount(double amount)
 {
   random_jump_amount_ = amount;
 }
+*/
 
 inline double ChompParameters::getCollisionThreshold() const
 {
@@ -178,15 +181,12 @@ inline double ChompParameters::getLearningRate() const
   return learning_rate_;
 }
 
-inline bool ChompParameters::getAnimatePath() const
-{
-  return animate_path_;
-}
-
+/*
 inline bool ChompParameters::getAddRandomness() const
 {
   return add_randomness_;
 }
+*/
 
 inline double ChompParameters::getSmoothnessCostVelocity() const
 {
@@ -203,6 +203,7 @@ inline double ChompParameters::getSmoothnessCostJerk() const
   return smoothness_cost_jerk_;
 }
 
+/*
 inline double ChompParameters::getHmcDiscretization() const
 {
   return hmc_discretization_;
@@ -222,6 +223,7 @@ inline bool ChompParameters::getUseHamiltonianMonteCarlo() const
 {
   return use_hamiltonian_monte_carlo_;
 }
+*/
 
 inline double ChompParameters::getRidgeFactor() const
 {
@@ -238,19 +240,14 @@ inline double ChompParameters::getPseudoInverseRidgeFactor() const
   return pseudo_inverse_ridge_factor_;
 }
 
-inline bool ChompParameters::getAnimateEndeffector() const
-{
-  return animate_endeffector_;
-}
-
 inline bool ChompParameters::getUseStochasticDescent() const
 {
   return use_stochastic_descent_;
 }
 
-inline std::string ChompParameters::getAnimateEndeffectorSegment() const
+inline std::string ChompParameters::getTrajectoryInitializationMethod() const
 {
-  return animate_endeffector_segment_;
+  return trajectory_initialization_method_;
 }
 
 }  // namespace chomp

moveit_planners/chomp/chomp_motion_planner/include/chomp_motion_planner/chomp_trajectory.h

@@ -115,6 +115,20 @@ class ChompTrajectory
    */
   void fillInMinJerk();
 
+  /**
+   * \brief Generates a linearly interpolated trajectory from the start index to end index
+   *
+   * Only modifies points from start_index_ to end_index_, inclusive
+   */
+  void fillInLinearInterpolation();
+
+  /**
+   * \brief Generates a cubic interpolation of the trajectory from the start index to end index
+   *
+   * Only modifies points from start_index_ to end_index_, inclusive
+   */
+  void fillInCubicInterpolation();
+
   /**
    * \brief Sets the start and end index for the modifiable part of the trajectory
    *