Skip to content

[geometry] Add optional MuJoCo multi-point contact for convex meshes - #24922

Open
xuchenhan-lbm wants to merge 5 commits into
RobotLocomotion:masterfrom
xuchen-han:mujoco-multipoint-pr
Open

[geometry] Add optional MuJoCo multi-point contact for convex meshes#24922
xuchenhan-lbm wants to merge 5 commits into
RobotLocomotion:masterfrom
xuchen-han:mujoco-multipoint-pr

Conversation

@xuchenhan-lbm

@xuchenhan-lbm xuchenhan-lbm commented Aug 26, 2026

Copy link
Copy Markdown
Contributor

Summary

This pull request adds a second narrowphase for point contact. The new
narrowphase uses MuJoCo's convex collision detection, and it gives a pair of
convex meshes up to four contact points. The feature is off by default. A user
turns it on with a new proximity property.

The problem

Drake's point contact model gives each colliding geometry pair one contact
point, which is the deepest point. Two flat faces that overlap thus touch at a
single point. A box on a table has one support point, and that point moves from
one corner of the box to another corner between time steps.

The usual solution is to add several small spheres to the body. This pull
request adds a second solution. A geometry pair can report a contact manifold.

What the pull request does

A new proximity property ("material", "point_contact_algorithm") selects the
narrowphase for one geometry. It has two values:

  • "single_point" is the default. It keeps the existing behavior. Every
    penetrating geometry pair reports one contact point.
  • "mujoco_multipoint" selects the new narrowphase. A geometry pair reports up
    to four contact points, and each point carries its own depth.

DefaultProximityProperties::point_contact_algorithm sets the property for a
whole scene.

A geometry pair uses the new narrowphase only when both geometries select
"mujoco_multipoint" and both geometries are Convex or Mesh shapes. Every
other pair keeps the existing behavior, and so does every scalar type that is
not double. Both ComputePointPairPenetration() and the point contact
fallback of ComputeContactSurfacesWithFallback() use the property.

MuJoCo computes the manifold with GJK, EPA, and contact polygon clipping. A
face-face contact and an edge-face contact give up to four points. A
vertex-face contact gives one point.

The commits

The five commits build on each other. Please read them in order.

  1. [workspace] adds the mujoco_internal external. Its license is
    Apache-2.0. The externals dm_control_internal and ccd_internal are
    precedents. The external pins the MuJoCo 3.12.0 release, and it compiles
    only src/engine/engine_collision_gjk.c and two small utility files. The
    commit also adds the LICENSE install rule and the mirror metadata.
  2. [geometry] adds the wrapper. mujoco_ccd_mesh_data builds the mesh
    polygon tables that MuJoCo's model compiler would build, from Drake's qhull
    convex hulls. mujoco_ccd_penetration runs MuJoCo's mjc_ccd() and makes
    one PenetrationAsPointPair from each witness pair. When MuJoCo's GJK or
    EPA is degenerate, the wrapper reports the pair as unresolved, so that the
    caller can use a different algorithm.
  3. [multibody] repairs a meshcat defect that this feature made visible. The
    point contact visualizer made the path of each arrow from the names of the
    two bodies, so several contacts of one body pair drew one arrow. The
    visualizer now gives a number to each repeated contact.
  4. [geometry] connects the feature to the proximity engine. This commit
    adds the property, the validation in AssignRole(), the table cache, the
    callback dispatch with the single-point fallback, and a stable result sort.
  5. [examples] adds examples/multibody/multipoint_contact. Two Convex
    boxes fall onto a Convex slab, and each box uses a different narrowphase. At
    a time step of 1 ms the single-point box turns at about 0.23 rad/s and sinks
    5.5 mm into the slab in five seconds. At a time step of 10 ms it falls
    through the slab. The multipoint box stays at its exact rest height at every
    time step.

Validation

The face-face, rotated, edge-face, and vertex-face results of the wrapper are
identical to the results of pip mujoco==3.12.0, which is the release that the
external pins. The tests in mujoco_ccd_penetration_test hold these expected
values.

