feat: add simple inverse-kinematics solver for multibodies (#632)

* feat: add a simple jacobian-based inverse-kinematics implementation for multibodies

* feat: add 2d inverse kinematics example

* feat: make forward_kinematics auto-fix the root’s degrees of freedom

* feat: add 3d inverse kinematics example

* chore: update changelog

* chore: clippy fixes

* chore: more clippy fixes

* fix tests

7 files changed, 448 insertions, 33 deletions

diff --git a/src/dynamics/joint/multibody_joint/mod.rs b/src/dynamics/joint/multibody_joint/mod.rs
index c789004..dc1fa75 100644
--- a/src/dynamics/joint/multibody_joint/mod.rs
+++ b/src/dynamics/joint/multibody_joint/mod.rs
@@ -1,6 +1,7 @@
 //! MultibodyJoints using the reduced-coordinates formalism or using constraints.
 
 pub use self::multibody::Multibody;
+pub use self::multibody_ik::InverseKinematicsOption;
 pub use self::multibody_joint::MultibodyJoint;
 pub use self::multibody_joint_set::{
     MultibodyIndex, MultibodyJointHandle, MultibodyJointSet, MultibodyLinkId,
@@ -13,5 +14,6 @@ mod multibody_joint_set;
 mod multibody_link;
 mod multibody_workspace;
 
+mod multibody_ik;
 mod multibody_joint;
 mod unit_multibody_joint;
diff --git a/src/dynamics/joint/multibody_joint/multibody.rs b/src/dynamics/joint/multibody_joint/multibody.rs
index 617d447..fb56087 100644
--- a/src/dynamics/joint/multibody_joint/multibody.rs
+++ b/src/dynamics/joint/multibody_joint/multibody.rs
@@ -89,6 +89,7 @@ impl Default for Multibody {
         Multibody::new()
     }
 }
+
 impl Multibody {
     /// Creates a new multibody with no link.
     pub fn new() -> Self {
@@ -115,6 +116,8 @@ impl Multibody {
 
     pub(crate) fn with_root(handle: RigidBodyHandle) -> Self {
         let mut mb = Multibody::new();
+        // NOTE: we have no way of knowing if the root in fixed at this point, so
+        //       we mark it as dynamic and will fixe later with `Self::update_root_type`.
         mb.root_is_dynamic = true;
         let joint = MultibodyJoint::free(Isometry::identity());
         mb.add_link(None, joint, handle);
@@ -747,6 +750,12 @@ impl Multibody {
         self.velocities.rows(0, self.ndofs)
     }
 
+    /// The body jacobian for link `link_id` calculated by the last call to [`Multibody::forward_kinematics`].
+    #[inline]
+    pub fn body_jacobian(&self, link_id: usize) -> &Jacobian<Real> {
+        &self.body_jacobians[link_id]
+    }
+
     /// The mutable generalized velocities of this multibodies.
     #[inline]
     pub fn generalized_velocity_mut(&mut self) -> DVectorViewMut<Real> {
@@ -762,17 +771,27 @@ impl Multibody {
     }
 
     /// Apply displacements, in generalized coordinates, to this multibody.
+    ///
+    /// Note this does **not** updates the link poses, only their generalized coordinates.
+    /// To update the link poses and associated rigid-bodies, call [`Self::forward_kinematics`]
+    /// or [`Self::finalize`].
     pub fn apply_displacements(&mut self, disp: &[Real]) {
         for link in self.links.iter_mut() {
             link.joint.apply_displacement(&disp[link.assembly_id..])
         }
     }
 
-    pub(crate) fn update_root_type(&mut self, bodies: &mut RigidBodySet) {
+    pub(crate) fn update_root_type(&mut self, bodies: &RigidBodySet, take_body_pose: bool) {
         if let Some(rb) = bodies.get(self.links[0].rigid_body) {
             if rb.is_dynamic() != self.root_is_dynamic {
+                let root_pose = if take_body_pose {
+                    *rb.position()
+                } else {
+                    self.links[0].local_to_world
+                };
+
                 if rb.is_dynamic() {
-                    let free_joint = MultibodyJoint::free(*rb.position());
+                    let free_joint = MultibodyJoint::free(root_pose);
                     let prev_root_ndofs = self.links[0].joint().ndofs();
                     self.links[0].joint = free_joint;
                     self.links[0].assembly_id = 0;
@@ -791,7 +810,7 @@ impl Multibody {
                     assert!(self.damping.len() >= SPATIAL_DIM);
                     assert!(self.accelerations.len() >= SPATIAL_DIM);
 
-                    let fixed_joint = MultibodyJoint::fixed(*rb.position());
+                    let fixed_joint = MultibodyJoint::fixed(root_pose);
                     let prev_root_ndofs = self.links[0].joint().ndofs();
                     self.links[0].joint = fixed_joint;
                     self.links[0].assembly_id = 0;
@@ -820,30 +839,86 @@ impl Multibody {
             }
 
             // Make sure the positions are properly set to match the rigid-body’s.
-            if self.links[0].joint.data.locked_axes.is_empty() {
-                self.links[0].joint.set_free_pos(*rb.position());
+            if take_body_pose {
+                if self.links[0].joint.data.locked_axes.is_empty() {
+                    self.links[0].joint.set_free_pos(*rb.position());
+                } else {
+                    self.links[0].joint.data.local_frame1 = *rb.position();
+                }
+            }
+        }
+    }
+
+    /// Update the rigid-body poses based on this multibody joint poses.
+    ///
+    /// This is typically called after [`Self::forward_kinematics`] to apply the new joint poses
+    /// to the rigid-bodies.
+    pub fn update_rigid_bodies(&self, bodies: &mut RigidBodySet, update_mass_properties: bool) {
+        self.update_rigid_bodies_internal(bodies, update_mass_properties, false, true)
+    }
+
+    pub(crate) fn update_rigid_bodies_internal(
+        &self,
+        bodies: &mut RigidBodySet,
+        update_mass_properties: bool,
+        update_next_positions_only: bool,
+        change_tracking: bool,
+    ) {
+        // Handle the children. They all have a parent within this multibody.
+        for link in self.links.iter() {
+            let rb = if change_tracking {
+                bodies.get_mut_internal_with_modification_tracking(link.rigid_body)
             } else {
-                self.links[0].joint.data.local_frame1 = *rb.position();
+                bodies.get_mut_internal(link.rigid_body)
+            };
+
+            if let Some(rb) = rb {
+                rb.pos.next_position = link.local_to_world;
+
+                if !update_next_positions_only {
+                    rb.pos.position = link.local_to_world;
+                }
+
+                if update_mass_properties {
+                    rb.mprops.update_world_mass_properties(&link.local_to_world);
+                }
             }
         }
     }
 
     // TODO: make a version that doesn’t write back to bodies and doesn’t update the jacobians
     //       (i.e. just something used by the velocity solver’s small steps).
-    /// Apply forward-kinematics to this multibody and its related rigid-bodies.
-    pub fn forward_kinematics(&mut self, bodies: &mut RigidBodySet, update_rb_mass_props: bool) {
+    /// Apply forward-kinematics to this multibody.
+    ///
+    /// This will update the [`MultibodyLink`] pose information as wall as the body jacobians.
+    /// This will also ensure that the multibody has the proper number of degrees of freedom if
+    /// its root node changed between dynamic and non-dynamic.
+    ///
+    /// Note that this does **not** update the poses of the [`RigidBody`] attached to the joints.
+    /// Run [`Self::update_rigid_bodies`] to trigger that update.
+    ///
+    /// This method updates `self` with the result of the forward-kinematics operation.
+    /// For a non-mutable version running forward kinematics on a single link, see
+    /// [`Self::forward_kinematics_single_link`].
+    ///
+    /// ## Parameters
+    /// - `bodies`: the set of rigid-bodies.
+    /// - `read_root_pose_from_rigid_body`: if set to `true`, the root joint (either a fixed joint,
+    ///   or a free joint) will have its pose set to its associated-rigid-body pose. Set this to `true`
+    ///   when the root rigid-body pose has been modified and needs to affect the multibody.
+    pub fn forward_kinematics(
+        &mut self,
+        bodies: &RigidBodySet,
+        read_root_pose_from_rigid_body: bool,
+    ) {
+        // Be sure the degrees of freedom match and take the root position if needed.
+        self.update_root_type(bodies, read_root_pose_from_rigid_body);
+
         // Special case for the root, which has no parent.
         {
             let link = &mut self.links[0];
             link.local_to_parent = link.joint.body_to_parent();
             link.local_to_world = link.local_to_parent;
-
-            if let Some(rb) = bodies.get_mut_internal(link.rigid_body) {
-                rb.pos.next_position = link.local_to_world;
-                if update_rb_mass_props {
-                    rb.mprops.update_world_mass_properties(&link.local_to_world);
-                }
-            }
         }
 
         // Handle the children. They all have a parent within this multibody.
@@ -865,20 +940,11 @@ impl Multibody {
                 link.shift23 = c3 - c2;
             }
 
-            let link_rb = bodies.index_mut_internal(link.rigid_body);
-            link_rb.pos.next_position = link.local_to_world;
-
             assert_eq!(
-                link_rb.body_type,
+                bodies[link.rigid_body].body_type,
                 RigidBodyType::Dynamic,
                 "A rigid-body that is not at the root of a multibody must be dynamic."
             );
-
-            if update_rb_mass_props {
-                link_rb
-                    .mprops
-                    .update_world_mass_properties(&link.local_to_world);
-            }
         }
 
         /*
@@ -887,6 +953,107 @@ impl Multibody {
         self.update_body_jacobians();
     }
 
+    /// Apply forward-kinematics to compute the position of a single link of this multibody.
+    ///
+    /// If `out_jacobian` is `Some`, this will simultaneously compute the new jacobian of this link.
+    /// If `displacement` is `Some`, the generalized position considered during transform propagation
+    /// is the sum of the current position of `self` and this `displacement`.
+    // TODO: this shares a lot of code with `forward_kinematics` and `update_body_jacobians`, except
+    //       that we are only traversing a single kinematic chain. Could this be refactored?
+    pub fn forward_kinematics_single_link(
+        &self,
+        bodies: &RigidBodySet,
+        link_id: usize,
+        displacement: Option<&[Real]>,
+        mut out_jacobian: Option<&mut Jacobian<Real>>,
+    ) -> Isometry<Real> {
+        let mut branch = vec![]; // Perf: avoid allocation.
+        let mut curr_id = Some(link_id);
+
+        while let Some(id) = curr_id {
+            branch.push(id);
+            curr_id = self.links[id].parent_id();
+        }
+
+        branch.reverse();
+
+        if let Some(out_jacobian) = out_jacobian.as_deref_mut() {
+            if out_jacobian.ncols() != self.ndofs {
+                *out_jacobian = Jacobian::zeros(self.ndofs);
+            } else {
+                out_jacobian.fill(0.0);
+            }
+        }
+
+        let mut parent_link: Option<MultibodyLink> = None;
+
+        for i in branch {
+            let mut link = self.links[i];
+
+            if let Some(displacement) = displacement {
+                link.joint
+                    .apply_displacement(&displacement[link.assembly_id..]);
+            }
+
+            let parent_to_world;
+
+            if let Some(parent_link) = parent_link {
+                link.local_to_parent = link.joint.body_to_parent();
+                link.local_to_world = parent_link.local_to_world * link.local_to_parent;
+
+                {
+                    let parent_rb = &bodies[parent_link.rigid_body];
+                    let link_rb = &bodies[link.rigid_body];
+                    let c0 = parent_link.local_to_world * parent_rb.mprops.local_mprops.local_com;
+                    let c2 = link.local_to_world
+                        * Point::from(link.joint.data.local_frame2.translation.vector);
+                    let c3 = link.local_to_world * link_rb.mprops.local_mprops.local_com;
+
+                    link.shift02 = c2 - c0;
+                    link.shift23 = c3 - c2;
+                }
+
+                parent_to_world = parent_link.local_to_world;
+
+                if let Some(out_jacobian) = out_jacobian.as_deref_mut() {
+                    let (mut link_j_v, parent_j_w) =
+                        out_jacobian.rows_range_pair_mut(0..DIM, DIM..DIM + ANG_DIM);
+                    let shift_tr = (link.shift02).gcross_matrix_tr();
+                    link_j_v.gemm(1.0, &shift_tr, &parent_j_w, 1.0);
+                }
+            } else {
+                link.local_to_parent = link.joint.body_to_parent();
+                link.local_to_world = link.local_to_parent;
+                parent_to_world = Isometry::identity();
+            }
+
+            if let Some(out_jacobian) = out_jacobian.as_deref_mut() {
+                let ndofs = link.joint.ndofs();
+                let mut tmp = SMatrix::<Real, SPATIAL_DIM, SPATIAL_DIM>::zeros();
+                let mut link_joint_j = tmp.columns_mut(0, ndofs);
+                let mut link_j_part = out_jacobian.columns_mut(link.assembly_id, ndofs);
+                link.joint.jacobian(
+                    &(parent_to_world.rotation * link.joint.data.local_frame1.rotation),
+                    &mut link_joint_j,
+                );
+                link_j_part += link_joint_j;
+
+                {
+                    let (mut link_j_v, link_j_w) =
+                        out_jacobian.rows_range_pair_mut(0..DIM, DIM..DIM + ANG_DIM);
+                    let shift_tr = link.shift23.gcross_matrix_tr();
+                    link_j_v.gemm(1.0, &shift_tr, &link_j_w, 1.0);
+                }
+            }
+
+            parent_link = Some(link);
+        }
+
+        parent_link
+            .map(|link| link.local_to_world)
+            .unwrap_or_else(Isometry::identity)
+    }
+
     /// The total number of freedoms of this multibody.
     #[inline]
     pub fn ndofs(&self) -> usize {
diff --git a/src/dynamics/joint/multibody_joint/multibody_ik.rs b/src/dynamics/joint/multibody_joint/multibody_ik.rs
new file mode 100644
index 0000000..7e0fc15
--- /dev/null
+++ b/src/dynamics/joint/multibody_joint/multibody_ik.rs
@@ -0,0 +1,238 @@
+use crate::dynamics::{JointAxesMask, Multibody, RigidBodySet};
+use crate::math::{Isometry, Jacobian, Real, ANG_DIM, DIM, SPATIAL_DIM};
+use na::{self, DVector, SMatrix};
+use parry::math::SpacialVector;
+
+#[derive(Copy, Clone, Debug, PartialEq)]
+/// Options for the jacobian-based Inverse Kinematics solver for multibodies.
+pub struct InverseKinematicsOption {
+    /// A damping coefficient.
+    ///
+    /// Small value can lead to overshooting preventing convergence. Large
+    /// values can slown down convergence, requiring more iterations to converge.
+    pub damping: Real,
+    /// The maximum number of iterations the iterative IK solver can take.
+    pub max_iters: usize,
+    /// The axes the IK solver will solve for.
+    pub constrained_axes: JointAxesMask,
+    /// The error threshold on the linear error.
+    ///
+    /// If errors on both linear and angular parts fall below this
+    /// threshold, the iterative resolution will stop.
+    pub epsilon_linear: Real,
+    /// The error threshold on the angular error.
+    ///
+    /// If errors on both linear and angular parts fall below this
+    /// threshold, the iterative resolution will stop.
+    pub epsilon_angular: Real,
+}
+
+impl Default for InverseKinematicsOption {
+    fn default() -> Self {
+        Self {
+            damping: 1.0,
+            max_iters: 10,
+            constrained_axes: JointAxesMask::all(),
+            epsilon_linear: 1.0e-3,
+            epsilon_angular: 1.0e-3,
+        }
+    }
+}
+
+impl Multibody {
+    /// Computes the displacement needed to have the link identified by `link_id` move by the
+    /// desired transform.
+    ///
+    /// The displacement calculated by this function is added to the `displacement` vector.
+    pub fn inverse_kinematics_delta(
+        &self,
+        link_id: usize,
+        desired_movement: &SpacialVector<Real>,
+        damping: Real,
+        displacements: &mut DVector<Real>,
+    ) {
+        let body_jacobian = self.body_jacobian(link_id);
+        Self::inverse_kinematics_delta_with_jacobian(
+            body_jacobian,
+            desired_movement,
+            damping,
+            displacements,
+        );
+    }
+
+    /// Computes the displacement needed to have a link with the given jacobian move by the
+    /// desired transform.
+    ///
+    /// The displacement calculated by this function is added to the `displacement` vector.
+    pub fn inverse_kinematics_delta_with_jacobian(
+        jacobian: &Jacobian<Real>,
+        desired_movement: &SpacialVector<Real>,
+        damping: Real,
+        displacements: &mut DVector<Real>,
+    ) {
+        let identity = SMatrix::<Real, SPATIAL_DIM, SPATIAL_DIM>::identity();
+        let jj = jacobian * &jacobian.transpose() + identity * (damping * damping);
+        let inv_jj = jj.pseudo_inverse(1.0e-5).unwrap_or(identity);
+        displacements.gemv_tr(1.0, jacobian, &(inv_jj * desired_movement), 1.0);
+    }
+
+    /// Computes the displacement needed to have the link identified by `link_id` have a pose
+    /// equal (or as close as possible) to `target_pose`.
+    ///
+    /// If `displacement` is given non-zero, the current pose of the rigid-body is considered to be
+    /// obtained from its current generalized coordinates summed with the `displacement` vector.
+    ///
+    /// The `displacements` vector is overwritten with the new displacement.
+    pub fn inverse_kinematics(
+        &self,
+        bodies: &RigidBodySet,
+        link_id: usize,
+        options: &InverseKinematicsOption,
+        target_pose: &Isometry<Real>,
+        displacements: &mut DVector<Real>,
+    ) {
+        let mut jacobian = Jacobian::zeros(0);
+
+        for _ in 0..options.max_iters {
+            let pose = self.forward_kinematics_single_link(
+                bodies,
+                link_id,
+                Some(displacements.as_slice()),
+                Some(&mut jacobian),
+            );
+
+            let delta_lin = target_pose.translation.vector - pose.translation.vector;
+            let delta_ang = (target_pose.rotation * pose.rotation.inverse()).scaled_axis();
+
+            #[cfg(feature = "dim2")]
+            let mut delta = na::vector![delta_lin.x, delta_lin.y, delta_ang.x];
+            #[cfg(feature = "dim3")]
+            let mut delta = na::vector![
+                delta_lin.x,
+                delta_lin.y,
+                delta_lin.z,
+                delta_ang.x,
+                delta_ang.y,
+                delta_ang.z
+            ];
+
+            if !options.constrained_axes.contains(JointAxesMask::X) {
+                delta[0] = 0.0;
+            }
+            if !options.constrained_axes.contains(JointAxesMask::Y) {
+                delta[1] = 0.0;
+            }
+            #[cfg(feature = "dim3")]
+            if !options.constrained_axes.contains(JointAxesMask::Z) {
+                delta[2] = 0.0;
+            }
+            if !options.constrained_axes.contains(JointAxesMask::ANG_X) {
+                delta[DIM] = 0.0;
+            }
+            #[cfg(feature = "dim3")]
+            if !options.constrained_axes.contains(JointAxesMask::ANG_Y) {
+                delta[DIM + 1] = 0.0;
+            }
+            #[cfg(feature = "dim3")]
+            if !options.constrained_axes.contains(JointAxesMask::ANG_Z) {
+                delta[DIM + 2] = 0.0;
+            }
+
+            // TODO: measure convergence on the error variation instead?
+            if delta.rows(0, DIM).norm() <= options.epsilon_linear
+                && delta.rows(DIM, ANG_DIM).norm() <= options.epsilon_angular
+            {
+                break;
+            }
+
+            Self::inverse_kinematics_delta_with_jacobian(
+                &jacobian,
+                &delta,
+                options.damping,
+                displacements,
+            );
+        }
+    }
+}
+
+#[cfg(test)]
+mod test {
+    use crate::dynamics::{
+        MultibodyJointHandle, MultibodyJointSet, RevoluteJointBuilder, RigidBodyBuilder,
+        RigidBodySet,
+    };
+    use crate::math::{Jacobian, Real, Vector};
+    use approx::assert_relative_eq;
+
+    #[test]
+    fn one_link_fwd_kinematics() {
+        let mut bodies = RigidBodySet::new();
+        let mut multibodies = MultibodyJointSet::new();
+
+        let num_segments = 10;
+        let body = RigidBodyBuilder::fixed();
+        let mut last_body = bodies.insert(body);
+        let mut last_link = MultibodyJointHandle::invalid();
+
+        for _ in 0..num_segments {
+            let body = RigidBodyBuilder::dynamic().can_sleep(false);
+            let new_body = bodies.insert(body);
+
+            #[cfg(feature = "dim2")]
+            let builder = RevoluteJointBuilder::new();
+            #[cfg(feature = "dim3")]
+            let builder = RevoluteJointBuilder::new(Vector::z_axis());
+            let link_ab = builder
+                .local_anchor1((Vector::y() * (0.5 / num_segments as Real)).into())
+                .local_anchor2((Vector::y() * (-0.5 / num_segments as Real)).into());
+            last_link = multibodies
+                .insert(last_body, new_body, link_ab, true)
+                .unwrap();
+
+            last_body = new_body;
+        }
+
+        let (multibody, last_id) = multibodies.get_mut(last_link).unwrap();
+        multibody.forward_kinematics(&bodies, true); // Be sure all the dofs are up to date.
+        assert_eq!(multibody.ndofs(), num_segments);
+
+        /*
+         * No displacement.
+         */
+        let mut jacobian2 = Jacobian::zeros(0);
+        let link_pose1 = *multibody.link(last_id).unwrap().local_to_world();
+        let jacobian1 = multibody.body_jacobian(last_id);
+        let link_pose2 =
+            multibody.forward_kinematics_single_link(&bodies, last_id, None, Some(&mut jacobian2));
+        assert_eq!(link_pose1, link_pose2);
+        assert_eq!(jacobian1, &jacobian2);
+
+        /*
+         * Arbitrary displacement.
+         */
+        let niter = 100;
+        let displacement_part: Vec<_> = (0..multibody.ndofs())
+            .map(|i| i as Real * -0.1 / niter as Real)
+            .collect();
+        let displacement_total: Vec<_> = displacement_part
+            .iter()
+            .map(|d| *d * niter as Real)
+            .collect();
+        let link_pose2 = multibody.forward_kinematics_single_link(
+            &bodies,
+            last_id,
+            Some(&displacement_total),
+            Some(&mut jacobian2),
+        );
+
+        for _ in 0..niter {
+            multibody.apply_displacements(&displacement_part);
+            multibody.forward_kinematics(&bodies, false);
+        }
+
+        let link_pose1 = *multibody.link(last_id).unwrap().local_to_world();
+        let jacobian1 = multibody.body_jacobian(last_id);
+        assert_relative_eq!(link_pose1, link_pose2, epsilon = 1.0e-5);
+        assert_relative_eq!(jacobian1, &jacobian2, epsilon = 1.0e-5);
+    }
+}
diff --git a/src/dynamics/joint/multibody_joint/multibody_joint_set.rs b/src/dynamics/joint/multibody_joint/multibody_joint_set.rs
index fa0ffdb..a95c10b 100644
--- a/src/dynamics/joint/multibody_joint/multibody_joint_set.rs
+++ b/src/dynamics/joint/multibody_joint/multibody_joint_set.rs
@@ -287,6 +287,13 @@ impl MultibodyJointSet {
     }
 
     /// Gets a mutable reference to a multibody, based on its temporary index.
+    /// `MultibodyJointSet`.
+    pub fn get_multibody_mut(&mut self, index: MultibodyIndex) -> Option<&mut Multibody> {
+        // TODO: modification tracking.
+        self.multibodies.get_mut(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`.
@@ -363,13 +370,13 @@ impl MultibodyJointSet {
         let parent1 = link1.parent_id();
 
         if parent1 == Some(id2.id) {
-            Some((MultibodyJointHandle(rb1.0), mb, &link1))
+            Some((MultibodyJointHandle(rb1.0), mb, link1))
         } else {
             let link2 = mb.link(id2.id)?;
             let parent2 = link2.parent_id();
 
             if parent2 == Some(id1.id) {
-                Some((MultibodyJointHandle(rb2.0), mb, &link2))
+                Some((MultibodyJointHandle(rb2.0), mb, link2))
             } else {
                 None
             }
diff --git a/src/dynamics/joint/multibody_joint/multibody_link.rs b/src/dynamics/joint/multibody_joint/multibody_link.rs
index 7336a6c..bf6d605 100644
--- a/src/dynamics/joint/multibody_joint/multibody_link.rs
+++ b/src/dynamics/joint/multibody_joint/multibody_link.rs
@@ -6,7 +6,7 @@ use crate::prelude::RigidBodyVelocity;
 
 /// One link of a multibody.
 #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
-#[derive(Clone)]
+#[derive(Copy, Clone)]
 pub struct MultibodyLink {
     // FIXME: make all those private.
     pub(crate) internal_id: usize,
diff --git a/src/dynamics/rigid_body.rs b/src/dynamics/rigid_body.rs
index b822aa9..25631fc 100644
--- a/src/dynamics/rigid_body.rs
+++ b/src/dynamics/rigid_body.rs
@@ -292,9 +292,9 @@ impl RigidBody {
         allow_rotations_z: bool,
         wake_up: bool,
     ) {
-        if self.mprops.flags.contains(LockedAxes::ROTATION_LOCKED_X) != !allow_rotations_x
-            || self.mprops.flags.contains(LockedAxes::ROTATION_LOCKED_Y) != !allow_rotations_y
-            || self.mprops.flags.contains(LockedAxes::ROTATION_LOCKED_Z) != !allow_rotations_z
+        if self.mprops.flags.contains(LockedAxes::ROTATION_LOCKED_X) == allow_rotations_x
+            || self.mprops.flags.contains(LockedAxes::ROTATION_LOCKED_Y) == allow_rotations_y
+            || self.mprops.flags.contains(LockedAxes::ROTATION_LOCKED_Z) == allow_rotations_z
         {
             if self.is_dynamic() && wake_up {
                 self.wake_up(true);
diff --git a/src/dynamics/solver/velocity_solver.rs b/src/dynamics/solver/velocity_solver.rs
index b0bafb8..9d5c279 100644
--- a/src/dynamics/solver/velocity_solver.rs
+++ b/src/dynamics/solver/velocity_solver.rs
@@ -251,8 +251,9 @@ impl VelocitySolver {
                 .rows(multibody.solver_id, multibody.ndofs());
             multibody.velocities.copy_from(&solver_vels);
             multibody.integrate(params.dt);
-            // PERF: we could have a mode where it doesn’t write back to the `bodies` yet.
-            multibody.forward_kinematics(bodies, !is_last_substep);
+            // PERF: don’t write back to the rigid-body poses `bodies` before the last step?
+            multibody.forward_kinematics(bodies, false);
+            multibody.update_rigid_bodies_internal(bodies, !is_last_substep, true, false);
 
             if !is_last_substep {
                 // These are very expensive and not needed if we don’t