Moveit: TrajOpt motion planner for MoveIt

[ommmid]

Add TrajOpt, an optimization based motion planning algorithm, to MoveIt.

2019 Summer Internship at PickNik
Mentor: @davetcoleman

MoveIt is designed to be highly plugin-based and is able to support many different planing libraries, as seen with last year’s GSOC addition of STOMP and CHOMP. A next good candidate to be added to MoveIt is the TrajOpt planner. Bullet collision detection is going to be used for TrajOpt planner to add to MoveIt.

• [ ] Assist with implementing features critical for TrajOpt, such as collision checking, including
  convex-convex collisions and continuous collision detection.
• [ ] Add TrajOpt as a library to be used in MoveIt
• [ ] Add adapters that allow TrajOpt to post-process paths produced by other planners
• [ ] Write Moveit tutorials and tests for TrajOpt motion planning
• [ ] Collaborating with @j-petit for integrating Bullet collision detection.

[welcome[bot]]

Thanks for reporting an issue. We will have a look asap. If you can think of a fix, please consider providing it as a pull request.

[davetcoleman]

We're really excited about this effort @ommmid is doing this summer as part of his internship at PickNik!

[ommmid]

A brief summery of the theories of TrajOpt from the published paper.

=> Basically, TrajOpt change a non-convex optimization problem to subproblems of convex optimization which can be solved locally.

=> source of constraints
Inequality constraints are obstacle avoidance, joint limits and speed limits
Equality constraints are end-effector pose at target at the end of trajectory.

=> The conversion from non-convex to subproblem of convex is through Sequential Convex Optimization.
Two important keys are trust region to keep the next iteration close enough to the current iteration and converting the equality and inequality constraints to penalty functions (L1)

=> Algorithm
1) For each penalty coefficient (stop if constraints are satisfied)
2) Construct a convex approximation of the problem and for each of these (stop if the function is converged)
3) Expand or shrink the trust region till the ratio of improvement in the real objective function (non convex) over our convex model is greater than some predefined value.

=> Penalty function for collision
1) Discrete-time no collision constraint: is based on signed distance (sd) = dist - penetration.
If sd > 0 the objects are non-colliding
If sd < 0 the objects are colliding
If we add a safety margin sf , then we want to have:
sd > sf
where sd(obstacle collision) > sf and sd (self collision) > sf

These constraints are relaxed by L1 penalty functions. But the sd function itself is given by (14) in the paper which by linearizing it and calculating the derivative (Jacobian), we can approximate it for local collision. The problem for this function arises in the case of face-face contact for which the function is not differential.

2) Continuous-time collision free: Here we use the swept-out volume which is the convex hull of the initial and final volumes for one discrete time and then consider this volume as an object which should be free from collision. This also involves inequality which can be then relaxed by the same method above (penalty L1 function). The collision checking time for continuous-time is twice as long.

=> The difference between TrajOpt and CHOMP:
Different collision detection (Euclidean distance vs convex-convex collision detection)
Different numerical optimization (projected gradient descent vs SQP)

[BryceStevenWilley]

A few corrections here, mostly in your wording:

=> source of constraints
Inequality constraints are obstacle avoidance, joint limits and speed limits
Equality constraints are end-effector pose at target at the end of trajectory.

There can actually be any given number of equality/inequality constraints, applying to any part of the trajectory, for example, making the end effector perpendicular to the horizontal plane for the entire trajectory. I think you might be talking abou the minimum required constraints here, in which case you still need to include the starting joint position as an equality constraint.

=> The difference between TrajOpt and CHOMP:
Different collision detection (Euclidean distance vs convex-convex collision detection)

Technically correct, but at the end of the day, for discrete cases, CHOMP and TrajOpt aren't that different; they both need the minimum SD for each link. They do differ in suggesting implementations to get this SD though, which is what you mention here (though I'd probably say Euclidean distance via sphere-tessellated links and signed distance fields). TrajOpt real contribution here was the continuous collision detection, not just the convex-convex part.

[ommmid]

@BryceStevenWilley thanks for the corrections. I agree that should be the minimum sources of constraints which should include both ends of the trajectory.

[ommmid]

Here is a graph of how ompl plugin is created, gets loaded and when FCL gets called in MoveIt. @davetcoleman @BryceStevenWilley please have a look and let me know if anything is wrong.

moveit_ompl_plugin.pdf

[BryceStevenWilley]

Overall looks good! Nothing is technically wrong that I can tell, though the flow is a little hard to follow. What did you use to make it? I'm curious how manual the process was.

[ommmid]

The whole process was manual actually. I made it in google drawings gradually as I was reviewing the code in MoveIt.

