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.