diff --git a/crates/rapier2d/tests/snapshot_portability.rs b/crates/rapier2d/tests/snapshot_portability.rs index 9cc450eb5..153e471cf 100644 --- a/crates/rapier2d/tests/snapshot_portability.rs +++ b/crates/rapier2d/tests/snapshot_portability.rs @@ -31,7 +31,7 @@ use rapier2d::prelude::*; /// Snapshot size in bytes, and its FNV-1a digest. Both, because the size alone localizes a /// failure: a differing size means a container's *encoding* changed, an equal size with a /// differing digest means the values did. -const GOLDEN: (usize, u64) = (88_532, 0xccad_c968_4d93_9348); +const GOLDEN: (usize, u64) = (88_572, 0x9e84_4370_55e1_8c88); const STEPS: usize = 60; diff --git a/crates/rapier3d-mjcf/src/loader/conversion.rs b/crates/rapier3d-mjcf/src/loader/conversion.rs index 378ea1c88..797cd9b3a 100644 --- a/crates/rapier3d-mjcf/src/loader/conversion.rs +++ b/crates/rapier3d-mjcf/src/loader/conversion.rs @@ -257,6 +257,7 @@ impl<'a> Conversion<'a> { joint, damping_per_dof: 0.0, armature_per_dof: 0.0, + frictionloss_per_dof: 0.0, spring_stiffness_per_dof: 0.0, spring_ref: 0.0, springdamper: None, @@ -322,6 +323,10 @@ 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; + // Dry joint friction, routed into the multibody's per-DoF + // `frictionloss` vector at insertion time — see + // `MjcfJoint::frictionloss_per_dof`. + let frictionloss_per_dof = j.frictionloss.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 @@ -341,6 +346,7 @@ impl<'a> Conversion<'a> { joint, damping_per_dof, armature_per_dof, + frictionloss_per_dof, spring_stiffness_per_dof, spring_ref, springdamper, diff --git a/crates/rapier3d-mjcf/src/loader/insert.rs b/crates/rapier3d-mjcf/src/loader/insert.rs index 344da822d..7e3a20c60 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_joint_coupling_to_multibody, add_spring_to_multibody, - add_springdamper_to_multibody, move_motor_damping_to_multibody, + add_armature_to_multibody, add_frictionloss_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; @@ -161,6 +161,7 @@ impl MjcfRobot { }; let damping_per_dof = j.damping_per_dof; let armature_per_dof = j.armature_per_dof; + let frictionloss_per_dof = j.frictionloss_per_dof; let spring_stiffness_per_dof = j.spring_stiffness_per_dof; let spring_ref = j.spring_ref; let springdamper = j.springdamper; @@ -211,6 +212,14 @@ impl MjcfRobot { if armature_per_dof > 0.0 { add_armature_to_multibody(multibody_joints, h, armature_per_dof); } + // Route MJCF `` into the multibody's + // per-DoF friction vector, where the solver turns it into a + // box-bounded constraint row. The serial-joint path has no + // such vector, so it keeps the motor approximation built by + // the joint builder. + if frictionloss_per_dof > 0.0 { + add_frictionloss_to_multibody(multibody_joints, h, frictionloss_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 diff --git a/crates/rapier3d-mjcf/src/loader/joint.rs b/crates/rapier3d-mjcf/src/loader/joint.rs index 886429447..72fbbe7e2 100644 --- a/crates/rapier3d-mjcf/src/loader/joint.rs +++ b/crates/rapier3d-mjcf/src/loader/joint.rs @@ -143,8 +143,20 @@ impl<'a> Conversion<'a> { } } - // Friction loss (lossy approximation): use a velocity motor - // capped at `frictionloss`. + // Friction loss, for the *impulse-joint* path only: a zero-target + // velocity motor capped at `frictionloss`. That is the same row the + // multibody path builds properly (zero target velocity, impulse + // bounded by `frictionloss·dt`), but squeezed into the joint's one + // motor slot. The multibody path instead routes the value through + // `Multibody::frictionloss` (`add_frictionloss_to_multibody`) and + // clears this motor, so the two never coexist. + // + // Skipped when a spring already owns the slot: `motor_velocity` + // zeroes the motor's stiffness and damping, which would silently + // delete the `` / `` spring + // installed just above. A spring is the more load-bearing of the + // two, and on the multibody path (where both are wanted together) + // friction no longer needs the slot at all. if joint.frictionloss > 0.0 { let axis = match joint.type_ { mb::JointType::Hinge | mb::JointType::Ball => Some(JointAxis::AngX), @@ -152,8 +164,15 @@ impl<'a> Conversion<'a> { _ => None, }; if let Some(ax) = axis { - builder = builder.motor_velocity(ax, 0.0, 0.0); - builder = builder.motor_max_force(ax, joint.frictionloss as Real); + if stiffness.is_some() { + log::warn!( + ": `frictionloss` is not applied on the impulse-joint path because the joint also has a spring, and both need the single motor slot. The multibody path applies both.", + joint.name, + ); + } else { + builder = builder.motor_velocity(ax, 0.0, 0.0); + builder = builder.motor_max_force(ax, joint.frictionloss as Real); + } } } } diff --git a/crates/rapier3d-mjcf/src/loader/mass.rs b/crates/rapier3d-mjcf/src/loader/mass.rs index fa48274d8..1da8ef52a 100644 --- a/crates/rapier3d-mjcf/src/loader/mass.rs +++ b/crates/rapier3d-mjcf/src/loader/mass.rs @@ -171,6 +171,7 @@ pub(super) fn move_motor_damping_to_multibody( handle: MultibodyJointHandle, damping: Real, ) { + use rapier3d::dynamics::JointAxesMask; use rapier3d::math::SPATIAL_DIM; let Some((multibody, link_id)) = multibody_joints.get_mut(handle) else { return; @@ -194,7 +195,19 @@ pub(super) fn move_motor_damping_to_multibody( let motor_bits = link.joint.data.motor_axes.bits(); for i in 0..SPATIAL_DIM { if (motor_bits & (1 << i)) != 0 { - link.joint.data.motors[i].damping = 0.0; + let motor = &mut link.joint.data.motors[i]; + motor.damping = 0.0; + // Damping was this motor's only contribution: what is left is a + // zero-target row with no gains, i.e. a rigid velocity lock with + // unlimited force. Drop the axis (like `add_spring_to_multibody` + // does); actuators re-enable it on the axes they drive. + if motor.stiffness == 0.0 + && motor.target_vel == 0.0 + && motor.max_force == Real::MAX + && let Some(flag) = JointAxesMask::from_bits(1u8 << i) + { + link.joint.data.motor_axes.remove(flag); + } } } // Drop the &mut MultibodyLink borrow before reborrowing the multibody. @@ -256,6 +269,74 @@ pub(super) fn add_armature_to_multibody( } } +/// After a joint has been inserted into a multibody, add the MJCF +/// `` to the multibody's per-DoF friction vector. The +/// solver emits one box-bounded constraint row per DoF with a non-zero entry, +/// driving that DoF's velocity to zero with the impulse capped at +/// `frictionloss · dt`. +/// +/// Applied uniformly to every free DoF of the joint, matching MJCF semantics +/// (a ball joint with `frictionloss=f` gets `f` on each of its angular DoFs). +/// +/// Also clears the zero-velocity motor the serial-joint builder installs as the +/// impulse path's approximation, so the two do not stack. Unlike that motor, +/// the constraint rows cover every free DoF and leave the motor slot free for +/// a spring or an actuator. +pub(super) fn add_frictionloss_to_multibody( + multibody_joints: &mut MultibodyJointSet, + handle: MultibodyJointHandle, + frictionloss: Real, +) { + use rapier3d::dynamics::JointAxesMask; + 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 vector + // (assembly_id isn't public), same as the armature 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 fl_vec = multibody.frictions_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 < fl_vec.len() { + fl_vec[idx] = frictionloss; + } + local_dof += 1; + } + } + + // Drop the impulse-path motor approximation. Only when it is the friction + // one: a motor carrying a spring (non-zero stiffness or damping) or an + // actuator target belongs to something else. + let Some(link) = multibody.links_mut().nth(link_id) else { + return; + }; + for axis in 0..SPATIAL_DIM { + if (locked_bits & (1 << axis)) == 0 { + let motor = &link.joint.data.motors[axis]; + let is_friction_motor = motor.stiffness == 0.0 + && motor.damping == 0.0 + && motor.target_vel == 0.0 + && motor.max_force == frictionloss; + if is_friction_motor && let Some(flag) = JointAxesMask::from_bits(1u8 << axis) { + link.joint.data.motor_axes.remove(flag); + } + } + } +} + /// 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 diff --git a/crates/rapier3d-mjcf/src/loader/types.rs b/crates/rapier3d-mjcf/src/loader/types.rs index 509bb8cfa..738a4ca1e 100644 --- a/crates/rapier3d-mjcf/src/loader/types.rs +++ b/crates/rapier3d-mjcf/src/loader/types.rs @@ -138,6 +138,18 @@ pub struct MjcfJoint { /// (huge along the joint axis, ~0 across it) and the multibody mass /// matrix ill-conditioned. pub armature_per_dof: Real, + /// MJCF `` value (dry joint friction, N or N·m). On + /// the multibody insertion path this becomes a per-DoF entry of the + /// multibody's `frictionloss` vector, which the solver turns into one + /// box-bounded constraint row per DoF. + /// + /// It is deliberately **not** a motor: MuJoCo's friction loss is a bound on + /// the force friction may generate, not a `-f·sign(q̇)` force, and a joint + /// commonly carries both a position servo and a friction loss. Routing it + /// through the joint's single motor slot (as this loader used to) made the + /// two fight: the servo's `forcerange` overwrote the friction bound, and + /// the friction entry wiped a `` spring's coefficients. + pub frictionloss_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 diff --git a/crates/rapier3d-mjcf/tests/frictionloss.rs b/crates/rapier3d-mjcf/tests/frictionloss.rs new file mode 100644 index 000000000..0f3162907 --- /dev/null +++ b/crates/rapier3d-mjcf/tests/frictionloss.rs @@ -0,0 +1,208 @@ +//! `` must reach the multibody's per-DoF friction vector, +//! where the solver turns it into a box-bounded constraint row, instead of +//! being squeezed into the joint's single motor slot. + +use rapier3d::prelude::*; +use rapier3d_mjcf::{MjcfLoaderOptions, MjcfMultibodyOptions, MjcfRobot}; + +/// A hinge with `frictionloss`, optionally also carrying a passive spring. +fn model(spring: bool) -> String { + let stiffness = if spring { r#" stiffness="5" "# } else { " " }; + format!( + r#" + + + + + + + + + +"# + ) +} + +/// Loads `xml` on the multibody path and returns the multibody plus its link. +fn load(xml: &str) -> (MultibodyJointSet, MultibodyJointHandle) { + let (robot, _) = MjcfRobot::from_str(xml, MjcfLoaderOptions::default(), ".").unwrap(); + let mut bodies = RigidBodySet::new(); + let mut colliders = ColliderSet::new(); + let mut multibody_joints = MultibodyJointSet::new(); + let mut impulse_joints = ImpulseJointSet::new(); + let handles = robot.insert_using_multibody_joints( + &mut bodies, + &mut colliders, + &mut multibody_joints, + &mut impulse_joints, + MjcfMultibodyOptions::empty(), + ); + let handle = handles.joints[0].joint.expect("hinge was not inserted"); + (multibody_joints, handle) +} + +#[test] +fn frictionloss_reaches_the_multibody_friction_vector() { + let (mut set, handle) = load(&model(false)); + let (multibody, link_id) = set.get_mut(handle).unwrap(); + + assert!( + multibody.frictions().iter().any(|v| *v == 3.0), + "frictionloss should land in the multibody's per-DoF vector, got {:?}", + multibody.frictions() + ); + + // The impulse-path motor approximation must be cleared, or the joint would + // carry both it and the constraint rows. + let link = multibody.links().nth(link_id).unwrap(); + assert!( + link.joint().data.motor_axes.is_empty(), + "the frictionloss motor approximation should be cleared on the multibody path" + ); +} + +#[test] +fn frictionloss_does_not_delete_a_spring_on_the_impulse_path() { + // The impulse-joint path has no per-DoF friction vector, so friction stays + // a motor approximation there and has to share the joint's single motor + // slot with the `` spring. It cannot: `motor_velocity` + // zeroes the motor's stiffness and damping. The loader now keeps the + // spring and skips the friction approximation (with a warning) rather than + // silently deleting the spring. + let xml = model(true); + 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); + + let joint = &impulse_joints.get(handles.joints[0].joint).unwrap().data; + let sprung = (0..SPATIAL_DIM).any(|i| joint.motors[i].stiffness > 0.0); + assert!( + sprung, + "the spring must survive: {:?}", + (0..SPATIAL_DIM) + .map(|i| joint.motors[i].stiffness) + .collect::>() + ); +} + +#[test] +fn frictionloss_still_approximated_on_the_impulse_path_without_a_spring() { + // With the slot free, the impulse path keeps the zero-velocity motor capped + // at `frictionloss` — the best a single motor slot can do. + let xml = model(false); + 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); + + let joint = &impulse_joints.get(handles.joints[0].joint).unwrap().data; + let capped = (0..SPATIAL_DIM).any(|i| joint.motors[i].max_force == 3.0); + assert!(capped, "frictionloss should still cap a motor here"); +} + +/// A `` with no spring and no actuator must not be left frozen. +/// +/// `move_motor_damping_to_multibody` moves the damping into the multibody's +/// per-DoF vector and zeroes the motor's damping. When damping was the motor's +/// only contribution, what remains is a zero-target row with no gains and no +/// force limit: a rigid velocity lock. The loader used to overwrite that motor +/// with the `frictionloss` approximation (capping it at a harmless value), so +/// the lock only became visible once friction stopped claiming the slot: the +/// tendon-driven finger joints of the MJCF shadow hand stopped moving. +#[test] +fn damping_without_a_spring_leaves_no_locking_motor() { + let xml = r#" + + + + + + + + + +"#; + let (mut set, handle) = load(xml); + let (multibody, link_id) = set.get_mut(handle).unwrap(); + + assert_eq!( + multibody.damping()[multibody.link(link_id).unwrap().assembly_id()], + 0.05, + "damping should reach the multibody's per-DoF vector" + ); + let link = multibody.links().nth(link_id).unwrap(); + assert!( + link.joint().data.motor_axes.is_empty(), + "a damping-only motor must be dropped, not left as an unbounded \ + zero-velocity lock: {:?}", + link.joint().data.motor_axes + ); +} + +/// The behavioural half of the check above: the link must swing under gravity. +#[test] +fn damped_joint_still_swings_under_gravity() { + let xml = r#" + + + + + + + + + +"#; + let (robot, _) = MjcfRobot::from_str(xml, MjcfLoaderOptions::default(), ".").unwrap(); + let mut bodies = RigidBodySet::new(); + let mut colliders = ColliderSet::new(); + let mut multibody_joints = MultibodyJointSet::new(); + let mut impulse_joints = ImpulseJointSet::new(); + let handles = robot.insert_using_multibody_joints( + &mut bodies, + &mut colliders, + &mut multibody_joints, + &mut impulse_joints, + MjcfMultibodyOptions::empty(), + ); + let handle = handles.joints[0].joint.expect("hinge was not inserted"); + + let mut pipeline = PhysicsPipeline::new(); + let params = IntegrationParameters::default(); + 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..60 { + pipeline.step( + Vector::new(0.0, 0.0, -9.81), + ¶ms, + &mut islands, + &mut broad_phase, + &mut narrow_phase, + &mut bodies, + &mut colliders, + &mut impulse_joints, + &mut multibody_joints, + &mut ccd, + &(), + &(), + ); + } + + let (multibody, link_id) = multibody_joints.get(handle).unwrap(); + let angle = multibody.link(link_id).unwrap().joint().coords()[3]; + assert!( + angle.abs() > 0.1, + "the damped hinge should swing under gravity, got {angle}" + ); +} diff --git a/crates/rapier3d/tests/snapshot_portability.rs b/crates/rapier3d/tests/snapshot_portability.rs index 685bbe68a..efc1faad0 100644 --- a/crates/rapier3d/tests/snapshot_portability.rs +++ b/crates/rapier3d/tests/snapshot_portability.rs @@ -31,7 +31,7 @@ use rapier3d::prelude::*; /// Snapshot size in bytes, and its FNV-1a digest. Both, because the size alone localizes a /// failure: a differing size means a container's *encoding* changed, an equal size with a /// differing digest means the values did. -const GOLDEN: (usize, u64) = (481_520, 0xe06b_d263_cd02_7324); +const GOLDEN: (usize, u64) = (481_608, 0xedfe_b12c_c030_e444); const STEPS: usize = 60; diff --git a/src/dynamics/joint/multibody_joint/mod.rs b/src/dynamics/joint/multibody_joint/mod.rs index 26d9f7897..58ebd2946 100644 --- a/src/dynamics/joint/multibody_joint/mod.rs +++ b/src/dynamics/joint/multibody_joint/mod.rs @@ -12,7 +12,9 @@ pub use self::multibody_joint_set::{MultibodyJointSet, MultibodyLinkId}; #[cfg(feature = "alloc")] pub use self::multibody_link::MultibodyLink; #[cfg(feature = "alloc")] -pub use self::unit_multibody_joint::{unit_joint_limit_constraint, unit_joint_motor_constraint}; +pub use self::unit_multibody_joint::{ + unit_joint_friction_constraint, unit_joint_limit_constraint, unit_joint_motor_constraint, +}; #[cfg(feature = "alloc")] mod multibody; diff --git a/src/dynamics/joint/multibody_joint/multibody.rs b/src/dynamics/joint/multibody_joint/multibody.rs index 46f9cb660..8126611b7 100644 --- a/src/dynamics/joint/multibody_joint/multibody.rs +++ b/src/dynamics/joint/multibody_joint/multibody.rs @@ -100,6 +100,8 @@ pub struct Multibody { pub(crate) damping: DVector, /// Per-DoF reflected rotor inertia (matches MuJoCo’s concept of `armature`). pub(crate) armature: DVector, + /// Per-DoF dry joint friction (matches MuJoCo’s `frictionloss`). + pub(crate) frictions: DVector, pub(crate) accelerations: DVector, body_jacobians: Vec>, @@ -151,6 +153,7 @@ impl Multibody { velocities: DVector::zeros(0), damping: DVector::zeros(0), armature: DVector::zeros(0), + frictions: DVector::zeros(0), accelerations: DVector::zeros(0), body_jacobians: Vec::new(), augmented_mass: DMatrix::zeros(0, 0), @@ -231,6 +234,9 @@ impl Multibody { mb.armature .rows_mut(assembly_id, link_ndofs) .copy_from(&self.armature.rows(link.assembly_id, link_ndofs)); + mb.frictions + .rows_mut(assembly_id, link_ndofs) + .copy_from(&self.frictions.rows(link.assembly_id, link_ndofs)); mb.accelerations .rows_mut(assembly_id, link_ndofs) .copy_from(&self.accelerations.rows(link.assembly_id, link_ndofs)); @@ -292,6 +298,9 @@ impl Multibody { self.armature .rows_mut(rhs_copy_shift, rhs_copy_ndofs) .copy_from(&rhs.armature.rows(rhs_root_ndofs, rhs_copy_ndofs)); + self.frictions + .rows_mut(rhs_copy_shift, rhs_copy_ndofs) + .copy_from(&rhs.frictions.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)); @@ -401,6 +410,19 @@ impl Multibody { &mut self.armature } + /// The vector of per-DoF dry joint friction (MuJoCo's `frictionloss`, in N + /// or N·m): the largest force friction may generate on that DoF. + #[inline] + pub fn frictions(&self) -> &DVector { + &self.frictions + } + + /// Mutable vector of per-DoF dry joint friction of this multibody. + #[inline] + pub fn frictions_mut(&mut self) -> &mut DVector { + &mut self.frictions + } + pub(crate) fn add_link( &mut self, parent: Option, // TODO: should be a RigidBodyHandle? @@ -467,6 +489,7 @@ impl Multibody { 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.frictions.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))); @@ -1076,6 +1099,18 @@ impl Multibody { &self.couplings } + /// The number of dry-friction rows `link_id`'s joint will emit: one per + /// free DoF whose `frictionloss` entry is non-zero. + pub(crate) fn num_friction_constraints(&self, link_id: usize) -> usize { + let Some(link) = self.link(link_id) else { + return 0; + }; + let ndofs = link.joint().ndofs(); + (0..ndofs) + .filter(|i| self.frictions[link.assembly_id + i] > 0.0) + .count() + } + /// Number of coupling constraints "owned" by `owner_link` — 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). @@ -1220,6 +1255,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.frictions = self.frictions.clone().insert_rows(0, SPATIAL_DIM, 0.0); self.accelerations = self.accelerations.clone().insert_rows(0, SPATIAL_DIM, 0.0); @@ -1230,6 +1266,7 @@ impl Multibody { assert!(self.velocities.len() >= SPATIAL_DIM); assert!(self.damping.len() >= SPATIAL_DIM); assert!(self.armature.len() >= SPATIAL_DIM); + assert!(self.frictions.len() >= SPATIAL_DIM); assert!(self.accelerations.len() >= SPATIAL_DIM); let fixed_joint = MultibodyJoint::fixed(root_pose); @@ -1242,12 +1279,14 @@ impl Multibody { self.velocities = DVector::zeros(0); self.damping = DVector::zeros(0); self.armature = DVector::zeros(0); + self.frictions = 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.frictions = self.frictions.index((prev_root_ndofs.., 0)).into_owned(); self.accelerations = self .accelerations .index((prev_root_ndofs.., 0)) diff --git a/src/dynamics/joint/multibody_joint/multibody_joint.rs b/src/dynamics/joint/multibody_joint/multibody_joint.rs index 052ca2559..632fcbffd 100644 --- a/src/dynamics/joint/multibody_joint/multibody_joint.rs +++ b/src/dynamics/joint/multibody_joint/multibody_joint.rs @@ -361,6 +361,24 @@ impl MultibodyJoint { self.data.softness, ); } + // Dry joint friction (MuJoCo `frictionloss`), keyed per-DoF on + // the multibody rather than per-axis on the joint. Zero (the + // default) emits nothing. + let friction = multibody.frictions()[link.assembly_id + curr_free_dof]; + if friction > 0.0 { + joint::unit_joint_friction_constraint( + params, + multibody, + link, + friction, + curr_free_dof, + j_id, + jacobians, + constraints, + &mut num_constraints, + self.data.softness, + ); + } curr_free_dof += 1; } } @@ -416,6 +434,25 @@ impl MultibodyJoint { &mut num_constraints, ); } + + // Dry joint friction (MuJoCo `frictionloss`), keyed per-DoF on + // the multibody rather than per-axis on the joint. Zero (the + // default) emits nothing. + let friction = multibody.frictions()[link.assembly_id + curr_free_dof]; + if friction > 0.0 { + joint::unit_joint_friction_constraint( + params, + multibody, + link, + friction, + curr_free_dof, + j_id, + jacobians, + constraints, + &mut num_constraints, + self.data.softness, + ); + } curr_free_dof += 1; } } diff --git a/src/dynamics/joint/multibody_joint/multibody_regression_tests.rs b/src/dynamics/joint/multibody_joint/multibody_regression_tests.rs index 817455447..e439f30d4 100644 --- a/src/dynamics/joint/multibody_joint/multibody_regression_tests.rs +++ b/src/dynamics/joint/multibody_joint/multibody_regression_tests.rs @@ -581,3 +581,70 @@ fn issue_907_contact_with_branch_off_fixed_root() { ); assert!(world.bodies[child].translation().y.is_finite()); } + +/// Builds a one-link pendulum hinged at the origin, its rod along +X so gravity +/// torques the hinge, steps it through the start of its descent, and returns +/// how far the link's centre fell. +/// +/// The horizon is deliberately short: a free pendulum reaches the bottom in +/// roughly half a second and swings back up, so drop-at-a-fixed-time only +/// decreases with friction while every case is still descending. +#[cfg(feature = "dim3")] +fn friction_pendulum_drop(friction: Real) -> Real { + const LINK_LEN: Real = 1.0; + let mut world = PhysicsWorld::new(); + + let root = world.insert_body(RigidBodyBuilder::fixed()); + let link = world.insert_body( + RigidBodyBuilder::dynamic() + .translation(Vector::new(LINK_LEN, 0.0, 0.0)) + .additional_mass(1.0), + ); + + let handle = world + .insert_multibody_joint( + root, + link, + RevoluteJointBuilder::new(Vector::new(0.0, 0.0, 1.0)) + .local_anchor1(Vector::ZERO) + .local_anchor2(Vector::new(-LINK_LEN, 0.0, 0.0)), + ) + .unwrap(); + + if friction > 0.0 { + let (multibody, _) = world.multibody_joints.get_mut(handle).unwrap(); + multibody.frictions_mut().fill(friction); + } + + for _ in 0..15 { + world.step(); + } + -world.bodies[link].translation().y +} + +/// Joint dry friction (MuJoCo `frictionloss`). +#[test] +#[cfg(feature = "dim3")] +fn joint_friction_bounds_the_joint_force() { + // Gravity torque about the hinge for a 1 kg rod of length 1 m. + let gravity_torque = 1.0 * 9.81 * 1.0; + + let free = friction_pendulum_drop(0.0); + let weak = friction_pendulum_drop(0.25 * gravity_torque); + let locked = friction_pendulum_drop(4.0 * gravity_torque); + let extreme = friction_pendulum_drop(1000.0 * gravity_torque); + + assert!(free > 0.05, "frictionless pendulum should fall: {free}"); + assert!( + weak < free * 0.9, + "sub-gravity friction should slow the fall: {weak} vs {free}" + ); + assert!( + locked.abs() < 1.0e-4, + "friction above the gravity torque should hold the joint at rest: {locked}" + ); + assert!( + extreme.abs() < 1.0e-4, + "an oversized friction bound must stay inert, not chatter: {extreme}" + ); +} diff --git a/src/dynamics/joint/multibody_joint/unit_multibody_joint.rs b/src/dynamics/joint/multibody_joint/unit_multibody_joint.rs index 5dcac40c6..fde5573ee 100644 --- a/src/dynamics/joint/multibody_joint/unit_multibody_joint.rs +++ b/src/dynamics/joint/multibody_joint/unit_multibody_joint.rs @@ -74,6 +74,65 @@ pub fn unit_joint_limit_constraint( *j_id += 2 * ndofs; } +/// Generates the dry-friction (MuJoCo `frictionloss`) velocity constraint for +/// one generalized DoF. +/// +/// `softness` supplies the CFM compliance (MuJoCo's `solreffriction`); only CFM +/// applies, never ERP, since there is no position error for a bias to chase. +#[allow(clippy::too_many_arguments)] +pub fn unit_joint_friction_constraint( + params: &IntegrationParameters, + multibody: &Multibody, + link: &MultibodyLink, + friction: Real, + dof_id: usize, + j_id: &mut usize, + jacobians: &mut DVector, + constraints: &mut [GenericJointConstraint], + insert_at: &mut usize, + softness: SpringCoefficients, +) { + let ndofs = multibody.ndofs(); + let cfm_coeff = softness.cfm_coeff(params.dt); + + let dof_j_id = *j_id + dof_id + link.assembly_id; + jacobians.rows_mut(*j_id, ndofs * 2).fill(0.0); + jacobians[dof_j_id] = 1.0; + jacobians[dof_j_id + ndofs] = 1.0; + multibody + .inv_augmented_mass() + .solve_mut(&mut jacobians.rows_mut(*j_id + ndofs, ndofs)); + + let lhs = jacobians[dof_j_id + ndofs]; // = J^t * M^-1 J + let max_impulse = friction * params.dt; + let cfm_gain = lhs * cfm_coeff; + + let constraint = GenericJointConstraint { + is_rigid_body1: false, + solver_vel1: u32::MAX, + ndofs1: 0, + j_id1: 0, + is_rigid_body2: false, + solver_vel2: multibody.solver_id, + ndofs2: ndofs, + j_id2: *j_id, + joint_id: usize::MAX, // TODO: we don’t support impulse writeback for internal constraints yet. + impulse: 0.0, + impulse_bounds: [-max_impulse, max_impulse], + inv_lhs: crate::utils::inv(lhs + cfm_gain), + rhs: 0.0, + rhs_wo_bias: 0.0, + cfm_coeff, + cfm_gain, + writeback_id: WritebackId::Friction(dof_id), + }; + + constraints[*insert_at] = constraint; + *insert_at += 1; + + *j_id += 2 * ndofs; +} + /// Initializes and generate the velocity constraints applicable to the multibody links attached /// to this multibody_joint. pub fn unit_joint_motor_constraint( diff --git a/src/dynamics/solver/joint_constraint/generic_joint_constraint.rs b/src/dynamics/solver/joint_constraint/generic_joint_constraint.rs index a7c0e9bfe..7ffb5285c 100644 --- a/src/dynamics/solver/joint_constraint/generic_joint_constraint.rs +++ b/src/dynamics/solver/joint_constraint/generic_joint_constraint.rs @@ -313,6 +313,8 @@ impl GenericJointConstraint { WritebackId::Dof(i) => joint.impulses[i] = self.impulse, WritebackId::Limit(i) => joint.data.limits[i].impulse = self.impulse, WritebackId::Motor(i) => joint.data.motors[i].impulse = self.impulse, + // Writeback not supported yet for internal friction. + WritebackId::Friction(_) => {} } } } 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 8f0fe987e..0f6fe6a16 100644 --- a/src/dynamics/solver/joint_constraint/generic_joint_constraint_builder.rs +++ b/src/dynamics/solver/joint_constraint/generic_joint_constraint_builder.rs @@ -179,49 +179,45 @@ impl JointGenericExternalConstraintBuilder { // constraints. Could we make this more incremental? let pos1; let pos2; - let mb1; - let mb2; - let world_com1; - let world_com2; - - match self.link1 { + let (world_com1, mb1) = 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); + ( + pos1 * self.local_body1.world_com, + 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); + (pos1.translation, LinkOrBodyRef::Body(body1)) } LinkOrBody::Fixed => { pos1 = Pose::IDENTITY; - world_com1 = pos1.translation; - mb1 = LinkOrBodyRef::Fixed; + (pos1.translation, LinkOrBodyRef::Fixed) } }; - match self.link2 { + let (world_com2, mb2) = match self.link2 { 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); + ( + pos2 * self.local_body2.world_com, + 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); + // The solver body pose is at the center of mass. + (pos2.translation, LinkOrBodyRef::Body(body2)) } LinkOrBody::Fixed => { pos2 = Pose::IDENTITY; - world_com2 = pos2.translation; - mb2 = LinkOrBodyRef::Fixed; + (pos2.translation, LinkOrBodyRef::Fixed) } }; @@ -267,9 +263,11 @@ 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(); - // 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) + // This link's own motor/limit/friction constraints, plus the DoF + // couplings it owns (a coupling is owned by its first joint's link). + link.joint().num_velocity_constraints() + + multibody.num_friction_constraints(link_id.id) + + multibody.num_couplings_owned_by(link_id.id) } pub fn generate( @@ -282,8 +280,9 @@ impl JointGenericInternalConstraintBuilder { ) { let multibody = &multibodies[link_id.multibody]; let link = multibody.link(link_id.id).unwrap(); - let num_constraints = - link.joint().num_velocity_constraints() + multibody.num_couplings_owned_by(link_id.id); + let num_constraints = link.joint().num_velocity_constraints() + + multibody.num_friction_constraints(link_id.id) + + multibody.num_couplings_owned_by(link_id.id); if num_constraints == 0 { return; diff --git a/src/dynamics/solver/joint_constraint/joint_constraint_builder.rs b/src/dynamics/solver/joint_constraint/joint_constraint_builder.rs index 72dbdff54..0adb35f44 100644 --- a/src/dynamics/solver/joint_constraint/joint_constraint_builder.rs +++ b/src/dynamics/solver/joint_constraint/joint_constraint_builder.rs @@ -140,6 +140,7 @@ impl JointConstraintBuilder { WritebackId::Dof(i) => self.prev_dof_impulses[i], WritebackId::Limit(i) => self.joint.limits[i].impulse, WritebackId::Motor(i) => self.joint.motors[i].impulse, + WritebackId::Friction(_) => 0.0, }; row.impulse = seed * coeff; } @@ -484,6 +485,9 @@ impl JointConstraintBuilderSimd { } #[cfg(feature = "dim3")] WritebackId::Motor(_) => {} + // Impulse-joint rows only; friction rows are + // multibody-internal and are never built here. + WritebackId::Friction(_) => {} } } } else { diff --git a/src/dynamics/solver/joint_constraint/joint_velocity_constraint.rs b/src/dynamics/solver/joint_constraint/joint_velocity_constraint.rs index e72272fff..494b3f9ee 100644 --- a/src/dynamics/solver/joint_constraint/joint_velocity_constraint.rs +++ b/src/dynamics/solver/joint_constraint/joint_velocity_constraint.rs @@ -39,6 +39,7 @@ pub enum WritebackId { Dof(usize), Limit(usize), Motor(usize), + Friction(usize), } // TODO: right now we only use this for impulse_joints. @@ -382,6 +383,7 @@ impl JointConstraint { WritebackId::Dof(i) => joint.impulses[i] = self.impulse, WritebackId::Limit(i) => joint.data.limits[i].impulse = self.impulse, WritebackId::Motor(i) => joint.data.motors[i].impulse = self.impulse, + WritebackId::Friction(_) => {} } } } @@ -534,6 +536,7 @@ impl JointConstraint { WritebackId::Dof(i) => joint.impulses[i] = impulses[ii], WritebackId::Limit(i) => joint.data.limits[i].impulse = impulses[ii], WritebackId::Motor(i) => joint.data.motors[i].impulse = impulses[ii], + WritebackId::Friction(_) => {} } } } diff --git a/src/pipeline/physics_pipeline/substep.rs b/src/pipeline/physics_pipeline/substep.rs index 75b86d03e..ac8e8f094 100644 --- a/src/pipeline/physics_pipeline/substep.rs +++ b/src/pipeline/physics_pipeline/substep.rs @@ -356,7 +356,7 @@ impl PhysicsPipeline { } // Persistent islands: apply the joint connectivity edits (in order). - let joint_island_events: Vec<_> = impulse_joints.island_events.drain(..).collect(); + let joint_island_events: Vec<_> = core::mem::take(&mut impulse_joints.island_events); for event in joint_island_events { islands.apply_impulse_joint_island_event(bodies, event); }