diff --git a/CHANGELOG.md b/CHANGELOG.md index 7fc1f097b..820b4e346 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -1,3 +1,27 @@ +## Unreleased + +### Added + +- Multibody joints now support a per-DoF **armature** (reflected rotor inertia, matching MuJoCo's + concept), exposed via `Multibody::armature()` / `armature_mut()`. The related + `Multibody::dof_inverse_inertia()` returns the articulated DoF inverse inertia including armature. +- Multibody joints now support passive **per-DoF springs** (MuJoCo-style spring/damper) via + `MultibodyJoint::set_spring()` / `spring()`, integrated implicitly. +- Multibody joints now support **holonomic DoF coupling**: constrain one generalized coordinate to + another via `MultibodyDofCoupling` and `Multibody::add_dof_coupling()` / `couplings()`. +- `MultibodyLink::assembly_id()` exposes the offset of a link's DoFs in the multibody's generalized + coordinate/velocity vectors. +- `rapier3d-mjcf`: support for tendons, equality constraints, materials (including visual-mesh + materials), actuator max-force, joint DoF coupling, armature, per-DoF spring/damper, and control. +- Testbed: the ability to specify a material. + +### Fixed + +- Corrected the local frame of impulse joints attached to multibodies. +- Added a missing coupling term for loop-closing impulse-joint constraints (bodies with a non-zero + center-of-mass offset). +- Fixed a crash occurring with the `parallel` feature enabled. + ## v0.33.0 (05 June 2026) ### Added diff --git a/Cargo.toml b/Cargo.toml index 9fc3adfc6..6fe8ad6f8 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -112,7 +112,7 @@ oorandom = { version = "11", default-features = false } #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 f1f7ac434..d9f0b7572 100644 --- a/crates/mjcf-rs/src/equality.rs +++ b/crates/mjcf-rs/src/equality.rs @@ -7,6 +7,11 @@ pub enum Equality { Connect(EqualityConnect), /// `` — rigid attachment between two bodies. 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. @@ -49,3 +54,37 @@ 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], +} + +/// `` 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/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/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/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..4652eaf81 100644 --- a/crates/mjcf-rs/src/loader/attach.rs +++ b/crates/mjcf-rs/src/loader/attach.rs @@ -171,6 +171,32 @@ 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)); + } + 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 359d32a67..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, 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}; @@ -131,7 +134,53 @@ 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" => { + 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)?, @@ -169,6 +218,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 +242,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())?), @@ -276,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/defaults.rs b/crates/mjcf-rs/src/loader/defaults.rs index b97985c1c..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; @@ -304,6 +329,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())?), @@ -373,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 8652be7b2..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)] @@ -106,6 +121,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, @@ -194,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() { @@ -288,6 +327,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..e778d51c9 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,14 +105,12 @@ 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)?, "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), @@ -115,6 +119,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/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 ab3c15f64..c84b54dd4 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, MjcfDofKind, MjcfEqualityJoint, MjcfJoint, MjcfJointCoupling, + MjcfQposDof, MjcfRobot, MjcfSensorBinding, MjcfVisualMesh, SensorObjectRef, }; pub(super) struct Conversion<'a> { @@ -144,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(); @@ -210,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; } @@ -238,6 +256,10 @@ impl<'a> Conversion<'a> { link2: rb_idx, joint, damping_per_dof: 0.0, + armature_per_dof: 0.0, + spring_stiffness_per_dof: 0.0, + spring_ref: 0.0, + springdamper: None, }); } return; @@ -265,29 +287,82 @@ 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 }; + // 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; + // 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, link2: cur_rapier, joint, damping_per_dof, + armature_per_dof, + spring_stiffness_per_dof, + 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; } @@ -524,6 +599,132 @@ 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 + ); + } + } + } + } + + /// ``: 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, + }); + } + + /// 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, + }); } } } @@ -638,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/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/insert.rs b/crates/rapier3d-mjcf/src/loader/insert.rs index fb25f0e74..344da822d 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::move_motor_damping_to_multibody; +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, +}; 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); @@ -152,6 +160,10 @@ impl MjcfRobot { continue; }; 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 @@ -193,6 +205,39 @@ 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); + } + // 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, @@ -200,6 +245,57 @@ 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, + ); + } + } + + // `` 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/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 ccea67379..b8feee5d7 100644 --- a/crates/rapier3d-mjcf/src/loader/mass.rs +++ b/crates/rapier3d-mjcf/src/loader/mass.rs @@ -10,14 +10,25 @@ 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, Multibody, MultibodyDofCoupling, MultibodyJointHandle, MultibodyJointSet, + RigidBodyHandle, RigidBodySet, +}; use rapier3d::math::{Matrix, Pose, Real, Vector}; 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 +39,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 +211,242 @@ 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; + } + } +} + +/// 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; + } + } +} + +/// 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..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, MjcfRobot, MjcfSensorBinding, - MjcfVisualMesh, SensorObjectRef, + MjcfActuatorBinding, MjcfBody, MjcfDofKind, MjcfEqualityJoint, MjcfJoint, MjcfQposDof, + MjcfRenderMaterial, MjcfRobot, MjcfSensorBinding, MjcfVisualMesh, SensorObjectRef, }; 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/runtime.rs b/crates/rapier3d-mjcf/src/loader/runtime.rs index c36f6a76a..d8ef2f4ed 100644 --- a/crates/rapier3d-mjcf/src/loader/runtime.rs +++ b/crates/rapier3d-mjcf/src/loader/runtime.rs @@ -5,14 +5,15 @@ use mjcf_rs::extras::Keyframe; use rapier3d::dynamics::{ - ImpulseJointHandle, ImpulseJointSet, JointAxis, 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. @@ -188,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. /// @@ -204,70 +440,225 @@ 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; + 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); - // 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, gain_scale); + } + } +} + +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], + ) { + 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 + // `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, gain_scale); + 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, + gain_scale: Real, +) { + use mjcf_rs::extras::ActuatorKind; + + let ax = JointAxis::AngX; + let lin_ax = JointAxis::LinX; + // 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 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 + // 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 * gain_scale), + 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() * 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); + data.set_motor_max_force(lin_ax, force_max); + } + 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; + // 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 * 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 * gain_scale); } } + ActuatorKind::IntVelocity | ActuatorKind::Other => { + // Not driven automatically — user-controlled. + } } } @@ -283,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 552498e3c..509bb8cfa 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`. @@ -108,6 +130,33 @@ 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, + /// 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. @@ -125,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 { @@ -165,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)] @@ -180,12 +320,22 @@ 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. 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 (`