Addition of CHOMP planning adapter for optimizing result of other pla…

…nners (moveit#1012)

* added functionality for CHOMP Optimization Adapter and made some changes in planning_scene.h / .cpp files to make this work
* added functionality for optimizing path obtained from OMPL, under construction but works fine right now
* added functionality to initialize trajectories in CHOMP when input trajectory has more points than CHOMP trajectory points
* hotfix moveit#1086

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

@@ -41,6 +41,9 @@
 #include <moveit/robot_model/robot_model.h>
 #include <chomp_motion_planner/chomp_utils.h>
 
+#include <moveit_msgs/MotionPlanDetailedResponse.h>
+#include <moveit_msgs/MotionPlanRequest.h>
+
 #include <vector>
 #include <eigen3/Eigen/Core>
 
@@ -129,6 +132,25 @@ class ChompTrajectory
    */
   void fillInCubicInterpolation();
 
+  /**
+   * \brief Receives the path obtained from a given MotionPlanDetailedResponse res object's trajectory (e.g., trajectory
+   * produced by OMPL) and puts it into the appropriate trajectory format required for CHOMP
+   * @param res
+   */
+  bool fillInFromTrajectory(moveit_msgs::MotionPlanDetailedResponse& res);
+
+  /**
+   * This function assigns the chomp_trajectory row / robot pose at index 'chomp_trajectory_point' obtained from input
+   * trajectory_msgs at index 'trajectory_msgs_point'
+   * @param trajectory_msg the input trajectory_msg
+   * @param num_joints_trajectory number of joints in the given robot trajectory
+   * @param trajectory_msgs_point index of the input trajectory_msg's point to get joint values from
+   * @param chomp_trajectory_point index of the chomp_trajectory's point to get joint values from
+   */
+  void assignCHOMPTrajectoryPointFromInputTrajectoryPoint(moveit_msgs::RobotTrajectory trajectory_msg,
+                                                          int num_joints_trajectory, int trajectory_msgs_point,
+                                                          int chomp_trajectory_point);
+
   /**
    * \brief Sets the start and end index for the modifiable part of the trajectory
    *

moveit_planners/chomp/chomp_motion_planner/src/chomp_planner.cpp

@@ -146,9 +146,21 @@ bool ChompPlanner::solve(const planning_scene::PlanningSceneConstPtr& planning_s
     trajectory.fillInLinearInterpolation();
   else if (params.trajectory_initialization_method_.compare("cubic") == 0)
     trajectory.fillInCubicInterpolation();
+  else if (params.trajectory_initialization_method_.compare("fillTrajectory") == 0)
+  {
+    if (!(trajectory.fillInFromTrajectory(res)))
+    {
+      ROS_ERROR_STREAM_NAMED("chomp_planner", "Input trajectory has less than 2 points, trajectory must contain "
+                                              "atleast start and goal state");
+      return false;
+    }
+  }
   else
     ROS_ERROR_STREAM_NAMED("chomp_planner", "invalid interpolation method specified in the chomp_planner file");
 
+  ROS_INFO_NAMED("chomp_planner", "CHOMP trajectory initialized using method: %s ",
+                 (params.trajectory_initialization_method_).c_str());
+
   // optimize!
   moveit::core::RobotState start_state(planning_scene->getCurrentState());
   moveit::core::robotStateMsgToRobotState(req.start_state, start_state);

moveit_planners/chomp/chomp_motion_planner/src/chomp_trajectory.cpp

@@ -252,4 +252,74 @@ void ChompTrajectory::fillInMinJerk()
   }
 }
 
+bool ChompTrajectory::fillInFromTrajectory(moveit_msgs::MotionPlanDetailedResponse& res)
+{
+  // create a RobotTrajectory msg to obtain the trajectory from the MotionPlanDetailedResponse object
+  moveit_msgs::RobotTrajectory trajectory_msg = res.trajectory[0];
+
+  // get the default number of points in the CHOMP trajectory
+  int num_chomp_trajectory_points = (*this).getNumPoints();
+  // get the number of points in the response trajectory obtained from OMPL
+  int num_response_points = trajectory_msg.joint_trajectory.points.size();
+
+  // check if input trajectory has less than two states (start and goal), function returns false if condition is true
+  if (num_response_points < 2)
+    return false;
+
+  // get the number of joints for each robot state
+  int num_joints_trajectory = trajectory_msg.joint_trajectory.points[0].positions.size();
+
+  // variables for populating the CHOMP trajectory with correct number of trajectory points
+  int repeated_factor = num_chomp_trajectory_points / num_response_points;
+  int repeated_balance_factor = num_chomp_trajectory_points % num_response_points;
+
+  // response_point_id stores the point at the stored index location.
+  int response_point_id = 0;
+  if (num_chomp_trajectory_points >= num_response_points)
+  {
+    for (int i = 0; i < num_response_points; i++)
+    {
+      // following for loop repeats each OMPL trajectory pose/row 'repeated_factor' times; alternatively, there could
+      // also be a linear interpolation between these points later if required
+      for (int k = 0; k < repeated_factor; k++)
+      {
+        assignCHOMPTrajectoryPointFromInputTrajectoryPoint(trajectory_msg, num_joints_trajectory, i, response_point_id);
+        response_point_id++;
+      }
+
+      // this populates the CHOMP trajectory row  for the remainder number of rows.
+      if (i < repeated_balance_factor)
+      {
+        assignCHOMPTrajectoryPointFromInputTrajectoryPoint(trajectory_msg, num_joints_trajectory, i, response_point_id);
+        response_point_id++;
+      }  // end of if
+    }    // end of for loop for loading in the trajectory poses/rows
+  }
+  else
+  {
+    // perform a decimation sampling in this block if the number of trajectory points in the MotionPlanDetailedResponse
+    // res object is more than the number of points in the CHOMP trajectory
+    double decimation_sampling_factor = ((double)num_response_points) / ((double)num_chomp_trajectory_points);
+
+    for (int i = 0; i < num_chomp_trajectory_points; i++)
+    {
+      int sampled_point = floor(i * decimation_sampling_factor);
+      assignCHOMPTrajectoryPointFromInputTrajectoryPoint(trajectory_msg, num_joints_trajectory, sampled_point, i);
+    }
+  }  // end of else
+  return true;
+}
+
+void ChompTrajectory::assignCHOMPTrajectoryPointFromInputTrajectoryPoint(moveit_msgs::RobotTrajectory trajectory_msg,
+                                                                         int num_joints_trajectory,
+                                                                         int trajectory_msgs_point_index,
+                                                                         int chomp_trajectory_point_index)
+{
+  // following loop iterates over all joints for a single robot pose, it assigns the corresponding input trajectory
+  // points into CHOMP trajectory points
+  for (int j = 0; j < num_joints_trajectory; j++)
+    (*this)(chomp_trajectory_point_index, j) =
+        trajectory_msg.joint_trajectory.points[trajectory_msgs_point_index].positions[j];
+}
+
 }  // namespace chomp

moveit_ros/planning/CMakeLists.txt

@@ -24,6 +24,7 @@ find_package(catkin REQUIRED COMPONENTS
   tf2_eigen
   tf2_msgs
   tf2_ros
+  chomp_motion_planner
 )
 
 find_package(Eigen3 REQUIRED)

moveit_ros/planning/package.xml

@@ -30,6 +30,7 @@
   <build_depend>tf2_geometry_msgs</build_depend>
   <build_depend>tf2_msgs</build_depend>
   <build_depend>tf2_ros</build_depend>
+  <build_depend>chomp_motion_planner</build_depend>
 
   <run_depend>moveit_core</run_depend>
   <run_depend>moveit_ros_perception</run_depend>

moveit_ros/planning/planning_request_adapter_plugins/CMakeLists.txt

@@ -7,7 +7,25 @@ set(SOURCE_FILES
   src/fix_start_state_path_constraints.cpp
   src/fix_workspace_bounds.cpp
   src/add_time_parameterization.cpp
-  src/add_iterative_spline_parameterization.cpp)
+  src/add_iterative_spline_parameterization.cpp
+  src/chomp_optimizer_adapter.cpp)
+
+find_package(catkin REQUIRED COMPONENTS
+  cmake_modules
+  roscpp
+  moveit_core
+  moveit_msgs
+  pluginlib
+  chomp_motion_planner
+  )
+
+find_package(Eigen3 REQUIRED)
+find_package(Boost REQUIRED)
+
+include_directories(
+  ../../../moveit_planners/chomp/chomp_motion_planner/include/
+  ../../../moveit_experimental/collision_distance_field/include/)
+
 
 add_library(${MOVEIT_LIB_NAME} ${SOURCE_FILES})
 set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION ${${PROJECT_NAME}_VERSION})
@@ -19,4 +37,3 @@ target_link_libraries(moveit_list_request_adapter_plugins ${catkin_LIBRARIES} ${
 install(TARGETS ${MOVEIT_LIB_NAME} moveit_list_request_adapter_plugins
   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
-