Implement multibody joints and the new solver

src/dynamics/joint/multibody_joint/mod.rs

@@ -0,0 +1,15 @@
+//! MultibodyJoints using the reduced-coordinates formalism or using constraints.
+
+pub use self::multibody::Multibody;
+pub use self::multibody_joint::MultibodyJoint;
+pub use self::multibody_joint_set::{MultibodyIndex, MultibodyJointHandle, MultibodyJointSet};
+pub use self::multibody_link::MultibodyLink;
+pub use self::unit_multibody_joint::{unit_joint_limit_constraint, unit_joint_motor_constraint};
+
+mod multibody;
+mod multibody_joint_set;
+mod multibody_link;
+mod multibody_workspace;
+
+mod multibody_joint;
+mod unit_multibody_joint;

src/dynamics/joint/multibody_joint/multibody_joint.rs

@@ -0,0 +1,571 @@
+use crate::dynamics::solver::AnyJointVelocityConstraint;
+use crate::dynamics::{
+    joint, FixedJoint, IntegrationParameters, JointAxesMask, JointData, Multibody, MultibodyLink,
+    RigidBodyVelocity,
+};
+use crate::math::{
+    Isometry, JacobianSliceMut, Matrix, Real, Rotation, SpacialVector, Translation, Vector,
+    ANG_DIM, DIM, SPATIAL_DIM,
+};
+use crate::utils::WCross;
+use na::{DVector, DVectorSliceMut};
+#[cfg(feature = "dim3")]
+use {
+    crate::utils::WCrossMatrix,
+    na::{UnitQuaternion, Vector3, VectorSlice3},
+};
+
+#[derive(Copy, Clone, Debug)]
+pub struct MultibodyJoint {
+    pub data: JointData,
+    pub(crate) coords: SpacialVector<Real>,
+    pub(crate) joint_rot: Rotation<Real>,
+    jacobian_v: Matrix<Real>,
+    jacobian_dot_v: Matrix<Real>,
+    jacobian_dot_veldiff_v: Matrix<Real>,
+}
+
+#[cfg(feature = "dim2")]
+fn revolute_locked_axes() -> JointAxesMask {
+    JointAxesMask::X | JointAxesMask::Y
+}
+
+#[cfg(feature = "dim3")]
+fn revolute_locked_axes() -> JointAxesMask {
+    JointAxesMask::X
+        | JointAxesMask::Y
+        | JointAxesMask::Z
+        | JointAxesMask::ANG_Y
+        | JointAxesMask::ANG_Z
+}
+
+impl MultibodyJoint {
+    pub fn new(data: JointData) -> Self {
+        Self {
+            data,
+            coords: na::zero(),
+            joint_rot: Rotation::identity(),
+            jacobian_v: na::zero(),
+            jacobian_dot_v: na::zero(),
+            jacobian_dot_veldiff_v: na::zero(),
+        }
+    }
+
+    pub(crate) fn free(pos: Isometry<Real>) -> Self {
+        let mut result = Self::new(JointData::default());
+        result.set_free_pos(pos);
+        result
+    }
+
+    pub(crate) fn fixed(pos: Isometry<Real>) -> Self {
+        Self::new(FixedJoint::new().local_frame1(pos).into())
+    }
+
+    pub(crate) fn set_free_pos(&mut self, pos: Isometry<Real>) {
+        self.coords
+            .fixed_rows_mut::<DIM>(0)
+            .copy_from(&pos.translation.vector);
+        self.joint_rot = pos.rotation;
+    }
+
+    pub fn local_joint_rot(&self) -> &Rotation<Real> {
+        &self.joint_rot
+    }
+
+    fn num_free_lin_dofs(&self) -> usize {
+        let locked_bits = self.data.locked_axes.bits();
+        DIM - (locked_bits & ((1 << DIM) - 1)).count_ones() as usize
+    }
+
+    /// The number of degrees of freedom allowed by the multibody_joint.
+    pub fn ndofs(&self) -> usize {
+        SPATIAL_DIM - self.data.locked_axes.bits().count_ones() as usize
+    }
+
+    /// The position of the multibody link containing this multibody_joint relative to its parent.
+    pub fn body_to_parent(&self) -> Isometry<Real> {
+        if self.data.locked_axes == revolute_locked_axes() {
+            // FIXME: this is a special case for the revolute joint.
+            // We have the mathematical formulation ready that works in the general case, but its
+            // implementation will take some time. So let’s make a special case for the alpha
+            // release and fix is soon after.
+            self.data.local_frame1.translation
+                * self.joint_rot
+                * self.data.local_frame2.translation.inverse()
+        } else {
+            let locked_bits = self.data.locked_axes.bits();
+            let mut transform = self.joint_rot * self.data.local_frame2.inverse();
+
+            for i in 0..DIM {
+                if (locked_bits & (1 << i)) == 0 {
+                    transform = Translation::from(Vector::ith(i, self.coords[i])) * transform;
+                }
+            }
+
+            self.data.local_frame1 * transform
+        }
+    }
+
+    /// Integrate the position of this multibody_joint.
+    pub fn integrate(&mut self, dt: Real, vels: &[Real]) {
+        if self.data.locked_axes == revolute_locked_axes() {
+            // FIXME: this is a special case for the revolute joint.
+            // We have the mathematical formulation ready that works in the general case, but its
+            // implementation will take some time. So let’s make a special case for the alpha
+            // release and fix is soon after.
+            #[cfg(feature = "dim3")]
+            let axis = self.data.local_frame1 * Vector::x_axis();
+            self.coords[DIM] += vels[0] * dt;
+
+            #[cfg(feature = "dim2")]
+            {
+                self.joint_rot = Rotation::from_angle(self.coords[DIM]);
+            }
+            #[cfg(feature = "dim3")]
+            {
+                self.joint_rot = Rotation::from_axis_angle(&axis, self.coords[DIM]);
+            }
+        } else {
+            let locked_bits = self.data.locked_axes.bits();
+            let mut curr_free_dof = 0;
+
+            for i in 0..DIM {
+                if (locked_bits & (1 << i)) == 0 {
+                    self.coords[i] += vels[curr_free_dof] * dt;
+                    curr_free_dof += 1;
+                }
+            }
+
+            let locked_ang_bits = locked_bits >> DIM;
+            let num_free_ang_dofs = ANG_DIM - locked_ang_bits.count_ones() as usize;
+            match num_free_ang_dofs {
+                0 => { /* No free dofs. */ }
+                1 => {
+                    todo!()
+                }
+                2 => {
+                    todo!()
+                }
+                #[cfg(feature = "dim3")]
+                3 => {
+                    let angvel = Vector3::from_row_slice(&vels[curr_free_dof..curr_free_dof + 3]);
+                    let disp = UnitQuaternion::new_eps(angvel * dt, 0.0);
+                    self.joint_rot = disp * self.joint_rot;
+                }
+                _ => unreachable!(),
+            }
+        }
+    }
+
+    /// Apply a displacement to the multibody_joint.
+    pub fn apply_displacement(&mut self, disp: &[Real]) {
+        self.integrate(1.0, disp);
+    }
+
+    /// Update the jacobians of this multibody_joint.
+    pub fn update_jacobians(&mut self, vels: &[Real]) {
+        if self.data.locked_axes == revolute_locked_axes() {
+            // FIXME: this is a special case for the revolute joint.
+            // We have the mathematical formulation ready that works in the general case, but its
+            // implementation will take some time. So let’s make a special case for the alpha
+            // release and fix is soon after.
+            #[cfg(feature = "dim2")]
+            let axis = 1.0;
+            #[cfg(feature = "dim3")]
+            let axis = self.data.local_frame1 * Vector::x_axis();
+            let body_shift = self.data.local_frame2.translation.vector;
+            let shift = self.joint_rot * -body_shift;
+            let shift_dot_veldiff = axis.gcross(shift);
+
+            #[cfg(feature = "dim2")]
+            {
+                self.jacobian_v.column_mut(0).copy_from(&axis.gcross(shift));
+            }
+            #[cfg(feature = "dim3")]
+            {
+                self.jacobian_v.column_mut(0).copy_from(&axis.gcross(shift));
+            }
+            self.jacobian_dot_veldiff_v
+                .column_mut(0)
+                .copy_from(&axis.gcross(shift_dot_veldiff));
+            self.jacobian_dot_v
+                .column_mut(0)
+                .copy_from(&(axis.gcross(shift_dot_veldiff) * vels[0]));
+        } else {
+            let locked_bits = self.data.locked_axes.bits();
+            let locked_ang_bits = locked_bits >> DIM;
+            let num_free_ang_dofs = ANG_DIM - locked_ang_bits.count_ones() as usize;
+            match num_free_ang_dofs {
+                0 => { /* No free dofs. */ }
+                1 => {
+                    todo!()
+                }
+                2 => {
+                    todo!()
+                }
+                #[cfg(feature = "dim3")]
+                3 => {
+                    let num_free_lin_dofs = self.num_free_lin_dofs();
+                    let inv_frame2 = self.data.local_frame2.inverse();
+                    let shift = self.joint_rot * inv_frame2.translation.vector;
+                    let angvel =
+                        VectorSlice3::from_slice(&vels[num_free_lin_dofs..num_free_lin_dofs + 3]);
+                    let inv_rotmat2 = inv_frame2.rotation.to_rotation_matrix().into_inner();
+
+                    self.jacobian_v = inv_rotmat2 * shift.gcross_matrix().transpose();
+                    self.jacobian_dot_v =
+                        inv_rotmat2 * angvel.cross(&shift).gcross_matrix().transpose();
+                }
+                _ => unreachable!(),
+            }
+        }
+    }
+
+    /// Sets in `out` the non-zero entries of the multibody_joint jacobian transformed by `transform`.
+    pub fn jacobian(&self, transform: &Isometry<Real>, out: &mut JacobianSliceMut<Real>) {
+        if self.data.locked_axes == revolute_locked_axes() {
+            // FIXME: this is a special case for the revolute joint.
+            // We have the mathematical formulation ready that works in the general case, but its
+            // implementation will take some time. So let’s make a special case for the alpha
+            // release and fix is soon after.
+            #[cfg(feature = "dim2")]
+            let axis = 1.0;
+            #[cfg(feature = "dim3")]
+            let axis = self.data.local_frame1 * Vector::x();
+            let jacobian = RigidBodyVelocity::new(self.jacobian_v.column(0).into_owned(), axis);
+            out.copy_from(jacobian.transformed(transform).as_vector())
+        } else {
+            let locked_bits = self.data.locked_axes.bits();
+            let mut curr_free_dof = 0;
+
+            for i in 0..DIM {
+                if (locked_bits & (1 << i)) == 0 {
+                    let transformed_axis = transform * self.data.local_frame1 * Vector::ith(i, 1.0);
+                    out.fixed_slice_mut::<DIM, 1>(0, curr_free_dof)
+                        .copy_from(&transformed_axis);
+                    curr_free_dof += 1;
+                }
+            }
+
+            let locked_ang_bits = locked_bits >> DIM;
+            let num_free_ang_dofs = ANG_DIM - locked_ang_bits.count_ones() as usize;
+            match num_free_ang_dofs {
+                0 => { /* No free dofs. */ }
+                1 => {
+                    todo!()
+                }
+                2 => {
+                    todo!()
+                }
+                #[cfg(feature = "dim3")]
+                3 => {
+                    let rotmat = transform.rotation.to_rotation_matrix();
+                    out.fixed_slice_mut::<3, 3>(0, curr_free_dof)
+                        .copy_from(&(rotmat * self.jacobian_v));
+                    out.fixed_slice_mut::<3, 3>(3, curr_free_dof)
+                        .copy_from(rotmat.matrix());
+                }
+                _ => unreachable!(),
+            }
+        }
+    }
+
+    /// Sets in `out` the non-zero entries of the time-derivative of the multibody_joint jacobian transformed by `transform`.
+    pub fn jacobian_dot(&self, transform: &Isometry<Real>, out: &mut JacobianSliceMut<Real>) {
+        if self.data.locked_axes == revolute_locked_axes() {
+            // FIXME: this is a special case for the revolute joint.
+            // We have the mathematical formulation ready that works in the general case, but its
+            // implementation will take some time. So let’s make a special case for the alpha
+            // release and fix is soon after.
+            let jacobian = RigidBodyVelocity::from_vectors(
+                self.jacobian_dot_v.column(0).into_owned(),
+                na::zero(),
+            );
+            out.copy_from(jacobian.transformed(transform).as_vector())
+        } else {
+            let locked_bits = self.data.locked_axes.bits();
+            let locked_ang_bits = locked_bits >> DIM;
+            let num_free_ang_dofs = ANG_DIM - locked_ang_bits.count_ones() as usize;
+            match num_free_ang_dofs {
+                0 => { /* No free dofs. */ }
+                1 => {
+                    todo!()
+                }
+                2 => {
+                    todo!()
+                }
+                #[cfg(feature = "dim3")]
+                3 => {
+                    let num_free_lin_dofs = self.num_free_lin_dofs();
+                    let rotmat = transform.rotation.to_rotation_matrix();
+                    out.fixed_slice_mut::<3, 3>(0, num_free_lin_dofs)
+                        .copy_from(&(rotmat * self.jacobian_dot_v));
+                }
+                _ => unreachable!(),
+            }
+        }
+    }
+
+    /// Sets in `out` the non-zero entries of the velocity-derivative of the time-derivative of the multibody_joint jacobian transformed by `transform`.
+    pub fn jacobian_dot_veldiff_mul_coordinates(
+        &self,
+        transform: &Isometry<Real>,
+        acc: &[Real],
+        out: &mut JacobianSliceMut<Real>,
+    ) {
+        if self.data.locked_axes == revolute_locked_axes() {
+            // FIXME: this is a special case for the revolute joint.
+            // We have the mathematical formulation ready that works in the general case, but its
+            // implementation will take some time. So let’s make a special case for the alpha
+            // release and fix is soon after.
+            let jacobian = RigidBodyVelocity::from_vectors(
+                self.jacobian_dot_veldiff_v.column(0).into_owned(),
+                na::zero(),
+            );
+            out.copy_from((jacobian.transformed(transform) * acc[0]).as_vector())
+        } else {
+            let locked_bits = self.data.locked_axes.bits();
+            let locked_ang_bits = locked_bits >> DIM;
+            let num_free_ang_dofs = ANG_DIM - locked_ang_bits.count_ones() as usize;
+            match num_free_ang_dofs {
+                0 => { /* No free dofs. */ }
+                1 => {
+                    todo!()
+                }
+                2 => {
+                    todo!()
+                }
+                #[cfg(feature = "dim3")]
+                3 => {
+                    let num_free_lin_dofs = self.num_free_lin_dofs();
+                    let angvel =
+                        Vector3::from_row_slice(&acc[num_free_lin_dofs..num_free_lin_dofs + 3]);
+                    let rotmat = transform.rotation.to_rotation_matrix();
+                    let res = rotmat * angvel.gcross_matrix() * self.jacobian_v;
+                    out.fixed_slice_mut::<3, 3>(0, num_free_lin_dofs)
+                        .copy_from(&res);
+                }
+                _ => unreachable!(),
+            }
+        }
+    }
+
+    /// Multiply the multibody_joint jacobian by generalized velocities to obtain the
+    /// relative velocity of the multibody link containing this multibody_joint.
+    pub fn jacobian_mul_coordinates(&self, acc: &[Real]) -> RigidBodyVelocity {
+        if self.data.locked_axes == revolute_locked_axes() {
+            // FIXME: this is a special case for the revolute joint.
+            // We have the mathematical formulation ready that works in the general case, but its
+            // implementation will take some time. So let’s make a special case for the alpha
+            // release and fix is soon after.
+            #[cfg(feature = "dim2")]
+            let axis = 1.0;
+            #[cfg(feature = "dim3")]
+            let axis = self.data.local_frame1 * Vector::x();
+            RigidBodyVelocity::new(self.jacobian_v.column(0).into_owned(), axis) * acc[0]
+        } else {
+            let locked_bits = self.data.locked_axes.bits();
+            let mut result = RigidBodyVelocity::zero();
+            let mut curr_free_dof = 0;
+
+            for i in 0..DIM {
+                if (locked_bits & (1 << i)) == 0 {
+                    result.linvel += self.data.local_frame1 * Vector::ith(i, acc[curr_free_dof]);
+                    curr_free_dof += 1;
+                }
+            }
+
+            let locked_ang_bits = locked_bits >> DIM;
+            let num_free_ang_dofs = ANG_DIM - locked_ang_bits.count_ones() as usize;
+            match num_free_ang_dofs {
+                0 => { /* No free dofs. */ }
+                1 => {
+                    todo!()
+                }
+                2 => {
+                    todo!()
+                }
+                #[cfg(feature = "dim3")]
+                3 => {
+                    let angvel = Vector3::from_row_slice(&acc[curr_free_dof..curr_free_dof + 3]);
+                    let linvel = self.jacobian_v * angvel;
+                    result += RigidBodyVelocity::new(linvel, angvel);
+                }
+                _ => unreachable!(),
+            }
+            result
+        }
+    }
+
+    /// Multiply the multibody_joint jacobian by generalized accelerations to obtain the
+    /// relative acceleration of the multibody link containing this multibody_joint.
+    pub fn jacobian_dot_mul_coordinates(&self, acc: &[Real]) -> RigidBodyVelocity {
+        if self.data.locked_axes == revolute_locked_axes() {
+            // FIXME: this is a special case for the revolute joint.
+            // We have the mathematical formulation ready that works in the general case, but its
+            // implementation will take some time. So let’s make a special case for the alpha
+            // release and fix is soon after.
+            RigidBodyVelocity::from_vectors(self.jacobian_dot_v.column(0).into_owned(), na::zero())
+                * acc[0]
+        } else {
+            let locked_bits = self.data.locked_axes.bits();
+
+            let locked_ang_bits = locked_bits >> DIM;
+            let num_free_ang_dofs = ANG_DIM - locked_ang_bits.count_ones() as usize;
+            match num_free_ang_dofs {
+                0 => {
+                    /* No free dofs. */
+                    RigidBodyVelocity::zero()
+                }
+                1 => {
+                    todo!()
+                }
+                2 => {
+                    todo!()
+                }
+                #[cfg(feature = "dim3")]
+                3 => {
+                    let num_free_lin_dofs = self.num_free_lin_dofs();
+                    let angvel =
+                        Vector3::from_row_slice(&acc[num_free_lin_dofs..num_free_lin_dofs + 3]);
+                    let linvel = self.jacobian_dot_v * angvel;
+                    RigidBodyVelocity::new(linvel, na::zero())
+                }
+                _ => unreachable!(),
+            }
+        }
+    }
+
+    /// Fill `out` with the non-zero entries of a damping that can be applied by default to ensure a good stability of the multibody_joint.
+    pub fn default_damping(&self, out: &mut DVectorSliceMut<Real>) {
+        let locked_bits = self.data.locked_axes.bits();
+        let mut curr_free_dof = self.num_free_lin_dofs();
+
+        // A default damping only for the angular dofs
+        for i in DIM..SPATIAL_DIM {
+            if locked_bits & (1 << i) == 0 {
+                // This is a free angular DOF.
+                out[curr_free_dof] = 0.2;
+                curr_free_dof += 1;
+            }
+        }
+    }
+
+    /// Maximum number of velocity constrains that can be generated by this multibody_joint.
+    pub fn num_velocity_constraints(&self) -> usize {
+        let locked_bits = self.data.locked_axes.bits();
+        let limit_bits = self.data.limit_axes.bits();
+        let motor_bits = self.data.motor_axes.bits();
+        let mut num_constraints = 0;
+
+        for i in 0..SPATIAL_DIM {
+            if (locked_bits & (1 << i)) == 0 {
+                if (limit_bits & (1 << i)) != 0 {
+                    num_constraints += 1;
+                }
+                if (motor_bits & (1 << i)) != 0 {
+                    num_constraints += 1;
+                }
+            }
+        }
+
+        num_constraints
+    }
+
+    /// Initialize and generate velocity constraints to enforce, e.g., multibody_joint limits and motors.
+    pub fn velocity_constraints(
+        &self,
+        params: &IntegrationParameters,
+        multibody: &Multibody,
+        link: &MultibodyLink,
+        dof_id: usize,
+        j_id: &mut usize,
+        jacobians: &mut DVector<Real>,
+        constraints: &mut Vec<AnyJointVelocityConstraint>,
+    ) {
+        let locked_bits = self.data.locked_axes.bits();
+        let limit_bits = self.data.limit_axes.bits();
+        let motor_bits = self.data.motor_axes.bits();
+        let mut curr_free_dof = 0;
+
+        for i in 0..DIM {
+            if (locked_bits & (1 << i)) == 0 {
+                if (limit_bits & (1 << i)) != 0 {
+                    joint::unit_joint_limit_constraint(
+                        params,
+                        multibody,
+                        link,
+                        [self.data.limits[i].min, self.data.limits[i].max],
+                        self.coords[i],
+                        dof_id + curr_free_dof,
+                        j_id,
+                        jacobians,
+                        constraints,
+                    );
+                }
+
+                if (motor_bits & (1 << i)) != 0 {
+                    joint::unit_joint_motor_constraint(
+                        params,
+                        multibody,
+                        link,
+                        &self.data.motors[i],
+                        self.coords[i],
+                        dof_id + curr_free_dof,
+                        j_id,
+                        jacobians,
+                        constraints,
+                    );
+                }
+                curr_free_dof += 1;
+            }
+        }
+
+        /*
+        let locked_ang_bits = locked_bits >> DIM;
+        let num_free_ang_dofs = ANG_DIM - locked_ang_bits.count_ones() as usize;
+        match num_free_ang_dofs {
+            0 => { /* No free dofs. */ }
+            1 => {}
+            2 => {
+                todo!()
+            }
+            3 => {}
+            _ => unreachable!(),
+        }
+         */
+        // TODO: we should make special cases for multi-angular-dofs limits/motors
+        for i in DIM..SPATIAL_DIM {
+            if (locked_bits & (1 << i)) == 0 {
+                if (limit_bits & (1 << i)) != 0 {
+                    joint::unit_joint_limit_constraint(
+                        params,
+                        multibody,
+                        link,
+                        [self.data.limits[i].min, self.data.limits[i].max],
+                        self.coords[i],
+                        dof_id + curr_free_dof,
+                        j_id,
+                        jacobians,
+                        constraints,
+                    );
+                }
+
+                if (motor_bits & (1 << i)) != 0 {
+                    joint::unit_joint_motor_constraint(
+                        params,
+                        multibody,
+                        link,
+                        &self.data.motors[i],
+                        self.coords[i],
+                        dof_id + curr_free_dof,
+                        j_id,
+                        jacobians,
+                        constraints,
+                    );
+                }
+                curr_free_dof += 1;
+            }
+        }
+    }
+}

