Collision detection fails on planar meshes -- Continuous Collision Detection support

[Skylion007]

I hope on using DART for simulating the gaite of laminated robots. While DART solves the joint values almost perfectly, the mesh collision is really terrible. I suspect that some of these meshes are so thin that the algorithm that converts meshes to primitives are rounding down to 0 instead of creating a primitive of size less then say 1 or .1.

I have attached an image of the type of mesh taht would fall through the floor. While this can be solved easily by specifying primitives easily other shapes like the flowing cannot.

Is there any way I can improve the collision detection on DART? For reference, while ODE does an awful job with solving the joints, the collision detection works great. I really hope that you could improve the mesh collision detection as it's honestly probably the best simulator I have used in most other aspects.

[mxgrey]

DART essentially "outsources" its collision detection to other libraries, like FCL or Bullet. The default collision detector is FCL.

If you think a different collision detector might solve your problems, you could try using the dart::collision::BulletCollisionDetector. Just install Bullet, compile DART from source, and set the collision detector for your world:

world->getConstraintSolver()->setCollisionDetector(
          new dart::collision::BulletCollisionDetector());

Another possible solution I can think of: If part of the problem is that your floor is infinitesimally thin, then you could just use a box primitive with a bit of thickness instead of a plane for your floor. That way there's no way for a thin mesh to phase through the floor. There would still be the possibility that thin meshes will phase through other thin meshes, but at least slipping through the floor would be taken care of. Of course if your floor needs to be something other than a square plane, this would not help much.

If you think that ODE has particularly good collision detection, then we could consider offering an ODE collision detection option in addition to FCL and Bullet. We're in the process of overhauling our collision detection API, so now would be a good time to consider this.

[mxgrey]

It occurs to me that I don't know how much of DART's API is exposed by Gazebo, so I don't know if changing the collision detector used by DART is possible in Gazebo. I'll be sure to look into this sometime soon.

[jslee02]

In gazebo, DART uses thin primitive box shape with 0.01 thickness for the ground instead of plane shape, and converts it to box-shaped mesh for fcl because fcl has some issues on collision detection with primitive shapes including plane shape. Whereas BulletCollisionDetector of DART uses actual primitive shapes for bullet and it will probably show better behavior. But I guess the posted issue is caused due to the thin ground so I would like to suggest you to use more thick box size for the ground first.

Unfortunately, there is no way to change the thickness through gazebo. If you don't mind to build gazebo from the source, you could change the box size for ground in gazebo as:

        // line 49 of DARTPlaneShape.hh
        dart::dynamics::BoxShape *dtBoxShape =
            new dart::dynamics::BoxShape(Eigen::Vector3d(2100, 2100, 0.01));
        dtBodyNode->addCollisionShape(dtBoxShape);
        dtBoxShape->setOffset(Eigen::Vector3d(0.0, 0.0, -0.005));

        // new code
        const double thickness = 1.0;  // any value you want to try
        dart::dynamics::BoxShape *dtBoxShape =
            new dart::dynamics::BoxShape(Eigen::Vector3d(2100, 2100, thickness));
        dtBodyNode->addCollisionShape(dtBoxShape);
        dtBoxShape->setOffset(Eigen::Vector3d(0.0, 0.0, -0.5 * thickness));

You can use different collision detector but it additionally requires to build DART from the source. So please try with different box size, and let us know if it doesn't work.

As a note, I'm currently working on the primitive shape issues of fcl so that we can use plane shape for the ground and hopefully it would be included in the next release of fcl.

[jslee02]

Also, if you let me know the version of gazebo you are using and provide the sdf or urdf file, I can quickly test it.

[Skylion007]

Sure, I have created some thin primitives using poly line in this SDF file:

http://www.mediafire.com/download/mduq257fjlbcbyu/exported.world

I am using Gazebo 4.1.0

[Skylion007]

PS: While having a thicker floor does help, it still will often go through the floor. More importantly, the robot will phase through itself even when self collision is enabled.

[mxgrey]