[ommmid]

We need a an environment in MoveIt similar to ROSBasicEnv from tesseract to do the trajopt. @Levi-Armstrong @BryceStevenWilley , any hint on how to adjust PlanningScene to become like ROSBasicEnv so we can pass it to ConstructProblem in trajopt?

[BryceStevenWilley]

Not really sure what you mean by this. All TrajOpt needs in construct problem is the definitions of the inequality and equality constraints (usually these are simply callback functions, such that the optimization can simply call the function object with the current variables, i.e. current trajectory), and the initial trajectory guess. From what I can tell, the only thing really necessary in tesseract's version is the collision managers, which will be equivalent to MoveIt's CollisionRobot + CollisionWorld's checkCollision (with Jen's current work).

[ommmid]

I mean what is passed to ConstrctProblem is request_.env:
https://github.com/ros-industrial-consortium/tesseract/blob/master/tesseract_planning/src/trajopt/trajopt_planner.cpp
TrajOptProbPtr prob = ConstructProblem(root, request_.env)

The env is derived from BasicEnv:
https://github.com/ros-industrial-consortium/tesseract/blob/master/tesseract_core/include/tesseract_core/basic_env.h

So I thought, in MoveIt, maybe we should make the same type of environment to pass to ConstructProblem so we can use optimize() function eventually.

[BryceStevenWilley]

I believe what you're looking for is https://github.com/ros-industrial-consortium/trajopt_ros/blob/ae07b788a91f2fd5c017456d441993a115ff270e/trajopt_moveit/src/trajopt_moveit_env.cpp#L31.

At the end of the day however, ConstructProblem is glue code, meant to combine tesseract specific structures to TrajOpt's core optimization. It seems awkward to have our own glue layer that converts to tesseract structures instead of simply using the optimization core directly.

[ommmid]

Tasks for the next days:

• [x] find out how to relate the response in trajopt and moveit
• [x] find out how to relate the request in trajopt and moveit
  
  
  
  • [x] write the PlanningContext class for trajopt, TrajOptPlanningContext
  
  
  • [x] figure out how to make the solve function in TrajOptPlanningContext,
  
  
  • [x] complete getPlanningContext function in TrajOptPlannerManager
  
  

Not to be too worried about dependency on tesseract for now and try make trajopt work for a motion planning task. Will try to remove the dependency gradually.

[davetcoleman]

@ommmid can you provide a public update of your progress here, please? Its been great seeing it coming along, internally!

[ommmid]

[felixvd]

Can you explain in a bit more detail or point to a post where the current state of things is explained? It is hard to resume work on this as is.

[ommmid]

Current state:

• Reference trajectory can be set by the user now, before was hard-coded
• OSQP is the default solver that is downloaded and installed in build directory
• When using motion planning display in rviz, the reference trajectory can not be set. In this case, the user receives warning if ref traj is empty
  Also, there have been some other small fixes but the collision cost "terminfo" is still missing which is the current major issue.

[ommmid]

Also for better understanding the trajopt algorithm and how to work with it, the tutorial is a good source. It gives more details and this graph is updated in it.

[felixvd]

But in principle, it runs with MoveIt as described in the tutorial, right? Would you consider this integration finished? What else would need to be done?

Also, I guess I could read the paper and code, but if you don't mind me asking: It says in the tutorial that the current implementation of TrajOpt only supports constraints in joint space. Is this a limitation that was there in the original implementation provided by the authors? Or is it a MoveIt-specific issue?

[BryceStevenWilley]

Is this a limitation that was there in the original implementation provided by the authors? Or is it a MoveIt-specific issue?

That's MoveIt-specific. The general approach in the paper for orientation constraints is to take the difference of the current eef pose to the desired pose, turn it into an (x, y, z, roll, pitch, yaw) error vector and accumulate that vector to the overall error.

[felixvd]

Thanks. Do you remember if the same limitation exists in the TrajOpt implementation? Or does the tutorial expect that to be installed and MoveIt just interfaces with that package?

[BryceStevenWilley]

The ROS-I/tesseract TrajOpt shouldn't have that limitation, it works with Cartesian constraints. And I don't remember if the tutorial still requires a dependency on tesseract at all, I don't think it does. Even if MoveIt does depend on tesseract, the ability to describe the Cartesian constraint through MoveIt shouldn't be in yet.

[karolyartur]

I was trying to build moveit_planners_trajopt package, but the following error happened during building:

CMake Error at /opt/ros/melodic/share/catkin/cmake/catkinConfig.cmake:83 (find_package):
  Could not find a package configuration file provided by "trajopt" with any
  of the following names:

    trajoptConfig.cmake
    trajopt-config.cmake

