From 256bbbe7e90e99bbdb1f841544896dce0919acf5 Mon Sep 17 00:00:00 2001 From: Xuchen Han Date: Sat, 22 Aug 2026 11:26:53 -0700 Subject: [PATCH 1/5] [workspace] Add mujoco_internal for native convex collision detection 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. --- MODULE.bazel | 1 + tools/workspace/BUILD.bazel | 2 + tools/workspace/default.bzl | 2 + tools/workspace/mujoco_internal/BUILD.bazel | 3 + .../mujoco_internal/package.BUILD.bazel | 100 ++++++++++++++++++ .../workspace/mujoco_internal/repository.bzl | 21 ++++ 6 files changed, 129 insertions(+) create mode 100644 tools/workspace/mujoco_internal/BUILD.bazel create mode 100644 tools/workspace/mujoco_internal/package.BUILD.bazel create mode 100644 tools/workspace/mujoco_internal/repository.bzl diff --git a/MODULE.bazel b/MODULE.bazel index 07e94237e6ed..250f20de8b96 100644 --- a/MODULE.bazel +++ b/MODULE.bazel @@ -328,6 +328,7 @@ use_repo( "libzip_internal", "metis_internal", "msgpack_internal", + "mujoco_internal", "nanoflann_internal", "nlopt_internal", "onetbb_internal", diff --git a/tools/workspace/BUILD.bazel b/tools/workspace/BUILD.bazel index 43b7824e997f..cb8e01bb0ad6 100644 --- a/tools/workspace/BUILD.bazel +++ b/tools/workspace/BUILD.bazel @@ -45,6 +45,7 @@ filegroup( "@mosek//:drake_repository_metadata.json", "@mpmath_py_internal//:drake_repository_metadata.json", "@msgpack_internal//:drake_repository_metadata.json", + "@mujoco_internal//:drake_repository_metadata.json", "@mujoco_menagerie_internal//:drake_repository_metadata.json", "@nanoflann_internal//:drake_repository_metadata.json", "@nlopt_internal//:drake_repository_metadata.json", @@ -191,6 +192,7 @@ _DRAKE_EXTERNAL_PACKAGE_INSTALLS = ["@%s//:install" % p for p in [ "meshcat", "metis_internal", "msgpack_internal", + "mujoco_internal", "nanoflann_internal", "nlopt_internal", "picosha2_internal", diff --git a/tools/workspace/default.bzl b/tools/workspace/default.bzl index 68842c11b09a..a76f94f9f067 100644 --- a/tools/workspace/default.bzl +++ b/tools/workspace/default.bzl @@ -35,6 +35,7 @@ load("//tools/workspace/metis_internal:repository.bzl", "metis_internal_reposito load("//tools/workspace/mosek:repository.bzl", "mosek_repository") load("//tools/workspace/mpmath_py_internal:repository.bzl", "mpmath_py_internal_repository") # noqa load("//tools/workspace/msgpack_internal:repository.bzl", "msgpack_internal_repository") # noqa +load("//tools/workspace/mujoco_internal:repository.bzl", "mujoco_internal_repository") # noqa load("//tools/workspace/mujoco_menagerie_internal:repository.bzl", "mujoco_menagerie_internal_repository") # noqa load("//tools/workspace/nanoflann_internal:repository.bzl", "nanoflann_internal_repository") # noqa load("//tools/workspace/nlopt_internal:repository.bzl", "nlopt_internal_repository") # noqa @@ -108,6 +109,7 @@ def _add_internal_repositories(): metis_internal_repository(name = "metis_internal", mirrors = mirrors) mpmath_py_internal_repository(name = "mpmath_py_internal", mirrors = mirrors) # noqa msgpack_internal_repository(name = "msgpack_internal", mirrors = mirrors) + mujoco_internal_repository(name = "mujoco_internal", mirrors = mirrors) mujoco_menagerie_internal_repository(name = "mujoco_menagerie_internal", mirrors = mirrors) # noqa nanoflann_internal_repository(name = "nanoflann_internal", mirrors = mirrors) # noqa nlopt_internal_repository(name = "nlopt_internal", mirrors = mirrors) diff --git a/tools/workspace/mujoco_internal/BUILD.bazel b/tools/workspace/mujoco_internal/BUILD.bazel new file mode 100644 index 000000000000..67914ea7e0a0 --- /dev/null +++ b/tools/workspace/mujoco_internal/BUILD.bazel @@ -0,0 +1,3 @@ +load("//tools/lint:lint.bzl", "add_lint_tests") + +add_lint_tests() diff --git a/tools/workspace/mujoco_internal/package.BUILD.bazel b/tools/workspace/mujoco_internal/package.BUILD.bazel new file mode 100644 index 000000000000..c1a16606bef6 --- /dev/null +++ b/tools/workspace/mujoco_internal/package.BUILD.bazel @@ -0,0 +1,100 @@ +# -*- bazel -*- + +load("@drake//tools/install:install.bzl", "install") +load("@drake//tools/skylark:cc.bzl", "cc_library") +load( + "@drake//tools/skylark:drake_cc.bzl", + "cc_linkonly_library", +) + +licenses(["notice"]) # Apache-2.0 + +package(default_visibility = ["//visibility:private"]) + +# Drake uses only MuJoCo's native convex collision detection (GJK + EPA with +# multi-point contact recovery via contact-polygon clipping), which lives in +# src/engine/engine_collision_gjk.c and is nearly self-contained: it needs +# only the small vector-math helpers in engine_util_blas.c and the warning +# hooks in engine_util_errmem.c. In particular, it does not touch mjModel or +# mjData; callers supply support functions and mesh polygon tables through +# the mjCCDObj struct (declared in engine_collision_convex.h, which we export +# as a header but do not compile). + +# The public mujoco/*.h headers plus the private engine headers needed to +# declare the mjc_ccd() entry point and the mjCCDObj struct. +cc_library( + name = "hdrs", + hdrs = [ + "include/mujoco/mjdata.h", + "include/mujoco/mjexport.h", + "include/mujoco/mjmacro.h", + "include/mujoco/mjmodel.h", + "include/mujoco/mjtype.h", + "src/engine/engine_collision_convex.h", + "src/engine/engine_collision_gjk.h", + "src/engine/engine_util_blas.h", + "src/engine/engine_util_errmem.h", + ], + # MJ_STATIC neutralizes the MJAPI dllexport/visibility attributes; we + # link the object code statically into Drake. + defines = ["MJ_STATIC"], + includes = [ + "include", + "src", + ], + isystem = True, + deps = [ + "@ccd_internal//:ccd", + ], +) + +# Compile the collision code (using both the exported headers and the private +# headers it includes). +cc_library( + name = "compiled", + srcs = [ + "src/engine/engine_callback.h", + "src/engine/engine_collision_gjk.c", + "src/engine/engine_crossplatform.h", + "src/engine/engine_macro.h", + "src/engine/engine_util_blas.c", + "src/engine/engine_util_blas_avx.h", + "src/engine/engine_util_errmem.c", + ], + copts = [ + "-Wno-all", + "-fvisibility=hidden", + ], + includes = [ + "include", + "src", + ], + isystem = True, + linkstatic = True, + deps = [":hdrs"], +) + +# Strip the private headers out; we just want the object code. +cc_linkonly_library( + name = "archive", + deps = [":compiled"], +) + +# Combine the public headers with the object code. +cc_library( + name = "mujoco_ccd", + linkstatic = True, + visibility = ["//visibility:public"], + deps = [ + ":archive", + ":hdrs", + ], +) + +install( + name = "install", + docs = ["LICENSE"], + visibility = ["//visibility:public"], +) + +exports_files(["drake_repository_metadata.json"]) diff --git a/tools/workspace/mujoco_internal/repository.bzl b/tools/workspace/mujoco_internal/repository.bzl new file mode 100644 index 000000000000..4bfaefedc25d --- /dev/null +++ b/tools/workspace/mujoco_internal/repository.bzl @@ -0,0 +1,21 @@ +load("//tools/workspace:github.bzl", "github_archive") + +def mujoco_internal_repository( + name, + mirrors = None): + github_archive( + name = name, + repository = "google-deepmind/mujoco", + # Note for upgraders: Drake compiles a small subset of MuJoCo's + # engine internals (the native convex collision detection, i.e. + # GJK/EPA with multi-point contact recovery) whose private headers + # and struct layouts may change between MuJoCo releases. After an + # upgrade, re-run the multipoint contact unit tests in + # //geometry/proximity; they pin down the expected contact manifolds + # numerically. + upgrade_type = "release", + commit = "3.12.0", + sha256 = "9faa979982c3e924e8aaff3b16983bba3a1ab19c81f4f73178ae9c2ea25e467d", # noqa + build_file = ":package.BUILD.bazel", + mirrors = mirrors, + ) From 9fe82fcf1631351c24a298604d3c89a4c46f3034 Mon Sep 17 00:00:00 2001 From: Xuchen Han Date: Sat, 22 Aug 2026 11:27:08 -0700 Subject: [PATCH 2/5] [geometry] Add MuJoCo multi-point convex mesh penetration 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. --- geometry/proximity/BUILD.bazel | 41 ++ geometry/proximity/mujoco_ccd_mesh_data.cc | 265 +++++++++++++ geometry/proximity/mujoco_ccd_mesh_data.h | 88 +++++ geometry/proximity/mujoco_ccd_penetration.cc | 264 +++++++++++++ geometry/proximity/mujoco_ccd_penetration.h | 58 +++ .../test/mujoco_ccd_penetration_test.cc | 360 ++++++++++++++++++ 6 files changed, 1076 insertions(+) create mode 100644 geometry/proximity/mujoco_ccd_mesh_data.cc create mode 100644 geometry/proximity/mujoco_ccd_mesh_data.h create mode 100644 geometry/proximity/mujoco_ccd_penetration.cc create mode 100644 geometry/proximity/mujoco_ccd_penetration.h create mode 100644 geometry/proximity/test/mujoco_ccd_penetration_test.cc diff --git a/geometry/proximity/BUILD.bazel b/geometry/proximity/BUILD.bazel index 4740ab774b99..9a2d84201c8e 100644 --- a/geometry/proximity/BUILD.bazel +++ b/geometry/proximity/BUILD.bazel @@ -67,6 +67,8 @@ drake_cc_package_library( ":mesh_to_vtk", ":mesh_traits", ":meshing_utilities", + ":mujoco_ccd_mesh_data", + ":mujoco_ccd_penetration", ":obj_to_surface_mesh", ":plane", ":polygon_surface_mesh", @@ -829,6 +831,34 @@ drake_cc_library( ], ) +drake_cc_library( + name = "mujoco_ccd_mesh_data", + srcs = ["mujoco_ccd_mesh_data.cc"], + hdrs = ["mujoco_ccd_mesh_data.h"], + deps = [ + ":polygon_surface_mesh", + "//common:essential", + "@eigen", + ], +) + +drake_cc_library( + name = "mujoco_ccd_penetration", + srcs = ["mujoco_ccd_penetration.cc"], + hdrs = ["mujoco_ccd_penetration.h"], + deps = [ + ":mujoco_ccd_mesh_data", + "//geometry:geometry_ids", + "//geometry/query_results:penetration_as_point_pair", + "//math:geometric_transform", + ], + implementation_deps = [ + "//common:essential", + "//common:unused", + "@mujoco_internal//:mujoco_ccd", + ], +) + drake_cc_library( name = "obj_to_surface_mesh", srcs = ["obj_to_surface_mesh.cc"], @@ -1740,6 +1770,17 @@ drake_cc_googletest( ], ) +drake_cc_googletest( + name = "mujoco_ccd_penetration_test", + deps = [ + ":mujoco_ccd_mesh_data", + ":mujoco_ccd_penetration", + ":polygon_surface_mesh", + "//common/test_utilities:eigen_matrix_compare", + "//math:geometric_transform", + ], +) + drake_cc_googletest( name = "obj_to_surface_mesh_test", data = [ diff --git a/geometry/proximity/mujoco_ccd_mesh_data.cc b/geometry/proximity/mujoco_ccd_mesh_data.cc new file mode 100644 index 000000000000..90fa7a184470 --- /dev/null +++ b/geometry/proximity/mujoco_ccd_mesh_data.cc @@ -0,0 +1,265 @@ +#include "drake/geometry/proximity/mujoco_ccd_mesh_data.h" + +#include +#include +#include +#include + +#include "drake/common/drake_throw.h" + +namespace drake { +namespace geometry { +namespace internal { +namespace { + +using Eigen::Vector3d; + +/* MuJoCo's coplanarity tolerance: faces whose normals fall into the same + 0.01-radian spherical-angle bin are merged into one polygon (see kAngleTol in + mjCMesh::MakePolygons()). */ +constexpr double kAngleTol = 0.01; + +/* One bucket of (near-)coplanar faces, replicating MuJoCo's MeshPolygon + class. Merged faces do not necessarily share edges when inserted, so edges + are grouped into "islands" until later insertions connect them; each island + that survives becomes one output polygon. */ +struct NormalBucket { + /* The directed boundary edges of the merged region so far. An inserted + face cancels edges that its own (reversed) edges match. */ + std::vector> edges; + /* islands[i] is the island id of edges[i]. */ + std::vector islands; + int num_islands{0}; +}; + +/* Replicates MuJoCo's mjuu_makenormal(): the unit normal of the triangle + (a, b, c), falling back to (1, 0, 0) when the triangle is degenerate. MuJoCo + assigns each merged polygon the normal its first three vertices produce (see + mjCMesh::MakePolygonNormals(), which overwrites the provisional bin-center + normals that MakePolygons() records). */ +Vector3d MakeNormal(const Vector3d& a, const Vector3d& b, const Vector3d& c) { + const Vector3d normal = (b - a).cross(c - a); + const double norm = normal.norm(); + constexpr double kMjEps = 1e-14; // MuJoCo's mjEPS. + if (norm < kMjEps) { + return Vector3d::UnitX(); + } + return normal / norm; +} + +/* Replicates MuJoCo's MeshPolygonKey(): quantizes the direction of a unit + normal to 0.01-radian bins of its spherical angles. Returns the bin as + integral-valued doubles (theta_bin, phi_bin). */ +std::pair QuantizedNormalKey(const Vector3d& n_in) { + // Adding 0.0 turns -0.0 into +0.0 so that atan2 is insensitive to the sign + // of zero, exactly as MuJoCo does. + const double nx = n_in.x() + 0.0; + const double ny = n_in.y() + 0.0; + const double nz = n_in.z() + 0.0; + if (std::abs(nz) > 1.0 - 1e-7) { + const double rphi = nz < 0 ? std::round(M_PI / kAngleTol) : 0.0; + return {0.0, rphi}; + } + const double rtheta = std::round(std::atan2(ny, nx) / kAngleTol); + const double rphi = std::round(std::acos(nz) / kAngleTol); + return {rtheta, rphi}; +} + +/* Replicates MuJoCo's MeshPolygon::CombineIslands(): renumbers the islands + when a newly inserted face connects `island1` and `island2`, keeping the + smaller index (returned via island1) and compacting indices above the + removed one. */ +void CombineIslands(NormalBucket* bucket, int* island1, int island2) { + if (island2 < *island1) { + std::swap(*island1, island2); + } + for (int& island : bucket->islands) { + if (island == island2) { + island = *island1; + } else if (island > island2) { + --island; + } + } +} + +/* Inserts one face (given as its CCW vertex loop) into the bucket. This + generalizes MuJoCo's MeshPolygon::InsertFace() from triangles to n-gons: + each directed edge of the face either cancels the reversed edge already on + some island's boundary (joining that island) or is added to the boundary. */ +void InsertFace(NormalBucket* bucket, const std::vector& loop) { + const int n = std::ssize(loop); + int island = -1; + std::vector> new_edges; + for (int i = 0; i < n; ++i) { + const int a = loop[i]; + const int b = loop[(i + 1) % n]; + bool cancelled = false; + for (int j = 0; j < std::ssize(bucket->edges); ++j) { + if (bucket->edges[j].first == b && bucket->edges[j].second == a) { + const int other = bucket->islands[j]; + bucket->edges.erase(bucket->edges.begin() + j); + bucket->islands.erase(bucket->islands.begin() + j); + if (island == -1) { + island = other; + } else if (other != island) { + --bucket->num_islands; + CombineIslands(bucket, &island, other); + } + cancelled = true; + break; + } + } + if (!cancelled) new_edges.push_back({a, b}); + } + if (island == -1) island = bucket->num_islands++; + for (const auto& edge : new_edges) { + bucket->edges.push_back(edge); + bucket->islands.push_back(island); + } +} + +/* Replicates MuJoCo's MeshPolygon::Paths(): traces each island's directed + boundary edges into a closed vertex loop. On a closed convex hull each + island's edges form exactly one cycle, so simple successor-following + recovers it. + + Known deliberate divergence: when a multi-face bucket boils down to exactly + three surviving edges that happen to sit in the edge list out of cyclic + order, MuJoCo's Paths() shortcut emits them in storage order (which can + yield a reversed-winding triangle); this implementation always follows + successors and emits the correctly wound loop. */ +std::vector> TracePaths(const NormalBucket& bucket) { + std::vector> paths; + const int num_edges = std::ssize(bucket.edges); + for (int i = 0; i < bucket.num_islands; ++i) { + std::vector path; + for (int j = 0; j < num_edges; ++j) { + if (bucket.islands[j] == i) { + path.push_back(bucket.edges[j].first); + path.push_back(bucket.edges[j].second); + break; + } + } + if (path.empty()) continue; + // Follow directed edges until the cycle closes (bounded by num_edges + // steps to be robust to malformed input). + for (int steps = 0; steps < num_edges; ++steps) { + const int next = path.back(); + bool advanced = false; + for (int j = 0; j < num_edges; ++j) { + if (bucket.islands[j] == i && bucket.edges[j].first == next) { + if (bucket.edges[j].second != path.front()) { + path.push_back(bucket.edges[j].second); + advanced = true; + } + break; + } + } + if (!advanced) break; + } + paths.push_back(std::move(path)); + } + return paths; +} + +} // namespace + +MujocoCcdMeshData MakeMujocoCcdMeshData( + const PolygonSurfaceMesh& hull) { + DRAKE_THROW_UNLESS(hull.num_vertices() >= 4); + MujocoCcdMeshData data; + + // Vertices (single precision, as MuJoCo stores them) and centroid. + data.nvert = hull.num_vertices(); + data.vert.reserve(3 * data.nvert); + Vector3d centroid = Vector3d::Zero(); + for (int v = 0; v < data.nvert; ++v) { + const Vector3d& p = hull.vertex(v); + data.vert.push_back(static_cast(p.x())); + data.vert.push_back(static_cast(p.y())); + data.vert.push_back(static_cast(p.z())); + centroid += p; + } + data.centroid = centroid / data.nvert; + + // Detect an inward-wound input (e.g. a hull whose face topology was cached + // at one scale and re-instantiated at a mirroring scale, which flips the + // loops' orientation): the signed volume enclosed by an outward-wound + // closed surface is positive. When inverted, every face loop is reversed on + // insertion so the tables are always outward-wound, as MuJoCo requires. + double signed_volume = 0.0; + for (int f = 0; f < hull.num_faces(); ++f) { + const SurfacePolygon face = hull.element(f); + const Vector3d& v0 = hull.vertex(face.vertex(0)); + for (int i = 1; i + 1 < face.num_vertices(); ++i) { + signed_volume += v0.dot(hull.vertex(face.vertex(i)) + .cross(hull.vertex(face.vertex(i + 1)))) / + 6.0; + } + } + const bool reverse_winding = signed_volume < 0.0; + + // Bucket the faces by quantized normal direction. A std::map keyed on the + // integral bin values gives deterministic output ordering (MuJoCo uses an + // unordered_map here; bucket *content* is identical, only the order of the + // output polygons may differ, which has no semantic effect). + std::map, NormalBucket> buckets; + for (int f = 0; f < hull.num_faces(); ++f) { + const Vector3d normal = + reverse_winding ? (-hull.face_normal(f)).eval() : hull.face_normal(f); + if (!normal.allFinite() || normal.norm() < 0.5) continue; // Degenerate. + const std::pair key = QuantizedNormalKey(normal); + NormalBucket& bucket = buckets[key]; + const SurfacePolygon face = hull.element(f); + const int n = face.num_vertices(); + std::vector loop(n); + for (int i = 0; i < n; ++i) { + loop[i] = face.vertex(reverse_winding ? n - 1 - i : i); + } + InsertFace(&bucket, loop); + } + + // Trace each bucket's islands into polygons and flatten the tables. + std::vector> vertex_polygons(data.nvert); + for (const auto& key_bucket : buckets) { + const NormalBucket& bucket = key_bucket.second; + for (std::vector& path : TracePaths(bucket)) { + const int path_size = static_cast(std::ssize(path)); + if (path_size < 3) continue; + const int poly_index = data.polynum++; + // The polygon's normal comes from its first three vertices, exactly as + // MuJoCo's mjCMesh::MakePolygonNormals() computes it. + const Vector3d normal = MakeNormal( + hull.vertex(path[0]), hull.vertex(path[1]), hull.vertex(path[2])); + data.polynormal.push_back(normal.x()); + data.polynormal.push_back(normal.y()); + data.polynormal.push_back(normal.z()); + data.polyvertadr.push_back(static_cast(std::ssize(data.polyvert))); + data.polyvertnum.push_back(path_size); + data.npolygonmax = std::max(data.npolygonmax, path_size); + for (int v : path) { + DRAKE_THROW_UNLESS(0 <= v && v < data.nvert); + data.polyvert.push_back(v); + vertex_polygons[v].push_back(poly_index); + } + } + } + + // Flatten the vertex-to-polygon adjacency. + data.polymapadr.reserve(data.nvert); + data.polymapnum.reserve(data.nvert); + for (int v = 0; v < data.nvert; ++v) { + const int degree = static_cast(std::ssize(vertex_polygons[v])); + data.polymapadr.push_back(static_cast(std::ssize(data.polymap))); + data.polymapnum.push_back(degree); + data.nmeshdegmax = std::max(data.nmeshdegmax, degree); + data.polymap.insert(data.polymap.end(), vertex_polygons[v].begin(), + vertex_polygons[v].end()); + } + + return data; +} + +} // namespace internal +} // namespace geometry +} // namespace drake diff --git a/geometry/proximity/mujoco_ccd_mesh_data.h b/geometry/proximity/mujoco_ccd_mesh_data.h new file mode 100644 index 000000000000..52241f3ea7e0 --- /dev/null +++ b/geometry/proximity/mujoco_ccd_mesh_data.h @@ -0,0 +1,88 @@ +#pragma once + +#include + +#include + +#include "drake/geometry/proximity/polygon_surface_mesh.h" + +namespace drake { +namespace geometry { +namespace internal { + +/* Precomputed collision data for one convex mesh, in the format consumed by + MuJoCo's native convex collision detection (GJK/EPA with multi-point contact + recovery by contact-polygon clipping). + + The polygon tables replicate what MuJoCo's model compiler produces in + mjCMesh::MakePolygons(): the (near-)coplanar faces of the convex hull are + merged into maximal polygons, and a vertex-to-polygon adjacency map is + provided so that the collision code can identify the mesh feature (face, + edge, or vertex) participating in a contact. Field names follow MuJoCo's + mjCCDObj mesh data (see mjModel's mesh_poly* arrays) to keep the + correspondence auditable, with all addresses local to this mesh (i.e., + zero-offset). + + Vertices are stored in single precision because that is how MuJoCo stores + mesh vertices; using the same representation keeps the contact results + bit-comparable with MuJoCo's. */ +struct MujocoCcdMeshData { + /* Hull vertex positions, measured and expressed in the mesh's canonical + frame; 3 * nvert floats. */ + std::vector vert; + int nvert{0}; + + /* The mean of the hull vertices, used to seed the collision query (MuJoCo + seeds with the geom center). It is strictly inside the hull. */ + Eigen::Vector3d centroid{Eigen::Vector3d::Zero()}; + + /* Number of merged polygons. */ + int polynum{0}; + /* Outward unit normal of each polygon (3 * polynum), computed from the + polygon's first three vertices exactly as MuJoCo's + mjCMesh::MakePolygonNormals() does. */ + std::vector polynormal; + /* Start index into `polyvert` for each polygon (size polynum). */ + std::vector polyvertadr; + /* Number of vertices of each polygon (size polynum). */ + std::vector polyvertnum; + /* Concatenated vertex indices of each polygon, wound counterclockwise when + viewed from outside the hull. */ + std::vector polyvert; + + /* Start index into `polymap` for each vertex (size nvert). */ + std::vector polymapadr; + /* Number of adjacent polygons of each vertex (size nvert). A vertex that + ended up interior to a merged polygon has count zero. */ + std::vector polymapnum; + /* Concatenated adjacent-polygon indices of each vertex. */ + std::vector polymap; + + /* Maximum number of vertices in any polygon; sizes MuJoCo's clipping + scratch buffers (MuJoCo's per-model `npolygonmax`). */ + int npolygonmax{0}; + /* Maximum number of polygons adjacent to any vertex (MuJoCo's per-model + `nmeshdegmax`). */ + int nmeshdegmax{0}; +}; + +/* Builds the MuJoCo collision data for the given convex hull, replicating the + preprocessing MuJoCo's model compiler performs in mjCMesh::MakePolygons(): + hull faces are bucketed by their outward normal direction quantized to + 0.01-radian bins, faces in the same bucket that share (directed) edges are + merged, and each merged region's boundary is traced into a single polygon. + + @param hull A convex polygonal surface mesh, e.g. the result of + MakeConvexHull(). Faces that are already merged (e.g. by + qhull) are handled fine; the quantized-normal merge is applied + on top. Faces are normally wound counterclockwise viewed from + the outside; a uniformly inward-wound mesh (e.g. cached hull + topology re-instantiated at a mirroring scale) is detected via + its negative enclosed volume and handled by reversing the + loops. + @pre hull is a closed convex surface with consistently oriented faces. */ +MujocoCcdMeshData MakeMujocoCcdMeshData(const PolygonSurfaceMesh& hull); + +} // namespace internal +} // namespace geometry +} // namespace drake diff --git a/geometry/proximity/mujoco_ccd_penetration.cc b/geometry/proximity/mujoco_ccd_penetration.cc new file mode 100644 index 000000000000..b504462e6b45 --- /dev/null +++ b/geometry/proximity/mujoco_ccd_penetration.cc @@ -0,0 +1,264 @@ +#include "drake/geometry/proximity/mujoco_ccd_penetration.h" + +#include +#include +#include +#include + +// MuJoCo's native convex collision detection (from @mujoco_internal). The +// header declares mjc_ccd()/mjc_ccdSize() and the mjCCDObj/mjCCDConfig/ +// mjCCDStatus structs, with C linkage. +#include +#include + +#include "drake/common/drake_assert.h" +#include "drake/common/text_logging.h" +#include "drake/common/unused.h" + +// mjc_ccd() references these two support functions from MuJoCo's +// engine_collision_convex.c (its sphere/capsule shrink-and-inflate path), +// which Drake does not compile. Drake only ever passes mjGEOM_MESH objects, +// so the referencing branch is unreachable at runtime; these exact ports of +// the MuJoCo definitions satisfy the linker (and behave correctly should +// they ever be reached). They are hidden so libdrake.so does not export the +// (non-namespaced) names. +extern "C" __attribute__((visibility("hidden"))) void mjc_pointSupport( + mjtNum res[3], mjCCDObj* obj, const mjtNum[3]) { + res[0] = obj->pos[0]; + res[1] = obj->pos[1]; + res[2] = obj->pos[2]; +} + +extern "C" __attribute__((visibility("hidden"))) void mjc_lineSupport( + mjtNum res[3], mjCCDObj* obj, const mjtNum dir[3]) { + const mjtNum* mat = obj->mat; + const mjtNum* pos = obj->pos; + const mjtNum length = obj->size[1]; + const mjtNum dot = mat[2] * dir[0] + mat[5] * dir[1] + mat[8] * dir[2]; + const mjtNum scl = dot >= 0 ? length : -length; + res[0] = mat[2] * scl + pos[0]; + res[1] = mat[5] * scl + pos[1]; + res[2] = mat[8] * scl + pos[2]; +} + +namespace drake { +namespace geometry { +namespace internal { +namespace { + +/* MuJoCo's model-compiler defaults for the CCD pipeline (mjOption's + ccd_tolerance and ccd_iterations in engine_init.c). */ +constexpr double kCcdTolerance = 1e-6; +constexpr int kCcdIterations = 35; + +/* MuJoCo produces at most 4 contacts for a mesh-mesh pair on the native + (zero-margin) path: larger clipped contact polygons are pruned to the + maximum-area quadrilateral (see maxContacts() in MuJoCo's + engine_collision_convex.c). */ +constexpr int kMaxContacts = 4; + +/* MuJoCo's EPA reports a degenerate polytope through mju_warning(). MuJoCo's + default handler prints to stderr and appends to a file named MUJOCO_LOG.TXT + in the process's working directory, which a Drake simulation must not do, so + the handler pointer is redirected here on first use. The pointer is a global + in Drake's private copy of MuJoCo (compiled with hidden visibility), so + redirecting it does not affect any other MuJoCo library in the process. The + warned-about cases are ones this file already tolerates (the affected points + come back non-finite and are dropped), so they are logged at debug level. */ +void LogMujocoWarning(const char* message) { + drake::log()->debug("MuJoCo convex collision detection: {}", message); +} + +void InstallMujocoWarningHandler() { + static const bool installed = []() { + mju_user_warning = &LogMujocoWarning; + return true; + }(); + unused(installed); +} + +/* Support function over the precomputed float vertices; a faithful port of + MuJoCo's mjc_meshSupport() (linear scan; the hill-climbing variant needs + mjModel graph data that we don't build). Records the winning vertex index in + obj->vertindex — EPA's duplicate-support detection and the multi-point + feature identification both depend on it. */ +void DrakeMeshSupport(mjtNum res[3], mjCCDObj* obj, const mjtNum dir[3]) { + const mjtNum* mat = obj->mat; + const mjtNum* pos = obj->pos; + const float* verts = obj->data.mesh.vert; + const int nverts = obj->data.mesh.nvert; + + // local_dir = matᵀ * dir (mat is row-major world-from-mesh rotation). + const mjtNum local_dir[3] = { + mat[0] * dir[0] + mat[3] * dir[1] + mat[6] * dir[2], + mat[1] * dir[0] + mat[4] * dir[1] + mat[7] * dir[2], + mat[2] * dir[0] + mat[5] * dir[1] + mat[8] * dir[2]}; + + mjtNum max = -FLT_MAX; + int imax = 0; + + // Seed with the previous winner (only affects tie-breaking; MuJoCo does the + // same in mjc_meshSupport). + if (obj->vertindex >= 0) { + imax = obj->vertindex; + const float* v = verts + 3 * imax; + max = local_dir[0] * v[0] + local_dir[1] * v[1] + local_dir[2] * v[2]; + } + + for (int i = 0; i < nverts; ++i) { + const float* v = verts + 3 * i; + const mjtNum vdot = + local_dir[0] * v[0] + local_dir[1] * v[1] + local_dir[2] * v[2]; + if (vdot > max) { + max = vdot; + imax = i; + } + } + obj->vertindex = imax; + + const float* v = verts + 3 * imax; + res[0] = mat[0] * v[0] + mat[1] * v[1] + mat[2] * v[2] + pos[0]; + res[1] = mat[3] * v[0] + mat[4] * v[1] + mat[5] * v[2] + pos[1]; + res[2] = mat[6] * v[0] + mat[7] * v[1] + mat[8] * v[2] + pos[2]; +} + +/* Center function used to seed the query. MuJoCo returns the geom position + (its mesh geoms are re-centered at compile time so the position is interior); + our mesh frames are arbitrary, so we use the hull's vertex centroid, stashed + in obj->size (unused for mjGEOM_MESH). */ +void DrakeMeshCenter(mjtNum res[3], const mjCCDObj* obj) { + const mjtNum* mat = obj->mat; + const mjtNum* pos = obj->pos; + const mjtNum* c = obj->size; + res[0] = mat[0] * c[0] + mat[1] * c[1] + mat[2] * c[2] + pos[0]; + res[1] = mat[3] * c[0] + mat[4] * c[1] + mat[5] * c[2] + pos[1]; + res[2] = mat[6] * c[0] + mat[7] * c[1] + mat[8] * c[2] + pos[2]; +} + +void InitCcdObj(const MujocoCcdMeshData& mesh, + const math::RigidTransformd& X_WG, mjCCDObj* obj) { + *obj = mjCCDObj{}; + obj->geom = 0; + obj->geom_type = mjGEOM_MESH; + obj->vertindex = -1; + obj->meshindex = -1; + obj->flex = -1; + obj->elem = -1; + obj->vert = -1; + obj->margin = 0; + obj->rotate[0] = 1; // Identity quaternion (unused for geoms). + + const Eigen::Matrix3d R = X_WG.rotation().matrix(); + for (int r = 0; r < 3; ++r) { + obj->pos[r] = X_WG.translation()[r]; + for (int c = 0; c < 3; ++c) { + obj->mat[3 * r + c] = R(r, c); + } + } + // The hull centroid rides in size[] for DrakeMeshCenter(). + for (int r = 0; r < 3; ++r) obj->size[r] = mesh.centroid[r]; + + obj->data.mesh.nvert = mesh.nvert; + obj->data.mesh.mesh_polynum = mesh.polynum; + obj->data.mesh.vert = mesh.vert.data(); + obj->data.mesh.mpolymapadr = mesh.polymapadr.data(); + obj->data.mesh.mpolymapnum = mesh.polymapnum.data(); + obj->data.mesh.polymap = mesh.polymap.data(); + obj->data.mesh.polyvertadr = mesh.polyvertadr.data(); + obj->data.mesh.polyvertnum = mesh.polyvertnum.data(); + obj->data.mesh.polyvert = mesh.polyvert.data(); + obj->data.mesh.polynormal = mesh.polynormal.data(); + obj->data.mesh.graph = nullptr; // Linear-scan support; no hill climbing. + obj->data.mesh.extrema = nullptr; + obj->center = &DrakeMeshCenter; + obj->support = &DrakeMeshSupport; +} + +} // namespace + +bool ComputeMujocoMultipointPenetration( + const MujocoCcdMeshData& mesh_A, const math::RigidTransformd& X_WA, + const MujocoCcdMeshData& mesh_B, const math::RigidTransformd& X_WB, + GeometryId id_A, GeometryId id_B, std::vector* scratch, + std::vector>* point_pairs) { + DRAKE_DEMAND(scratch != nullptr); + DRAKE_DEMAND(point_pairs != nullptr); + static_assert(sizeof(mjtNum) == sizeof(double), + "Drake requires MuJoCo compiled with double precision."); + InstallMujocoWarningHandler(); + + mjCCDObj obj_A, obj_B; + InitCcdObj(mesh_A, X_WA, &obj_A); + InitCcdObj(mesh_B, X_WB, &obj_B); + + mjCCDConfig config; + config.max_iterations = kCcdIterations; + config.tolerance = kCcdTolerance; + config.max_contacts = kMaxContacts; + config.dist_cutoff = 0; // Penetration only; no distance recovery. + config.npolygonmax = std::max(mesh_A.npolygonmax, mesh_B.npolygonmax); + config.nmeshdegmax = std::max(mesh_A.nmeshdegmax, mesh_B.nmeshdegmax); + const size_t bytes = + mjc_ccdSize(config.npolygonmax, config.nmeshdegmax, kCcdIterations); + // The scratch vector's element type is double so that the buffer satisfies + // MuJoCo's 8-byte alignment requirement. + scratch->resize((bytes + sizeof(double) - 1) / sizeof(double)); + config.buffer = scratch->data(); + + mjCCDStatus status{}; + const mjtNum dist = mjc_ccd(&config, &status, &obj_A, &obj_B); + // A positive distance is a definitive "separated". A distance of exactly + // zero is not: GJK also reports zero when it stalls at its starting point + // because the two vertex centroids coincide (EPA is then skipped), so that + // case falls through to the loop below, appends nothing, and is reported + // as unresolved. + if (dist > 0) return true; + + constexpr double kEps = std::numeric_limits::epsilon(); + const int first_new = static_cast(point_pairs->size()); + for (int i = 0; i < status.nx; ++i) { + // MuJoCo reports a signed distance per witness pair; negative means + // penetration. Guard against osculation just like Drake's existing + // point-pair fallback does. The comparison is written so that a NaN + // distance (EPA can degenerate on very deep penetrations) is also + // dropped rather than poisoning downstream solvers, and the witness + // points get the same defense. + const double depth = -status.dist[i]; + if (!(depth > kEps)) continue; + PenetrationAsPointPair pair; + pair.id_A = id_A; + pair.id_B = id_B; + pair.p_WCa = Eigen::Vector3d(status.x1[3 * i + 0], status.x1[3 * i + 1], + status.x1[3 * i + 2]); + pair.p_WCb = Eigen::Vector3d(status.x2[3 * i + 0], status.x2[3 * i + 1], + status.x2[3 * i + 2]); + if (!pair.p_WCa.allFinite() || !pair.p_WCb.allFinite()) continue; + // In penetration the witness on A lies inside B and vice versa, so the + // witness separation p_WCb - p_WCa has length `depth` and points out of + // B into A; normalizing it yields nhat_BA_W and satisfies Drake's + // invariant depth = (p_WCb - p_WCa) ⋅ nhat_BA_W. + pair.nhat_BA_W = (pair.p_WCb - pair.p_WCa).normalized(); + // Degenerate manifold recovery can emit coincident witness points with a + // finite plane distance, which normalizes to NaN; drop those too. + if (!pair.nhat_BA_W.allFinite()) continue; + pair.depth = depth; + // Degenerate clipping can also emit (near-)coincident contact points. + // MuJoCo's regularized solver tolerates the duplicated constraint; + // Drake's contact solvers should not be handed a singular manifold, so + // keep only the first of any coincident points. + bool duplicate = false; + for (int j = first_new; j < static_cast(point_pairs->size()); ++j) { + if (((*point_pairs)[j].p_WCa - pair.p_WCa).squaredNorm() < 1e-16) { + duplicate = true; + break; + } + } + if (duplicate) continue; + point_pairs->push_back(std::move(pair)); + } + return static_cast(point_pairs->size()) > first_new; +} + +} // namespace internal +} // namespace geometry +} // namespace drake diff --git a/geometry/proximity/mujoco_ccd_penetration.h b/geometry/proximity/mujoco_ccd_penetration.h new file mode 100644 index 000000000000..ebbfef672dac --- /dev/null +++ b/geometry/proximity/mujoco_ccd_penetration.h @@ -0,0 +1,58 @@ +#pragma once + +#include + +#include "drake/geometry/geometry_ids.h" +#include "drake/geometry/proximity/mujoco_ccd_mesh_data.h" +#include "drake/geometry/query_results/penetration_as_point_pair.h" +#include "drake/math/rigid_transform.h" + +namespace drake { +namespace geometry { +namespace internal { + +/* Computes the penetration between two convex meshes using MuJoCo's native + convex collision detection: GJK decides whether the two hulls penetrate, EPA + finds the penetration direction, and — when the contact involves a face of at + least one mesh — a multi-point contact manifold is recovered by clipping the + two participating faces against each other. Face-face and edge-face contacts + produce up to four contact points (MuJoCo prunes larger clipped polygons to + the maximum-area quadrilateral); vertex-face contacts produce the single + deepest point, matching Drake's existing single-point behavior. + + Each returned point pair reports its own depth, so contact points on a + slightly tilted face are graded rather than sharing one depth. + + Non-penetrating geometries append nothing. Points with depth below machine + epsilon are dropped, mirroring the osculation guard in Drake's existing + point-pair fallback. + + @param mesh_A Precomputed collision data of geometry A's convex hull. + @param X_WA Pose of geometry A in the world frame. + @param mesh_B Precomputed collision data of geometry B's convex hull. + @param X_WB Pose of geometry B in the world frame. + @param id_A Reported as PenetrationAsPointPair::id_A. + @param id_B Reported as PenetrationAsPointPair::id_B. + @param scratch Reusable scratch memory for MuJoCo's polytope and + clipping buffers; resized as needed. Passing the same + vector across calls avoids reallocation. + @param point_pairs The results are appended to this vector, in a + deterministic order for fixed inputs. + @returns true when MuJoCo resolved the pair: the hulls are separated (nothing + is appended) or at least one usable contact point was appended. + Returns false, appending nothing, when MuJoCo's GJK/EPA degenerated + on the pair. That happens when the two hulls' vertex centroids + coincide (GJK then stalls at its starting point and reports a + distance of exactly zero without running EPA) and when EPA returns + non-finite witness points under very deep penetration. The caller + should then fall back to another algorithm rather than report no + contact for a penetrating pair. */ +bool ComputeMujocoMultipointPenetration( + const MujocoCcdMeshData& mesh_A, const math::RigidTransformd& X_WA, + const MujocoCcdMeshData& mesh_B, const math::RigidTransformd& X_WB, + GeometryId id_A, GeometryId id_B, std::vector* scratch, + std::vector>* point_pairs); + +} // namespace internal +} // namespace geometry +} // namespace drake diff --git a/geometry/proximity/test/mujoco_ccd_penetration_test.cc b/geometry/proximity/test/mujoco_ccd_penetration_test.cc new file mode 100644 index 000000000000..0e537db6bf2a --- /dev/null +++ b/geometry/proximity/test/mujoco_ccd_penetration_test.cc @@ -0,0 +1,360 @@ +#include "drake/geometry/proximity/mujoco_ccd_penetration.h" + +#include +#include +#include + +#include + +#include "drake/common/test_utilities/eigen_matrix_compare.h" +#include "drake/geometry/proximity/mujoco_ccd_mesh_data.h" +#include "drake/geometry/proximity/polygon_surface_mesh.h" +#include "drake/math/rigid_transform.h" +#include "drake/math/rotation_matrix.h" + +namespace drake { +namespace geometry { +namespace internal { +namespace { + +using Eigen::Vector3d; +using math::RigidTransformd; +using math::RotationMatrixd; + +/* Makes an axis-aligned box [-h, h]³ (scaled per axis) as a + PolygonSurfaceMesh with six quadrilateral faces, wound counterclockwise when + viewed from outside — the same convention MakeConvexHull() produces. */ +PolygonSurfaceMesh MakeBoxMesh(const Vector3d& h) { + std::vector vertices = { + {-h.x(), -h.y(), -h.z()}, {h.x(), -h.y(), -h.z()}, {h.x(), h.y(), -h.z()}, + {-h.x(), h.y(), -h.z()}, {-h.x(), -h.y(), h.z()}, {h.x(), -h.y(), h.z()}, + {h.x(), h.y(), h.z()}, {-h.x(), h.y(), h.z()}}; + // clang-format off + std::vector face_data = { + 4, 4, 5, 6, 7, // +z + 4, 0, 3, 2, 1, // -z + 4, 1, 2, 6, 5, // +x + 4, 0, 4, 7, 3, // -x + 4, 2, 3, 7, 6, // +y + 4, 0, 1, 5, 4}; // -y + // clang-format on + return PolygonSurfaceMesh(std::move(face_data), std::move(vertices)); +} + +/* The same box, but with each quad split into two triangles; exercises the + quantized-normal merge in MakeMujocoCcdMeshData(). */ +PolygonSurfaceMesh MakeTriangulatedBoxMesh(const Vector3d& h) { + const PolygonSurfaceMesh quads = MakeBoxMesh(h); + std::vector vertices; + for (int v = 0; v < quads.num_vertices(); ++v) { + vertices.push_back(quads.vertex(v)); + } + std::vector face_data; + for (int f = 0; f < quads.num_faces(); ++f) { + const SurfacePolygon face = quads.element(f); + const int a = face.vertex(0), b = face.vertex(1), c = face.vertex(2), + d = face.vertex(3); + face_data.insert(face_data.end(), {3, a, b, c}); + face_data.insert(face_data.end(), {3, a, c, d}); + } + return PolygonSurfaceMesh(std::move(face_data), std::move(vertices)); +} + +GTEST_TEST(MujocoCcdMeshDataTest, CubeTables) { + const MujocoCcdMeshData data = + MakeMujocoCcdMeshData(MakeBoxMesh(Vector3d::Ones())); + EXPECT_EQ(data.nvert, 8); + EXPECT_EQ(data.polynum, 6); + EXPECT_EQ(data.npolygonmax, 4); + // Every cube vertex borders exactly three faces. + EXPECT_EQ(data.nmeshdegmax, 3); + for (int v = 0; v < data.nvert; ++v) { + EXPECT_EQ(data.polymapnum[v], 3); + } + EXPECT_TRUE(CompareMatrices(data.centroid, Vector3d::Zero(), 1e-14)); + // Each polygon's stored winding must produce the stored outward normal + // (right-hand rule), and the normal must have unit length. + for (int p = 0; p < data.polynum; ++p) { + const Vector3d n(data.polynormal[3 * p], data.polynormal[3 * p + 1], + data.polynormal[3 * p + 2]); + EXPECT_NEAR(n.norm(), 1.0, 1e-12); + const int adr = data.polyvertadr[p]; + auto vert = [&](int i) { + const int vi = data.polyvert[adr + i]; + return Vector3d(data.vert[3 * vi], data.vert[3 * vi + 1], + data.vert[3 * vi + 2]); + }; + const Vector3d winding_normal = + (vert(1) - vert(0)).cross(vert(2) - vert(1)).normalized(); + // The stored normal is computed from the polygon's own vertices (as + // MuJoCo's MakePolygonNormals does), so it matches the winding exactly. + EXPECT_GT(winding_normal.dot(n), 1.0 - 1e-12); + } +} + +GTEST_TEST(MujocoCcdMeshDataTest, TriangulatedCubeMerges) { + const MujocoCcdMeshData data = + MakeMujocoCcdMeshData(MakeTriangulatedBoxMesh(Vector3d::Ones())); + // The twelve coplanar triangles merge back into six quadrilaterals. + EXPECT_EQ(data.polynum, 6); + EXPECT_EQ(data.npolygonmax, 4); + EXPECT_EQ(data.nmeshdegmax, 3); +} + +class MujocoCcdPenetrationTest : public ::testing::Test { + protected: + std::vector> Compute( + const MujocoCcdMeshData& mesh_A, const RigidTransformd& X_WA, + const MujocoCcdMeshData& mesh_B, const RigidTransformd& X_WB) { + std::vector> pairs; + resolved_ = ComputeMujocoMultipointPenetration( + mesh_A, X_WA, mesh_B, X_WB, id_A_, id_B_, &scratch_, &pairs); + return pairs; + } + + static void CheckInvariants( + const std::vector>& pairs) { + for (const auto& pair : pairs) { + EXPECT_GT(pair.depth, 0.0); + EXPECT_NEAR(pair.nhat_BA_W.norm(), 1.0, 1e-12); + EXPECT_NEAR((pair.p_WCb - pair.p_WCa).dot(pair.nhat_BA_W), pair.depth, + 1e-9); + } + } + + const GeometryId id_A_{GeometryId::get_new_id()}; + const GeometryId id_B_{GeometryId::get_new_id()}; + std::vector scratch_; + // The return value of the most recent Compute() call. + bool resolved_{false}; +}; + +/* Two unit cubes in face-face contact, laterally offset so the overlap + region is a proper rectangle: the manifold must contain four points at the + corners of the overlap rectangle, each with (approximately) the full + penetration depth. This is the case where MuJoCo's manifold differs the most + from a single-point contact. */ +TEST_F(MujocoCcdPenetrationTest, FaceFaceContact) { + const MujocoCcdMeshData cube = + MakeMujocoCcdMeshData(MakeBoxMesh(Vector3d::Ones())); + const double kDepth = 1e-3; + const RigidTransformd X_WA; // Bottom cube: z ∈ [-1, 1]. + // Top cube: z ∈ [1 - kDepth, 3 - kDepth], shifted in x and y. + const RigidTransformd X_WB(Vector3d(0.4, 0.3, 2.0 - kDepth)); + + const auto pairs = Compute(cube, X_WA, cube, X_WB); + EXPECT_TRUE(resolved_); + ASSERT_EQ(ssize(pairs), 4); + CheckInvariants(pairs); + + // The overlap rectangle is x ∈ [-0.6, 1] × y ∈ [-0.7, 1] at z ≈ 1. + // (Vertices pass through single precision, so expect ~1e-7 noise.) + const double kTol = 1e-6; + std::vector corners = { + {-0.6, -0.7, 0}, {1, -0.7, 0}, {1, 1, 0}, {-0.6, 1, 0}}; + for (const auto& pair : pairs) { + EXPECT_NEAR(pair.depth, kDepth, 1e-8); + // Normal points out of B (top cube) into A (bottom cube): -z. + EXPECT_TRUE(CompareMatrices(pair.nhat_BA_W, -Vector3d::UnitZ(), 1e-9)); + // Witness on A is on the bottom cube's top face (z = 1); witness on B on + // the top cube's bottom face (z = 1 - kDepth). + EXPECT_NEAR(pair.p_WCa.z(), 1.0, kTol); + EXPECT_NEAR(pair.p_WCb.z(), 1.0 - kDepth, kTol); + // Each contact lies at one of the overlap-rectangle corners. + const auto is_close = [&](const Vector3d& c) { + return (pair.p_WCb.head<2>() - c.head<2>()).norm() < 1e-5; + }; + EXPECT_TRUE(std::any_of(corners.begin(), corners.end(), is_close)) + << "Unexpected contact at (" << pair.p_WCb.x() << ", " << pair.p_WCb.y() + << ")"; + } +} + +/* A cube rotated 45° about z on top of another: the clipped contact polygon + is an octagon, which MuJoCo prunes to its maximum-area quadrilateral. */ +TEST_F(MujocoCcdPenetrationTest, RotatedFaceFaceContactPrunesToFour) { + const MujocoCcdMeshData cube = + MakeMujocoCcdMeshData(MakeBoxMesh(Vector3d::Ones())); + const double kDepth = 1e-3; + const RigidTransformd X_WA; + const RigidTransformd X_WB(RotationMatrixd::MakeZRotation(M_PI / 4), + Vector3d(0, 0, 2.0 - kDepth)); + + const auto pairs = Compute(cube, X_WA, cube, X_WB); + ASSERT_EQ(ssize(pairs), 4); + CheckInvariants(pairs); + for (const auto& pair : pairs) { + EXPECT_NEAR(pair.depth, kDepth, 1e-8); + EXPECT_TRUE(CompareMatrices(pair.nhat_BA_W, -Vector3d::UnitZ(), 1e-9)); + } +} + +/* A cube rotated 45° about x rests edge-down on the other's top face: + edge-face contact produces exactly two contact points, the ends of the + penetrating edge segment. */ +TEST_F(MujocoCcdPenetrationTest, EdgeFaceContact) { + const MujocoCcdMeshData cube = + MakeMujocoCcdMeshData(MakeBoxMesh(Vector3d::Ones())); + const double kDepth = 1e-3; + const RigidTransformd X_WA; + // Rotated 45° about x, the lowest feature is an edge along x at + // z = -sqrt(2); place it kDepth below A's top face. + const RigidTransformd X_WB(RotationMatrixd::MakeXRotation(M_PI / 4), + Vector3d(0, 0, 1.0 + std::sqrt(2.0) - kDepth)); + + const auto pairs = Compute(cube, X_WA, cube, X_WB); + ASSERT_EQ(ssize(pairs), 2); + CheckInvariants(pairs); + for (const auto& pair : pairs) { + EXPECT_NEAR(pair.depth, kDepth, 1e-6); + EXPECT_NEAR(pair.p_WCb.y(), 0.0, 1e-5); + EXPECT_NEAR(std::abs(pair.p_WCb.x()), 1.0, 1e-5); + } +} + +/* A cube balanced on its corner produces a single (vertex-face) contact, + exactly like Drake's existing single-point narrowphase. */ +TEST_F(MujocoCcdPenetrationTest, VertexFaceContact) { + const MujocoCcdMeshData cube = + MakeMujocoCcdMeshData(MakeBoxMesh(Vector3d::Ones())); + const double kDepth = 1e-3; + const RigidTransformd X_WA; + // Rotate the cube so a corner points straight down. The corner (1,1,1) has + // direction (1,1,1)/sqrt(3); rotate that direction onto -z. + const Eigen::Quaterniond q = + Eigen::Quaterniond::FromTwoVectors(Vector3d(1, 1, 1), -Vector3d::UnitZ()); + const RigidTransformd X_WB(RotationMatrixd(q), + Vector3d(0, 0, 1.0 + std::sqrt(3.0) - kDepth)); + + const auto pairs = Compute(cube, X_WA, cube, X_WB); + ASSERT_EQ(ssize(pairs), 1); + CheckInvariants(pairs); + EXPECT_NEAR(pairs[0].depth, kDepth, 1e-6); + EXPECT_NEAR(pairs[0].p_WCa.x(), 0.0, 1e-5); + EXPECT_NEAR(pairs[0].p_WCa.y(), 0.0, 1e-5); +} + +/* A slightly tilted face-face contact grades the per-point depths instead of + assigning every point the deepest value. */ +TEST_F(MujocoCcdPenetrationTest, TiltedContactHasGradedDepths) { + const MujocoCcdMeshData cube = + MakeMujocoCcdMeshData(MakeBoxMesh(Vector3d::Ones())); + // The tilt is small enough that MuJoCo still treats the two faces as + // parallel (its face-alignment tolerance is about 5 degrees), so the contact + // is a face-face manifold rather than an edge-face pair. + const double kTilt = 2e-4; + const RigidTransformd X_WA; + const RigidTransformd X_WB(RotationMatrixd::MakeXRotation(kTilt), + Vector3d(0, 0, 2.0 - 1e-3)); + + const auto pairs = Compute(cube, X_WA, cube, X_WB); + ASSERT_EQ(ssize(pairs), 4); + CheckInvariants(pairs); + double min_depth = 1.0, max_depth = 0.0; + for (const auto& pair : pairs) { + min_depth = std::min(min_depth, pair.depth); + max_depth = std::max(max_depth, pair.depth); + } + // The tilt separates depths by roughly 2 * tan(kTilt) ≈ 4e-4. + EXPECT_GT(max_depth - min_depth, 1e-4); +} + +TEST_F(MujocoCcdPenetrationTest, SeparatedProducesNothing) { + const MujocoCcdMeshData cube = + MakeMujocoCcdMeshData(MakeBoxMesh(Vector3d::Ones())); + const RigidTransformd X_WA; + const RigidTransformd X_WB(Vector3d(0, 0, 2.5)); + EXPECT_TRUE(Compute(cube, X_WA, cube, X_WB).empty()); + // Separation is a definitive answer. + EXPECT_TRUE(resolved_); +} + +/* Two identical hulls at the same pose have coincident vertex centroids, so + MuJoCo's GJK stalls at its starting point and reports a distance of exactly + zero without running EPA. The function must report that it could not resolve + the pair (appending nothing) so that the caller can fall back to another + algorithm instead of reporting no contact for fully overlapping hulls. */ +TEST_F(MujocoCcdPenetrationTest, CoincidentHullsAreUnresolved) { + const MujocoCcdMeshData cube = + MakeMujocoCcdMeshData(MakeBoxMesh(Vector3d::Ones())); + const RigidTransformd X_WA; + EXPECT_TRUE(Compute(cube, X_WA, cube, X_WA).empty()); + EXPECT_FALSE(resolved_); +} + +TEST_F(MujocoCcdPenetrationTest, Deterministic) { + const MujocoCcdMeshData cube = + MakeMujocoCcdMeshData(MakeBoxMesh(Vector3d(0.7, 1.1, 0.9))); + const RigidTransformd X_WA(RotationMatrixd::MakeYRotation(0.1), + Vector3d(0.05, -0.02, 0)); + const RigidTransformd X_WB(RotationMatrixd::MakeXRotation(-0.07), + Vector3d(0.3, 0.2, 1.7)); + const auto pairs1 = Compute(cube, X_WA, cube, X_WB); + const auto pairs2 = Compute(cube, X_WA, cube, X_WB); + ASSERT_EQ(ssize(pairs1), ssize(pairs2)); + for (int i = 0; i < ssize(pairs1); ++i) { + EXPECT_EQ(pairs1[i].depth, pairs2[i].depth); + EXPECT_EQ(pairs1[i].p_WCa, pairs2[i].p_WCa); + EXPECT_EQ(pairs1[i].p_WCb, pairs2[i].p_WCb); + EXPECT_EQ(pairs1[i].nhat_BA_W, pairs2[i].nhat_BA_W); + } +} + +/* An inward-wound input (every face loop reversed, e.g. a cached hull + topology re-instantiated at a mirroring scale) is detected via its negative + enclosed volume and produces the same manifold as the outward-wound cube. */ +TEST_F(MujocoCcdPenetrationTest, InwardWoundInputIsRepaired) { + const PolygonSurfaceMesh quads = MakeBoxMesh(Vector3d::Ones()); + std::vector vertices; + for (int v = 0; v < quads.num_vertices(); ++v) { + vertices.push_back(quads.vertex(v)); + } + std::vector face_data; + for (int f = 0; f < quads.num_faces(); ++f) { + const SurfacePolygon face = quads.element(f); + face_data.push_back(face.num_vertices()); + for (int i = face.num_vertices() - 1; i >= 0; --i) { + face_data.push_back(face.vertex(i)); + } + } + const MujocoCcdMeshData inward_cube = MakeMujocoCcdMeshData( + PolygonSurfaceMesh(std::move(face_data), std::move(vertices))); + const MujocoCcdMeshData cube = + MakeMujocoCcdMeshData(MakeBoxMesh(Vector3d::Ones())); + const double kDepth = 1e-3; + const RigidTransformd X_WA; + const RigidTransformd X_WB(Vector3d(0.4, 0.3, 2.0 - kDepth)); + const auto pairs = Compute(inward_cube, X_WA, inward_cube, X_WB); + const auto expected = Compute(cube, X_WA, cube, X_WB); + ASSERT_EQ(ssize(pairs), 4); + CheckInvariants(pairs); + ASSERT_EQ(ssize(expected), 4); + for (int i = 0; i < 4; ++i) { + EXPECT_TRUE(CompareMatrices(pairs[i].p_WCa, expected[i].p_WCa, 1e-12)); + EXPECT_TRUE( + CompareMatrices(pairs[i].nhat_BA_W, expected[i].nhat_BA_W, 1e-12)); + EXPECT_NEAR(pairs[i].depth, expected[i].depth, 1e-12); + } +} + +/* The triangulated cube (after the quantized-normal merge) must behave + identically to the natively-quadrilateral cube. */ +TEST_F(MujocoCcdPenetrationTest, TriangulatedCubeMatchesQuadCube) { + const MujocoCcdMeshData quad_cube = + MakeMujocoCcdMeshData(MakeBoxMesh(Vector3d::Ones())); + const MujocoCcdMeshData tri_cube = + MakeMujocoCcdMeshData(MakeTriangulatedBoxMesh(Vector3d::Ones())); + const double kDepth = 1e-3; + const RigidTransformd X_WA; + const RigidTransformd X_WB(Vector3d(0.4, 0.3, 2.0 - kDepth)); + + const auto pairs_quad = Compute(quad_cube, X_WA, quad_cube, X_WB); + const auto pairs_tri = Compute(tri_cube, X_WA, tri_cube, X_WB); + ASSERT_EQ(ssize(pairs_quad), 4); + ASSERT_EQ(ssize(pairs_tri), 4); +} + +} // namespace +} // namespace internal +} // namespace geometry +} // namespace drake From 94c658ab299ec713a6e27ab2e398908a737e0c71 Mon Sep 17 00:00:00 2001 From: Xuchen Han Date: Mon, 24 Aug 2026 16:26:25 -0700 Subject: [PATCH 3/5] [multibody] Draw one meshcat arrow for each point contact 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. --- multibody/meshcat/BUILD.bazel | 8 +++ multibody/meshcat/point_contact_visualizer.cc | 20 +++++- multibody/meshcat/point_contact_visualizer.h | 5 +- .../test/point_contact_visualizer_test.cc | 65 +++++++++++++++++++ 4 files changed, 93 insertions(+), 5 deletions(-) create mode 100644 multibody/meshcat/test/point_contact_visualizer_test.cc diff --git a/multibody/meshcat/BUILD.bazel b/multibody/meshcat/BUILD.bazel index 64a74d6c0812..51e7adf2abb6 100644 --- a/multibody/meshcat/BUILD.bazel +++ b/multibody/meshcat/BUILD.bazel @@ -71,6 +71,14 @@ drake_cc_googletest( ], ) +drake_cc_googletest( + name = "point_contact_visualizer_test", + deps = [ + ":point_contact_visualizer", + "@msgpack_internal//:msgpack", + ], +) + drake_cc_library( name = "contact_visualizer_params", hdrs = ["contact_visualizer_params.h"], diff --git a/multibody/meshcat/point_contact_visualizer.cc b/multibody/meshcat/point_contact_visualizer.cc index bd966cc7885f..9e2345ed72b5 100644 --- a/multibody/meshcat/point_contact_visualizer.cc +++ b/multibody/meshcat/point_contact_visualizer.cc @@ -1,5 +1,7 @@ #include "drake/multibody/meshcat/point_contact_visualizer.h" +#include +#include #include #include @@ -44,11 +46,23 @@ void PointContactVisualizer::Update( status.active = false; } - // Process the new contacts to find the active ones. + // Process the new contacts to find the active ones. A body pair can have + // more than one contact at a time (a body with several collision geometries, + // or a multi-point contact manifold), so the repeats are numbered and each + // contact gets its own meshcat path instead of overdrawing the first one. + std::unordered_map pair_counts; for (const PointContactVisualizerItem& item : items) { - // Find our meshcat state for this contact pair. - const std::string path = + // Find our meshcat state for this contact. + std::string path = fmt::format("{}/{}+{}", params_.prefix, item.body_A, item.body_B); + const int repeat = pair_counts[path]++; + if (repeat > 0) { + // The separator is '#' rather than '+' so that a numbered path can never + // coincide with the base path of some other body pair. (Body names that + // contain '+' could already make two pairs share a base path; that is a + // pre-existing limitation of the naming scheme.) + path = fmt::format("{}#{}", path, repeat); + } VisibilityStatus& status = FindOrAdd(path); // Decide whether the contact should be shown. diff --git a/multibody/meshcat/point_contact_visualizer.h b/multibody/meshcat/point_contact_visualizer.h index 279b9e7de1f1..5d302966e90a 100644 --- a/multibody/meshcat/point_contact_visualizer.h +++ b/multibody/meshcat/point_contact_visualizer.h @@ -34,8 +34,9 @@ struct PointContactVisualizerItem { It draws double-sided arrows at the location of the contact force with length scaled by the magnitude of the contact force. -This is unit tested via contact_visualizer_test overall; there is currently no -point-contact-specific unit test. +This is unit tested via contact_visualizer_test overall; the arrow naming for +body pairs with several simultaneous contacts is covered by +point_contact_visualizer_test. */ class PointContactVisualizer { public: diff --git a/multibody/meshcat/test/point_contact_visualizer_test.cc b/multibody/meshcat/test/point_contact_visualizer_test.cc new file mode 100644 index 000000000000..789f0a795f72 --- /dev/null +++ b/multibody/meshcat/test/point_contact_visualizer_test.cc @@ -0,0 +1,65 @@ +#include "drake/multibody/meshcat/point_contact_visualizer.h" + +#include +#include +#include + +#include +#include + +#include "drake/geometry/meshcat_types_internal.h" + +namespace drake { +namespace multibody { +namespace meshcat { +namespace { + +using Eigen::Vector3d; +using geometry::Meshcat; + +// Helper to query meshcat whether an item is visible or not. +bool visible(const Meshcat& meshcat, std::string_view path) { + std::string property = meshcat.GetPackedProperty(path, "visible"); + msgpack::object_handle oh = msgpack::unpack(property.data(), property.size()); + auto data = oh.get().as>(); + return data.value; +} + +// A body pair can report several point contacts at once (a body with several +// collision geometries, or a multi-point contact manifold). Each contact must +// get its own arrow instead of overdrawing the first one. +GTEST_TEST(PointContactVisualizer, RepeatedBodyPair) { + auto meshcat = std::make_shared(); + const ContactVisualizerParams params{}; + internal::PointContactVisualizer visualizer(meshcat, params); + + const Vector3d force(0, 0, 10); + std::vector items; + items.push_back({"body_A", "body_B", force, Vector3d(0, 0, 0)}); + items.push_back({"body_A", "body_B", force, Vector3d(1, 0, 0)}); + items.push_back({"body_A", "body_B", force, Vector3d(0, 1, 0)}); + visualizer.Update(0, items); + + const std::string base = fmt::format("{}/body_A+body_B", params.prefix); + EXPECT_TRUE(meshcat->HasPath(base)); + EXPECT_TRUE(meshcat->HasPath(base + "#1")); + EXPECT_TRUE(meshcat->HasPath(base + "#2")); + EXPECT_FALSE(meshcat->HasPath(base + "#3")); + EXPECT_TRUE(visible(*meshcat, base)); + EXPECT_TRUE(visible(*meshcat, base + "#1")); + EXPECT_TRUE(visible(*meshcat, base + "#2")); + + // When the pair drops back to a single contact, the surplus arrows are + // hidden (not deleted, to avoid flicker if they return) and the remaining + // contact keeps the un-numbered path. + items.erase(items.begin() + 1, items.end()); + visualizer.Update(1, items); + EXPECT_TRUE(visible(*meshcat, base)); + EXPECT_FALSE(visible(*meshcat, base + "#1")); + EXPECT_FALSE(visible(*meshcat, base + "#2")); +} + +} // namespace +} // namespace meshcat +} // namespace multibody +} // namespace drake From b4079751cfcc185f6b4c94bee1db9227e46e7bb1 Mon Sep 17 00:00:00 2001 From: Xuchen Han Date: Sat, 22 Aug 2026 11:55:24 -0700 Subject: [PATCH 4/5] [geometry] Connect MuJoCo multi-point contact to the proximity engine 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. --- bindings/generated_docstrings/geometry.h | 36 +++ .../generated_docstrings/geometry_proximity.h | 2 + .../pydrake/geometry/test/scene_graph_test.py | 2 + geometry/BUILD.bazel | 15 ++ geometry/geometry_state.cc | 15 ++ geometry/proximity/BUILD.bazel | 4 + .../penetration_as_point_pair_callback.cc | 45 +++- .../penetration_as_point_pair_callback.h | 35 ++- geometry/proximity_engine.cc | 98 +++++++- geometry/proximity_properties.cc | 19 ++ geometry/proximity_properties.h | 16 ++ geometry/query_object.h | 7 + geometry/scene_graph_config.cc | 9 + geometry/scene_graph_config.h | 22 ++ geometry/test/geometry_state_test.cc | 41 ++++ .../test/mujoco_multipoint_engine_test.cc | 214 ++++++++++++++++++ geometry/test/scene_graph_config_test.cc | 13 ++ 17 files changed, 576 insertions(+), 17 deletions(-) create mode 100644 geometry/test/mujoco_multipoint_engine_test.cc diff --git a/bindings/generated_docstrings/geometry.h b/bindings/generated_docstrings/geometry.h index cc508d65ec62..e986391e44c6 100644 --- a/bindings/generated_docstrings/geometry.h +++ b/bindings/generated_docstrings/geometry.h @@ -990,6 +990,31 @@ Collision Detection. Currently margin only applies to *compliant* hydroelastic contact and it does not affect point contact.)"""; } margin; + // Symbol: drake::geometry::DefaultProximityProperties::point_contact_algorithm + struct /* point_contact_algorithm */ { + // Source: drake/geometry/scene_graph_config.h + const char* doc = +R"""((Experimental) Selects the narrowphase algorithm used to compute point +contact for this geometry. There are two valid options: - +"single_point": the existing behavior; every penetrating geometry pair +reports exactly one contact point (the deepest one). - +"mujoco_multipoint": pairs where *both* geometries are Convex or Mesh +shapes (and both request this algorithm) report a contact manifold of +up to four contact points, computed with MuJoCo's native convex +collision detection (GJK/EPA plus contact-polygon clipping over the +meshes' convex hulls); each point carries its own depth. Face-face and +edge-face contacts gain the extra points; vertex-face contacts still +report one point. All other geometry pairs keep the "single_point" +behavior. A mesh whose convex hull has no volume (for example, a +planar mesh) reports a single point even when it selects this +algorithm. + +This affects QueryObject∷ComputePointPairPenetration() and the point +contact fallback of QueryObject∷ComputeContactSurfacesWithFallback() +for double-valued scene graphs. Scene graphs of other scalar types +ignore the property; those scalar types do not support penetration +queries between Convex or Mesh shapes in the first place.)"""; + } point_contact_algorithm; // Symbol: drake::geometry::DefaultProximityProperties::point_stiffness struct /* point_stiffness */ { // Source: drake/geometry/scene_graph_config.h @@ -1049,6 +1074,7 @@ R"""(See also: std::make_pair("hunt_crossley_dissipation", hunt_crossley_dissipation.doc), std::make_pair("hydroelastic_modulus", hydroelastic_modulus.doc), std::make_pair("margin", margin.doc), + std::make_pair("point_contact_algorithm", point_contact_algorithm.doc), std::make_pair("point_stiffness", point_stiffness.doc), std::make_pair("relaxation_time", relaxation_time.doc), std::make_pair("resolution_hint", resolution_hint.doc), @@ -6129,6 +6155,16 @@ values reported here are confirmed, observed worst case answers. For Mesh shapes, their convex hulls are used in this query. It is not* computationally efficient or particularly accurate. +Note: + By default every penetrating geometry pair reports exactly one + point pair (its deepest point). A Convex-Convex, Convex-%Mesh, or + Mesh-%Mesh pair where *both* geometries select the (experimental) + "mujoco_multipoint" point contact algorithm (see + DefaultProximityProperties∷point_contact_algorithm) instead + reports a contact manifold of *up to four* point pairs for ``T`` = + ``double``, each with its own depth, ordered deterministically for + fixed poses. + Raises: RuntimeError if a Shape-Shape pair is in collision and indicated as ``throws`` in the support table above.)"""; diff --git a/bindings/generated_docstrings/geometry_proximity.h b/bindings/generated_docstrings/geometry_proximity.h index 1695432186e1..bf5c7e8122c3 100644 --- a/bindings/generated_docstrings/geometry_proximity.h +++ b/bindings/generated_docstrings/geometry_proximity.h @@ -49,6 +49,8 @@ // #include "drake/geometry/proximity/mesh_to_vtk.h" // #include "drake/geometry/proximity/mesh_traits.h" // #include "drake/geometry/proximity/meshing_utilities.h" +// #include "drake/geometry/proximity/mujoco_ccd_mesh_data.h" +// #include "drake/geometry/proximity/mujoco_ccd_penetration.h" // #include "drake/geometry/proximity/obb.h" // #include "drake/geometry/proximity/obj_to_surface_mesh.h" // #include "drake/geometry/proximity/plane.h" diff --git a/bindings/pydrake/geometry/test/scene_graph_test.py b/bindings/pydrake/geometry/test/scene_graph_test.py index 1e5a03756664..9022892274c9 100644 --- a/bindings/pydrake/geometry/test/scene_graph_test.py +++ b/bindings/pydrake/geometry/test/scene_graph_test.py @@ -485,6 +485,7 @@ def test_scene_graph_config(self): hunt_crossley_dissipation=7, relaxation_time=None, # Test optionality. point_stiffness=9, + point_contact_algorithm="mujoco_multipoint", ) param_init_scene_graph = mut.SceneGraphConfig( default_proximity_properties=param_init_props @@ -493,6 +494,7 @@ def test_scene_graph_config(self): got_props = param_init_scene_graph.default_proximity_properties self.assertEqual(got_props.relaxation_time, None) self.assertEqual(got_props.point_stiffness, 9) + self.assertEqual(got_props.point_contact_algorithm, "mujoco_multipoint") @numpy_compare.check_all_types def test_scene_graph_renderer_with_context(self, T): diff --git a/geometry/BUILD.bazel b/geometry/BUILD.bazel index 19145b387e9c..27dbbca2e867 100644 --- a/geometry/BUILD.bazel +++ b/geometry/BUILD.bazel @@ -93,6 +93,7 @@ drake_cc_library( "//math", ], implementation_deps = [ + ":proximity_properties", ":read_obj", ":utilities", "//geometry/proximity", @@ -102,6 +103,7 @@ drake_cc_library( "//geometry/proximity:distance_to_shape_callback", "//geometry/proximity:find_collision_candidates_callback", "//geometry/proximity:hydroelastic_calculator", + "//geometry/proximity:mujoco_ccd_mesh_data", "//geometry/proximity:penetration_as_point_pair_callback", "@fcl_internal//:fcl", "@fmt", @@ -1007,6 +1009,19 @@ drake_cc_googletest( ], ) +drake_cc_googletest( + name = "mujoco_multipoint_engine_test", + deps = [ + ":geometry_ids", + ":proximity_engine", + ":proximity_properties", + ":shape_specification", + "//common:memory_file", + "//common/test_utilities:expect_throws_message", + "//math", + ], +) + drake_cc_googletest( name = "kinematics_vector_test", deps = [ diff --git a/geometry/geometry_state.cc b/geometry/geometry_state.cc index a7687f32cbad..56b604b84728 100644 --- a/geometry/geometry_state.cc +++ b/geometry/geometry_state.cc @@ -45,6 +45,7 @@ using internal::kHcDissipation; using internal::kHydroGroup; using internal::kMargin; using internal::kMaterialGroup; +using internal::kPointContactAlgorithm; using internal::kPointStiffness; using internal::kRelaxationTime; using internal::kRezHint; @@ -251,6 +252,14 @@ bool BackfillDefaults(ProximityProperties* properties, defaults.hunt_crossley_dissipation); result |= backfill(kMaterialGroup, kRelaxationTime, defaults.relaxation_time); result |= backfill(kMaterialGroup, kPointStiffness, defaults.point_stiffness); + // A geometry without the point contact algorithm property already behaves + // as "single_point", so that default is not written; writing it would turn + // ApplyProximityDefaults() into a modification (and a re-processing of the + // geometry) for otherwise fully specified properties. + if (defaults.point_contact_algorithm != internal::kSinglePointAlgorithm) { + result |= backfill(kMaterialGroup, kPointContactAlgorithm, + std::make_optional(defaults.point_contact_algorithm)); + } result |= backfill(kHydroGroup, kMargin, defaults.margin); if (defaults.static_friction.has_value()) { // DefaultProximityProperties::ValidateOrThrow() enforces invariants on @@ -1306,6 +1315,12 @@ void GeometryState::AssignRole(SourceId source_id, GeometryId geometry_id, InternalGeometry& geometry = ValidateRoleAssign(source_id, geometry_id, Role::kProximity, assign); + // An unrecognized point contact algorithm is rejected here, before anything + // has been modified, so that a bad value cannot leave this GeometryState + // and its proximity engine in disagreement. The check covers every shape, + // although the engine only consults the property for Convex and Mesh. + internal::GetPointContactAlgorithmOrThrow(properties); + geometry_version_.modify_proximity(); switch (assign) { case RoleAssign::kNew: { diff --git a/geometry/proximity/BUILD.bazel b/geometry/proximity/BUILD.bazel index 9a2d84201c8e..e76c6a22323e 100644 --- a/geometry/proximity/BUILD.bazel +++ b/geometry/proximity/BUILD.bazel @@ -885,6 +885,7 @@ drake_cc_library( deps = [ ":collision_filter", ":distance_to_point_callback", + ":mujoco_ccd_mesh_data", "//common:default_scalars", "//common:drake_export", "//common:nice_type_name", @@ -892,6 +893,9 @@ drake_cc_library( "//geometry/query_results:signed_distance_to_point", "@fcl_internal//:fcl", ], + implementation_deps = [ + ":mujoco_ccd_penetration", + ], ) drake_cc_library( diff --git a/geometry/proximity/penetration_as_point_pair_callback.cc b/geometry/proximity/penetration_as_point_pair_callback.cc index 76f7171921da..3c0917cc667f 100644 --- a/geometry/proximity/penetration_as_point_pair_callback.cc +++ b/geometry/proximity/penetration_as_point_pair_callback.cc @@ -7,6 +7,7 @@ #include "drake/common/eigen_types.h" #include "drake/common/nice_type_name.h" #include "drake/geometry/proximity/distance_to_point_callback.h" +#include "drake/geometry/proximity/mujoco_ccd_penetration.h" #include "drake/geometry/query_results/signed_distance_to_point.h" namespace drake { @@ -392,10 +393,7 @@ bool Callback(fcl::CollisionObjectd* fcl_object_A_ptr, // Since we want *all* collisions, we return false. if (!can_collide) return false; - auto result = MaybeMakePointPair(fcl_object_A_ptr, fcl_object_B_ptr, data); - if (result.has_value()) { - data.point_pairs.push_back(std::move(*result)); - } + MakePointPairs(fcl_object_A_ptr, fcl_object_B_ptr, data, &data.point_pairs); return false; } @@ -453,8 +451,45 @@ std::optional> MaybeMakePointPair( return {}; } +template +void MakePointPairs(fcl::CollisionObjectd* fcl_object_A_ptr, + fcl::CollisionObjectd* fcl_object_B_ptr, + const CallbackData& data, + std::vector>* point_pairs) { + DRAKE_DEMAND(point_pairs != nullptr); + // The MuJoCo multi-point algorithm is only implemented for doubles; other + // scalars always take the single-point path below. + if constexpr (std::is_same_v) { + if (data.mujoco_ccd_geometries != nullptr) { + GeometryId id_A = EncodedData(*fcl_object_A_ptr).id(); + GeometryId id_B = EncodedData(*fcl_object_B_ptr).id(); + // Match MaybeMakePointPair's canonical ordering of the pair. + if (id_B < id_A) std::swap(id_A, id_B); + const MujocoCcdGeometries& catalog = *data.mujoco_ccd_geometries; + auto iter_A = catalog.find(id_A); + auto iter_B = iter_A != catalog.end() ? catalog.find(id_B) : iter_A; + if (iter_A != catalog.end() && iter_B != catalog.end()) { + const bool resolved = ComputeMujocoMultipointPenetration( + *iter_A->second, data.X_WGs.at(id_A), *iter_B->second, + data.X_WGs.at(id_B), id_A, id_B, &data.mujoco_ccd_scratch, + point_pairs); + if (resolved) return; + // MuJoCo's GJK/EPA degenerated on this pair (for example, the two + // hulls' vertex centroids coincide, or EPA returned non-finite + // witness points under very deep penetration). Fall through to the + // single-point algorithm so that a penetrating pair is never left + // unreported. + } + } + } + auto result = MaybeMakePointPair(fcl_object_A_ptr, fcl_object_B_ptr, data); + if (result.has_value()) { + point_pairs->push_back(std::move(*result)); + } +} + DRAKE_DEFINE_FUNCTION_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS( - (&Callback, &MaybeMakePointPair)); + (&Callback, &MaybeMakePointPair, &MakePointPairs)); } // namespace penetration_as_point_pair } // namespace internal diff --git a/geometry/proximity/penetration_as_point_pair_callback.h b/geometry/proximity/penetration_as_point_pair_callback.h index 8ea2cc9d82a4..77ae57b221f8 100644 --- a/geometry/proximity/penetration_as_point_pair_callback.h +++ b/geometry/proximity/penetration_as_point_pair_callback.h @@ -1,5 +1,6 @@ #pragma once +#include #include #include #include @@ -8,12 +9,21 @@ #include "drake/common/drake_export.h" #include "drake/geometry/proximity/collision_filter.h" +#include "drake/geometry/proximity/mujoco_ccd_mesh_data.h" #include "drake/geometry/query_results/penetration_as_point_pair.h" #include "drake/math/rigid_transform.h" namespace drake { namespace geometry { namespace internal { + +/* The catalog of geometries that opted into the "mujoco_multipoint" point + contact algorithm (see DefaultProximityProperties::point_contact_algorithm): + for each such Convex or Mesh geometry, the precomputed collision data of its + convex hull. */ +using MujocoCcdGeometries = + std::unordered_map>; + namespace penetration_as_point_pair DRAKE_NO_EXPORT { /* Supporting data for the detecting collision between geometries and reporting @@ -58,6 +68,15 @@ struct CallbackData { /* The results of the collision query. */ std::vector>& point_pairs; + + /* When non-null, geometry pairs where *both* ids appear in this catalog are + evaluated with MuJoCo's multi-point convex collision detection (producing + up to four PenetrationAsPointPair results per geometry pair) instead of + the single-point fcl query. Only consulted for T = double. Aliased. */ + const MujocoCcdGeometries* mujoco_ccd_geometries{nullptr}; + + /* Reusable scratch memory for the MuJoCo query. */ + mutable std::vector mujoco_ccd_scratch; }; /* Callback function for FCL's collide() function for retrieving a *single* @@ -70,12 +89,26 @@ bool Callback(fcl::CollisionObjectd* fcl_object_A_ptr, /* Given two objects that are candidates for a collision, returns the point-pair contact result. If the penetration depth turns out to be negative - (no collision), returns nullopt. */ + (no collision), returns nullopt. This is always the *single-point* result, + regardless of data.mujoco_ccd_geometries. */ template std::optional> MaybeMakePointPair( fcl::CollisionObjectd* fcl_object_A_ptr, fcl::CollisionObjectd* fcl_object_B_ptr, const CallbackData& data); +/* Given two objects that are candidates for a collision, appends their + point-pair contact results (if any) to `point_pairs`. When + data.mujoco_ccd_geometries contains both geometries (and T = double), the + pair is evaluated with MuJoCo's multi-point convex collision detection and + up to four point pairs are appended; otherwise this appends the single + MaybeMakePointPair() result. Results are appended in a deterministic order + for fixed inputs. */ +template +void MakePointPairs(fcl::CollisionObjectd* fcl_object_A_ptr, + fcl::CollisionObjectd* fcl_object_B_ptr, + const CallbackData& data, + std::vector>* point_pairs); + // clang-format off } // namespace penetration_as_point_pair // clang-format on diff --git a/geometry/proximity_engine.cc b/geometry/proximity_engine.cc index 73b63e0a9250..57db6301a1bd 100644 --- a/geometry/proximity_engine.cc +++ b/geometry/proximity_engine.cc @@ -28,7 +28,9 @@ #include "drake/geometry/proximity/find_collision_candidates_callback.h" #include "drake/geometry/proximity/hydroelastic_calculator.h" #include "drake/geometry/proximity/hydroelastic_internal.h" +#include "drake/geometry/proximity/mujoco_ccd_mesh_data.h" #include "drake/geometry/proximity/penetration_as_point_pair_callback.h" +#include "drake/geometry/proximity_properties.h" #include "drake/geometry/read_obj.h" #include "drake/geometry/utilities.h" @@ -77,6 +79,13 @@ struct ConvexHullCacheEntry { // InflateAabbForHydroelasticTypesOnly() mutates the geometry's aabb_local // in-place. std::map, shared_ptr> scaled_hulls; + // Sub-cache of MuJoCo convex collision data (see MujocoCcdMeshData), keyed + // like `scaled_hulls` and populated only for geometries whose proximity + // properties select the "mujoco_multipoint" point contact algorithm. (The + // margin component of the key is irrelevant to this data — margin only + // affects the fcl aabb — so two keys differing only in margin hold + // duplicate but equivalent entries.) + std::map, shared_ptr> ccd_data; }; class MapStringToConvexHullCache @@ -224,6 +233,7 @@ class ProximityEngine::Impl : public ShapeReifier { mesh_distance_boundary_cahe_ = other.mesh_distance_boundary_cahe_; convex_hull_cache_ = other.convex_hull_cache_; geometry_to_hull_key_ = other.geometry_to_hull_key_; + mujoco_ccd_geometries_ = other.mujoco_ccd_geometries_; dynamic_tree_.clear(); dynamic_objects_.clear(); anchored_tree_.clear(); @@ -286,6 +296,7 @@ class ProximityEngine::Impl : public ShapeReifier { engine->mesh_distance_boundary_cahe_ = this->mesh_distance_boundary_cahe_; engine->convex_hull_cache_ = this->convex_hull_cache_; engine->geometry_to_hull_key_ = this->geometry_to_hull_key_; + engine->mujoco_ccd_geometries_ = this->mujoco_ccd_geometries_; engine->distance_tolerance_ = this->distance_tolerance_; return engine; @@ -459,10 +470,46 @@ class ProximityEngine::Impl : public ShapeReifier { geometries_for_deformable_contact_.MaybeAddRigidGeometry( geometry.shape(), id, new_properties, X_WG); + // Refresh the geometry's membership in the MuJoCo multi-point contact + // catalog per the new properties. + MaybeAddMujocoCcdGeometry(id, new_properties); + // We must also update the FCL representation in case margin was updated. MaybeUpdateFclLocalAabbWithMargin(geometry, new_properties); } + // Adds (or removes) the geometry's MuJoCo convex collision data based on + // its proximity properties: after this call, the geometry is in + // mujoco_ccd_geometries_ iff it is a hull-based shape (Mesh or Convex) + // whose ("material", "point_contact_algorithm") property says + // "mujoco_multipoint". The tables are cached per (mesh source, scale) so + // geometries sharing a mesh file share them. + void MaybeAddMujocoCcdGeometry(GeometryId id, + const ProximityProperties& properties) { + // GeometryState validates the property before calling the engine; this + // check also protects direct users of the engine. + const std::string algorithm = GetPointContactAlgorithmOrThrow(properties); + mujoco_ccd_geometries_.erase(id); + auto key_iter = geometry_to_hull_key_.find(id); + if (key_iter == geometry_to_hull_key_.end()) return; + if (algorithm == kSinglePointAlgorithm) return; + const auto& [cache_key, scale_key] = key_iter->second; + ConvexHullCacheEntry& entry = convex_hull_cache_.at(cache_key); + shared_ptr& ccd = entry.ccd_data[scale_key]; + if (ccd == nullptr) { + const Vector3d scale(scale_key[0], scale_key[1], scale_key[2]); + std::vector vertices; + vertices.reserve(entry.unit_vertices->size()); + for (const Vector3d& unit_vertex : *entry.unit_vertices) { + vertices.push_back(unit_vertex.array() * scale.array()); + } + ccd = std::make_shared( + MakeMujocoCcdMeshData(PolygonSurfaceMesh( + std::vector(*entry.faces), std::move(vertices)))); + } + mujoco_ccd_geometries_[id] = ccd; + } + // Returns true if the geometry with the given Id has been registered in // `this` ProximityEngine as a deformable geometry (via // "AddDeformableGeometry()") and has not been since removed (via @@ -498,6 +545,7 @@ class ProximityEngine::Impl : public ShapeReifier { hydroelastic_geometries_.RemoveGeometry(id); geometries_for_deformable_contact_.RemoveGeometry(id); mesh_distance_boundary_cahe_.Remove(id); + mujoco_ccd_geometries_.erase(id); // Evict the convex hull cache entry for this geometry if it is the last // user. The CollisionObjectd was already destroyed above (by the inner @@ -509,6 +557,11 @@ class ProximityEngine::Impl : public ShapeReifier { if (auto cache_it = convex_hull_cache_.find(file_key); cache_it != convex_hull_cache_.end()) { auto& scaled_hulls = cache_it->second.scaled_hulls; + auto& ccd_data = cache_it->second.ccd_data; + if (auto ccd_it = ccd_data.find(scale_key); + ccd_it != ccd_data.end() && ccd_it->second.use_count() == 1) { + ccd_data.erase(ccd_it); + } if (auto hull_it = scaled_hulls.find(scale_key); hull_it != scaled_hulls.end() && hull_it->second.use_count() == 1) { scaled_hulls.erase(hull_it); @@ -847,6 +900,9 @@ class ProximityEngine::Impl : public ShapeReifier { std::vector> contacts; penetration_as_point_pair::CallbackData data{&collision_filter_, &X_WGs, &contacts}; + if (!mujoco_ccd_geometries_.empty()) { + data.mujoco_ccd_geometries = &mujoco_ccd_geometries_; + } // Perform a query of the dynamic objects against themselves. dynamic_tree_.collide(&data, penetration_as_point_pair::Callback); @@ -856,10 +912,12 @@ class ProximityEngine::Impl : public ShapeReifier { FclCollide(dynamic_tree_, anchored_tree_, &data, penetration_as_point_pair::Callback); - std::sort(contacts.begin(), contacts.end(), - [](const auto& a, const auto& b) { - return Order(a, b); - }); + // Note: stable to preserve the deterministic emission order of multiple + // contact points reported for the same geometry pair. + std::stable_sort(contacts.begin(), contacts.end(), + [](const auto& a, const auto& b) { + return Order(a, b); + }); return contacts; } @@ -950,29 +1008,35 @@ class ProximityEngine::Impl : public ShapeReifier { &X_WGs, &hydroelastic_geometries_, representation}; penetration_as_point_pair::CallbackData point_data{&collision_filter_, &X_WGs, point_pairs}; + if (!mujoco_ccd_geometries_.empty()) { + point_data.mujoco_ccd_geometries = &mujoco_ccd_geometries_; + } // As a suggestion to future thread parallelizers, make available fully // allocated and prepared vectors for results of the parallelizable steps. + // (A geometry pair can contribute more than one point pair when the + // multi-point contact algorithm is in use, hence a list per candidate.) vector>> surface_ptrs(candidates.size()); - vector>> point_pair_maybes( + vector>> point_pair_lists( candidates.size()); // TODO(rpoyner-tri): try some thread parallelism here. for (int k = 0; k < ssize(candidates); ++k) { const auto& [id0, id1] = candidates[k]; auto [result, surface] = calculator.MaybeMakeContactSurface(id0, id1); if (ContactSurfaceFailed(result)) { - auto penetration = penetration_as_point_pair::MaybeMakePointPair( - GetFclPtr(id0), GetFclPtr(id1), point_data); - if (penetration.has_value()) { - point_pair_maybes[k] = penetration; - } + penetration_as_point_pair::MakePointPairs( + GetFclPtr(id0), GetFclPtr(id1), point_data, &point_pair_lists[k]); } else if (surface != nullptr) { surface_ptrs[k] = std::move(surface); } } CullFlatten(&surface_ptrs, surfaces); DRAKE_ASSERT(IsSortedByOrder(*surfaces)); - CullFlatten(&point_pair_maybes, point_pairs); + for (auto& list : point_pair_lists) { + for (auto& point_pair : list) { + point_pairs->push_back(std::move(point_pair)); + } + } DRAKE_ASSERT(IsSortedByOrder(*point_pairs)); } @@ -1331,6 +1395,12 @@ class ProximityEngine::Impl : public ShapeReifier { geometry_to_hull_key_[static_cast(user_data)->id] = { cache_key, scale_key}; + // If the geometry's properties select the MuJoCo multi-point contact + // algorithm, derive (or reuse) the collision tables for this hull. + MaybeAddMujocoCcdGeometry( + static_cast(user_data)->id, + static_cast(user_data)->properties); + TakeShapeOwnership(fcl_convex, user_data); ProcessHydroelastic(mesh, user_data); // TODO(DamrongGuoy): Right now ProcessGeometriesForDeformableContact() @@ -1504,6 +1574,12 @@ class ProximityEngine::Impl : public ShapeReifier { // RemoveGeometry() to evict stale cache entries. std::unordered_map>> geometry_to_hull_key_{}; + + // The Mesh/Convex geometries whose proximity properties opted into the + // "mujoco_multipoint" point contact algorithm, with the precomputed + // collision data of their convex hulls (shared with convex_hull_cache_). + // See MaybeAddMujocoCcdGeometry(). + MujocoCcdGeometries mujoco_ccd_geometries_{}; }; template diff --git a/geometry/proximity_properties.cc b/geometry/proximity_properties.cc index bea386c734c1..dc6c1649c590 100644 --- a/geometry/proximity_properties.cc +++ b/geometry/proximity_properties.cc @@ -1,6 +1,7 @@ #include "drake/geometry/proximity_properties.h" #include +#include #include namespace drake { @@ -12,6 +13,9 @@ const char* const kFriction = "coulomb_friction"; const char* const kHcDissipation = "hunt_crossley_dissipation"; const char* const kRelaxationTime = "relaxation_time"; const char* const kPointStiffness = "point_contact_stiffness"; +const char* const kPointContactAlgorithm = "point_contact_algorithm"; +const char* const kSinglePointAlgorithm = "single_point"; +const char* const kMujocoMultipointAlgorithm = "mujoco_multipoint"; const char* const kHydroGroup = "hydroelastic"; const char* const kElastic = "hydroelastic_modulus"; @@ -20,6 +24,21 @@ const char* const kComplianceType = "compliance_type"; const char* const kSlabThickness = "slab_thickness"; const char* const kMargin = "margin"; +std::string GetPointContactAlgorithmOrThrow( + const ProximityProperties& properties) { + const std::string algorithm = properties.GetPropertyOrDefault( + kMaterialGroup, kPointContactAlgorithm, kSinglePointAlgorithm); + if (algorithm != kSinglePointAlgorithm && + algorithm != kMujocoMultipointAlgorithm) { + throw std::logic_error(fmt::format( + "Unrecognized ('{}', '{}') proximity property value '{}'; the " + "recognized values are '{}' and '{}'.", + kMaterialGroup, kPointContactAlgorithm, algorithm, + kSinglePointAlgorithm, kMujocoMultipointAlgorithm)); + } + return algorithm; +} + namespace { // Use a switch() statement here, to ensure the compiler sends us a reminder diff --git a/geometry/proximity_properties.h b/geometry/proximity_properties.h index bf4954ebef39..dcd976864300 100644 --- a/geometry/proximity_properties.h +++ b/geometry/proximity_properties.h @@ -40,6 +40,22 @@ extern const char* const kRelaxationTime; ///< Linear dissipation ///< property name. extern const char* const kPointStiffness; ///< Point stiffness property ///< name. +extern const char* const kPointContactAlgorithm; ///< Point contact + ///< narrowphase algorithm + ///< property name. + +/* The recognized values of the kPointContactAlgorithm property; see + DefaultProximityProperties::point_contact_algorithm for what they select. */ +extern const char* const kSinglePointAlgorithm; +extern const char* const kMujocoMultipointAlgorithm; + +/* Returns the point contact algorithm that `properties` selects: the value of + its kPointContactAlgorithm property, or kSinglePointAlgorithm when the + property is absent. + @throws std::exception if the property is present with a value other than + kSinglePointAlgorithm or kMujocoMultipointAlgorithm. */ +std::string GetPointContactAlgorithmOrThrow( + const ProximityProperties& properties); //@} diff --git a/geometry/query_object.h b/geometry/query_object.h index 93ec14cf0167..d869a2c17204 100644 --- a/geometry/query_object.h +++ b/geometry/query_object.h @@ -320,6 +320,13 @@ class QueryObject { the same. @warning For Mesh shapes, their convex hulls are used in this query. It is *not* computationally efficient or particularly accurate. + @note By default every penetrating geometry pair reports exactly one point + pair (its deepest point). A Convex-Convex, Convex-%Mesh, or + %Mesh-%Mesh pair where *both* geometries select the (experimental) + "mujoco_multipoint" point contact algorithm (see + DefaultProximityProperties::point_contact_algorithm) instead reports + a contact manifold of *up to four* point pairs for `T` = `double`, + each with its own depth, ordered deterministically for fixed poses. @throws std::exception if a Shape-Shape pair is in collision and indicated as `throws` in the support table above. */ // clang-format on diff --git a/geometry/scene_graph_config.cc b/geometry/scene_graph_config.cc index cea78e455729..72cc5558b7ae 100644 --- a/geometry/scene_graph_config.cc +++ b/geometry/scene_graph_config.cc @@ -79,6 +79,15 @@ void DefaultProximityProperties::ValidateOrThrow() const { DRAKE_ENFORCE(point_stiffness, kPositive); #undef DRAKE_ENFORCE + if (point_contact_algorithm != internal::kSinglePointAlgorithm && + point_contact_algorithm != internal::kMujocoMultipointAlgorithm) { + throw std::logic_error(fmt::format( + "Invalid scene graph configuration: 'point_contact_algorithm' ('{}') " + "must be either '{}' or '{}'.", + point_contact_algorithm, internal::kSinglePointAlgorithm, + internal::kMujocoMultipointAlgorithm)); + } + // Require either both friction quantities or neither. if (static_friction.has_value() != dynamic_friction.has_value()) { auto value_or_nullopt = [](auto x) { diff --git a/geometry/scene_graph_config.h b/geometry/scene_graph_config.h index 98172388b044..2b72811e4aa2 100644 --- a/geometry/scene_graph_config.h +++ b/geometry/scene_graph_config.h @@ -27,6 +27,7 @@ struct DefaultProximityProperties { a->Visit(DRAKE_NVP(hunt_crossley_dissipation)); a->Visit(DRAKE_NVP(relaxation_time)); a->Visit(DRAKE_NVP(point_stiffness)); + a->Visit(DRAKE_NVP(point_contact_algorithm)); ValidateOrThrow(); } /** @name Hydroelastic Contact Properties @@ -153,6 +154,27 @@ struct DefaultProximityProperties { details on Drake's defaults along with guidelines on how to estimate parameters specific to your model. */ std::optional point_stiffness{1e6}; + + /** (Experimental) Selects the narrowphase algorithm used to compute point + contact for this geometry. There are two valid options: + - "single_point": the existing behavior; every penetrating geometry pair + reports exactly one contact point (the deepest one). + - "mujoco_multipoint": pairs where *both* geometries are Convex or Mesh + shapes (and both request this algorithm) report a contact manifold of up + to four contact points, computed with MuJoCo's native convex collision + detection (GJK/EPA plus contact-polygon clipping over the meshes' convex + hulls); each point carries its own depth. Face-face and edge-face + contacts gain the extra points; vertex-face contacts still report one + point. All other geometry pairs keep the "single_point" behavior. A mesh + whose convex hull has no volume (for example, a planar mesh) reports a + single point even when it selects this algorithm. + + This affects QueryObject::ComputePointPairPenetration() and the point + contact fallback of QueryObject::ComputeContactSurfacesWithFallback() for + double-valued scene graphs. Scene graphs of other scalar types ignore the + property; those scalar types do not support penetration queries between + Convex or Mesh shapes in the first place. */ + std::string point_contact_algorithm{"single_point"}; /// @} /** Throws if the values are inconsistent. */ diff --git a/geometry/test/geometry_state_test.cc b/geometry/test/geometry_state_test.cc index 6bec779fd54d..c3e36b3445e4 100644 --- a/geometry/test/geometry_state_test.cc +++ b/geometry/test/geometry_state_test.cc @@ -53,6 +53,7 @@ using internal::kFriction; using internal::kHcDissipation; using internal::kHydroGroup; using internal::kMaterialGroup; +using internal::kPointContactAlgorithm; using internal::kPointStiffness; using internal::kRelaxationTime; using internal::kRezHint; @@ -4680,6 +4681,46 @@ TEST_F(ApplyProximityDefaultsTests, EmptyPropsEmptyDefaults) { EXPECT_FALSE(props->HasProperty(kMaterialGroup, kHcDissipation)); EXPECT_FALSE(props->HasProperty(kMaterialGroup, kRelaxationTime)); EXPECT_FALSE(props->HasProperty(kMaterialGroup, kPointStiffness)); + EXPECT_FALSE(props->HasProperty(kMaterialGroup, kPointContactAlgorithm)); +} + +TEST_F(ApplyProximityDefaultsTests, PointContactAlgorithm) { + // The default algorithm ("single_point") is already what a geometry without + // the property gets, so it is not written; otherwise every fully specified + // geometry would be modified (and re-processed) by ApplyProximityDefaults. + // Any other value is written. + ProximityProperties empty_props; + auto default_id = AddSphere("default_algorithm", &empty_props); + auto multipoint_id = AddSphere("multipoint_algorithm", &empty_props); + geometry_state_.ApplyProximityDefaults(full_defaults_, default_id); + DefaultProximityProperties multipoint_defaults = full_defaults_; + multipoint_defaults.point_contact_algorithm = "mujoco_multipoint"; + geometry_state_.ApplyProximityDefaults(multipoint_defaults, multipoint_id); + const auto* default_props = + geometry_state_.GetProximityProperties(default_id); + ASSERT_NE(default_props, nullptr); + EXPECT_FALSE( + default_props->HasProperty(kMaterialGroup, kPointContactAlgorithm)); + const auto* multipoint_props = + geometry_state_.GetProximityProperties(multipoint_id); + ASSERT_NE(multipoint_props, nullptr); + EXPECT_EQ(multipoint_props->GetProperty(kMaterialGroup, + kPointContactAlgorithm), + "mujoco_multipoint"); +} + +TEST_F(ApplyProximityDefaultsTests, BadPointContactAlgorithmIsRejected) { + // An unrecognized algorithm is rejected when the proximity role is assigned, + // for every shape (this one is a sphere, which never uses the property). + ProximityProperties bad_props; + bad_props.AddProperty(kMaterialGroup, kPointContactAlgorithm, + std::string("psychic_guesswork")); + auto instance = std::make_unique(math::RigidTransformd{}, + Sphere(0.1), "bad"); + instance->set_proximity_properties(bad_props); + DRAKE_EXPECT_THROWS_MESSAGE(geometry_state_.RegisterGeometry( + source_id_, frame_id_, std::move(instance)), + ".*point_contact_algorithm.*psychic_guesswork.*"); } TEST_F(ApplyProximityDefaultsTests, EmptyPropsFullDefaults) { diff --git a/geometry/test/mujoco_multipoint_engine_test.cc b/geometry/test/mujoco_multipoint_engine_test.cc new file mode 100644 index 000000000000..8faf2451424e --- /dev/null +++ b/geometry/test/mujoco_multipoint_engine_test.cc @@ -0,0 +1,214 @@ +/* Integration tests for the "mujoco_multipoint" point contact algorithm at + the ProximityEngine level: property-driven opt-in, the plain point-pair + query, and the point-contact fallback of the hydroelastic query. The + algorithm itself is tested in + geometry/proximity/test/mujoco_ccd_penetration_test.cc. */ + +#include +#include +#include +#include +#include + +#include + +#include "drake/common/memory_file.h" +#include "drake/common/test_utilities/expect_throws_message.h" +#include "drake/geometry/geometry_ids.h" +#include "drake/geometry/proximity_engine.h" +#include "drake/geometry/proximity_properties.h" +#include "drake/geometry/shape_specification.h" +#include "drake/math/rigid_transform.h" + +namespace drake { +namespace geometry { +namespace internal { +namespace { + +using Eigen::Vector3d; +using math::RigidTransformd; + +/* A cube spanning [-1, 1]³ as a Convex shape (from an in-memory obj). */ +Convex MakeCubeConvex() { + static const char* const kObj = R"""( +v -1 -1 -1 +v 1 -1 -1 +v 1 1 -1 +v -1 1 -1 +v -1 -1 1 +v 1 -1 1 +v 1 1 1 +v -1 1 1 +f 5 6 7 8 +f 1 4 3 2 +f 2 3 7 6 +f 1 5 8 4 +f 3 4 8 7 +f 1 2 6 5 +)"""; + return Convex(InMemoryMesh{MemoryFile(kObj, ".obj", "cube.obj")}); +} + +ProximityProperties MakeProps(const std::string& algorithm) { + ProximityProperties props; + props.AddProperty(kMaterialGroup, kPointContactAlgorithm, algorithm); + return props; +} + +class MujocoMultipointEngineTest : public ::testing::Test { + protected: + /* Adds two dynamic cube geometries in face-face contact (the top cube + penetrates the bottom cube's top face by 1 mm, laterally offset so the + overlap region is a proper rectangle), with the given per-geometry + algorithm properties. */ + void AddCubePair(const ProximityProperties& props_A, + const ProximityProperties& props_B) { + const Convex cube = MakeCubeConvex(); + X_WGs_[id_A_] = RigidTransformd(); + X_WGs_[id_B_] = RigidTransformd(Vector3d(0.4, 0.3, 2.0 - 1e-3)); + engine_.AddDynamicGeometry(cube, X_WGs_[id_A_], id_A_, props_A); + engine_.AddDynamicGeometry(cube, X_WGs_[id_B_], id_B_, props_B); + engine_.UpdateWorldPoses(X_WGs_); + } + + ProximityEngine engine_; + std::unordered_map X_WGs_; + const GeometryId id_A_{GeometryId::get_new_id()}; + const GeometryId id_B_{GeometryId::get_new_id()}; +}; + +/* When both geometries opt in, the pair reports a four-point manifold. */ +TEST_F(MujocoMultipointEngineTest, MultipointManifold) { + AddCubePair(MakeProps("mujoco_multipoint"), MakeProps("mujoco_multipoint")); + const auto pairs = engine_.ComputePointPairPenetration(X_WGs_); + ASSERT_EQ(ssize(pairs), 4); + for (const auto& pair : pairs) { + EXPECT_EQ(pair.id_A, std::min(id_A_, id_B_)); + EXPECT_EQ(pair.id_B, std::max(id_A_, id_B_)); + EXPECT_NEAR(pair.depth, 1e-3, 1e-8); + } +} + +/* Without the property, behavior is the classic single point. */ +TEST_F(MujocoMultipointEngineTest, DefaultSinglePoint) { + AddCubePair(ProximityProperties(), ProximityProperties()); + const auto pairs = engine_.ComputePointPairPenetration(X_WGs_); + ASSERT_EQ(ssize(pairs), 1); +} + +/* An explicit "single_point" property matches the default. */ +TEST_F(MujocoMultipointEngineTest, ExplicitSinglePoint) { + AddCubePair(MakeProps("single_point"), MakeProps("single_point")); + const auto pairs = engine_.ComputePointPairPenetration(X_WGs_); + ASSERT_EQ(ssize(pairs), 1); +} + +/* Both geometries must opt in; a mixed pair keeps the single point. */ +TEST_F(MujocoMultipointEngineTest, MixedOptInIsSinglePoint) { + AddCubePair(MakeProps("mujoco_multipoint"), ProximityProperties()); + const auto pairs = engine_.ComputePointPairPenetration(X_WGs_); + ASSERT_EQ(ssize(pairs), 1); +} + +/* An unrecognized algorithm value throws at registration time. */ +TEST_F(MujocoMultipointEngineTest, BadAlgorithmThrows) { + const Convex cube = MakeCubeConvex(); + DRAKE_EXPECT_THROWS_MESSAGE( + engine_.AddDynamicGeometry(cube, RigidTransformd(), id_A_, + MakeProps("hydroelastic_hopes_and_dreams")), + ".*point_contact_algorithm.*hydroelastic_hopes_and_dreams.*"); +} + +/* A non-hull shape (sphere) quietly ignores the property; its pairs use the + single-point path even against an opted-in mesh. */ +TEST_F(MujocoMultipointEngineTest, NonMeshShapeIgnoresProperty) { + const Convex cube = MakeCubeConvex(); + X_WGs_[id_A_] = RigidTransformd(); + X_WGs_[id_B_] = RigidTransformd(Vector3d(0, 0, 1.9)); + engine_.AddDynamicGeometry(cube, X_WGs_[id_A_], id_A_, + MakeProps("mujoco_multipoint")); + engine_.AddDynamicGeometry(Sphere(1.0), X_WGs_[id_B_], id_B_, + MakeProps("mujoco_multipoint")); + engine_.UpdateWorldPoses(X_WGs_); + const auto pairs = engine_.ComputePointPairPenetration(X_WGs_); + ASSERT_EQ(ssize(pairs), 1); +} + +/* Two opted-in hulls at the same pose defeat MuJoCo's GJK (their vertex + centroids coincide, see mujoco_ccd_penetration.h); the engine then falls back + to the single-point algorithm rather than reporting no contact at all. */ +TEST_F(MujocoMultipointEngineTest, CoincidentHullsFallBackToSinglePoint) { + const Convex cube = MakeCubeConvex(); + X_WGs_[id_A_] = RigidTransformd(); + X_WGs_[id_B_] = RigidTransformd(); + engine_.AddDynamicGeometry(cube, X_WGs_[id_A_], id_A_, + MakeProps("mujoco_multipoint")); + engine_.AddDynamicGeometry(cube, X_WGs_[id_B_], id_B_, + MakeProps("mujoco_multipoint")); + engine_.UpdateWorldPoses(X_WGs_); + const auto pairs = engine_.ComputePointPairPenetration(X_WGs_); + ASSERT_EQ(ssize(pairs), 1); + EXPECT_GT(pairs[0].depth, 0.0); + EXPECT_TRUE(pairs[0].p_WCa.allFinite()); + EXPECT_TRUE(pairs[0].p_WCb.allFinite()); + EXPECT_NEAR(pairs[0].nhat_BA_W.norm(), 1.0, 1e-12); +} + +/* The point-contact fallback of the hydroelastic-with-fallback query also + produces the manifold: two rigid-hydroelastic convex meshes fail to make a + contact surface and fall back to (multi-)point contact. */ +TEST_F(MujocoMultipointEngineTest, HydroelasticFallbackUsesManifold) { + ProximityProperties props = MakeProps("mujoco_multipoint"); + AddRigidHydroelasticProperties(1.0, &props); + AddCubePair(props, props); + std::vector> surfaces; + std::vector> point_pairs; + engine_.ComputeContactSurfacesWithFallback( + HydroelasticContactRepresentation::kPolygon, X_WGs_, &surfaces, + &point_pairs); + EXPECT_EQ(ssize(surfaces), 0); + ASSERT_EQ(ssize(point_pairs), 4); +} + +/* Two geometries sharing one mesh file at mirroring scales share the hull + cache entry, whose face winding was fixed by the first registrant; the + mirrored instance's tables must be re-wound outward, or every one of its + face-face manifolds silently degrades or inverts. */ +TEST_F(MujocoMultipointEngineTest, MirroredScaleSharedMesh) { + const Convex cube = MakeCubeConvex(); + const Convex mirrored(cube.source(), Eigen::Vector3d(-1, 1, 1)); + X_WGs_[id_A_] = RigidTransformd(); + X_WGs_[id_B_] = RigidTransformd(Vector3d(0.4, 0.3, 2.0 - 1e-3)); + // The plain cube registers first and populates the shared cache entry; + // the mirrored instance then reuses its topology. + engine_.AddDynamicGeometry(cube, X_WGs_[id_A_], id_A_, + MakeProps("mujoco_multipoint")); + engine_.AddDynamicGeometry(mirrored, X_WGs_[id_B_], id_B_, + MakeProps("mujoco_multipoint")); + engine_.UpdateWorldPoses(X_WGs_); + const auto pairs = engine_.ComputePointPairPenetration(X_WGs_); + ASSERT_EQ(ssize(pairs), 4); + for (const auto& pair : pairs) { + EXPECT_NEAR(pair.depth, 1e-3, 1e-8); + // The normal must point out of B (the upper, mirrored cube): -z. An + // inward-wound table would flip it or lose the manifold entirely. + EXPECT_NEAR(pair.nhat_BA_W.z(), -1.0, 1e-9); + } +} + +/* Updating properties moves a geometry in or out of the multipoint catalog. */ +TEST_F(MujocoMultipointEngineTest, PropertyUpdateTogglesAlgorithm) { + AddCubePair(MakeProps("mujoco_multipoint"), MakeProps("mujoco_multipoint")); + ASSERT_EQ(ssize(engine_.ComputePointPairPenetration(X_WGs_)), 4); + // Note: UpdateRepresentationForNewProperties requires an InternalGeometry; + // exercising it end-to-end happens through GeometryState. Here we at least + // confirm RemoveGeometry() evicts the catalog entry. + engine_.RemoveGeometry(id_B_, /* is_dynamic = */ true); + X_WGs_.erase(id_B_); + EXPECT_EQ(ssize(engine_.ComputePointPairPenetration(X_WGs_)), 0); +} + +} // namespace +} // namespace internal +} // namespace geometry +} // namespace drake diff --git a/geometry/test/scene_graph_config_test.cc b/geometry/test/scene_graph_config_test.cc index 07a059381b06..f52a05d03dac 100644 --- a/geometry/test/scene_graph_config_test.cc +++ b/geometry/test/scene_graph_config_test.cc @@ -25,6 +25,7 @@ const char* const kExampleConfig = R"""( hunt_crossley_dissipation: 7.0 relaxation_time: 8.0 point_stiffness: 9.0 + point_contact_algorithm: mujoco_multipoint )"""; GTEST_TEST(SceneGraphConfigTest, YamlTest) { @@ -39,9 +40,21 @@ GTEST_TEST(SceneGraphConfigTest, YamlTest) { EXPECT_EQ(props.hunt_crossley_dissipation, 7); EXPECT_EQ(props.relaxation_time, 8); EXPECT_EQ(props.point_stiffness, 9); + EXPECT_EQ(props.point_contact_algorithm, "mujoco_multipoint"); EXPECT_EQ("\n" + SaveYamlString(config), kExampleConfig); } +GTEST_TEST(SceneGraphConfigTest, ValidatePointContactAlgorithm) { + SceneGraphConfig config; + auto& props = config.default_proximity_properties; + props.point_contact_algorithm = "psychic_guesswork"; + DRAKE_EXPECT_THROWS_MESSAGE( + config.ValidateOrThrow(), + "Invalid scene graph configuration: 'point_contact_algorithm'" + " \\('psychic_guesswork'\\) must be either 'single_point' or" + " 'mujoco_multipoint'."); +} + GTEST_TEST(SceneGraphConfigTest, ValidDefault) { const SceneGraphConfig kDefault; EXPECT_NO_THROW(kDefault.ValidateOrThrow()); From 8d694b8d509c497b22631f9b21f1527c6a9c4020 Mon Sep 17 00:00:00 2001 From: Xuchen Han Date: Mon, 24 Aug 2026 16:50:37 -0700 Subject: [PATCH 5/5] [examples] Add a multi-point contact demonstration 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. --- .../multibody/multipoint_contact/BUILD.bazel | 15 ++ .../multibody/multipoint_contact/README.md | 107 ++++++++ .../multipoint_contact/falling_box.py | 242 ++++++++++++++++++ 3 files changed, 364 insertions(+) create mode 100644 examples/multibody/multipoint_contact/BUILD.bazel create mode 100644 examples/multibody/multipoint_contact/README.md create mode 100644 examples/multibody/multipoint_contact/falling_box.py diff --git a/examples/multibody/multipoint_contact/BUILD.bazel b/examples/multibody/multipoint_contact/BUILD.bazel new file mode 100644 index 000000000000..214fe524aa73 --- /dev/null +++ b/examples/multibody/multipoint_contact/BUILD.bazel @@ -0,0 +1,15 @@ +load("//tools/lint:lint.bzl", "add_lint_tests") +load("//tools/skylark:drake_py.bzl", "drake_py_binary") + +drake_py_binary( + name = "falling_box", + srcs = ["falling_box.py"], + deps = ["//bindings/pydrake"], + add_test_rule = True, + test_rule_args = [ + "--target_realtime_rate=0", + "--simulation_time=0.5", + ], +) + +add_lint_tests() diff --git a/examples/multibody/multipoint_contact/README.md b/examples/multibody/multipoint_contact/README.md new file mode 100644 index 000000000000..393b7cff6797 --- /dev/null +++ b/examples/multibody/multipoint_contact/README.md @@ -0,0 +1,107 @@ +# Multi-point contact between convex shapes + +This example shows what the `"mujoco_multipoint"` point contact algorithm +changes, by dropping two boxes onto a slab side by side: the box on the left +touches the slab through Drake's default single contact point, the box on the +right through a four-point contact manifold. + +## Background + +Drake's point contact model resolves each colliding geometry pair to a single +contact point, the point of deepest penetration, even when two flat faces +overlap. A box resting on a table is then supported at one point and cannot +be in static equilibrium there: the support point wanders across the bottom +face and the box rocks. The usual workaround is to decorate a body with +several small spheres so that it touches through several points (see +`examples/multibody/cylinder_with_multicontact`). + +The proximity property `("material", "point_contact_algorithm")` selects an +alternative narrowphase for point contact. Its value `"mujoco_multipoint"` +uses MuJoCo's native convex collision detection (GJK/EPA followed by clipping +the two touching faces against each other) to report a contact manifold of up +to four points, each with its own penetration depth, for pairs of `Convex` or +`Mesh` shapes. Face-face and edge-face contacts gain the extra points; +vertex-face contacts still report a single point. Two rules matter for setting +up a scene: + +- *Both* geometries of a pair must select `"mujoco_multipoint"`; a pair with + one geometry left at the default `"single_point"` reports one point. +- Only `Convex` and `Mesh` shapes are eligible. A `Box` shape is not, so this + example models its boxes as `Convex` shapes built from an in-memory OBJ file + listing the eight corners (`make_convex_box()` in `falling_box.py`). + +The property can be set per geometry, as this example does, or for a whole +scene at once through +`SceneGraphConfig.default_proximity_properties.point_contact_algorithm`. + +## The scene + +- A 1 m x 0.5 m x 0.05 m slab, a box modeled as a `Convex` shape, welded to + the world with its top face at z = 0. It selects `"mujoco_multipoint"`. +- Two identical 20 cm x 20 cm x 4 cm boxes of 0.5 kg, also `Convex` shapes. + The orange box on the left selects `"single_point"`; the blue box on the + right selects `"mujoco_multipoint"`. +- Both boxes start level, 5 cm above the slab, and fall onto it. + +The plant uses `contact_model = "point"` with a 1 ms discrete time step. + +## Running + +``` +bazel run //examples/multibody/multipoint_contact:falling_box +``` + +Open the Meshcat URL printed at startup. The contact forces are drawn as +arrows, one per contact point, so the left box shows a single arrow and the +right box shows four once it has landed. The console prints, every half +second, the number of contact points each box has with the slab, the height +of the box center above the slab in millimeters (20.00 for a box at rest on +the slab), and the box's angular speed in rad/s: + +``` + single_point box multipoint box + time contacts height |w| contacts height |w| + 0.50 1 17.46 0.242 4 20.00 0.000 + 1.00 1 16.31 0.237 4 20.00 0.000 + ... + 5.00 1 14.50 0.228 4 20.00 0.000 +``` + +Use `--simulation_time`, `--target_realtime_rate`, `--time_step`, and +`--drop_height` to change the run. + +## What to look for + +Both boxes reach the slab about a tenth of a second after the start. From +then on the two halves of the table diverge. + +The single-point box reports one contact, and that contact hops between the +corners of its bottom face from one time step to the next. The box never comes +to rest: it rocks at about 0.23 rad/s for the whole run, creeps sideways and +in yaw by a few degrees over several seconds, and sinks into the slab. Its +center, which would sit 20.00 mm above the slab at rest, reads 16.3 mm after +one second and 14.5 mm after five, so the box has sunk 5.5 mm into a slab it +should be resting on. (The contact stiffness alone would account for a few +micrometers of penetration under the box's 5 N weight.) In Meshcat the single +force arrow jumps around the bottom face. + +The multipoint box reports four contacts, one at each corner of its bottom +face, and rests at exactly 20.00 mm with zero angular velocity from the moment +it lands. Meshcat shows four steady arrows. + +The size of the single-point artifact depends on the time step; the manifold +result does not. With `--time_step=0.01` the single-point box falls straight +through the slab within half a second while the multipoint box still rests on +it. With `--time_step=0.0001` the single-point box settles to within 0.01 mm of +its rest height but still wobbles at 0.02 to 0.04 rad/s. + +Why one point is not enough: a level box supported at a single point is in +equilibrium only if that point lies directly under its center of mass. The +deepest point of a face-face overlap is a corner (whichever one a tiny +numerical tilt favors), so the normal force there tips the box toward the +opposite corner, which then becomes the deepest point, and the box rocks from +corner to corner. Each exchange of support corners lets the center of mass +fall a little before the contact catches it, so the box ratchets downward, +and a larger time step makes each exchange coarser and the sinking faster. +Four contact points spanning the overlap region support the box the way a +table does. diff --git a/examples/multibody/multipoint_contact/falling_box.py b/examples/multibody/multipoint_contact/falling_box.py new file mode 100644 index 000000000000..91d39e6d08b7 --- /dev/null +++ b/examples/multibody/multipoint_contact/falling_box.py @@ -0,0 +1,242 @@ +"""Drops two boxes onto a slab to show what the "mujoco_multipoint" point +contact algorithm changes. + +Drake's default point contact narrowphase reports one contact point per +colliding geometry pair, the point of deepest penetration, even when two flat +faces overlap. Setting the proximity property +("material", "point_contact_algorithm") to "mujoco_multipoint" on both +geometries of a Convex (or Mesh) pair makes the pair report a contact manifold +of up to four points instead, so every shape in this scene is a box modeled as +a Convex shape. + +The slab and the box on the right select "mujoco_multipoint"; the box on the +left selects "single_point" (the default), so its contact with the slab +reports one point. Both boxes fall flat onto the slab. A box supported at a +single point has no static equilibrium: the support point hops between +corners of the bottom face, so the left box keeps rocking, creeps sideways, +and sinks into the slab. The right box lands on a four-point manifold and +comes to rest at once. + +The console prints, for each box, how many contact points it has with the +slab, the height of its center above the slab, and its angular speed; +Meshcat draws one contact force arrow per contact point. +""" + +import argparse + +import numpy as np + +from pydrake.common import MemoryFile +from pydrake.geometry import ( + AddContactMaterial, + Convex, + InMemoryMesh, + Meshcat, + ProximityProperties, +) +from pydrake.math import RigidTransform +from pydrake.multibody.plant import ( + AddMultibodyPlant, + CoulombFriction, + MultibodyPlantConfig, +) +from pydrake.multibody.tree import SpatialInertia +from pydrake.systems.analysis import Simulator +from pydrake.systems.framework import DiagramBuilder +from pydrake.visualization import AddDefaultVisualization + +# Dimensions are in meters; masses in kilograms. +SLAB_SIZE = (1.0, 0.5, 0.05) +BOX_SIZE = (0.2, 0.2, 0.04) +BOX_MASS = 0.5 + + +def make_convex_box(size, name): + """Returns the axis-aligned box of the given (x, y, z) size, centered at + the origin, as a Convex shape: the convex hull of an in-memory OBJ file + that lists the eight corners. Drake's Box shape always reports a single + contact point; only Convex and Mesh shapes are eligible for the + multi-point contact manifold. + """ + hx, hy, hz = np.asarray(size) / 2 + lines = [ + f"v {x} {y} {z}" + for z in (-hz, hz) + for y in (-hy, hy) + for x in (-hx, hx) + ] + # Faces reference the vertices above by 1-based index, wound + # counter-clockwise when seen from outside the box. + lines += [ + "f 1 3 4 2", # -z + "f 5 6 8 7", # +z + "f 1 2 6 5", # -y + "f 3 7 8 4", # +y + "f 1 5 7 3", # -x + "f 2 4 8 6", # +x + ] + obj = "\n".join(lines) + "\n" + return Convex(InMemoryMesh(mesh_file=MemoryFile(obj, ".obj", name))) + + +def make_contact_properties(point_contact_algorithm): + """Returns proximity properties that select the given point contact + algorithm, either "single_point" or "mujoco_multipoint". Stiffness and + dissipation are left to the SceneGraph defaults. To select an algorithm + for a whole scene at once, set + SceneGraphConfig.default_proximity_properties.point_contact_algorithm + instead of a per-geometry property. + """ + properties = ProximityProperties() + AddContactMaterial( + properties=properties, friction=CoulombFriction(0.5, 0.5) + ) + properties.AddProperty( + "material", "point_contact_algorithm", point_contact_algorithm + ) + return properties + + +def add_box(plant, name, point_contact_algorithm, color): + body = plant.AddRigidBody( + name, SpatialInertia.SolidBoxWithMass(BOX_MASS, *BOX_SIZE) + ) + shape = make_convex_box(BOX_SIZE, f"{name}.obj") + plant.RegisterCollisionGeometry( + body, + RigidTransform(), + shape, + f"{name}_collision", + make_contact_properties(point_contact_algorithm), + ) + plant.RegisterVisualGeometry( + body, RigidTransform(), shape, f"{name}_visual", color + ) + return body + + +def main(): + parser = argparse.ArgumentParser(description=__doc__) + parser.add_argument( + "--simulation_time", + type=float, + default=5.0, + help="Duration of the simulation in seconds.", + ) + parser.add_argument( + "--target_realtime_rate", + type=float, + default=1.0, + help="Target realtime rate.", + ) + parser.add_argument( + "--time_step", + type=float, + default=1e-3, + help="Discrete time step of the MultibodyPlant in seconds.", + ) + parser.add_argument( + "--drop_height", + type=float, + default=0.05, + help="Initial gap between the boxes and the slab in meters.", + ) + args = parser.parse_args() + + builder = DiagramBuilder() + plant, _ = AddMultibodyPlant( + MultibodyPlantConfig(time_step=args.time_step, contact_model="point"), + builder, + ) + + # The slab is a box modeled as a Convex shape, fixed to the world with its + # top face at z = 0. + slab_shape = make_convex_box(SLAB_SIZE, "slab.obj") + X_WSlab = RigidTransform([0.0, 0.0, -SLAB_SIZE[2] / 2]) + plant.RegisterCollisionGeometry( + plant.world_body(), + X_WSlab, + slab_shape, + "slab_collision", + make_contact_properties("mujoco_multipoint"), + ) + plant.RegisterVisualGeometry( + plant.world_body(), + X_WSlab, + slab_shape, + "slab_visual", + [0.8, 0.8, 0.75, 1.0], + ) + + # A pair reports a manifold only when *both* of its geometries select + # "mujoco_multipoint". The left box selects "single_point", so its contact + # with the slab behaves like Drake's default narrowphase. + boxes = [ + add_box( + plant, "single_point_box", "single_point", [0.9, 0.5, 0.2, 1.0] + ), + add_box( + plant, "multipoint_box", "mujoco_multipoint", [0.2, 0.5, 0.9, 1.0] + ), + ] + plant.Finalize() + + meshcat = Meshcat() + AddDefaultVisualization(builder=builder, meshcat=meshcat) + meshcat.SetCameraPose( + camera_in_world=[0.6, -1.2, 0.6], target_in_world=[0.0, 0.0, 0.0] + ) + diagram = builder.Build() + + simulator = Simulator(diagram) + simulator.set_target_realtime_rate(args.target_realtime_rate) + plant_context = plant.GetMyMutableContextFromRoot( + simulator.get_mutable_context() + ) + + # Both boxes start level, a small distance above the slab. + z0 = BOX_SIZE[2] / 2 + args.drop_height + for body, x in zip(boxes, (-0.25, 0.25)): + plant.SetFreeBodyPose(plant_context, body, RigidTransform([x, 0.0, z0])) + + def report(t): + """Prints, for each box, the number of point contacts it has with the + slab, the height of its center above the slab in millimeters (a box + at rest on the slab reads 20.00), and its angular speed in rad/s. + """ + results = plant.get_contact_results_output_port().Eval(plant_context) + counts = {body.index(): 0 for body in boxes} + for i in range(results.num_point_pair_contacts()): + info = results.point_pair_contact_info(i) + for index in (info.bodyA_index(), info.bodyB_index()): + if index in counts: + counts[index] += 1 + columns = [f"{t:7.2f}"] + for body in boxes: + height_mm = ( + 1000 + * plant.EvalBodyPoseInWorld(plant_context, body).translation()[ + 2 + ] + ) + w_WB = plant.EvalBodySpatialVelocityInWorld( + plant_context, body + ).rotational() + columns.append( + f"{counts[body.index()]:8d} {height_mm:7.2f} " + f"{np.linalg.norm(w_WB):6.3f}" + ) + print(" ".join(columns)) + + print(" single_point box multipoint box") + print(" time contacts height |w| contacts height |w|") + simulator.Initialize() + t = 0.0 + while t < args.simulation_time: + t = min(t + 0.5, args.simulation_time) + simulator.AdvanceTo(t) + report(t) + + +if __name__ == "__main__": + main()