I more or less figured it out, so I thought I'd share my solution in case anyone was thinking of tackling the same issue.

Currently, CHOMP default does not support seeding; I ended up using STOMP for this reason. STOMP seeds through the trajectory_constraints message (which is apparently unused by MoveIt!).

First, ensure that the robot package knows where the trajectory optimizer package is by modifying the CMakeLists.txt and the package.xml files (to include STOMP, I added the stomp_core package) .

from CMakeLists.txt:

find_package(catkin REQUIRED COMPONENTS
moveit_core
stomp_core
moveit_experimental
moveit_visual_tools
moveit_ros_planning
moveit_ros_planning_interface
pluginlib
geometric_shapes
industrial_trajectory_filters
)

catkin_package(
  LIBRARIES
    interactivity_utils
  INCLUDE_DIRS
    ${THIS_PACKAGE_INCLUDE_DIRS}
  CATKIN_DEPENDS
    moveit_core
    stomp_core
    moveit_experimental
    moveit_visual_tools
    moveit_ros_planning_interface
    interactive_markers
    industrial_trajectory_filters
  DEPENDS
    EIGEN3
)

from package.xml:

<build_depend>stomp_core</build_depend>
<run_depend>stomp_core</run_depend>

Also, don't forget to include the your c++ file in the CMakeLists.txt file so that the package knows it's a node!

from CMakeLists.txt:

add_executable(motion_planning_bot src/motion_planning_bot.cpp)
target_link_libraries(motion_planning_bot ${catkin_LIBRARIES} ${Boost_LIBRARIES})
install(TARGETS motion_planning_bot DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})

There's probably a bit of redundancy in the above, but it will still work nonetheless.

The following code seeds the trajectory to STOMP (req is a planning_interface::MotionPlanRequest object and response is a moveit_msgs::MotionPlanResponse)

trajectory_msgs::JointTrajectory seed = response.trajectory.joint_trajectory;
const auto dof = seed.joint_names.size();

for (size_t i = 0; i < seed.points.size(); ++i) // for each time step
{
  moveit_msgs::Constraints c;

  if (seed.points[i].positions.size() != dof)
    throw std::runtime_error("All trajectory position fields must have same dimensions as joint_names");

  for (size_t j = 0; j < dof; ++j) // for each joint
  {
    moveit_msgs::JointConstraint jc;
    jc.joint_name = seed.joint_names[j];
    jc.position = seed.points[i].positions[j];

    c.joint_constraints.push_back(jc);
  }
  req.trajectory_constraints.constraints.push_back(std::move(c));
}

I'd recommend also creating another publisher to visualize the trajectory_msg::JointTrajectory results via the rqt plot plugin. It's a good way to visualize the effects of the trajectory optimizer.

ros::Publisher rqt_publisher = node_handle.advertise<trajectory_msgs::JointTrajectory>("/rqt_publisher/", 1);      
for(unsigned iter = 0; iter < size2; iter++) {
    response.trajectory.joint_trajectory.points[iter].time_from_start = ros::Duration(0.1*iter);
}
rqt_publisher.publish(response.trajectory.joint_trajectory);

MoveIt! uses geometric planners and adds time parameterization afterwards (for velocity/acceleration obedience). The default plan obtained does not have time, so I just spaced it out nicely to display on a graph. I haven't looked into displaying the points after time parameterization..

Unfortunately, I did have STOMP fail to compute a plan with an initial seed frequently; it unfortunately had a 40% success rate with FMT, and still failed 10-15% time with RRT. Not really sure what's happening there.

Comments