Open Dynamics Engine v0.025 User Guide
Russell Smith, Saturday 14 July, 2001

This document is Copyright © 2001 Russell Smith.
This is an UNFINISHED document, there is still a lot of stuff to write.

Contents


1. Introduction

The Open Dynamics Engine (ODE) is a free, industrial quality library for simulating articulated rigid body dynamics. For example, it is good for simulating ground vehicles, legged creatures, and moving objects in VR environments. It is fast, flexible and robust, and it has built-in collision detection. ODE is being developed by Russell Smith.

If ``rigid body simulation'' does not make much sense to you, check out What is a Physics SDK?.

The is the user guide for the third prototype release of ODE (version 0.025). This release is to help continue testing the core algorithms and solidifying the API. There will probably only be a few (hopefully minor) changes to the API in the future.

1.1. Current features

ODE is best for simulating articulated rigid body structures. An articulated structure is created when rigid bodies of various shapes are connected together with joints of various kinds. Examples are ground vehicles (where the wheels are connected to the chassis) or legged creatures (where the legs are connected to the body).

ODE is designed to be used in interactive or real-time simulation. It is particularly good for simulating moving objects in changeable virtual reality environments. This is because it is fast, robust and stable, and the user has complete freedom to change the structure of the system even while the simulation is running.

ODE uses a highly stable integrator, so that the simulation errors should not grow out of control. The physical meaning of this is that the simulated system should not "explode" for no reason (believe me, this happens a lot with other simulators if you are not careful). ODE emphasizes speed and stability over physical accuracy.

ODE has hard contacts. This means that a special non-penetration constraint is used whenever two bodies collide. The alternative, used in many other simulators, is to use virtual springs to represent contacts. This is difficult to do right and extremely error-prone.

ODE has a built-in collision detection system. However you can ignore it and do your own collision detection if you want to. The current collision primitives are sphere, box and plane - more collision objects will come later, as well as fast identification of potentially intersecting objects.

Here are the features:

One current problem is that the mu parameter that controls Coulomb friction is not yet interpreted correctly - it specifies the maximum friction force that can be present at a contact, rather than specifying the maximum ratio of tangential to normal force. This will be fixed in the next release. [Note: I need lots more documentation about how the friction approximation works].

1.2. New in the 0.025 release

1.3. New in the 0.02 release

1.4. Future features

1.4.1. Plan for 0.03 release

1.4.2. Plan for 0.04 release

1.4.3. Other stuff for later releases

1.5. ODE's license

ODE is Copyright © 2001 Russell Smith.

This library is free software; you can redistribute it and/or modify it under the terms of the GNU Lesser General Public License as published by the Free Software Foundation; either version 2.1 of the License, or (at your option) any later version.

This library is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License for more details.

1.6. Call for volunteers

Think you can help? Please write to me.


2. Concepts

2.1. Background