However, trajopt is required for moveit_planners_trajopt, so it is also built before building moveit_planners_trajopt, and it builds successfully. I am able to find it with rospack find trajopt as well.
Why is CMake still unable to find the trajopt package?

I cloned trajopt _ros from here and I am on branch master

The tesseract and tesseract_ext modules are also built from source and for both I use the master branches.

Moveit is also on master branch at commit 6b56cd68.

I use ros melodic and ubuntu 18.04.

[ommmid]

But in principle, it runs with MoveIt as described in the tutorial, right? Would you consider this integration finished? What else would need to be done?

Also, I guess I could read the paper and code, but if you don't mind me asking: It says in the tutorial that the current implementation of TrajOpt only supports constraints in joint space. Is this a limitation that was there in the original implementation provided by the authors? Or is it a MoveIt-specific issue?

Felix, constraints in Cartesian space is not implemented in this integration yet. The original trajopt can definitely work with constraints in joint or Cartesian space. This is a to-do in the current integration.

About dependency, the trajopt in MoveIt does not depend on tesseract or the original trajopt repo at all. We do not install trajopt or tesseract and have MoveIt interfacing with them; instead, the needed code was copied, edited and adjusted so it could work with MoveIt environment instead of tesseract environment.

The plan was to do this transition step by step going through all the "terminfo"s and convert them to what MoveIt can accept and work with. Things left to be addressed:

Cartesian constraint (CartPoseTermInfo)
Joint Velocity constraint (JointVelTermInfo)
Joint Acc constraint (JointAccTermInfo)
Joint Jerk constraint (JointJerkTermInfo)
Collision cost (CollisionTermInfo)

[ommmid]

I was trying to build moveit_planners_trajopt package, but the following error happened during building:

CMake Error at /opt/ros/melodic/share/catkin/cmake/catkinConfig.cmake:83 (find_package):
  Could not find a package configuration file provided by "trajopt" with any
  of the following names:

    trajoptConfig.cmake
    trajopt-config.cmake

However, trajopt is required for moveit_planners_trajopt, so it is also built before building moveit_planners_trajopt, and it builds successfully. I am able to find it with rospack find trajopt as well.
Why is CMake still unable to find the trajopt package?

I cloned trajopt _ros from here and I am on branch master

The tesseract and tesseract_ext modules are also built from source and for both I use the master branches.

Moveit is also on master branch at commit 6b56cd6.

I use ros melodic and ubuntu 18.04.

You do not need external trajopt and tesseract repos. What is the branch you pulled down for trajopt in MoveIt? This is the pr you want to pull down

[simonschmeisser]

Is the plan to first have somebody implement the missing 5 TermInfos and then merge #1626 ?

Is anybody working on this? Could @JeroenDM do this as part of GSOC?

Does not having CollisionTermInfo mean there is no collision checking at all yet?

[JeroenDM]

Is anybody working on this? Could @JeroenDM do this as part of GSOC?

I included adding the missing terms for Cartesian constraints, based on the existing implementation in trajopt_ros. Also adding JointVelTermInfo, JointAccTermInfo, JointJerkTermInfo in one go is probably realistic. But I plan to stay away from CollisionTermInfo.(Given that my main focus is on OMPL's constrained planning integration now, and not on collision checking or the new collision interface.)

Ideally, a second GSoC project materializes that works the collision interface :)

(Google doc of my proposal draft)

[ommmid]

Is anybody working on this? Could @JeroenDM do this as part of GSOC?

I included adding the missing terms for Cartesian constraints, based on the existing implementation in trajopt_ros. Also adding JointVelTermInfo, JointAccTermInfo, JointJerkTermInfo in one go is probably realistic. But I plan to stay away from CollisionTermInfo.(Given that my main focus is on OMPL's constrained planning integration now, and not on collision checking or the new collision interface.)

Ideally, a second GSoC project materializes that works the collision interface :)

(Google doc of my proposal draft)

CollisionTermInfo is not a collision detector. So, you do not have to deal with collision checking process and methods. Collisionterminfo uses collision detectors like Bullet to formulate a collision cost function for optimization process. Again, collision checking part is done by collision detectors.
To have a strong proposal for trajopt integration, we definitely want to have collision as a cost in the optimization process which is the main issue in the current integration.

[karolyartur]

I was trying to build moveit_planners_trajopt package, but the following error happened during building:

CMake Error at /opt/ros/melodic/share/catkin/cmake/catkinConfig.cmake:83 (find_package):
  Could not find a package configuration file provided by "trajopt" with any
  of the following names:

    trajoptConfig.cmake
    trajopt-config.cmake

