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

22 files changed, 755 insertions, 212 deletions

diff --git a/CHANGELOG.md b/CHANGELOG.md
index 74f0497..a7c1efe 100644
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@@ -1,3 +1,26 @@
+## Unreleased
+
+### Added
+
+- Add `Multibody::inverse_kinematics`, `Multibody::inverse_kinematics_delta`,
+  and `::inverse_kinematics_delta_with_jacobian`
+  for running inverse kinematics on a multibody to align one its links pose to the given prescribed pose.
+- Add `InverseKinematicsOption` to customize some behaviors of the inverse-kinematics solver.
+- Add `Multibody::body_jacobian` to get the jacobian of a specific link.
+- Add `Multibody::update_rigid_bodies` to update rigid-bodies based on the multibody links poses.
+- Add `Multibody::forward_kinematics_single_link` to run forward-kinematics to compute the new pose and jacobian of a
+  single link without mutating the multibody. This can take an optional displacement on generalized coordinates that are
+  taken into account during transform propagation.
+
+### Modified
+
+- The `Multibody::forward_kinematics` method will no longer automatically update the poses of the `RigidBody` associated
+  to each joint. Instead `Multibody::update_rigid_bodies` has to be called explicitly.
+- The `Multibody::forward_kinematics` method will automatically adjust the multibody’s degrees of freedom if the root
+  rigid-body changed type (between dynamic and non-dynamic). It can also optionally apply the root’s rigid-body pose
+  instead of the root link’s pose (useful for example if you modified the root rigid-body pose externally and wanted
+  to propagate it to the multibody).
+
 ## v0.19.0 (05 May 2024)
 
 ### Fix
diff --git a/crates/rapier_testbed2d-f64/Cargo.toml b/crates/rapier_testbed2d-f64/Cargo.toml
index 2a090bf..800ac49 100644
--- a/crates/rapier_testbed2d-f64/Cargo.toml
+++ b/crates/rapier_testbed2d-f64/Cargo.toml
@@ -28,7 +28,7 @@ other-backends = ["wrapped2d"]
 features = ["parallel", "other-backends"]
 
 [dependencies]
-nalgebra = { version = "0.32", features = ["rand"] }
+nalgebra = { version = "0.32", features = ["rand", "glam025"] }
 rand = "0.8"
 rand_pcg = "0.3"
 instant = { version = "0.1", features = ["web-sys", "now"] }
diff --git a/crates/rapier_testbed2d/Cargo.toml b/crates/rapier_testbed2d/Cargo.toml
index 6165e23..3935fcb 100644
--- a/crates/rapier_testbed2d/Cargo.toml
+++ b/crates/rapier_testbed2d/Cargo.toml
@@ -28,7 +28,7 @@ other-backends = ["wrapped2d"]
 features = ["parallel", "other-backends"]
 
 [dependencies]
-nalgebra = { version = "0.32", features = ["rand"] }
+nalgebra = { version = "0.32", features = ["rand", "glam025"] }
 rand = "0.8"
 rand_pcg = "0.3"
 instant = { version = "0.1", features = ["web-sys", "now"] }
diff --git a/crates/rapier_testbed3d-f64/Cargo.toml b/crates/rapier_testbed3d-f64/Cargo.toml
index 99d6e85..8fd5628 100644
--- a/crates/rapier_testbed3d-f64/Cargo.toml
+++ b/crates/rapier_testbed3d-f64/Cargo.toml
@@ -27,7 +27,7 @@ parallel = ["rapier/parallel", "num_cpus"]
 features = ["parallel"]
 
 [dependencies]
-nalgebra = { version = "0.32", features = ["rand"] }
+nalgebra = { version = "0.32", features = ["rand", "glam025"] }
 rand = "0.8"
 rand_pcg = "0.3"
 instant = { version = "0.1", features = ["web-sys", "now"] }
diff --git a/crates/rapier_testbed3d/Cargo.toml b/crates/rapier_testbed3d/Cargo.toml
index e8e7f3f..a3f3c5d 100644
--- a/crates/rapier_testbed3d/Cargo.toml
+++ b/crates/rapier_testbed3d/Cargo.toml
@@ -28,7 +28,7 @@ other-backends = ["physx", "physx-sys", "glam"]
 features = ["parallel", "other-backends"]
 
 [dependencies]
-nalgebra = { version = "0.32", features = ["rand"] }
+nalgebra = { version = "0.32", features = ["rand", "glam025"] }
 rand = "0.8"
 rand_pcg = "0.3"
 instant = { version = "0.1", features = ["web-sys", "now"] }
diff --git a/examples2d/all_examples2.rs b/examples2d/all_examples2.rs
index 08fd996..39d85f6 100644
--- a/examples2d/all_examples2.rs
+++ b/examples2d/all_examples2.rs
@@ -20,6 +20,7 @@ mod debug_total_overlap2;
 mod debug_vertical_column2;
 mod drum2;
 mod heightfield2;
