[geometry] Add optional MuJoCo multi-point contact for convex meshes - #24922
Open
xuchenhan-lbm wants to merge 5 commits into
Open
[geometry] Add optional MuJoCo multi-point contact for convex meshes#24922xuchenhan-lbm wants to merge 5 commits into
xuchenhan-lbm wants to merge 5 commits into
Conversation
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
reviewed
Aug 27, 2026
xuchen-han
left a comment
Contributor
There was a problem hiding this comment.
+(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).
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
Add this suggestion to a batch that can be applied as a single commit.This suggestion is invalid because no changes were made to the code.Suggestions cannot be applied while the pull request is closed.Suggestions cannot be applied while viewing a subset of changes.Only one suggestion per line can be applied in a batch.Add this suggestion to a batch that can be applied as a single commit.Applying suggestions on deleted lines is not supported.You must change the existing code in this line in order to create a valid suggestion.Outdated suggestions cannot be applied.This suggestion has been applied or marked resolved.Suggestions cannot be applied from pending reviews.Suggestions cannot be applied on multi-line comments.Suggestions cannot be applied while the pull request is queued to merge.Suggestion cannot be applied right now. Please check back later.
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 thenarrowphase for one geometry. It has two values:
"single_point"is the default. It keeps the existing behavior. Everypenetrating geometry pair reports one contact point.
"mujoco_multipoint"selects the new narrowphase. A geometry pair reports upto four contact points, and each point carries its own depth.
DefaultProximityProperties::point_contact_algorithmsets the property for awhole scene.
A geometry pair uses the new narrowphase only when both geometries select
"mujoco_multipoint"and both geometries are Convex or Mesh shapes. Everyother pair keeps the existing behavior, and so does every scalar type that is
not
double. BothComputePointPairPenetration()and the point contactfallback 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.
mujoco_internalexternal. Its license isApache-2.0. The externals
dm_control_internalandccd_internalareprecedents. The external pins the MuJoCo 3.12.0 release, and it compiles
only
src/engine/engine_collision_gjk.cand two small utility files. Thecommit also adds the LICENSE install rule and the mirror metadata.
mujoco_ccd_mesh_databuilds the meshpolygon tables that MuJoCo's model compiler would build, from Drake's qhull
convex hulls.
mujoco_ccd_penetrationruns MuJoCo'smjc_ccd()and makesone
PenetrationAsPointPairfrom each witness pair. When MuJoCo's GJK orEPA is degenerate, the wrapper reports the pair as unresolved, so that the
caller can use a different algorithm.
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.
adds the property, the validation in
AssignRole(), the table cache, thecallback dispatch with the single-point fallback, and a stable result sort.
examples/multibody/multipoint_contact. Two Convexboxes 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 theexternal pins. The tests in
mujoco_ccd_penetration_testhold these expectedvalues.
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
doublescalar type only. Other scalar types keepthe existing behavior. They do not support penetration queries between Convex
or Mesh shapes.
just a single contact point.
choice below 10 vertices. A large hull needs MuJoCo's hill climbing variant,
which needs mesh graph preprocessing that this port does not include.
point.
mujoco_ccd_mesh_data.ccdocuments 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