ros-industrial / stomp_ros

ROS packages for the STOMP planner (split out of industrial_moveit)
Apache License 2.0
37 stars 28 forks source link

stomp_ros

Build Status: Ubuntu Bionic (Actions) Build Status: Ubuntu Focal (Actions)

Build

Seeding Stomp

The STOMP planner works through optimization: it starts with a given trajectory, called the seed, and iteratively attempts to improve it. This seed is set:

  1. By default, it is set to the joint interpolated path between the start and end joint configurations.
  2. If you wish, you can set your own seed trajectory.

The StompPlanner class works off of the moveit_msgs/MotionPlanRequest message type which does not provide an interface for seeds. Until that is added, we bastardize the unused MotionPlanRequest::trajectory_constraints field to serve this purpose. Use the StompPlanner::encodeSeedTrajectory(const trajectory_msgs::JointTrajectory& seed) static function to do this:


StompPlanner planner = makeStompPlanner(); // However you initialize
planning_interface::MotionPlanRequest request;

// set your nominal goals, start conditions, etc...

trajectory_msgs::JointTrajectory seed_traj; // Look up your seed traj
request.trajectory_constraints = StompPlanner::encodeSeedTrajectory(seed_traj);

// Call the planning service or the planner itself
planner.setMotionPlanRequest(request)

MotionPlanResponse res;
planner.solve(res);

There is no current way to set this through the MoveGroupInterface class.