From 1e7867ffd3bd1de361e0dae81217ca0a0f9abbe1 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?S=C3=A9bastien=20Crozet?= Date: Thu, 11 Jun 2026 11:02:11 +0200 Subject: [PATCH 01/16] fix local frame for impulse joint on multibodies --- .../generic_joint_constraint_builder.rs | 32 +++++++++++++++++-- 1 file changed, 29 insertions(+), 3 deletions(-) diff --git a/src/dynamics/solver/joint_constraint/generic_joint_constraint_builder.rs b/src/dynamics/solver/joint_constraint/generic_joint_constraint_builder.rs index 79f673d11..fb0ac835e 100644 --- a/src/dynamics/solver/joint_constraint/generic_joint_constraint_builder.rs +++ b/src/dynamics/solver/joint_constraint/generic_joint_constraint_builder.rs @@ -129,7 +129,21 @@ impl JointGenericExternalConstraintBuilder { } let mut joint_data = joint.data; - joint_data.transform_to_solver_body_space(rb1, rb2); + // NOTE: multibody links are positioned by `link.local_to_world` which is + // the body-frame (origin-centered) pose, unlike regular dynamic + // bodies whose solver pose is com-centered. So the com shift from + // `transform_to_solver_body_space` must NOT be applied to sides + // attached to a multibody link. + if rb1.is_fixed() { + joint_data.local_frame1 = rb1.pos.position * joint_data.local_frame1; + } else if matches!(link1, LinkOrBody::Body(_)) { + joint_data.local_frame1.translation -= rb1.mprops.local_mprops.local_com; + } + if rb2.is_fixed() { + joint_data.local_frame2 = rb2.pos.position * joint_data.local_frame2; + } else if matches!(link2, LinkOrBody::Body(_)) { + joint_data.local_frame2.translation -= rb2.mprops.local_mprops.local_com; + } *out_builder = GenericJointConstraintBuilder::External(Self { link1, link2, @@ -165,18 +179,27 @@ impl JointGenericExternalConstraintBuilder { let mb1; let mb2; + let world_com1; + let world_com2; + match self.link1 { LinkOrBody::Link(link) => { let mb = &multibodies[link.multibody]; pos1 = mb.link(link.id).unwrap().local_to_world; + // The link pose is origin-centered; the link’s body jacobian + // measures linear velocities at its center-of-mass, so the + // lever arms must be taken relative to the world com. + world_com1 = pos1 * self.local_body1.world_com; mb1 = LinkOrBodyRef::Link(mb, link.id); } LinkOrBody::Body(body1) => { pos1 = bodies.get_pose(body1).pose(); + world_com1 = pos1.translation; // the solver body pose is at the center of mass. mb1 = LinkOrBodyRef::Body(body1); } LinkOrBody::Fixed => { pos1 = Pose::IDENTITY; + world_com1 = pos1.translation; mb1 = LinkOrBodyRef::Fixed; } }; @@ -184,14 +207,17 @@ impl JointGenericExternalConstraintBuilder { LinkOrBody::Link(link) => { let mb = &multibodies[link.multibody]; pos2 = mb.link(link.id).unwrap().local_to_world; + world_com2 = pos2 * self.local_body2.world_com; mb2 = LinkOrBodyRef::Link(mb, link.id); } LinkOrBody::Body(body2) => { pos2 = bodies.get_pose(body2).pose(); + world_com2 = pos2.translation; // the solver body pose is at the center of mass. mb2 = LinkOrBodyRef::Body(body2); } LinkOrBody::Fixed => { pos2 = Pose::IDENTITY; + world_com2 = pos2.translation; mb2 = LinkOrBodyRef::Fixed; } }; @@ -200,11 +226,11 @@ impl JointGenericExternalConstraintBuilder { let frame2 = pos2 * self.joint.local_frame2; let joint_body1 = JointSolverBody { - world_com: pos1.translation, // the solver body pose is at the center of mass. + world_com: world_com1, ..self.local_body1 }; let joint_body2 = JointSolverBody { - world_com: pos2.translation, // the solver body pose is at the center of mass. + world_com: world_com2, ..self.local_body2 }; From dfd531ad31b5b0748e9a4108bddbdcbe99e66db7 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?S=C3=A9bastien=20Crozet?= Date: Thu, 11 Jun 2026 12:20:31 +0200 Subject: [PATCH 02/16] fix(rapier): add missing coupling term for loop-closing impulse joint constraints --- .../joint/multibody_joint/multibody.rs | 83 +++++++++++++++++++ .../generic_joint_constraint_builder.rs | 38 +++++++++ 2 files changed, 121 insertions(+) diff --git a/src/dynamics/joint/multibody_joint/multibody.rs b/src/dynamics/joint/multibody_joint/multibody.rs index da3a097fe..b742e975c 100644 --- a/src/dynamics/joint/multibody_joint/multibody.rs +++ b/src/dynamics/joint/multibody_joint/multibody.rs @@ -1228,6 +1228,89 @@ impl Multibody { (j.dot(&invm_j), j.dot(&self.generalized_velocity())) } + /// Fills `jacobians` with the relative jacobian `J = J2ᵀ·f2 − J1ᵀ·f1` of two + /// links of `self` (followed by its product with the inverse augmented mass), + /// where `fk = (unit_forcek, unit_torquek)`. + /// + /// This is the jacobian of a velocity constraint between two links of the + /// same multibody (e.g. a loop closure). The difference must be computed + /// explicitly — keeping one block per link loses the `J1ᵀ·W·J2` coupling in + /// the constraint’s effective mass since both blocks act on the same + /// generalized velocities. + /// + /// Rows that vanish by cancellation (the constrained direction is not + /// expressible in the multibody’s reduced coordinates, e.g. a loop-closure + /// anchor coinciding with the joint pivot it closes over) are zeroed so the + /// solver skips them instead of dividing by floating-point noise. + pub(crate) fn fill_relative_jacobians( + &self, + link_id1: usize, + unit_force1: Vector, + unit_torque1: AngVector, + link_id2: usize, + unit_force2: Vector, + unit_torque2: AngVector, + j_id: &mut usize, + jacobians: &mut DVector, + ) { + if self.ndofs == 0 { + return; + } + + let wj_id = *j_id + self.ndofs; + let force1 = Force { + linear: unit_force1, + angular: unit_torque1, + }; + let force2 = Force { + linear: unit_force2, + angular: unit_torque2, + }; + + let link1 = &self.links[link_id1]; + let link2 = &self.links[link_id2]; + + { + let jb1 = &self.body_jacobians[link1.internal_id]; + let jb2 = &self.body_jacobians[link2.internal_id]; + + // Use the (overwritten below) W·J slot as scratch for J1ᵀ·f1. + let (mut out_j, mut scratch) = jacobians + .rows_range_pair_mut(*j_id..*j_id + self.ndofs, wj_id..wj_id + self.ndofs); + jb2.tr_mul_to(force2.as_vector(), &mut out_j); + jb1.tr_mul_to(force1.as_vector(), &mut scratch); + out_j.axpy(-1.0, &scratch, 1.0); + + // Cancellation guard. The reference scale is the magnitude of the + // dot-product operands (not of their results, which may themselves + // be pure cancellation noise when the constrained direction isn’t + // expressible by the multibody’s dofs at all). A row this small + // compared to ~1000× the machine epsilon times that scale is + // numerical noise, not an actual constraint direction. + let scale_sq = jb1.norm_squared() * force1.as_vector().norm_squared() + + jb2.norm_squared() * force2.as_vector().norm_squared(); + let eps = Real::EPSILON * 1.0e3; + if out_j.norm_squared() <= eps * eps * scale_sq { + out_j.fill(0.0); + } + } + + // TODO: Optimize with a copy_nonoverlapping? + for i in 0..self.ndofs { + jacobians[wj_id + i] = jacobians[*j_id + i]; + } + + { + let mut out_invm_j = jacobians.rows_mut(wj_id, self.ndofs); + self.augmented_mass_indices + .with_rearranged_rows_mut(&mut out_invm_j, |out_invm_j| { + self.inv_augmented_mass.solve_mut(out_invm_j); + }); + } + + *j_id += self.ndofs * 2; + } + // #[cfg(feature = "parallel")] // #[inline] // pub(crate) fn has_active_internal_constraints(&self) -> bool { diff --git a/src/dynamics/solver/joint_constraint/generic_joint_constraint_builder.rs b/src/dynamics/solver/joint_constraint/generic_joint_constraint_builder.rs index fb0ac835e..ac6f0ca3f 100644 --- a/src/dynamics/solver/joint_constraint/generic_joint_constraint_builder.rs +++ b/src/dynamics/solver/joint_constraint/generic_joint_constraint_builder.rs @@ -376,6 +376,44 @@ impl JointConstraintHelper { ang_jac1: AngVector, ang_jac2: AngVector, ) -> GenericJointConstraint { + // When both attachment points are links of the same multibody (e.g. a + // loop closure), the two jacobian blocks act on the same generalized + // velocities and must be combined into a single relative jacobian + // `J2 − J1`. Keeping one block per link would lose the `J1ᵀ·W·J2` + // coupling in the constraint’s effective mass, yielding wildly wrong + // impulses (and divisions by noise when the blocks nearly cancel). + // The block is stored on the "2" side so that the solver’s + // `vel2 - vel1` measurement reads `(J2 − J1)·v` as expected. + if let (LinkOrBodyRef::Link(mb_a, link_id1), LinkOrBodyRef::Link(mb_b, link_id2)) = + (mb1, mb2) + && core::ptr::eq(mb_a, mb_b) + { + let block_j_id = *j_id; + mb_a.fill_relative_jacobians( + link_id1, lin_jac, ang_jac1, link_id2, lin_jac, ang_jac2, j_id, jacobians, + ); + + return GenericJointConstraint { + is_rigid_body1: true, + is_rigid_body2: false, + solver_vel1: u32::MAX, + solver_vel2: mb_a.solver_id, + ndofs1: 0, + j_id1: block_j_id, + ndofs2: mb_a.ndofs(), + j_id2: block_j_id, + joint_id, + impulse: 0.0, + impulse_bounds: [-Real::MAX, Real::MAX], + inv_lhs: 0.0, + rhs: 0.0, + rhs_wo_bias: 0.0, + cfm_coeff: 0.0, + cfm_gain: 0.0, + writeback_id, + }; + } + let j_id1 = *j_id; let (ndofs1, solver_vel1, is_rigid_body1) = match mb1 { LinkOrBodyRef::Link(mb1, link_id1) => { From 0b3e392aea6da8bc33b97dab196ae932bb75710a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?S=C3=A9bastien=20Crozet?= Date: Thu, 11 Jun 2026 12:24:17 +0200 Subject: [PATCH 03/16] feat: add support for the multibody joitns armature property --- crates/rapier3d-mjcf/src/loader/conversion.rs | 6 ++ crates/rapier3d-mjcf/src/loader/insert.rs | 9 +- crates/rapier3d-mjcf/src/loader/mass.rs | 98 ++++++++++--------- crates/rapier3d-mjcf/src/loader/types.rs | 8 ++ .../joint/multibody_joint/multibody.rs | 44 ++++++++- 5 files changed, 117 insertions(+), 48 deletions(-) diff --git a/crates/rapier3d-mjcf/src/loader/conversion.rs b/crates/rapier3d-mjcf/src/loader/conversion.rs index ab3c15f64..138dc2722 100644 --- a/crates/rapier3d-mjcf/src/loader/conversion.rs +++ b/crates/rapier3d-mjcf/src/loader/conversion.rs @@ -238,6 +238,7 @@ impl<'a> Conversion<'a> { link2: rb_idx, joint, damping_per_dof: 0.0, + armature_per_dof: 0.0, }); } return; @@ -281,12 +282,17 @@ impl<'a> Conversion<'a> { } else { j.damping as Real }; + // Armature (reflected rotor inertia) is routed into the multibody's + // generalized mass matrix at insertion time rather than baked into + // the link's spatial inertia — see `MjcfJoint::armature_per_dof`. + let armature_per_dof = j.armature.max(0.0) as Real; self.robot.joints.push(MjcfJoint { name: j.name.clone(), link1: prev_rapier, link2: cur_rapier, joint, damping_per_dof, + armature_per_dof, }); prev_rapier = cur_rapier; prev_world_pose = cur_world_pose; diff --git a/crates/rapier3d-mjcf/src/loader/insert.rs b/crates/rapier3d-mjcf/src/loader/insert.rs index fb25f0e74..b00a268fc 100644 --- a/crates/rapier3d-mjcf/src/loader/insert.rs +++ b/crates/rapier3d-mjcf/src/loader/insert.rs @@ -14,7 +14,7 @@ use rapier3d::math::Pose; use super::handles::{ MjcfActuatorHandle, MjcfBodyHandle, MjcfColliderHandle, MjcfJointHandle, MjcfRobotHandles, }; -use super::mass::move_motor_damping_to_multibody; +use super::mass::{add_armature_to_multibody, move_motor_damping_to_multibody}; use super::options::MjcfMultibodyOptions; use super::types::MjcfRobot; @@ -152,6 +152,7 @@ impl MjcfRobot { continue; }; let damping_per_dof = j.damping_per_dof; + let armature_per_dof = j.armature_per_dof; let joint_name = j.name.clone(); // `limit_axes` / `motor_axes` are bitmasks over the joint's // DoFs; clearing them is equivalent to "joint built without @@ -193,6 +194,12 @@ impl MjcfRobot { if damping_per_dof > 0.0 { move_motor_damping_to_multibody(multibody_joints, h, damping_per_dof); } + // Route MJCF `` into the multibody's per-DoF + // mass-matrix diagonal (joint-space rotor inertia), instead of + // baking it into the link's spatial inertia tensor. + if armature_per_dof > 0.0 { + add_armature_to_multibody(multibody_joints, h, armature_per_dof); + } } joint_handles.push(MjcfJointHandle { joint: h, diff --git a/crates/rapier3d-mjcf/src/loader/mass.rs b/crates/rapier3d-mjcf/src/loader/mass.rs index ccea67379..84e3ba333 100644 --- a/crates/rapier3d-mjcf/src/loader/mass.rs +++ b/crates/rapier3d-mjcf/src/loader/mass.rs @@ -17,7 +17,15 @@ use super::conversion::Conversion; impl<'a> Conversion<'a> { /// Compute mass properties from `` (or derive from geoms when - /// the compiler asks for it). Adds `` contributions. + /// the compiler asks for it). + /// + /// Note: `` is intentionally **not** folded into the + /// returned inertia tensor. MuJoCo's armature is a reflected rotor inertia + /// that belongs on the diagonal of the joint-space mass matrix, not on the + /// link's spatial inertia — baking it into the tensor produces extreme + /// anisotropy (huge along the joint axis, ~0 across it) and an + /// ill-conditioned multibody mass matrix. It is routed through + /// [`add_armature_to_multibody`] at insertion time instead. pub(super) fn derive_mass_properties(&self, entry: &BodyEntry) -> Option { let s = self.options.scale; let from_inertial = entry @@ -28,53 +36,11 @@ impl<'a> Conversion<'a> { let force_geom = self.model.compiler.inertia_from_geom == InertiaFromGeom::True; let auto_geom = self.model.compiler.inertia_from_geom == InertiaFromGeom::Auto && entry.body.inertial.is_none(); - let mut mp = if force_geom || auto_geom { + let mp = if force_geom || auto_geom { self.geoms_to_mass_props(entry).or(from_inertial) } else { from_inertial }; - // Add armature contributions along each joint axis. For an axis - // `a`, the rotor inertia is `armature * outer(a, a)`, applied to - // the link's inertia tensor in the body frame. - for j in &entry.body.joints { - if j.armature <= 0.0 { - continue; - } - let axis = match j.type_ { - mb::JointType::Hinge | mb::JointType::Slide => Some(Vector::new( - j.axis[0] as Real, - j.axis[1] as Real, - j.axis[2] as Real, - )), - _ => None, - }; - let Some(axis) = axis else { continue }; - let n = axis.length(); - if n < 1e-30 { - continue; - } - let a = axis / n; - let arm = j.armature as Real; - let extra = Matrix::from_cols_array(&[ - arm * a.x * a.x, - arm * a.x * a.y, - arm * a.x * a.z, - arm * a.x * a.y, - arm * a.y * a.y, - arm * a.y * a.z, - arm * a.x * a.z, - arm * a.y * a.z, - arm * a.z * a.z, - ]); - let cur = mp.unwrap_or_default(); - let cur_inertia = cur.reconstruct_inertia_matrix(); - let new_inertia = cur_inertia + extra; - mp = Some(MassProperties::with_inertia_matrix( - cur.local_com, - cur.mass(), - new_inertia, - )); - } mp.map(|mp_| clamp_mass_properties(mp_, &self.model.compiler)) } @@ -242,3 +208,47 @@ pub(super) fn move_motor_damping_to_multibody( } } } + +/// After a joint has been inserted into a multibody, add the MJCF +/// `` (reflected rotor inertia) to the multibody's per-DoF +/// armature vector. Each entry lands on the diagonal of the generalized mass +/// matrix — the joint-space placement MuJoCo uses — rather than in the link's +/// spatial inertia tensor. +/// +/// The armature is applied uniformly to every free DoF of the joint, so a ball +/// joint with MJCF armature=a gets `a` on each of its three angular DoFs, +/// matching MJCF semantics. +pub(super) fn add_armature_to_multibody( + multibody_joints: &mut MultibodyJointSet, + handle: MultibodyJointHandle, + armature: Real, +) { + use rapier3d::math::SPATIAL_DIM; + let Some((multibody, link_id)) = multibody_joints.get_mut(handle) else { + return; + }; + // Reconstruct this link's DoF offset in the multibody's flat armature + // vector (assembly_id isn't public), same as the damping helper above. + let mut offset = 0; + for (i, link) in multibody.links().enumerate() { + if i == link_id { + break; + } + offset += link.joint().ndofs(); + } + let Some(link) = multibody.links().nth(link_id) else { + return; + }; + let locked_bits = link.joint.data.locked_axes.bits(); + let armature_vec = multibody.armature_mut(); + let mut local_dof = 0; + for i in 0..SPATIAL_DIM { + if (locked_bits & (1 << i)) == 0 { + let idx = offset + local_dof; + if idx < armature_vec.len() { + armature_vec[idx] = armature; + } + local_dof += 1; + } + } +} diff --git a/crates/rapier3d-mjcf/src/loader/types.rs b/crates/rapier3d-mjcf/src/loader/types.rs index 552498e3c..eaa6250b9 100644 --- a/crates/rapier3d-mjcf/src/loader/types.rs +++ b/crates/rapier3d-mjcf/src/loader/types.rs @@ -108,6 +108,14 @@ pub struct MjcfJoint { /// loads, and naturally covers all 3 DoFs of a ball joint instead of /// only the motorised AngX). pub damping_per_dof: Real, + /// MJCF `` value (reflected rotor inertia). On the + /// multibody insertion path this is added to the generalized mass-matrix + /// diagonal of each of the joint's DoFs, matching MuJoCo's joint-space + /// semantics. It is deliberately **not** baked into the link's spatial + /// inertia tensor: doing so makes the inertia extremely anisotropic + /// (huge along the joint axis, ~0 across it) and the multibody mass + /// matrix ill-conditioned. + pub armature_per_dof: Real, } /// One `` constraint materialized as an extra rapier joint. diff --git a/src/dynamics/joint/multibody_joint/multibody.rs b/src/dynamics/joint/multibody_joint/multibody.rs index b742e975c..a8e3ad920 100644 --- a/src/dynamics/joint/multibody_joint/multibody.rs +++ b/src/dynamics/joint/multibody_joint/multibody.rs @@ -66,6 +66,8 @@ pub struct Multibody { pub(crate) links: MultibodyLinkVec, pub(crate) velocities: DVector, pub(crate) damping: DVector, + /// Per-DoF reflected rotor inertia (matches MuJoCo’s concept of `armature`). + pub(crate) armature: DVector, pub(crate) accelerations: DVector, body_jacobians: Vec>, @@ -112,6 +114,7 @@ impl Multibody { links: MultibodyLinkVec(Vec::new()), velocities: DVector::zeros(0), damping: DVector::zeros(0), + armature: DVector::zeros(0), accelerations: DVector::zeros(0), body_jacobians: Vec::new(), augmented_mass: DMatrix::zeros(0, 0), @@ -188,6 +191,9 @@ impl Multibody { mb.damping .rows_mut(assembly_id, link_ndofs) .copy_from(&self.damping.rows(link.assembly_id, link_ndofs)); + mb.armature + .rows_mut(assembly_id, link_ndofs) + .copy_from(&self.armature.rows(link.assembly_id, link_ndofs)); mb.accelerations .rows_mut(assembly_id, link_ndofs) .copy_from(&self.accelerations.rows(link.assembly_id, link_ndofs)); @@ -246,6 +252,9 @@ impl Multibody { self.damping .rows_mut(rhs_copy_shift, rhs_copy_ndofs) .copy_from(&rhs.damping.rows(rhs_root_ndofs, rhs_copy_ndofs)); + self.armature + .rows_mut(rhs_copy_shift, rhs_copy_ndofs) + .copy_from(&rhs.armature.rows(rhs_root_ndofs, rhs_copy_ndofs)); self.accelerations .rows_mut(rhs_copy_shift, rhs_copy_ndofs) .copy_from(&rhs.accelerations.rows(rhs_root_ndofs, rhs_copy_ndofs)); @@ -341,6 +350,24 @@ impl Multibody { &mut self.damping } + /// The vector of per-DoF armature (reflected rotor inertia) of this + /// multibody. + /// + /// This acts as additional inertia added directly to the mass matrix. + /// Use this to simulate the intrinsic weight distribution of the joint + /// itself. + #[inline] + pub fn armature(&self) -> &DVector { + &self.armature + } + + /// Mutable vector of per-DoF armature (reflected rotor inertia) of this + /// multibody. + #[inline] + pub fn armature_mut(&mut self) -> &mut DVector { + &mut self.armature + } + pub(crate) fn add_link( &mut self, parent: Option, // TODO: should be a RigidBodyHandle? @@ -406,6 +433,7 @@ impl Multibody { let len = self.velocities.len(); self.velocities.resize_vertically_mut(len + ndofs, 0.0); self.damping.resize_vertically_mut(len + ndofs, 0.0); + self.armature.resize_vertically_mut(len + ndofs, 0.0); self.accelerations.resize_vertically_mut(len + ndofs, 0.0); self.body_jacobians .extend((0..num_jacobians).map(|_| Jacobian::zeros(0))); @@ -781,11 +809,17 @@ impl Multibody { } /* - * Damping. + * Damping and armature. + * + * Damping is a velocity-proportional force made implicit, so it adds + * `dt · d` to the mass-matrix diagonal. Armature is a "reflected + * rotor inertia" (additional inertia to account for the joint’s + * mass and geometry itself): it adds to the diagonal directly. */ for i in 0..self.ndofs { - self.acc_augmented_mass[(i, i)] += self.damping[i] * dt; - self.augmented_mass[(i, i)] += self.damping[i] * dt; + let diag = self.damping[i] * dt + self.armature[i]; + self.acc_augmented_mass[(i, i)] += diag; + self.augmented_mass[(i, i)] += diag; } let effective_dim = self @@ -887,6 +921,7 @@ impl Multibody { self.velocities = self.velocities.clone().insert_rows(0, SPATIAL_DIM, 0.0); self.damping = self.damping.clone().insert_rows(0, SPATIAL_DIM, 0.0); + self.armature = self.armature.clone().insert_rows(0, SPATIAL_DIM, 0.0); self.accelerations = self.accelerations.clone().insert_rows(0, SPATIAL_DIM, 0.0); @@ -896,6 +931,7 @@ impl Multibody { } else { assert!(self.velocities.len() >= SPATIAL_DIM); assert!(self.damping.len() >= SPATIAL_DIM); + assert!(self.armature.len() >= SPATIAL_DIM); assert!(self.accelerations.len() >= SPATIAL_DIM); let fixed_joint = MultibodyJoint::fixed(root_pose); @@ -907,11 +943,13 @@ impl Multibody { if self.ndofs == 0 { self.velocities = DVector::zeros(0); self.damping = DVector::zeros(0); + self.armature = DVector::zeros(0); self.accelerations = DVector::zeros(0); } else { self.velocities = self.velocities.index((prev_root_ndofs.., 0)).into_owned(); self.damping = self.damping.index((prev_root_ndofs.., 0)).into_owned(); + self.armature = self.armature.index((prev_root_ndofs.., 0)).into_owned(); self.accelerations = self .accelerations .index((prev_root_ndofs.., 0)) From 83cec95780eaf865bc2c5572b8a33937767f98a1 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?S=C3=A9bastien=20Crozet?= Date: Thu, 11 Jun 2026 15:24:18 +0200 Subject: [PATCH 04/16] =?UTF-8?q?checkpoint:=E2=80=AFsupport=20mjcf=20cont?= =?UTF-8?q?rol?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- Cargo.toml | 2 +- crates/mjcf-rs/src/extras.rs | 7 + crates/mjcf-rs/src/loader/blocks.rs | 4 + crates/mjcf-rs/src/loader/defaults.rs | 2 + crates/mjcf-rs/src/loader/prototypes.rs | 8 + crates/mjcf-rs/src/loader/state.rs | 18 +- crates/rapier3d-mjcf/src/loader/runtime.rs | 199 +++++++++++++----- crates/rapier3d-mjcf/tests/cassie_smoke.rs | 34 ++- .../rapier3d-mjcf/tests/loader_regressions.rs | 34 +++ crates/rapier3d-mjcf/tests/phase4.rs | 64 ++++-- crates/rapier3d-mjcf/tests/phase5.rs | 123 +++++++++++ .../rapier3d/tests/loop_closure_com_repro.rs | 176 ++++++++++++++++ crates/rapier3d/tests/multibody_armature.rs | 123 +++++++++++ crates/rapier_testbed2d-f64/Cargo.toml | 2 +- crates/rapier_testbed2d/Cargo.toml | 2 +- crates/rapier_testbed3d/Cargo.toml | 2 +- examples2d/Cargo.toml | 2 +- examples3d-f64/Cargo.toml | 2 +- examples3d/Cargo.toml | 2 +- examples3d/mujoco_menagerie3.rs | 61 ++++-- 20 files changed, 744 insertions(+), 123 deletions(-) create mode 100644 crates/rapier3d/tests/loop_closure_com_repro.rs create mode 100644 crates/rapier3d/tests/multibody_armature.rs diff --git a/Cargo.toml b/Cargo.toml index 9fc3adfc6..842855358 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -102,7 +102,7 @@ oorandom = { version = "11", default-features = false } #xurdf = { path = "../xurdf/xurdf" } #simba = { path = "../simba" } -#kiss3d = { path = "../kiss3d" } +kiss3d = { path = "../kiss3d" } #parry2d = { path = "../parry/crates/parry2d" } #parry3d = { path = "../parry/crates/parry3d" } #parry2d-f64 = { path = "../parry/crates/parry2d-f64" } diff --git a/crates/mjcf-rs/src/extras.rs b/crates/mjcf-rs/src/extras.rs index c98618951..8e1026d67 100644 --- a/crates/mjcf-rs/src/extras.rs +++ b/crates/mjcf-rs/src/extras.rs @@ -32,6 +32,13 @@ pub struct Actuator { pub gainprm: Vec, /// `biasprm` (up to 10 entries). pub biasprm: Vec, + /// `gaintype` (`fixed`, `affine`, `muscle`, …). `None` ⇒ MJCF default + /// (`fixed`). Used to interpret `` actuators. + pub gain_type: Option, + /// `biastype` (`none`, `affine`, `muscle`, …). `None` ⇒ MJCF default + /// (`none`). An `affine` bias turns a `` actuator into a + /// position/velocity servo: `bias = biasprm0 + biasprm1·q + biasprm2·q̇`. + pub bias_type: Option, /// `dyntype`. pub dyn_type: Option, /// `dynprm`. diff --git a/crates/mjcf-rs/src/loader/blocks.rs b/crates/mjcf-rs/src/loader/blocks.rs index 359d32a67..8dde1599a 100644 --- a/crates/mjcf-rs/src/loader/blocks.rs +++ b/crates/mjcf-rs/src/loader/blocks.rs @@ -169,6 +169,8 @@ impl ParseState { force_limited: proto.force_limited.unwrap_or(Tristate::Auto), gainprm: proto.gainprm.unwrap_or_default(), biasprm: proto.biasprm.unwrap_or_default(), + gain_type: proto.gain_type.clone(), + bias_type: proto.bias_type.clone(), dyn_type: proto.dyn_type.clone(), dynprm: proto.dynprm.unwrap_or_default(), kp: proto.kp, @@ -191,6 +193,8 @@ impl ParseState { "forcelimited" => a.force_limited = parse_tristate(attr.value())?, "gainprm" => a.gainprm = parse_f64_list(attr.value())?, "biasprm" => a.biasprm = parse_f64_list(attr.value())?, + "gaintype" => a.gain_type = Some(attr.value().to_string()), + "biastype" => a.bias_type = Some(attr.value().to_string()), "dyntype" => a.dyn_type = Some(attr.value().to_string()), "dynprm" => a.dynprm = parse_f64_list(attr.value())?, "kp" => a.kp = Some(parse_f64(attr.value())?), diff --git a/crates/mjcf-rs/src/loader/defaults.rs b/crates/mjcf-rs/src/loader/defaults.rs index b97985c1c..9acea48f5 100644 --- a/crates/mjcf-rs/src/loader/defaults.rs +++ b/crates/mjcf-rs/src/loader/defaults.rs @@ -304,6 +304,8 @@ impl ParseState { "forcelimited" => p.force_limited = Some(parse_tristate(attr.value())?), "gainprm" => p.gainprm = Some(parse_f64_list(attr.value())?), "biasprm" => p.biasprm = Some(parse_f64_list(attr.value())?), + "gaintype" => p.gain_type = Some(attr.value().to_string()), + "biastype" => p.bias_type = Some(attr.value().to_string()), "dyntype" => p.dyn_type = Some(attr.value().to_string()), "dynprm" => p.dynprm = Some(parse_f64_list(attr.value())?), "kp" => p.kp = Some(parse_f64(attr.value())?), diff --git a/crates/mjcf-rs/src/loader/prototypes.rs b/crates/mjcf-rs/src/loader/prototypes.rs index 8652be7b2..75f232f19 100644 --- a/crates/mjcf-rs/src/loader/prototypes.rs +++ b/crates/mjcf-rs/src/loader/prototypes.rs @@ -106,6 +106,8 @@ pub(super) struct ActuatorPrototype { pub(super) force_limited: Option, pub(super) gainprm: Option>, pub(super) biasprm: Option>, + pub(super) gain_type: Option, + pub(super) bias_type: Option, pub(super) dyn_type: Option, pub(super) dynprm: Option>, pub(super) kp: Option, @@ -288,6 +290,12 @@ pub(super) fn merge_actuator_proto( if top.biasprm.is_some() { acc.biasprm = top.biasprm; } + if top.gain_type.is_some() { + acc.gain_type = top.gain_type; + } + if top.bias_type.is_some() { + acc.bias_type = top.bias_type; + } if top.dyn_type.is_some() { acc.dyn_type = top.dyn_type; } diff --git a/crates/mjcf-rs/src/loader/state.rs b/crates/mjcf-rs/src/loader/state.rs index 7854354d6..ee799e112 100644 --- a/crates/mjcf-rs/src/loader/state.rs +++ b/crates/mjcf-rs/src/loader/state.rs @@ -78,11 +78,17 @@ impl ParseState { } pub(super) fn process_root_children(&mut self, root: Node) -> Result<(), ParseError> { - // Process top-level elements in two passes: - // 1. compiler / option / size / default / asset / contact / equality / tendon - // — anything that doesn't depend on the body tree + // Process top-level elements in phases: + // 1. compiler / option / size / default / contact / equality / tendon + // 1b. asset — deferred past this file's `` blocks. `` + // can legally appear *before* `` in the document (e.g. + // robotiq_2f85's mesh assets precede the defaults that set their + // `scale`), and a mesh resolves its `scale`/`inertia` against its + // default class. Parsing assets after all same-file defaults are + // registered keeps that resolution correct. // 2. worldbody / actuator / sensor / keyframe // can appear anywhere; we resolve recursively in place. + let mut deferred_assets: Vec = Vec::new(); let mut deferred_bodies: Vec = Vec::new(); let mut deferred_actuators: Vec = Vec::new(); let mut deferred_sensors: Vec = Vec::new(); @@ -99,7 +105,7 @@ impl ParseState { // Pass-through / ignored for now. } "default" => self.parse_default(child, None)?, - "asset" => self.parse_asset(child)?, + "asset" => deferred_assets.push(child), "include" => self.parse_include(child, /*nested=*/ false)?, "worldbody" => deferred_bodies.push(child), "contact" => self.parse_contact(child)?, @@ -115,6 +121,10 @@ impl ParseState { } } } + // Phase 1b — assets, now that every same-file `` is registered. + for n in deferred_assets { + self.parse_asset(n)?; + } // Pass 2 — needs the full / table. for n in deferred_bodies { self.parse_worldbody(n)?; diff --git a/crates/rapier3d-mjcf/src/loader/runtime.rs b/crates/rapier3d-mjcf/src/loader/runtime.rs index c36f6a76a..27d430013 100644 --- a/crates/rapier3d-mjcf/src/loader/runtime.rs +++ b/crates/rapier3d-mjcf/src/loader/runtime.rs @@ -5,7 +5,8 @@ use mjcf_rs::extras::Keyframe; use rapier3d::dynamics::{ - ImpulseJointHandle, ImpulseJointSet, JointAxis, RigidBody, RigidBodySet, RigidBodyType, + GenericJoint, ImpulseJointHandle, ImpulseJointSet, JointAxis, MultibodyJointHandle, + MultibodyJointSet, RigidBody, RigidBodySet, RigidBodyType, }; use rapier3d::math::{Pose, Real, Rotation, Vector}; @@ -204,73 +205,157 @@ impl MjcfRobotHandles { /// - everything else (`general`, `intvelocity`, …) is left to the user /// — the metadata is preserved on `Self::actuators`. pub fn apply_controls(&self, joints: &mut ImpulseJointSet, ctrl: &[Real]) { - use mjcf_rs::extras::ActuatorKind; for (i, ah) in self.actuators.iter().enumerate() { let Some(handle) = ah.joint else { continue }; let Some(joint) = joints.get_mut(handle, true) else { continue; }; let u = ctrl.get(i).copied().unwrap_or(0.0); - // Determine the axis the actuator drives — for hinge/ball, the - // angular X axis is the joint's free axis (after the per-joint - // basis rotation we applied in build_serial_joint). - let ax = JointAxis::AngX; // default (hinge / ball) - let lin_ax = JointAxis::LinX; - let kp = ah.actuator.kp.unwrap_or_default() as Real; - let kv = ah.actuator.kv.unwrap_or_default() as Real; - let gear = ah.actuator.gear[0] as Real; - let force_max = ah - .actuator - .force_range - .map(|r| r[1].abs() as Real) - .unwrap_or(Real::INFINITY); - match ah.actuator.kind { - ActuatorKind::Motor => { - // MJCF `` is a *force* (or torque) input: it adds - // `ctrl * gear` to the joint's generalized force each - // step. Rapier's joint motors have no "constant force" - // mode, so we emulate it by aiming the motor at a very - // large velocity in the direction of the desired force, - // then capping the applied force at `|ctrl * gear|`. - // With `stiffness = damping = 0` plus a hard cap, the - // motor saturates at its `max_force` as long as the - // joint hasn't reached the (very far) target velocity — - // which it never does — so the effective output is a - // constant `ctrl * gear` along the joint axis. - // - // We don't know whether the joint is rotational or - // prismatic, so we configure both axes and let the - // locked one's motor be a no-op. - let force = u * gear; - let max = force.abs().min(force_max); - let far_vel = if force >= 0.0 { 1.0e9 } else { -1.0e9 }; - joint.data.set_motor_velocity(ax, far_vel, 0.0); - joint.data.set_motor_velocity(lin_ax, far_vel, 0.0); - joint.data.set_motor_max_force(ax, max); - joint.data.set_motor_max_force(lin_ax, max); - } - ActuatorKind::Position => { - joint.data.set_motor_position(ax, u, kp, kv); - joint.data.set_motor_position(lin_ax, u, kp, kv); - } - ActuatorKind::Velocity => { - joint.data.set_motor_velocity(ax, u, kv); - joint.data.set_motor_velocity(lin_ax, u, kv); - } - ActuatorKind::Damper => { - let damp = - ah.actuator.gainprm.first().copied().unwrap_or(0.0) as Real * u.abs(); - joint.data.set_motor_velocity(ax, 0.0, damp); - joint.data.set_motor_velocity(lin_ax, 0.0, damp); - } - _ => { - // intvelocity / general / other — user-driven. - } + configure_actuator_motor(&mut joint.data, &ah.actuator, u); + } + } +} + +impl MjcfRobotHandles> { + /// Apply per-actuator control values to the multibody joints they drive. + /// + /// This is the multibody-path counterpart of + /// [`apply_controls`](MjcfRobotHandles::::apply_controls): + /// it reproduces MuJoCo's "actuation" by writing each actuator's motor + /// onto the multibody link it drives. The per-actuator interpretation is + /// identical (see [`configure_actuator_motor`]); call it once per frame + /// before `pipeline.step`. + /// + /// `ctrl` is a flat array of one scalar per actuator, in the order of + /// [`MjcfRobot::actuators`] (and [`MjcfRobotHandles::actuators`]). + /// Actuators whose joint was dropped from the multibody chain (a loop + /// closure) are skipped. + /// + /// Each driven body is woken so a steady control input keeps holding the + /// pose even after the multibody would otherwise have gone to sleep + /// (mirroring the impulse path, which wakes through the joint graph). + pub fn apply_controls_multibody( + &self, + bodies: &mut RigidBodySet, + multibody_joints: &mut MultibodyJointSet, + ctrl: &[Real], + ) { + for (i, ah) in self.actuators.iter().enumerate() { + // `H` is `Option` here, so `ah.joint` is + // `Option>`: the outer `None` means "no actuator joint", + // the inner `None` means "joint dropped as a loop closure". + let Some(Some(handle)) = ah.joint else { continue }; + let Some((mb, link_id)) = multibody_joints.get_mut(handle) else { + continue; + }; + let Some(link) = mb.links_mut().nth(link_id) else { + continue; + }; + let u = ctrl.get(i).copied().unwrap_or(0.0); + configure_actuator_motor(&mut link.joint.data, &ah.actuator, u); + let rb = link.rigid_body_handle(); + if let Some(body) = bodies.get_mut(rb) { + body.wake_up(true); } } } } +/// Write the joint-motor configuration for one MJCF actuator driving one +/// rapier joint, given the control value `u`. Shared by the impulse- and +/// multibody-joint control paths. MJCF semantics: +/// +/// - `` → constant generalized force `u * gear` (emulated with a +/// far-target velocity motor capped at `|u * gear|`). +/// - `` → `set_motor_position(u, kp, kv)`. +/// - `` → `set_motor_velocity(u, kv)`. +/// - `` → zero-velocity target with damping `gainprm[0] * |u|`. +/// - `` with `biastype="affine"` → a position/velocity servo. The +/// affine model `force = gainprm0·u + biasprm0 + biasprm1·q + biasprm2·q̇` +/// matches a spring `stiffness·(target − q) − damping·q̇` with +/// `stiffness = −biasprm1`, `damping = −biasprm2`, and +/// `target = (gainprm0·u + biasprm0) / stiffness`. This is what holds e.g. +/// the flybody legs in their commanded pose. +/// - `` with a non-affine bias → constant force `gainprm0·u * gear` +/// (same emulation as ``). +/// - other kinds (`intvelocity`, muscle, …) are left untouched. +/// +/// The joint's free axis is the angular (hinge/ball) or linear (slide) X axis +/// after the per-joint basis rotation applied in `build_serial_joint`; we +/// configure both and let the locked one's motor be a no-op. +fn configure_actuator_motor( + data: &mut GenericJoint, + actuator: &mjcf_rs::extras::Actuator, + u: Real, +) { + use mjcf_rs::extras::ActuatorKind; + + let ax = JointAxis::AngX; + let lin_ax = JointAxis::LinX; + let kp = actuator.kp.unwrap_or_default() as Real; + let kv = actuator.kv.unwrap_or_default() as Real; + let gear = actuator.gear[0] as Real; + let force_max = actuator + .force_range + .map(|r| r[1].abs() as Real) + .unwrap_or(Real::INFINITY); + + // Emulate a constant generalized force: aim the motor at a far-away + // velocity in the force's direction and cap the impulse at the force + // magnitude. With stiffness = damping = 0, the motor saturates at its + // max_force every step (the target is never reached), so the net effect + // is a constant force along the joint axis. + let apply_constant_force = |data: &mut GenericJoint, force: Real| { + let max = force.abs().min(force_max); + let far_vel = if force >= 0.0 { 1.0e9 } else { -1.0e9 }; + data.set_motor_velocity(ax, far_vel, 0.0); + data.set_motor_velocity(lin_ax, far_vel, 0.0); + data.set_motor_max_force(ax, max); + data.set_motor_max_force(lin_ax, max); + }; + + match actuator.kind { + ActuatorKind::Motor => apply_constant_force(data, u * gear), + ActuatorKind::Position => { + data.set_motor_position(ax, u, kp, kv); + data.set_motor_position(lin_ax, u, kp, kv); + } + ActuatorKind::Velocity => { + data.set_motor_velocity(ax, u, kv); + data.set_motor_velocity(lin_ax, u, kv); + } + ActuatorKind::Damper => { + let damp = actuator.gainprm.first().copied().unwrap_or(0.0) as Real * u.abs(); + data.set_motor_velocity(ax, 0.0, damp); + data.set_motor_velocity(lin_ax, 0.0, damp); + } + ActuatorKind::General => { + let g0 = actuator.gainprm.first().copied().unwrap_or(0.0) as Real; + let b0 = actuator.biasprm.first().copied().unwrap_or(0.0) as Real; + let b1 = actuator.biasprm.get(1).copied().unwrap_or(0.0) as Real; + let b2 = actuator.biasprm.get(2).copied().unwrap_or(0.0) as Real; + let affine = actuator.bias_type.as_deref() == Some("affine"); + // A position servo needs a real restoring term (`-biasprm1·q`, + // i.e. stiffness > 0); without it the affine bias is just a + // constant + velocity-damping force. + if affine && -b1 > 0.0 { + let stiffness = -b1; + let damping = -b2; + let target = (g0 * u + b0) / stiffness; + data.set_motor_position(ax, target, stiffness, damping); + data.set_motor_position(lin_ax, target, stiffness, damping); + } else { + // `gaintype="fixed"`, non-restoring bias: constant force + // `gainprm0·u` through the transmission. + apply_constant_force(data, g0 * u * gear); + } + } + ActuatorKind::IntVelocity | ActuatorKind::Other => { + // Not driven automatically — user-controlled. + } + } +} + /// One scalar / vector / quaternion reading from a sensor. #[derive(Copy, Clone, Debug, PartialEq)] pub enum MjcfSensorValue { diff --git a/crates/rapier3d-mjcf/tests/cassie_smoke.rs b/crates/rapier3d-mjcf/tests/cassie_smoke.rs index 61a74e89c..2431e171f 100644 --- a/crates/rapier3d-mjcf/tests/cassie_smoke.rs +++ b/crates/rapier3d-mjcf/tests/cassie_smoke.rs @@ -89,26 +89,17 @@ fn cassie_multibody_loads_without_nan() { } /// Cassie has 4 `` loop closures (achilles-rod and -/// plantar-rod tie the foot/heel-spring back into the chain). Those loops -/// rely on the `` values declared on the leg joints to stay -/// numerically stable — without that damping, the impulse-joint solver -/// can drift under default-timestep multibody simulation and blow up -/// within a second of sim time. -/// -/// This regression captures that mode: keep equality constraints active -/// but disable the motors and verify the failure mode is reproducible -/// (we don't claim it should work — but the test documents the trade-off). -/// `disable_joint_motors=true` skips the motors used for springs and -/// friction-loss but does NOT skip the per-DoF damping path (per-DoF -/// damping is dynamics-level, not a motor). Cassie still blows up under -/// this configuration however: its `` springs on the -/// shin and heel-spring joints are structural to keeping the loop -/// closures balanced, and removing the springs (motors) destabilizes the -/// loops faster than the surviving damping can compensate. +/// plantar-rod tie the foot/heel-spring back into the chain). This +/// configuration — loop closures active, joint motors disabled — used to +/// NaN within ~40 steps because of two bugs in the generic impulse-joint +/// solver path (com-shifted anchors composed with origin-centered link +/// poses, and a per-link two-block effective mass that dropped the +/// `J1ᵀ·W·J2` coupling for constraints between links of the same +/// multibody). With those fixed, the dangling legs just swing and the +/// closures hold; this regression keeps that configuration NaN-free. #[test] -#[ignore = "requires the cassie assets — documents an unstable configuration"] -#[should_panic(expected = "NaN")] -fn cassie_unstable_without_motors_and_with_loop_closures() { +#[ignore = "requires the cassie assets"] +fn cassie_stable_without_motors_and_with_loop_closures() { let options = MjcfLoaderOptions { scale: 10.0, skip_plane_geoms: true, @@ -137,14 +128,13 @@ fn cassie_unstable_without_motors_and_with_loop_closures() { MjcfMultibodyOptions::DISABLE_SELF_CONTACTS, ); - // 50 steps is enough — without damping the loops typically NaN by - // step ~40 under default integration parameters. + // The broken solver used to NaN by step ~40; run well past that. step( &mut bodies, &mut colliders, &mut impulse_joints, &mut multibody_joints, - 50, + 500, gravity, ); } diff --git a/crates/rapier3d-mjcf/tests/loader_regressions.rs b/crates/rapier3d-mjcf/tests/loader_regressions.rs index 66316e446..03893b326 100644 --- a/crates/rapier3d-mjcf/tests/loader_regressions.rs +++ b/crates/rapier3d-mjcf/tests/loader_regressions.rs @@ -548,3 +548,37 @@ mod mesh_synthesis { ); } } + +/// `` may legally appear *before* `` in an MJCF document, and a +/// `` resolves its `scale` against its default class. The loader must +/// register all `` blocks before parsing `` so the scale is +/// applied regardless of document order. +/// +/// Regression: robotiq_2f85 declares its mesh assets before the `` +/// that sets `scale="0.001"`. With document-order parsing the meshes kept +/// `scale=[1,1,1]`, so mesh-derived masses (base_mount/silicone_pad, which +/// have no ``) came out ~1e9x too large, producing an +/// ill-conditioned multibody mass matrix that exploded to NaN. +#[test] +fn mesh_scale_inherits_default_when_asset_precedes_default() { + let xml = r#" + + + + + + + + + + + + "#; + let (_, model) = MjcfRobot::from_str(xml, MjcfLoaderOptions::default(), ".").unwrap(); + let mesh = model.assets.mesh("part").expect("mesh `part` should be parsed"); + assert_eq!( + mesh.scale, + [0.001, 0.001, 0.001], + "mesh did not inherit scale from its default class when preceded " + ); +} diff --git a/crates/rapier3d-mjcf/tests/phase4.rs b/crates/rapier3d-mjcf/tests/phase4.rs index 025a4ac62..f093d77aa 100644 --- a/crates/rapier3d-mjcf/tests/phase4.rs +++ b/crates/rapier3d-mjcf/tests/phase4.rs @@ -70,37 +70,44 @@ fn springref_is_baked_in_radians() { } #[test] -fn armature_changes_local_mprops_inertia() { - // Direct check that armature is reflected in the body's - // additional_local_mprops (before insertion). The local inertia about - // the joint axis should be larger with armature than without. +fn armature_routes_through_per_dof_armature_not_spatial_inertia() { + // MuJoCo's `armature` is a reflected rotor inertia that belongs on the + // diagonal of the joint-space mass matrix, NOT in the link's spatial + // inertia tensor. On the multibody path it must land in the multibody's + // per-DoF armature vector and leave the body's spatial inertia untouched + // (baking it into the tensor produces extreme anisotropy and an + // ill-conditioned mass matrix — see the low_cost_robot_arm regression). let xml_arm = r#" - + "#; let xml_no = r#" - + "#; - let (rob_arm, _) = MjcfRobot::from_str(xml_arm, MjcfLoaderOptions::default(), ".").unwrap(); - let (rob_no, _) = MjcfRobot::from_str(xml_no, MjcfLoaderOptions::default(), ".").unwrap(); - // Insert into a rapier set so update_mass_properties runs. - fn principal_xx(robot: MjcfRobot) -> Real { + + // Returns (spatial inertia trace after a step, armature on the joint DoF). + fn inspect(xml: &str) -> (Real, Real) { + let (robot, _) = MjcfRobot::from_str(xml, MjcfLoaderOptions::default(), ".").unwrap(); let mut bodies = RigidBodySet::new(); let mut colliders = ColliderSet::new(); let mut impulse_joints = ImpulseJointSet::new(); - let handles = - robot.insert_using_impulse_joints(&mut bodies, &mut colliders, &mut impulse_joints); - // Trigger a step so update_mass_properties fires. let mut multibody_joints = MultibodyJointSet::new(); - let gravity = Vector::new(0.0, 0.0, 0.0); + let handles = robot.insert_using_multibody_joints( + &mut bodies, + &mut colliders, + &mut multibody_joints, + &mut impulse_joints, + rapier3d_mjcf::MjcfMultibodyOptions::empty(), + ); + // Step so update_mass_properties / update_mass_matrix fire. step_n( &mut bodies, &mut colliders, &mut impulse_joints, &mut multibody_joints, 1, - gravity, + Vector::new(0.0, 0.0, 0.0), ); let h = handles.bodies[1].as_ref().unwrap().body; let p = bodies @@ -109,12 +116,29 @@ fn armature_changes_local_mprops_inertia() { .mass_properties() .local_mprops .principal_inertia(); - // Sum to be insensitive to eigenvalue ordering. - p.x + p.y + p.z + let trace = p.x + p.y + p.z; + let mb_handle = handles.joints[0].joint.expect("multibody joint inserted"); + let (mb, _) = multibody_joints.get_mut(mb_handle).unwrap(); + // The hinge's free DoF lives at the end of the generalized vectors + // (after the fixed root's zero DoFs). + let last = mb.armature().len() - 1; + (trace, mb.armature()[last]) } - let arm = principal_xx(rob_arm); - let no = principal_xx(rob_no); - assert!(arm > no + 0.4, "arm trace = {arm}, no trace = {no}"); + + let (arm_trace, arm_value) = inspect(xml_arm); + let (no_trace, no_value) = inspect(xml_no); + + // Armature shows up in the per-DoF armature vector... + assert!( + (arm_value - 0.5).abs() < 1e-6, + "armature on DoF = {arm_value}, expected 0.5" + ); + assert!(no_value.abs() < 1e-6, "no-armature DoF = {no_value}"); + // ...and does NOT inflate the spatial inertia tensor. + assert!( + (arm_trace - no_trace).abs() < 1e-6, + "spatial inertia changed by armature: arm = {arm_trace}, no = {no_trace}" + ); } #[test] diff --git a/crates/rapier3d-mjcf/tests/phase5.rs b/crates/rapier3d-mjcf/tests/phase5.rs index 28d210370..3a153f222 100644 --- a/crates/rapier3d-mjcf/tests/phase5.rs +++ b/crates/rapier3d-mjcf/tests/phase5.rs @@ -102,6 +102,129 @@ fn motor_actuator_drives_slider() { assert!(cart.linvel().x > 0.5, "cart vx = {}", cart.linvel().x); } +// An affine `` actuator is a position servo: +// force = gainprm0·ctrl + biasprm0 + biasprm1·q + biasprm2·q̇. +// With gainprm=[k], biasprm=[0,-k,0] it holds the joint at `ctrl`. This is +// the actuator type the flybody legs use. `base` is welded to the world (no +// joint ⇒ fixed) so the `arm` hinge actually rotates in the world frame. +const AFFINE_SERVO_XML: &str = r#" + + +"#; + +/// The rapier handle of the `arm` body in the model above. +fn arm_handle(robot: &MjcfRobot, handles: &rapier3d_mjcf::MjcfRobotHandles) -> RigidBodyHandle { + for (i, bh) in handles.bodies.iter().enumerate() { + if robot.bodies[i].name.as_deref() == Some("arm") { + return bh.as_ref().unwrap().body; + } + } + panic!("arm body not found"); +} + +#[test] +fn affine_general_actuator_parses_as_servo() { + let (robot, _) = MjcfRobot::from_str(AFFINE_SERVO_XML, MjcfLoaderOptions::default(), ".").unwrap(); + let a = &robot.actuators[0].actuator; + assert_eq!(a.bias_type.as_deref(), Some("affine")); + assert_eq!(a.gainprm.first().copied(), Some(10.0)); + assert_eq!(a.biasprm.get(1).copied(), Some(-10.0)); + assert_eq!(a.biasprm.get(2).copied(), Some(-2.0)); +} + +/// Drive the AFFINE_SERVO_XML model to a target angle and return the arm's +/// final rotation about Z. `multibody` selects the insertion + control path. +/// Re-applies the control every frame against a persistent solver state (so +/// it also exercises the per-frame wake path), and uses one `IslandManager` +/// across the whole run. +fn drive_servo_to(target: Real, multibody: bool) -> Real { + use rapier3d_mjcf::MjcfMultibodyOptions; + let (robot, _) = MjcfRobot::from_str(AFFINE_SERVO_XML, MjcfLoaderOptions::default(), ".").unwrap(); + let mut bodies = RigidBodySet::new(); + let mut colliders = ColliderSet::new(); + let mut impulse_joints = ImpulseJointSet::new(); + let mut multibody_joints = MultibodyJointSet::new(); + + enum Ctl { + Impulse(rapier3d_mjcf::MjcfRobotHandles), + Multibody(rapier3d_mjcf::MjcfRobotHandles>), + } + let (ctl, arm) = if multibody { + let handles = robot.clone().insert_using_multibody_joints( + &mut bodies, + &mut colliders, + &mut multibody_joints, + &mut impulse_joints, + MjcfMultibodyOptions::empty(), + ); + let arm = arm_handle(&robot, &handles); + (Ctl::Multibody(handles), arm) + } else { + let handles = + robot.clone().insert_using_impulse_joints(&mut bodies, &mut colliders, &mut impulse_joints); + let arm = arm_handle(&robot, &handles); + (Ctl::Impulse(handles), arm) + }; + + let mut ccd = CCDSolver::new(); + let mut pipeline = PhysicsPipeline::new(); + let ip = IntegrationParameters::default(); + let mut islands = IslandManager::new(); + let mut broad_phase = DefaultBroadPhase::new(); + let mut narrow_phase = NarrowPhase::new(); + for _ in 0..40 { + match &ctl { + Ctl::Impulse(h) => h.apply_controls(&mut impulse_joints, &[target]), + Ctl::Multibody(h) => { + h.apply_controls_multibody(&mut bodies, &mut multibody_joints, &[target]) + } + } + pipeline.step( + Vector::new(0.0, 0.0, 0.0), + &ip, + &mut islands, + &mut broad_phase, + &mut narrow_phase, + &mut bodies, + &mut colliders, + &mut impulse_joints, + &mut multibody_joints, + &mut ccd, + &(), + &(), + ); + } + bodies[arm].rotation().to_scaled_axis().z +} + +#[test] +fn affine_general_actuator_drives_hinge_impulse_path() { + let angle = drive_servo_to(0.5, false); + assert!((angle - 0.5).abs() < 0.1, "servo didn't reach target: angle = {angle}"); +} + +#[test] +fn affine_general_actuator_drives_hinge_multibody_path() { + let angle = drive_servo_to(0.5, true); + assert!((angle - 0.5).abs() < 0.1, "multibody servo didn't reach target: angle = {angle}"); +} + #[test] fn keyframe_mocap_application() { let xml = r#" diff --git a/crates/rapier3d/tests/loop_closure_com_repro.rs b/crates/rapier3d/tests/loop_closure_com_repro.rs new file mode 100644 index 000000000..46f3ff5b0 --- /dev/null +++ b/crates/rapier3d/tests/loop_closure_com_repro.rs @@ -0,0 +1,176 @@ +//! Repro for loop-closure instability on multibodies (mujoco_menagerie3 / +//! agility_cassie symptom): impulse joints between links of the same +//! multibody, with off-origin link centers-of-mass. +//! +//! Chain: fixed base -> link1 -> link2 (revolute multibody joints), plus a +//! loop-closure impulse joint (3 locked linear axes) between link1 and link2 +//! whose anchors coincide exactly in the initial configuration. +//! +//! Two solver bugs used to make this scene explode (|v| ~ 1e5 within one +//! step) or freeze solid: +//! +//! - The generic external constraint builder applied the com-shift of +//! `transform_to_solver_body_space` to multibody links, but a link's solver +//! pose (`link.local_to_world`) is origin-centered, unlike a regular +//! rigid-body's com-centered solver pose. The resulting anchors were +//! displaced by `-R·local_com`, creating a phantom position error that the +//! solver could never resolve -> energy pumped every step. +//! +//! - When both endpoints of a generic constraint are links of the same +//! multibody, the per-row effective mass was computed from two independent +//! jacobian blocks (`J1ᵀWJ1 + J2ᵀWJ2`), dropping the `J1ᵀWJ2` coupling of +//! the shared generalized velocities. Rows whose true relative jacobian +//! (`J2 - J1`) nearly vanishes degenerated into divisions by float noise, +//! producing garbage impulses in the millions. + +use rapier3d::prelude::*; + +struct World { + bodies: RigidBodySet, + colliders: ColliderSet, + impulse_joints: ImpulseJointSet, + multibody_joints: MultibodyJointSet, + link1: RigidBodyHandle, + link2: RigidBodyHandle, +} + +/// Builds the double-pendulum-with-closure scene. The closure pins the +/// world point `closure_anchor` (expressed in link1's local frame; it must +/// lie at `closure_anchor + (1, 0, 0)` in world space which is also used to +/// derive link2's local anchor so the constraint starts exactly satisfied). +fn build(use_multibody: bool, closure_anchor1: Vector) -> World { + let mut bodies = RigidBodySet::new(); + let colliders = ColliderSet::new(); + let mut impulse_joints = ImpulseJointSet::new(); + let mut multibody_joints = MultibodyJointSet::new(); + + // Distinct off-origin COMs (like cassie's achilles rod / heel spring). + let com1 = Vector::new(0.0, 0.5, 0.0); + let com2 = Vector::new(0.0, -0.3, 0.0); + let inertia = Vector::new(0.1, 0.1, 0.1); + + let base = bodies.insert(RigidBodyBuilder::fixed()); + let link1 = bodies.insert( + RigidBodyBuilder::dynamic() + .translation(Vector::new(1.0, 0.0, 0.0)) + .additional_mass_properties(MassProperties::new(com1, 1.0, inertia)), + ); + let link2 = bodies.insert( + RigidBodyBuilder::dynamic() + .translation(Vector::new(2.0, 0.0, 0.0)) + .additional_mass_properties(MassProperties::new(com2, 1.0, inertia)), + ); + + let rev1 = RevoluteJointBuilder::new(Vector::Z) + .local_anchor1(Vector::new(0.0, 0.0, 0.0)) + .local_anchor2(Vector::new(-1.0, 0.0, 0.0)); + let rev2 = RevoluteJointBuilder::new(Vector::Z) + .local_anchor1(Vector::new(1.0, 0.0, 0.0)) + .local_anchor2(Vector::new(0.0, 0.0, 0.0)); + + if use_multibody { + multibody_joints.insert(base, link1, rev1, true).unwrap(); + multibody_joints.insert(link1, link2, rev2, true).unwrap(); + } else { + impulse_joints.insert(base, link1, rev1, true); + impulse_joints.insert(link1, link2, rev2, true); + } + + // Loop closure between a point of link1 and the same world point on + // link2 (link2's origin sits at (2, 0, 0)). + let anchor2 = Vector::new(1.0, 0.0, 0.0) + closure_anchor1 - Vector::new(2.0, 0.0, 0.0); + let closure = GenericJointBuilder::new(JointAxesMask::LIN_AXES) + .local_frame1(Pose::from_translation(closure_anchor1)) + .local_frame2(Pose::from_translation(anchor2)) + .build(); + impulse_joints.insert(link1, link2, closure, true); + + World { + bodies, + colliders, + impulse_joints, + multibody_joints, + link1, + link2, + } +} + +/// Steps 100 frames under gravity; returns the max linear velocity reached +/// and the max world-space distance between the two closure anchors. +fn simulate(w: &mut World, closure_anchor1: Vector) -> (Real, Real) { + let anchor2 = Vector::new(1.0, 0.0, 0.0) + closure_anchor1 - Vector::new(2.0, 0.0, 0.0); + let gravity = Vector::new(0.0, -9.81, 0.0); + let integration_parameters = IntegrationParameters::default(); + let mut pipeline = PhysicsPipeline::new(); + let mut islands = IslandManager::new(); + let mut broad_phase = DefaultBroadPhase::new(); + let mut narrow_phase = NarrowPhase::new(); + let mut ccd = CCDSolver::new(); + + let mut max_vel: Real = 0.0; + let mut max_anchor_err: Real = 0.0; + for _ in 0..100 { + pipeline.step( + gravity, + &integration_parameters, + &mut islands, + &mut broad_phase, + &mut narrow_phase, + &mut w.bodies, + &mut w.colliders, + &mut w.impulse_joints, + &mut w.multibody_joints, + &mut ccd, + &(), + &(), + ); + for h in [w.link1, w.link2] { + max_vel = max_vel.max(w.bodies[h].linvel().length()); + } + let p1 = w.bodies[w.link1].position() * closure_anchor1; + let p2 = w.bodies[w.link2].position() * anchor2; + max_anchor_err = max_anchor_err.max((p2 - p1).length()); + } + (max_vel, max_anchor_err) +} + +/// Closure anchored exactly at rev2's pivot: it is geometrically redundant +/// (the pivot is already pinned), so the scene must behave like a free +/// double pendulum — swinging (not frozen by garbage impulses from the +/// degenerate constraint rows), and not exploding. +#[test] +fn redundant_loop_closure_on_multibody_links() { + let anchor = Vector::new(1.0, 0.0, 0.0); // world (2, 0, 0) = rev2 pivot + let mut w = build(true, anchor); + let (max_vel, _) = simulate(&mut w, anchor); + println!("multibody, redundant closure: max |linvel| = {max_vel}"); + assert!(max_vel < 50.0, "system exploded: {max_vel}"); + assert!(max_vel > 1.0, "system is frozen: {max_vel}"); +} + +/// Closure anchored away from rev2's pivot: together with rev2 it welds +/// link1 and link2, so the assembly swings as a rigid compound pendulum. +/// The closure must hold (anchors stay coincident) without exploding. +#[test] +fn welding_loop_closure_on_multibody_links() { + let anchor = Vector::new(2.0, 0.0, 0.0); // world (3, 0, 0) + let mut w = build(true, anchor); + let (max_vel, max_err) = simulate(&mut w, anchor); + println!("multibody, welding closure: max |linvel| = {max_vel}, max anchor err = {max_err}"); + assert!(max_vel < 50.0, "system exploded: {max_vel}"); + assert!(max_vel > 1.0, "system is frozen: {max_vel}"); + assert!(max_err < 1.0e-2, "loop closure not enforced: {max_err}"); +} + +/// Same scenes built with impulse joints everywhere (no multibody), as a +/// sanity reference for the regular two-body solver path. +#[test] +fn welding_loop_closure_on_rigid_bodies() { + let anchor = Vector::new(2.0, 0.0, 0.0); + let mut w = build(false, anchor); + let (max_vel, max_err) = simulate(&mut w, anchor); + println!("rigid bodies, welding closure: max |linvel| = {max_vel}, max anchor err = {max_err}"); + assert!(max_vel < 50.0, "system exploded: {max_vel}"); + assert!(max_vel > 1.0, "system is frozen: {max_vel}"); + assert!(max_err < 1.0e-2, "loop closure not enforced: {max_err}"); +} diff --git a/crates/rapier3d/tests/multibody_armature.rs b/crates/rapier3d/tests/multibody_armature.rs new file mode 100644 index 000000000..60a4db0f6 --- /dev/null +++ b/crates/rapier3d/tests/multibody_armature.rs @@ -0,0 +1,123 @@ +//! Armature is a per-DoF reflected rotor inertia added to the diagonal of a +//! multibody's generalized mass matrix (MuJoCo `` semantics). +//! +//! These tests pin the two properties that define it: +//! 1. It resists joint acceleration — a heavier rotor makes the joint spin up +//! more slowly under the same torque — and behaves like a genuine inertia +//! (no `dt` scaling, no velocity-proportional force, unlike damping). +//! 2. It is a joint-space quantity only: it does NOT modify the link's spatial +//! (3x3) inertia tensor. + +use rapier3d::prelude::*; + +/// Build a fixed base + one dynamic link joined by a revolute multibody joint, +/// with the link's center of mass offset from the hinge so gravity applies a +/// torque. Optionally set `armature` on the joint DoF. Returns the joint's +/// angular velocity magnitude after `steps` steps. +fn spin_up(armature: Real, steps: usize) -> Real { + let mut bodies = RigidBodySet::new(); + let mut colliders = ColliderSet::new(); + let mut impulse_joints = ImpulseJointSet::new(); + let mut multibody_joints = MultibodyJointSet::new(); + + let base = bodies.insert(RigidBodyBuilder::fixed()); + // COM offset 1m along +x from the hinge (hinge about Z at the origin). + let link = bodies.insert( + RigidBodyBuilder::dynamic() + .translation(Vector::new(1.0, 0.0, 0.0)) + .additional_mass_properties(MassProperties::new( + Vector::ZERO, + 1.0, + Vector::new(0.01, 0.01, 0.01), + )), + ); + let rev = RevoluteJointBuilder::new(Vector::Z) + .local_anchor1(Vector::new(0.0, 0.0, 0.0)) + .local_anchor2(Vector::new(-1.0, 0.0, 0.0)); + let handle = multibody_joints.insert(base, link, rev, true).unwrap(); + + let gravity = Vector::new(0.0, -9.81, 0.0); + let integration_parameters = IntegrationParameters::default(); + let mut pipeline = PhysicsPipeline::new(); + let mut islands = IslandManager::new(); + let mut broad_phase = DefaultBroadPhase::new(); + let mut narrow_phase = NarrowPhase::new(); + let mut ccd = CCDSolver::new(); + + let mut step_once = |bodies: &mut RigidBodySet, mbj: &mut MultibodyJointSet| { + pipeline.step( + gravity, + &integration_parameters, + &mut islands, + &mut broad_phase, + &mut narrow_phase, + bodies, + &mut colliders, + &mut impulse_joints, + mbj, + &mut ccd, + &(), + &(), + ); + }; + + // Warm up one step so the root joint type settles (a fixed base starts as + // a 6-DoF free root and collapses to 0 DoFs on the first step, which + // re-indexes the generalized vectors). Both branches take this identical + // step, so it doesn't bias the comparison. + step_once(&mut bodies, &mut multibody_joints); + + if armature != 0.0 { + let (mb, _) = multibody_joints.get_mut(handle).unwrap(); + // Root is now fixed (0 DoFs); the single revolute DoF is all that's + // left in the generalized vectors. + let v = mb.armature_mut(); + assert_eq!(v.len(), 1, "expected one DoF after the root settled"); + v[0] = armature; + } + + for _ in 0..steps { + step_once(&mut bodies, &mut multibody_joints); + } + bodies[link].angvel().length() +} + +#[test] +fn armature_resists_joint_acceleration() { + // The link's own inertia about the hinge is dominated by m·r² = 1·1² = 1. + // An armature of 4 roughly quintuples the effective rotor inertia, so the + // joint should spin up much more slowly under the same gravity torque. + let w_no = spin_up(0.0, 30); + let w_arm = spin_up(4.0, 30); + + assert!(w_no > 0.0, "control case didn't move: {w_no}"); + assert!( + w_arm < w_no * 0.5, + "armature didn't slow the joint enough: no_armature={w_no}, armature={w_arm}" + ); +} + +#[test] +fn armature_does_not_change_spatial_inertia() { + // Adding armature must not touch the link's spatial mass properties — it's + // purely a generalized-mass-matrix diagonal term. + let mut bodies = RigidBodySet::new(); + let mut multibody_joints = MultibodyJointSet::new(); + let base = bodies.insert(RigidBodyBuilder::fixed()); + let inertia = Vector::new(0.01, 0.02, 0.03); + let link = bodies.insert( + RigidBodyBuilder::dynamic() + .translation(Vector::new(1.0, 0.0, 0.0)) + .additional_mass_properties(MassProperties::new(Vector::ZERO, 1.0, inertia)), + ); + let rev = RevoluteJointBuilder::new(Vector::Z).local_anchor2(Vector::new(-1.0, 0.0, 0.0)); + let handle = multibody_joints.insert(base, link, rev, true).unwrap(); + + let before = bodies[link].mass_properties().local_mprops.principal_inertia(); + + let (mb, _) = multibody_joints.get_mut(handle).unwrap(); + mb.armature_mut()[0] = 5.0; + + let after = bodies[link].mass_properties().local_mprops.principal_inertia(); + assert_eq!(before, after, "armature must not alter the spatial inertia tensor"); +} diff --git a/crates/rapier_testbed2d-f64/Cargo.toml b/crates/rapier_testbed2d-f64/Cargo.toml index c2fff2751..516613f71 100644 --- a/crates/rapier_testbed2d-f64/Cargo.toml +++ b/crates/rapier_testbed2d-f64/Cargo.toml @@ -60,7 +60,7 @@ puffin_egui = { version = "0.29", optional = true } serde = { version = "1", features = ["derive"] } serde_json = "1" indexmap = { version = "2", features = ["serde"] } -kiss3d = { version = "0.43.0", features = ["egui", "serde"] } +kiss3d = { version = "0.44.0", features = ["egui", "serde"] } log = "0.4" env_logger = "0.11" diff --git a/crates/rapier_testbed2d/Cargo.toml b/crates/rapier_testbed2d/Cargo.toml index b7da9e620..aa4517107 100644 --- a/crates/rapier_testbed2d/Cargo.toml +++ b/crates/rapier_testbed2d/Cargo.toml @@ -60,7 +60,7 @@ puffin_egui = { version = "0.29", optional = true } serde = { version = "1", features = ["derive"] } serde_json = "1" indexmap = { version = "2", features = ["serde"] } -kiss3d = { version = "0.43.0", features = ["egui", "serde"] } +kiss3d = { version = "0.44.0", features = ["egui", "serde"] } log = "0.4" env_logger = "0.11" diff --git a/crates/rapier_testbed3d/Cargo.toml b/crates/rapier_testbed3d/Cargo.toml index b0d298aa0..90872ee1d 100644 --- a/crates/rapier_testbed3d/Cargo.toml +++ b/crates/rapier_testbed3d/Cargo.toml @@ -62,7 +62,7 @@ egui = "0.34" profiling = "1.0" puffin_egui = { version = "0.29", optional = true } indexmap = { version = "2", features = ["serde"] } -kiss3d = { version = "0.43.0", features = ["egui", "serde"] } +kiss3d = { version = "0.44.0", features = ["egui", "serde"] } log = "0.4" env_logger = "0.11" diff --git a/examples2d/Cargo.toml b/examples2d/Cargo.toml index 882c20418..8923d447e 100644 --- a/examples2d/Cargo.toml +++ b/examples2d/Cargo.toml @@ -17,7 +17,7 @@ enhanced-determinism = ["rapier2d/enhanced-determinism"] rand = "0.10" lyon = "0.17" dot_vox = "5" -kiss3d = { version = "0.43.0", features = ["egui", "serde"] } +kiss3d = { version = "0.44.0", features = ["egui", "serde"] } [target.'cfg(not(target_arch = "wasm32"))'.dependencies] usvg = "0.14" diff --git a/examples3d-f64/Cargo.toml b/examples3d-f64/Cargo.toml index 5f525337d..a44e81c0b 100644 --- a/examples3d-f64/Cargo.toml +++ b/examples3d-f64/Cargo.toml @@ -19,7 +19,7 @@ wasm-bindgen = "0.2" obj-rs = { version = "0.7", default-features = false } bincode = "1" serde = "1" -kiss3d = { version = "0.43.0", features = ["egui", "serde"] } +kiss3d = { version = "0.44.0", features = ["egui", "serde"] } [dependencies.rapier_testbed3d-f64] path = "../crates/rapier_testbed3d-f64" diff --git a/examples3d/Cargo.toml b/examples3d/Cargo.toml index b0fe5109f..6b8a1571f 100644 --- a/examples3d/Cargo.toml +++ b/examples3d/Cargo.toml @@ -23,7 +23,7 @@ bincode = "1" serde_json = "1" dot_vox = "5" glam = { version = "0.33", features = ["fast-math"] } -kiss3d = { version = "0.43.0", features = ["egui", "serde"] } +kiss3d = { version = "0.44.0", features = ["egui", "serde", "rt_switcher"] } [dependencies.rapier_testbed3d] path = "../crates/rapier_testbed3d" diff --git a/examples3d/mujoco_menagerie3.rs b/examples3d/mujoco_menagerie3.rs index 30c046ece..fb1518b79 100644 --- a/examples3d/mujoco_menagerie3.rs +++ b/examples3d/mujoco_menagerie3.rs @@ -5,7 +5,7 @@ use std::sync::atomic::{AtomicUsize, Ordering}; use kiss3d::color::Color; use rapier_testbed3d::{Testbed, settings::StringDisplayMode}; use rapier3d::prelude::*; -use rapier3d_mjcf::{MjcfLoaderOptions, MjcfMultibodyOptions, MjcfRobot}; +use rapier3d_mjcf::{MjcfLoaderOptions, MjcfMultibodyOptions, MjcfRobot, MjcfRobotHandles}; /// Roots the scene-picker walks. Each entry is expected to contain `/scene*.xml` one level /// deep. We recommend simply cloning `https://github.com/google-deepmind/mujoco_menagerie` into @@ -48,6 +48,14 @@ pub fn init_world(testbed: &mut Testbed) { let disable_collisions = testbed .example_settings_mut() .get_or_set_bool("Disable collisions", true); + // Drives every actuator each frame with a zero control input — MuJoCo's + // "actuation" flag. For affine ``/`` actuators (a + // position servo) this holds each joint at its neutral pose, e.g. keeping + // the flybody legs spread instead of letting them retract. Only effective + // on the multibody path (controls are applied to multibody joints). + let enable_controls = testbed + .example_settings_mut() + .get_or_set_bool("Enable joint controls", false); let selected = testbed.example_settings_mut().get_or_set_string_with( "Scene", default_idx, @@ -61,14 +69,33 @@ pub fn init_world(testbed: &mut Testbed) { let mut world = PhysicsWorld::new(); let mut loaded = None; + let mut mb_handles = None; if let Some((path, _)) = scenes.get(selected) { match load_into_world(path, &mut world, use_multibody, disable_collisions) { - Ok(handles) => loaded = Some(handles), + Ok((robot, body_handles, mb)) => { + loaded = Some((robot, body_handles)); + mb_handles = mb; + } Err(e) => eprintln!("Failed to load `{}`: {e}", path.display()), } add_floor(&mut world); } testbed.set_physics_world(world); + + // "Enable joint controls" → drive the model's actuators every frame with a + // zero control vector (hold the neutral pose). Re-registered on each + // `init_world` (the testbed clears callbacks when a setting toggles), so + // ticking the checkbox turns actuation on/off. Multibody path only. + if enable_controls && let Some(handles) = mb_handles { + let ctrl = vec![0.0 as Real; handles.actuators.len()]; + testbed.add_callback(move |_, physics, _, _| { + handles.apply_controls_multibody( + &mut physics.bodies, + &mut physics.multibody_joints, + &ctrl, + ); + }); + } // MJCF is Z-up by default — keep the camera convention consistent // with the model so orbit controls feel right. testbed.set_up_axis(Vec3::Z); @@ -142,13 +169,19 @@ fn add_floor(world: &mut PhysicsWorld) { /// the impulse-joint insertion path, and returns the source robot /// together with the rapier body handle of each `MjcfBody` (`None` for /// bodies that weren't inserted — typically the implicit world body when -/// no joint references it). +/// no joint references it). The third element is the full multibody handle +/// set (`Some` only on the multibody path), kept so the caller can drive the +/// model's actuators via `apply_controls_multibody`. +type MultibodyHandles = MjcfRobotHandles>; fn load_into_world( path: &Path, world: &mut PhysicsWorld, use_multibody: bool, disable_collisions: bool, -) -> Result<(MjcfRobot, Vec>), Box> { +) -> Result< + (MjcfRobot, Vec>, Option), + Box, +> { let (mut robot, model) = MjcfRobot::from_file(path, loader_options())?; if disable_collisions { @@ -179,9 +212,9 @@ fn load_into_world( MjcfMultibodyOptions::DISABLE_SELF_CONTACTS } else { MjcfMultibodyOptions::default() - } | MjcfMultibodyOptions::SKIP_LOOP_CLOSURES; + }; // | MjcfMultibodyOptions::SKIP_LOOP_CLOSURES; - let body_handles = if use_multibody { + let (body_handles, mb_handles) = if use_multibody { let handles = robot.clone().insert_using_multibody_joints( &mut world.bodies, &mut world.colliders, @@ -189,25 +222,27 @@ fn load_into_world( &mut world.impulse_joints, mb_options, ); - handles + let body_handles = handles .bodies - .into_iter() - .map(|b| b.map(|h| h.body)) - .collect() + .iter() + .map(|b| b.as_ref().map(|h| h.body)) + .collect(); + (body_handles, Some(handles)) } else { let handles = robot.clone().insert_using_impulse_joints( &mut world.bodies, &mut world.colliders, &mut world.impulse_joints, ); - handles + let body_handles = handles .bodies .into_iter() .map(|b| b.map(|h| h.body)) - .collect() + .collect(); + (body_handles, None) }; - Ok((robot, body_handles)) + Ok((robot, body_handles, mb_handles)) } /// For each MJCF body that has visual meshes, register them against the From c82831be7d88a7def9c348a8e281d8cf885f9d4a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?S=C3=A9bastien=20Crozet?= Date: Thu, 11 Jun 2026 15:24:49 +0200 Subject: [PATCH 05/16] checkpoint: implement mujoco-style per-dof spring/damper --- crates/rapier3d-mjcf/src/loader/conversion.rs | 57 ++++++-- crates/rapier3d-mjcf/src/loader/insert.rs | 67 ++++++++- crates/rapier3d-mjcf/src/loader/joint.rs | 52 ++++--- crates/rapier3d-mjcf/src/loader/mass.rs | 134 +++++++++++++++++- crates/rapier3d-mjcf/src/loader/options.rs | 19 ++- crates/rapier3d-mjcf/src/loader/types.rs | 19 +++ crates/rapier3d-mjcf/tests/cassie_smoke.rs | 5 + crates/rapier3d-mjcf/tests/phase4.rs | 68 +++++++++ .../rapier3d/tests/multibody_dof_inertia.rs | 125 ++++++++++++++++ crates/rapier3d/tests/multibody_spring.rs | 100 +++++++++++++ examples3d/mujoco_menagerie3.rs | 27 +++- .../joint/multibody_joint/multibody.rs | 83 +++++++++++ .../joint/multibody_joint/multibody_joint.rs | 23 +++ 13 files changed, 732 insertions(+), 47 deletions(-) create mode 100644 crates/rapier3d/tests/multibody_dof_inertia.rs create mode 100644 crates/rapier3d/tests/multibody_spring.rs diff --git a/crates/rapier3d-mjcf/src/loader/conversion.rs b/crates/rapier3d-mjcf/src/loader/conversion.rs index 138dc2722..59325739e 100644 --- a/crates/rapier3d-mjcf/src/loader/conversion.rs +++ b/crates/rapier3d-mjcf/src/loader/conversion.rs @@ -239,6 +239,9 @@ impl<'a> Conversion<'a> { joint, damping_per_dof: 0.0, armature_per_dof: 0.0, + spring_stiffness_per_dof: 0.0, + spring_ref: 0.0, + springdamper: None, }); } return; @@ -266,19 +269,34 @@ impl<'a> Conversion<'a> { // joint's pos/axis encodes the geometric DoF. let cur_world_pose = body_world_pose; let joint = self.build_serial_joint(j, prev_world_pose, cur_world_pose); - // Per-DoF damping: applied uniformly across the joint's free - // DoFs. The `disable_joint_motors` option intentionally does - // NOT affect this — per-DoF damping is not a motor, it's a - // friction-like term on the dynamics equations. - let damping_per_dof = if let Some(sd) = j.springdamper { - let timeconst = sd[0]; - let dampratio = sd[1]; - if timeconst > 0.0 { - let omega = std::f64::consts::TAU / timeconst; - (2.0 * dampratio * omega) as Real - } else { - 0.0 + + // MuJoCo's `` requests a + // spring with that time constant and damping ratio; the actual + // stiffness/damping depend on the assembled joint-space inertia, so + // they're resolved post-assembly (see `add_springdamper_to_multibody`). + // MuJoCo requires both values strictly positive (it errors + // otherwise); we warn-and-ignore an out-of-spec pair. A valid + // springdamper overrides the explicit stiffness/damping. + let springdamper = match j.springdamper { + Some(sd) if sd[0] > 0.0 && sd[1] > 0.0 => Some((sd[0] as Real, sd[1] as Real)), + Some(_) => { + log::warn!( + ": springdamper needs two positive values \ + (timeconst dampratio); ignoring it (MuJoCo rejects this)", + j.name + ); + None } + None => None, + }; + + // Per-DoF damping: applied uniformly across the joint's free DoFs. + // The `disable_joint_motors` option intentionally does NOT affect + // this — per-DoF damping is not a motor, it's a friction-like term + // on the dynamics equations. A valid `springdamper` supplies its own + // damping post-assembly, so it overrides `` here. + let damping_per_dof = if springdamper.is_some() { + 0.0 } else { j.damping as Real }; @@ -286,6 +304,18 @@ impl<'a> Conversion<'a> { // generalized mass matrix at insertion time rather than baked into // the link's spatial inertia — see `MjcfJoint::armature_per_dof`. let armature_per_dof = j.armature.max(0.0) as Real; + // Passive spring carried so the multibody path can integrate it + // implicitly (a valid `springdamper` overrides ``, + // leaving this 0 and supplying the stiffness post-assembly). A + // spring is *not* a motor, so it is deliberately independent of + // `disable_joint_motors` / `SKIP_JOINT_MOTORS`; use + // `SKIP_JOINT_SPRINGS` to drop it. + let spring_ref = (j.springref - j.ref_) as Real; + let spring_stiffness_per_dof = if springdamper.is_none() && j.stiffness > 0.0 { + j.stiffness as Real + } else { + 0.0 + }; self.robot.joints.push(MjcfJoint { name: j.name.clone(), link1: prev_rapier, @@ -293,6 +323,9 @@ impl<'a> Conversion<'a> { joint, damping_per_dof, armature_per_dof, + spring_stiffness_per_dof, + spring_ref, + springdamper, }); prev_rapier = cur_rapier; prev_world_pose = cur_world_pose; diff --git a/crates/rapier3d-mjcf/src/loader/insert.rs b/crates/rapier3d-mjcf/src/loader/insert.rs index b00a268fc..b9b58713f 100644 --- a/crates/rapier3d-mjcf/src/loader/insert.rs +++ b/crates/rapier3d-mjcf/src/loader/insert.rs @@ -9,12 +9,15 @@ use rapier3d::dynamics::{ RigidBodySet, }; use rapier3d::geometry::ColliderSet; -use rapier3d::math::Pose; +use rapier3d::math::{Pose, Real}; use super::handles::{ MjcfActuatorHandle, MjcfBodyHandle, MjcfColliderHandle, MjcfJointHandle, MjcfRobotHandles, }; -use super::mass::{add_armature_to_multibody, move_motor_damping_to_multibody}; +use super::mass::{ + add_armature_to_multibody, add_spring_to_multibody, add_springdamper_to_multibody, + move_motor_damping_to_multibody, +}; use super::options::MjcfMultibodyOptions; use super::types::MjcfRobot; @@ -116,6 +119,7 @@ impl MjcfRobot { let skip_loop_closures = options.contains(MjcfMultibodyOptions::SKIP_LOOP_CLOSURES); let skip_motors = options.contains(MjcfMultibodyOptions::SKIP_JOINT_MOTORS); let skip_limits = options.contains(MjcfMultibodyOptions::SKIP_JOINT_LIMITS); + let skip_springs = options.contains(MjcfMultibodyOptions::SKIP_JOINT_SPRINGS); let world_referenced = self.joints.iter().any(|j| j.link1 == 0 || j.link2 == 0) || (!skip_loop_closures && self @@ -140,6 +144,10 @@ impl MjcfRobot { }); } let mut joint_handles = Vec::with_capacity(self.joints.len()); + // `` springs need the assembled joint-space inertia, + // so they're resolved after the whole multibody is built. Collect + // (handle, timeconst, dampratio, rest) here and apply them below. + let mut springdamper_joints: Vec<(MultibodyJointHandle, Real, Real, Real)> = Vec::new(); for j in self.joints { let l1 = body_handles[j.link1].as_ref().map(|b| b.body); let l2 = body_handles[j.link2].as_ref().map(|b| b.body); @@ -153,6 +161,9 @@ impl MjcfRobot { }; let damping_per_dof = j.damping_per_dof; let armature_per_dof = j.armature_per_dof; + let spring_stiffness_per_dof = j.spring_stiffness_per_dof; + let spring_ref = j.spring_ref; + let springdamper = j.springdamper; let joint_name = j.name.clone(); // `limit_axes` / `motor_axes` are bitmasks over the joint's // DoFs; clearing them is equivalent to "joint built without @@ -200,6 +211,33 @@ impl MjcfRobot { if armature_per_dof > 0.0 { add_armature_to_multibody(multibody_joints, h, armature_per_dof); } + // Integrate MJCF `` springs implicitly on the + // multibody (stable for stiff springs on low-inertia links), + // replacing the explicit position motor that the serial-joint + // builder set up. `add_spring_to_multibody` always clears that + // explicit motor (so the unstable explicit spring never applies + // on the multibody path); passing stiffness 0 when springs are + // disabled leaves the joint with no spring force at all. A + // spring is not a motor, so only `SKIP_JOINT_SPRINGS` disables + // it — `SKIP_JOINT_MOTORS` leaves passive springs in place. + if spring_stiffness_per_dof > 0.0 { + let stiffness = if skip_springs { + 0.0 + } else { + spring_stiffness_per_dof + }; + add_spring_to_multibody(multibody_joints, h, stiffness, spring_ref); + } + // ``: stiffness/damping depend on the + // assembled inertia, so defer to the post-assembly pass below. + // When springs are disabled, still clear the explicit motor now. + if let Some((timeconst, dampratio)) = springdamper { + if skip_springs { + add_spring_to_multibody(multibody_joints, h, 0.0, spring_ref); + } else { + springdamper_joints.push((h, timeconst, dampratio, spring_ref)); + } + } } joint_handles.push(MjcfJointHandle { joint: h, @@ -207,6 +245,31 @@ impl MjcfRobot { link2: l2, }); } + + // Post-assembly pass for ``: now that the whole + // multibody is built, resolve each springdamper against the assembled + // joint-space inertia (MuJoCo's `dof_invweight0`). The bodies' mass + // properties must be finalized first — `additional_mass_properties` + // only reaches `local_mprops` after a mass recompute, which otherwise + // wouldn't happen until the first step. + if !springdamper_joints.is_empty() { + for bh in body_handles.iter().flatten() { + if let Some(body) = bodies.get_mut(bh.body) { + body.recompute_mass_properties_from_colliders(colliders); + } + } + for (h, timeconst, dampratio, rest) in springdamper_joints { + add_springdamper_to_multibody( + multibody_joints, + bodies, + h, + timeconst, + dampratio, + rest, + ); + } + } + let mut eq_handles = Vec::with_capacity(self.equality_joints.len()); if !skip_loop_closures { for eq in self.equality_joints { diff --git a/crates/rapier3d-mjcf/src/loader/joint.rs b/crates/rapier3d-mjcf/src/loader/joint.rs index eea496579..886429447 100644 --- a/crates/rapier3d-mjcf/src/loader/joint.rs +++ b/crates/rapier3d-mjcf/src/loader/joint.rs @@ -99,25 +99,29 @@ impl<'a> Conversion<'a> { if !self.options.disable_joint_motors { // Spring / damping / armature / etc. - let stiffness = if let Some(sd) = joint.springdamper { - let timeconst = sd[0]; - let dampratio = sd[1]; - // MuJoCo uses an under-damped spring with: stiffness = (2π/T)² * m_eff - // and damping = 2 ξ √(stiffness * m_eff). We don't know m_eff - // here — fall back to interpreting the pair as (stiffness, damping) - // when timeconst is zero (the explicit form). - if timeconst > 0.0 { - // Treat (2π/T)² as the stiffness coefficient assuming m_eff=1. - let omega = std::f64::consts::TAU / timeconst; - Some((omega * omega, 2.0 * dampratio * omega)) - } else { - Some((dampratio, 0.0)) + // + // This is the *impulse-joint* path's spring motor. It uses the + // AccelerationBased motor model (below), which already decouples + // the effective stiffness from the link inertia — so `springdamper` + // maps to `(ω², 2·ζ·ω)` with no inertia term (the multibody path's + // implicit spring, built in `conversion.rs`, does fold the inertia + // in because it applies a physical force). MuJoCo requires both + // springdamper values positive; an out-of-spec pair is ignored here + // and falls through to the explicit `stiffness`/`damping`. + let springdamper = match joint.springdamper { + Some(sd) if sd[0] > 0.0 && sd[1] > 0.0 => { + let omega = std::f64::consts::TAU / sd[0]; + Some((omega * omega, 2.0 * sd[1] * omega)) } - } else if joint.stiffness > 0.0 || joint.damping > 0.0 { - Some((joint.stiffness, joint.damping)) - } else { - None + _ => None, }; + let stiffness = springdamper.or({ + if joint.stiffness > 0.0 || joint.damping > 0.0 { + Some((joint.stiffness, joint.damping)) + } else { + None + } + }); let target_pos = joint.springref - joint.ref_; if let Some((k, d)) = stiffness { let axis = match joint.type_ { @@ -126,12 +130,14 @@ impl<'a> Conversion<'a> { _ => None, }; if let Some(ax) = axis { - // AccelerationBased decouples the motor's effective - // stiffness from the link's inertia, which makes the - // timestep-stability margin uniform across the chain. For - // stiff MJCF springs (e.g. cassie's shin/heel springs at - // k=1250–1500) ForceBased was numerically unstable on the - // multibody path and produced NaN within a few steps. + // Build the spring as a position motor. This is what the + // *impulse-joint* path uses. The multibody path replaces it + // with an implicit spring at insertion time (see + // `add_spring_to_multibody`), which is stable for stiff + // springs on low-inertia links where this motor injects + // energy. AccelerationBased decouples the motor's effective + // stiffness from the link's inertia, giving a uniform + // timestep-stability margin along the chain. builder = builder.motor_model(ax, MotorModel::AccelerationBased); builder = builder.motor_position(ax, target_pos as Real, k as Real, d as Real); } diff --git a/crates/rapier3d-mjcf/src/loader/mass.rs b/crates/rapier3d-mjcf/src/loader/mass.rs index 84e3ba333..d1afd6cc3 100644 --- a/crates/rapier3d-mjcf/src/loader/mass.rs +++ b/crates/rapier3d-mjcf/src/loader/mass.rs @@ -10,7 +10,9 @@ use mjcf_rs::body as mb; use mjcf_rs::compiler::InertiaFromGeom; use mjcf_rs::model::BodyEntry; -use rapier3d::dynamics::{MassProperties, MultibodyJointHandle, MultibodyJointSet}; +use rapier3d::dynamics::{ + MassProperties, MultibodyJointHandle, MultibodyJointSet, RigidBodySet, +}; use rapier3d::math::{Matrix, Pose, Real, Vector}; use super::conversion::Conversion; @@ -252,3 +254,133 @@ pub(super) fn add_armature_to_multibody( } } } + +/// After a joint has been inserted into a multibody, install a passive +/// `` spring as an *implicit* spring on the +/// multibody link (force `-k·(q − rest)`, integrated implicitly in the +/// generalized dynamics) and remove the explicit position-motor spring that +/// the serial-joint builder created for the impulse path. The spring is +/// applied to every free DoF of the joint, matching MuJoCo's per-joint +/// `stiffness`. +/// +/// Implicit integration keeps stiff springs stable on low-inertia links, +/// where the explicit motor injects energy (e.g. robotiq's `spring_link` +/// driving the near-massless follower, or flybody's leg springs). +pub(super) fn add_spring_to_multibody( + multibody_joints: &mut MultibodyJointSet, + handle: MultibodyJointHandle, + stiffness: Real, + rest: Real, +) { + use rapier3d::dynamics::JointAxesMask; + use rapier3d::math::SPATIAL_DIM; + let Some((multibody, link_id)) = multibody_joints.get_mut(handle) else { + return; + }; + let Some(link) = multibody.links_mut().nth(link_id) else { + return; + }; + let locked_bits = link.joint.data.locked_axes.bits(); + for axis in 0..SPATIAL_DIM { + if (locked_bits & (1 << axis)) == 0 { + link.joint.set_spring(axis, stiffness, rest); + // Drop the explicit motor spring on this axis so the implicit + // spring isn't double-applied. Actuators (applied later via + // `apply_controls`) re-enable the motor on the axes they drive. + if let Some(flag) = JointAxesMask::from_bits(1u8 << axis) { + link.joint.data.motor_axes.remove(flag); + } + } + } +} + +/// Install a MJCF `` spring, +/// resolved against the *assembled* multibody so it realizes the requested +/// time constant and damping ratio — MuJoCo's `AutoSpringDamper`. Must run +/// after the whole multibody is built and its bodies' mass properties are up +/// to date (`dof_inverse_inertia` reads `diag(M⁻¹)`). +/// +/// With effective inertia `I = ndof / Σ dof_invweight0` over the joint's DoFs: +/// `stiffness = I / (T²·ζ²)` and `damping = 2·I / T`. The stiffness becomes an +/// implicit spring (like [`add_spring_to_multibody`]) and the damping goes to +/// the per-DoF damping vector; the explicit motor on those axes is cleared. +pub(super) fn add_springdamper_to_multibody( + multibody_joints: &mut MultibodyJointSet, + bodies: &RigidBodySet, + handle: MultibodyJointHandle, + timeconst: Real, + dampratio: Real, + rest: Real, +) { + use rapier3d::dynamics::JointAxesMask; + use rapier3d::math::SPATIAL_DIM; + + let Some((multibody, link_id)) = multibody_joints.get_mut(handle) else { + return; + }; + + // Per-DoF inverse joint-space inertia (also finalizes the root type). + let invweight = multibody.dof_inverse_inertia(bodies); + + // This link's DoF offset and count in the generalized vectors. + let mut offset = 0; + let mut ndofs = 0; + for (i, link) in multibody.links().enumerate() { + let nd = link.joint().ndofs(); + if i == link_id { + ndofs = nd; + break; + } + offset += nd; + } + if ndofs == 0 { + return; + } + + // MuJoCo `AutoSpringDamper`: inertia = ndof / Σ invweight0, then + // stiffness = inertia / (T²·ζ²), damping = 2·inertia / T. + // + // NOTE: the effective inertia is evaluated once here, at the load-time + // configuration (MuJoCo does the same at `qpos0`), and `stiffness`/`damping` + // are then *constant* — that's what makes this a physical spring. The + // requested time constant / damping ratio hold exactly only at this pose; + // as the articulated inertia changes with configuration the effective ω/ζ + // drift, which is correct spring behavior. Re-deriving the coefficients from + // the current inertia every frame would instead hold ω/ζ constant by making + // the *stiffness* configuration-dependent — a normalized impedance, not a + // spring, and a divergence from MuJoCo. (The integration itself already uses + // the current per-frame mass matrix; only this calibration is fixed.) + let eps = 1.0e-9; + let sum_inv: Real = (0..ndofs) + .map(|d| invweight.get(offset + d).copied().unwrap_or(0.0)) + .sum(); + let inertia = if sum_inv > eps { + ndofs as Real / sum_inv + } else { + 0.0 + }; + let stiffness = inertia / (timeconst * timeconst * dampratio * dampratio).max(eps); + let damping = 2.0 * inertia / timeconst.max(eps); + + // Stiffness → implicit spring; clear the explicit motor on those axes. + if let Some(link) = multibody.links_mut().nth(link_id) { + let locked_bits = link.joint.data.locked_axes.bits(); + for axis in 0..SPATIAL_DIM { + if (locked_bits & (1 << axis)) == 0 { + link.joint.set_spring(axis, stiffness, rest); + if let Some(flag) = JointAxesMask::from_bits(1u8 << axis) { + link.joint.data.motor_axes.remove(flag); + } + } + } + } + + // Damping → per-DoF damping vector (the joint's DoFs are offset..+ndofs). + let damping_vec = multibody.damping_mut(); + for d in 0..ndofs { + let idx = offset + d; + if idx < damping_vec.len() { + damping_vec[idx] = damping; + } + } +} diff --git a/crates/rapier3d-mjcf/src/loader/options.rs b/crates/rapier3d-mjcf/src/loader/options.rs index 2c2bf6e14..cf2ede60c 100644 --- a/crates/rapier3d-mjcf/src/loader/options.rs +++ b/crates/rapier3d-mjcf/src/loader/options.rs @@ -28,12 +28,12 @@ bitflags::bitflags! { /// debugging a chain that otherwise drifts under loop closure. const SKIP_LOOP_CLOSURES = 0b0100; /// If set, the motor entries baked into each joint by the loader - /// (``, ``, ``) are stripped before the joint is handed to - /// the multibody. Only the multibody path is affected — the - /// impulse-joint path still sees motors. Useful when motor - /// stiffness destabilizes the articulated solver and you want - /// to compare against a pure kinematic chain. + /// (``, and the actuator motors set at runtime) + /// are stripped before the joint is handed to the multibody. Only the + /// multibody path is affected — the impulse-joint path still sees + /// motors. Passive `` springs are **not** motors and + /// are unaffected; use [`SKIP_JOINT_SPRINGS`](Self::SKIP_JOINT_SPRINGS) + /// for those. Useful when comparing against a pure kinematic chain. const SKIP_JOINT_MOTORS = 0b1000; /// If set, the joint limits baked into each joint by the loader /// (``) are stripped before the joint is handed to @@ -43,6 +43,13 @@ bitflags::bitflags! { /// rest pose (the limit then fights the joint immediately at /// t=0). const SKIP_JOINT_LIMITS = 0b1_0000; + /// If set, MJCF `` passive springs are **not** + /// installed as implicit springs on the multibody. Only the multibody + /// path is affected. The springs are integrated implicitly by default + /// (stable for stiff springs on low-inertia links); set this to compare + /// against a spring-free chain or to debug a model whose declared + /// `springref`/`stiffness` is unexpected. + const SKIP_JOINT_SPRINGS = 0b10_0000; } } diff --git a/crates/rapier3d-mjcf/src/loader/types.rs b/crates/rapier3d-mjcf/src/loader/types.rs index eaa6250b9..039eeb206 100644 --- a/crates/rapier3d-mjcf/src/loader/types.rs +++ b/crates/rapier3d-mjcf/src/loader/types.rs @@ -116,6 +116,25 @@ pub struct MjcfJoint { /// (huge along the joint axis, ~0 across it) and the multibody mass /// matrix ill-conditioned. pub armature_per_dof: Real, + /// MJCF `` (passive spring). On the multibody path this + /// can be integrated implicitly in the generalized dynamics (added to the + /// mass-matrix diagonal as `dt²·k` with a force `-k·(q − ref)`), which is + /// numerically stable for stiff springs on low-inertia links — unlike the + /// explicit position motor used otherwise. `0` if the joint has no spring. + pub spring_stiffness_per_dof: Real, + /// Rest position of the spring (`springref − ref`, in rapier's joint + /// coordinate convention). Used for both the explicit-stiffness spring and + /// the `springdamper` spring below. + pub spring_ref: Real, + /// MJCF `` (both positive). Unlike + /// `spring_stiffness_per_dof`/`damping_per_dof` — which are physical + /// coefficients known up front — a `springdamper` requests a spring with a + /// given *time constant and damping ratio*, so its stiffness and damping + /// must be computed from the assembled joint-space inertia (MuJoCo's + /// `dof_invweight0`). The multibody insertion path resolves it in a + /// post-assembly pass; a valid `springdamper` overrides the explicit + /// stiffness/damping (which are then left at 0 here). + pub springdamper: Option<(Real, Real)>, } /// One `` constraint materialized as an extra rapier joint. diff --git a/crates/rapier3d-mjcf/tests/cassie_smoke.rs b/crates/rapier3d-mjcf/tests/cassie_smoke.rs index 2431e171f..4f9a87fa0 100644 --- a/crates/rapier3d-mjcf/tests/cassie_smoke.rs +++ b/crates/rapier3d-mjcf/tests/cassie_smoke.rs @@ -97,6 +97,11 @@ fn cassie_multibody_loads_without_nan() { /// `J1ᵀ·W·J2` coupling for constraints between links of the same /// multibody). With those fixed, the dangling legs just swing and the /// closures hold; this regression keeps that configuration NaN-free. +/// +/// Note: `disable_joint_motors` disables actuator/friction-loss motors only — +/// the passive `` springs are not motors and stay active +/// (integrated implicitly), so this also exercises the loop closures together +/// with cassie's stiff shin/heel springs. #[test] #[ignore = "requires the cassie assets"] fn cassie_stable_without_motors_and_with_loop_closures() { diff --git a/crates/rapier3d-mjcf/tests/phase4.rs b/crates/rapier3d-mjcf/tests/phase4.rs index f093d77aa..d57e4a114 100644 --- a/crates/rapier3d-mjcf/tests/phase4.rs +++ b/crates/rapier3d-mjcf/tests/phase4.rs @@ -69,6 +69,74 @@ fn springref_is_baked_in_radians() { assert!((sr - std::f64::consts::FRAC_PI_4).abs() < 1e-12); } +#[test] +fn springdamper_stiffness_uses_assembled_joint_inertia() { + // `springdamper="T ζ"` is resolved post-assembly against the joint-space + // inertia: stiffness = I / (T²·ζ²). For a fixed-base single hinge about Z, + // I is just the link's inertia about Z. With T=0.5, ζ=1 → k = I / 0.25 = 4I. + let xml = |izz: f64| { + format!( + r#" + + + "# + ) + }; + let resolved_stiffness = |izz: f64| -> Real { + let opts = MjcfLoaderOptions { + make_roots_fixed: true, + ..Default::default() + }; + let (robot, _) = MjcfRobot::from_str(&xml(izz), opts, ".").unwrap(); + let mut bodies = RigidBodySet::new(); + let mut colliders = ColliderSet::new(); + let mut impulse_joints = ImpulseJointSet::new(); + let mut multibody_joints = MultibodyJointSet::new(); + let handles = robot.insert_using_multibody_joints( + &mut bodies, + &mut colliders, + &mut multibody_joints, + &mut impulse_joints, + rapier3d_mjcf::MjcfMultibodyOptions::empty(), + ); + let h = handles.joints[0].joint.expect("multibody joint"); + let (mb, link_id) = multibody_joints.get_mut(h).unwrap(); + // The revolute's free DoF is the angular X axis (index 3). + mb.links().nth(link_id).unwrap().joint().spring(3).0 + }; + + let k = resolved_stiffness(0.3); + let expected = (0.3 / 0.25) as Real; // I / (T²ζ²) + assert!( + (k - expected).abs() < expected * 0.02, + "springdamper stiffness = {k}, expected ≈ {expected} (= I / (T²·ζ²))" + ); + // Doubling the joint inertia doubles the resolved stiffness. + let ratio = resolved_stiffness(0.6) / k; + assert!( + (ratio - 2.0).abs() < 0.02, + "stiffness should scale with the assembled joint inertia; ratio = {ratio}" + ); +} + +#[test] +fn invalid_springdamper_is_ignored_and_falls_through() { + // A `springdamper` that isn't two positive values is an error in MuJoCo; + // the loader warns and ignores it, falling through to the explicit + // `stiffness` (it does NOT reinterpret the second value as a stiffness). + let xml = r#" + + + + + "#; + let (robot, _) = MjcfRobot::from_str(xml, MjcfLoaderOptions::default(), ".").unwrap(); + assert_eq!( + robot.joints[0].spring_stiffness_per_dof, 7.0, + "invalid springdamper should fall through to the explicit stiffness" + ); +} + #[test] fn armature_routes_through_per_dof_armature_not_spatial_inertia() { // MuJoCo's `armature` is a reflected rotor inertia that belongs on the diff --git a/crates/rapier3d/tests/multibody_dof_inertia.rs b/crates/rapier3d/tests/multibody_dof_inertia.rs new file mode 100644 index 000000000..e0ba2a324 --- /dev/null +++ b/crates/rapier3d/tests/multibody_dof_inertia.rs @@ -0,0 +1,125 @@ +//! `Multibody::dof_inverse_inertia` returns MuJoCo's `dof_invweight0` — +//! `diag(M⁻¹)` of the joint-space inertia (armature included). These tests pin +//! the two regimes that matter: a single leaf joint (where it's just the link's +//! own inertia about the axis), and a chain (where the articulated coupling +//! makes the apparent inertia at a parent DoF depend on its children). + +use rapier3d::prelude::*; + +#[test] +fn leaf_joint_inverse_inertia_is_link_inertia() { + // Fixed base, one dynamic link rotating in place about Z. The joint-space + // inertia is just the link's inertia about Z + armature, so the inverse + // inertia is 1 / (Izz + armature). + let mut bodies = RigidBodySet::new(); + let mut multibody_joints = MultibodyJointSet::new(); + let base = bodies.insert(RigidBodyBuilder::fixed()); + let izz = 0.3; + let link = bodies.insert( + RigidBodyBuilder::dynamic().additional_mass_properties(MassProperties::new( + Vector::ZERO, + 1.0, + Vector::new(0.1, 0.2, izz), + )), + ); + let h = multibody_joints + .insert(base, link, RevoluteJointBuilder::new(Vector::Z), true) + .unwrap(); + + // `additional_mass_properties` only lands in `local_mprops` after a mass + // recompute (otherwise the mass matrix is zero before the first step). + let colliders = ColliderSet::new(); + bodies + .get_mut(link) + .unwrap() + .recompute_mass_properties_from_colliders(&colliders); + + // Add a rotor armature on the joint DoF. + let armature = 0.05; + { + let (mb, _) = multibody_joints.get_mut(h).unwrap(); + // forward_kinematics happens inside dof_inverse_inertia; the free root + // collapses to 0 DoFs, leaving the single revolute DoF. + let inv = mb.dof_inverse_inertia(&bodies); + // Before setting armature: 1 / Izz. + assert_eq!(inv.len(), 1); + assert!((inv[0] - 1.0 / izz).abs() < 1e-4, "inv = {}, want {}", inv[0], 1.0 / izz); + // Now set armature and recheck. + let n = mb.armature().len(); + mb.armature_mut()[n - 1] = armature; + } + let (mb, _) = multibody_joints.get_mut(h).unwrap(); + let inv = mb.dof_inverse_inertia(&bodies); + let expected = 1.0 / (izz + armature); + assert!( + (inv[0] - expected).abs() < 1e-4, + "with armature: inv = {}, want {}", + inv[0], + expected + ); +} + +#[test] +fn parent_joint_inverse_inertia_accounts_for_children() { + // Base→link1(hinge Z)→link2(hinge Z), all rotating about the same world Z + // through the origin so the joint-space inertia is diagonal and easy to + // reason about: M[0,0] = Izz1 + Izz2 (link1's DoF carries both links), + // M[1,1] = Izz2. The off-diagonal is Izz2 (shared axis), so M is NOT + // diagonal and diag(M⁻¹) differs from 1/diag(M) — this is the articulated + // coupling we want. + let mut bodies = RigidBodySet::new(); + let mut multibody_joints = MultibodyJointSet::new(); + let base = bodies.insert(RigidBodyBuilder::fixed()); + let (i1, i2) = (0.4, 0.25); + let link1 = bodies.insert( + RigidBodyBuilder::dynamic().additional_mass_properties(MassProperties::new( + Vector::ZERO, + 1.0, + Vector::new(0.1, 0.1, i1), + )), + ); + let link2 = bodies.insert( + RigidBodyBuilder::dynamic().additional_mass_properties(MassProperties::new( + Vector::ZERO, + 1.0, + Vector::new(0.1, 0.1, i2), + )), + ); + multibody_joints + .insert(base, link1, RevoluteJointBuilder::new(Vector::Z), true) + .unwrap(); + let h = multibody_joints + .insert(link1, link2, RevoluteJointBuilder::new(Vector::Z), true) + .unwrap(); + + let colliders = ColliderSet::new(); + for b in [link1, link2] { + bodies + .get_mut(b) + .unwrap() + .recompute_mass_properties_from_colliders(&colliders); + } + + let (mb, _) = multibody_joints.get_mut(h).unwrap(); + let inv = mb.dof_inverse_inertia(&bodies); + assert_eq!(inv.len(), 2); + + // Both links rotate about the same axis through the origin, so: + // M = [[i1+i2, i2], [i2, i2]] + // and its inverse has diagonal [1/i1, (i1+i2)/(i1*i2)]. + let inv0_expected = 1.0 / i1; + let inv1_expected = (i1 + i2) / (i1 * i2); + assert!( + (inv[0] - inv0_expected).abs() < 1e-3, + "dof0 inv inertia = {}, want {}", + inv[0], + inv0_expected + ); + assert!( + (inv[1] - inv1_expected).abs() < 1e-3, + "dof1 inv inertia = {}, want {} (must reflect coupling, not 1/i2={})", + inv[1], + inv1_expected, + 1.0 / i2 + ); +} diff --git a/crates/rapier3d/tests/multibody_spring.rs b/crates/rapier3d/tests/multibody_spring.rs new file mode 100644 index 000000000..d5a73f5f6 --- /dev/null +++ b/crates/rapier3d/tests/multibody_spring.rs @@ -0,0 +1,100 @@ +//! Implicit joint springs on a multibody. A spring set on a multibody joint +//! applies a generalized force `-stiffness·(q − rest)` integrated implicitly +//! (its `dt²·k` term lands on the mass-matrix diagonal). These tests pin the +//! two defining properties: it drives the joint to its rest angle, and it does +//! so stably even on a low-inertia link where an explicit motor would diverge. + +use rapier3d::prelude::*; + +/// Fixed base + one dynamic link on a revolute multibody joint. Sets a spring +/// (rest angle `rest`, the given `stiffness`) on the joint plus a little joint +/// damping so it settles, then returns the link's final rotation about Z after +/// `steps` steps with gravity off. +fn settle_angle(stiffness: Real, rest: Real, damping: Real, inertia: Real, steps: usize) -> Real { + let mut bodies = RigidBodySet::new(); + let mut colliders = ColliderSet::new(); + let mut impulse_joints = ImpulseJointSet::new(); + let mut multibody_joints = MultibodyJointSet::new(); + + let base = bodies.insert(RigidBodyBuilder::fixed()); + // Link rotates in place about the shared origin (COM on the hinge axis), so + // the effective joint inertia is exactly `inertia` — no lever-arm term. + let link = bodies.insert( + RigidBodyBuilder::dynamic().additional_mass_properties(MassProperties::new( + Vector::ZERO, + 1.0, + Vector::new(inertia, inertia, inertia), + )), + ); + let rev = RevoluteJointBuilder::new(Vector::Z); + let handle = multibody_joints.insert(base, link, rev, true).unwrap(); + + let gravity = Vector::ZERO; + let integration_parameters = IntegrationParameters::default(); + let mut pipeline = PhysicsPipeline::new(); + let mut islands = IslandManager::new(); + let mut broad_phase = DefaultBroadPhase::new(); + let mut narrow_phase = NarrowPhase::new(); + let mut ccd = CCDSolver::new(); + + let mut step = |bodies: &mut RigidBodySet, mbj: &mut MultibodyJointSet| { + pipeline.step( + gravity, + &integration_parameters, + &mut islands, + &mut broad_phase, + &mut narrow_phase, + bodies, + &mut colliders, + &mut impulse_joints, + mbj, + &mut ccd, + &(), + &(), + ); + }; + + // Warm up one step so the fixed-base root collapses to 0 DoFs (it starts as + // a 6-DoF free root), leaving the single revolute DoF. + step(&mut bodies, &mut multibody_joints); + + { + let (mb, link_id) = multibody_joints.get_mut(handle).unwrap(); + let link_mut = mb.links_mut().nth(link_id).unwrap(); + // The revolute's free axis is the angular X axis (index DIM = 3). + link_mut.joint.set_spring(3, stiffness, rest); + // A little joint-space damping so the (otherwise conservative) spring + // settles in a finite number of steps. + let n = mb.damping().len(); + mb.damping_mut()[n - 1] = damping; + } + + for _ in 0..steps { + step(&mut bodies, &mut multibody_joints); + } + bodies[link].rotation().to_scaled_axis().z +} + +#[test] +fn implicit_spring_drives_joint_to_rest_angle() { + // Spring rest = +0.6 rad; the joint starts at 0. With ~critical damping + // (d ≈ 2·√(k·I) = 2·√(5·0.05) = 1.0) it settles at the rest angle. + let angle = settle_angle(5.0, 0.6, 1.0, 0.05, 400); + assert!( + (angle - 0.6).abs() < 0.05, + "spring did not settle at its rest angle: angle = {angle}" + ); +} + +#[test] +fn implicit_spring_is_stable_on_tiny_inertia() { + // The whole point of implicit integration: a stiff spring on a near-massless + // link (like robotiq's follower / flybody's leg segments) stays bounded + // where an explicit position motor diverges to thousands of rad/s. + let angle = settle_angle(50.0, 0.3, 0.01, 1.0e-6, 600); + assert!(angle.is_finite(), "spring blew up on tiny inertia: {angle}"); + assert!( + (angle - 0.3).abs() < 0.1, + "spring on tiny inertia didn't reach rest: angle = {angle}" + ); +} diff --git a/examples3d/mujoco_menagerie3.rs b/examples3d/mujoco_menagerie3.rs index fb1518b79..1f2191dfb 100644 --- a/examples3d/mujoco_menagerie3.rs +++ b/examples3d/mujoco_menagerie3.rs @@ -56,6 +56,12 @@ pub fn init_world(testbed: &mut Testbed) { let enable_controls = testbed .example_settings_mut() .get_or_set_bool("Enable joint controls", false); + // MJCF `` passive springs (integrated implicitly on the + // multibody path). Unchecking removes them — useful to see a model without + // its return springs (e.g. robotiq's gripper preload, cassie's leg springs). + let enable_springs = testbed + .example_settings_mut() + .get_or_set_bool("Enable joint springs", true); let selected = testbed.example_settings_mut().get_or_set_string_with( "Scene", default_idx, @@ -71,7 +77,13 @@ pub fn init_world(testbed: &mut Testbed) { let mut loaded = None; let mut mb_handles = None; if let Some((path, _)) = scenes.get(selected) { - match load_into_world(path, &mut world, use_multibody, disable_collisions) { + match load_into_world( + path, + &mut world, + use_multibody, + disable_collisions, + enable_springs, + ) { Ok((robot, body_handles, mb)) => { loaded = Some((robot, body_handles)); mb_handles = mb; @@ -103,6 +115,8 @@ pub fn init_world(testbed: &mut Testbed) { if !use_multibody { testbed.integration_parameters_mut().dt = 1.0 / 240.0; testbed.integration_parameters_mut().num_solver_iterations = 12; + } else { + testbed.integration_parameters_mut().num_internal_pgs_iterations = 4; } // Refit the camera only on actual scene changes, otherwise a @@ -178,6 +192,7 @@ fn load_into_world( world: &mut PhysicsWorld, use_multibody: bool, disable_collisions: bool, + enable_springs: bool, ) -> Result< (MjcfRobot, Vec>, Option), Box, @@ -208,11 +223,17 @@ fn load_into_world( let gravity_mag = (gx * gx + gy * gy + gz * gz).sqrt(); world.gravity = Vector::new(0.0, 0.0, -gravity_mag); - let mb_options = if disable_collisions { + let mut mb_options = if disable_collisions { MjcfMultibodyOptions::DISABLE_SELF_CONTACTS } else { MjcfMultibodyOptions::default() - }; // | MjcfMultibodyOptions::SKIP_LOOP_CLOSURES; + }; + // `` springs are integrated implicitly by default; + // unchecking the toggle strips them so you can see the model without its + // passive return springs (and contrast against the implicit-spring fix). + if !enable_springs { + mb_options |= MjcfMultibodyOptions::SKIP_JOINT_SPRINGS; + } let (body_handles, mb_handles) = if use_multibody { let handles = robot.clone().insert_using_multibody_joints( diff --git a/src/dynamics/joint/multibody_joint/multibody.rs b/src/dynamics/joint/multibody_joint/multibody.rs index a8e3ad920..9743f98f1 100644 --- a/src/dynamics/joint/multibody_joint/multibody.rs +++ b/src/dynamics/joint/multibody_joint/multibody.rs @@ -511,6 +511,25 @@ impl Multibody { self.accelerations .cmpy(-1.0, &self.damping, &self.velocities, 1.0); + // Implicit joint springs: generalized force `-k·(q − rest)` evaluated at + // the current position (the implicit `dt²·k` correction lives on the + // mass-matrix diagonal, see `update_mass_matrix`). + for li in 0..self.links.len() { + let mut idx = self.links[li].assembly_id; + let locked = self.links[li].joint.data.locked_axes.bits(); + for a in 0..SPATIAL_DIM { + if (locked >> a) & 1 == 0 { + let k = self.links[li].joint.spring_stiffness[a]; + if k != 0.0 { + let q = self.links[li].joint.coords[a]; + let rest = self.links[li].joint.spring_ref[a]; + self.accelerations[idx] += -k * (q - rest); + } + idx += 1; + } + } + } + self.augmented_mass_indices .with_rearranged_rows_mut(&mut self.accelerations, |accs| { self.acc_inv_augmented_mass.solve_mut(accs); @@ -822,6 +841,31 @@ impl Multibody { self.augmented_mass[(i, i)] += diag; } + // Implicit joint springs. A passive spring contributes a generalized + // force `-k·(q − rest)`; integrating it implicitly (evaluating it at the + // end-of-step position `q + dt·v⁺`) adds `dt²·k` to the mass-matrix + // diagonal here, with the `-k·(q − rest)` term added in + // `update_acceleration`. This is what keeps a stiff spring on a + // low-inertia link stable where an explicit position motor injects + // energy. The spring lives on the link's `MultibodyJoint` so it travels + // with the link through topology changes. + let dt2 = dt * dt; + for li in 0..self.links.len() { + let mut idx = self.links[li].assembly_id; + let locked = self.links[li].joint.data.locked_axes.bits(); + for a in 0..SPATIAL_DIM { + if (locked >> a) & 1 == 0 { + let k = self.links[li].joint.spring_stiffness[a]; + if k != 0.0 { + let d = k * dt2; + self.acc_augmented_mass[(idx, idx)] += d; + self.augmented_mass[(idx, idx)] += d; + } + idx += 1; + } + } + } + let effective_dim = self .augmented_mass_indices .dim_after_removal(self.acc_augmented_mass.nrows()); @@ -851,6 +895,45 @@ impl Multibody { ); } + /// Per-DoF inverse joint-space inertia `diag(M⁻¹)` at the current + /// configuration, where `M` is the generalized mass matrix *including + /// armature* but excluding joint damping and springs. This is MuJoCo's + /// `dof_invweight0`: the apparent inverse inertia seen at each DoF when all + /// other DoFs are free, accounting for the full articulated coupling. + /// + /// It (re)runs forward kinematics and reassembles the mass matrix, so it is + /// intended for occasional use (e.g. sizing `` springs + /// at load time), not for every simulation step. + pub fn dof_inverse_inertia(&mut self, bodies: &RigidBodySet) -> DVector { + // Resolve the root joint type (a fixed base may still be a 6-DoF free + // root pre-collapse) so `ndofs` is final, then assemble `M`. Using + // `dt = 0` drops the `dt·damping` and `dt²·stiffness` diagonal terms, + // leaving exactly `M + armature`. + self.forward_kinematics(bodies, false); + self.update_mass_matrix(0.0, bodies); + + let n = self.ndofs; + let mut out = DVector::zeros(n); + if n == 0 { + return out; + } + // `(M⁻¹)[i, i]` for each DoF: solve `M x = e_i` and read `x[i]`. The + // factorization in `inv_augmented_mass` lives in the kinematic-reduced + // ordering, so route the unit vector through the same rearrangement the + // solver uses. + let mut e = DVector::zeros(n); + for i in 0..n { + e.fill(0.0); + e[i] = 1.0; + self.augmented_mass_indices + .with_rearranged_rows_mut(&mut e, |b| { + self.inv_augmented_mass.solve_mut(b); + }); + out[i] = e[i]; + } + out + } + /// The generalized velocity at the multibody_joint of the given link. #[inline] pub fn joint_velocity(&self, link: &MultibodyLink) -> DVectorView<'_, Real> { diff --git a/src/dynamics/joint/multibody_joint/multibody_joint.rs b/src/dynamics/joint/multibody_joint/multibody_joint.rs index 0f768f3d0..052ca2559 100644 --- a/src/dynamics/joint/multibody_joint/multibody_joint.rs +++ b/src/dynamics/joint/multibody_joint/multibody_joint.rs @@ -29,6 +29,13 @@ pub struct MultibodyJoint { pub kinematic: bool, pub(crate) coords: SpatialVector, pub(crate) joint_rot: Rotation, + /// Per-DoF spring stiffness (a passive joint spring integrated implicitly + /// in the generalized dynamics). Zero on axes with no spring. + pub(crate) spring_stiffness: SpatialVector, + /// Per-DoF spring rest position, in the joint's generalized coordinate + /// (same convention as [`Self::coords`]). Only meaningful where + /// `spring_stiffness` is non-zero. + pub(crate) spring_ref: SpatialVector, } impl MultibodyJoint { @@ -39,9 +46,25 @@ impl MultibodyJoint { kinematic, coords: Default::default(), joint_rot: Rotation::IDENTITY, + spring_stiffness: Default::default(), + spring_ref: Default::default(), } } + /// Sets a passive joint spring on `axis` (an index into the 6-DoF spatial + /// layout: `0..DIM` linear, `DIM..SPATIAL_DIM` angular). The spring applies + /// a generalized force `-stiffness · (q − rest)` integrated implicitly. + pub fn set_spring(&mut self, axis: usize, stiffness: Real, rest: Real) { + self.spring_stiffness[axis] = stiffness; + self.spring_ref[axis] = rest; + } + + /// The passive joint spring on `axis` as `(stiffness, rest)`, as set by + /// [`Self::set_spring`]. `(0, 0)` if no spring is set on that axis. + pub fn spring(&self, axis: usize) -> (Real, Real) { + (self.spring_stiffness[axis], self.spring_ref[axis]) + } + pub(crate) fn free(pos: Pose) -> Self { let mut result = Self::new(GenericJoint::default(), false); result.set_free_pos(pos); From 0d1fb7a7e09ebc6a8303152b69abc2baa50e5c60 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?S=C3=A9bastien=20Crozet?= Date: Thu, 25 Jun 2026 10:31:14 +0200 Subject: [PATCH 06/16] feat(mjcf-rs): add amterials loading + parse equality constraints --- crates/mjcf-rs/src/equality.rs | 20 ++++++++++++ crates/mjcf-rs/src/loader/asset.rs | 36 ++++++++++++++++------ crates/mjcf-rs/src/loader/attach.rs | 13 ++++++++ crates/mjcf-rs/src/loader/blocks.rs | 27 ++++++++++++++-- crates/mjcf-rs/src/loader/defaults.rs | 41 +++++++++++++++++++++++-- crates/mjcf-rs/src/loader/prototypes.rs | 37 ++++++++++++++++++++++ 6 files changed, 160 insertions(+), 14 deletions(-) diff --git a/crates/mjcf-rs/src/equality.rs b/crates/mjcf-rs/src/equality.rs index f1f7ac434..032b30e2b 100644 --- a/crates/mjcf-rs/src/equality.rs +++ b/crates/mjcf-rs/src/equality.rs @@ -7,6 +7,8 @@ pub enum Equality { Connect(EqualityConnect), /// `` — rigid attachment between two bodies. Weld(EqualityWeld), + /// `` — polynomial coupling between two joints' positions. + Joint(EqualityJoint), } /// Common metadata shared by all equality variants. @@ -49,3 +51,21 @@ pub struct EqualityWeld { /// `torquescale` attribute. pub torque_scale: f64, } + +/// `` element: couples the position of `joint2` to that of +/// `joint1` through a quartic polynomial of the offset from their reference +/// positions: +/// `q2 − ref2 = p0 + p1·(q1 − ref1) + p2·(q1 − ref1)² + p3·(q1 − ref1)³ + p4·(q1 − ref1)⁴`. +/// `joint2` may be omitted, in which case `joint1` is constrained to the +/// constant `p0`. +#[derive(Clone, Debug, Default)] +pub struct EqualityJoint { + /// Common metadata. + pub common: EqualityCommon, + /// First (independent) joint. + pub joint1: String, + /// Second (dependent) joint. `None` constrains `joint1` to `polycoef[0]`. + pub joint2: Option, + /// Polynomial coefficients `[p0, p1, p2, p3, p4]` (default `[0,1,0,0,0]`). + pub polycoef: [f64; 5], +} diff --git a/crates/mjcf-rs/src/loader/asset.rs b/crates/mjcf-rs/src/loader/asset.rs index 8e6cc5ff4..e874f6133 100644 --- a/crates/mjcf-rs/src/loader/asset.rs +++ b/crates/mjcf-rs/src/loader/asset.rs @@ -16,6 +16,7 @@ use super::parse_utils::{ absolutize_model_assets, asset_default_name, parse_f64, parse_f64_list, parse_quat, parse_u32, parse_vec3, parse_vec4, }; +use super::prototypes::merge_material_proto; use super::state::ParseState; impl ParseState { @@ -125,21 +126,36 @@ impl ParseState { self.model.assets.textures.push(t); } "material" => { - let mut m = Material::default(); + // Resolve the material against its `` + // chain: the material's own attributes win, then the class + // defaults fill the rest, then MuJoCo's built-in defaults + // (specular/shininess 0.5; metallic/roughness -1, a sentinel + // meaning "unset — fall back to the legacy Phong shininess"). + let inst = self.parse_material_prototype(child)?; + let mut name = None; + let mut class = None; for attr in child.attributes() { match attr.name() { - "name" => m.name = Some(attr.value().to_string()), - "class" => m.class = Some(attr.value().to_string()), - "texture" => m.texture = Some(attr.value().to_string()), - "rgba" => m.rgba = Some(parse_vec4(attr.value(), "material", "rgba")?), - "emission" => m.emission = parse_f64(attr.value())?, - "specular" => m.specular = parse_f64(attr.value())?, - "shininess" => m.shininess = parse_f64(attr.value())?, - "roughness" => m.roughness = parse_f64(attr.value())?, - "metallic" => m.metallic = parse_f64(attr.value())?, + "name" => name = Some(attr.value().to_string()), + "class" => class = Some(attr.value().to_string()), _ => {} } } + let p = merge_material_proto( + Some(self.merged_material_proto(class.as_deref())), + inst, + ); + let m = Material { + name, + class, + texture: p.texture, + rgba: p.rgba, + emission: p.emission.unwrap_or(0.0), + specular: p.specular.unwrap_or(0.5), + shininess: p.shininess.unwrap_or(0.5), + roughness: p.roughness.unwrap_or(-1.0), + metallic: p.metallic.unwrap_or(-1.0), + }; self.model.assets.materials.push(m); } "include" => { diff --git a/crates/mjcf-rs/src/loader/attach.rs b/crates/mjcf-rs/src/loader/attach.rs index 9d1b9be8c..3b0a7af28 100644 --- a/crates/mjcf-rs/src/loader/attach.rs +++ b/crates/mjcf-rs/src/loader/attach.rs @@ -171,6 +171,19 @@ impl ParseState { .equality .push(crate::equality::Equality::Weld(new_w)); } + crate::equality::Equality::Joint(j) => { + let mut new_j = j.clone(); + if let Some(n) = new_j.common.name.take() { + new_j.common.name = Some(prefixed(prefix, &n)); + } + new_j.joint1 = prefixed(prefix, &new_j.joint1); + if let Some(j2) = new_j.joint2.take() { + new_j.joint2 = Some(prefixed(prefix, &j2)); + } + self.model + .equality + .push(crate::equality::Equality::Joint(new_j)); + } } } } diff --git a/crates/mjcf-rs/src/loader/blocks.rs b/crates/mjcf-rs/src/loader/blocks.rs index 8dde1599a..185fdb759 100644 --- a/crates/mjcf-rs/src/loader/blocks.rs +++ b/crates/mjcf-rs/src/loader/blocks.rs @@ -6,7 +6,7 @@ use roxmltree::Node; use crate::Pose; use crate::contact::{ContactExclude, ContactPair}; -use crate::equality::{Equality, EqualityCommon, EqualityConnect, EqualityWeld}; +use crate::equality::{Equality, EqualityCommon, EqualityConnect, EqualityJoint, EqualityWeld}; use crate::error::ParseError; use crate::extras::{Actuator, ActuatorKind, Keyframe, Sensor}; use crate::types::Tristate; @@ -131,7 +131,30 @@ impl ParseState { } self.model.equality.push(Equality::Weld(w)); } - "joint" | "tendon" | "flex" | "flexvert" | "flexstrain" => { + "joint" => { + let mut ej = EqualityJoint { + common, + polycoef: [0.0, 1.0, 0.0, 0.0, 0.0], + ..Default::default() + }; + for attr in child.attributes() { + match attr.name() { + "joint1" => ej.joint1 = attr.value().to_string(), + "joint2" => ej.joint2 = Some(attr.value().to_string()), + "polycoef" => { + let v = parse_f64_list(attr.value())?; + for (i, slot) in ej.polycoef.iter_mut().enumerate() { + if let Some(c) = v.get(i) { + *slot = *c; + } + } + } + _ => {} + } + } + self.model.equality.push(Equality::Joint(ej)); + } + "tendon" | "flex" | "flexvert" | "flexstrain" => { log::debug!("equality `{tag}` is out of scope; skipped"); } "include" => self.parse_include(child, true)?, diff --git a/crates/mjcf-rs/src/loader/defaults.rs b/crates/mjcf-rs/src/loader/defaults.rs index 9acea48f5..67d489d46 100644 --- a/crates/mjcf-rs/src/loader/defaults.rs +++ b/crates/mjcf-rs/src/loader/defaults.rs @@ -19,8 +19,9 @@ use super::parse_utils::{ }; use super::prototypes::{ ActuatorPrototype, DefaultsClass, EqualityPrototype, GeomPrototype, JointPrototype, - MeshPrototype, PairPrototype, SitePrototype, merge_actuator_proto, merge_equality_proto, - merge_geom_proto, merge_joint_proto, merge_mesh_proto, merge_pair_proto, merge_site_proto, + MaterialPrototype, MeshPrototype, PairPrototype, SitePrototype, merge_actuator_proto, + merge_equality_proto, merge_geom_proto, merge_joint_proto, merge_material_proto, + merge_mesh_proto, merge_pair_proto, merge_site_proto, }; use super::state::ParseState; @@ -58,6 +59,10 @@ impl ParseState { let p = self.parse_site_prototype(child)?; class.site = Some(merge_site_proto(class.site.clone(), p)); } + "material" => { + let p = self.parse_material_prototype(child)?; + class.material = Some(merge_material_proto(class.material.clone(), p)); + } "mesh" => { let p = self.parse_mesh_prototype(child)?; class.mesh = Some(merge_mesh_proto(class.mesh.clone(), p)); @@ -220,6 +225,26 @@ impl ParseState { Ok(p) } + pub(super) fn parse_material_prototype( + &self, + node: Node, + ) -> Result { + let mut p = MaterialPrototype::default(); + for attr in node.attributes() { + match attr.name() { + "texture" => p.texture = Some(attr.value().to_string()), + "rgba" => p.rgba = Some(parse_vec4(attr.value(), "material", "rgba")?), + "emission" => p.emission = Some(parse_f64(attr.value())?), + "specular" => p.specular = Some(parse_f64(attr.value())?), + "shininess" => p.shininess = Some(parse_f64(attr.value())?), + "roughness" => p.roughness = Some(parse_f64(attr.value())?), + "metallic" => p.metallic = Some(parse_f64(attr.value())?), + _ => {} + } + } + Ok(p) + } + pub(super) fn parse_mesh_prototype(&self, node: Node) -> Result { let mut p = MeshPrototype::default(); let mut pose_pos = None; @@ -375,6 +400,18 @@ impl ParseState { acc } + pub(super) fn merged_material_proto(&self, class: Option<&str>) -> MaterialPrototype { + let mut acc = MaterialPrototype::default(); + for c in self.class_chain(class).into_iter().rev() { + if let Some(d) = self.defaults.get(&c) + && let Some(p) = &d.material + { + acc = merge_material_proto(Some(acc), p.clone()); + } + } + acc + } + pub(super) fn merged_mesh_proto(&self, class: Option<&str>) -> MeshPrototype { let mut acc = MeshPrototype::default(); for c in self.class_chain(class).into_iter().rev() { diff --git a/crates/mjcf-rs/src/loader/prototypes.rs b/crates/mjcf-rs/src/loader/prototypes.rs index 75f232f19..d5de24d18 100644 --- a/crates/mjcf-rs/src/loader/prototypes.rs +++ b/crates/mjcf-rs/src/loader/prototypes.rs @@ -26,6 +26,21 @@ pub(super) struct DefaultsClass { pub(super) velocity: Option, pub(super) intvelocity: Option, pub(super) damper: Option, + pub(super) material: Option, +} + +/// Per-class default `` attributes. Mirrors the numeric fields of +/// [`crate::assets::Material`] as `Option`s so a class default only fills the +/// attributes the material itself didn't set. +#[derive(Default, Clone)] +pub(super) struct MaterialPrototype { + pub(super) texture: Option, + pub(super) rgba: Option<[f64; 4]>, + pub(super) emission: Option, + pub(super) specular: Option, + pub(super) shininess: Option, + pub(super) roughness: Option, + pub(super) metallic: Option, } #[derive(Default, Clone)] @@ -196,6 +211,28 @@ pub(super) fn merge_geom_proto(base: Option, top: GeomPrototype) acc } +pub(super) fn merge_material_proto( + base: Option, + top: MaterialPrototype, +) -> MaterialPrototype { + let mut acc = base.unwrap_or_default(); + macro_rules! over { + ($f:ident) => { + if top.$f.is_some() { + acc.$f = top.$f; + } + }; + } + over!(texture); + over!(rgba); + over!(emission); + over!(specular); + over!(shininess); + over!(roughness); + over!(metallic); + acc +} + pub(super) fn merge_site_proto(base: Option, top: SitePrototype) -> SitePrototype { let mut acc = base.unwrap_or_default(); if top.pose.is_some() { From 7df48f49c4ea9589c712589db322faa1c13ee456 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?S=C3=A9bastien=20Crozet?= Date: Thu, 25 Jun 2026 10:33:03 +0200 Subject: [PATCH 07/16] feat(rapier3d-mjcf): keep track of the visual mesh materials --- crates/rapier3d-mjcf/src/loader/geom.rs | 42 +++++++++++++++++++++--- crates/rapier3d-mjcf/src/loader/types.rs | 42 ++++++++++++++++++++++++ 2 files changed, 79 insertions(+), 5 deletions(-) diff --git a/crates/rapier3d-mjcf/src/loader/geom.rs b/crates/rapier3d-mjcf/src/loader/geom.rs index 0f13b547e..663cefd16 100644 --- a/crates/rapier3d-mjcf/src/loader/geom.rs +++ b/crates/rapier3d-mjcf/src/loader/geom.rs @@ -526,7 +526,7 @@ impl<'a> Conversion<'a> { (shape, local_pose, None, None, None) }; - let (material_rgba, material_texture) = self.resolve_geom_material(g); + let (material_rgba, material_texture, material) = self.resolve_geom_material(g); let rgba = g .rgba .map(|c| [c[0] as f32, c[1] as f32, c[2] as f32, c[3] as f32]) @@ -540,6 +540,7 @@ impl<'a> Conversion<'a> { uvs, normals, texture, + material, }) } @@ -563,12 +564,16 @@ impl<'a> Conversion<'a> { fn resolve_geom_material( &self, g: &mb::Geom, - ) -> (Option<[f32; 4]>, Option) { + ) -> ( + Option<[f32; 4]>, + Option, + Option, + ) { let Some(name) = g.material.as_deref() else { - return (None, None); + return (None, None, None); }; let Some(material) = self.model.assets.material(name) else { - return (None, None); + return (None, None, None); }; let rgba = material .rgba @@ -579,7 +584,34 @@ impl<'a> Conversion<'a> { .and_then(|tname| self.model.assets.texture(tname)) .filter(|t| t.file.is_some()) .and_then(|t| self.model.resolve_texture_file(t, self.base_dir)); - (rgba, texture_path) + + // Map MuJoCo's material model onto metallic-roughness PBR. `metallic` + // and `roughness` carry the -1 "unset" sentinel (set at parse time); + // when unset, fall back to the legacy Phong — dielectric (metallic 0) + // with roughness derived from `shininess`. `specular` becomes the + // dielectric reflectance, and `emission` scales the material color into + // an emissive term. + let metallic = if material.metallic >= 0.0 { + material.metallic as f32 + } else { + 0.0 + }; + let roughness = if material.roughness >= 0.0 { + material.roughness as f32 + } else { + 1.0 - material.shininess as f32 + }; + let e = material.emission as f32; + let base = material.rgba.unwrap_or([1.0, 1.0, 1.0, 1.0]); + let pbr = super::types::MjcfRenderMaterial { + metallic: metallic.clamp(0.0, 1.0), + // A literal 0 roughness is a perfect mirror that reads as a shading + // bug; clamp to a small floor like real renderers do. + roughness: roughness.clamp(0.04, 1.0), + reflectance: (material.specular as f32).clamp(0.0, 1.0), + emissive: [base[0] as f32 * e, base[1] as f32 * e, base[2] as f32 * e], + }; + (rgba, texture_path, Some(pbr)) } } diff --git a/crates/rapier3d-mjcf/src/loader/types.rs b/crates/rapier3d-mjcf/src/loader/types.rs index 039eeb206..0965163ae 100644 --- a/crates/rapier3d-mjcf/src/loader/types.rs +++ b/crates/rapier3d-mjcf/src/loader/types.rs @@ -88,6 +88,28 @@ pub struct MjcfVisualMesh { /// MJCF `` when set, otherwise from the OBJ /// MTL's `map_Kd`. `None` for geoms that aren't textured. pub texture: Option, + /// PBR shading parameters resolved from the geom's ``. `None` + /// when the geom references no material — the renderer then keeps its own + /// default shading rather than being forced to a metallic-roughness look. + pub material: Option, +} + +/// PBR shading parameters resolved from an MJCF ``, expressed in the +/// metallic-roughness model a modern renderer consumes. MuJoCo's legacy Phong +/// `specular`/`shininess` are folded in here too: `reflectance` carries the +/// specular intensity, and `roughness` falls back to `1 − shininess` when the +/// material doesn't set `roughness` explicitly. +#[derive(Copy, Clone, Debug, PartialEq)] +pub struct MjcfRenderMaterial { + /// Metallic factor in `[0, 1]` (MuJoCo `metallic`; 0 = dielectric). + pub metallic: f32, + /// Surface roughness in `[0, 1]` (MuJoCo `roughness`, or `1 − shininess`). + pub roughness: f32, + /// Dielectric specular reflectance in `[0, 1]` (MuJoCo `specular`), which a + /// renderer maps to the F0 Fresnel term. + pub reflectance: f32, + /// Emissive color `material.rgb × emission`; `[0, 0, 0]` for non-emissive. + pub emissive: [f32; 3], } /// One joint from the MJCF model materialized as a rapier `GenericJoint`. @@ -152,6 +174,24 @@ pub struct MjcfEqualityJoint { pub joint: GenericJoint, } +/// One `` coupling, resolved to a *linear* relation between +/// two of [`MjcfRobot::joints`]: `q2 = coeff·q1 + offset` (rapier joint +/// coordinates). Higher-order `polycoef` terms are unsupported. Applied as a +/// multibody DoF coupling at insertion time. +#[derive(Copy, Clone, Debug)] +pub struct MjcfJointCoupling { + /// Index of the first (independent) joint in [`MjcfRobot::joints`]. + pub joint1: usize, + /// Index of the second (dependent) joint in [`MjcfRobot::joints`]. + pub joint2: usize, + /// Linear coupling coefficient (`polycoef[1]`). + pub coeff: Real, + /// Constant offset (`polycoef[0]`). + pub offset: Real, + /// Whether the constraint is active. + pub active: bool, +} + /// `` ready to drive a rapier joint motor. #[derive(Clone, Debug)] pub struct MjcfActuatorBinding { @@ -207,6 +247,8 @@ pub struct MjcfRobot { pub joints: Vec, /// Extra joints created from `` constraints. pub equality_joints: Vec, + /// `` couplings between two joints' coordinates. + pub joint_couplings: Vec, /// Actuator bindings. pub actuators: Vec, /// Sensor bindings. From 7f41c5601a64e862a269fbf5d9806eebc5c4b8e7 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?S=C3=A9bastien=20Crozet?= Date: Thu, 25 Jun 2026 10:33:49 +0200 Subject: [PATCH 08/16] feat(rapier3d-mjcf): joint dof coupling support --- crates/rapier3d-mjcf/src/loader/conversion.rs | 48 ++++++++++++- crates/rapier3d-mjcf/src/loader/insert.rs | 30 +++++++- crates/rapier3d-mjcf/src/loader/mass.rs | 68 ++++++++++++++++++- crates/rapier3d-mjcf/src/loader/mod.rs | 4 +- 4 files changed, 143 insertions(+), 7 deletions(-) diff --git a/crates/rapier3d-mjcf/src/loader/conversion.rs b/crates/rapier3d-mjcf/src/loader/conversion.rs index 59325739e..3baa632b1 100644 --- a/crates/rapier3d-mjcf/src/loader/conversion.rs +++ b/crates/rapier3d-mjcf/src/loader/conversion.rs @@ -10,6 +10,7 @@ use std::path::Path; use mjcf_rs::Pose as MPose; use mjcf_rs::body as mb; use mjcf_rs::equality::{Equality as MjcfEquality, EqualityConnect, EqualityWeld}; +use mjcf_rs::equality::EqualityJoint as MjcfEqualityJointDef; use mjcf_rs::extras::Sensor as MjcfSensor; use mjcf_rs::glam::DVec3; use mjcf_rs::model::{BodyEntry, BodyId, Model}; @@ -23,8 +24,8 @@ use rapier3d::math::{Pose, Real, Vector}; use super::mass::scale_mass_properties; use super::options::MjcfLoaderOptions; use super::types::{ - MjcfActuatorBinding, MjcfBody, MjcfEqualityJoint, MjcfJoint, MjcfRobot, MjcfSensorBinding, - MjcfVisualMesh, SensorObjectRef, + MjcfActuatorBinding, MjcfBody, MjcfEqualityJoint, MjcfJoint, MjcfJointCoupling, MjcfRobot, + MjcfSensorBinding, MjcfVisualMesh, SensorObjectRef, }; pub(super) struct Conversion<'a> { @@ -563,10 +564,53 @@ impl<'a> Conversion<'a> { }); } } + MjcfEquality::Joint(j) => self.materialize_joint_equality(j), } } } + /// ``: resolve the linear (first-order) coupling + /// `q2 = polycoef[1]·q1 + polycoef[0]` between two joints and record it for + /// the multibody insertion pass. The MJCF polynomial is in `(q − ref)`, but + /// rapier joint coordinates already subtract `ref`, so the reference terms + /// cancel and the relation is exactly `q2 = polycoef[1]·q1 + polycoef[0]`. + fn materialize_joint_equality(&mut self, j: &MjcfEqualityJointDef) { + if j.polycoef[2] != 0.0 || j.polycoef[3] != 0.0 || j.polycoef[4] != 0.0 { + log::warn!( + ": only a linear polycoef is supported; \ + higher-order terms are ignored", + j.common.name + ); + } + let Some(joint2_name) = j.joint2.as_deref() else { + log::warn!( + ": the single-joint form (joint2 omitted) is \ + unsupported; skipping", + j.common.name + ); + return; + }; + let (Some(joint1), Some(joint2)) = ( + self.robot.joint_name_to_idx.get(&j.joint1).copied(), + self.robot.joint_name_to_idx.get(joint2_name).copied(), + ) else { + log::warn!( + ": unknown joint1 {:?} or joint2 {:?}; skipping", + j.common.name, + j.joint1, + joint2_name, + ); + return; + }; + self.robot.joint_couplings.push(MjcfJointCoupling { + joint1, + joint2, + coeff: j.polycoef[1] as Real, + offset: j.polycoef[0] as Real, + active: j.common.active, + }); + } + fn resolve_connect_frames(&self, c: &EqualityConnect) -> Option<(usize, usize, Pose, Pose)> { let Some(body1) = self.robot.body_name_to_idx.get(&c.body1).copied() else { log::warn!( diff --git a/crates/rapier3d-mjcf/src/loader/insert.rs b/crates/rapier3d-mjcf/src/loader/insert.rs index b9b58713f..344da822d 100644 --- a/crates/rapier3d-mjcf/src/loader/insert.rs +++ b/crates/rapier3d-mjcf/src/loader/insert.rs @@ -15,8 +15,8 @@ use super::handles::{ MjcfActuatorHandle, MjcfBodyHandle, MjcfColliderHandle, MjcfJointHandle, MjcfRobotHandles, }; use super::mass::{ - add_armature_to_multibody, add_spring_to_multibody, add_springdamper_to_multibody, - move_motor_damping_to_multibody, + add_armature_to_multibody, add_joint_coupling_to_multibody, add_spring_to_multibody, + add_springdamper_to_multibody, move_motor_damping_to_multibody, }; use super::options::MjcfMultibodyOptions; use super::types::MjcfRobot; @@ -270,6 +270,32 @@ impl MjcfRobot { } } + // `` couplings: install each as a multibody DoF + // coupling between the two joints' free DoFs (resolved from the joint + // handles built above). Loop closures and couplings are both off when + // `skip_loop_closures` is set. + if !skip_loop_closures { + for c in &self.joint_couplings { + if !c.active { + continue; + } + let (Some(jh1), Some(jh2)) = + (joint_handles.get(c.joint1), joint_handles.get(c.joint2)) + else { + continue; + }; + let Some(h1) = jh1.joint else { continue }; + add_joint_coupling_to_multibody( + multibody_joints, + h1, + jh1.link2, + jh2.link2, + c.coeff, + c.offset, + ); + } + } + let mut eq_handles = Vec::with_capacity(self.equality_joints.len()); if !skip_loop_closures { for eq in self.equality_joints { diff --git a/crates/rapier3d-mjcf/src/loader/mass.rs b/crates/rapier3d-mjcf/src/loader/mass.rs index d1afd6cc3..b8feee5d7 100644 --- a/crates/rapier3d-mjcf/src/loader/mass.rs +++ b/crates/rapier3d-mjcf/src/loader/mass.rs @@ -11,7 +11,8 @@ use mjcf_rs::compiler::InertiaFromGeom; use mjcf_rs::model::BodyEntry; use rapier3d::dynamics::{ - MassProperties, MultibodyJointHandle, MultibodyJointSet, RigidBodySet, + MassProperties, Multibody, MultibodyDofCoupling, MultibodyJointHandle, MultibodyJointSet, + RigidBodyHandle, RigidBodySet, }; use rapier3d::math::{Matrix, Pose, Real, Vector}; @@ -384,3 +385,68 @@ pub(super) fn add_springdamper_to_multibody( } } } + +/// The local DoF index and spatial axis of a single-DoF multibody joint's free +/// axis (the first non-locked axis). Returns `None` if the link or a free axis +/// can't be found. +fn single_free_axis(multibody: &Multibody, link_id: usize) -> Option<(usize, usize)> { + use rapier3d::math::SPATIAL_DIM; + let link = multibody.links().nth(link_id)?; + let locked = link.joint.data.locked_axes.bits(); + for axis in 0..SPATIAL_DIM { + if (locked & (1 << axis)) == 0 { + // First free axis ⇒ local DoF index 0 within the link's slice. + return Some((0, axis)); + } + } + None +} + +/// Install a `` coupling `q2 = coeff·q1 + offset` as a DoF +/// coupling on the multibody shared by the two joints (`child1`/`child2` are +/// the child bodies of joint1/joint2; `handle1` is joint1's multibody handle). +/// Both joints must live in the same multibody and be single-DoF (hinge/slide). +pub(super) fn add_joint_coupling_to_multibody( + multibody_joints: &mut MultibodyJointSet, + handle1: MultibodyJointHandle, + child1: RigidBodyHandle, + child2: RigidBodyHandle, + coeff: Real, + offset: Real, +) { + let (Some(l1), Some(l2)) = ( + multibody_joints.rigid_body_link(child1).copied(), + multibody_joints.rigid_body_link(child2).copied(), + ) else { + return; + }; + if l1.multibody != l2.multibody { + log::warn!( + ": joint1 and joint2 are in different multibodies; \ + cross-multibody coupling is unsupported, skipping" + ); + return; + } + let link2_id = l2.id; + + let Some((multibody, link1_id)) = multibody_joints.get_mut(handle1) else { + return; + }; + let (Some((dof1, axis1)), Some((dof2, axis2))) = ( + single_free_axis(multibody, link1_id), + single_free_axis(multibody, link2_id), + ) else { + log::warn!(": a coupled joint has no free DoF; skipping"); + return; + }; + multibody.add_dof_coupling(MultibodyDofCoupling { + link1: link1_id, + dof1, + axis1, + link2: link2_id, + dof2, + axis2, + coeff, + offset, + }); +} diff --git a/crates/rapier3d-mjcf/src/loader/mod.rs b/crates/rapier3d-mjcf/src/loader/mod.rs index a421a1411..c3c337f89 100644 --- a/crates/rapier3d-mjcf/src/loader/mod.rs +++ b/crates/rapier3d-mjcf/src/loader/mod.rs @@ -22,6 +22,6 @@ pub use handles::{ pub use options::{ContactFilterMode, MjcfLoaderOptions, MjcfMultibodyOptions}; pub use runtime::MjcfSensorValue; pub use types::{ - MjcfActuatorBinding, MjcfBody, MjcfEqualityJoint, MjcfJoint, MjcfRobot, MjcfSensorBinding, - MjcfVisualMesh, SensorObjectRef, + MjcfActuatorBinding, MjcfBody, MjcfEqualityJoint, MjcfJoint, MjcfRenderMaterial, MjcfRobot, + MjcfSensorBinding, MjcfVisualMesh, SensorObjectRef, }; From 6446af03380f2be05f869e33fdf7f62083d46260 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?S=C3=A9bastien=20Crozet?= Date: Thu, 25 Jun 2026 10:34:07 +0200 Subject: [PATCH 09/16] feat(rapier3d-mjcf): actuator max force support --- crates/rapier3d-mjcf/src/loader/runtime.rs | 15 +++++++++++++++ 1 file changed, 15 insertions(+) diff --git a/crates/rapier3d-mjcf/src/loader/runtime.rs b/crates/rapier3d-mjcf/src/loader/runtime.rs index 27d430013..81062f6f2 100644 --- a/crates/rapier3d-mjcf/src/loader/runtime.rs +++ b/crates/rapier3d-mjcf/src/loader/runtime.rs @@ -319,15 +319,28 @@ fn configure_actuator_motor( ActuatorKind::Position => { data.set_motor_position(ax, u, kp, kv); data.set_motor_position(lin_ax, u, kp, kv); + // Apply the actuator's `forcerange` as the motor's max force. + // `set_motor_position` leaves `max_force` untouched, so without this + // the servo inherits whatever joint construction left it at (e.g. the + // `frictionloss` passed to `motor_max_force` when building the joint), + // which starves position servos — the joint can't deliver enough + // torque to hold against gravity. `force_max` is `INFINITY` when no + // `forcerange` is given, which leaves the motor unbounded (a no-op). + data.set_motor_max_force(ax, force_max); + data.set_motor_max_force(lin_ax, force_max); } ActuatorKind::Velocity => { data.set_motor_velocity(ax, u, kv); data.set_motor_velocity(lin_ax, u, kv); + data.set_motor_max_force(ax, force_max); + data.set_motor_max_force(lin_ax, force_max); } ActuatorKind::Damper => { let damp = actuator.gainprm.first().copied().unwrap_or(0.0) as Real * u.abs(); data.set_motor_velocity(ax, 0.0, damp); data.set_motor_velocity(lin_ax, 0.0, damp); + data.set_motor_max_force(ax, force_max); + data.set_motor_max_force(lin_ax, force_max); } ActuatorKind::General => { let g0 = actuator.gainprm.first().copied().unwrap_or(0.0) as Real; @@ -344,6 +357,8 @@ fn configure_actuator_motor( let target = (g0 * u + b0) / stiffness; data.set_motor_position(ax, target, stiffness, damping); data.set_motor_position(lin_ax, target, stiffness, damping); + data.set_motor_max_force(ax, force_max); + data.set_motor_max_force(lin_ax, force_max); } else { // `gaintype="fixed"`, non-restoring bias: constant force // `gainprm0·u` through the transmission. From ab4a97549220865642219e51c91c43ab4135875e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?S=C3=A9bastien=20Crozet?= Date: Thu, 25 Jun 2026 10:35:03 +0200 Subject: [PATCH 10/16] chore: more tests --- crates/rapier3d-mjcf/tests/equality_joint.rs | 108 ++++++++++++++++ .../rapier3d-mjcf/tests/material_defaults.rs | 116 ++++++++++++++++++ crates/rapier3d/tests/multibody_coupling.rs | 102 +++++++++++++++ 3 files changed, 326 insertions(+) create mode 100644 crates/rapier3d-mjcf/tests/equality_joint.rs create mode 100644 crates/rapier3d-mjcf/tests/material_defaults.rs create mode 100644 crates/rapier3d/tests/multibody_coupling.rs diff --git a/crates/rapier3d-mjcf/tests/equality_joint.rs b/crates/rapier3d-mjcf/tests/equality_joint.rs new file mode 100644 index 000000000..54e72c245 --- /dev/null +++ b/crates/rapier3d-mjcf/tests/equality_joint.rs @@ -0,0 +1,108 @@ +//! `` joint-coupling constraint (the robotiq gripper's two +//! driver joints moving together). The loader parses the polynomial coupling, +//! records its linear form, and installs it as a multibody DoF coupling. + +use rapier3d::prelude::*; +use rapier3d_mjcf::{MjcfLoaderOptions, MjcfMultibodyOptions, MjcfRobot}; + +// `base` is welded to the world (fixed); `arm1`/`arm2` are sibling hinges about +// Z, coupled `q2 = 2·q1`. A position actuator drives `q1`. +const XML: &str = r#" + + +"#; + +#[test] +fn equality_joint_parses_to_linear_coupling() { + let (robot, _) = MjcfRobot::from_str(XML, MjcfLoaderOptions::default(), ".").unwrap(); + assert_eq!(robot.joint_couplings.len(), 1); + let c = &robot.joint_couplings[0]; + assert_eq!(robot.joints[c.joint1].name.as_deref(), Some("j1")); + assert_eq!(robot.joints[c.joint2].name.as_deref(), Some("j2")); + assert_eq!(c.coeff, 2.0); // polycoef[1] + assert_eq!(c.offset, 0.0); // polycoef[0] + assert!(c.active); +} + +#[test] +fn equality_joint_couples_multibody_joints() { + let (robot, _) = MjcfRobot::from_str(XML, MjcfLoaderOptions::default(), ".").unwrap(); + let mut bodies = RigidBodySet::new(); + let mut colliders = ColliderSet::new(); + let mut impulse_joints = ImpulseJointSet::new(); + let mut multibody_joints = MultibodyJointSet::new(); + let handles = robot.clone().insert_using_multibody_joints( + &mut bodies, + &mut colliders, + &mut multibody_joints, + &mut impulse_joints, + MjcfMultibodyOptions::empty(), + ); + + // Handles of the two driver joints, in `robot.joints` order. + let h1 = handles.joints[robot.joint_couplings[0].joint1] + .joint + .unwrap(); + let h2 = handles.joints[robot.joint_couplings[0].joint2] + .joint + .unwrap(); + + // Read a hinge's generalized coordinate (AngX = spatial axis 3). + let read_q = |mbj: &MultibodyJointSet, h| { + let (mb, link_id) = mbj.get(h).unwrap(); + mb.links().nth(link_id).unwrap().joint().coords()[3] + }; + + let ip = IntegrationParameters::default(); + let mut pipeline = PhysicsPipeline::new(); + let mut islands = IslandManager::new(); + let mut broad_phase = DefaultBroadPhase::new(); + let mut narrow_phase = NarrowPhase::new(); + let mut ccd = CCDSolver::new(); + // Drive j1 toward 0.3; the coupling must keep j2 = 2·j1. + for _ in 0..400 { + handles.apply_controls_multibody(&mut bodies, &mut multibody_joints, &[0.3]); + pipeline.step( + Vector::ZERO, + &ip, + &mut islands, + &mut broad_phase, + &mut narrow_phase, + &mut bodies, + &mut colliders, + &mut impulse_joints, + &mut multibody_joints, + &mut ccd, + &(), + &(), + ); + } + + let q1 = read_q(&multibody_joints, h1); + let q2 = read_q(&multibody_joints, h2); + assert!((q1 - 0.3).abs() < 0.05, "driven joint q1 = {q1}, expected ~0.3"); + assert!( + (q2 - 2.0 * q1).abs() < 0.02, + "coupling q2 = 2·q1 not enforced: q1 = {q1}, q2 = {q2}" + ); +} diff --git a/crates/rapier3d-mjcf/tests/material_defaults.rs b/crates/rapier3d-mjcf/tests/material_defaults.rs new file mode 100644 index 000000000..47e6e3e7e --- /dev/null +++ b/crates/rapier3d-mjcf/tests/material_defaults.rs @@ -0,0 +1,116 @@ +//! `` elements inherit attributes from their `` +//! block (MuJoCo class-default resolution), and the loader maps the result to +//! PBR shading parameters. + +use rapier3d_mjcf::{MjcfLoaderOptions, MjcfRenderMaterial, MjcfRobot}; + +fn first_material(xml: &str) -> MjcfRenderMaterial { + let opts = MjcfLoaderOptions { + create_colliders_from_visual_shapes: false, + ..Default::default() + }; + let (robot, _) = MjcfRobot::from_str(xml, opts, ".").unwrap(); + robot + .bodies + .iter() + .flat_map(|b| b.visual_meshes.iter()) + .find_map(|vm| vm.material) + .expect("a visual mesh with a resolved material") +} + +const MODEL: &str = r#" + + + + + + + + + + + + + + + + + + + + + + + +"#; + +fn material_named(xml: &str, geom: &str) -> MjcfRenderMaterial { + // The three geoms map 1:1 onto the three bodies, in declaration order. + let opts = MjcfLoaderOptions { + create_colliders_from_visual_shapes: false, + ..Default::default() + }; + let (robot, _) = MjcfRobot::from_str(xml, opts, ".").unwrap(); + let idx = match geom { + "g_class" => 0, + "g_over" => 1, + "g_plain" => 2, + _ => unreachable!(), + }; + robot + .bodies + .iter() + .filter(|b| !b.visual_meshes.is_empty()) + .nth(idx) + .and_then(|b| b.visual_meshes.first()) + .and_then(|vm| vm.material) + .expect("resolved material") +} + +#[test] +fn material_inherits_class_defaults() { + // `from_class` sets only rgba; specular/shininess/metallic come from the + // `shiny` default class: metallic 1, reflectance 0.9, roughness 1 − 0.8. + let m = material_named(MODEL, "g_class"); + assert!((m.metallic - 1.0).abs() < 1e-6, "metallic = {}", m.metallic); + assert!((m.reflectance - 0.9).abs() < 1e-6, "reflectance = {}", m.reflectance); + assert!( + (m.roughness - 0.2).abs() < 1e-6, + "roughness = {} (expected 1 − shininess(0.8))", + m.roughness + ); +} + +#[test] +fn material_own_attribute_overrides_class_default() { + // `overrides` keeps the class metallic/specular but sets its own shininess, + // so roughness = 1 − 0.1, not 1 − 0.8. + let m = material_named(MODEL, "g_over"); + assert!((m.metallic - 1.0).abs() < 1e-6, "metallic = {}", m.metallic); + assert!( + (m.roughness - 0.9).abs() < 1e-6, + "own shininess(0.1) should win over class: roughness = {}", + m.roughness + ); +} + +#[test] +fn material_without_class_uses_mujoco_defaults() { + // `plain` has no class: MuJoCo defaults specular/shininess 0.5, dielectric. + let m = material_named(MODEL, "g_plain"); + assert!((m.metallic - 0.0).abs() < 1e-6, "metallic = {}", m.metallic); + assert!((m.reflectance - 0.5).abs() < 1e-6, "reflectance = {}", m.reflectance); + assert!( + (m.roughness - 0.5).abs() < 1e-6, + "default shininess(0.5) → roughness = {}", + m.roughness + ); +} + +#[test] +fn smoke_first_material_resolves() { + let _ = first_material(MODEL); +} diff --git a/crates/rapier3d/tests/multibody_coupling.rs b/crates/rapier3d/tests/multibody_coupling.rs new file mode 100644 index 000000000..cbcb78c43 --- /dev/null +++ b/crates/rapier3d/tests/multibody_coupling.rs @@ -0,0 +1,102 @@ +//! `Multibody` DoF couplings (`q2 = coeff·q1 + offset`) enforced as velocity +//! constraints — the mechanism behind MuJoCo's `` (e.g. the +//! robotiq gripper's two driver joints moving together). + +use rapier3d::dynamics::MultibodyDofCoupling; +use rapier3d::prelude::*; + +/// Two sibling hinges (both children of a fixed base, rotating about Z in +/// place), coupled `q2 = coeff·q1 + offset`. A position motor drives `q1` to +/// `target`; after settling, `q2` should track `coeff·target + offset`. +fn run_coupling(coeff: Real, offset: Real, target: Real) -> (Real, Real) { + let mut bodies = RigidBodySet::new(); + let mut colliders = ColliderSet::new(); + let mut impulse_joints = ImpulseJointSet::new(); + let mut multibody_joints = MultibodyJointSet::new(); + + let inertia = Vector::new(0.1, 0.1, 0.1); + let base = bodies.insert(RigidBodyBuilder::fixed()); + let link1 = bodies.insert( + RigidBodyBuilder::dynamic() + .additional_mass_properties(MassProperties::new(Vector::ZERO, 1.0, inertia)), + ); + let link2 = bodies.insert( + RigidBodyBuilder::dynamic() + .additional_mass_properties(MassProperties::new(Vector::ZERO, 1.0, inertia)), + ); + + let h1 = multibody_joints + .insert(base, link1, RevoluteJointBuilder::new(Vector::Z), true) + .unwrap(); + let h2 = multibody_joints + .insert(base, link2, RevoluteJointBuilder::new(Vector::Z), true) + .unwrap(); + + let link1_id = multibody_joints.get(h1).unwrap().1; + let link2_id = multibody_joints.get(h2).unwrap().1; + + { + let (mb, _) = multibody_joints.get_mut(h1).unwrap(); + // Couple q2 = coeff·q1 + offset (AngX is spatial axis 3, the hinge DoF). + mb.add_dof_coupling(MultibodyDofCoupling { + link1: link1_id, + dof1: 0, + axis1: 3, + link2: link2_id, + dof2: 0, + axis2: 3, + coeff, + offset, + }); + // Drive link1's hinge to `target`. + let link1_joint = &mut mb.links_mut().nth(link1_id).unwrap().joint.data; + link1_joint.set_motor_position(JointAxis::AngX, target, 20.0, 2.0); + } + + let gravity = Vector::ZERO; + let integration_parameters = IntegrationParameters::default(); + let mut pipeline = PhysicsPipeline::new(); + let mut islands = IslandManager::new(); + let mut broad_phase = DefaultBroadPhase::new(); + let mut narrow_phase = NarrowPhase::new(); + let mut ccd = CCDSolver::new(); + for _ in 0..300 { + pipeline.step( + gravity, + &integration_parameters, + &mut islands, + &mut broad_phase, + &mut narrow_phase, + &mut bodies, + &mut colliders, + &mut impulse_joints, + &mut multibody_joints, + &mut ccd, + &(), + &(), + ); + } + let q1 = bodies[link1].rotation().to_scaled_axis().z; + let q2 = bodies[link2].rotation().to_scaled_axis().z; + (q1, q2) +} + +#[test] +fn one_to_one_coupling_makes_joints_track() { + // q2 = q1: the robotiq case. Drive q1 to 0.5; q2 must follow. + let (q1, q2) = run_coupling(1.0, 0.0, 0.5); + assert!((q1 - 0.5).abs() < 0.05, "driven joint didn't reach target: q1 = {q1}"); + assert!((q2 - q1).abs() < 0.02, "coupling not enforced: q1 = {q1}, q2 = {q2}"); +} + +#[test] +fn scaled_and_offset_coupling() { + // q2 = -0.5·q1 + 0.2: a gear ratio with an offset. + let (q1, q2) = run_coupling(-0.5, 0.2, 0.6); + let expected = -0.5 * q1 + 0.2; + assert!((q1 - 0.6).abs() < 0.05, "driven joint didn't reach target: q1 = {q1}"); + assert!( + (q2 - expected).abs() < 0.02, + "coupling q2 = -0.5·q1 + 0.2 not enforced: q1 = {q1}, q2 = {q2}, expected {expected}" + ); +} From 57d72be432eae010d31d1a391d9b6464ba06f7a2 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?S=C3=A9bastien=20Crozet?= Date: Thu, 25 Jun 2026 10:36:38 +0200 Subject: [PATCH 11/16] feat(testbed): add the ability so specify a material --- src_testbed/graphics.rs | 50 +++++++++++++++++++++++-- src_testbed/lib.rs | 2 +- src_testbed/testbed/graphics_context.rs | 2 + src_testbed/testbed/testbed.rs | 2 + 4 files changed, 52 insertions(+), 4 deletions(-) diff --git a/src_testbed/graphics.rs b/src_testbed/graphics.rs index 1d9bc3d07..9c2ece282 100644 --- a/src_testbed/graphics.rs +++ b/src_testbed/graphics.rs @@ -121,6 +121,31 @@ const GROUND_COLOR: Color = Color { a: 1.0, }; +/// PBR shading parameters for a body render mesh, applied on top of its base +/// color/texture. +#[derive(Copy, Clone, Debug)] +pub struct RenderMaterial { + /// Metallic factor in `[0, 1]`. + pub metallic: f32, + /// Surface roughness in `[0, 1]`. + pub roughness: f32, + /// Dielectric specular reflectance in `[0, 1]` (mapped to the F0 term). + pub reflectance: f32, + /// Emissive color (added on top of lit shading); `[0, 0, 0]` for none. + pub emissive: [f32; 3], +} + +impl Default for RenderMaterial { + fn default() -> Self { + Self { + metallic: 0.0, + roughness: 0.7, + reflectance: 0.5, + emissive: [0.0; 3], + } + } +} + impl GraphicsManager { pub fn new() -> GraphicsManager { GraphicsManager { @@ -211,6 +236,7 @@ impl GraphicsManager { uvs: Option<&[[f32; 2]]>, normals: Option<&[[f32; 3]]>, texture: Option<&Path>, + material: Option, ) { // Plumbing per-vertex UVs into a custom kiss3d mesh is only // supported in 3D; 2D always uses the standard node path. @@ -278,8 +304,8 @@ impl GraphicsManager { }; #[cfg(feature = "dim2")] let mut node = { - // Custom UV/normal plumbing is unsupported in 2D; ignore them. - let _ = (uvs, normals); + // Custom UV/normal plumbing and PBR materials are unsupported in 2D. + let _ = (uvs, normals, material); Self::create_individual_node(&mut self.scene, &**shape, color, false) }; @@ -290,7 +316,25 @@ impl GraphicsManager { // culling here too — visual meshes are routinely viewed from // both sides and authored winding isn't guaranteed. #[cfg(feature = "dim3")] - n.enable_backface_culling_recursive(false); + { + n.enable_backface_culling_recursive(false); + if let Some(mat) = material { + n.set_metallic_recursive(mat.metallic); + n.set_roughness_recursive(mat.roughness); + n.set_reflectance(mat.reflectance); + n.set_emissive_recursive(Color::new( + mat.emissive[0], + mat.emissive[1], + mat.emissive[2], + 1.0, + )); + } + // A translucent base color needs the blend pass; opaque meshes + // keep the default opaque path. + if color.a < 1.0 { + n.set_alpha_mode(kiss3d::scene::AlphaMode::Blend); + } + } if let Some(tex_path) = texture { // The cache key uses the absolute path string so the // same texture file is uploaded only once across the diff --git a/src_testbed/lib.rs b/src_testbed/lib.rs index 7547f77fc..21af98740 100644 --- a/src_testbed/lib.rs +++ b/src_testbed/lib.rs @@ -2,7 +2,7 @@ extern crate nalgebra as na; -pub use crate::graphics::GraphicsManager; +pub use crate::graphics::{GraphicsManager, RenderMaterial}; pub use crate::harness::plugin::HarnessPlugin; pub use crate::physics::PhysicsState; pub use crate::plugin::TestbedPlugin; diff --git a/src_testbed/testbed/graphics_context.rs b/src_testbed/testbed/graphics_context.rs index e83a10971..3439bede7 100644 --- a/src_testbed/testbed/graphics_context.rs +++ b/src_testbed/testbed/graphics_context.rs @@ -68,6 +68,7 @@ impl<'a> TestbedGraphics<'a> { uvs: Option<&[[f32; 2]]>, normals: Option<&[[f32; 3]]>, texture: Option<&std::path::Path>, + material: Option, ) { self.graphics.add_body_render_mesh( self.window, @@ -78,6 +79,7 @@ impl<'a> TestbedGraphics<'a> { uvs, normals, texture, + material, ); } diff --git a/src_testbed/testbed/testbed.rs b/src_testbed/testbed/testbed.rs index 065eda5ca..f6259c6ed 100644 --- a/src_testbed/testbed/testbed.rs +++ b/src_testbed/testbed/testbed.rs @@ -308,6 +308,7 @@ impl Testbed<'_> { uvs: Option<&[[f32; 2]]>, normals: Option<&[[f32; 3]]>, texture: Option<&std::path::Path>, + material: Option, ) { if let Some(graphics) = &mut self.graphics { graphics.graphics.add_body_render_mesh( @@ -319,6 +320,7 @@ impl Testbed<'_> { uvs, normals, texture, + material, ); } } From 402f0d2d389511eebb2210e089ba1dc5c0bb9e0b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?S=C3=A9bastien=20Crozet?= Date: Thu, 25 Jun 2026 10:38:24 +0200 Subject: [PATCH 12/16] feat(rapier): suport multibody dofs coupling --- examples3d/mujoco_menagerie3.rs | 14 +- src/dynamics/joint/multibody_joint/mod.rs | 2 +- .../joint/multibody_joint/multibody.rs | 140 +++++++++++++++++- .../generic_joint_constraint_builder.rs | 22 ++- src/dynamics/solver/velocity_solver.rs | 4 +- 5 files changed, 167 insertions(+), 15 deletions(-) diff --git a/examples3d/mujoco_menagerie3.rs b/examples3d/mujoco_menagerie3.rs index 1f2191dfb..adc35c77f 100644 --- a/examples3d/mujoco_menagerie3.rs +++ b/examples3d/mujoco_menagerie3.rs @@ -3,7 +3,7 @@ use std::path::{Path, PathBuf}; use std::sync::atomic::{AtomicUsize, Ordering}; use kiss3d::color::Color; -use rapier_testbed3d::{Testbed, settings::StringDisplayMode}; +use rapier_testbed3d::{RenderMaterial, Testbed, settings::StringDisplayMode}; use rapier3d::prelude::*; use rapier3d_mjcf::{MjcfLoaderOptions, MjcfMultibodyOptions, MjcfRobot, MjcfRobotHandles}; @@ -169,10 +169,11 @@ fn add_floor(world: &mut PhysicsWorld) { let mut floor_hext = total_aabb.half_extents(); let mut floor_center = total_aabb.center(); - floor_center.z -= floor_hext.z * 2.0; floor_hext.x *= 10.0; floor_hext.y *= 10.0; - // floor_hext.z = 1.0; + floor_center.z -= floor_hext.z; + floor_hext.z = 0.2; + floor_center.z -= 0.2; world.insert_collider( ColliderBuilder::cuboid(floor_hext.x, floor_hext.y, floor_hext.z).translation(floor_center), None, @@ -309,6 +310,12 @@ fn register_visual_meshes( } else { untextured_fallback }); + let material = vm.material.map(|m| RenderMaterial { + metallic: m.metallic, + roughness: m.roughness, + reflectance: m.reflectance, + emissive: m.emissive, + }); testbed.add_body_render_mesh( *handle, &vm.shape, @@ -317,6 +324,7 @@ fn register_visual_meshes( vm.uvs.as_deref(), vm.normals.as_deref(), vm.texture.as_deref(), + material, ); } } diff --git a/src/dynamics/joint/multibody_joint/mod.rs b/src/dynamics/joint/multibody_joint/mod.rs index 5e97e7909..26d9f7897 100644 --- a/src/dynamics/joint/multibody_joint/mod.rs +++ b/src/dynamics/joint/multibody_joint/mod.rs @@ -1,7 +1,7 @@ //! MultibodyJoints using the reduced-coordinates formalism or using constraints. #[cfg(feature = "alloc")] -pub use self::multibody::Multibody; +pub use self::multibody::{Multibody, MultibodyDofCoupling}; #[cfg(feature = "alloc")] pub use self::multibody_ik::InverseKinematicsOption; #[cfg(feature = "alloc")] diff --git a/src/dynamics/joint/multibody_joint/multibody.rs b/src/dynamics/joint/multibody_joint/multibody.rs index 9743f98f1..536318b31 100644 --- a/src/dynamics/joint/multibody_joint/multibody.rs +++ b/src/dynamics/joint/multibody_joint/multibody.rs @@ -1,7 +1,11 @@ use super::multibody_link::{MultibodyLink, MultibodyLinkVec}; use super::multibody_workspace::MultibodyWorkspace; use crate::alloc_prelude::*; -use crate::dynamics::{RigidBodyHandle, RigidBodySet, RigidBodyType, RigidBodyVelocity}; +use crate::dynamics::integration_parameters::SpringCoefficients; +use crate::dynamics::solver::{GenericJointConstraint, WritebackId}; +use crate::dynamics::{ + IntegrationParameters, RigidBodyHandle, RigidBodySet, RigidBodyType, RigidBodyVelocity, +}; use crate::math::{ ANG_DIM, AngDim, AngVector, DIM, DVector, Dim, Jacobian, Pose, Real, SPATIAL_DIM, SimdAngVector, Vector, @@ -58,6 +62,34 @@ fn concat_rb_mass_matrix( result } +/// A holonomic coupling between two generalized coordinates of a single +/// [`Multibody`], `q2 = coeff · q1 + offset`, enforced as a velocity-level +/// equality constraint. This is how MuJoCo's `` (a polynomial +/// joint-to-joint coupling) is represented for the linear (first-order) case — +/// e.g. the robotiq gripper's two driver joints moving together. +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +#[derive(Copy, Clone, Debug)] +pub struct MultibodyDofCoupling { + /// Internal id of the link carrying the first joint. + pub link1: usize, + /// Local free-DoF index of the coupled DoF within `link1` (its position in + /// that link's slice of the generalized vectors). + pub dof1: usize, + /// Spatial-coordinate axis (`0..6`) of `link1`'s coupled DoF, used to read + /// its generalized position from the joint coords. + pub axis1: usize, + /// Internal id of the link carrying the second joint. + pub link2: usize, + /// Local free-DoF index of the coupled DoF within `link2`. + pub dof2: usize, + /// Spatial-coordinate axis (`0..6`) of `link2`'s coupled DoF. + pub axis2: usize, + /// Linear coupling coefficient: the constraint is `q2 − coeff·q1 − offset = 0`. + pub coeff: Real, + /// Constant offset of the coupling. + pub offset: Real, +} + /// An articulated body simulated using the reduced-coordinates approach. #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] #[derive(Clone, Debug)] @@ -88,6 +120,10 @@ pub struct Multibody { pub(crate) root_is_dynamic: bool, pub(crate) solver_id: u32, self_contacts_enabled: bool, + /// Holonomic couplings between two of this multibody's generalized + /// coordinates (`q2 = coeff·q1 + offset`), e.g. MuJoCo's + /// ``. Resolved as velocity constraints each step. + couplings: Vec, /* * Workspaces. @@ -130,6 +166,7 @@ impl Multibody { i_coriolis_dt: Jacobian::zeros(0), root_is_dynamic: false, self_contacts_enabled, + couplings: Vec::new(), // solver_workspace: Some(SolverWorkspace::new()), } } @@ -439,7 +476,7 @@ impl Multibody { .extend((0..num_jacobians).map(|_| Jacobian::zeros(0))); } - pub(crate) fn update_acceleration(&mut self, bodies: &RigidBodySet) { + pub(crate) fn update_acceleration(&mut self, dt: Real, bodies: &RigidBodySet) { if self.ndofs == 0 { return; // Nothing to do. } @@ -511,9 +548,14 @@ impl Multibody { self.accelerations .cmpy(-1.0, &self.damping, &self.velocities, 1.0); - // Implicit joint springs: generalized force `-k·(q − rest)` evaluated at - // the current position (the implicit `dt²·k` correction lives on the - // mass-matrix diagonal, see `update_mass_matrix`). + // Implicit joint springs. The backward-Euler spring force evaluated at + // the end-of-step position `q⁺ = q + dt·v⁺` is `-k·(q − rest) − k·dt·v⁺`. + // The `−k·dt·v⁺` part is made implicit by the `dt²·k` term on the + // mass-matrix diagonal (see `update_mass_matrix`); for that to be + // consistent the generalized force here must include both the position + // term `-k·(q − rest)` *and* the velocity-coupling term `-k·dt·v` + // (at the current `v`). Omitting the latter leaves the spring only + // semi-implicit. for li in 0..self.links.len() { let mut idx = self.links[li].assembly_id; let locked = self.links[li].joint.data.locked_axes.bits(); @@ -523,7 +565,8 @@ impl Multibody { if k != 0.0 { let q = self.links[li].joint.coords[a]; let rest = self.links[li].joint.spring_ref[a]; - self.accelerations[idx] += -k * (q - rest); + self.accelerations[idx] += + -k * (q - rest) - k * dt * self.velocities[idx]; } idx += 1; } @@ -934,6 +977,91 @@ impl Multibody { out } + /// Adds a holonomic coupling between two of this multibody's generalized + /// coordinates (`q2 = coeff·q1 + offset`), enforced as a velocity-level + /// equality constraint each step. See [`MultibodyDofCoupling`]. + pub fn add_dof_coupling(&mut self, coupling: MultibodyDofCoupling) { + self.couplings.push(coupling); + } + + /// The DoF couplings declared on this multibody. + pub fn couplings(&self) -> &[MultibodyDofCoupling] { + &self.couplings + } + + /// Number of coupling constraints "owned" by `owner_link` — i.e. couplings + /// whose first joint (`link1`) is that link. Each coupling is generated once, + /// by `link1` (which always has a free DoF and so is an active link in the + /// solver island, unlike a possibly-fixed root). + pub(crate) fn num_couplings_owned_by(&self, owner_link: usize) -> usize { + self.couplings.iter().filter(|c| c.link1 == owner_link).count() + } + + /// Generates the velocity constraints for the DoF couplings owned by + /// `owner_link`, writing them into `out[..]`. Each coupling + /// `q2 = coeff·q1 + offset` becomes a single bilateral constraint with the + /// generalized jacobian `J = e_{q2} − coeff·e_{q1}` and a right-hand side + /// that pulls the position drift `q2 − coeff·q1 − offset` back to zero. + pub(crate) fn coupling_velocity_constraints( + &self, + owner_link: usize, + params: &IntegrationParameters, + mut j_id: usize, + jacobians: &mut DVector, + out: &mut [GenericJointConstraint], + ) -> usize { + let ndofs = self.ndofs; + let erp_inv_dt = SpringCoefficients::::joint_defaults().erp_inv_dt(params.dt); + + let mut i = 0; + for c in self.couplings.iter().filter(|c| c.link1 == owner_link) { + let g1 = self.links[c.link1].assembly_id + c.dof1; + let g2 = self.links[c.link2].assembly_id + c.dof2; + let q1 = self.links[c.link1].joint().coords()[c.axis1]; + let q2 = self.links[c.link2].joint().coords()[c.axis2]; + + // Jacobian J = e_{g2} − coeff·e_{g1}, then WJ = M⁻¹ J. + jacobians.rows_mut(j_id, ndofs * 2).fill(0.0); + jacobians[j_id + g2] += 1.0; + jacobians[j_id + g1] -= c.coeff; + for k in 0..ndofs { + jacobians[j_id + ndofs + k] = jacobians[j_id + k]; + } + self.inv_augmented_mass + .solve_mut(&mut jacobians.rows_mut(j_id + ndofs, ndofs)); + + // lhs = Jᵀ M⁻¹ J = Σ J[k]·WJ[k]; only g1/g2 entries of J are nonzero. + let lhs = jacobians[j_id + ndofs + g2] - c.coeff * jacobians[j_id + ndofs + g1]; + + let drift = q2 - c.coeff * q1 - c.offset; + + out[i] = GenericJointConstraint { + is_rigid_body1: false, + solver_vel1: u32::MAX, + ndofs1: 0, + j_id1: 0, + is_rigid_body2: false, + solver_vel2: self.solver_id, + ndofs2: ndofs, + j_id2: j_id, + joint_id: usize::MAX, // internal: no impulse writeback. + impulse: 0.0, + impulse_bounds: [-Real::MAX, Real::MAX], + inv_lhs: crate::utils::inv(lhs), + rhs: drift * erp_inv_dt, + rhs_wo_bias: 0.0, + cfm_coeff: 0.0, + cfm_gain: 0.0, + writeback_id: WritebackId::Dof(0), + }; + + j_id += 2 * ndofs; + i += 1; + } + + i + } + /// The generalized velocity at the multibody_joint of the given link. #[inline] pub fn joint_velocity(&self, link: &MultibodyLink) -> DVectorView<'_, Real> { diff --git a/src/dynamics/solver/joint_constraint/generic_joint_constraint_builder.rs b/src/dynamics/solver/joint_constraint/generic_joint_constraint_builder.rs index ac6f0ca3f..d3de9f347 100644 --- a/src/dynamics/solver/joint_constraint/generic_joint_constraint_builder.rs +++ b/src/dynamics/solver/joint_constraint/generic_joint_constraint_builder.rs @@ -264,7 +264,9 @@ impl JointGenericInternalConstraintBuilder { pub fn num_constraints(multibodies: &MultibodyJointSet, link_id: &MultibodyLinkId) -> usize { let multibody = &multibodies[link_id.multibody]; let link = multibody.link(link_id.id).unwrap(); - link.joint().num_velocity_constraints() + // This link's own motor/limit constraints, plus the DoF couplings it + // owns (a coupling is owned by its first joint's link). + link.joint().num_velocity_constraints() + multibody.num_couplings_owned_by(link_id.id) } pub fn generate( @@ -277,7 +279,8 @@ impl JointGenericInternalConstraintBuilder { ) { let multibody = &multibodies[link_id.multibody]; let link = multibody.link(link_id.id).unwrap(); - let num_constraints = link.joint().num_velocity_constraints(); + let num_constraints = link.joint().num_velocity_constraints() + + multibody.num_couplings_owned_by(link_id.id); if num_constraints == 0 { return; @@ -306,7 +309,7 @@ impl JointGenericInternalConstraintBuilder { ) { let mb = &multibodies[self.link.multibody]; let link = mb.link(self.link.id).unwrap(); - link.joint().velocity_constraints( + let n_own = link.joint().velocity_constraints( params, mb, link, @@ -314,6 +317,19 @@ impl JointGenericInternalConstraintBuilder { jacobians, &mut out[self.constraint_id..], ); + // DoF couplings owned by this link follow its own constraints, both in + // the jacobian buffer (each constraint reserves `2·ndofs`) and in the + // output slice. + if mb.num_couplings_owned_by(self.link.id) > 0 { + let coupling_j_id = self.j_id + n_own * mb.ndofs() * 2; + mb.coupling_velocity_constraints( + self.link.id, + params, + coupling_j_id, + jacobians, + &mut out[self.constraint_id + n_own..], + ); + } } } diff --git a/src/dynamics/solver/velocity_solver.rs b/src/dynamics/solver/velocity_solver.rs index 31b7cc26d..d4b317027 100644 --- a/src/dynamics/solver/velocity_solver.rs +++ b/src/dynamics/solver/velocity_solver.rs @@ -133,7 +133,7 @@ impl VelocitySolver { .unwrap(); multibody.update_velocities(bodies); multibody.update_mass_matrix(params.dt, bodies); - multibody.update_acceleration(bodies); + multibody.update_acceleration(params.dt, bodies); let mut solver_vels_incr = self .generic_solver_vels_increment @@ -278,7 +278,7 @@ impl VelocitySolver { // have to run another step. multibody.update_velocities(bodies); multibody.update_mass_matrix(params.dt, bodies); - multibody.update_acceleration(bodies); + multibody.update_acceleration(params.dt, bodies); let mut solver_vels_incr = self .generic_solver_vels_increment From eca7b49f7de4664ba6f460381d5f91003ee240a2 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?S=C3=A9bastien=20Crozet?= Date: Thu, 25 Jun 2026 21:03:05 +0200 Subject: [PATCH 13/16] feat(mjcf): support tendons --- Cargo.toml | 4 +- crates/mjcf-rs/src/equality.rs | 19 + crates/mjcf-rs/src/lib.rs | 1 + crates/mjcf-rs/src/loader/attach.rs | 13 + crates/mjcf-rs/src/loader/blocks.rs | 69 +++- crates/mjcf-rs/src/loader/state.rs | 4 +- crates/mjcf-rs/src/model.rs | 2 + crates/mjcf-rs/src/tendon.rs | 25 ++ crates/rapier3d-mjcf/README.md | 36 +- crates/rapier3d-mjcf/src/loader/conversion.rs | 160 +++++++- crates/rapier3d-mjcf/src/loader/mod.rs | 4 +- crates/rapier3d-mjcf/src/loader/runtime.rs | 378 +++++++++++++++++- crates/rapier3d-mjcf/src/loader/types.rs | 81 ++++ examples3d/mujoco_menagerie3.rs | 205 +++++++++- .../joint/multibody_joint/multibody_link.rs | 12 + src_testbed/settings.rs | 30 ++ src_testbed/ui.rs | 56 ++- 17 files changed, 1038 insertions(+), 61 deletions(-) create mode 100644 crates/mjcf-rs/src/tendon.rs diff --git a/Cargo.toml b/Cargo.toml index 842855358..cced7839d 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -102,7 +102,7 @@ oorandom = { version = "11", default-features = false } #xurdf = { path = "../xurdf/xurdf" } #simba = { path = "../simba" } -kiss3d = { path = "../kiss3d" } +#kiss3d = { path = "../kiss3d" } #parry2d = { path = "../parry/crates/parry2d" } #parry3d = { path = "../parry/crates/parry3d" } #parry2d-f64 = { path = "../parry/crates/parry2d-f64" } @@ -112,7 +112,7 @@ kiss3d = { path = "../kiss3d" } #wide = { path = "../wide" } #glamx = { path = "../glamx" } -#kiss3d = { git = "https://github.com/sebcrozet/kiss3d" } +kiss3d = { git = "https://github.com/sebcrozet/kiss3d", branch = "render-2d" } #nalgebra = { git = "https://github.com/dimforge/nalgebra", branch = "dev" } #parry2d = { git = "https://github.com/dimforge/parry", branch = "master" } #parry3d = { git = "https://github.com/dimforge/parry", branch = "master" } diff --git a/crates/mjcf-rs/src/equality.rs b/crates/mjcf-rs/src/equality.rs index 032b30e2b..d9f0b7572 100644 --- a/crates/mjcf-rs/src/equality.rs +++ b/crates/mjcf-rs/src/equality.rs @@ -9,6 +9,9 @@ pub enum Equality { Weld(EqualityWeld), /// `` — polynomial coupling between two joints' positions. Joint(EqualityJoint), + /// `` — polynomial coupling between two tendon lengths, + /// or (with `tendon2` omitted) a fixed tendon pinned to a constant length. + Tendon(EqualityTendon), } /// Common metadata shared by all equality variants. @@ -69,3 +72,19 @@ pub struct EqualityJoint { /// Polynomial coefficients `[p0, p1, p2, p3, p4]` (default `[0,1,0,0,0]`). pub polycoef: [f64; 5], } + +/// `` element: couples the length of `tendon2` to that of +/// `tendon1` through the same quartic polynomial form as [`EqualityJoint`]. +/// `tendon2` may be omitted, in which case `tendon1`'s length is constrained to +/// the constant `polycoef[0]`. +#[derive(Clone, Debug, Default)] +pub struct EqualityTendon { + /// Common metadata. + pub common: EqualityCommon, + /// First (independent) tendon name. + pub tendon1: String, + /// Second (dependent) tendon name. `None` constrains `tendon1` to `polycoef[0]`. + pub tendon2: Option, + /// Polynomial coefficients `[p0, p1, p2, p3, p4]` (default `[0,1,0,0,0]`). + pub polycoef: [f64; 5], +} diff --git a/crates/mjcf-rs/src/lib.rs b/crates/mjcf-rs/src/lib.rs index 61788b792..124192f57 100644 --- a/crates/mjcf-rs/src/lib.rs +++ b/crates/mjcf-rs/src/lib.rs @@ -46,6 +46,7 @@ pub mod extras; mod loader; pub mod model; pub mod normals; +pub mod tendon; pub mod types; #[cfg(feature = "msh")] diff --git a/crates/mjcf-rs/src/loader/attach.rs b/crates/mjcf-rs/src/loader/attach.rs index 3b0a7af28..4652eaf81 100644 --- a/crates/mjcf-rs/src/loader/attach.rs +++ b/crates/mjcf-rs/src/loader/attach.rs @@ -184,6 +184,19 @@ impl ParseState { .equality .push(crate::equality::Equality::Joint(new_j)); } + crate::equality::Equality::Tendon(t) => { + let mut new_t = t.clone(); + if let Some(n) = new_t.common.name.take() { + new_t.common.name = Some(prefixed(prefix, &n)); + } + new_t.tendon1 = prefixed(prefix, &new_t.tendon1); + if let Some(t2) = new_t.tendon2.take() { + new_t.tendon2 = Some(prefixed(prefix, &t2)); + } + self.model + .equality + .push(crate::equality::Equality::Tendon(new_t)); + } } } } diff --git a/crates/mjcf-rs/src/loader/blocks.rs b/crates/mjcf-rs/src/loader/blocks.rs index 185fdb759..4b7c17ddd 100644 --- a/crates/mjcf-rs/src/loader/blocks.rs +++ b/crates/mjcf-rs/src/loader/blocks.rs @@ -6,9 +6,12 @@ use roxmltree::Node; use crate::Pose; use crate::contact::{ContactExclude, ContactPair}; -use crate::equality::{Equality, EqualityCommon, EqualityConnect, EqualityJoint, EqualityWeld}; +use crate::equality::{ + Equality, EqualityCommon, EqualityConnect, EqualityJoint, EqualityTendon, EqualityWeld, +}; use crate::error::ParseError; use crate::extras::{Actuator, ActuatorKind, Keyframe, Sensor}; +use crate::tendon::{FixedTendon, TendonJoint}; use crate::types::Tristate; use glamx::glam::{DQuat, DVec3}; @@ -154,7 +157,30 @@ impl ParseState { } self.model.equality.push(Equality::Joint(ej)); } - "tendon" | "flex" | "flexvert" | "flexstrain" => { + "tendon" => { + let mut et = EqualityTendon { + common, + polycoef: [0.0, 1.0, 0.0, 0.0, 0.0], + ..Default::default() + }; + for attr in child.attributes() { + match attr.name() { + "tendon1" => et.tendon1 = attr.value().to_string(), + "tendon2" => et.tendon2 = Some(attr.value().to_string()), + "polycoef" => { + let v = parse_f64_list(attr.value())?; + for (i, slot) in et.polycoef.iter_mut().enumerate() { + if let Some(c) = v.get(i) { + *slot = *c; + } + } + } + _ => {} + } + } + self.model.equality.push(Equality::Tendon(et)); + } + "flex" | "flexvert" | "flexstrain" => { log::debug!("equality `{tag}` is out of scope; skipped"); } "include" => self.parse_include(child, true)?, @@ -303,4 +329,43 @@ impl ParseState { } Ok(()) } + + /// Parse a top-level `` block. Only `` tendons are kept; + /// `` tendons (site-routed cables) are out of scope and skipped. + pub(super) fn parse_tendon(&mut self, node: Node) -> Result<(), ParseError> { + for child in node.children().filter(|n| n.is_element()) { + match child.tag_name().name() { + "include" => self.parse_include(child, true)?, + "spatial" => { + log::debug!(" is out of scope; skipped"); + } + "fixed" => { + let mut t = FixedTendon { + name: child.attribute("name").map(|s| s.to_string()), + class: child.attribute("class").map(|s| s.to_string()), + joints: Vec::new(), + }; + for term in child.children().filter(|n| n.is_element()) { + if term.tag_name().name() != "joint" { + continue; + } + let Some(joint) = term.attribute("joint") else { + continue; + }; + let coef = match term.attribute("coef") { + Some(c) => parse_f64(c)?, + None => 1.0, + }; + t.joints.push(TendonJoint { + joint: joint.to_string(), + coef, + }); + } + self.model.tendons.push(t); + } + _ => {} + } + } + Ok(()) + } } diff --git a/crates/mjcf-rs/src/loader/state.rs b/crates/mjcf-rs/src/loader/state.rs index ee799e112..e778d51c9 100644 --- a/crates/mjcf-rs/src/loader/state.rs +++ b/crates/mjcf-rs/src/loader/state.rs @@ -110,9 +110,7 @@ impl ParseState { "worldbody" => deferred_bodies.push(child), "contact" => self.parse_contact(child)?, "equality" => self.parse_equality(child)?, - "tendon" => { - // Out of scope; record nothing. - } + "tendon" => self.parse_tendon(child)?, "actuator" => deferred_actuators.push(child), "sensor" => deferred_sensors.push(child), "keyframe" => deferred_keyframes.push(child), diff --git a/crates/mjcf-rs/src/model.rs b/crates/mjcf-rs/src/model.rs index 706099740..a164fea7b 100644 --- a/crates/mjcf-rs/src/model.rs +++ b/crates/mjcf-rs/src/model.rs @@ -40,6 +40,8 @@ pub struct Model { pub sensors: Vec, /// `` entries. pub keyframes: Vec, + /// `` elements (spatial tendons are not represented). + pub tendons: Vec, } /// One entry in the flat body list. diff --git a/crates/mjcf-rs/src/tendon.rs b/crates/mjcf-rs/src/tendon.rs new file mode 100644 index 000000000..1842414e4 --- /dev/null +++ b/crates/mjcf-rs/src/tendon.rs @@ -0,0 +1,25 @@ +//! `` element AST. Only **fixed** tendons are represented — a fixed +//! tendon is a linear combination of joint coordinates, `L = Σ coefᵢ·qᵢ`, used +//! for actuator transmission and (via ``) length coupling. +//! Spatial tendons (site-routed cables with wrapping) are out of scope. + +/// One `(joint, coefficient)` term of a [`FixedTendon`]. +#[derive(Clone, Debug, Default)] +pub struct TendonJoint { + /// Referenced joint name. + pub joint: String, + /// Coefficient of this joint in the tendon length (`coef`, default 1). + pub coef: f64, +} + +/// A `` element: the scalar length `Σ coefᵢ·qᵢ` over its joints. +#[derive(Clone, Debug, Default)] +pub struct FixedTendon { + /// `name` attribute. + pub name: Option, + /// `` class. + pub class: Option, + /// The `(joint, coef)` terms, in document order. The first term's joint is + /// treated as the tendon's "primary" coordinate by the loader. + pub joints: Vec, +} diff --git a/crates/rapier3d-mjcf/README.md b/crates/rapier3d-mjcf/README.md index 3da17d864..e9737be6d 100644 --- a/crates/rapier3d-mjcf/README.md +++ b/crates/rapier3d-mjcf/README.md @@ -146,24 +146,46 @@ convention. (Tracked for a future polish pass.) `MjcfRobotHandles::apply_controls(impulse_joints, ctrl)` drives the rapier joint motors for ``, ``, ``, and `` actuator types. `` and `` are - recorded but left to the user to wire up. -- ✅ `` mocap state — `apply_mocap_keyframe` writes - `mpos`/`mquat` into `KinematicPositionBased` rapier bodies. - Per-joint `qpos` / `qvel` keyframe state is preserved on the AST but - not auto-applied (qpos ordering across multibody chains needs more - bookkeeping; tracked for a future polish pass). + recorded but left to the user to wire up. The `*_scaled` variants + (`apply_controls_scaled` / `apply_controls_multibody_scaled`) take a + `gain_scale` that uniformly softens (`< 1`) or stiffens (`> 1`) every + actuator's gains and force limits — handy to ease a servo-driven move + that would otherwise saturate and snap. +- ✅ `` state — `MjcfRobotHandles::apply_keyframe` applies a + keyframe's full state: `qpos` / `qvel` joint coordinates (using + `MjcfRobot::qpos_dofs`, a precomputed map from MuJoCo's + generalized-coordinate order onto the rapier joints/bodies), a + floating base's world pose, and `mpos` / `mquat` mocap state. It works + on both insertion paths — the multibody path writes the multibody's + generalized coordinates (then runs forward-kinematics), the + impulse-joint path runs forward-kinematics over the joint tree and + writes each body's world pose. `apply_mocap_keyframe` (mocap only) and + `MjcfRobot::keyframe_by_name` remain available. (Per-joint `qvel` is + applied on the multibody path; the impulse path applies only a floating + base's `qvel`.) - ✅ `` reading via `MjcfRobot::read_sensor` for the state-derivable subset: `framepos`, `framequat`, `framelinvel`, `frameangvel`, `velocimeter`, `gyro`, `subtreemass`, `subtreecom`, `clock`. Sensors that need contact-area integration (`touch`, `rangefinder`, `geomdist`, `camprojection`) are out of scope. +- ✅ `` (linear joint-coordinate tendons). An actuator with + `tendon="…"` is bound to the tendon's first joint, and the tendon's other + joints are coupled to it (`q_k = (coef_k/coef_0)·q_0`, a multibody DoF + coupling — exact for equal-inertia joints, which covers every menagerie + fixed tendon) so they move as one. This is what makes e.g. the shadow hand's + tendon-driven middle+distal finger segments curl together. Pairs already + coupled by an explicit `` (franka, robotiq) are left alone. + `` (site-routed cables with wrapping) and `` + length constraints remain out of scope. ### Out of scope ❌ - `` / MuJoCo plugins. - `` / `` / `` (deformables). - `` (procedural body clusters). -- `` (any kind) and `` of tendons / joints / flex. +- `` and `` of tendons / flex (fixed-tendon joint + coupling and actuation *are* supported — see above). +- Fixed-tendon passive `stiffness`/`damping`/`range`/`frictionloss`. - `` types: `cylinder`, `muscle`, `adhesion`. - `` types that need contact-area integration: `touch`, `rangefinder`, `geomdist`, `camprojection`. diff --git a/crates/rapier3d-mjcf/src/loader/conversion.rs b/crates/rapier3d-mjcf/src/loader/conversion.rs index 3baa632b1..c84b54dd4 100644 --- a/crates/rapier3d-mjcf/src/loader/conversion.rs +++ b/crates/rapier3d-mjcf/src/loader/conversion.rs @@ -24,8 +24,8 @@ use rapier3d::math::{Pose, Real, Vector}; use super::mass::scale_mass_properties; use super::options::MjcfLoaderOptions; use super::types::{ - MjcfActuatorBinding, MjcfBody, MjcfEqualityJoint, MjcfJoint, MjcfJointCoupling, MjcfRobot, - MjcfSensorBinding, MjcfVisualMesh, SensorObjectRef, + MjcfActuatorBinding, MjcfBody, MjcfDofKind, MjcfEqualityJoint, MjcfJoint, MjcfJointCoupling, + MjcfQposDof, MjcfRobot, MjcfSensorBinding, MjcfVisualMesh, SensorObjectRef, }; pub(super) struct Conversion<'a> { @@ -145,11 +145,19 @@ impl<'a> Conversion<'a> { // body indices. self.materialize_equality(); + // Fixed tendons: co-actuation couplings between their joints. After + // `materialize_equality` so we can skip pairs already coupled by an + // explicit ``. + self.materialize_tendons(); + // Bindings for actuators / sensors. self.bind_extras(); - // Keyframes pass-through. + // Keyframes pass-through. The qpos/qvel layout was filled in lock-step + // with body/joint creation (`materialize_body`), so it already follows + // MuJoCo's generalized-coordinate order. self.robot.keyframes = self.model.keyframes.clone(); + self.robot.base_shift = self.options.shift; // Contact data pass-through (resolved into hooks at insert time). self.robot.contact_excludes = self.model.contact.excludes.clone(); @@ -211,7 +219,16 @@ impl<'a> Conversion<'a> { ); } // Free body — no rapier joint, body is dynamic and not constrained. - let _ = self.push_body_for_mjcf(mjcf_id, entry, body_world_pose, false); + let body_idx = self.push_body_for_mjcf(mjcf_id, entry, body_world_pose, false); + // A `` contributes a 6-DoF slot to the keyframe layout: + // its 7 qpos give the floating base's absolute world pose. + self.robot.qpos_dofs.push(MjcfQposDof { + kind: MjcfDofKind::Free, + joint: None, + body: body_idx, + reference: 0.0, + scale: self.options.scale, + }); return; } @@ -328,6 +345,24 @@ impl<'a> Conversion<'a> { spring_ref, springdamper, }); + // Record this joint's slot in the keyframe `qpos`/`qvel` layout + // (Free is filtered out above; the synthesized fixed joints created + // for jointless bodies are 0-DoF and likewise contribute nothing). + let dof_kind = match j.type_ { + mb::JointType::Hinge => Some(MjcfDofKind::Hinge), + mb::JointType::Slide => Some(MjcfDofKind::Slide), + mb::JointType::Ball => Some(MjcfDofKind::Ball), + mb::JointType::Free => None, + }; + if let Some(kind) = dof_kind { + self.robot.qpos_dofs.push(MjcfQposDof { + kind, + joint: Some(self.robot.joints.len() - 1), + body: cur_rapier, + reference: j.ref_ as Real, + scale: self.options.scale, + }); + } prev_rapier = cur_rapier; prev_world_pose = cur_world_pose; } @@ -565,6 +600,17 @@ impl<'a> Conversion<'a> { } } MjcfEquality::Joint(j) => self.materialize_joint_equality(j), + MjcfEquality::Tendon(t) => { + // `` pins a tendon length (or couples two + // tendons). No menagerie model uses it — fixed-tendon joint + // coupling there is expressed with `` + // instead — so it's not yet materialized. + log::warn!( + ": tendon length equality is not yet \ + supported; skipping", + t.common.name + ); + } } } } @@ -611,6 +657,78 @@ impl<'a> Conversion<'a> { }); } + /// Resolve a fixed tendon's `(joint, coef)` terms to `(rapier-joint-index, + /// coef)`, dropping (with a warning) joints rapier doesn't have a 1-DoF + /// joint for. + fn resolve_tendon_joints(&self, t: &mjcf_rs::tendon::FixedTendon) -> Vec<(usize, Real)> { + t.joints + .iter() + .filter_map(|term| { + match self.robot.joint_name_to_idx.get(&term.joint).copied() { + Some(idx) => Some((idx, term.coef as Real)), + None => { + log::warn!( + ": unknown joint \"{}\"; ignoring this term", + t.name, + term.joint, + ); + None + } + } + }) + .collect() + } + + /// The rapier-joint index a `tendon=` actuator should drive: the first + /// resolvable joint of the named fixed tendon (its other joints are dragged + /// along by the co-actuation couplings from [`Self::materialize_tendons`]). + fn tendon_primary_joint(&self, tendon_name: &str) -> Option { + let t = self + .model + .tendons + .iter() + .find(|t| t.name.as_deref() == Some(tendon_name))?; + self.resolve_tendon_joints(t).first().map(|&(idx, _)| idx) + } + + /// A fixed tendon couples its joints only through being actuated/loaded as + /// one unit (MuJoCo applies the actuator/passive force `coefᵢ·f` to each + /// joint). Rapier has no tendon transmission, so we approximate this by + /// coupling each joint `k ≥ 1` to the first joint: `q_k = (coef_k/coef_0)·q_0`. + /// That is exact when the joints have equal inertia (the case for every + /// menagerie fixed tendon, whose coefficients are equal), and makes e.g. + /// the shadow hand's middle+distal finger segments curl together. + /// + /// Pairs already coupled by an explicit `` (franka, + /// robotiq) are left alone, and single-joint tendons need no coupling. + fn materialize_tendons(&mut self) { + for t in &self.model.tendons { + let joints = self.resolve_tendon_joints(t); + let Some(&(idx0, c0)) = joints.first() else { + continue; + }; + if c0 == 0.0 { + continue; + } + for &(idxk, ck) in &joints[1..] { + let already_coupled = self.robot.joint_couplings.iter().any(|c| { + (c.joint1 == idx0 && c.joint2 == idxk) + || (c.joint1 == idxk && c.joint2 == idx0) + }); + if already_coupled { + continue; + } + self.robot.joint_couplings.push(MjcfJointCoupling { + joint1: idx0, + joint2: idxk, + coeff: ck / c0, + offset: 0.0, + active: true, + }); + } + } + } + fn resolve_connect_frames(&self, c: &EqualityConnect) -> Option<(usize, usize, Pose, Pose)> { let Some(body1) = self.robot.body_name_to_idx.get(&c.body1).copied() else { log::warn!( @@ -721,16 +839,32 @@ impl<'a> Conversion<'a> { } }, None => { - if a.tendon.is_some() { - // Tendons are out of scope; not actionable. No warning. - } else if a.body.is_none() && a.site.is_none() { - log::warn!( - ": no `joint=`, `tendon=`, `body=`, or `site=` \ - reference; actuator will be recorded but apply_controls() will skip it", - a.name, - ); + if let Some(tendon_name) = a.tendon.as_deref() { + // Drive the fixed tendon through its primary joint; the + // other joints follow via the co-actuation couplings + // installed by `materialize_tendons`. (Spatial tendons + // resolve to no joint and stay unbound.) + match self.tendon_primary_joint(tendon_name) { + Some(idx) => Some(idx), + None => { + log::warn!( + ": unknown or \ + spatial tendon (no usable joint); apply_controls() will skip it", + a.name, + ); + None + } + } + } else { + if a.body.is_none() && a.site.is_none() { + log::warn!( + ": no `joint=`, `tendon=`, `body=`, or `site=` \ + reference; actuator will be recorded but apply_controls() will skip it", + a.name, + ); + } + None } - None } }; self.robot.actuators.push(MjcfActuatorBinding { diff --git a/crates/rapier3d-mjcf/src/loader/mod.rs b/crates/rapier3d-mjcf/src/loader/mod.rs index c3c337f89..ac0aba1b6 100644 --- a/crates/rapier3d-mjcf/src/loader/mod.rs +++ b/crates/rapier3d-mjcf/src/loader/mod.rs @@ -22,6 +22,6 @@ pub use handles::{ pub use options::{ContactFilterMode, MjcfLoaderOptions, MjcfMultibodyOptions}; pub use runtime::MjcfSensorValue; pub use types::{ - MjcfActuatorBinding, MjcfBody, MjcfEqualityJoint, MjcfJoint, MjcfRenderMaterial, MjcfRobot, - MjcfSensorBinding, MjcfVisualMesh, SensorObjectRef, + MjcfActuatorBinding, MjcfBody, MjcfDofKind, MjcfEqualityJoint, MjcfJoint, MjcfQposDof, + MjcfRenderMaterial, MjcfRobot, MjcfSensorBinding, MjcfVisualMesh, SensorObjectRef, }; diff --git a/crates/rapier3d-mjcf/src/loader/runtime.rs b/crates/rapier3d-mjcf/src/loader/runtime.rs index 81062f6f2..d8ef2f4ed 100644 --- a/crates/rapier3d-mjcf/src/loader/runtime.rs +++ b/crates/rapier3d-mjcf/src/loader/runtime.rs @@ -5,15 +5,15 @@ use mjcf_rs::extras::Keyframe; use rapier3d::dynamics::{ - GenericJoint, ImpulseJointHandle, ImpulseJointSet, JointAxis, MultibodyJointHandle, - MultibodyJointSet, RigidBody, RigidBodySet, RigidBodyType, + GenericJoint, ImpulseJointHandle, ImpulseJointSet, JointAxis, MultibodyIndex, + MultibodyJointHandle, MultibodyJointSet, RigidBody, RigidBodySet, RigidBodyType, }; use rapier3d::math::{Pose, Real, Rotation, Vector}; use crate::hooks::{MjcfContactHooks, PairOverride}; use super::handles::MjcfRobotHandles; -use super::types::{MjcfRobot, SensorObjectRef}; +use super::types::{MjcfDofKind, MjcfQposDof, MjcfRobot, SensorObjectRef}; impl MjcfRobotHandles { /// Apply per-body gravity compensation as an explicit per-step force. @@ -189,6 +189,241 @@ impl MjcfRobotHandles { } } +/// The world pose encoded by a free joint's 7 `qpos` (`x y z` + `w x y z` +/// quaternion, MuJoCo's wxyz order), with the loader's length `scale` applied +/// to the translation and the loader's `shift` pre-multiplied so the floating +/// base lands in the same frame as the rest of the model. +fn free_pose(qp: &[f64], scale: Real, shift: Pose) -> Pose { + let translation = Vector::new(qp[0] as Real, qp[1] as Real, qp[2] as Real) * scale; + let rotation = Rotation::from_xyzw(qp[4] as Real, qp[5] as Real, qp[6] as Real, qp[3] as Real); + shift * Pose::from_parts(translation, rotation) +} + +/// A hinge/slide DoF's target rapier joint coordinate from its single `qpos` +/// scalar. MuJoCo stores the absolute coordinate, so we subtract `` +/// (and scale lengths) to reach rapier's frame-relative convention — the same +/// transform the serial-joint builder applies to the joint limits. +fn scalar_coord(dof: &MjcfQposDof, qp: &[f64]) -> Real { + match dof.kind { + MjcfDofKind::Hinge => qp[0] as Real - dof.reference, + MjcfDofKind::Slide => (qp[0] as Real - dof.reference) * dof.scale, + _ => 0.0, + } +} + +/// The target relative rotation of a ball DoF from its 4 `qpos` (wxyz). +fn ball_rotation(qp: &[f64]) -> Rotation { + Rotation::from_xyzw(qp[1] as Real, qp[2] as Real, qp[3] as Real, qp[0] as Real) +} + +impl MjcfRobotHandles> { + /// Apply a keyframe's full state (`qpos`, `qvel`, and `mpos`/`mquat`) to a + /// robot inserted through [`insert_using_multibody_joints`](MjcfRobot::insert_using_multibody_joints). + /// + /// Joint coordinates are written to the multibody's generalized + /// coordinates (then propagated through forward-kinematics so the rapier + /// rigid-bodies match), and a floating base's `qpos` sets its root body's + /// world pose. Call this once after insertion to start the model in a + /// declared keyframe (e.g. MuJoCo's `home`). + /// + /// `qvel` is written to the generalized velocities of articulated joints + /// and as the world-frame linear/angular velocity of a floating base. + /// Mocap bodies are handled exactly as [`apply_mocap_keyframe`](Self::apply_mocap_keyframe). + pub fn apply_keyframe( + &self, + bodies: &mut RigidBodySet, + multibody_joints: &mut MultibodyJointSet, + robot: &MjcfRobot, + key: &Keyframe, + ) { + self.apply_mocap_keyframe(bodies, robot, key); + + // Multibodies whose root pose / joint coordinates changed: re-run + // forward-kinematics on each once at the end. + let mut touched: Vec = Vec::new(); + let touch = |touched: &mut Vec, idx: MultibodyIndex| { + if !touched.contains(&idx) { + touched.push(idx); + } + }; + + let mut qpos_i = 0; + let mut qvel_i = 0; + for dof in &robot.qpos_dofs { + let qpos = key.qpos.get(qpos_i..qpos_i + dof.kind.qpos_width()); + let qvel = key.qvel.get(qvel_i..qvel_i + dof.kind.qvel_width()); + qpos_i += dof.kind.qpos_width(); + qvel_i += dof.kind.qvel_width(); + + if dof.kind == MjcfDofKind::Free { + let Some(handle) = self.bodies.get(dof.body).and_then(|b| b.as_ref()) else { + continue; + }; + if let Some(rb) = bodies.get_mut(handle.body) { + if let Some(qp) = qpos { + rb.set_position(free_pose(qp, dof.scale, robot.base_shift), true); + } + if let Some(qv) = qvel { + let lin = Vector::new(qv[0] as Real, qv[1] as Real, qv[2] as Real) * dof.scale; + let ang = Vector::new(qv[3] as Real, qv[4] as Real, qv[5] as Real); + rb.set_linvel(lin, true); + rb.set_angvel(ang, true); + } + } + if let Some(link) = multibody_joints.rigid_body_link(handle.body) { + touch(&mut touched, link.multibody); + } + continue; + } + + // Articulated DoF (hinge / slide / ball): write to the multibody's + // generalized coordinates. + let Some(jidx) = dof.joint else { continue }; + let Some(jh) = self.joints.get(jidx) else { continue }; + // `joint` is `None` for a joint that was dropped as a loop closure. + let Some(handle) = jh.joint else { continue }; + if let Some(link) = multibody_joints.rigid_body_link(jh.link2) { + touch(&mut touched, link.multibody); + } + let Some((mb, link_id)) = multibody_joints.get_mut(handle) else { + continue; + }; + + // Compute the displacement (target − current) under an immutable + // borrow, then apply it under a mutable one. + let (assembly_id, ndofs, disp) = { + let Some(link) = mb.link(link_id) else { continue }; + let joint = link.joint(); + let coords = joint.coords(); + let disp = qpos.map(|qp| match dof.kind { + MjcfDofKind::Hinge => vec![scalar_coord(dof, qp) - coords[3]], + MjcfDofKind::Slide => vec![scalar_coord(dof, qp) - coords[0]], + MjcfDofKind::Ball => { + // Reach the target relative rotation from the current + // one: `from_scaled_axis(disp) * joint_rot == target`. + let delta = ball_rotation(qp) * joint.joint_rot().inverse(); + let sa = delta.to_scaled_axis(); + vec![sa.x, sa.y, sa.z] + } + MjcfDofKind::Free => vec![], + }); + (link.assembly_id(), joint.ndofs(), disp) + }; + if let Some(disp) = disp + && let Some(link) = mb.link_mut(link_id) + { + link.joint.apply_displacement(&disp); + } + if let Some(qv) = qvel { + let scale = if dof.kind == MjcfDofKind::Slide { + dof.scale + } else { + 1.0 + }; + let mut vels = mb.generalized_velocity_mut(); + for k in 0..ndofs.min(qv.len()) { + vels[assembly_id + k] = qv[k] as Real * scale; + } + } + } + + for idx in touched { + if let Some(mb) = multibody_joints.get_multibody_mut(idx) { + mb.forward_kinematics(bodies, true); + mb.update_rigid_bodies(bodies, false); + } + } + } +} + +impl MjcfRobotHandles { + /// Apply a keyframe's `qpos` (and a floating base's `qvel`) to a robot + /// inserted through [`insert_using_impulse_joints`](MjcfRobot::insert_using_impulse_joints). + /// + /// In the impulse-joint path every body carries a global pose, so the + /// keyframe is realized by running forward-kinematics over the joint tree + /// (anchored at the unchanged fixed/welded roots, or at a floating base set + /// from its `qpos`) and writing each body's resulting world pose. The joint + /// constraints are already satisfied by those poses. + /// + /// Only a floating base's `qvel` is applied (as world-frame body velocity); + /// per-joint `qvel` has no generalized-coordinate home in this path and is + /// ignored. Mocap bodies are handled as [`apply_mocap_keyframe`](Self::apply_mocap_keyframe). + pub fn apply_keyframe(&self, bodies: &mut RigidBodySet, robot: &MjcfRobot, key: &Keyframe) { + self.apply_mocap_keyframe(bodies, robot, key); + + // Per-joint relative transform from the keyframe; identity where a + // joint carries no qpos (synthesized welds keep the bodies at rest). + let mut joint_tf: Vec = vec![Pose::default(); robot.joints.len()]; + // Forward-kinematics world poses, seeded with the load-time poses so + // fixed/welded roots and the world body keep their place. + let mut world: Vec = robot.bodies.iter().map(|b| *b.body.position()).collect(); + // Floating-base velocities, applied after the pose write-back. + let mut free_vels: Vec<(usize, Vector, Vector)> = Vec::new(); + + let mut qpos_i = 0; + let mut qvel_i = 0; + for dof in &robot.qpos_dofs { + let qpos = key.qpos.get(qpos_i..qpos_i + dof.kind.qpos_width()); + let qvel = key.qvel.get(qvel_i..qvel_i + dof.kind.qvel_width()); + qpos_i += dof.kind.qpos_width(); + qvel_i += dof.kind.qvel_width(); + + match dof.kind { + MjcfDofKind::Free => { + if let Some(qp) = qpos { + world[dof.body] = free_pose(qp, dof.scale, robot.base_shift); + } + if let Some(qv) = qvel { + let lin = Vector::new(qv[0] as Real, qv[1] as Real, qv[2] as Real) * dof.scale; + let ang = Vector::new(qv[3] as Real, qv[4] as Real, qv[5] as Real); + free_vels.push((dof.body, lin, ang)); + } + } + MjcfDofKind::Hinge | MjcfDofKind::Slide => { + if let (Some(jidx), Some(qp)) = (dof.joint, qpos) { + let q = scalar_coord(dof, qp); + joint_tf[jidx] = if dof.kind == MjcfDofKind::Hinge { + Pose::from_rotation(Rotation::from_axis_angle(Vector::X, q)) + } else { + Pose::from_translation(Vector::X * q) + }; + } + } + MjcfDofKind::Ball => { + if let (Some(jidx), Some(qp)) = (dof.joint, qpos) { + joint_tf[jidx] = Pose::from_rotation(ball_rotation(qp)); + } + } + } + } + + // Forward-kinematics: `robot.joints` is in topological order, so each + // joint's parent body is already positioned. This mirrors rapier's + // multibody `body_to_parent`: world₂ = world₁ · frame1 · q · frame2⁻¹. + for (jidx, j) in robot.joints.iter().enumerate() { + world[j.link2] = + world[j.link1] * j.joint.local_frame1 * joint_tf[jidx] * j.joint.local_frame2.inverse(); + } + + for (i, handle) in self.bodies.iter().enumerate() { + if let Some(handle) = handle + && let Some(rb) = bodies.get_mut(handle.body) + { + rb.set_position(world[i], true); + } + } + for (body, lin, ang) in free_vels { + if let Some(handle) = self.bodies.get(body).and_then(|b| b.as_ref()) + && let Some(rb) = bodies.get_mut(handle.body) + { + rb.set_linvel(lin, true); + rb.set_angvel(ang, true); + } + } + } +} + impl MjcfRobotHandles { /// Apply per-actuator control values to the impulse joints they drive. /// @@ -205,13 +440,26 @@ impl MjcfRobotHandles { /// - everything else (`general`, `intvelocity`, …) is left to the user /// — the metadata is preserved on `Self::actuators`. pub fn apply_controls(&self, joints: &mut ImpulseJointSet, ctrl: &[Real]) { + self.apply_controls_scaled(joints, ctrl, 1.0); + } + + /// Like [`apply_controls`](Self::apply_controls) but uniformly scales every + /// actuator's strength by `gain_scale` (gains and force limits; see + /// [`configure_actuator_motor`]). `gain_scale < 1` softens the actuation — + /// e.g. to ease servo-driven moves that would otherwise saturate and snap. + pub fn apply_controls_scaled( + &self, + joints: &mut ImpulseJointSet, + ctrl: &[Real], + gain_scale: Real, + ) { for (i, ah) in self.actuators.iter().enumerate() { let Some(handle) = ah.joint else { continue }; let Some(joint) = joints.get_mut(handle, true) else { continue; }; let u = ctrl.get(i).copied().unwrap_or(0.0); - configure_actuator_motor(&mut joint.data, &ah.actuator, u); + configure_actuator_motor(&mut joint.data, &ah.actuator, u, gain_scale); } } } @@ -239,6 +487,21 @@ impl MjcfRobotHandles> { bodies: &mut RigidBodySet, multibody_joints: &mut MultibodyJointSet, ctrl: &[Real], + ) { + self.apply_controls_multibody_scaled(bodies, multibody_joints, ctrl, 1.0); + } + + /// Like [`apply_controls_multibody`](Self::apply_controls_multibody) but + /// uniformly scales every actuator's strength by `gain_scale` (gains and + /// force limits; see [`configure_actuator_motor`]). `gain_scale < 1` softens + /// the actuation, so a servo-driven move (e.g. easing between keyframes) + /// ramps in instead of saturating to its force limit and arriving instantly. + pub fn apply_controls_multibody_scaled( + &self, + bodies: &mut RigidBodySet, + multibody_joints: &mut MultibodyJointSet, + ctrl: &[Real], + gain_scale: Real, ) { for (i, ah) in self.actuators.iter().enumerate() { // `H` is `Option` here, so `ah.joint` is @@ -252,7 +515,7 @@ impl MjcfRobotHandles> { continue; }; let u = ctrl.get(i).copied().unwrap_or(0.0); - configure_actuator_motor(&mut link.joint.data, &ah.actuator, u); + configure_actuator_motor(&mut link.joint.data, &ah.actuator, u, gain_scale); let rb = link.rigid_body_handle(); if let Some(body) = bodies.get_mut(rb) { body.wake_up(true); @@ -287,18 +550,38 @@ fn configure_actuator_motor( data: &mut GenericJoint, actuator: &mjcf_rs::extras::Actuator, u: Real, + gain_scale: Real, ) { use mjcf_rs::extras::ActuatorKind; let ax = JointAxis::AngX; let lin_ax = JointAxis::LinX; - let kp = actuator.kp.unwrap_or_default() as Real; - let kv = actuator.kv.unwrap_or_default() as Real; + // MuJoCo's `` defaults `kp` to 1 when it isn't set (directly or + // through a `` class), not 0. Defaulting to 0 here would give a + // zero-gain — i.e. completely limp — servo, which is what made e.g. the + // shadow hand's many `kp`-less finger actuators unable to hold/move their + // joints. `kv` (velocity gain) does default to 0 in MuJoCo. + // + // `gain_scale` uniformly scales the actuator's strength: the servo gains + // (`kp`/`kv`, or an affine ``'s stiffness/damping — target pose + // unchanged) and the force cap (`forcerange`) and constant forces. Below 1 + // the actuator pushes more softly, so a servo-driven move (e.g. snapping + // between keyframes) eases in instead of saturating to its force limit and + // arriving almost instantly. `1.0` reproduces the model as authored. + let kp = actuator.kp.unwrap_or(1.0) as Real * gain_scale; + let kv = actuator.kv.unwrap_or_default() as Real * gain_scale; let gear = actuator.gear[0] as Real; - let force_max = actuator + let base_force_max = actuator .force_range .map(|r| r[1].abs() as Real) .unwrap_or(Real::INFINITY); + // Scale the force cap too (an unbounded `INFINITY` cap stays unbounded — + // and avoids `INFINITY * 0` = `NaN` for a zero scale). + let force_max = if base_force_max.is_finite() { + base_force_max * gain_scale + } else { + base_force_max + }; // Emulate a constant generalized force: aim the motor at a far-away // velocity in the force's direction and cap the impulse at the force @@ -315,7 +598,7 @@ fn configure_actuator_motor( }; match actuator.kind { - ActuatorKind::Motor => apply_constant_force(data, u * gear), + ActuatorKind::Motor => apply_constant_force(data, u * gear * gain_scale), ActuatorKind::Position => { data.set_motor_position(ax, u, kp, kv); data.set_motor_position(lin_ax, u, kp, kv); @@ -336,7 +619,8 @@ fn configure_actuator_motor( data.set_motor_max_force(lin_ax, force_max); } ActuatorKind::Damper => { - let damp = actuator.gainprm.first().copied().unwrap_or(0.0) as Real * u.abs(); + let damp = + actuator.gainprm.first().copied().unwrap_or(0.0) as Real * u.abs() * gain_scale; data.set_motor_velocity(ax, 0.0, damp); data.set_motor_velocity(lin_ax, 0.0, damp); data.set_motor_max_force(ax, force_max); @@ -354,15 +638,22 @@ fn configure_actuator_motor( if affine && -b1 > 0.0 { let stiffness = -b1; let damping = -b2; + // Target uses the unscaled stiffness so the commanded pose is + // unchanged; only the gains (force) scale. let target = (g0 * u + b0) / stiffness; - data.set_motor_position(ax, target, stiffness, damping); - data.set_motor_position(lin_ax, target, stiffness, damping); + data.set_motor_position(ax, target, stiffness * gain_scale, damping * gain_scale); + data.set_motor_position( + lin_ax, + target, + stiffness * gain_scale, + damping * gain_scale, + ); data.set_motor_max_force(ax, force_max); data.set_motor_max_force(lin_ax, force_max); } else { // `gaintype="fixed"`, non-restoring bias: constant force // `gainprm0·u` through the transmission. - apply_constant_force(data, g0 * u * gear); + apply_constant_force(data, g0 * u * gear * gain_scale); } } ActuatorKind::IntVelocity | ActuatorKind::Other => { @@ -383,6 +674,67 @@ pub enum MjcfSensorValue { } impl MjcfRobot { + /// Look up a keyframe by its ``. Returns `None` if no + /// keyframe with that name exists. + pub fn keyframe_by_name(&self, name: &str) -> Option<&Keyframe> { + self.keyframes + .iter() + .find(|k| k.name.as_deref() == Some(name)) + } + + /// Build a per-actuator control vector that commands the given keyframe's + /// pose, in the order of [`Self::actuators`]. + /// + /// This is what lets a keyframe *hold* under actuation instead of being + /// dragged back to the zero configuration: driving the actuators with a + /// plain zero vector (the obvious default) makes every position servo pull + /// its joint to 0, undoing the keyframe. Each entry is resolved as: + /// + /// - the keyframe's explicit `ctrl[i]` when it provides one (MuJoCo's own + /// commanded input for the pose), otherwise + /// - for an actuator driving a 1-DoF (hinge/slide) joint, that joint's + /// keyframe `qpos` in rapier coordinates (the natural position-servo + /// target), otherwise + /// - `0`. + /// + /// Pass the result to [`apply_controls`](MjcfRobotHandles::::apply_controls) + /// / [`apply_controls_multibody`](MjcfRobotHandles::>::apply_controls_multibody). + pub fn keyframe_controls(&self, key: &Keyframe) -> Vec { + // Per-joint position target (rapier coordinates) derived from `qpos`. + let mut joint_target: Vec> = vec![None; self.joints.len()]; + let mut qpos_i = 0; + for dof in &self.qpos_dofs { + if let Some(jidx) = dof.joint + && let Some(&q) = key.qpos.get(qpos_i) + { + let target = match dof.kind { + MjcfDofKind::Hinge => Some(q as Real - dof.reference), + MjcfDofKind::Slide => Some((q as Real - dof.reference) * dof.scale), + // Ball/free are multi-DoF; no single scalar servo target. + _ => None, + }; + if let Some(t) = target { + joint_target[jidx] = Some(t); + } + } + qpos_i += dof.kind.qpos_width(); + } + + self.actuators + .iter() + .enumerate() + .map(|(i, a)| { + if let Some(&c) = key.ctrl.get(i) { + c as Real + } else { + a.joint_index + .and_then(|j| joint_target.get(j).copied().flatten()) + .unwrap_or(0.0) + } + }) + .collect() + } + /// Read a sensor's current value from the rapier state. Returns `None` /// for sensors the loader doesn't know how to evaluate. pub fn read_sensor( diff --git a/crates/rapier3d-mjcf/src/loader/types.rs b/crates/rapier3d-mjcf/src/loader/types.rs index 0965163ae..509bb8cfa 100644 --- a/crates/rapier3d-mjcf/src/loader/types.rs +++ b/crates/rapier3d-mjcf/src/loader/types.rs @@ -232,6 +232,79 @@ pub enum SensorObjectRef { }, } +/// The MJCF joint kind backing one keyframe `qpos`/`qvel` slot. Decides how +/// many `qpos` / `qvel` scalars the slot consumes and how they are written to +/// the rapier model. +#[derive(Copy, Clone, Debug, PartialEq, Eq)] +pub enum MjcfDofKind { + /// 6-DoF free / floating base. Consumes 7 `qpos` (xyz position + wxyz + /// quaternion, world frame) and 6 `qvel` (linear + angular). Maps to a + /// free rapier body — there is no rapier joint, so the world pose is + /// written directly (see [`MjcfQposDof::body`]). + Free, + /// 3-DoF ball. Consumes 4 `qpos` (wxyz quaternion) and 3 `qvel`. + Ball, + /// 1-DoF revolute. Consumes 1 `qpos` (angle) and 1 `qvel`. + Hinge, + /// 1-DoF prismatic. Consumes 1 `qpos` (length) and 1 `qvel`. + Slide, +} + +impl MjcfDofKind { + /// Number of `qpos` scalars this DoF consumes. + pub fn qpos_width(self) -> usize { + match self { + MjcfDofKind::Free => 7, + MjcfDofKind::Ball => 4, + MjcfDofKind::Hinge | MjcfDofKind::Slide => 1, + } + } + + /// Number of `qvel` scalars this DoF consumes. + pub fn qvel_width(self) -> usize { + match self { + MjcfDofKind::Free => 6, + MjcfDofKind::Ball => 3, + MjcfDofKind::Hinge | MjcfDofKind::Slide => 1, + } + } +} + +/// One MJCF joint's slot in a keyframe's `qpos` / `qvel` arrays, resolved to +/// where it must be written in the rapier model. +/// +/// The entries in [`MjcfRobot::qpos_dofs`] are ordered exactly as MuJoCo lays +/// out `qpos` (and `qvel`): joints in kinematic-tree order — bodies in +/// declaration order, and within a body its joints in declaration order. +/// Welds and other 0-DoF couplings contribute nothing, matching MuJoCo. So +/// reading a keyframe is a single left-to-right walk of this list, advancing a +/// `qpos` cursor by [`MjcfDofKind::qpos_width`] (and a `qvel` cursor by +/// [`MjcfDofKind::qvel_width`]) per entry. +#[derive(Clone, Debug)] +pub struct MjcfQposDof { + /// The joint kind — decides the `qpos`/`qvel` width and how the slot maps + /// onto rapier. + pub kind: MjcfDofKind, + /// Index into [`MjcfRobot::joints`] of the rapier joint this slot drives. + /// `None` for [`MjcfDofKind::Free`], which has no rapier joint — its pose + /// is written to [`Self::body`] directly. + pub joint: Option, + /// Index into [`MjcfRobot::bodies`] of the child body this slot moves. For + /// [`MjcfDofKind::Free`] this is the floating body whose world pose `qpos` + /// sets. + pub body: usize, + /// MJCF `` (the joint's zero offset). Subtracted from `qpos` + /// to convert MuJoCo's absolute joint coordinate to rapier's + /// frame-relative one, matching the limit convention in the serial-joint + /// builder. A length (pre-scale) for [`MjcfDofKind::Slide`]; unused for + /// `Free`/`Ball`. + pub reference: Real, + /// Length scale (`MjcfLoaderOptions::scale`) applied to `Slide` coordinates + /// and to the `Free` base translation, matching the scaling the loader + /// applied to the model geometry. + pub scale: Real, +} + /// A robot loaded from an MJCF file: a flat list of bodies, joints, and /// extras. #[derive(Clone, Debug, Default)] @@ -255,6 +328,14 @@ pub struct MjcfRobot { pub sensors: Vec, /// Keyframes preserved from the model. pub keyframes: Vec, + /// Layout that maps a keyframe's `qpos` / `qvel` arrays onto + /// [`Self::joints`] / [`Self::bodies`], in MuJoCo's generalized-coordinate + /// order. Drives [`MjcfRobotHandles::apply_keyframe`](super::MjcfRobotHandles::apply_keyframe). + pub qpos_dofs: Vec, + /// The `MjcfLoaderOptions::shift` the loader applied to every body pose. + /// Re-applied when a keyframe writes a free body's absolute world pose so + /// the floating base lands in the same frame as the rest of the model. + pub base_shift: Pose, /// Resolved gravity vector (`