Ask Your Question
1

How do you change motion_planning_api tutorial to use chomp?

asked 2018-05-25 17:07:09 -0600

tzahroof gravatar image

updated 2018-05-26 22:27:24 -0600

Hello,

I am a new moveit user trying to use CHOMP on my 7DoF arm. I would ultimately like to take a seed trajectory I've generated from another planner and then pass it into CHOMP. I took a look at the moveit_tutorials and noticed that although there is a chomp_interface tutorial, I don't see how to adjust the moveit motion planning api tutorial's c++ code to load CHOMP. What changes need to be made?

Sorry for the basic question!

edit: http://docs.ros.org/kinetic/api/movei... ^the above is the tutorial I'm referencing

edit retag flag offensive close merge delete

Comments

Can you please update your question with a link to the tutorial that you're referring to?

jayess gravatar image jayess  ( 2018-05-25 23:58:24 -0600 )edit

http://docs.ros.org/kinetic/api/movei...

Sorry about that! The above is the tutorial I'm referencing

tzahroof gravatar image tzahroof  ( 2018-05-26 22:27:40 -0600 )edit

1 Answer

Sort by ยป oldest newest most voted
0

answered 2018-07-13 16:42:33 -0600

tzahroof gravatar image

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.

edit flag offensive delete link more

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

1 follower

Stats

Asked: 2018-05-25 17:07:09 -0600

Seen: 264 times

Last updated: Jul 13 '18