Have you tried reducing the time step of the simulation? Scaling the time step based on the velocity of the fastest moving body might reduce how often objects tend to phase through each other.

Although truthfully, for objects that are extremely thin and parallel, there isn't much that can be done to avoid that phasing, except to use continuous collision detection, which DART doesn't currently support.

[Skylion007]

mxgrey, I have tried reduce the time step of the simulation and it has not helped. You say that DART does not support continuous collision detection (CCD)? But it does support Bullet's collision detection engine which does in turn have a CCD feature so would it not be easy to modify the code to support that? Additionally, we could try to bring in ODE's collision detection algorithm into DART which is very well documented. I am not that familiar with the code base so how easy would it to be implement either of those two features? I also do not think my project would afford me to those implement those features myself if they are difficult to implement and I might be forced to either look for a different physics engine or a work around.

[mxgrey]

@jslee02 would have a better idea of how difficult it would be to implement. I haven't had any involvement in DART's collision detection/handling development yet. We do have an issue posted about implementing an ODE collision detection extension #295 from about six months ago; it just hasn't been a priority yet.

Continuous collision detection is actually supported in Bullet, ODE, and FCL collision detectors, although I wouldn't know which one makes the "most effective" use of it, and I don't immediately know what changes we would need to make to interface with it.

I think the primary reason we haven't made use of CCD in DART yet is because CCD can be very expensive and slow for meshes, especially high resolution meshes, and DART was built with the expectation that most of the geometries that get used would be non-flat meshes for which CCD would be slow and unimportant.

That said, having CCD available as an option is obviously valuable and would be worthwhile to implement, but it's hard for me to guess when it could be finished. It wouldn't surprise me if it were very easy or if it were very difficult. I'll start looking over what kind of CCD interfaces are offered by FCL, Bullet, and ODE, and try to get an idea of how hard it would be to get working in DART. I should have a better idea of it by the end of the weekend.

[mxgrey]

After looking things over, I think integrating continuous collision detection into the simulation framework will take a considerable amount of development and testing, and I don't think it would be good to rush that, especially since there are some considerable upcoming changes to the way DART handles geometry. So unless someone is eager to volunteer to hack together a working prototype within the current API, I wouldn't anticipate having continuous collision detection in DART simulations for at least a couple months.

If you can find another simulator that meets your needs better than DART, then I would probably have to recommend going with that. One thing for you to consider is that DART is a toolkit in addition to a simulation framework, so you may still be able to take advantage of some of its functionality while using another simulator, if DART offers some features besides simulation that you still find useful (for example, computing mass matrices, Coriolis, gravity terms, etc).

In the meantime, we can keep this issue open and post updates here when we do start to get CCD implemented in DART.

[Skylion007]

This is how thick I need to make the models before they do not pass through the floor. It makes me wonder if the issue really is continuous collision detection or perhaps some issue with the algorithms that create collision primitives from the meshes. (Note, the thickness of the floor has apparently no effect, this is the required thickness whether the floor is a plane or whether the floor is a block 1000m thick.)

[Skylion007]

@jslee02 Where you ever able to determine if any of Dart's supported collision engines solved this issue?

[mxgrey]

(Note, the thickness of the floor has apparently no effect, this is the required thickness whether the floor is a plane or whether the floor is a block 1000m thick.)

Unfortunately the thickness of the floor is expected to be irrelevant with the default FCL collision detector, because as @jslee02 mentioned earlier:

In gazebo, DART uses thin primitive box shape with 0.01 thickness for the ground instead of plane shape, and converts it to box-shaped mesh for fcl because fcl has some issues on collision detection with primitive shapes including plane shape.

This problem might be avoidable using the Bullet collision detector.

But tunnelling is a well known problem for discrete collision detection on thin geometries. In this scenario, the thickness of your model needs to be greater than double the total distance that would be travelled in a single time step. Since DART uses leapfrog integration, if your object is starting from rest and sitting perfectly on top of the ground plane, the necessary thickness would be 2 * a * t^2 where a is acceleration due to gravity and t is the size of the time step. The default settings in DART are a = 9.8 and t = 1e-3, so your shape thickness would need to be roughly 2e-5 thick, but I don’t know what the default time step is for Gazebo, so the necessary thickness might be different for you.

