There are three main approaches used to modeling contact with ā€œrigidā€ body systems: 1) rigid contact approximated with stiff compliant contact, 2) hybrid models with collision event detection, impulsive reset maps, and continuous (constrained) dynamics between collision events, and 3) rigid contact approximated with time-averaged forces (impulses) in a time-stepping scheme.1

Collision Detection

  • FCL
  • libccd
  • Bullet
  • ODE

Collision resolution

normal force + friction force

  • MuJoCo
  • MJX
  • Bullet
  • Drake
  • ODE
  • jiminy
  • RigidBodyDynamics.jl
  • PhysX2

This browser does not support PDFs. Please download the PDF to view it: Download PDF .


Implementation of jiminy3

// Extract some proxies
contactOptions_t const & contactOptions_ = engineOptions_->contacts;

// Compute the penetration speed
float64_t const vDepth =;

// Compute normal force
float64_t const fextNormal = - std::min(contactOptions_.stiffness * depth +
                                        contactOptions_.damping * vDepth, 0.0);
fextInWorld.noalias() = fextNormal * nGround;

// Compute friction forces
vector3_t const vTangential = vContactInWorld - vDepth * nGround;
float64_t const vRatio = std::min(vTangential.norm() / contactOptions_.transitionVelocity, 1.0);
float64_t const fextTangential = contactOptions_.friction * vRatio * fextNormal;
fextInWorld.noalias() -= fextTangential * vTangential;

// Add blending factor
if (contactOptions_.transitionEps > EPS)
    float64_t const blendingFactor = - depth / contactOptions_.transitionEps;
    float64_t const blendingLaw = std::tanh(2.0 * blendingFactor);
    fextInWorld *= blendingLaw;

Implementation of RigidBodyDynamics.jl4

function normal_force(model::HuntCrossleyModel, ::Nothing, z, zĢ‡)
    zn = z^model.n
    f = model.Ī» * zn * zĢ‡ + model.k * zn # (2) in Marhefka, Orin (note: z is penetration, returning repelling force)

function friction_force(model::ViscoelasticCoulombModel, state::ViscoelasticCoulombState,
        fnormal, tangential_velocity::FreeVector3D)
    T = typeof(fnormal)
    Ī¼ = model.Ī¼
    k = model.k
    b = model.b
    x = convert(FreeVector3D{SVector{3, T}}, state.tangential_displacement)
    v = tangential_velocity

    # compute friction force that would be needed to avoid slip
    fstick = -k * x - b * v

    # limit friction force to lie within friction cone
    fstick_normĀ² = dot(fstick, fstick)
    fstick_max_normĀ² = (Ī¼ * fnormal)^2
    ftangential = if fstick_normĀ² > fstick_max_normĀ²
        fstick * sqrt(fstick_max_normĀ² / fstick_normĀ²)

Implementation of MIT Cheetah Software5