src/dynamics/joint/multibody_joint/multibody_joint_set.rs

@@ -0,0 +1,352 @@
+use crate::data::{Arena, Coarena, ComponentSet, ComponentSetMut};
+use crate::dynamics::joint::MultibodyLink;
+use crate::dynamics::{
+    IslandManager, JointData, Multibody, MultibodyJoint, RigidBodyActivation, RigidBodyHandle,
+    RigidBodyIds, RigidBodyType,
+};
+use crate::geometry::{InteractionGraph, RigidBodyGraphIndex};
+use crate::parry::partitioning::IndexedData;
+use std::ops::Index;
+
+/// The unique handle of an multibody_joint added to a `MultibodyJointSet`.
+#[derive(Copy, Clone, Debug, PartialEq, Eq, Hash)]
+#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
+#[repr(transparent)]
+pub struct MultibodyJointHandle(pub crate::data::arena::Index);
+
+/// The temporary index of a multibody added to a `MultibodyJointSet`.
+#[derive(Copy, Clone, Debug, PartialEq, Eq, Hash)]
+#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
+#[repr(transparent)]
+pub struct MultibodyIndex(pub crate::data::arena::Index);
+
+impl MultibodyJointHandle {
+    /// Converts this handle into its (index, generation) components.
+    pub fn into_raw_parts(self) -> (u32, u32) {
+        self.0.into_raw_parts()
+    }
+
+    /// Reconstructs an handle from its (index, generation) components.
+    pub fn from_raw_parts(id: u32, generation: u32) -> Self {
+        Self(crate::data::arena::Index::from_raw_parts(id, generation))
+    }
+
+    /// An always-invalid rigid-body handle.
+    pub fn invalid() -> Self {
+        Self(crate::data::arena::Index::from_raw_parts(
+            crate::INVALID_U32,
+            crate::INVALID_U32,
+        ))
+    }
+}
+
+impl Default for MultibodyJointHandle {
+    fn default() -> Self {
+        Self::invalid()
+    }
+}
+
+impl IndexedData for MultibodyJointHandle {
+    fn default() -> Self {
+        Self(IndexedData::default())
+    }
+    fn index(&self) -> usize {
+        self.0.index()
+    }
+}
+
+#[derive(Copy, Clone, Debug, PartialEq, Eq)]
+pub struct MultibodyJointLink {
+    pub graph_id: RigidBodyGraphIndex,
+    pub multibody: MultibodyIndex,
+    pub id: usize,
+}
+
+impl Default for MultibodyJointLink {
+    fn default() -> Self {
+        Self {
+            graph_id: RigidBodyGraphIndex::new(crate::INVALID_U32),
+            multibody: MultibodyIndex(crate::data::arena::Index::from_raw_parts(
+                crate::INVALID_U32,
+                crate::INVALID_U32,
+            )),
+            id: 0,
+        }
+    }
+}
+
+/// A set of rigid bodies that can be handled by a physics pipeline.
+pub struct MultibodyJointSet {
+    pub(crate) multibodies: Arena<Multibody>, // NOTE: a Slab would be sufficient.
+    pub(crate) rb2mb: Coarena<MultibodyJointLink>,
+    // NOTE: this is mostly for the island extraction. So perhaps we won’t need
+    //       that any more in the future when we improve our island builder.
+    pub(crate) connectivity_graph: InteractionGraph<RigidBodyHandle, ()>,
+}
+
+impl MultibodyJointSet {
+    /// Create a new empty set of multibodies.
+    pub fn new() -> Self {
+        Self {
+            multibodies: Arena::new(),
+            rb2mb: Coarena::new(),
+            connectivity_graph: InteractionGraph::new(),
+        }
+    }
+
+    pub fn iter(&self) -> impl Iterator<Item = (MultibodyJointHandle, &Multibody, &MultibodyLink)> {
+        self.rb2mb
+            .iter()
+            .filter(|(_, link)| link.id > 0) // The first link of a rigid-body hasn’t been added by the user.
+            .map(|(h, link)| {
+                let mb = &self.multibodies[link.multibody.0];
+                (MultibodyJointHandle(h), mb, mb.link(link.id).unwrap())
+            })
+    }
+
+    /// Inserts a new multibody_joint into this set.
+    pub fn insert(
+        &mut self,
+        body1: RigidBodyHandle,
+        body2: RigidBodyHandle,
+        data: impl Into<JointData>,
+    ) -> Option<MultibodyJointHandle> {
+        let data = data.into();
+        let link1 = self.rb2mb.get(body1.0).copied().unwrap_or_else(|| {
+            let mb_handle = self.multibodies.insert(Multibody::with_root(body1));
+            MultibodyJointLink {
+                graph_id: self.connectivity_graph.graph.add_node(body1),
+                multibody: MultibodyIndex(mb_handle),
+                id: 0,
+            }
+        });
+
+        let link2 = self.rb2mb.get(body2.0).copied().unwrap_or_else(|| {
+            let mb_handle = self.multibodies.insert(Multibody::with_root(body2));
+            MultibodyJointLink {
+                graph_id: self.connectivity_graph.graph.add_node(body2),
+                multibody: MultibodyIndex(mb_handle),
+                id: 0,
+            }
+        });
+
+        if link1.multibody == link2.multibody || link2.id != 0 {
+            // This would introduce an invalid configuration.
+            return None;
+        }
+
+        self.connectivity_graph
+            .graph
+            .add_edge(link1.graph_id, link2.graph_id, ());
+        self.rb2mb.insert(body1.0, link1);
+        self.rb2mb.insert(body2.0, link2);
+
+        let mb2 = self.multibodies.remove(link2.multibody.0).unwrap();
+        let multibody1 = &mut self.multibodies[link1.multibody.0];
+
+        for mb_link2 in mb2.links() {
+            let link = self.rb2mb.get_mut(mb_link2.rigid_body.0).unwrap();
+            link.multibody = link1.multibody;
+            link.id += multibody1.num_links();
+        }
+
+        multibody1.append(mb2, link1.id, MultibodyJoint::new(data));
+
+        // Because each rigid-body can only have one parent link,
+        // we can use the second rigid-body’s handle as the multibody_joint’s
+        // handle.
+        Some(MultibodyJointHandle(body2.0))
+    }
+
+    /// Removes an multibody_joint from this set.
+    pub fn remove<Bodies>(
+        &mut self,
+        handle: MultibodyJointHandle,
+        islands: &mut IslandManager,
+        bodies: &mut Bodies,
+        wake_up: bool,
+    ) where
+        Bodies: ComponentSetMut<RigidBodyActivation>
+            + ComponentSet<RigidBodyType>
+            + ComponentSetMut<RigidBodyIds>,
+    {
+        if let Some(removed) = self.rb2mb.get(handle.0).copied() {
+            let multibody = self.multibodies.remove(removed.multibody.0).unwrap();
+
+            // Remove the edge from the connectivity graph.
+            if let Some(parent_link) = multibody.link(removed.id).unwrap().parent_id() {
+                let parent_rb = multibody.link(parent_link).unwrap().rigid_body;
+                self.connectivity_graph.remove_edge(
+                    self.rb2mb.get(parent_rb.0).unwrap().graph_id,
+                    removed.graph_id,
+                );
+
+                if wake_up {
+                    islands.wake_up(bodies, RigidBodyHandle(handle.0), true);
+                    islands.wake_up(bodies, parent_rb, true);
+                }
+
+                // TODO: remove the node if it no longer has any attached edges?
+
+                // Extract the individual sub-trees generated by this removal.
+                let multibodies = multibody.remove_link(removed.id, true);
+
+                // Update the rb2mb mapping.
+                for multibody in multibodies {
+                    if multibody.num_links() == 1 {
+                        // We don’t have any multibody_joint attached to this body, remove it.
+                        if let Some(other) = self.connectivity_graph.remove_node(removed.graph_id) {
+                            self.rb2mb.get_mut(other.0).unwrap().graph_id = removed.graph_id;
+                        }
+                    } else {
+                        let mb_id = self.multibodies.insert(multibody);
+                        for link in self.multibodies[mb_id].links() {
+                            let ids = self.rb2mb.get_mut(link.rigid_body.0).unwrap();
+                            ids.multibody = MultibodyIndex(mb_id);
+                            ids.id = link.internal_id;
+                        }
+                    }
+                }
+            }
+        }
+    }
+
+    /// Removes all the multibody_joints from the multibody the given rigid-body is part of.
+    pub fn remove_multibody_articulations<Bodies>(
+        &mut self,
+        handle: RigidBodyHandle,
+        islands: &mut IslandManager,
+        bodies: &mut Bodies,
+        wake_up: bool,
+    ) where
+        Bodies: ComponentSetMut<RigidBodyActivation>
+            + ComponentSet<RigidBodyType>
+            + ComponentSetMut<RigidBodyIds>,
+    {
+        if let Some(removed) = self.rb2mb.get(handle.0).copied() {
+            // Remove the multibody.
+            let multibody = self.multibodies.remove(removed.multibody.0).unwrap();
+            for link in multibody.links() {
+                let rb_handle = link.rigid_body;
+
+                if wake_up {
+                    islands.wake_up(bodies, rb_handle, true);
+                }
+
+                // Remove the rigid-body <-> multibody mapping for this link.
+                let removed = self.rb2mb.remove(rb_handle.0, Default::default()).unwrap();
+                // Remove the node (and all it’s edges) from the connectivity graph.
+                if let Some(other) = self.connectivity_graph.remove_node(removed.graph_id) {
+                    self.rb2mb.get_mut(other.0).unwrap().graph_id = removed.graph_id;
+                }
+            }
+        }
+    }
+
+    pub fn remove_articulations_attached_to_rigid_body<Bodies>(
+        &mut self,
+        rb_to_remove: RigidBodyHandle,
+        islands: &mut IslandManager,
+        bodies: &mut Bodies,
+    ) where
+        Bodies: ComponentSetMut<RigidBodyActivation>
+            + ComponentSet<RigidBodyType>
+            + ComponentSetMut<RigidBodyIds>,
+    {
+        // TODO: optimize this.
+        if let Some(link_to_remove) = self.rb2mb.get(rb_to_remove.0).copied() {
+            let mut articulations_to_remove = vec![];
+            for (rb1, rb2, _) in self
+                .connectivity_graph
+                .interactions_with(link_to_remove.graph_id)
+            {
+                // There is an multibody_joint that we need to remove between these two bodies.
+                // If this is an outbound edge, then the multibody_joint’s handle is equal to the
+                // second body handle.
+                if rb1 == rb_to_remove {
+                    articulations_to_remove.push(MultibodyJointHandle(rb2.0));
+                } else {
+                    articulations_to_remove.push(MultibodyJointHandle(rb1.0));
+                }
+
+                islands.wake_up(bodies, rb1, true);
+                islands.wake_up(bodies, rb2, true);
+            }
+
+            for articulation_handle in articulations_to_remove {
+                self.remove(articulation_handle, islands, bodies, true);
+            }
+        }
+    }
+
+    /// Returns the link of this multibody attached to the given rigid-body.
+    ///
+    /// Returns `None` if `rb` isn’t part of any rigid-body.
+    pub fn rigid_body_link(&self, rb: RigidBodyHandle) -> Option<&MultibodyJointLink> {
+        self.rb2mb.get(rb.0)
+    }
+
+    /// Gets a reference to a multibody, based on its temporary index.
+    pub fn get_multibody(&self, index: MultibodyIndex) -> Option<&Multibody> {
+        self.multibodies.get(index.0)
+    }
+
+    /// Gets a mutable reference to a multibody, based on its temporary index.
+    ///
+    /// This method will bypass any modification-detection automatically done by the
+    /// `MultibodyJointSet`.
+    pub fn get_multibody_mut_internal(&mut self, index: MultibodyIndex) -> Option<&mut Multibody> {
+        self.multibodies.get_mut(index.0)
+    }
+
+    /// Gets a reference to the multibody identified by its `handle`.
+    pub fn get(&self, handle: MultibodyJointHandle) -> Option<(&Multibody, usize)> {
+        let link = self.rb2mb.get(handle.0)?;
+        let multibody = self.multibodies.get(link.multibody.0)?;
+        Some((multibody, link.id))
+    }
+
+    /// Gets a mutable reference to the multibody identified by its `handle`.
+    pub fn get_mut_internal(
+        &mut self,
+        handle: MultibodyJointHandle,
+    ) -> Option<(&mut Multibody, usize)> {
+        let link = self.rb2mb.get(handle.0)?;
+        let multibody = self.multibodies.get_mut(link.multibody.0)?;
+        Some((multibody, link.id))
+    }
+
+    /// Iterate through the handles of all the rigid-bodies attached to this rigid-body
+    /// by an multibody_joint.
+    pub fn attached_bodies<'a>(
+        &'a self,
+        body: RigidBodyHandle,
+    ) -> impl Iterator<Item = RigidBodyHandle> + 'a {
+        self.rb2mb
+            .get(body.0)
+            .into_iter()
+            .flat_map(move |id| self.connectivity_graph.interactions_with(id.graph_id))
+            .map(move |inter| crate::utils::select_other((inter.0, inter.1), body))
+    }
+
+    /// Iterates through all the multibodies on this set.
+    pub fn multibodies(&self) -> impl Iterator<Item = &Multibody> {
+        self.multibodies.iter().map(|e| e.1)
+    }
+}
+
+impl Index<MultibodyIndex> for MultibodyJointSet {
+    type Output = Multibody;
+
+    fn index(&self, index: MultibodyIndex) -> &Multibody {
+        &self.multibodies[index.0]
+    }
+}
+
+// impl Index<MultibodyJointHandle> for MultibodyJointSet {
+//     type Output = Multibody;
+//
+//     fn index(&self, index: MultibodyJointHandle) -> &Multibody {
+//         &self.multibodies[index.0]
+//     }
+// }

