From a7f74813b6d13119c8db2e6ab73695d06e3658c6 Mon Sep 17 00:00:00 2001 From: Haixuan Xavier Tao Date: Mon, 27 Jul 2026 18:01:16 +0200 Subject: [PATCH 1/3] Contact lever arms about the link world COM, not the collider pose MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit The MB contact init used the contacting collider world translation as a proxy for the link origin when building angular lever arms (a documented follow-up: links_workspace was unbound over a 10-storage-buffer concern that no longer holds). The body jacobians measure link velocity at the link WORLD COM, so for any link whose collider is offset from its COM the contact rows are wrong by that offset. Measured on a G1 humanoid (foot collider ~4.6cm from the link COM), against an analytic MuJoCo mj_jac reference at an identical pose: the ankle-pitch column of a foot contact row was 8x too small (0.008 vs 0.056) — ground reactions transmitted almost no ankle torque, making quiet standing impossible at any motor stiffness. With this fix the contact rows match the reference to ~2% (ankle column within 3%) and J·M⁻¹·Jᵀ effective masses agree with a same-model MuJoCo reference to 2-8%. Fix: bind links_static + links_workspace and use local_to_world · local_mprops.com for both the A side and the self-contact B side. Co-Authored-By: Claude Opus 4.8 Claude-Session: https://claude.ai/code/session_01B7NC7U2wDx2tSmT9mkeF9h --- .../dynamics/multibody/multibody_solver.rs | 2 + .../dynamics/multibody/contact_constraints.rs | 40 ++++++++++--------- 2 files changed, 23 insertions(+), 19 deletions(-) diff --git a/src_rbd/dynamics/multibody/multibody_solver.rs b/src_rbd/dynamics/multibody/multibody_solver.rs index 20ef22f..89ea425 100644 --- a/src_rbd/dynamics/multibody/multibody_solver.rs +++ b/src_rbd/dynamics/multibody/multibody_solver.rs @@ -251,6 +251,8 @@ impl GpuMultibodySolver { args.mprops, args.collider_world_poses, args.contacts, + &mb.links_static, + &mb.links_workspace, )?; self.finalize_contact_constraints.call( diff --git a/src_rbd_shaders/dynamics/multibody/contact_constraints.rs b/src_rbd_shaders/dynamics/multibody/contact_constraints.rs index 922015a..10158ac 100644 --- a/src_rbd_shaders/dynamics/multibody/contact_constraints.rs +++ b/src_rbd_shaders/dynamics/multibody/contact_constraints.rs @@ -28,6 +28,7 @@ use crate::{ANG_DIM, AngVector, DIM, Pose, Vector, gcross, gdot}; use super::types::{ CONTACT_CONSTRAINTS_PER_POINT, MAX_MB_CONTACT_CONSTRAINTS_PER_MB, MAX_MB_CONTACTS_PER_MB, MB_CONTACT_KIND_NORMAL, MB_CONTACT_KIND_TANGENT, MultibodyContactConstraint, MultibodyInfo, + MultibodyLinkStatic, MultibodyLinkWorkspace, }; #[cfg(feature = "dim2")] @@ -132,6 +133,10 @@ pub fn gpu_mb_init_contact_constraints( #[spirv(storage_buffer, descriptor_set = 1, binding = 0)] mprops: &[WorldMassProperties], #[spirv(storage_buffer, descriptor_set = 1, binding = 1)] poses: &[Pose], #[spirv(storage_buffer, descriptor_set = 1, binding = 2)] contacts: &[IndexedManifold], + #[spirv(storage_buffer, descriptor_set = 1, binding = 3)] + links_static: &[MultibodyLinkStatic], + #[spirv(storage_buffer, descriptor_set = 1, binding = 4)] + links_workspace: &[MultibodyLinkWorkspace], #[spirv(uniform, descriptor_set = 0, binding = 6)] batch_ids: &BatchIndices, ) { let batch_id = invocation_id.y; @@ -243,23 +248,18 @@ pub fn gpu_mb_init_contact_constraints( }; let free_im = if is_self { 0.0 } else { free_mp.inv_mass.x }; - // Multibody-link origins come from the collider poses buffer instead - // of `links_workspace` (which we no longer bind in this kernel — see - // binding-count cap discussion). This uses the contacting collider's - // world translation as a proxy for its link's world origin. It is exact - // when the collider sits at the link origin (the historical one-collider - // case); for a link carrying a collider offset from its origin the - // angular lever arm `pt_world - link_origin_a` is off by that offset. - // Binding `body_poses` here to use the true link origin would exceed the - // 10-storage-buffer cap — left as a follow-up. `mb_link_id_a` always - // corresponds to side `id1`/`b1` when `mb_on_1 || is_self`, and to - // `id2`/`b2` otherwise. `mb_link_id_b` is only used in the self-contact - // case where it corresponds to `id2`. - let link_origin_a = if is_self || mb_on_1 { - pose1.translation - } else { - pose2.translation - }; + // Angular lever arms must be taken about the link's WORLD COM — the + // reference point of the body jacobians (rapier: "the lever arms must + // be taken relative to the world com"). The previous collider-pose + // proxy was off by the collider's offset from the COM. + let stat_slice = batch_ids + .mb_links_batch(batch_id, links_static) + .offset(mb.first_link as usize); + let ws_slice = batch_ids + .mb_links_batch(batch_id, links_workspace) + .offset(mb.first_link as usize); + let link_origin_a = ws_slice[mb_link_id_a as usize].local_to_world + * stat_slice[mb_link_id_a as usize].local_mprops.com; let link_origin_b_default = link_origin_a; for k in 0..im.contact.len { @@ -321,7 +321,8 @@ pub fn gpu_mb_init_contact_constraints( // Self-contact: B-side link is the collider at `id2`, so its // world pose is `pose2` (already loaded). Avoids a // `links_workspace` binding. - let link_origin_b = pose2.translation; + let link_origin_b = ws_slice[mb_link_id_b as usize].local_to_world + * stat_slice[mb_link_id_b as usize].local_mprops.com; let _ = link_origin_b_default; let shift_b = pt_world - link_origin_b; let torque_b_normal = gcross(shift_b, lin_jac); @@ -453,7 +454,8 @@ pub fn gpu_mb_init_contact_constraints( ); let (ang_jac_tang, ii_ang_jac_tang) = if is_self { - let link_origin_b = pose2.translation; + let link_origin_b = ws_slice[mb_link_id_b as usize].local_to_world + * stat_slice[mb_link_id_b as usize].local_mprops.com; let shift_b = pt_world - link_origin_b; let torque_b_tang = gcross(shift_b, free_tangent); fill_contact_jac_row( From 41cd9fbaafe4fb9d9b6c2bbc429f7e7eb22696f7 Mon Sep 17 00:00:00 2001 From: haixuantao Date: Fri, 7 Aug 2026 12:06:39 +0200 Subject: [PATCH 2/3] fix ci: remove unused pose2 read + rustfmt Co-Authored-By: Claude Fable 5 --- .../dynamics/multibody/contact_constraints.rs | 9 ++++----- 1 file changed, 4 insertions(+), 5 deletions(-) diff --git a/src_rbd_shaders/dynamics/multibody/contact_constraints.rs b/src_rbd_shaders/dynamics/multibody/contact_constraints.rs index 10158ac..1270bd6 100644 --- a/src_rbd_shaders/dynamics/multibody/contact_constraints.rs +++ b/src_rbd_shaders/dynamics/multibody/contact_constraints.rs @@ -232,7 +232,6 @@ pub fn gpu_mb_init_contact_constraints( } let pose1 = poses.read(colliders_start + id1 as usize); - let pose2 = poses.read(colliders_start + id2 as usize); let world_normal = pose1.rotation * im.contact.normal_a; let lin_jac = if is_self || mb_on_1 { world_normal @@ -318,9 +317,9 @@ pub fn gpu_mb_init_contact_constraints( // contacts they collapse to zero because both sides are folded // into `J_mb` already. let (ang_jac_normal, ii_ang_jac_normal) = if is_self { - // Self-contact: B-side link is the collider at `id2`, so its - // world pose is `pose2` (already loaded). Avoids a - // `links_workspace` binding. + // Self-contact: B-side link is the collider at `id2`; its + // lever arm is taken about the link world COM from the + // workspace. let link_origin_b = ws_slice[mb_link_id_b as usize].local_to_world * stat_slice[mb_link_id_b as usize].local_mprops.com; let _ = link_origin_b_default; @@ -455,7 +454,7 @@ pub fn gpu_mb_init_contact_constraints( let (ang_jac_tang, ii_ang_jac_tang) = if is_self { let link_origin_b = ws_slice[mb_link_id_b as usize].local_to_world - * stat_slice[mb_link_id_b as usize].local_mprops.com; + * stat_slice[mb_link_id_b as usize].local_mprops.com; let shift_b = pt_world - link_origin_b; let torque_b_tang = gcross(shift_b, free_tangent); fill_contact_jac_row( From 42706dc511384cd0ab7c94353ce7ee47f351d943 Mon Sep 17 00:00:00 2001 From: haixuantao Date: Fri, 7 Aug 2026 12:28:04 +0200 Subject: [PATCH 3/3] fix ci: remove now-unused id2 binding Co-Authored-By: Claude Fable 5 --- src_rbd_shaders/dynamics/multibody/contact_constraints.rs | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/src_rbd_shaders/dynamics/multibody/contact_constraints.rs b/src_rbd_shaders/dynamics/multibody/contact_constraints.rs index 1270bd6..9ca0d1a 100644 --- a/src_rbd_shaders/dynamics/multibody/contact_constraints.rs +++ b/src_rbd_shaders/dynamics/multibody/contact_constraints.rs @@ -192,7 +192,6 @@ pub fn gpu_mb_init_contact_constraints( continue; } let id1 = im.colliders.x; - let id2 = im.colliders.y; let b1 = im.bodies.x; let b2 = im.bodies.y; @@ -317,8 +316,8 @@ pub fn gpu_mb_init_contact_constraints( // contacts they collapse to zero because both sides are folded // into `J_mb` already. let (ang_jac_normal, ii_ang_jac_normal) = if is_self { - // Self-contact: B-side link is the collider at `id2`; its - // lever arm is taken about the link world COM from the + // Self-contact: B-side link is the pair's second collider; + // its lever arm is taken about the link world COM from the // workspace. let link_origin_b = ws_slice[mb_link_id_b as usize].local_to_world * stat_slice[mb_link_id_b as usize].local_mprops.com;