However, trajopt is required for moveit_planners_trajopt, so it is also built before building moveit_planners_trajopt, and it builds successfully. I am able to find it with rospack find trajopt as well.
Why is CMake still unable to find the trajopt package?
I cloned trajopt _ros from here and I am on branch master
The tesseract and tesseract_ext modules are also built from source and for both I use the master branches.
Moveit is also on master branch at commit 6b56cd6.
I use ros melodic and ubuntu 18.04.

You do not need external trajopt and tesseract repos. What is the branch you pulled down for trajopt in MoveIt? This is the pr you want to pull down

Thank you! I managed to build it, using the trajopt branch at commit 391fb6a2.
Now the following problem happens:

When I try to run the panda_moveit_config_demo with trajopt pipeline (roslaunch panda_moveit_config demo.launch pipeline:=trajopt) the move_group node dies because of segmentation fault right at the start.

The output is the following:

Planning scene monitors started.
You can set logging level with TRAJOPT_LOG_THRESH. Valid values: FATAL ERROR WARN INFO DEBUG TRACE. Defaulting to ERROR
[ INFO] [1585286618.697977541]:  ======================================= initialize gets called
[ INFO] [1585286618.698012387]:  ======================================= group name: hand, robot model: panda
[ INFO] [1585286618.698056169]:  ======================================= TrajOptPlanningContext is constructed
[move_group-5] process has died [pid 26302, exit code -11, cmd /root/underlay_ws/devel/lib/moveit_ros_move_group/move_group --debug /joint_states:=/joint_states_desired __name:=move_group __log:=/root/.ros/log/2120fe26-6fe3-11ea-9689-b068e672dc5d/move_group-5.log].
log file: /root/.ros/log/2120fe26-6fe3-11ea-9689-b068e672dc5d/move_group-5*.log

[JeroenDM]

CollisionTermInfo is not a collision detector. So, you do not have to deal with collision checking process and methods. Collisionterminfo uses collision detectors like Bullet to formulate a collision cost function for optimization process. Again, collision checking part is done by collision detectors.

Thank you for the clarification. If I understand correctly it would still require major changes to the original collision_terms.cpp as this uses collision detectors from tesseract (which wraps both bullet and fcl).
This would have to be replaced by the interface from last year's GSoC project.

To have a strong proposal for trajopt integration, we definitely want to have collision as a cost in the optimization process which is the main issue in the current integration.

I completely agree! But as TrajOpt integration is not the focus of my proposal I will leave this for someone else :) (or future me) .

[JeroenDM]

When I try to run the panda_moveit_config_demo with trajopt pipeline (roslaunch panda_moveit_config demo.launch pipeline:=trajopt) the move_group node dies because of segmentation fault right at the start.

The output is the following:

Planning scene monitors started.
You can set logging level with TRAJOPT_LOG_THRESH. Valid values: FATAL ERROR WARN INFO DEBUG TRACE. Defaulting to ERROR
[ INFO] [1585286618.697977541]:  ======================================= initialize gets called
[ INFO] [1585286618.698012387]:  ======================================= group name: hand, robot model: panda
[ INFO] [1585286618.698056169]:  ======================================= TrajOptPlanningContext is constructed
[move_group-5] process has died [pid 26302, exit code -11, cmd /root/underlay_ws/devel/lib/moveit_ros_move_group/move_group --debug /joint_states:=/joint_states_desired __name:=move_group __log:=/root/.ros/log/2120fe26-6fe3-11ea-9689-b068e672dc5d/move_group-5.log].
log file: /root/.ros/log/2120fe26-6fe3-11ea-9689-b068e672dc5d/move_group-5*.log

I get exactly the same error.
The log file mentioned in the error move_group-5*.log also seems to be missing.

[ommmid]

@karolyartur @JeroenDM It is resolved. The problem was trajopt_problem_ from trajopt_interface.cpp. It should be working fine now. I tested it in docker too.

[karolyartur]

@karolyartur @JeroenDM It is resolved. The problem was trajopt_problem_ from trajopt_interface.cpp. It should be working fine now. I tested it in docker too.

Thank you!
The previous issue was solved. However I have some new difficulties which I think are related to the tuning of the config parameters.

First of all, does the Trajopt planner always expect the start_state field of the motion plan request to be filled? Even if I set the start_fixed problem info parameter to true, so the start pose should be the current pose of the robot? I would like to construct my own motion plan requests, but if I leave the start_state field empty, I always get the error that Start state violates joint limits. Can I just ignore this if I would like to use the current state as the start pose? Is there a setup that I miss to ignore this?

