[geometry] Add opt-in MuJoCo multi-point contact for convex meshes - #13
Closed
xuchenhan-lbm wants to merge 5 commits into
Closed
[geometry] Add opt-in MuJoCo multi-point contact for convex meshes#13xuchenhan-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.
xuchenhan-lbm
force-pushed
the
mujoco-multipoint-pr
branch
from
August 26, 2026 23:55
7b79ac4 to
8d694b8
Compare
Collaborator
Author
|
Superseded by the upstream pull request: RobotLocomotion#24922. The branch now holds the rebased and rewritten commits that the upstream pull request uses. |
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
2 participants
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.
This PR adds an opt-in multi-point narrowphase for point contact, using MuJoCo's native convex collision detection.
Why
Drake's point contact model resolves each colliding geometry pair to a single contact point, the deepest one, even when two flat faces overlap. A box resting on a table is then supported at one wandering point: it cannot be in static equilibrium, so it rocks from corner to corner, creeps sideways, and (because each corner exchange lets the center of mass fall a little before the contact catches it) ratchets downward through the surface. The usual workaround is to decorate bodies with several small spheres. This PR instead lets a Convex/Mesh pair report a contact manifold of up to four points, each with its own depth, computed by MuJoCo's GJK/EPA with contact-polygon clipping.
What it does
A new proximity property
("material", "point_contact_algorithm")selects the narrowphase per geometry, with values"single_point"(the default, existing behavior) and"mujoco_multipoint". It can be set scene-wide throughDefaultProximityProperties::point_contact_algorithm. A pair reports the manifold only when both geometries opt in and both are Convex or Mesh shapes; every other pair, and every scalar type other than double, keeps the existing single-point behavior. BothComputePointPairPenetration()and the point fallback ofComputeContactSurfacesWithFallback()honor the property.The five commits build on each other, so they read best in order:
mujoco_internalexternal (Apache-2.0; precedent:dm_control_internal,ccd_internal), pinned to the MuJoCo 3.12.0 release. It compiles onlysrc/engine/engine_collision_gjk.cand its two small utility dependencies. LICENSE install and mirror metadata are wired in.mujoco_ccd_mesh_datareplicates the mesh polygon tables MuJoCo's model compiler would build (from Drake's qhull convex hulls, including MuJoCo's exact merged-polygon normals and a winding repair for mirrored scales), andmujoco_ccd_penetrationruns MuJoCo'smjc_ccdand converts each witness pair into aPenetrationAsPointPair. When MuJoCo's GJK/EPA degenerates (coincident hull centroids, or non-finite EPA output under very deep penetration) the wrapper reports the pair as unresolved so the caller can fall back.examples/multibody/multipoint_contact: two Convex boxes fall flat onto a Convex slab, one per narrowphase. At a 1 ms time step the single-point box rocks at about 0.23 rad/s and sinks 5.5 mm into the slab over five seconds, and at a 10 ms step it falls through entirely; the manifold box rests at its exact rest height immediately at every step size.Validation
The wrapper's face-face, rotated, edge-face, and vertex-face expectations were validated bit-for-bit against pip
mujoco==3.12.0(the same version the external pins); the expectations are baked intomujoco_ccd_penetration_test. On an internal 153-scene manipulation corpus the narrowphase was cost-neutral at the median, with the largest win on a rigid mesh-stack scene (1.46x faster, settled penetration 53 mm down to 4.5 mm).Known limitations
mujoco_ccd_mesh_data.cc(the 3-edge-shortcut winding quirk ofMeshPolygon::Paths()is not replicated).🤖 Generated with Claude Code
This change is