From 2144f3e5c7f27f26d18ba8bef2bf8590ecf1aca8 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?S=C3=A9bastien=20Crozet?= Date: Fri, 25 Sep 2026 21:13:45 +0200 Subject: [PATCH 1/3] feat(python): multibody state writing, MJCF names and actuator parameters --- .../python/rapier3d/_rapier3d.pyi | 44 +++++ bindings/python/rapier-py-3d/src/joints.rs | 157 +++++++++++++++ bindings/python/rapier-py-3d/src/loaders.rs | 186 +++++++++++++++--- bindings/python/tests/test_loaders.py | 81 ++++++++ bindings/python/tests/test_multibody.py | 110 +++++++++++ 5 files changed, 552 insertions(+), 26 deletions(-) diff --git a/bindings/python/rapier-py-3d/python/rapier3d/_rapier3d.pyi b/bindings/python/rapier-py-3d/python/rapier3d/_rapier3d.pyi index 1b984e27f..caf0c29d2 100644 --- a/bindings/python/rapier-py-3d/python/rapier3d/_rapier3d.pyi +++ b/bindings/python/rapier-py-3d/python/rapier3d/_rapier3d.pyi @@ -758,6 +758,10 @@ class MultibodyLink: def coords(self) -> list[float]: ... @property def joint_rot(self) -> Rotation3: ... + @property + def assembly_id(self) -> int: ... + @property + def ndofs(self) -> int: ... class Multibody: @property @@ -772,6 +776,16 @@ class Multibody: def set_self_contacts_enabled(self, v: bool) -> None: ... def damping(self) -> list[float]: ... def set_damping(self, values: list[float]) -> None: ... + def armature(self) -> list[float]: ... + def set_armature(self, values: list[float]) -> None: ... + def generalized_position(self) -> list[float]: ... + def set_link_motor( + self, link_id: int, axis: JointAxis, target_pos: float, + target_vel: float, stiffness: float, damping: float, + ) -> None: ... + def set_link_motor_max_force(self, link_id: int, axis: JointAxis, max_force: float) -> None: ... + def set_link_motor_model(self, link_id: int, axis: JointAxis, model: MotorModel) -> None: ... + def link_motor(self, link_id: int, axis: JointAxis) -> JointMotor | None: ... def generalized_velocity(self) -> list[float]: ... def set_generalized_velocity(self, values: list[float]) -> None: ... def apply_displacements(self, displacements: Sequence[float]) -> None: ... @@ -1052,7 +1066,21 @@ class MjcfActuatorHandle: @property def name(self) -> str | None: ... @property + def kind(self) -> str: ... + @property + def joint_name(self) -> str | None: ... + @property def joint(self) -> ImpulseJointHandle | MultibodyJointHandle | None: ... + @property + def gear(self) -> list[float]: ... + @property + def ctrl_range(self) -> list[float] | None: ... + @property + def force_range(self) -> list[float] | None: ... + @property + def kp(self) -> float | None: ... + @property + def kv(self) -> float | None: ... class MjcfContactHooks: def filter_contact_pair(self, ctx: PairFilterContext) -> SolverFlags | None: ... @@ -1068,6 +1096,14 @@ class MjcfRobotHandles: @property def actuators(self) -> list[MjcfActuatorHandle]: ... @property + def body_names(self) -> list[str | None]: ... + @property + def joint_names(self) -> list[str | None]: ... + @property + def body_name_to_idx(self) -> dict[str, int]: ... + @property + def joint_name_to_idx(self) -> dict[str, int]: ... + @property def keyframe_names(self) -> list[str | None]: ... def apply_controls( self, @@ -1096,6 +1132,14 @@ class MjcfRobot: def gravity(self) -> Vec3: ... @property def keyframe_names(self) -> list[str | None]: ... + @property + def body_names(self) -> list[str | None]: ... + @property + def joint_names(self) -> list[str | None]: ... + @property + def body_name_to_idx(self) -> dict[str, int]: ... + @property + def joint_name_to_idx(self) -> dict[str, int]: ... def append_transform(self, transform: IsometryLike) -> None: ... def insert_using_impulse_joints( self, bodies: RigidBodySet, colliders: ColliderSet, impulse_joints: ImpulseJointSet diff --git a/bindings/python/rapier-py-3d/src/joints.rs b/bindings/python/rapier-py-3d/src/joints.rs index 3815fede4..54d0f8a8c 100644 --- a/bindings/python/rapier-py-3d/src/joints.rs +++ b/bindings/python/rapier-py-3d/src/joints.rs @@ -3265,6 +3265,29 @@ impl MultibodyLink { fn joint_rot(&self) -> Rotation3 { Rotation3(self.0.joint().joint_rot().into()) } + /// Index of this link's first degree of freedom in the + /// articulation-wide generalized vectors. + /// + /// The link owns the :py:attr:`ndofs` entries starting at this + /// index in :py:meth:`Multibody.generalized_position`, + /// :py:meth:`Multibody.generalized_velocity`, and friends. + #[getter] + fn assembly_id(&self) -> usize { + self.0.assembly_id() + } + /// Number of degrees of freedom of this link's joint. + #[getter] + fn ndofs(&self) -> usize { + self.0.joint().ndofs() + } +} + +/// Indices, in the 6-entry spatial layout of +/// `MultibodyJoint::coords`, of the joint's unlocked axes, in the +/// order the solver assigns its degrees of freedom. +fn free_dof_axes(joint: &rapier::dynamics::MultibodyJoint) -> impl Iterator { + let locked = joint.data.locked_axes.bits(); + (0..rapier::math::SPATIAL_DIM).filter(move |i| locked & (1u8 << *i) == 0) } // ================================================================= @@ -3330,6 +3353,27 @@ impl Multibody { Ok(f(mb)) }) } + /// Run `f` on link `link_id`, or raise `IndexError` if there is no + /// such link. + fn with_link_mut( + &mut self, + link_id: usize, + f: impl FnOnce(&mut rapier::dynamics::MultibodyLink), + ) -> PyResult<()> { + self.with_mut(|mb| match mb.link_mut(link_id) { + Some(link) => { + f(link); + Ok(()) + } + None => Err(link_index_error(link_id)), + })? + } +} + +/// The `IndexError` raised when a link id does not name a link of the +/// articulation. +fn link_index_error(link_id: usize) -> PyErr { + crate::pyo3::exceptions::PyIndexError::new_err(format!("no multibody link with id {link_id}")) } #[pymethods] @@ -3395,6 +3439,119 @@ impl Multibody { Ok(()) })? } + /// Per-degree-of-freedom armature (reflected rotor inertia, + /// length :attr:`ndofs`). + /// + /// Armature is added straight to the generalized mass-matrix + /// diagonal, which is what makes a stiff position motor stable at + /// large gains. + fn armature(&self) -> PyResult> { + self.with_ref(|mb| mb.armature().iter().copied().collect()) + } + /// Set the per-DOF armature values. + /// + /// :raises ValueError: If ``values`` length differs from `ndofs`. + fn set_armature(&mut self, values: Vec) -> PyResult<()> { + self.with_mut(|mb| { + let a = mb.armature_mut(); + if values.len() != a.len() { + return Err(PyValueError::new_err(format!( + "expected {} armature values (ndofs), got {}", + a.len(), + values.len() + ))); + } + for (i, v) in values.iter().enumerate() { + a[i] = *v; + } + Ok(()) + })? + } + /// Generalized position vector (one entry per DOF). + /// + /// Entry ``link.assembly_id + k`` is the ``k``-th unlocked + /// coordinate of that link's joint, so the layout matches + /// :py:meth:`generalized_velocity` and the vectors accepted by + /// :py:meth:`apply_displacements`. + fn generalized_position(&self) -> PyResult> { + self.with_ref(|mb| { + let mut out = vec![0.0; mb.ndofs()]; + for link in mb.links() { + let joint = link.joint(); + let coords = joint.coords(); + for (k, axis) in free_dof_axes(joint).enumerate() { + out[link.assembly_id() + k] = coords[axis]; + } + } + out + }) + } + /// Fully configure the motor on ``axis`` of link ``link_id``'s + /// joint (the reduced-coordinate counterpart of + /// :py:meth:`GenericJoint.set_motor`). + /// + /// :raises IndexError: If ``link_id`` is out of range. + fn set_link_motor( + &mut self, + link_id: usize, + axis: JointAxis, + target_pos: Real, + target_vel: Real, + stiffness: Real, + damping: Real, + ) -> PyResult<()> { + self.with_link_mut(link_id, |link| { + link.joint + .data + .set_motor(axis.to_rapier(), target_pos, target_vel, stiffness, damping); + }) + } + /// Clamp the maximum force the motor on ``axis`` of link + /// ``link_id`` can apply. + /// + /// :raises IndexError: If ``link_id`` is out of range. + fn set_link_motor_max_force( + &mut self, + link_id: usize, + axis: JointAxis, + max_force: Real, + ) -> PyResult<()> { + self.with_link_mut(link_id, |link| { + link.joint + .data + .set_motor_max_force(axis.to_rapier(), max_force); + }) + } + /// Select the motor model on ``axis`` of link ``link_id`` (see + /// :class:`MotorModel`). + /// + /// :raises IndexError: If ``link_id`` is out of range. + fn set_link_motor_model( + &mut self, + link_id: usize, + axis: JointAxis, + model: MotorModel, + ) -> PyResult<()> { + self.with_link_mut(link_id, |link| { + link.joint + .data + .set_motor_model(axis.to_rapier(), model.to_rapier()); + }) + } + /// Return the motor configured on ``axis`` of link ``link_id``, + /// if any. + /// + /// :raises IndexError: If ``link_id`` is out of range. + fn link_motor(&self, link_id: usize, axis: JointAxis) -> PyResult> { + self.with_ref(|mb| { + let link = mb.link(link_id).ok_or_else(|| link_index_error(link_id))?; + Ok(link + .joint() + .data + .motor(axis.to_rapier()) + .map(JointMotor::from_rapier)) + })? + } /// Generalized velocity vector (one entry per DOF). fn generalized_velocity(&self) -> PyResult> { self.with_ref(|mb| mb.generalized_velocity().iter().copied().collect()) diff --git a/bindings/python/rapier-py-3d/src/loaders.rs b/bindings/python/rapier-py-3d/src/loaders.rs index 02c5ac2c8..e8ea58201 100644 --- a/bindings/python/rapier-py-3d/src/loaders.rs +++ b/bindings/python/rapier-py-3d/src/loaders.rs @@ -1321,17 +1321,40 @@ pub struct MjcfJointHandle { /// Handle of one MJCF ```` after insertion. /// /// :ivar name: The actuator's ``name`` attribute, if any. +/// :ivar kind: Actuator subtype, one of ``"Motor"``, ``"Position"``, +/// ``"Velocity"``, ``"IntVelocity"``, ``"Damper"``, ``"General"``. +/// :ivar joint_name: Name of the MJCF joint it drives, if any. /// :ivar joint: The joint the actuator drives: an /// :class:`ImpulseJointHandle` (impulse-joint path), a /// :class:`MultibodyJointHandle` (multibody path), or ``None`` if it /// drives no joint (e.g. a tendon) or its joint was dropped as a loop /// closure. +/// :ivar gear: The six ``gear`` entries. +/// :ivar ctrl_range: ``[min, max]`` from ``ctrlrange``, or ``None``. +/// :ivar force_range: ``[min, max]`` from ``forcerange``, or ``None``. +/// :ivar kp: ``kp`` of a ```` actuator, or ``None``. +/// :ivar kv: ``kv`` of a ```` / ```` actuator, or +/// ``None``. #[pyclass(name = "MjcfActuatorHandle", module = "rapier")] pub struct MjcfActuatorHandle { #[pyo3(get)] pub name: Option, #[pyo3(get)] + pub kind: String, + #[pyo3(get)] + pub joint_name: Option, + #[pyo3(get)] pub joint: crate::pyo3::PyObject, + #[pyo3(get)] + pub gear: Vec, + #[pyo3(get)] + pub ctrl_range: Option<[f64; 2]>, + #[pyo3(get)] + pub force_range: Option<[f64; 2]>, + #[pyo3(get)] + pub kp: Option, + #[pyo3(get)] + pub kv: Option, } #[pymethods] @@ -1376,6 +1399,14 @@ enum MjcfInsertedHandles { /// :ivar actuators: One :class:`MjcfActuatorHandle` per ````, /// in model order (the order of the ``ctrl`` values of /// :meth:`apply_controls`). +/// :ivar body_names: MJCF name of each body, aligned with +/// :py:attr:`bodies`. Copied off the robot at insertion time, since +/// inserting consumes it. +/// :ivar joint_names: MJCF name of each joint, aligned with +/// :py:attr:`joints`. +/// :ivar body_name_to_idx: Body name to index into :py:attr:`bodies`. +/// :ivar joint_name_to_idx: Joint name to index into +/// :py:attr:`joints`. #[pyclass(name = "MjcfRobotHandles", module = "rapier")] pub struct MjcfRobotHandles { #[pyo3(get)] @@ -1386,6 +1417,14 @@ pub struct MjcfRobotHandles { pub equality_joints: Vec>, #[pyo3(get)] pub actuators: Vec>, + #[pyo3(get)] + pub body_names: Vec>, + #[pyo3(get)] + pub joint_names: Vec>, + #[pyo3(get)] + pub body_name_to_idx: std::collections::HashMap, + #[pyo3(get)] + pub joint_name_to_idx: std::collections::HashMap, /// The inserted robot, without its colliders and visual meshes (only its /// metadata is needed by the runtime helpers). robot: Box, @@ -1696,6 +1735,67 @@ pub struct MjcfRobot { pub inner: Option, } +/// The MJCF naming tables, copied out of an `MjcfRobot` so they +/// survive the insertion that consumes it. +struct MjcfNames { + body_names: Vec>, + joint_names: Vec>, + body_name_to_idx: std::collections::HashMap, + joint_name_to_idx: std::collections::HashMap, +} + +impl MjcfNames { + fn collect(robot: &rapier3d_mjcf::MjcfRobot) -> Self { + Self { + body_names: robot.bodies.iter().map(|b| b.name.clone()).collect(), + joint_names: robot.joints.iter().map(|j| j.name.clone()).collect(), + body_name_to_idx: robot.body_name_to_idx.clone(), + joint_name_to_idx: robot.joint_name_to_idx.clone(), + } + } +} + +/// Convert the per-actuator handles of an insertion, mapping each +/// driven joint handle to Python with `conv`. +fn actuator_handles( + py: crate::pyo3::Python<'_>, + actuators: Vec>, + conv: impl Fn(H) -> crate::pyo3::PyObject, +) -> crate::pyo3::PyResult>> { + actuators + .into_iter() + .map(|ah| { + let a = ah.actuator; + crate::pyo3::Py::new( + py, + MjcfActuatorHandle { + name: a.name, + kind: format!("{:?}", a.kind), + joint_name: a.joint, + joint: match ah.joint { + Some(h) => conv(h), + None => py.None(), + }, + gear: a.gear.to_vec(), + ctrl_range: a.ctrl_range, + force_range: a.force_range, + kp: a.kp, + kv: a.kv, + }, + ) + }) + .collect() +} + +impl MjcfRobot { + /// The still-unconsumed robot, or the "already consumed" error. + fn robot(&self) -> crate::pyo3::PyResult<&rapier3d_mjcf::MjcfRobot> { + self.inner + .as_ref() + .ok_or_else(|| crate::errors::MjcfError::new_err("MjcfRobot was already consumed")) + } +} + #[pymethods] impl MjcfRobot { /// Parse an MJCF file and return ``(MjcfRobot, MjcfModel)``. @@ -1743,6 +1843,54 @@ impl MjcfRobot { Ok((MjcfRobot { inner: Some(robot) }, MjcfModel { raw: model })) } + /// MJCF name of every body, in model order. + /// + /// Entry ``i`` lines up with ``MjcfRobotHandles.bodies[i]``. It is + /// ``None`` for bodies the loader synthesized (intermediate links + /// of a multi-joint ````). + /// + /// :raises MjcfError: if this robot has already been consumed. + #[getter] + fn body_names(&self) -> crate::pyo3::PyResult>> { + Ok(self + .robot()? + .bodies + .iter() + .map(|b| b.name.clone()) + .collect()) + } + /// MJCF name of every joint, in model order. + /// + /// Entry ``i`` lines up with ``MjcfRobotHandles.joints[i]``. It is + /// ``None`` for the fixed joints the loader synthesizes to attach + /// jointless bodies to their parent. + /// + /// :raises MjcfError: if this robot has already been consumed. + #[getter] + fn joint_names(&self) -> crate::pyo3::PyResult>> { + Ok(self + .robot()? + .joints + .iter() + .map(|j| j.name.clone()) + .collect()) + } + /// Map from MJCF body name to its index in :py:attr:`body_names`. + /// + /// :raises MjcfError: if this robot has already been consumed. + #[getter] + fn body_name_to_idx(&self) -> crate::pyo3::PyResult> { + Ok(self.robot()?.body_name_to_idx.clone()) + } + /// Map from MJCF joint name to its index in + /// :py:attr:`joint_names`. + /// + /// :raises MjcfError: if this robot has already been consumed. + #[getter] + fn joint_name_to_idx(&self) -> crate::pyo3::PyResult> { + Ok(self.robot()?.joint_name_to_idx.clone()) + } + /// Prepend ``transform`` to the robot's root poses. /// /// Repositions the whole robot before insertion (e.g. to place a @@ -1814,6 +1962,7 @@ impl MjcfRobot { .take() .ok_or_else(|| crate::errors::MjcfError::new_err("MjcfRobot was already consumed"))?; let metadata = MjcfRobotHandles::robot_metadata(&robot); + let names = MjcfNames::collect(&robot); let handles = robot.insert_using_impulse_joints( &mut bodies.0, &mut colliders.0, @@ -1822,6 +1971,7 @@ impl MjcfRobot { { let inserted = handles.clone(); let conv = |h| ImpulseJointHandle(h).into_py(py); + let actuators = actuator_handles(py, handles.actuators, &conv)?; let bodies: Vec> = handles .bodies .into_iter() @@ -1872,19 +2022,6 @@ impl MjcfRobot { .expect("alloc MjcfJointHandle") }) .collect(); - let actuators = handles - .actuators - .into_iter() - .map(|ah| { - crate::pyo3::Py::new( - py, - MjcfActuatorHandle { - name: ah.actuator.name, - joint: ah.joint.map(conv).into_py(py), - }, - ) - }) - .collect::>>()?; crate::pyo3::Py::new( py, MjcfRobotHandles { @@ -1894,6 +2031,10 @@ impl MjcfRobot { actuators, robot: metadata, inserted: MjcfInsertedHandles::Impulse(inserted), + body_names: names.body_names, + joint_names: names.joint_names, + body_name_to_idx: names.body_name_to_idx, + joint_name_to_idx: names.joint_name_to_idx, }, ) } @@ -1929,6 +2070,7 @@ impl MjcfRobot { .inner .take() .ok_or_else(|| crate::errors::MjcfError::new_err("MjcfRobot was already consumed"))?; + let names = MjcfNames::collect(&robot); let opts = options.map(|o| o.0).unwrap_or_default(); let metadata = MjcfRobotHandles::robot_metadata(&robot); let handles = robot.insert_using_multibody_joints( @@ -1941,6 +2083,7 @@ impl MjcfRobot { { let inserted = handles.clone(); let conv = |h: Option<_>| h.map(MultibodyJointHandle).into_py(py); + let actuators = actuator_handles(py, handles.actuators, &conv)?; let bodies: Vec> = handles .bodies .into_iter() @@ -1991,19 +2134,6 @@ impl MjcfRobot { .expect("alloc MjcfJointHandle") }) .collect(); - let actuators = handles - .actuators - .into_iter() - .map(|ah| { - crate::pyo3::Py::new( - py, - MjcfActuatorHandle { - name: ah.actuator.name, - joint: conv(ah.joint.flatten()), - }, - ) - }) - .collect::>>()?; crate::pyo3::Py::new( py, MjcfRobotHandles { @@ -2013,6 +2143,10 @@ impl MjcfRobot { actuators, robot: metadata, inserted: MjcfInsertedHandles::Multibody(inserted), + body_names: names.body_names, + joint_names: names.joint_names, + body_name_to_idx: names.body_name_to_idx, + joint_name_to_idx: names.joint_name_to_idx, }, ) } diff --git a/bindings/python/tests/test_loaders.py b/bindings/python/tests/test_loaders.py index 914ac241d..ecca0c4de 100644 --- a/bindings/python/tests/test_loaders.py +++ b/bindings/python/tests/test_loaders.py @@ -302,6 +302,87 @@ def test_mjcf_insert_using_multibody_joints() -> None: assert len(handles.joints) == 1 +ACTUATOR_PARAMS_MJCF = """ + + + + + + + + + + + + + + +""" + + +def test_mjcf_names_readable_before_insertion() -> None: + """The MJCF naming tables are reachable while the robot is still alive.""" + robot, _model = mjcf_loader.MjcfRobot.from_str(SIMPLE_MJCF) + # Entry 0 is the implicit world body, which has no MJCF name. + assert robot.body_names == [None, "base", "link"] + assert robot.joint_names == ["hinge"] + assert robot.body_name_to_idx["link"] == 2 + assert robot.joint_name_to_idx == {"hinge": 0} + + +def test_mjcf_names_survive_insertion() -> None: + """Insertion consumes the robot but the handles carry the names over.""" + robot, _model = mjcf_loader.MjcfRobot.from_str(SIMPLE_MJCF) + bodies = rapier.RigidBodySet() + colliders = rapier.ColliderSet() + joints = rapier.ImpulseJointSet() + handles = robot.insert_using_impulse_joints(bodies, colliders, joints) + + assert handles.body_names == [None, "base", "link"] + assert handles.joint_names == ["hinge"] + assert handles.joint_name_to_idx == {"hinge": 0} + # The name tables index into the parallel handle lists. + idx = handles.body_name_to_idx["link"] + assert handles.bodies[idx] is not None + with pytest.raises(mjcf_loader.MjcfError): + _ = robot.body_names + + +def test_mjcf_actuator_handles_carry_their_parameters() -> None: + """`` entries reach Python with their parameters and joint handle.""" + robot, _model = mjcf_loader.MjcfRobot.from_str(ACTUATOR_PARAMS_MJCF) + bodies = rapier.RigidBodySet() + colliders = rapier.ColliderSet() + joints = rapier.ImpulseJointSet() + handles = robot.insert_using_impulse_joints(bodies, colliders, joints) + + assert len(handles.actuators) == 1 + a = handles.actuators[0] + assert a.name == "servo" + assert a.kind == "Position" + assert a.joint_name == "hinge" + assert a.kp == pytest.approx(30.0) + assert a.ctrl_range == pytest.approx([-1.0, 1.0]) + assert a.force_range == pytest.approx([-9.0, 9.0]) + assert isinstance(a.joint, rapier.ImpulseJointHandle) + assert a.joint == handles.joints[0].joint + + +def test_mjcf_actuator_handles_multibody_path() -> None: + """Same on the multibody path, where the joint handle is Optional.""" + robot, _model = mjcf_loader.MjcfRobot.from_str(ACTUATOR_PARAMS_MJCF) + bodies = rapier.RigidBodySet() + colliders = rapier.ColliderSet() + mb = rapier.MultibodyJointSet() + impulse = rapier.ImpulseJointSet() + handles = robot.insert_using_multibody_joints(bodies, colliders, mb, impulse) + + a = handles.actuators[0] + assert a.name == "servo" + assert a.joint_name == "hinge" + assert isinstance(a.joint, rapier.MultibodyJointHandle) + + def test_mjcf_consumed_after_insert() -> None: """Inserting consumes the robot; a second insert raises MjcfError.""" robot, _model = mjcf_loader.MjcfRobot.from_str(SIMPLE_MJCF) diff --git a/bindings/python/tests/test_multibody.py b/bindings/python/tests/test_multibody.py index f597964ff..f4424bad2 100644 --- a/bindings/python/tests/test_multibody.py +++ b/bindings/python/tests/test_multibody.py @@ -290,3 +290,113 @@ def failing(link): w.multibody_joints.inverse_kinematics_for_link( w.rigid_bodies, end_effector, target, options, joint_can_move=failing ) + + +# ---- Writing multibody state ---------------------------------------------- + + +def _pendulum(ns, n_links=2): + """A fixed-root chain of `n_links` revolute links around the local Y axis. + + Returns `(world, multibody, link_handles)` after one step, so the root has + already collapsed from its initial 6-DOF free joint to 0 DOF. + """ + w = ns.PhysicsWorld(gravity=(0, 0, 0)) + handles = [w.rigid_bodies.insert(ns.RigidBody.fixed().build())] + last_joint = None + for i in range(n_links): + h = w.rigid_bodies.insert( + ns.RigidBody.dynamic(translation=(float(i + 1), 0, 0)).build() + ) + w.colliders.insert_with_parent( + ns.Collider.cuboid(0.4, 0.4, 0.4).density(1.0).build(), + h, w.rigid_bodies, + ) + last_joint = w.multibody_joints.insert( + handles[-1], h, + ns.RevoluteJoint.builder(axis=(0, 1, 0)) + .local_anchor1((0.5, 0, 0)) + .local_anchor2((-0.5, 0, 0)) + .build(), + ) + handles.append(h) + w.step() + mb = w.multibody_joints.multibody(last_joint) + assert mb is not None + return w, mb, handles + + +def test_multibody_link_assembly_id_and_ndofs(ns): + """Each link reports where its DOFs sit in the generalized vectors.""" + _w, mb, _ = _pendulum(ns, n_links=3) + assert mb.ndofs == 3 + # Root is fixed: no DOFs. Then one revolute DOF per link, in order. + assert [mb.get_link(i).ndofs for i in range(4)] == [0, 1, 1, 1] + assert [mb.get_link(i).assembly_id for i in range(4)] == [0, 0, 1, 2] + + +def test_multibody_generalized_position_starts_at_zero(ns): + """A freshly built chain sits at the origin of its joint coordinates.""" + _w, mb, _ = _pendulum(ns, n_links=2) + q = mb.generalized_position() + assert len(q) == mb.ndofs == 2 + assert all(abs(x) < 1e-5 for x in q) + + +def test_multibody_apply_displacements_round_trips(ns): + """`apply_displacements` moves the joint coordinates by exactly `disp`.""" + w, mb, _ = _pendulum(ns, n_links=2) + target = [0.3, -0.7] + q0 = mb.generalized_position() + mb.apply_displacements([target[i] - q0[i] for i in range(mb.ndofs)]) + mb.forward_kinematics(w.rigid_bodies, False) + mb.update_rigid_bodies(w.rigid_bodies, False) + + q = mb.generalized_position() + assert q == pytest.approx(target, abs=1e-6) + # The rigid bodies followed: the second link is no longer at y = 0. + link2 = w.rigid_bodies.get(mb.get_link(2).rigid_body) + assert abs(link2.translation[2]) > 1e-3 + + +def test_multibody_armature_round_trips(ns): + """`set_armature` / `armature` round-trip one value per DOF.""" + _w, mb, _ = _pendulum(ns, n_links=2) + assert mb.armature() == pytest.approx([0.0, 0.0]) + mb.set_armature([0.25, 0.5]) + assert mb.armature() == pytest.approx([0.25, 0.5]) + with pytest.raises(ValueError): + mb.set_armature([1.0, 2.0, 3.0]) + + +def test_multibody_link_motor_reaches_its_target(ns): + """A position motor on a link's joint drives that DOF to its target.""" + w, mb, _ = _pendulum(ns, n_links=1) + target = 0.4 + mb.set_link_motor(1, ns.JointAxis.ANG_X, target, 0.0, 4500.0, 450.0) + mb.set_link_motor_model(1, ns.JointAxis.ANG_X, ns.MotorModel.FORCE_BASED) + mb.set_link_motor_max_force(1, ns.JointAxis.ANG_X, 1.0e4) + + motor = mb.link_motor(1, ns.JointAxis.ANG_X) + assert motor is not None + assert motor.target_pos == pytest.approx(target) + assert motor.stiffness == pytest.approx(4500.0) + assert motor.damping == pytest.approx(450.0) + assert motor.max_force == pytest.approx(1.0e4) + + for _ in range(400): + w.step() + assert mb.generalized_position()[0] == pytest.approx(target, abs=1e-3) + assert abs(mb.generalized_velocity()[0]) < 1e-2 + + +def test_multibody_link_motor_rejects_unknown_link(ns): + _w, mb, _ = _pendulum(ns, n_links=1) + with pytest.raises(IndexError): + mb.set_link_motor(99, ns.JointAxis.ANG_X, 0.0, 0.0, 1.0, 1.0) + with pytest.raises(IndexError): + mb.set_link_motor_max_force(99, ns.JointAxis.ANG_X, 1.0) + with pytest.raises(IndexError): + mb.set_link_motor_model(99, ns.JointAxis.ANG_X, ns.MotorModel.FORCE_BASED) + with pytest.raises(IndexError): + mb.link_motor(99, ns.JointAxis.ANG_X) From cc5927ea74820948b89cb22c32d630e545ffc90f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?S=C3=A9bastien=20Crozet?= Date: Fri, 25 Sep 2026 18:55:12 +0200 Subject: [PATCH 2/3] fix link to the soft-body blog-post --- website/docs/user_guides/templates/soft_bodies.mdx | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/website/docs/user_guides/templates/soft_bodies.mdx b/website/docs/user_guides/templates/soft_bodies.mdx index 6b3eaf85e..a30b54364 100644 --- a/website/docs/user_guides/templates/soft_bodies.mdx +++ b/website/docs/user_guides/templates/soft_bodies.mdx @@ -7,7 +7,7 @@ sidebar_label: Soft-bodies import Tabs from '@theme/Tabs'; import TabItem from '@theme/TabItem'; -_For a high-level overview of the methods behind our soft-body implementation, see our [blog-post](https://dimforge.com/blog/2026/09/25/soft-bodies-in-the-rapier-physics-engine)._ +_For a high-level overview of the methods behind our soft-body implementation, see our [blog-post](https://dimforge.com/blog/2026/09/25/advanced-soft-bodies-for-games-in-the-rapier-physics-engine)._ Rigid-bodies can't deform in any way, which is precisely what makes them cheap and easy to control. Soft-bodies, aka. _deformable bodies_, are made for everything that must bend, stretch, or squash: ropes, cloth, jelly, balloons, the From 1f6e49c23d106618c91a12b34d429c028f8e6f06 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?S=C3=A9bastien=20Crozet?= Date: Sun, 27 Sep 2026 19:23:51 +0200 Subject: [PATCH 3/3] =?UTF-8?q?chore:=E2=80=AFclippy=20fixes?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- bindings/python/rapier-py-3d/src/loaders.rs | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/bindings/python/rapier-py-3d/src/loaders.rs b/bindings/python/rapier-py-3d/src/loaders.rs index e8ea58201..63e5e597d 100644 --- a/bindings/python/rapier-py-3d/src/loaders.rs +++ b/bindings/python/rapier-py-3d/src/loaders.rs @@ -1971,7 +1971,7 @@ impl MjcfRobot { { let inserted = handles.clone(); let conv = |h| ImpulseJointHandle(h).into_py(py); - let actuators = actuator_handles(py, handles.actuators, &conv)?; + let actuators = actuator_handles(py, handles.actuators, conv)?; let bodies: Vec> = handles .bodies .into_iter() @@ -2083,7 +2083,7 @@ impl MjcfRobot { { let inserted = handles.clone(); let conv = |h: Option<_>| h.map(MultibodyJointHandle).into_py(py); - let actuators = actuator_handles(py, handles.actuators, &conv)?; + let actuators = actuator_handles(py, handles.actuators, conv)?; let bodies: Vec> = handles .bodies .into_iter()