Second, if I use the planner from Rviz Motion Planning plugin with the panda_movit_config demo scene, I am able to plan successfully with the config parameters set like here, but sometimes (seemingly when the goal pose is close to the start pose) the planning fails with an erros of: Goal constraints are violated: panda_joint1 . Could you give me a hint how to change the parameters to solve this issue? The difference in the response and goal joint positions is rather large in some cases (up to ~ 0.3), so increasing the threshold is not a reasonable solution.

[ommmid]

@karolyartur setting start_fixed to true will add the current state as a constraint for optimization but still looks for a constraint in start_state of the motion plan request. The reason is that the current state is not assumed to be the start state in trajopt. setting start_fixed simply means add the current state constraint, it does not mean current state and start state are the same. Look at this picture

In the event that they are the same, you could set start_fixed to either false (recommended) or true and then manually in your code, set the start_state of the motion plan request to the current state.

It seems start_fixed creates confusion. Maybe we revise this in the process of completing the integration in the near future.

However, if you get Start state violates joint limits error, it means that your motion plan request start_state is NOT empty, it has values that violates the joint values. So maybe you want to look at your motion plan request's start state again.

About the second issue, one thing to watch is at what step you want to apply your constraint i.e. first_timestep and last_timestep. Also, perhaps changing trust_shrink/expand_ratio would help. Maybe you can share one of the failing cases, your input start/goals constraints, here

[ommmid]

I am trying to adapt CalcCollisions function from original trajopt_ros reop. Couple of questions:

The ContactMap type in MoveIt has a vector of Contact associated with each collision pair. Why do we have a vector instead of one signed distance? Is this vector showing multiple contact points between two bodies? Each element in this vector has a depth member. Do we get the smallest depth as the distance between the two bodies in collision?

Each CollisionResult also has a distance member. Is it the minimum distance among all collision pairs?

In trajopt_ros, ContactResultVector is a type that is storing the ContactResult information. This ContactResult has a distance member and the names of two bodies in collision which makes sense. This distance is the signed distance. I need to read this distance from checkCollision in planning_scene to pass it to CalcCollisions and I want to make sure I get the right distance

@BryceStevenWilley @j-petit @felixvd

[BryceStevenWilley]

Is this vector showing multiple contact points between two bodies?

From what I can recall, yes.

Each element in this vector has a depth member. Do we get the smallest depth as the distance between the two bodies in collision?

Since each element is a single contact point, I believe the depth is the penetration dist of that single point.

Each CollisionResult also has a distance member. Is it the minimum distance among all collision pairs?

That should be the case. The CollisionResult is filled in here for bullet.

[j-petit]

FCL can return all contact points (or up to a certain number) of two objects but Bullet will only return the deepest penetration depth. I had a screenshot of this behavior somewhere side by side but it was somewhere on the slack channel which is now deleted (at least for me...).

This means Bullet will only return a single contact point in the ContactMap.

[ommmid]

FCL can return all contact points (or up to a certain number) of two objects but Bullet will only return the deepest penetration depth. I had a screenshot of this behavior somewhere side by side but it was somewhere on the slack channel which is now deleted (at least for me...).

This means Bullet will only return a single contact point in the ContactMap.

I just checked it with visualization_collision tutorial in MoveIt. Here is the contact points calculated by Bullet which only returns one point for each pair:

For FCL is something like this:

which is multiple points.

Now, if I set the CollisionRequest.distance to true and use Bullet, I get info for pairs regardless if they are in contact or not. In this case, I think depth is representing distances whenever it is positive and is representing penetrations whenever it is negative, is that correct? which is signed distance basically for all the pairs in the scene?

[ommmid]

@BryceStevenWilley any idea how to calculate cc_nearest_points for MoveIt? Contact type does not have such a thing.
Also, I just wanted to get more information, so looked at their current work but they got rid of this cc_nearest_points

[felixvd]

Have you tried asking on their repository or had a look at the git history to see why the function was removed? There must have been a reason. Would be great if this could move forward :)

[Levi-Armstrong]

The cc_nearest_point is when performing continuous collision checking. This will store the nearest point in world coordinates during the motion between two states. Then this is used to calculate point on the link at state0 and state1 in world which gets stored in nearest_points. Hope this helps.

[jingweirobot]

@karolyartur Hi Sir.

How did you solve this issue? Thanks in advance

CMake Error at /opt/ros/melodic/share/catkin/cmake/catkinConfig.cmake:83 (find_package):
Could not find a package configuration file provided by "trajopt" with any
of the following names:

trajoptConfig.cmake
trajopt-config.cmake