Moveit: Add continous FCL collision checking for speedup

[davetcoleman]

MoveIt! currently only collision checks motions/segments/edges by discretizing the segment at some resolution into individual states that are then sent one-by-one to FCL. This is also how MoveIt! plans with OMPL. Switching to FCL's powerful continuous collision checking will likely result in an important speedup (CC is the slowest part of planning).

Currently there are unimplemented functions for this in MoveIt!

We probably want to use FCL's BroadPhaseContinuousCollisionManager

An example implementation is already in OMPLApp

Would be great to find someone to work on this

@Levi-Armstrong, @mamoll, @panjia1983, @jslee02

[mamoll]

In OMPL.app, discrete collision checking is actually significantly faster than continuous collision checking (see CollisionCheckers.cpp for a comparison program. Of course, this depends on the collision checking resolution and most likely depends on the type of robot (OMPL.app only does rigid body motion planning). Finally, OMPL.app deals only with triangle soup meshes internally. It could be that _in general_ cont. collision detection is actually faster.

[davetcoleman]

MoveIt! only does rigid body motion planning, but they key difference (I hope) is that we are doing large URDF robot models rather than simpler OMPL mesh models

[v4hn]

is 50-60 links "large" in terms of ompl?

[mamoll]

@davetcoleman, by rigid body motion planning I meant planning in SE(2) and SE(3) and not articulated mechanisms.

@v4hn, yes, but not outrageous :-). We have used OMPL to model the flexibility of large macromolecules which have thousands of degrees of freedom. Typically, when the number of DOFs gets large, the number of constraints also increases. You probably need a special sampler and/or projection of the c-space to effectively explore a 50-60 dof configuration space.

[Levi-Armstrong]

@davetcoleman; I started implementing this into MoveIt and I don't think it would take much to finish but I would like to discuss one aspect of the implementation. For the FCL CCD you need to define the motion of the object between two transforms and since it is a robot this is not straight forward because one link motion can be affected by another links motion. FCL provides four types of motion (SplineMotion, ScrewMotion, InterpMotion, TranslationMotion). I think there is three option.

Option 1: Assume that the motion between two states is small and use the InterpMotion. This would be the simplest.

Option 2: Assume that the motion can be approximated by a 4 point spline and use the SplineMotion. The two intermediate points would be determined from joint interpolation between the two states. This would be a little more work than option 1.

Option 3: The path between two state is broken up into multiple intermediate states based on some criteria and use option 1 or 2. This would be similar to OMPL's longest_valid_segment parameter.

[davetcoleman]

I think we should use the option that most closely mimics how interpolation is already being done inside OMPL with MoveIt!. For joint-based planning, currently every edge is defined as the straight line interpolation of each joint between the two states. It might be more complicated for Cartesian planning, which is used in OMPL when you specify certain types of constraints, but I'm not sure off the top of my head.

At least for joint-based planning, this sounds like Option 1

[mamoll]

@davetcoleman, I am not sure how FCL is integrated in MoveIt!, but I imagine it performs CCD between every rigid body of the robot (i.e., every link) and the rest of the robot and environment. The SE(3) motion of each link when performing straight line interpolation in joint space will not be a straight line in SE(3) (nor a ScrewMotion).

All of the options still introduce some approximation error. Part of the appeal of CCD is hard guarantees that no collision is missed. With the approximations that guarantee is out the window. The other benefit could be a performance increase over discrete collision checking, but I am actually skeptical that this will be the case.

[arocchi]

Now that https://github.com/ros-planning/moveit/pull/1156 is merged it looks like this is now closer than ever to happening. @Levi-Armstrong do you still have this on your radar?

[davetcoleman]

I believe @Levi-Armstrong has been focusing https://github.com/ros-industrial-consortium/tesseract, but the MoveIt! maintainer team would really appreciate help bringing those changes back into moveit

[jcvtieck]

Hi,
what is the current status of the integration of FCL continuous collision checking?
is it still in development?
what is missing?

[davetcoleman]

@j-petit @mamoll ?

[j-petit]

There has no progress been so far on integrating CCD with FCL, however I integrated Bullet with CCD this summer which currently still lives on a feature-bullet-trajopt branch but should be merged into master soon.