[Here is where I will write some background information about rigid body dynamics and simulation. But in the meantime, please refer to Baraff's excellent SIGGRAPH tutorial].

2.2. Rigid bodies

A rigid body has various properties from the point of view of the simulation. Some properties change over time:

Other body properties are usually constant over time: Conceptually each body has an x-y-z coordinate frame embedded in it, that moves and rotates with the body:

The origin of this coordinate frame is the body's point of reference. Some values in ODE (vectors, matrices etc) are relative to the body coordinate frame, and others are relative to the global coordinate frame.

Note that the shape of a rigid body is not a dynamical properties (except insofar as it influences the various mass properties). It is only collision detection that cares about the detailed shape of the body.

2.3. Integration

The process of simulating the rigid body system through time is called integration. Each integration step advances the current time by a given step size, adjusting the state of all the rigid bodies for the new time value. There are two main issues to consider when working with any integrator:

ODE's current integrator is very stable, but not particularly accurate unless the step size is small. For most uses of ODE this is not a problem -- ODE's behavior still looks perfectly physical in almost all cases. However ODE should not be used for quantitative engineering until this accuracy issue has been addressed in a future release.

2.4. Force accumulators

Between each integrator step the user can call functions to apply forces to the rigid body. These forces are added to "force accumulators" in the rigid body object. When the next integrator step happens, the sum of all the applied forces will be used to push the body around. The forces accumulators are set to zero after each integrator step.

2.5. Joints and constraints

In real life a joint is something like a hinge, that is used to connect two objects. In ODE a joint is very similar: It is a relationship that is enforced between two bodies so that they can only have certain positions and orientations relative to each other. This relationship is called a constraint -- the words joint and constraint are often used interchangeably. The following picture shows three different constraint types:

The first is a ball and socket joint that constraints the ``ball'' of one body to be in the same location as the ``socket'' of another body. The second is a hinge joint that constraints the two parts of the hinge to be in the same location and to line up along the hinge axle. The third is a slider joint that constraints the ``piston'' and ``socket'' to line up, and additionally constraints the two bodies to have the same orientation.

Each time the integrator takes a step all the joints are allowed to apply constraint forces to the bodies they affect. These forces are calculated such that the bodies move in such a way to preserve all the joint relationships.

Each joint has a number of parameters controlling its geometry. An example is the position of the ball-and-socket point for a ball-and-socket joint. The functions to set joint parameters all take global coordinates, not body-relative coordinates. A consequence of this is that the rigid bodies that a joint connects must be positioned correctly before the joint is attached.

2.6. Joint groups

A joint group is a special container that holds joints in a world. Joints can be added to a group, and then when those joints are no longer needed the entire group of joints can be very quickly destroyed with one function call. However, individual joints in a group can not be destroyed before the entire group is emptied.

This is most useful with contact joints, which are added and remove from the world in groups every time step.

2.7. Joint error and the error reduction parameter

When a joint attaches two bodies, those bodies are required to have certain positions and orientations relative to each other. However, it is possible for the bodies to be in positions where the joint constraints are not met. This ``joint error'' can happen in two ways:

  1. If the user sets the position/orientation of one body without correctly setting the position/orientation of the other body.
  2. During the simulation, errors can creep in that result in the bodies drifting away from their required positions.
Here is an example of error in a ball and socket joint (where the ball and socket do not line up):

There is a mechanism to reduce joint error: during each simulation step each joint applies a special force to bring its bodies back into correct alignment. This force is controlled by the error reduction parameter (ERP), which has a value between 0 and 1.

The ERP specifies what proportion of the joint error will be fixed during the next simulation step. If ERP=0 then no correcting force is applied and the bodies will eventually drift apart as the simulation proceeds. If ERP=1 then the simulation will attempt to fix all joint error during the next time step. However, setting ERP=1 is not recommended, as the joint error will not be completely fixed due to various internal approximations. A value of ERP=0.1 to 0.8 is recommended (0.2 is the default).

A global ERP value can be set that affects most joints in the simulation. However some joints have local ERP values that control various aspects of the joint.

2.8. Constraint force mixing (CFM)

Traditionally the constraint equation for every joint has the form

J * v = c

where v is a velocity vector for the bodies involved, J is a ``Jacobian'' matrix with one row for every degree of freedom the joint removes from the system, and c is a right hand side vector. At the next time step, a vector lambda is calculated (of the same size as c) such that the forces applied to the bodies to preserve the joint constraint are

force = JT * lambda

ODE adds a new twist. ODE's constraint equation has the form

J * v = c + CFM * lambda

where CFM is a square diagonal matrix. CFM mixes the resulting constraint force in with the constraint that produces it. By using CFM you can model tire slip, suspension, soft ground and other things.

Solving for lambda gives

(J M-1 JT + CFM/h) lambda = c/h

Thus CFM simply adds to the diagonal of the original system matrix. This has the additional benefit of taking the system away from singularity and improving factorizer accuracy.

[write more...]

2.9. Collision handling

[There is a lot that needs to be written about collision handling.]

Collisions between bodies or between bodies and the static environment are handled as follows:

  1. Before each simulation step, the user calls collision detection functions to determine what is touching what. These functions return a list of contact points. Each contact point specifies a position in space, a surface normal vector, and a penetration depth.

  2. A special contact joint is created for each contact point. The contact joint is given extra information about the contact, for example the friction present at the contact surface, how bouncy or soft it is, and various other properties.

  3. The contact joints are put in a joint "group", which allows them to be added to and removed from the system very quickly. The simulation will speed goes down as the number of contacts goes up, so various strategies can be used to limit the number of contact points.

  4. A simulation step is taken.

  5. All contact joints are removed from the system.
Note that the built-in collision functions do not have to be used - other collision detection libraries can be used as long as they provide the right kinds of contact point information.

2.10. Typical simulation code

A typical simulation will proceed like this:

  1. Create a dynamics world.
  2. Create bodies in the dynamics world.
  3. Set the state (position etc) of all bodies.
  4. Create joints in the dynamics world.
  5. Attach the joints to the bodies.
  6. Set the parameters of all joints.
  7. Create a collision world and collision geometry objects, as necessary.
  8. Create a joint group to hold the contact joints.
  9. Loop:
    1. Apply forces to the bodies as necessary.
    2. Adjust the joint parameters as necessary.
    3. Call collision detection.
    4. Create a contact joint for every collision point, and put it in the contact joint group.
    5. Take a simulation step.
    6. Remove all joints in the contact joint group.
  10. Destroy the dynamics and collision worlds.

2.11. Physics model

[Discuss the various methods and approximations that are used.]


3. Data types and conventions

3.1. The basic data types

The ODE library can be built to use either single or double precision floating point numbers. Single precision is faster and uses less memory, but the simulation will have more numerical error that can result in visible problems. You will get less accuracy and stability with single precision.

[must describe what factors influence accuracy and stability].

The floating point data type is dReal. Other commonly used types are dVector3, dVector4, dMatrix3, dMatrix4, dQuaternion.

3.2. Objects and IDs

There are various kinds of object that can be created:

Functions that deal with these objects take and return object IDs. The object ID types are dWorldID, dBodyID, etc.

3.3. Argument conventions

All 3-vectors (x,y,z) supplied to ``set'' functions are given as individual x,y,z arguments.

All 3-vector result arguments to get() function are pointers to arrays of dReal.

Larger vectors are always supplied and returned as pointers to arrays of dReal.

All coordinates are in the global frame except where otherwise specified.

3.4. C versus C++

The ODE library is written in C++, but its public interface is made of simple C functions, not classes. Why is this?

3.5. Debugging

The ODE library can be compiled in "debugging" or "release" mode. Debugging mode is slower, but function arguments are checked and many compile-time tests are done to ensure internal consistency. Release mode is faster, but no checking is done.


4. World

The world object is a container for rigid bodies and joints.


dWorldID dWorldCreate();
Create a new, empty world and return its ID number.


void dWorldDestroy (dWorldID);
Destroy a world and everything in it.


void dWorldSetGravity (dWorldID, dReal x, dReal y, dReal z);
void dWorldGetGravity (dWorldID, dVector3 gravity);
Set and get the world's global gravity vector. The units are m/s/s, so Earth's gravity vector would be (0,0,-9.81), assuming that +z is up. The default is no gravity, i.e. (0,0,0).


void dWorldSetERP (dWorldID, dReal erp);
dReal dWorldGetERP (dWorldID);
Set and get the global ERP value, that controls how much error correction is performed in each time step. Typical values are in the range 0.1--0.8. The default is 0.2.


void dWorldSetCFM (dWorldID, dReal cfm);
dReal dWorldGetCFM (dWorldID);
Set and get the global CFM (constraint force mixing) value. Typical values are in the range 1e-9 -- 1. The default is 1e-5 if single precision is being used, or 1e-10 if double precision is being used.


void dWorldStep (dWorldID, dReal stepsize);
Step the world.


5. Rigid body functions


dBodyID dBodyCreate (dWorldID);
Create a body in the given world with default mass parameters at position (0,0,0). Return its ID.


void dBodyDestroy (dBodyID);
Destroy a body. All joints that are attached to this body will be put into limbo (i.e. unattached and not affecting the simulation, but they will NOT be deleted).


void  dBodySetData (dBodyID, void *data);
void *dBodyGetData (dBodyID);
Get and set the body's user-data pointer.


void dBodySetPosition   (dBodyID, dReal x, dReal y, dReal z);
void dBodySetRotation   (dBodyID, const dMatrix3 R);
void dBodySetQuaternion (dBodyID, const dQuaternion q);
void dBodySetLinearVel  (dBodyID, dReal x, dReal y, dReal z);
void dBodySetAngularVel (dBodyID, dReal x, dReal y, dReal z);
const dReal * dBodyGetPosition   (dBodyID);
const dReal * dBodyGetRotation   (dBodyID);
const dReal * dBodyGetQuaternion (dBodyID);
const dReal * dBodyGetLinearVel  (dBodyID);
const dReal * dBodyGetAngularVel (dBodyID);
These functions set and get the position, rotation, linear and angular velocity of the body. After setting a group of bodies, the outcome of the simulation is undefined if the new configuration is inconsistent with the joints/constraints that are present. When getting, the returned values are pointers to internal data structures, so the vectors are valid until any changes are made to the rigid body system structure.

Hmmm. dBodyGetRotation returns a 4x3 rotation matrix.


void dBodySetMass (dBodyID, const dMass *mass);
void dBodyGetMass (dBodyID, dMass *mass);
Set/get the mass of the body (see the mass functions).


void dBodyAddForce            (dBodyID, dReal fx, dReal fy, dReal fz);
void dBodyAddTorque           (dBodyID, dReal fx, dReal fy, dReal fz);
void dBodyAddRelForce         (dBodyID, dReal fx, dReal fy, dReal fz);
void dBodyAddRelTorque        (dBodyID, dReal fx, dReal fy, dReal fz);
void dBodyAddForceAtPos       (dBodyID, dReal fx, dReal fy, dReal fz,
                                        dReal px, dReal py, dReal pz);
void dBodyAddRelForceAtPos    (dBodyID, dReal fx, dReal fy, dReal fz,
                                        dReal px, dReal py, dReal pz);
void dBodyAddRelForceAtRelPos (dBodyID, dReal fx, dReal fy, dReal fz,
                                        dReal px, dReal py, dReal pz);
Add forces to bodies (absolute or relative coordinates). The forces are accumulated on to each body, and the accumulators are zeroed after each time step.


void dBodyGetPointPos    (dBodyID, dReal px, dReal py, dReal pz,
                          dVector3 result);
void dBodyGetPointVel    (dBodyID, dReal px, dReal py, dReal pz,
                          dVector3 result);
void dBodyGetPointRelVel (dBodyID, dReal px, dReal py, dReal pz,
                          dVector3 result);
Utility functions.


6. Joint types and joint functions

6.1. Creating and destroying


dJointID dJointCreateBall (dWorldID, dJointGroupID);
dJointID dJointCreateHinge (dWorldID, dJointGroupID);
dJointID dJointCreateSlider (dWorldID, dJointGroupID);
dJointID dJointCreateContact (dWorldID, dJointGroupID,
                              const dContact *);
dJointID dJointCreateHinge2 (dWorldID, dJointGroupID);
dJointID dJointCreateFixed (dWorldID, dJointGroupID);
Create a new joint of a given type. The joint is initially in "limbo" (i.e. it has no effect on the simulation) because it does not connect to any bodies. The joint group ID is 0 to allocate the joint normally. If it is nonzero the joint is allocated in the given joint group. The contact joint will be initialized with the given dContact structure.


void dJointDestroy (dJointID);
Destroy a joint, disconnecting it from its attached bodies and removing it from the world. However, if the joint is a member of a group then this function has no effect - to destroy that joint the group must be emptied or destroyed.


dJointGroupID dJointGroupCreate (int max_size);
Create a joint group. max_size is in bytes.


void dJointGroupDestroy (dJointGroupID);
Destroy a joint group. All joints in the joint group will be destroyed.


void dJointGroupEmpty (dJointGroupID);
Empty a joint group. All joints in the joint group will be destroyed, but the joint group itself will not be destroyed.

6.2. Miscellaneous


void dJointAttach (dJointID, dBodyID body1, dBodyID body2);
Attach the joint to some new bodies. If the joint is already attached, it will be detached from the old bodies first. To attach this joint to only one body, set body1 or body2 to zero - a zero body refers to the static environment. Setting both bodies to zero puts the joint into "limbo", i.e. it will have no effect on the simulation.

Some joints, like hinge-2 need to be attached to two bodies to work.


void dJointSetData (dJointID, void *data);
void *dJointGetData (dJointID);
Get and set the joint's user-data pointer.


int dAreConnected (dBodyID, dBodyID);
Utility function: return 1 if the two bodies are connected together by a joint, otherwise return 0.

6.3. Joint parameter setting functions

6.3.1. Ball and socket

A ball and socket joint looks like this:


void dJointSetBallAnchor (dJointID, dReal x, dReal y, dReal z);
Set the joint anchor point.


void dJointGetBallAnchor (dJointID, dVector3 result);
Get the joint anchor point.

6.3.2. Hinge

A hinge joint looks like this:


void dJointSetHingeAnchor (dJointID, dReal x, dReal y, dReal z);
void dJointSetHingeAxis (dJointID, dReal x, dReal y, dReal z);
Set hinge anchor and axis parameters.


void dJointGetHingeAnchor (dJointID, dVector3 result);
void dJointGetHingeAxis (dJointID, dVector3 result);
Get hinge anchor and axis parameters.


dReal dJointGetHingeAngle (dJointID);
dReal dJointGetHingeAngleRate (dJointID);
Get the hinge angle and the time derivative of this value. The angle is measured between the two bodies, or between the body and the static environment. The angle will be between -pi..pi.

When the hinge anchor or axis is set, the current position of the attached bodies is examined and that position will be the zero angle.

6.3.3. Slider

A slider joint looks like this:


void dJointSetSliderAxis (dJointID, dReal x, dReal y, dReal z);
Set the slider axis parameter.


void dJointGetSliderAxis (dJointID, dVector3 result);
Get the slider axis parameter.


dReal dJointGetSliderPosition (dJointID);
dReal dJointGetSliderPositionRate (dJointID);
Get the slider linear position (i.e. the slider's ``extension'') and the time derivative of this value.

When the axis is set, the current position of the attached bodies is examined and that position will be the zero position.

6.3.4. Hinge-2

A hinge-2 joint looks like this:

The hinge-2 joint is the same as two hinges connected in series, with different hinge axes. An example, shown in the above picture is the steering wheel of a car, where one axis allows the wheel to be steered and the other axis allows the wheel to rotate.

The hinge-2 joint has an anchor point and two hinge axes. Axis 1 is specified relative to body 1 (this would be the steering axis if body 1 is the chassis). Axis 2 is specified relative to body 2 (this would be the wheel axis if body 2 is the wheel).

Axis 1 can have joint limits and a motor, axis 2 can only have a motor.

Axis 1 can function as a suspension axis, i.e. the constraint can be compressible along that axis.


void dJointSetHinge2Anchor (dJointID, dReal x, dReal y, dReal z);
void dJointSetHinge2Axis1 (dJointID, dReal x, dReal y, dReal z);
void dJointSetHinge2Axis2 (dJointID, dReal x, dReal y, dReal z);
Set hinge-2 anchor and axis parameters. Axis 1 and axis 2 must not lie along the same line.


void dJointGetHinge2Anchor (dJointID, dVector3 result);
void dJointGetHinge2Axis1 (dJointID, dVector3 result);
void dJointGetHinge2Axis2 (dJointID, dVector3 result);
Get hinge-2 anchor and axis parameters.


dReal dJointGetHinge2Angle1 (dJointID);
dReal dJointGetHinge2Angle1Rate (dJointID);
dReal dJointGetHinge2Angle2Rate (dJointID);
Get the hinge-2 angles (around axis 1 and axis 2) and the time derivatives of these values.

When the anchor or axis is set, the current position of the attached bodies is examined and that position will be the zero angles.

6.3.5. Fixed

The fixed joint maintains a fixed relative position and orientation between two bodies, or between a body and the static environment. Using this joint is almost never a good idea in practice, except when debugging. If you need two bodies to be glued together it is better to represent that as a single body.

Currently the fixed joint does not support a non-identity relative rotation between two bodies, it only supports a relative offset.


void dJointSetFixed (dJointID);
Call this on the fixed joint after it has been attached to remember the current desired relative offset between the bodies.

6.3.6. Contact

A contact joint looks like this:

The contact joint prevents body 1 and body 2 from inter-penetrating at the contact point. It does this by only allowing the bodies to have an ``outgoing'' velocity in the direction of the contact normal. Contact joints typically have a lifetime of one time step. They are created and deleted in response to collision detection.

Contact joints can simulate friction at the contact by applying special forces in the two friction directions that are perpendicular to the normal.

When a contact joint is created, a dContact structure must be supplied. This has the following definition:
struct dContact {
  dSurfaceParameters surface;
  dContactGeom geom;
  dVector3 fdir1;
};
geom is a substructure that is set by the collision functions. It is described in the collision section.

fdir1 is a "first friction direction" vector that defines a direction along which frictional force is applied. It must be of unit length and perpendicular to the contact normal (so it is typically tangential to the contact surface). It should only be defined if the dContactFDir1 flag is set in surface.mode. The "second friction direction" is a vector computed to be perpendicular to both the contact normal and fdir1.

surface is a substructure that is set by the user. Its members define the properties of the colliding surfaces. It has the following members:

6.4. General

The joint geometry parameter setting functions should only be called after the joint has been attached to bodies, and those bodies have been correctly positioned, otherwise the joint may not be initialized correctly. If the joint is not already attached, these functions will do nothing.

For the parameter getting functions, if the system is out of alignment (i.e. there is some joint error) then the anchor/axis values will be correct with respect to body 1 only (or body 2 if you specified body 1 as zero in the dJointAttach() function).

The default anchor for all joints is (0,0,0). The default axis for all joints is (1,0,0).

When an axis is set it will be normalized to unit length. The adjusted axis is what the axis getting functions will return.

When measuring a joint angle or position, a value of zero corresponds to the initial position of the bodies relative to each other.

Note that there are no functions to set joint angles or positions (or their rates) directly, instead you must set the corresponding body positions and velocities.

6.5. Stop and motor parameters

When a joint is first created there is nothing to prevent it from moving through its entire range of motion. For example a hinge will be able to move through its entire angle, and a slider will slide to any length.

This range of motion can be limited by setting stops on the joint. The joint angle (or position) will be prevented from going below the low stop value, or from going above the high stop value. Note that a joint angle (or position) of zero corresponds to the initial body positions.

As well as stops, many joint types can have motors. A motor applies a torque (or force) to a joint's degree(s) of freedom to get it to pivot (or slide) at a desired speed. Motors have force limits, which means they can apply no more than a given maximum force/torque to the joint.

Motors have two parameters: a desired speed, and the maximum force that is available to reach that speed. This is a very simple model of real life motors, engines or servos. However, is it quite useful when modeling a motor (or engine or servo) that is geared down with a gearbox before being connected to the joint. Such devices are often controlled by setting a desired speed, and can only generate a maximum amount of power to achieve that speed (which corresponds to a certain amount of force available at the joint).

Motors can also be used to accurately model dry (or Coulomb) friction in joints. Simply set the desired velocity to zero and set the maximum force to some constant value - then all joint motion will be impeded by that force.

The alternative to using joint stops and motors is to simply apply forces to the affected bodies yourself. Applying motor forces is easy, and joint stops can be emulated with restraining spring forces. However applying forces directly is often not a good approach and can lead to severe stability problems if it is not done carefully.

Consider the case of applying a force to a body to achieve a desired velocity. To calculate this force you use information about the current velocity, something like this:

force = k * (desired speed - current speed)

This has several problems. First, the parameter k must be tuned by hand. If it is too low the body will take a long time to come up to speed. If it is too high the simulation will become unstable. Second, even if k is chosen well the body will still take a few time steps to come up to speed. Third, if any other ``external'' forces are being applied to the body, the desired velocity may never even be reached (a more complicated force equation would be needed, which would have extra parameters and its own problems).

Joint motors solve all these problems: they bring the body up to speed in one time step, provided that does not take more force than is allowed. Joint motors need no extra parameters because they are actually implemented as constraints. They can effectively see one time step into the future to work out the correct force. This makes joint motors more computationally expensive than computing the forces yourself, but they are much more robust and stable, and far less time consuming to design with. This is especially true with larger rigid body systems.

Similar arguments apply to joint stops.

6.5.1. Parameter functions

Here are the functions that set stop and motor parameters (as well as other kinds of parameters) on a joint:


void dJointSetHingeParam (dJointID, int parameter, dReal value);
void dJointSetSliderParam (dJointID, int parameter, dReal value);
void dJointSetHinge2Param (dJointID, int parameter, dReal value);
dReal dJointGetHingeParam (dJointID, int parameter);
dReal dJointGetSliderParam (dJointID, int parameter);
dReal dJointGetHinge2Param (dJointID, int parameter);
Set/get limit/motor parameters for each joint type. The parameter numbers are:

dParamLoStopLow stop angle or position. Setting this to -dInfinity (the default value) turns off the low stop. For rotational joints, this stop must be greater than -Pi to be effective.
dParamHiStopHigh stop angle or position. Setting this to dInfinity (the default value) turns off the high stop. For rotational joints, this stop must be less than Pi to be effective. If the high stop is less than the low stop then both stops will be ineffective.
dParamVelDesired motor velocity (this will be an angular or linear velocity).
dParamFMaxThe maximum force or torque that the motor will use to achieve the desired velocity. This must always be greater than or equal to zero. Setting this to zero (the default value) turns off the motor.
dParamFudgeFactorThe current joint stop/motor implementation has a small problem: when the joint is at one stop and the motor is set to move it away from the stop, too much force may be applied for one time step, causing a ``jumping'' motion. This fudge factor is used to scale this excess force. It should have a value between zero and one (the default value). If the jumping motion is too visible in a joint, the value can be reduced. Making this value too small can prevent the motor from being able to move the joint away from a stop.
dParamBounceThe bouncyness of the stops. This is a restitution parameter in the range 0..1. 0 means the stops are not bouncy at all, 1 means maximum bouncyness.
dParamStopERPThe error reduction parameter (ERP) used by the stops.
dParamStopCFMThe constraint force mixing (CFM) value used by the stops. Together with the ERP value this can be used to get spongy or soft stops. Note that this is intended for unpowered joints, it does not really work as expected when a powered joint reaches its limit.
dParamSuspensionERPSuspension error reduction parameter (ERP). Currently this is only implemented on the hinge-2 joint.
dParamSuspensionCFMSuspension constraint force mixing (CFM) value. Currently this is only implemented on the hinge-2 joint.

If a particular parameter is not implemented by a given joint, setting it will have no effect.

These parameter names can be optionally followed by a digit (currently only 2) to indicate the second set of parameters, e.g. for the second axis in a hinge-2 joint.


7. Support functions

7.1. Rotation functions

Rigid body orientations are represented with quaternions. A quaternion is four numbers [cos(theta/2) sin(theta/2)*u] where theta is a rotation angle and `u' is a unit length rotation axis.

Every rigid body also has a 3x3 rotation matrix that is derived from the quaternion. The rotation matrix and the quaternion always match.

Some information about quaternions:

The following are utility functions for dealing with rotation matrices and quaternions.


void dRSetIdentity (dMatrix3 R);
Set R to the identity matrix (i.e. no rotation).


void dRFromAxisAndAngle (dMatrix3 R,
                         dReal ax, dReal ay, dReal az, dReal angle);
Compute the rotation matrix R as a rotation of angle radians along the axis (ax,ay,az).


void dRFromEulerAngles (dMatrix3 R,
                        dReal phi, dReal theta, dReal psi);
Compute the rotation matrix R from the three Euler rotation angles.


void dRFrom2Axes (dMatrix3 R, dReal ax, dReal ay, dReal az,
                  dReal bx, dReal by, dReal bz);
Compute the rotation matrix R from the two vectors `a' (ax,ay,az) and `b' (bx,by,bz). `a' and `b' are the desired x and y axes of the rotated coordinate system. If necessary, `a' and `b' will be made unit length, and `b' will be projected so that it is perpendicular to `a'. The desired z axis is the cross product of `a' and `b'.


void dQSetIdentity (dQuaternion q);
Set q to the identity rotation (i.e. no rotation).


void dQFromAxisAndAngle (dQuaternion q, dReal ax, dReal ay, dReal az,
                         dReal angle);
Compute q as a rotation of angle radians along the axis (ax,ay,az).


void dQMultiply0 (dQuaternion qa,
                  const dQuaternion qb, const dQuaternion qc);
void dQMultiply1 (dQuaternion qa,
                  const dQuaternion qb, const dQuaternion qc);
void dQMultiply2 (dQuaternion qa,
                  const dQuaternion qb, const dQuaternion qc);
void dQMultiply3 (dQuaternion qa,
                  const dQuaternion qb, const dQuaternion qc);
Set qa = qb*qc. This is that same as qa = rotation qc followed by rotation qa. The 0/1/2 versions are analogous to the multiply functions, i.e. 1 uses the inverse of qb, and 2 uses the inverse of qc. Option 3 uses the inverse of both.


void dQtoR (const dQuaternion q, dMatrix3 R);
Convert quaternion q to rotation matrix R.


void dRtoQ (const dMatrix3 R, dQuaternion q);
Convert rotation matrix R to quaternion q.


void dWtoDQ (const dVector3 w, const dQuaternion q, dVector4 dq);
Given an existing orientation q and an angular velocity vector w, return in dq the resulting dq/dt.

7.2. Mass functions

The mass parameters of a rigid body are described by a dMass structure:
typedef struct dMass {
  dReal mass;   // total mass of the rigid body
  dVector4 c;   // center of gravity position in body frame (x,y,z)
  dMatrix3 I;   // 3x3 inertia tensor in body frame, about POR
} dMass;

The following functions operate on this structure:


void dMassSetZero (dMass *);
Set all the mass parameters to zero.


void dMassSetParameters (dMass *, dReal themass,
                         dReal cgx, dReal cgy, dReal cgz,
                         dReal I11, dReal I22, dReal I33,
                         dReal I12, dReal I13, dReal I23);
Set the mass parameters to the given values. themass is the mass of the body. (cx,cy,cz) is the center of gravity position in the body frame. The Ixx values are the elements of the inertia matrix:
    [ I11 I12 I13 ]
    [ I12 I22 I23 ]
    [ I13 I23 I33 ]


void dMassSetSphere (dMass *, dReal density, dReal radius);
Set the mass parameters to represent a sphere of the given radius and density, with the center of mass at (0,0,0) relative to the body.


void dMassSetCappedCylinder (dMass *, dReal density, int direction,
                             dReal a, dReal b);
Set the mass parameters to represent a capped cylinder of the given parameters and density, with the center of mass at (0,0,0) relative to the body. The radius of the cylinder (and the spherical cap) is a. The length of the cylinder (not counting the spherical cap) is b. The cylinder's long axis is oriented along the body's x, y or z axis according to the value of direction (1=x, 2=y, 3=z).


void dMassSetBox (dMass *, dReal density,
                  dReal lx, dReal ly, dReal lz);
Set the mass parameters to represent a box of the given dimensions and density, with the center of mass at (0,0,0) relative to the body. The side lengths of the box along the x, y and z axes are lx, ly and lz.


void dMassAdjust (dMass *, dReal newmass);
Given mass parameters for some object, adjust them so the total mass is now newmass. This is useful when using the above functions to set the mass parameters for certain objects - they take the object density, not the total mass.


void dMassTranslate (dMass *, dReal x, dReal y, dReal z);
Given mass parameters for some object, adjust them to represent the object displaced by (x,y,z) relative to the body frame.


void dMassRotate (dMass *, const dMatrix3 R);
Given mass parameters for some object, adjust them to represent the object rotated by R relative to the body frame.

7.3. Math functions

[There are quite a lot of these, but they're not standardized enough to document yet].

7.4. Error and memory functions

[Document these later].


8. Collision functions

This chapter describes the built-in collision detection system of ODE. Using ODE's collision detection is optional - an alternative collision detection system can be used as long as it can supply the right kinds of information to ODE.

8.1. Contact points

If two bodies touch, or if a body touches a static feature in its environment, the contact is represented by one or more "contact points". Each contact point has a corresponding dContactGeom structure:
struct dContactGeom {
  dVector3 pos;       // contact position
  dVector3 normal;    // normal vector
  dReal depth;        // penetration depth
};
pos records the contact position, in global coordinates.

depth is the depth to which the two bodies inter-penetrate each other. If the depth is zero then the two bodies have a grazing contact, i.e. they "only just" touch. However, this is rare - the simulation is not perfectly accurate and will often step the bodies too far so that the depth is nonzero.

normal is a unit length vector that is, generally speaking, perpendicular to the contact surface.

The convention is that if body 1 is moved along the normal vector by a distance depth (or equivalently if body 2 is moved the same distance in the opposite direction) then the contact depth will be reduced to zero. This means that the normal vector points "in" to body 1.

In real life, contact between two bodies is a sophisticated thing. Representing contacts by contact points is only an approximation. Contact "patches" or "surfaces" might be more physically accurate, but representing these things in high speed simulation software is a challenge.

Each extra contact point added to the simulation will slow it down some more, so sometimes we are forced to ignore contact points in the interests of speed. For example, when two boxes collide many contact points may be needed to properly represent the geometry of the situation, but we may choose to keep only the best three. Thus we are piling approximation on top of approximation.

8.2. Geometry objects

To use ODE's collision detection, geometry objects must be associated with the rigid bodies. A geometry object represents a rigid shape in space. Geometry objects are distinct from rigid bodies in that a geometry object has geometrical properties (size, shape, position and orientation) but no dynamical properties (such as velocity or mass).

Every geometry object is an instance of a class, such as sphere, plane, or box. You can define your own classes as well.

Every geometry object has a position vector and a 3*3 rotation matrix. If a geometry object is associated with a rigid body then its position and rotation is actually the position and rotation of that body. The point of reference for the standard classes usually corresponds to their centers of mass. This makes them particularly easy to connect to dynamics objects. If other points of reference are required, composite objects should be used to encapsulate the primitives.

8.2.1. Sphere functions


dGeomID dCreateSphere (dSpaceID space, dReal radius);
Create a sphere geometry object of the given radius, insert it into the given space, and return its ID. The point of reference for a sphere is its center.


dReal dGeomSphereGetRadius (dGeomID sphere);
Return the radius of the given sphere.

8.2.2. Box functions


dGeomID dCreateBox (dSpaceID space, dReal lx, dReal ly, dReal lz);
Create a box geometry object of the given x/y/z side lengths (lx,ly,lz), insert it into the given space, and return its ID. The point of reference for a box is its center.


void dGeomBoxGetLengths (dGeomID box, dVector3 result);
Return in result the side lengths of the given box.

8.2.3. Plane functions


dGeomID dCreatePlane (dSpaceID space,
                      dReal a, dReal b, dReal c, dReal d);
Create a plane geometry object of the given parameters, insert it into the given space, and return its ID. The plane equation is
a*x+b*y+c*z = d
The plane's normal vector is (a,b,c), and it must have length 1. Unlike other geometry objects, planes disregard their assigned position and rotation, i.e. the parameters are always in global coordinates. In other words it is assumed that the plane is always part of the static environment and not tied to any movable object.


void dGeomPlaneGetParams (dGeomID plane, dVector4 result);
Return in result the parameters of the given plane.

8.2.4. Capped cylinder functions


dGeomID dCreateCCylinder (dSpaceID space, dReal a, dReal b, int dir);
void dGeomCCylinderGetParams (dGeomID ccylinder,
                              dReal *a, dReal *b, int *dir);
(not implemented yet.)

8.2.5. General geometry object functions


void dGeomSetData (dGeomID, void *);
void *dGeomGetData (dGeomID);
These functions set and get the user-defined data pointer stored in the geometry object.


void dGeomSetBody (dGeomID, dBodyID);
dBodyID dGeomGetBody (dGeomID);
These functions set and get the body associated with the geometry object. Setting the body automatically attaches the position vector and rotation matrix of the body to the geometry object. Setting a body ID of zero gives the geometry object its own position and rotation, independent from any body.


void dGeomSetPosition (dGeomID, dReal x, dReal y, dReal z);
void dGeomSetRotation (dGeomID, const dMatrix3 R);
Set the position vector and rotation matrix of the geometry object. These functions are analogous to dBodySetPosition() and dBodySetRotation(). If the geometry object is attached to a body, the body's position / rotation will also be changed.


const dReal * dGeomGetPosition (dGeomID);
const dReal * dGeomGetRotation (dGeomID);
Return pointers to the geometry object's position vector and rotation matrix. The returned values are pointers to internal data structures, so the vectors are valid until any changes are made to the geometry object. If the geometry object is attached to a body, the body's position / rotation pointers will be returned, i.e. the result will be identical to calling dBodyGetPosition() or dBodyGetRotation().


void dGeomDestroy (dGeomID);
Destroy a geometry object, removing it from the space first.

8.3. Collision detection


int dCollide (dGeomID o1, dGeomID o2, int flags,
              dContactGeom *contact, int skip);
Given two geometry objects that potentially touch (o1 and o2), generate contact information for them. Internally, this just calls the correct class-specific collision functions for o1 and o2.

flags specifies how contacts should be generated if the objects touch. Currently the lower 8 bits of flags specifies the maximum number of contact points to generate. If this number is zero, this function just pretends that it is one - in other words you can not ask for zero contacts. All other bits in flags must be zero. In the future the other bits may be used to select other contact generation strategies.

contact must point to an array of contact geometry information that can hold at least the maximum number of contacts. Note: the elements of the contact array do not necessarily have to be contiguous. skip is the number of bytes between each dContactGeom structure in the contact array. If skip is sizeof(dContactGeom) then contact points to a "normal" (C-style) contact array. If skip is larger than this, then the dContactGeom structures are embedded in some other larger structures. It is an error for skip to be smaller than sizeof(dContactGeom).

If the objects touch, this returns the number of contact points generated (and updates the contact array), otherwise it returns 0 (and the contact array is not touched).

8.4. User defined classes

You can define your own geometry classes using the functions in this section. The standard geometry classes do not have any special access to the internals of ODE, they use the public functions exactly as you would.

Every geometry class has a unique number (0,1,2, etc...). A new geometry class (call it `X') must provide the following to ODE:

  1. Functions that will handle collision detection and contact generation between X and one or more other classes. These functions must be of type dColliderFn, which is defined as
    typedef int dColliderFn (dGeomID o1, dGeomID o2, int flags,
                             dContactGeom *contact, int skip);
    	
    This has exactly the same interface as dCollide(). Each function will handle a specific collision case, where o1 has type X and o2 has some other known type.

  2. A "selector" function, of type dGetColliderFnFn, which is defined as
    typedef dColliderFn * dGetColliderFnFn (int num);
    	
    This function takes a class number (num), and returns the collider function that can handle colliding X with class num. It should return 0 if X does not know how to collide with class num. Note that if classes X and Y are to collide, only one needs to provide a function to collide with the other.

    This function is called infrequently - the return values are cached and reused.

  3. A function that will compute the axis aligned bounding box of instances of this class. This function must be of type dGetAABBFn, which is defined as
    typedef void dGetAABBFn (dGeomID g, dReal aabb[6]);
    	
    This function is given g, which has type X, and returns the axis-aligned bounding box for g. The aabb array has elements (minx, maxx, miny, maxy, minz, maxz).

  4. The number of bytes of "class data" that instances of this class need. For example a sphere stores its radius in the class data area, and a box stores its side lengths there.

Here are the functions used to manage custom classes:


int dCreateGeomClass (const dGeomClass *classptr);
Register a new geometry class, defined by classptr. The number of the new class is returned. The convention used in ODE is to assign the class number to a global variable with the name dXxxClass where Xxx is the class name (e.g. dSphereClass).

Here is the definition of the dGeomClass structure:
struct dGeomClass {
  int bytes;                  // bytes of custom data needed
  dGetColliderFnFn *collider; // collider function
  dGetAABBFn *aabb;           // bounding box function
};


void * dGeomGetClassData (dGeomID);
Given a geometry object, return a pointer to the class's custom data (this will be a block of the required number of bytes). Internal ODE classes may use a slightly faster macro instead.


dGeomID dCreateGeom (int classnum);
Create a geometry object of the given class number. The custom data block will initially be set to 0. This object can be added to a space using dSpaceAdd().


int dGeomGetClass (dGeomID);
Given a geometry object, this returns its class number.

When you implement a new class you will usually write a function that does the following:

  1. If the class has not yet been created, create it. You should be careful to only ever create the class once.
  2. Call dCreateGeom() to make an instance of the class.
  3. Set up the custom data area.
This is what dCreateSphere() and the other geometry creation functions do.

8.5. Utility functions


int dBoxTouchesBox (const dVector3 _p1, const dMatrix3 R1,
                    const dVector3 side1, const dVector3 _p2,
                    const dMatrix3 R2, const dVector3 side2);
Given boxes (p1,R1,side1) and (p2,R2,side2), return 1 if they intersect or 0 if not. p is the center of the box, R is the rotation matrix for the box, and side is a vector of x/y/z side lengths.

8.6. Space

A space is a container for geometry objects. It is similar to the rigid body "world", except for collision instead of dynamics.

The space does high level collision culling, which means that it can identify which pairs of geometry objects are potentially touching. You can safely call dCollide() for only those pairs, instead of having to call dCollide() for every object-object pair. This can save a huge amount of time. Various collision culling algorithms will be available, however only the first is currently implemented:

Here are the functions used for spaces:


dSpaceID dSpaceCreate();
Create a space.


void dSpaceDestroy (dSpaceID);
Destroy a space. When a space is destroyed, all the geometry objects in that space are automatically destroyed as well.


void dSpaceAdd (dSpaceID, dGeomID);
void dSpaceRemove (dSpaceID, dGeomID);
Add and remove geometry objects to/from the space. These functions are normally only called by the geometry object creation/deletion functions.


void dSpaceCollide (dSpaceID space, void *data,
                    dNearCallback *callback);
Call a callback function one or more times, for all potentially intersecting objects in the space. The callback function is of type dNearCallback, which is defined as:
typedef void dNearCallback (void *data, dGeomID o1, dGeomID o2);
The data variable is passed from
dSpaceCollide() to the callback function. Its meaning is user defined. The o1 and o2 arguments are the geometry objects that may be near each other. The callback function can call dCollide() on o1 and o2, perhaps first determining whether to collide them at all based on other information.

8.7. Future

Future interface enhancements:


9. How to make good simulations

[just notes for now]

9.1. Integrator accuracy and stability

9.2. Behavior may depend on step size

9.3. Making things go faster

What factors does execution speed depend on? Each joint removes a number of degrees of freedom (DOFs) from the system. For example the ball and socket removes three, and the hinge removes five. For each separate group of bodies connected by joints, where:

then the computing time per step for the group is proportional to:
k1 O(m1) + k2 O(m23) + k2 O(n)

ODE currently relies on factorization of a ``system'' matrix that has one row/column for each DOF removed (this is where the O(m23) comes from). In a 10 body chain that uses ball and socket joints, roughly 30-40% of the time is spent filling in this matrix, and 30-40% of the time is spent factorizing it.

Thus, to speed up your simulation you might consider:

In the future ODE will implement techniques that scale better with the number of joints.

9.4. Making things stable

9.5. Using constraint force mixing (CFM)

9.6. Avoiding singularities

9.7. Other stuff


10. FAQ

10.1. How do I connect a body to the static environment with a joint?

Use dJointAttach with arguments (body,0) or (0,body).

10.2. Does ODE need or use graphics library X ?

No. ODE is a computational engine, and is completely independent of any graphics library. However the examples that come with ODE use OpenGL, and most interesting uses of ODE will need some graphics library to make the simulation visible to the user. But that's your problem.

10.3. Why do my rigid bodies bounce or penetrate on collision? My restitution is zero!

Sometimes when rigid bodies collide without restitution, they appear to inter-penetrate slightly and then get pushed apart so that they only just touch. The problem gets worse as the time step gets larger. What is going on?

The contact joint constraint is only applied after the collision is detected. If a fixed time step is being used, it is likely that the bodies have already penetrated when this happens. The error reduction mechanism will push the bodies apart, but this can take a few time steps (depending on the value of the ERP parameter).

This penetration and pushing apart sometimes makes the bodies look like they are bouncing, although it is completely independent of whether restitution is on or not.

Some other simulators have individual rigid bodies take variable sized timesteps to make sure bodies never penetrate much. However ODE takes fixed size steps, as automatically choosing a non-penetrating step size is problematic for an articulated rigid body simulator (the entire ARB structure must be stepped to account for the first penetration, which may result in very small steps).

There are three fixes for this problem:

10.4. How can an immovable body be created?

In other words, how can you create a body that doesn't move, but that interacts with other bodies? The answer is to create a geometry object only, without the corresponding rigid body object. The geometry object is associated with a rigid body ID of zero. Then in the contact callback when you detect a collision between two geometry objects with a nonzero body ID and a zero body ID, you can simply pass those two IDs to the dJointAttach() function as normal. This will create a contact between the rigid body and the static environment.

Don't try to get the same effect by setting a very high mass/inertia on the ``motionless'' body and then resetting it's position/orientation on each time step. This can cause unexpected simulation errors.


11. Known BUGS


12. ODE internals

[only notes for now]

Why don't I implement a proper friction pyramid or friction cone (e.g. Baraff's version) ?} Because I have to factor non-symmetric (and possibly indefinite) matrices, for either static or dynamic friction. Speed was considered more important - the current friction approximation only needs a symmetric factorization, which is twice as fast.

12.1. Matrix storage conventions

Matrix operations like factorization are expensive, so we must store the data in a way that is most useful to the matrix code. I want to do 4-way SIMD optimizations later, so the format is this: store the matrix by rows, and each row is rounded up to a multiple of 4 elements. The extra "padding" elements at the end of each row/column must be set to 0. This is called the "standard format". Hopefully this decision will remain good in the future, as more and more processors have 4-way SIMD (especially for fast 3D graphics).

The exception: matrices that have only one column or row (vectors), are always stored as consecutive elements in standard row format, i.e. there is no interior padding, only padding at the end.

Thus: all 3x1 floating point vectors are stored as 4x1 vectors: (x,x,x,0).

12.2. Internals FAQ

12.2.1. Why to some structures have a dx prefix and some have a d prefix?

The dx prefix is used for internal structures that should never be visible externally. The d prefix is used for structures that are part of the public interface.


Russell Smith, Saturday 14 July, 2001