An internal corpus of 153 manipulation scenes showed no change in the median
cost. The largest improvement was a scene with a stack of rigid meshes. That
scene ran 1.46 times faster, and its settled penetration decreased from 53 mm
to 4.5 mm.

Known limitations

  • The feature works for the double scalar type only. Other scalar types keep
    the existing behavior. They do not support penetration queries between Convex
    or Mesh shapes.
  • The feature works for Convex and Mesh pairs only. A primitive shape still reports
    just a single contact point.
  • The support function is a linear scan of the vertices. This is MuJoCo's own
    choice below 10 vertices. A large hull needs MuJoCo's hill climbing variant,
    which needs mesh graph preprocessing that this port does not include.
  • A mesh whose convex hull has no volume, such as a planar mesh, reports one
    point.
  • mujoco_ccd_mesh_data.cc documents one deliberate difference from MuJoCo.
    This port does not copy the three-edge shortcut in MeshPolygon::Paths(),
    which can give a triangle with reversed winding.

This change is Reviewable

A later commit in this series uses MuJoCo's convex collision detection
to compute point contact between convex meshes. This commit adds the
external that supplies that code.

The external compiles three C files from the MuJoCo source archive. The
file src/engine/engine_collision_gjk.c holds the collision code itself.
The other two files supply the vector math helpers and the warning
hooks that the collision code needs. The external does not compile the
rest of MuJoCo, and the compiled code does not touch mjModel or mjData.

The external pins the MuJoCo 3.12.0 release. The port was validated
against this release, so an upgrade can change the computed
contacts. The collision code also reads private MuJoCo headers, and
the layout of their structs can change between releases. A person who
upgrades this external must therefore run the multi-point contact
unit tests in geometry/proximity again. The note in repository.bzl
says this.

The commit adds the external to two lists. The install list makes the
installed artifacts ship MuJoCo's Apache-2.0 LICENSE file. The mirror
metadata list makes the source archive available from Drake's mirrors.
Drake's point contact model gives each colliding geometry pair one
contact point. Two flat faces that overlap thus touch at a single
point. This commit adds an alternative computation that gives a pair of
convex meshes up to four contact points. A later commit connects the
computation to the proximity engine. This commit adds the computation
and its tests only, so it changes no existing behavior.

The new code has two parts.

MujocoCcdMeshData holds the polygon tables of one convex hull.
MakeMujocoCcdMeshData() builds these tables from a Drake convex
hull. It copies the procedure of MuJoCo's model compiler, which merges
the coplanar facets of the hull into polygons. It then computes the
normal of each merged polygon from the first three vertices of that
polygon, as MuJoCo's MakePolygonNormals() does. The computed depths
are therefore identical to the depths that MuJoCo computes. A mirroring
scale turns a hull inside out, and such a hull has a negative enclosed
volume. The function finds this condition and reverses each face loop
before it builds the tables.

ComputeMujocoMultipointPenetration() runs MuJoCo's mjc_ccd() on two
of these hulls. It makes one PenetrationAsPointPair from each witness
pair, and each point pair carries its own depth. A face-face contact
and an edge-face contact give up to four points. A vertex-face contact
gives one point, which is the existing Drake behavior.

The function reports whether MuJoCo resolved the pair. MuJoCo does not
resolve the pair in two conditions. GJK stops at its start point when
the vertex centroids of the two hulls are at the same position. EPA
returns non-finite witness points when the penetration is very
deep. In both conditions the function appends nothing and returns
false, and the caller must then use a different algorithm. A pair
that penetrates is thus never reported as separated.

Degenerate polygon clipping can give two contact points at the same
position. The function keeps the first of these points only, because
a Drake contact solver must not receive a singular manifold.

MuJoCo's default warning handler prints to stderr and appends to a
file named MUJOCO_LOG.TXT in the working directory. A Drake simulation
must not write this file. The code therefore sends MuJoCo's warnings
to Drake's logger at the debug level.