src/dynamics/joint/multibody_joint/multibody_link.rs

@@ -0,0 +1,173 @@
+use std::ops::{Deref, DerefMut};
+
+use crate::dynamics::{MultibodyJoint, RigidBodyHandle};
+use crate::math::{Isometry, Real};
+use crate::prelude::RigidBodyVelocity;
+
+pub(crate) struct KinematicState {
+    pub joint: MultibodyJoint,
+    pub parent_to_world: Isometry<Real>,
+    // TODO: should this be removed in favor of the rigid-body position?
+    pub local_to_world: Isometry<Real>,
+    pub local_to_parent: Isometry<Real>,
+}
+
+impl Clone for KinematicState {
+    fn clone(&self) -> Self {
+        Self {
+            joint: self.joint.clone(),
+            parent_to_world: self.parent_to_world.clone(),
+            local_to_world: self.local_to_world.clone(),
+            local_to_parent: self.local_to_parent.clone(),
+        }
+    }
+}
+
+/// One link of a multibody.
+pub struct MultibodyLink {
+    pub(crate) name: String,
+    // FIXME: make all those private.
+    pub(crate) internal_id: usize,
+    pub(crate) assembly_id: usize,
+    pub(crate) is_leaf: bool,
+
+    pub(crate) parent_internal_id: usize,
+    pub(crate) rigid_body: RigidBodyHandle,
+
+    /*
+     * Change at each time step.
+     */
+    pub(crate) state: KinematicState,
+
+    // FIXME: put this on a workspace buffer instead ?
+    pub(crate) velocity_dot_wrt_joint: RigidBodyVelocity,
+    pub(crate) velocity_wrt_joint: RigidBodyVelocity,
+}
+
+impl MultibodyLink {
+    /// Creates a new multibody link.
+    pub fn new(
+        rigid_body: RigidBodyHandle,
+        internal_id: usize,
+        assembly_id: usize,
+        parent_internal_id: usize,
+        joint: MultibodyJoint,
+        parent_to_world: Isometry<Real>,
+        local_to_world: Isometry<Real>,
+        local_to_parent: Isometry<Real>,
+    ) -> Self {
+        let is_leaf = true;
+        let velocity_dot_wrt_joint = RigidBodyVelocity::zero();
+        let velocity_wrt_joint = RigidBodyVelocity::zero();
+        let kinematic_state = KinematicState {
+            joint,
+            parent_to_world,
+            local_to_world,
+            local_to_parent,
+        };
+
+        MultibodyLink {
+            name: String::new(),
+            internal_id,
+            assembly_id,
+            is_leaf,
+            parent_internal_id,
+            state: kinematic_state,
+            velocity_dot_wrt_joint,
+            velocity_wrt_joint,
+            rigid_body,
+        }
+    }
+
+    pub fn joint(&self) -> &MultibodyJoint {
+        &self.state.joint
+    }
+
+    pub fn rigid_body_handle(&self) -> RigidBodyHandle {
+        self.rigid_body
+    }
+
+    /// Checks if this link is the root of the multibody.
+    #[inline]
+    pub fn is_root(&self) -> bool {
+        self.internal_id == 0
+    }
+
+    /// This link's name.
+    #[inline]
+    pub fn name(&self) -> &str {
+        &self.name
+    }
+
+    /// Sets this link's name.
+    #[inline]
+    pub fn set_name(&mut self, name: String) {
+        self.name = name
+    }
+
+    /// The handle of this multibody link.
+    #[inline]
+    pub fn link_id(&self) -> usize {
+        self.internal_id
+    }
+
+    /// The handle of the parent link.
+    #[inline]
+    pub fn parent_id(&self) -> Option<usize> {
+        if self.internal_id != 0 {
+            Some(self.parent_internal_id)
+        } else {
+            None
+        }
+    }
+
+    #[inline]
+    pub fn local_to_world(&self) -> &Isometry<Real> {
+        &self.state.local_to_world
+    }
+
+    #[inline]
+    pub fn local_to_parent(&self) -> &Isometry<Real> {
+        &self.state.local_to_parent
+    }
+}
+
+// FIXME: keep this even if we already have the Index2 traits?
+pub(crate) struct MultibodyLinkVec(pub Vec<MultibodyLink>);
+
+impl MultibodyLinkVec {
+    #[inline]
+    pub fn get_mut_with_parent(&mut self, i: usize) -> (&mut MultibodyLink, &MultibodyLink) {
+        let parent_id = self[i].parent_internal_id;
+
+        assert!(
+            parent_id != i,
+            "Internal error: circular rigid body dependency."
+        );
+        assert!(parent_id < self.len(), "Invalid parent index.");
+
+        unsafe {
+            let rb = &mut *(self.get_unchecked_mut(i) as *mut _);
+            let parent_rb = &*(self.get_unchecked(parent_id) as *const _);
+            (rb, parent_rb)
+        }
+    }
+}
+
+impl Deref for MultibodyLinkVec {
+    type Target = Vec<MultibodyLink>;
+
+    #[inline]
+    fn deref(&self) -> &Vec<MultibodyLink> {
+        let MultibodyLinkVec(ref me) = *self;
+        me
+    }
+}
+
+impl DerefMut for MultibodyLinkVec {
+    #[inline]
+    fn deref_mut(&mut self) -> &mut Vec<MultibodyLink> {
+        let MultibodyLinkVec(ref mut me) = *self;
+        me
+    }
+}