+mod inverse_kinematics2;
 mod joint_motor_position2;
 mod joints2;
 mod locked_rotations2;
@@ -84,6 +85,7 @@ pub fn main() {
         ("Damping", damping2::init_world),
         ("Drum", drum2::init_world),
         ("Heightfield", heightfield2::init_world),
+        ("Inverse kinematics", inverse_kinematics2::init_world),
         ("Joints", joints2::init_world),
         ("Locked rotations", locked_rotations2::init_world),
         ("One-way platforms", one_way_platforms2::init_world),
diff --git a/examples2d/inverse_kinematics2.rs b/examples2d/inverse_kinematics2.rs
new file mode 100644
index 0000000..985bdb2
--- /dev/null
+++ b/examples2d/inverse_kinematics2.rs
@@ -0,0 +1,96 @@
+use rapier2d::prelude::*;
+use rapier_testbed2d::Testbed;
+
+pub fn init_world(testbed: &mut Testbed) {
+    /*
+     * World
+     */
+    let mut bodies = RigidBodySet::new();
+    let mut colliders = ColliderSet::new();
+    let impulse_joints = ImpulseJointSet::new();
+    let mut multibody_joints = MultibodyJointSet::new();
+
+    /*
+     * Ground
+     */
+    let ground_size = 1.0;
+    let ground_height = 0.01;
+
+    let rigid_body = RigidBodyBuilder::fixed().translation(vector![0.0, -ground_height]);
+    let floor_handle = bodies.insert(rigid_body);
+    let collider = ColliderBuilder::cuboid(ground_size, ground_height);
+    colliders.insert_with_parent(collider, floor_handle, &mut bodies);
+
+    /*
+     * Setup groups
+     */
+    let num_segments = 10;
+    let body = RigidBodyBuilder::fixed();
+    let mut last_body = bodies.insert(body);
+    let mut last_link = MultibodyJointHandle::invalid();
+
+    for i in 0..num_segments {
+        let size = 1.0 / num_segments as f32;
+        let body = RigidBodyBuilder::dynamic().can_sleep(false);
+        let new_body = bodies.insert(body);
+        // NOTE: we add a sensor collider just to make the destbed draw a rectangle to make
+        //       the demo look nicer. IK could be used without cuboid.
+        let collider = ColliderBuilder::cuboid(size / 8.0, size / 2.0)
+            .density(0.0)
+            .sensor(true);
+        colliders.insert_with_parent(collider, new_body, &mut bodies);
+
+        let link_ab = RevoluteJointBuilder::new()
+            .local_anchor1(point![0.0, size / 2.0 * (i != 0) as usize as f32])
+            .local_anchor2(point![0.0, -size / 2.0])
+            .build()
+            .data;
+
+        last_link = multibody_joints
+            .insert(last_body, new_body, link_ab, true)
+            .unwrap();
+
+        last_body = new_body;
+    }
+
+    let mut displacements = DVector::zeros(0);
+
+    testbed.add_callback(move |graphics, physics, _, _| {
+        let Some(graphics) = graphics else { return };
+        if let Some((multibody, link_id)) = physics.multibody_joints.get_mut(last_link) {
+            // Ensure our displacement vector has the right number of elements.
+            if displacements.nrows() < multibody.ndofs() {
+                displacements = DVector::zeros(multibody.ndofs());
+            } else {
+                displacements.fill(0.0);
+            }
+
+            let Some(mouse_point) = graphics.mouse().point else {
+                return;
+            };
+
+            // We will have the endpoint track the mouse position.
+            let target_point = mouse_point.coords;
+
+            let options = InverseKinematicsOption {
+                constrained_axes: JointAxesMask::LIN_AXES,
+                ..Default::default()
+            };
+
+            multibody.inverse_kinematics(
+                &physics.bodies,
+                link_id,
+                &options,
+                &Isometry::from(target_point),
+                &mut displacements,
+            );
+            multibody.apply_displacements(displacements.as_slice());
+        }
+    });
+
+    /*
+     * Set up the testbed.
+     */
+    testbed.set_world(bodies, colliders, impulse_joints, multibody_joints);
+    testbed.look_at(point![0.0, 0.0], 300.0);
+}
diff --git a/examples3d/all_examples3.rs b/examples3d/all_examples3.rs
index 4c4d962..3e6398f 100644
--- a/examples3d/all_examples3.rs
+++ b/examples3d/all_examples3.rs
@@ -42,6 +42,7 @@ mod debug_cube_high_mass_ratio3;
 mod debug_internal_edges3;
 mod debug_long_chain3;
 mod debug_multibody_ang_motor_pos3;
+mod inverse_kinematics3;
 mod joint_motor_position3;
 mod keva3;
 mod locked_rotations3;
@@ -108,6 +109,7 @@ pub fn main() {
         ("Dynamic trimeshes", dynamic_trimesh3::init_world),
         ("Heightfield", heightfield3::init_world),
         ("Impulse Joints", joints3::init_world_with_joints),
+        ("Inverse kinematics", inverse_kinematics3::init_world),
         ("Joint Motor Position", joint_motor_position3::init_world),
         ("Locked rotations", locked_rotations3::init_world),
         ("One-way platforms", one_way_platforms3::init_world),
diff --git a/examples3d/inverse_kinematics3.rs b/examples3d/inverse_kinematics3.rs
new file mode 100644
index 0000000..7905ae0
--- /dev/null
+++ b/examples3d/inverse_kinematics3.rs
@@ -0,0 +1,103 @@
+use rapier3d::prelude::*;
+use rapier_testbed3d::Testbed;
+
+pub fn init_world(testbed: &mut Testbed) {
+    /*
+     * World
+     */
+    let mut bodies = RigidBodySet::new();
+    let mut colliders = ColliderSet::new();
+    let impulse_joints = ImpulseJointSet::new();
+    let mut multibody_joints = MultibodyJointSet::new();
+
+    /*
+     * Ground
+     */
+    let ground_size = 0.2;
+    let ground_height = 0.01;
+
+    let rigid_body = RigidBodyBuilder::fixed().translation(vector![0.0, -ground_height, 0.0]);
+    let floor_handle = bodies.insert(rigid_body);
+    let collider = ColliderBuilder::cuboid(ground_size, ground_height, ground_size);
+    colliders.insert_with_parent(collider, floor_handle, &mut bodies);
+
+    /*
+     * Setup groups
+     */
+    let num_segments = 10;
+    let body = RigidBodyBuilder::fixed();
+    let mut last_body = bodies.insert(body);
+    let mut last_link = MultibodyJointHandle::invalid();
+
+    for i in 0..num_segments {
+        let size = 1.0 / num_segments as f32;
+        let body = RigidBodyBuilder::dynamic().can_sleep(false);
+        let new_body = bodies.insert(body);
+        // NOTE: we add a sensor collider just to make the destbed draw a rectangle to make
+        //       the demo look nicer. IK could be used without cuboid.
+        let collider = ColliderBuilder::cuboid(size / 8.0, size / 2.0, size / 8.0)
+            .density(0.0)
+            .sensor(true);
+        colliders.insert_with_parent(collider, new_body, &mut bodies);
+
+        let link_ab = SphericalJointBuilder::new()
+            .local_anchor1(point![0.0, size / 2.0 * (i != 0) as usize as f32, 0.0])
+            .local_anchor2(point![0.0, -size / 2.0, 0.0])
+            .build()
+            .data;
+
+        last_link = multibody_joints
+            .insert(last_body, new_body, link_ab, true)
+            .unwrap();
+
+        last_body = new_body;
+    }
+
+    let mut displacements = DVector::zeros(0);
+
+    testbed.add_callback(move |graphics, physics, _, _| {
+        let Some(graphics) = graphics else { return };
+        if let Some((multibody, link_id)) = physics.multibody_joints.get_mut(last_link) {
+            // Ensure our displacement vector has the right number of elements.
+            if displacements.nrows() < multibody.ndofs() {
+                displacements = DVector::zeros(multibody.ndofs());
+            } else {
+                displacements.fill(0.0);
+            }
+
+            let Some(mouse_ray) = graphics.mouse().ray else {
+                return;
+            };
+
+            // Cast a ray on a plane aligned with the camera passing through the origin.
+            let halfspace = HalfSpace {
+                normal: -UnitVector::new_normalize(graphics.camera_fwd_dir()),
+            };
+            let mouse_ray = Ray::new(mouse_ray.0, mouse_ray.1);
+            let Some(hit) = halfspace.cast_local_ray(&mouse_ray, f32::MAX, false) else {
+                return;
+            };
+            let target_point = mouse_ray.point_at(hit);
+
+            let options = InverseKinematicsOption {
+                constrained_axes: JointAxesMask::LIN_AXES,
+                ..Default::default()
+            };
+
+            multibody.inverse_kinematics(
+                &physics.bodies,
+                link_id,
+                &options,
+                &Isometry::from(target_point),
+                &mut displacements,
+            );
+            multibody.apply_displacements(displacements.as_slice());
+        }
+    });
+
+    /*
+     * Set up the testbed.
+     */
+    testbed.set_world(bodies, colliders, impulse_joints, multibody_joints);
+    testbed.look_at(point![0.0, 0.5, 2.5], point![0.0, 0.5, 0.0]);
+}
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>,
+    ) {
+