template <typename T>
void ContactSpringDamper<T>::_groundContactWithOffset(T K, T D) {
  for (size_t i = 0; i < CC::_nContact; i++) {
    Vec3<T> v =
        CC::_cp_frame_list[i].transpose() *
        CC::_model->_vGC[CC::_idx_list[i]];  // velocity in plane coordinates
    T z = CC::_cp_penetration_list[i];       // the penetration into the ground
    T zd = v[2];                             // the penetration velocity
    T zr = std::sqrt(std::max(T(0), -z));  // sqrt penetration into the ground,
                                           // or zero if we aren't penetrating
    T normalForce =
        zr *
        (-K * z - D * zd);  // normal force is spring-damper * sqrt(penetration)

    // set output force to zero for now.
    CC::_cp_local_force_list[i][0] = 0;
    CC::_cp_local_force_list[i][1] = 0;
    CC::_cp_local_force_list[i][2] = 0;

    if (normalForce > 0) {
      CC::_cp_local_force_list[i][2] =
          normalForce;  // set the normal force. This is in the plane's
                        // coordinates for now

      // first, assume sticking
      // this means the tangential deformation happens at the speed of the foot.
      deflectionRate[CC::_idx_list[i]] = v.template topLeftCorner<2, 1>();
      Vec2<T> tangentialSpringForce =
          K * zr *
          _tangentialDeflections[CC::_idx_list[i]];  // tangential force due to
                                                     // "spring"
      Vec2<T> tangentialForce =
          -tangentialSpringForce -
          D * zr * deflectionRate[CC::_idx_list[i]];  // add damping to get
                                                      // total tangential

      // check for slipping:
      T slipForce = CC::_cp_mu_list[i] *
                    normalForce;  // maximum force magnitude without slipping
      T tangentialForceMagnitude =
              .norm();  // actual force magnitude if we assume sticking
      T r = tangentialForceMagnitude / slipForce;  // ratio of force/max_force

      if (r > 1) {
        // we are slipping.
        tangentialForce =
            tangentialForce / r;  // adjust tangential force to avoid slipping
        deflectionRate[CC::_idx_list[i]] =
            -(tangentialForce + tangentialSpringForce) / (D * zr);
      // set forces
      CC::_cp_local_force_list[i][0] = tangentialForce[0];
      CC::_cp_local_force_list[i][1] = tangentialForce[1];
    // move back into robot frame
    CC::_cp_force_list[CC::_idx_list[i]] =
        CC::_cp_frame_list[i] * CC::_cp_local_force_list[i];

Implementation of MuJoCo

mj_step() -> mj_forward() -> mj_forwardSkip() -> mj_fwdConstraint() -> mj_solPGS()

Implementation of Drake: Pressure Field Contact6 7

There are many ways to model contact between rigid bodies. Drake uses an approach we call ā€œcompliantā€ contact. In compliant contact, nominally rigid bodies are allowed to penetrate slightly, as if the rigid body had a slightly deformable layer, but whose compression has no appreciable effect on the bodyā€™s mass properties. The contact force between two deformed bodies is distributed over a contact patch with an uneven pressure distribution over that patch. It is common in robotics to model that as a single point contact or a set of point contacts. Hydroelastic contact instead attempts to approximate the patch and pressure distribution to provide much richer and more realistic contact behavior. For a high-level overview, see this blog post.8 Drake implements two models for resolving contact to forces: point contact and hydroelastic contact.9

The hydroelastic contact model combines two ideas: elastic foundation and hydrostatic pressure. Thus the model introduces an object-centric virtual or elastic pressure field $p_e$ to mimic the hydrostatic pressure field of a fluid. In practice, Drake generates a pressure field for primitive shapes that is maximum at the medial axis, zero at the boundary, and linearly interpolated in between. In Drake, users specify how stiff a compliant object is through the hydroelastic modulus $E_h$, the value of the pressure field at the medial axis. How to generate pressure fields for arbitrary non-convex geometries is currently a topic of active research.

Unlike Finite Element models, the hydroelastic contact model is stateless and the deformed configuration of a body is approximated. Given two overlapping (undeformed) objects A and B with pressure fields $p_A$ and $p_B$, respectively, the contact surface $\mathcal{S}^{\cap}$ is modeled as the surface of equal pressure. Total forces and moments on these bodies are the result of the integral of the equilibrium pressure field $p_e = p_A = p_B$ on the contact surface $\mathcal{S}^{\cap}$.

Comparison of Bullet, Havok, MuJoCo, ODE and PhysX

This browser does not support PDFs. Please download the PDF to view it: Download PDF .


Integration on SO(3)

This browser does not support PDFs. Please download the PDF to view it: Download PDF .


\(q_t=q_0+t*v_q \to q_t=q_0*\exp(t*v_q)\) 10




  4. Marhefka, Duane W. and David E. Orin. ā€œA compliant contact model with nonlinear damping for simulation of robotic systems.ā€ IEEE Trans. Syst. Man Cybern. Part A 29 (1999): 566-572.Ā 

  5. Azad, Morteza and Roy Featherstone. ā€œModeling the contact between a rolling sphere and a compliant ground plane.ā€ (2010).Ā 



  8. Rethinking Contact Simulation for Robot ManipulationĀ 

  9. Hydroelastic Contact User GuideĀ