If you’re finding that your object is able to tunnel through the floor even if its thickness is greater than double the distance it travels in the time step where it passes through the floor, then you may very well be correct that the collision geometries are somehow not being constructed correctly.

[Skylion007]

Hmm, interesting the thickness of my material that is tunnelling through the material is about 5e-5 thick, which is very close to the limit but just above it. Yet it was still tunnelling through it. However, the material may sometime spawn as high as 1m above the ground, so you are saying I need to have the material start at rest on the ground plane? I'll try that, but that means it will clip if it goes at any significant velocity through the ground plane, which will make it impossible to say have the robot actually walk on the plane. With these parameters, what is the maximum speed I need to set the simulation to?

Update: starting at pose 0 0 0 seems to work.

But what is the maximum speed the robot can travel without phasing through the mesh then?

[mxgrey]

Right, the necessary thickness estimate I gave you is only meant for debugging purposes. If the object has any initial velocity, it will need to be considerably thicker.

which will make it impossible to say have the robot actually walk on the plane. ... With these parameters, what is the maximum speed I need to set the simulation to?

I'm not 100% clear on what walking looks like for a laminated robot, so it's hard for me to guess what kind of time step you would need, or if it's even feasible to reduce the time step enough. The trouble with making the time step arbitrary small is that the time required for the simulation to run becomes intractable. I feel pretty certain that laminated robots would qualify as an edge use case for which discrete collision detection is not sufficient. I think the only way you'll get a simulation that is fast enough to be useful is if the simulator can do continuous collision handling.

[Skylion007]

Alas, I wouldn't be so stubborn on this issue if I could find any other physics engine (except for Simbody) used by Gazebo that solves joints with the same level of accuracy. If I use ODE, I can unwanted behavior like so.

[mxgrey]

Yeah, it's completely understandable. ODE is known to have some pretty serious stability issues which DART avoids entirely by using minimal coordinates.

I don't know what kind of collision handling Simbody offers, but I'm not seeing anything about them supporting continuous collision detection.

CCD would definitely be beneficial for DART in general, not just for your use case, but unfortunately we're ... let's say understaffed ... compared to the number of features that we have on our todo list.

[Skylion007]

Thank you for all your help. It's just so excruciating that most of Dart's collision engine support CCD and it's not implemented. According to the bullet wiki (linked above), one should be able to support CCD (with Bullet collision's engine) with just two additional lines of code per object.

  bodyNP.node().setCcdMotionThreshold(1e-7) 
  bodyNP.node().setCcdSweptSphereRadius(0.50)

I'll just patiently wait for the feature to be implemented and try to hack and work my way around the problem. Limiting the simulation speed to below a few meters per second is not a viable long term solution for my simulation, but I can try to hack my way around the problem until then. Thank you so much for all your help in this matter though.

[mxgrey]

A big part of the challenge is that DART's simulation currently uses fixed step sizes, which means toggling CCD on for the collision detectors is not sufficient, because DART will also need to decompose the fixed step sizes based on what time the collisions actually occur.

It might be possible to quickly hack together the necessary changes, and I'd gladly entertain a pull request from anyone who wants to give it a try, but we already have changes planned that would impact the API for CCD, so I'd rather not spend my time sloppily kludging it together when it would just end up being overhauled in the very near future.

[mxgrey]

Although I should point out: According to @jslee02 , DART's Bullet collision detector does support primitives correctly, unlike the default FCL collision detector. If you can switch DART to use the Bullet collision detector within Gazebo, then you will almost certainly be able to avoid the issue of tunnelling through the ground. But you will still most likely have tunnelling issues with self-collisions.

[stale[bot]]

This issue has been automatically marked as stale because it has not had recent activity. It will be closed in 7 days if no further activity occurs. Thank you for your contributions.