mjc_ccd() refers to two support functions that Drake does not compile.
This commit supplies them as extern "C" definitions with hidden
visibility, so that libdrake.so exports no symbol that lacks a
namespace.
The meshcat point contact visualizer made the path of each arrow from
the names of the two bodies. One body pair can report more than one
point contact at the same time. This occurs when a body has several
collision geometries and the query object port is not connected. It
also occurs when a geometry pair reports a contact manifold. Each
contact after the first one then wrote to the path of the first arrow,
so the visualizer drew one arrow instead of several.

The visualizer now counts the contacts of each body pair and gives
a number to each contact after the first one. The paths are "A+B",
"A+B#1", and so on. When the count decreases, the visualizer hides
the arrows that it no longer needs. This is the same behavior that
the visualizer already had for a body pair that stops touching.
This commit adds the proximity property ("material",
"point_contact_algorithm"). The property selects
the narrowphase that computes point contact for one
geometry, and it has two values. The value "single_point"
is the default and keeps the existing behavior. The value
"mujoco_multipoint" selects the computation that the previous commit
added. DefaultProximityProperties::point_contact_algorithm sets the
property for a whole scene.

A geometry pair uses the multi-point narrowphase only when both
geometries select "mujoco_multipoint" and both geometries are Convex or
Mesh shapes.  Such a pair reports up to four contact points instead
of the single deepest point. Every other pair keeps the existing
behavior. Both QueryObject::ComputePointPairPenetration() and the point
contact fallback of QueryObject::ComputeContactSurfacesWithFallback()
use the property.

The proximity engine builds the MuJoCo polygon tables when it
processes a geometry, and it caches them beside the convex hull
cache. Two geometries that use the same mesh file at the same scale
thus share one set of tables. When MuJoCo cannot resolve a pair,
which mujoco_ccd_penetration.h describes, the engine computes that
pair with the single-point narrowphase instead.

GeometryState validates the value of the property in AssignRole(),
before it modifies any state. An unrecognized value therefore cannot
leave GeometryState and the proximity engine in disagreement. The
check applies to every shape, although only Convex and Mesh shapes
read the property.

BackfillDefaults() writes the property only when the scene-wide
default is not "single_point". ApplyProximityDefaults() thus makes
no change to a geometry whose properties are already complete.

A geometry pair can now report more than one point pair. The
query therefore sorts its results with std::stable_sort(),
so that the documented order stays deterministic.
QueryObject::ComputePointPairPenetration() documents that one geometry
pair can appear more than one time.
The new example drops two boxes onto a slab and prints what each
box does.  All three bodies are Convex shapes, because a Box shape
cannot use the multi-point narrowphase.

The slab and the right box select "mujoco_multipoint". The left
box keeps "single_point". The left box lands on one contact point,
and that point moves from one corner of the bottom face to another
corner. The box therefore turns from side to side, moves sideways,
and sinks 5.5 mm into the slab in five seconds. At a time step of 10
ms the left box falls through the slab. The right box lands on four
contact points and stays at rest immediately.

The example prints the number of contacts, the height, and the angular
speed of each box every half second. Meshcat draws one force arrow
for each contact point. The README file describes the scene and
the results.

@xuchen-han xuchen-han left a comment

Copy link
Copy Markdown
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

+(status: do not merge) +a:@SeanCurtis-TRI for a design review. I have gone through the non-test non-example files to clean things up and I'm hoping to find a path and get this feature merged upstream (in pieces of course).

@xuchen-han made 1 comment.
Reviewable status: LGTM missing from assignee SeanCurtis-TRI(platform), needs at least two assigned reviewers, labeled "do not merge", commits need curation (https://drake.mit.edu/reviewable.html#curated-commits), missing label for release notes (waiting on SeanCurtis-TRI).

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment

Projects

None yet

Development

Successfully merging this pull request may close these issues.

3 participants