Collision and Contact Detection

A key component of the GraspIt! engine is the collision and contact detection. When collisions are enabled, anytime you interact with a GraspIt! world, either by moving an object or using the joint draggers on a robot, the engine will prevent any movement that brings two objects in collision and stop it at the moment of contact. Points of contact are then marked with contact indicators.

Important note: although GraspIt! (through the Inventor scene graph) can display all of these geometry types, the collision detection system only works with triangles. When a body geometry is loaded, GraspIt! will use the Inventor scene graph to facet it into triangles, which are then used to build the body model for the collision detection system.

In GraspIt! there is a very important difference between contact and collision. We define collision as two bodies that are interpenetrating, no matter by how much. In general, most algorithms in GraspIt! consider collision to be an illegal state and will attempt to find collision-free states. Contact is defined as two bodies that are closer together than a given threshold, but are not interpenetrating. In general, this threshold is set to be 0.1mm. If you would like to change this threshold, you will have to go inside the code: it is the THRESHOLD static member of the Contact class.

You can also enable or disable collisions, for either the entire simulation world, one particular body, or a pair of bodies. This is all done using the Toggle Collision switch in the main GraspIt! interface. Depending on what is selected when you press the button, the following will happen:

  • if nothing is selected, collisions are toggled for the entire simulation world
  • if a single body is selected, collisions are toggled for that particular body (it can / can not collide with any other body in the world)
  • if a pair of bodies is selected, collisions are toggled for that particular pair (they can / can not collide with each other; collisions with all the other bodies in the world are unaffected).

Most GraspIt! functionality that involves moving bodies around (including user interaction with joint draggers) works as follows:

  • move the bodies freely as long as there is no collision;
  • when collision is detected, attempt to interpolate between the collision state and the last known collision-free state until the bodies are no longer colliding, but are separated by less than the contact threshold;
  • find all points where two bodies are separated by less than the contact threshold and mark them as contacts.

Contacts

In GraspIt! a contact is defined as any point where two bodies are separated by less than the contact threshold, but not interpenetrating. The collision detection engine will find these points for you, and also do some pruning, as explained below. A GraspIt! contact, defined in the Contact class, encapsulates the following information:

  • the location of the contact on each of the two bodies (the points on the two bodies that are separated by less than the contact threshold)
  • the contact normal (defined as the normalized vector between the two points mentioned above)
  • the Contact Wrench Space: the space of forces and torques that can be applied at the contact. This is a crucial concept, which is at the base of most grasp quality computations and dynamic simulations. In the simplest case, that of Coulomb friction, this space is a 3D cone. See the section of this document for more details.
  • a visual marker showing the location of the contact in the GraspIt! GUI. In the case of rigid body contacts, this is actually a rendering of the friction cone, aligned with the contact normal.

Note that everywhere in GraspIt! we refer to contacts as points. The reason is that geometry in GraspIt! never deforms, so we can never explicitly compute areas of contact. There are two important aspects though. First, two rigid bodies might be locally similar, so that more than one point is within the contact threshold. If that happens, the collision detection just returns many point contacts in a small area. All these point contacts are then pruned, keeping only those contacts that are on the contour of the area (the boundary of the convex hull of contact locations). According to the theory on contacts that we rely on, this will have no net effect on any grasp quality computations, but will reduce computation time by reducing the number of contacts).

Second, GraspIt! can simulate some of the effects of soft bodies in contact without explicitly computing the deformation. This is done by using a different version of the Contact Wrench Space. See the chapter of this manual for details.

Software implementation

For many algorithms, the collision engine is the computational bottleneck. It is very important to have an efficient engine, but we also require this engine to be very robust and work well on triangle meshes (as opposed to analytical primitives). The GPL version of GraspIt! comes with its own collision detection implementation, using a number of common bounding box hierarchy methods. However, fast collision detection is a research area in itself, and we are sure there are many ways to improve our implementation. Any bug reports, patches or optimizations for the collision detection engine are highly appreciated.

From a software perspective, we have built the collision and contact detection libraries to be as modular as possible. This allows the complete replacement of our current collision detection with the library of your choice. If you are interested in doing this, check out the CollisionInterface class and its subclasses. If you know of a good collision detection library that is fast and robust, works with triangle meshes, and has a GPL-compatible license, we would love to hear about it.

Under development: Multi-threading

We have also implemented a crude multi-threaded support for the collision detection mechanism. This is still very much work in progress, both from a design and implementation standpoint. The overall concept is as follows: if you have multiple threads in your GraspIt! code, each thread can add its own bodies to the simulation world. The rule we have implemented is that bodies from different threads never collide with each other. The only exception is that all bodies collide with bodies from the main thread.

The reason for this implementation is to allow you to test multiple scenarios in parallel, without worrying about collision detection. As an example, suppose you have a hand and a glass sitting on a table, and you want to evaluate many grasps quickly. In this case, you would populate the world with the table and the glass in the main thread. Then you would fire up many threads, each loading its own copy of the hand. Each thread can then tests its own grasps independently, without needing to synchronize with the other threads or worrying about colliding with the hands from other threads. Of course, if your code is single-threaded, you can just ignore all of this and pretend it does not exist.

The general steps for GraspIt! multi-threading are:

  • in the main thread, load all the objects that will be shared between threads
  • fire up all of your computation threads. In each thread, inform the collision detection mechanism that it is now servicing a new thread (see the CollisionInterface class for what method to call for this)
  • in each thread, load all the objects or robots that are specific to that thread
  • in each thread, run any computation like you normally would.

For examples, see the EGPlanner class which has support for running in its own thread.

Under development: Object cloning

We also have a mechanism for “cloning” objects in GraspIt! so you can easily create multiple copies of a body or robot without having to load it from a file multiple times and without wasting memory. As far as any GraspIt! algorithm is concerned, a clone is a totally independent body with its own position in the world, contacts, etc. However, under the hood, a clone will share all the scene graph geometry and collision detection bounding box hierarchy with the original. See the Body::cloneFrom(...) and Robot::cloneFrom(...) methods for details. This mechanism works well, with one exception: we never implemented a nice cleanup phase that handles the case where the original is deleted and the clone still lingers in the world. This situation is almost guaranteed to cause a crash.

In practice, cloning and multi-threading often go together. If you have lots of computation that you might parallelize, you can create clones of your moving objects (usually the robots and hands) and pass them to different threads, where each thread will do its own work. See again the EGPlanner class for details - it can run in its own thread using a cloned hand for computations.