src/dynamics/joint/multibody_joint/multibody_workspace.rs

@@ -0,0 +1,27 @@
+use crate::dynamics::RigidBodyVelocity;
+use crate::math::Real;
+use na::DVector;
+
+/// A temporary workspace for various updates of the multibody.
+#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
+#[derive(Clone)]
+pub(crate) struct MultibodyWorkspace {
+    pub accs: Vec<RigidBodyVelocity>,
+    pub ndofs_vec: DVector<Real>,
+}
+
+impl MultibodyWorkspace {
+    /// Create an empty workspace.
+    pub fn new() -> Self {
+        MultibodyWorkspace {
+            accs: Vec::new(),
+            ndofs_vec: DVector::zeros(0),
+        }
+    }
+
+    /// Resize the workspace so it is enough for `nlinks` links.
+    pub fn resize(&mut self, nlinks: usize, ndofs: usize) {
+        self.accs.resize(nlinks, RigidBodyVelocity::zero());
+        self.ndofs_vec = DVector::zeros(ndofs)
+    }
+}

src/dynamics/joint/multibody_joint/unit_multibody_joint.rs

@@ -0,0 +1,122 @@
+#![allow(missing_docs)] // For downcast.
+
+use crate::dynamics::joint::MultibodyLink;
+use crate::dynamics::solver::{
+    AnyJointVelocityConstraint, JointGenericVelocityGroundConstraint, WritebackId,
+};
+use crate::dynamics::{IntegrationParameters, JointMotor, Multibody};
+use crate::math::Real;
+use na::DVector;
+
+/// Initializes and generate the velocity constraints applicable to the multibody links attached
+/// to this multibody_joint.
+pub fn unit_joint_limit_constraint(
+    params: &IntegrationParameters,
+    multibody: &Multibody,
+    link: &MultibodyLink,
+    limits: [Real; 2],
+    curr_pos: Real,
+    dof_id: usize,
+    j_id: &mut usize,
+    jacobians: &mut DVector<Real>,
+    constraints: &mut Vec<AnyJointVelocityConstraint>,
+) {
+    let ndofs = multibody.ndofs();
+    let joint_velocity = multibody.joint_velocity(link);
+
+    let min_enabled = curr_pos < limits[0];
+    let max_enabled = limits[1] < curr_pos;
+    let erp_inv_dt = params.erp_inv_dt();
+    let rhs_bias = ((curr_pos - limits[1]).max(0.0) - (limits[0] - curr_pos).max(0.0)) * erp_inv_dt;
+    let rhs_wo_bias = joint_velocity[dof_id];
+
+    let dof_j_id = *j_id + dof_id + link.assembly_id;
+    jacobians.rows_mut(*j_id, ndofs * 2).fill(0.0);
+    jacobians[dof_j_id] = 1.0;
+    jacobians[dof_j_id + ndofs] = 1.0;
+    multibody
+        .inv_augmented_mass()
+        .solve_mut(&mut jacobians.rows_mut(*j_id + ndofs, ndofs));
+
+    let lhs = jacobians[dof_j_id + ndofs]; // = J^t * M^-1 J
+    let impulse_bounds = [
+        min_enabled as u32 as Real * -Real::MAX,
+        max_enabled as u32 as Real * Real::MAX,
+    ];
+
+    let constraint = JointGenericVelocityGroundConstraint {
+        mj_lambda2: multibody.solver_id,
+        ndofs2: ndofs,
+        j_id2: *j_id,
+        joint_id: usize::MAX,
+        impulse: 0.0,
+        impulse_bounds,
+        inv_lhs: crate::utils::inv(lhs),
+        rhs: rhs_wo_bias + rhs_bias,
+        rhs_wo_bias,
+        writeback_id: WritebackId::Limit(dof_id),
+    };
+
+    constraints.push(AnyJointVelocityConstraint::JointGenericGroundConstraint(
+        constraint,
+    ));
+    *j_id += 2 * ndofs;
+}
+
+/// Initializes and generate the velocity constraints applicable to the multibody links attached
+/// to this multibody_joint.
+pub fn unit_joint_motor_constraint(
+    params: &IntegrationParameters,
+    multibody: &Multibody,
+    link: &MultibodyLink,
+    motor: &JointMotor,
+    curr_pos: Real,
+    dof_id: usize,
+    j_id: &mut usize,
+    jacobians: &mut DVector<Real>,
+    constraints: &mut Vec<AnyJointVelocityConstraint>,
+) {
+    let ndofs = multibody.ndofs();
+    let joint_velocity = multibody.joint_velocity(link);
+
+    let motor_params = motor.motor_params(params.dt);
+
+    let dof_j_id = *j_id + dof_id + link.assembly_id;
+    jacobians.rows_mut(*j_id, ndofs * 2).fill(0.0);
+    jacobians[dof_j_id] = 1.0;
+    jacobians[dof_j_id + ndofs] = 1.0;
+    multibody
+        .inv_augmented_mass()
+        .solve_mut(&mut jacobians.rows_mut(*j_id + ndofs, ndofs));
+
+    let lhs = jacobians[dof_j_id + ndofs]; // = J^t * M^-1 J
+    let impulse_bounds = [-motor_params.max_impulse, motor_params.max_impulse];
+
+    let mut rhs_wo_bias = 0.0;
+    if motor_params.stiffness != 0.0 {
+        rhs_wo_bias += (curr_pos - motor_params.target_pos) * motor_params.stiffness;
+    }
+
+    if motor_params.damping != 0.0 {
+        let dvel = joint_velocity[dof_id];
+        rhs_wo_bias += (dvel - motor_params.target_vel) * motor_params.damping;
+    }
+
+    let constraint = JointGenericVelocityGroundConstraint {
+        mj_lambda2: multibody.solver_id,
+        ndofs2: ndofs,
+        j_id2: *j_id,
+        joint_id: usize::MAX,
+        impulse: 0.0,
+        impulse_bounds,
+        inv_lhs: crate::utils::inv(lhs),
+        rhs: rhs_wo_bias,
+        rhs_wo_bias,
+        writeback_id: WritebackId::Limit(dof_id),
+    };
+
+    constraints.push(AnyJointVelocityConstraint::JointGenericGroundConstraint(
+        constraint,
+    ));
+    *j_id += 2 * ndofs;
+}