From ed13bf56474d8e2a642087941b1e36ecb93c0f36 Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Thu, 6 Aug 2026 04:09:41 -0700 Subject: [PATCH] Mocap bodies are the weld root of their own kinematic subtree. PiperOrigin-RevId: 960212286 Change-Id: Ibeff129c3110576c8846c76ba6ab756f23c0fa2e --- doc/XMLreference.rst | 5 +- doc/changelog.rst | 6 + doc/computation/index.rst | 7 +- doc/includes/references.h | 2 +- doc/modeling.rst | 7 + doc/overview.rst | 12 +- doc/programming/simulation.rst | 2 + include/mujoco/mjmodel.h | 2 +- python/mujoco/introspect/structs.py | 2 +- src/engine/engine_collision_driver.c | 41 +++-- src/engine/engine_core_util.c | 32 ++-- src/engine/engine_setconst.c | 4 +- src/engine/engine_sleep.c | 32 ++-- src/engine/engine_vis_visualize.c | 4 +- .../filament/support/model_renderables.cc | 4 +- src/user/user_model.cc | 4 +- src/user/user_objects.cc | 13 +- test/engine/engine_sleep_test.cc | 141 ++++++++++++++++++ test/user/user_model_test.cc | 3 +- 19 files changed, 260 insertions(+), 63 deletions(-) diff --git a/doc/XMLreference.rst b/doc/XMLreference.rst index 63bf1952..87992af4 100644 --- a/doc/XMLreference.rst +++ b/doc/XMLreference.rst @@ -2090,7 +2090,10 @@ defined. Its body name is automatically defined as "world". time step. The size of these arrays is adjusted by the compiler so as to match the number of mocap bodies in the model. This mechanism can be used to stream motion capture data into the simulation. Mocap bodies can also be moved via mouse perturbations in the interactive visualizer, even in dynamic simulation mode. This can be useful for - creating props with adjustable position and orientation. + creating props with adjustable position and orientation. Mocap bodies are the weld root of their own kinematic + subtree rather than being welded to the world: their children receive standard parent-child collision filtering, + they do not generate contacts with static geometry, and contact with a mocap body wakes + :ref:`sleeping` bodies. See :ref:`mocap bodies` for more details. .. _body-pos: diff --git a/doc/changelog.rst b/doc/changelog.rst index 3b91f6db..b9bc24b3 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -53,6 +53,12 @@ Engine - Changed the default value of :ref:`bvactive` from "true" to "false". This avoids unnecessarily clearing bounding volume hierarchy visualization flags at every simulation step, which can be a bottleneck for models with large meshes. + - Mocap bodies and their dof-less descendants are now the root of their own weld group: ``mjModel.body_weldid`` of a + mocap body equals its own id rather than 0. Consequences: dragging a mocap body into sleeping objects now wakes + them; children of mocap bodies receive standard :ref:`parent-child collision filtering`; + mocap bodies no longer count as static geometry for ray casting, and contact-matching sensors aggregate their + contacts under the mocap body rather than the world; and geom pairs where neither body can move no longer generate + contacts. Models ^^^^^^ diff --git a/doc/computation/index.rst b/doc/computation/index.rst index d060ff54..8fe11777 100644 --- a/doc/computation/index.rst +++ b/doc/computation/index.rst @@ -1666,8 +1666,11 @@ Filtering 3. The two geoms cannot belong to the same body. Furthermore, they cannot belong to a parent and a child body, unless the parent is the world body. The motivation is to avoid permanent contacts within bodies and joints. Note that if several bodies are welded together in the sense that there are no joints between them, they are treated as a - single body for the purposes of this test. The parent-filter test can be disabled by the user, while the same-body - test cannot be disabled. + single body for the purposes of this test. :ref:`Mocap bodies` and their dof-less descendants form their + own weld group, distinct from the world weld, so the parent-child exclusion applies to children of mocap bodies as + usual. Additionally, geom pairs where neither body can move (both weld groups have no degrees of freedom) are + skipped, so mocap bodies do not generate contacts with static geometry or with each other. The parent-filter test + can be disabled by the user, while the same-body test cannot be disabled. 4. The two geoms must be "compatible" in the following sense. Each geom has integer parameters ``contype`` and ``conaffinity``. The boolean expression below must be true for the test to pass: diff --git a/doc/includes/references.h b/doc/includes/references.h index 6c8ebc11..4ce5b8be 100644 --- a/doc/includes/references.h +++ b/doc/includes/references.h @@ -698,7 +698,7 @@ typedef struct mjModel_ { // bodies int* body_parentid; // id of body's parent (nbody x 1) int* body_rootid; // ancestor that is direct child of world (nbody x 1) - int* body_weldid; // top ancestor with no dofs to this body (nbody x 1) + int* body_weldid; // top dof-less ancestor; mocap: own root (nbody x 1) int* body_mocapid; // id of mocap data; -1: none (nbody x 1) int* body_jntnum; // number of joints for this body (nbody x 1) int* body_jntadr; // start addr of joints; -1: no joints (nbody x 1) diff --git a/doc/modeling.rst b/doc/modeling.rst index 263b2ed3..4da2b996 100644 --- a/doc/modeling.rst +++ b/doc/modeling.rst @@ -1544,6 +1544,13 @@ virtual objects cannot push on your physical hand, so your hand (and thereby the violate the simulated physics. But at the same time we want the resulting simulation to be reasonable. How do we do this? +Mocap bodies and their dof-less descendants form their own *weld group*, rooted at the mocap body +(``mjModel.body_weldid`` equals the mocap body's own id rather than 0, the world). This has several consequences: +children of mocap bodies receive the standard parent-child collision exclusion; mocap bodies do not generate contacts +with static geometry or with each other; and when :ref:`sleeping` is enabled, mocap bodies count as awake — +contact with a mocap body, or an active equality constraint connecting to one, wakes sleeping objects, so dragging a +mocap body through a pile of sleeping objects behaves as expected. + The first step is to define a mocap body in the MJCF model, and implement code that reads the data stream at runtime and sets :ref:`mjData.mocap_pos ` and :ref:`mjData.mocap_quat ` to the position and orientation received from the motion capture system. The :ref:`simulate.cc ` code sample uses the mouse as a motion capture diff --git a/doc/overview.rst b/doc/overview.rst index e8123722..b3ab291e 100644 --- a/doc/overview.rst +++ b/doc/overview.rst @@ -804,13 +804,17 @@ section, prevents objects from falling through the floor or moving through walls the following situation: The user comments out the root joint of a floating-base model, perhaps in order to prevent it from falling; now that the -base body is counted as static, new collisions appear that were not there before and the user is confused. There are two -easy ways to avoid this problem: +base body is counted as static, new collisions appear that were not there before and the user is confused. There are +three easy ways to avoid this problem: -1. Don't remove the root joint. Perhaps it is enough to :ref:`disable gravity` and possibly add some +1. Make the newly static body a :ref:`mocap body`. Mocap bodies are the root of their own body group for + the purposes of collision filtering, rather than being welded to the world, so the parent-child exclusion applies to + their children as usual. As a bonus, the body can be moved around interactively. + +2. Don't remove the root joint. Perhaps it is enough to :ref:`disable gravity` and possibly add some :ref:`fluid viscosity