Address issues with the genral-case for multibody joints

src/dynamics/joint/multibody_joint/multibody.rs

@@ -9,7 +9,7 @@ use crate::dynamics::{
 #[cfg(feature = "dim3")]
 use crate::math::Matrix;
 use crate::math::{
-    AngDim, AngVector, Dim, Isometry, Jacobian, Real, Vector, ANG_DIM, DIM, SPATIAL_DIM,
+    AngDim, AngVector, Dim, Isometry, Jacobian, Point, Real, Vector, ANG_DIM, DIM, SPATIAL_DIM,
 };
 use crate::prelude::MultibodyJoint;
 use crate::utils::{IndexMut2, WAngularInertia, WCross, WCrossMatrix};
@@ -66,7 +66,7 @@ pub struct Multibody {
     pub(crate) accelerations: DVector<Real>,
 
     body_jacobians: Vec<Jacobian<Real>>,
-    // FIXME: use sparse matrices.
+    // TODO: use sparse matrices?
     augmented_mass: DMatrix<Real>,
     inv_augmented_mass: LU<Real, Dynamic, Dynamic>,
 
@@ -141,7 +141,7 @@ impl Multibody {
 
             if is_new_root {
                 let joint = MultibodyJoint::fixed(*link.local_to_world());
-                link.state.joint = joint;
+                link.joint = joint;
             }
 
             curr_mb.ndofs += link.joint().ndofs();
@@ -178,7 +178,7 @@ impl Multibody {
     }
 
     pub fn append(&mut self, mut rhs: Multibody, parent: usize, joint: MultibodyJoint) {
-        let rhs_root_ndofs = rhs.links[0].state.joint.ndofs();
+        let rhs_root_ndofs = rhs.links[0].joint.ndofs();
         let rhs_copy_shift = self.ndofs + rhs_root_ndofs;
         let rhs_copy_ndofs = rhs.ndofs - rhs_root_ndofs;
 
@@ -195,17 +195,14 @@ impl Multibody {
 
         // Adjust the first link.
         {
-            rhs.links[0].state.joint = joint;
+            rhs.links[0].joint = joint;
             rhs.links[0].assembly_id = self.velocities.len();
             rhs.links[0].internal_id = self.links.len();
             rhs.links[0].parent_internal_id = parent;
         }
 
         // Grow buffers and append data from rhs.
-        self.grow_buffers(
+        self.grow_buffers(rhs_copy_ndofs + rhs.links[0].joint.ndofs(), rhs.links.len());
-            rhs_copy_ndofs + rhs.links[0].state.joint.ndofs(),
-            rhs.links.len(),
-        );
 
         if rhs_copy_ndofs > 0 {
             self.velocities
@@ -220,7 +217,6 @@ impl Multibody {
         }
 
         rhs.links[0]
-            .state
             .joint
             .default_damping(&mut self.damping.rows_mut(base_assembly_id, rhs_root_ndofs));
 
@@ -261,17 +257,6 @@ impl Multibody {
         self.links.get_mut(id)
     }
 
-    /// The links of this multibody with the given `name`.
-    pub fn links_with_name<'a>(
-        &'a self,
-        name: &'a str,
-    ) -> impl Iterator<Item = (usize, &'a MultibodyLink)> {
-        self.links
-            .iter()
-            .enumerate()
-            .filter(move |(_i, l)| l.name == name)
-    }
-
     /// The number of links on this multibody.
     pub fn num_links(&self) -> usize {
         self.links.len()
@@ -306,7 +291,7 @@ impl Multibody {
     pub fn add_link(
         &mut self,
         parent: Option<usize>, // FIXME: should be a RigidBodyHandle?
-        mut dof: MultibodyJoint,
+        dof: MultibodyJoint,
         body: RigidBodyHandle,
     ) -> &mut MultibodyLink {
         assert!(
@@ -335,21 +320,16 @@ impl Multibody {
         /*
          * Create the multibody.
          */
-        dof.update_jacobians(&self.velocities.as_slice()[assembly_id..]);
         let local_to_parent = dof.body_to_parent();
         let local_to_world;
-        let parent_to_world;
 
         let parent_internal_id;
         if let Some(parent) = parent {
             parent_internal_id = parent;
             let parent_link = &mut self.links[parent_internal_id];
-            parent_link.is_leaf = false;
+            local_to_world = parent_link.local_to_world * local_to_parent;
-            parent_to_world = parent_link.state.local_to_world;
-            local_to_world = parent_link.state.local_to_world * local_to_parent;
         } else {
             parent_internal_id = 0;
-            parent_to_world = Isometry::identity();
             local_to_world = local_to_parent;
         }
 
@@ -359,7 +339,6 @@ impl Multibody {
             assembly_id,
             parent_internal_id,
             dof,
-            parent_to_world,
             local_to_world,
             local_to_parent,
         );
@@ -400,29 +379,31 @@ impl Multibody {
                 &RigidBodyForces,
             ) = bodies.index_bundle(link.rigid_body.0);
 
-            let mut acc = link.velocity_dot_wrt_joint;
+            let mut acc = RigidBodyVelocity::zero();
 
             if i != 0 {
                 let parent_id = link.parent_internal_id;
                 let parent_link = &self.links[parent_id];
-
+                let parent_rb_vels: &RigidBodyVelocity = bodies.index(parent_link.rigid_body.0);
-                let (parent_rb_vels, parent_rb_mprops): (&RigidBodyVelocity, &RigidBodyMassProps) =
-                    bodies.index_bundle(parent_link.rigid_body.0);
 
                 acc += self.workspace.accs[parent_id];
-                acc.linvel += parent_rb_vels.angvel.gcross(link.velocity_wrt_joint.linvel);
+                // The 2.0 originates from the two identical terms of Jdot (the terms become
+                // identical once they are multiplied by the generalized velocities).
+                acc.linvel += 2.0 * parent_rb_vels.angvel.gcross(link.joint_velocity.linvel);
                 #[cfg(feature = "dim3")]
                 {
-                    acc.angvel += parent_rb_vels.angvel.cross(&link.velocity_wrt_joint.angvel);
+                    acc.angvel += parent_rb_vels.angvel.cross(&link.joint_velocity.angvel);
                 }
 
-                let shift = rb_mprops.world_com - parent_rb_mprops.world_com;
+                acc.linvel += parent_rb_vels
-                let dvel = rb_vels.linvel - parent_rb_vels.linvel;
+                    .angvel
-
+                    .gcross(parent_rb_vels.angvel.gcross(link.shift02));
-                acc.linvel += parent_rb_vels.angvel.gcross(dvel);
+                acc.linvel += self.workspace.accs[parent_id].angvel.gcross(link.shift02);
-                acc.linvel += self.workspace.accs[parent_id].angvel.gcross(shift);
             }
 
+            acc.linvel += rb_vels.angvel.gcross(rb_vels.angvel.gcross(link.shift23));
+            acc.linvel += self.workspace.accs[i].angvel.gcross(link.shift23);
+
             self.workspace.accs[i] = acc;
 
             // TODO: should gyroscopic forces already be computed by the rigid-body itself
@@ -469,18 +450,12 @@ impl Multibody {
          * NOTE: this is needed for kinematic bodies too.
          */
         let link = &mut self.links[0];
-        let velocity_wrt_joint = link
+        let joint_velocity = link
-            .state
             .joint
             .jacobian_mul_coordinates(&self.velocities.as_slice()[link.assembly_id..]);
-        let velocity_dot_wrt_joint = link
-            .state
-            .joint
-            .jacobian_dot_mul_coordinates(&self.velocities.as_slice()[link.assembly_id..]);
 
-        link.velocity_dot_wrt_joint = velocity_dot_wrt_joint;
+        link.joint_velocity = joint_velocity;
-        link.velocity_wrt_joint = velocity_wrt_joint;
+        bodies.set_internal(link.rigid_body.0, link.joint_velocity);
-        bodies.set_internal(link.rigid_body.0, link.velocity_wrt_joint);
 
         for i in 1..self.links.len() {
             let (link, parent_link) = self.links.get_mut_with_parent(i);
@@ -488,22 +463,16 @@ impl Multibody {
             let (parent_rb_vels, parent_rb_mprops): (&RigidBodyVelocity, &RigidBodyMassProps) =
                 bodies.index_bundle(parent_link.rigid_body.0);
 
-            let velocity_wrt_joint = link
+            let joint_velocity = link
-                .state
                 .joint
                 .jacobian_mul_coordinates(&self.velocities.as_slice()[link.assembly_id..]);
-            let velocity_dot_wrt_joint = link
+            link.joint_velocity = joint_velocity.transformed(
-                .state
+                &(parent_link.local_to_world.rotation * link.joint.data.local_frame1.rotation),
-                .joint
+            );
-                .jacobian_dot_mul_coordinates(&self.velocities.as_slice()[link.assembly_id..]);
+            let mut new_rb_vels = *parent_rb_vels + link.joint_velocity;
-
-            link.velocity_dot_wrt_joint =
-                velocity_dot_wrt_joint.transformed(&parent_link.state.local_to_world);
-            link.velocity_wrt_joint =
-                velocity_wrt_joint.transformed(&parent_link.state.local_to_world);
-            let mut new_rb_vels = *parent_rb_vels + link.velocity_wrt_joint;
             let shift = rb_mprops.world_com - parent_rb_mprops.world_com;
             new_rb_vels.linvel += parent_rb_vels.angvel.gcross(shift);
+            new_rb_vels.linvel += link.joint_velocity.angvel.gcross(link.shift23);
 
             bodies.set_internal(link.rigid_body.0, new_rb_vels);
         }
@@ -514,23 +483,21 @@ impl Multibody {
         self.update_inertias(dt, bodies);
     }
 
-    fn update_body_next_jacobians<Bodies>(&mut self, bodies: &Bodies)
+    fn update_body_jacobians(&mut self) {
-    where
-        Bodies: ComponentSet<RigidBodyMassProps>,
-    {
         for i in 0..self.links.len() {
             let link = &self.links[i];
-            let rb_mprops = bodies.index(link.rigid_body.0);
 
             if self.body_jacobians[i].ncols() != self.ndofs {
                 // FIXME: use a resize instead.
                 self.body_jacobians[i] = Jacobian::zeros(self.ndofs);
             }
 
+            let parent_to_world;
+
             if i != 0 {
                 let parent_id = link.parent_internal_id;
                 let parent_link = &self.links[parent_id];
-                let parent_rb_mprops = bodies.index(parent_link.rigid_body.0);
+                parent_to_world = parent_link.local_to_world;
 
                 let (link_j, parent_j) = self.body_jacobians.index_mut_const(i, parent_id);
                 link_j.copy_from(&parent_j);
@@ -539,25 +506,31 @@ impl Multibody {
                     let mut link_j_v = link_j.fixed_rows_mut::<DIM>(0);
                     let parent_j_w = parent_j.fixed_rows::<ANG_DIM>(DIM);
 
-                    let shift_tr = (link.state.local_to_world * rb_mprops.local_mprops.local_com
+                    let shift_tr = (link.shift02).gcross_matrix_tr();
-                        - parent_link.state.local_to_world
-                            * parent_rb_mprops.local_mprops.local_com)
-                        .gcross_matrix_tr();
                     link_j_v.gemm(1.0, &shift_tr, &parent_j_w, 1.0);
                 }
             } else {
                 self.body_jacobians[i].fill(0.0);
+                parent_to_world = Isometry::identity();
             }
 
-            let ndofs = link.state.joint.ndofs();
+            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 = self.body_jacobians[i].columns_mut(link.assembly_id, ndofs);
-            link.state
+            link.joint.jacobian(
-                .joint
+                &(parent_to_world.rotation * link.joint.data.local_frame1.rotation),
-                .jacobian(&link.state.parent_to_world, &mut link_joint_j);
+                &mut link_joint_j,
-
+            );
             link_j_part += link_joint_j;
+
+            {
+                let link_j = &mut self.body_jacobians[i];
+                let (mut link_j_v, link_j_w) =
+                    link_j.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);
+            }
         }
     }
 
@@ -626,64 +599,63 @@ impl Multibody {
              *
              */
             let rb_j = &self.body_jacobians[i];
-            let rb_j_v = rb_j.fixed_rows::<DIM>(0);
+            let rb_j_w = rb_j.fixed_rows::<ANG_DIM>(DIM);
 
-            let ndofs = link.state.joint.ndofs();
+            let ndofs = link.joint.ndofs();
 
             if i != 0 {
                 let parent_id = link.parent_internal_id;
                 let parent_link = &self.links[parent_id];
-                let (parent_rb_vels, parent_rb_mprops): (&RigidBodyVelocity, &RigidBodyMassProps) =
+                let parent_rb_vels: &RigidBodyVelocity = bodies.index(parent_link.rigid_body.0);
-                    bodies.index_bundle(parent_link.rigid_body.0);
                 let parent_j = &self.body_jacobians[parent_id];
-                let parent_j_v = parent_j.fixed_rows::<DIM>(0);
                 let parent_j_w = parent_j.fixed_rows::<ANG_DIM>(DIM);
                 let parent_w = parent_rb_vels.angvel.gcross_matrix();
 
                 let (coriolis_v, parent_coriolis_v) = self.coriolis_v.index_mut2(i, parent_id);
                 let (coriolis_w, parent_coriolis_w) = self.coriolis_w.index_mut2(i, parent_id);
 
-                // JDot + JDot/u * qdot
                 coriolis_v.copy_from(&parent_coriolis_v);
                 coriolis_w.copy_from(&parent_coriolis_w);
 
-                let shift_cross =
+                // [c1 - c0].gcross() * (JDot + JDot/u * qdot)"
-                    (rb_mprops.world_com - parent_rb_mprops.world_com).gcross_matrix_tr();
+                let shift_cross_tr = link.shift02.gcross_matrix_tr();
-                coriolis_v.gemm(1.0, &shift_cross, &parent_coriolis_w, 1.0);
+                coriolis_v.gemm(1.0, &shift_cross_tr, &parent_coriolis_w, 1.0);
 
-                // JDot
+                // JDot (but the 2.0 originates from the sum of two identical terms in JDot and JDot/u * gdot)
-                let dvel_cross = (rb_vels.linvel - parent_rb_vels.linvel).gcross_matrix_tr();
+                let dvel_cross = (rb_vels.angvel.gcross(link.shift02)
+                    + 2.0 * link.joint_velocity.linvel)
+                    .gcross_matrix_tr();
                 coriolis_v.gemm(1.0, &dvel_cross, &parent_j_w, 1.0);
 
                 // JDot/u * qdot
                 coriolis_v.gemm(
                     1.0,
-                    &link.velocity_wrt_joint.linvel.gcross_matrix_tr(),
+                    &link.joint_velocity.linvel.gcross_matrix_tr(),
                     &parent_j_w,
                     1.0,
                 );
-                coriolis_v.gemm(1.0, &parent_w, &rb_j_v, 1.0);
+                coriolis_v.gemm(1.0, &(parent_w * shift_cross_tr), &parent_j_w, 1.0);
-                coriolis_v.gemm(-1.0, &parent_w, &parent_j_v, 1.0);
 
                 #[cfg(feature = "dim3")]
                 {
-                    let vel_wrt_joint_w = link.velocity_wrt_joint.angvel.gcross_matrix();
+                    let vel_wrt_joint_w = link.joint_velocity.angvel.gcross_matrix();
                     coriolis_w.gemm(-1.0, &vel_wrt_joint_w, &parent_j_w, 1.0);
                 }
 
-                // JDot
+                // JDot (but the 2.0 originates from the sum of two identical terms in JDot and JDot/u * gdot)
                 {
                     let mut coriolis_v_part = coriolis_v.columns_mut(link.assembly_id, ndofs);
 
                     let mut tmp1 = SMatrix::<Real, SPATIAL_DIM, SPATIAL_DIM>::zeros();
                     let mut rb_joint_j = tmp1.columns_mut(0, ndofs);
-                    link.state
+                    link.joint.jacobian(
-                        .joint
+                        &(parent_link.local_to_world.rotation
-                        .jacobian(&parent_link.state.local_to_world, &mut rb_joint_j);
+                            * link.joint.data.local_frame1.rotation),
+                        &mut rb_joint_j,
+                    );
 
                     let rb_joint_j_v = rb_joint_j.fixed_rows::<DIM>(0);
-
+                    coriolis_v_part.gemm(2.0, &parent_w, &rb_joint_j_v, 1.0);
-                    coriolis_v_part.gemm(1.0, &parent_w, &rb_joint_j_v, 1.0);
 
                     #[cfg(feature = "dim3")]
                     {
@@ -701,37 +673,26 @@ impl Multibody {
             let coriolis_w = &mut self.coriolis_w[i];
 
             {
-                let mut tmp1 = SMatrix::<Real, SPATIAL_DIM, SPATIAL_DIM>::zeros();
+                // [c3 - c2].gcross() * (JDot + JDot/u * qdot)
-                let mut tmp2 = SMatrix::<Real, SPATIAL_DIM, SPATIAL_DIM>::zeros();
+                let shift_cross_tr = link.shift23.gcross_matrix_tr();
-                let mut rb_joint_j_dot = tmp1.columns_mut(0, ndofs);
+                coriolis_v.gemm(1.0, &shift_cross_tr, &coriolis_w, 1.0);
-                let mut rb_joint_j_dot_veldiff = tmp2.columns_mut(0, ndofs);
-
-                link.state
-                    .joint
-                    .jacobian_dot(&link.state.parent_to_world, &mut rb_joint_j_dot);
-                link.state.joint.jacobian_dot_veldiff_mul_coordinates(
-                    &link.state.parent_to_world,
-                    &self.velocities.as_slice()[link.assembly_id..],
-                    &mut rb_joint_j_dot_veldiff,
-                );
-
-                let rb_joint_j_v_dot = rb_joint_j_dot.fixed_rows::<DIM>(0);
-                let rb_joint_j_w_dot = rb_joint_j_dot.fixed_rows::<ANG_DIM>(DIM);
-                let rb_joint_j_v_dot_veldiff = rb_joint_j_dot_veldiff.fixed_rows::<DIM>(0);
-                let rb_joint_j_w_dot_veldiff = rb_joint_j_dot_veldiff.fixed_rows::<ANG_DIM>(DIM);
-
-                let mut coriolis_v_part = coriolis_v.columns_mut(link.assembly_id, ndofs);
-                let mut coriolis_w_part = coriolis_w.columns_mut(link.assembly_id, ndofs);
 
                 // JDot
-                coriolis_v_part += rb_joint_j_v_dot;
+                let dvel_cross = rb_vels.angvel.gcross(link.shift23).gcross_matrix_tr();
-                coriolis_w_part += rb_joint_j_w_dot;
+                coriolis_v.gemm(1.0, &dvel_cross, &rb_j_w, 1.0);
 
                 // JDot/u * qdot
-                coriolis_v_part += rb_joint_j_v_dot_veldiff;
+                coriolis_v.gemm(
-                coriolis_w_part += rb_joint_j_w_dot_veldiff;
+                    1.0,
+                    &(rb_vels.angvel.gcross_matrix() * shift_cross_tr),
+                    &rb_j_w,
+                    1.0,
+                );
             }
 
+            let coriolis_v = &mut self.coriolis_v[i];
+            let coriolis_w = &mut self.coriolis_w[i];
+
             /*
              * Meld with the mass matrix.
              */
@@ -768,6 +729,8 @@ impl Multibody {
         // FIXME: avoid allocation inside LU at each timestep.
         self.acc_inv_augmented_mass = LU::new(self.acc_augmented_mass.clone());
         self.inv_augmented_mass = LU::new(self.augmented_mass.clone());
+        // self.acc_inv_augmented_mass = self.inv_augmented_mass.clone();
+        // self.augmented_mass = self.acc_augmented_mass.clone();
         // self.inv_augmented_mass = self.acc_inv_augmented_mass.clone();
     }
 
@@ -797,19 +760,16 @@ impl Multibody {
     }
 
     #[inline]
-    pub fn integrate_next(&mut self, dt: Real) {
+    pub fn integrate(&mut self, dt: Real) {
         for rb in self.links.iter_mut() {
-            rb.state
+            rb.joint
-                .joint
                 .integrate(dt, &self.velocities.as_slice()[rb.assembly_id..])
         }
     }
 
-    pub fn apply_next_displacements(&mut self, disp: &[Real]) {
+    pub fn apply_displacements(&mut self, disp: &[Real]) {
         for link in self.links.iter_mut() {
-            link.state
+            link.joint.apply_displacement(&disp[link.assembly_id..])
-                .joint
-                .apply_displacement(&disp[link.assembly_id..])
         }
     }
 
@@ -825,7 +785,7 @@ impl Multibody {
                 if rb_type.is_dynamic() {
                     let free_joint = MultibodyJoint::free(rb_pos.position);
                     let prev_root_ndofs = self.links[0].joint().ndofs();
-                    self.links[0].state.joint = free_joint;
+                    self.links[0].joint = free_joint;
                     self.links[0].assembly_id = 0;
                     self.ndofs += SPATIAL_DIM;
 
@@ -844,7 +804,7 @@ impl Multibody {
 
                     let fixed_joint = MultibodyJoint::fixed(rb_pos.position);
                     let prev_root_ndofs = self.links[0].joint().ndofs();
-                    self.links[0].state.joint = fixed_joint;
+                    self.links[0].joint = fixed_joint;
                     self.links[0].assembly_id = 0;
                     self.ndofs -= prev_root_ndofs;
 
@@ -871,15 +831,15 @@ impl Multibody {
             }
 
             // Make sure the positions are properly set to match the rigid-body’s.
-            if self.links[0].state.joint.data.locked_axes.is_empty() {
+            if self.links[0].joint.data.locked_axes.is_empty() {
-                self.links[0].state.joint.set_free_pos(rb_pos.position);
+                self.links[0].joint.set_free_pos(rb_pos.position);
             } else {
-                self.links[0].state.joint.data.local_frame1 = rb_pos.position;
+                self.links[0].joint.data.local_frame1 = rb_pos.position;
             }
         }
     }
 
-    pub fn forward_kinematics_next<Bodies>(&mut self, bodies: &mut Bodies, update_mass_props: bool)
+    pub fn forward_kinematics<Bodies>(&mut self, bodies: &mut Bodies, update_mass_props: bool)
     where
         Bodies: ComponentSet<RigidBodyType>
             + ComponentSetMut<RigidBodyMassProps>
@@ -888,19 +848,16 @@ impl Multibody {
         // Special case for the root, which has no parent.
         {
             let link = &mut self.links[0];
-            link.state
+            link.local_to_parent = link.joint.body_to_parent();
-                .joint
+            link.local_to_world = link.local_to_parent;
-                .update_jacobians(&self.velocities.as_slice());
-            link.state.local_to_parent = link.state.joint.body_to_parent();
-            link.state.local_to_world = link.state.local_to_parent;
 
-            bodies.map_mut_internal(link.rigid_body.0, |poss: &mut RigidBodyPosition| {
+            bodies.map_mut_internal(link.rigid_body.0, |rb_pos: &mut RigidBodyPosition| {
-                poss.next_position = link.state.local_to_world;
+                rb_pos.next_position = link.local_to_world;
             });
 
             if update_mass_props {
                 bodies.map_mut_internal(link.rigid_body.0, |mprops: &mut RigidBodyMassProps| {
-                    mprops.update_world_mass_properties(&link.state.local_to_world)
+                    mprops.update_world_mass_properties(&link.local_to_world)
                 });
             }
         }
@@ -909,16 +866,23 @@ impl Multibody {
         for i in 1..self.links.len() {
             let (link, parent_link) = self.links.get_mut_with_parent(i);
 
-            link.state
+            link.local_to_parent = link.joint.body_to_parent();
-                .joint
+            link.local_to_world = parent_link.local_to_world * link.local_to_parent;
-                .update_jacobians(&self.velocities.as_slice()[link.assembly_id..]);
-            link.state.local_to_parent = link.state.joint.body_to_parent();
-            link.state.local_to_world =
-                parent_link.state.local_to_world * link.state.local_to_parent;
-            link.state.parent_to_world = parent_link.state.local_to_world;
 
-            bodies.map_mut_internal(link.rigid_body.0, |poss: &mut RigidBodyPosition| {
+            {
-                poss.next_position = link.state.local_to_world;
+                let parent_rb_mprops: &RigidBodyMassProps = bodies.index(parent_link.rigid_body.0);
+                let rb_mprops: &RigidBodyMassProps = bodies.index(link.rigid_body.0);
+                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 * rb_mprops.local_mprops.local_com;
+
+                link.shift02 = c2 - c0;
+                link.shift23 = c3 - c2;
+            }
+
+            bodies.map_mut_internal(link.rigid_body.0, |rb_pos: &mut RigidBodyPosition| {
+                rb_pos.next_position = link.local_to_world;
             });
 
             let rb_type: &RigidBodyType = bodies.index(link.rigid_body.0);
@@ -929,8 +893,8 @@ impl Multibody {
             );
 
             if update_mass_props {
-                bodies.map_mut_internal(link.rigid_body.0, |mprops: &mut RigidBodyMassProps| {
+                bodies.map_mut_internal(link.rigid_body.0, |rb_mprops: &mut RigidBodyMassProps| {
-                    mprops.update_world_mass_properties(&link.state.local_to_world)
+                    rb_mprops.update_world_mass_properties(&link.local_to_world)
                 });
             }
         }
@@ -938,7 +902,7 @@ impl Multibody {
         /*
          * Compute body jacobians.
          */
-        self.update_body_next_jacobians(bodies);
+        self.update_body_jacobians();
     }
 
     #[inline]

src/dynamics/joint/multibody_joint/multibody_joint.rs

@@ -1,42 +1,21 @@
 use crate::dynamics::solver::AnyJointVelocityConstraint;
 use crate::dynamics::{
-    joint, FixedJoint, IntegrationParameters, JointAxesMask, JointData, Multibody, MultibodyLink,
+    joint, FixedJoint, IntegrationParameters, JointData, Multibody, MultibodyLink,
     RigidBodyVelocity,
 };
 use crate::math::{
-    Isometry, JacobianSliceMut, Matrix, Real, Rotation, SpacialVector, Translation, Vector,
+    Isometry, JacobianSliceMut, Real, Rotation, SpacialVector, Translation, Vector, ANG_DIM, DIM,
-    ANG_DIM, DIM, SPATIAL_DIM,
+    SPATIAL_DIM,
 };
-use crate::utils::WCross;
 use na::{DVector, DVectorSliceMut};
 #[cfg(feature = "dim3")]
-use {
+use na::{UnitQuaternion, Vector3};
-    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 {
@@ -45,9 +24,6 @@ impl MultibodyJoint {
             data,
             coords: na::zero(),
             joint_rot: Rotation::identity(),
-            jacobian_v: na::zero(),
-            jacobian_dot_v: na::zero(),
-            jacobian_dot_veldiff_v: na::zero(),
         }
     }
 
@@ -84,76 +60,59 @@ impl MultibodyJoint {
 
     /// 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() {
+        let locked_bits = self.data.locked_axes.bits();
-            // FIXME: this is a special case for the revolute joint.
+        let mut transform = self.joint_rot * self.data.local_frame2.inverse();
-            // 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 {
+        for i in 0..DIM {
-                if (locked_bits & (1 << i)) == 0 {
+            if (locked_bits & (1 << i)) == 0 {
-                    transform = Translation::from(Vector::ith(i, self.coords[i])) * transform;
+                transform = Translation::from(Vector::ith(i, self.coords[i])) * transform;
-                }
             }
-
-            self.data.local_frame1 * 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() {
+        let locked_bits = self.data.locked_axes.bits();
-            // FIXME: this is a special case for the revolute joint.
+        let mut curr_free_dof = 0;
-            // 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")]
+        for i in 0..DIM {
-            {
+            if (locked_bits & (1 << i)) == 0 {
-                self.joint_rot = Rotation::from_angle(self.coords[DIM]);
+                self.coords[i] += vels[curr_free_dof] * dt;
+                curr_free_dof += 1;
             }
-            #[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 {
+        let locked_ang_bits = locked_bits >> DIM;
-                if (locked_bits & (1 << i)) == 0 {
+        let num_free_ang_dofs = ANG_DIM - locked_ang_bits.count_ones() as usize;
-                    self.coords[i] += vels[curr_free_dof] * dt;
+        match num_free_ang_dofs {
-                    curr_free_dof += 1;
+            0 => { /* No free dofs. */ }
-                }
+            1 => {
-            }
+                let dof_id = (!locked_ang_bits).trailing_zeros() as usize;
-
+                self.coords[DIM + dof_id] += vels[curr_free_dof] * dt;
-            let locked_ang_bits = locked_bits >> DIM;
+                #[cfg(feature = "dim2")]
-            let num_free_ang_dofs = ANG_DIM - locked_ang_bits.count_ones() as usize;
+                {
-            match num_free_ang_dofs {
+                    self.joint_rot = Rotation::new(self.coords[DIM + dof_id]);
-                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]);
+                    self.joint_rot = Rotation::from_axis_angle(
-                    let disp = UnitQuaternion::new_eps(angvel * dt, 0.0);
+                        &Vector::ith_axis(dof_id),
-                    self.joint_rot = disp * self.joint_rot;
+                        self.coords[DIM + dof_id],
+                    );
                 }
-                _ => unreachable!(),
             }
+            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!(),
         }
     }
 
@@ -162,278 +121,91 @@ impl MultibodyJoint {
         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>) {
+    pub fn jacobian(&self, transform: &Rotation<Real>, out: &mut JacobianSliceMut<Real>) {
-        if self.data.locked_axes == revolute_locked_axes() {
+        let locked_bits = self.data.locked_axes.bits();
-            // FIXME: this is a special case for the revolute joint.
+        let mut curr_free_dof = 0;
-            // 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
+        for i in 0..DIM {
-            // release and fix is soon after.
+            if (locked_bits & (1 << i)) == 0 {
-            #[cfg(feature = "dim2")]
+                let transformed_axis = transform * Vector::ith(i, 1.0);
-            let axis = 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 => {
+                #[cfg(feature = "dim2")]
+                {
+                    out[(DIM, curr_free_dof)] = 1.0;
+                }
+
+                #[cfg(feature = "dim3")]
+                {
+                    let dof_id = (!locked_ang_bits).trailing_zeros() as usize;
+                    let rotmat = transform.to_rotation_matrix().into_inner();
+                    out.fixed_slice_mut::<ANG_DIM, 1>(DIM, curr_free_dof)
+                        .copy_from(&rotmat.column(dof_id));
+                }
+            }
+            2 => {
+                todo!()
+            }
             #[cfg(feature = "dim3")]
-            let axis = self.data.local_frame1 * Vector::x();
+            3 => {
-            let jacobian = RigidBodyVelocity::new(self.jacobian_v.column(0).into_owned(), axis);
+                let rotmat = transform.to_rotation_matrix();
-            out.copy_from(jacobian.transformed(transform).as_vector())
+                out.fixed_slice_mut::<3, 3>(3, curr_free_dof)
-        } else {
+                    .copy_from(rotmat.matrix());
-            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!(),
             }
+            _ => 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() {
+        let locked_bits = self.data.locked_axes.bits();
-            // FIXME: this is a special case for the revolute joint.
+        let mut result = RigidBodyVelocity::zero();
-            // We have the mathematical formulation ready that works in the general case, but its
+        let mut curr_free_dof = 0;
-            // implementation will take some time. So let’s make a special case for the alpha
+
-            // release and fix is soon after.
+        for i in 0..DIM {
-            #[cfg(feature = "dim2")]
+            if (locked_bits & (1 << i)) == 0 {
-            let axis = 1.0;
+                result.linvel += 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 => {
+                #[cfg(feature = "dim2")]
+                {
+                    result.angvel += acc[curr_free_dof];
+                }
+                #[cfg(feature = "dim3")]
+                {
+                    let dof_id = (!locked_ang_bits).trailing_zeros() as usize;
+                    result.angvel[dof_id] += acc[curr_free_dof];
+                }
+            }
+            2 => {
+                todo!()
+            }
             #[cfg(feature = "dim3")]
-            let axis = self.data.local_frame1 * Vector::x();
+            3 => {
-            RigidBodyVelocity::new(self.jacobian_v.column(0).into_owned(), axis) * acc[0]
+                let angvel = Vector3::from_row_slice(&acc[curr_free_dof..curr_free_dof + 3]);
-        } else {
+                result.angvel += angvel;
-            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!(),
             }
+            _ => unreachable!(),
         }
+        result
     }
 
     /// 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.
@@ -445,7 +217,7 @@ impl MultibodyJoint {
         for i in DIM..SPATIAL_DIM {
             if locked_bits & (1 << i) == 0 {
                 // This is a free angular DOF.
-                out[curr_free_dof] = 0.2;
+                out[curr_free_dof] = 0.1;
                 curr_free_dof += 1;
             }
         }

src/dynamics/joint/multibody_joint/multibody_link.rs

@@ -1,35 +1,14 @@
 use std::ops::{Deref, DerefMut};
 
 use crate::dynamics::{MultibodyJoint, RigidBodyHandle};
-use crate::math::{Isometry, Real};
+use crate::math::{Isometry, Real, Vector};
 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,
@@ -37,11 +16,15 @@ pub struct MultibodyLink {
     /*
      * Change at each time step.
      */
-    pub(crate) state: KinematicState,
+    pub joint: MultibodyJoint,
+    // TODO: should this be removed in favor of the rigid-body position?
+    pub local_to_world: Isometry<Real>,
+    pub local_to_parent: Isometry<Real>,
+    pub shift02: Vector<Real>,
+    pub shift23: Vector<Real>,
 
-    // FIXME: put this on a workspace buffer instead ?
+    /// The velocity added by the joint, in world-space.
-    pub(crate) velocity_dot_wrt_joint: RigidBodyVelocity,
+    pub(crate) joint_velocity: RigidBodyVelocity,
-    pub(crate) velocity_wrt_joint: RigidBodyVelocity,
 }
 
 impl MultibodyLink {
@@ -52,35 +35,27 @@ impl MultibodyLink {
         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 joint_velocity = RigidBodyVelocity::zero();
-        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,
+            joint,
-            velocity_dot_wrt_joint,
+            local_to_world,
-            velocity_wrt_joint,
+            local_to_parent,
+            shift02: na::zero(),
+            shift23: na::zero(),
+            joint_velocity,
             rigid_body,
         }
     }
 
     pub fn joint(&self) -> &MultibodyJoint {
-        &self.state.joint
+        &self.joint
     }
 
     pub fn rigid_body_handle(&self) -> RigidBodyHandle {
@@ -93,18 +68,6 @@ impl MultibodyLink {
         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 {
@@ -123,12 +86,12 @@ impl MultibodyLink {
 
     #[inline]
     pub fn local_to_world(&self) -> &Isometry<Real> {
-        &self.state.local_to_world
+        &self.local_to_world
     }
 
     #[inline]
     pub fn local_to_parent(&self) -> &Isometry<Real> {
-        &self.state.local_to_parent
+        &self.local_to_parent
     }
 }
 

src/dynamics/rigid_body_components.rs

@@ -389,19 +389,6 @@ impl RigidBodyVelocity {
         }
     }
 
-    #[cfg(feature = "dim2")]
-    pub(crate) fn from_vectors(linvel: Vector<Real>, angvel: na::Vector1<Real>) -> Self {
-        Self {
-            linvel,
-            angvel: angvel.x,
-        }
-    }
-
-    #[cfg(feature = "dim3")]
-    pub(crate) fn from_vectors(linvel: Vector<Real>, angvel: Vector<Real>) -> Self {
-        Self { linvel, angvel }
-    }
-
     /// Velocities set to zero.
     #[must_use]
     pub fn zero() -> Self {
@@ -463,21 +450,9 @@ impl RigidBodyVelocity {
         unsafe { std::mem::transmute(self) }
     }
 
-    /// Return `self` transformed by `transform`.
-    #[must_use]
-    pub fn transformed(self, transform: &Isometry<Real>) -> Self {
-        Self {
-            linvel: transform * self.linvel,
-            #[cfg(feature = "dim2")]
-            angvel: self.angvel,
-            #[cfg(feature = "dim3")]
-            angvel: transform * self.angvel,
-        }
-    }
-
     /// Return `self` rotated by `rotation`.
     #[must_use]
-    pub fn rotated(self, rotation: &Rotation<Real>) -> Self {
+    pub fn transformed(self, rotation: &Rotation<Real>) -> Self {
         Self {
             linvel: rotation * self.linvel,
             #[cfg(feature = "dim2")]

src/dynamics/solver/island_solver.rs

@@ -132,8 +132,8 @@ impl IslandSolver {
                     if link.id == 0 || link.id == 1 && !multibody.root_is_dynamic {
                         let accels = &multibody.accelerations;
                         multibody.velocities.axpy(params.dt, accels, 1.0);
-                        multibody.integrate_next(params.dt);
+                        multibody.integrate(params.dt);
-                        multibody.forward_kinematics_next(bodies, false);
+                        multibody.forward_kinematics(bodies, false);
                     }
                 } else {
                     // Since we didn't run the velocity solver we need to integrate the accelerations here

src/dynamics/solver/velocity_solver.rs

@@ -145,8 +145,8 @@ impl VelocitySolver {
                         .rows(multibody.solver_id, multibody.ndofs());
                     let prev_vels = multibody.velocities.clone(); // FIXME: avoid allocations.
                     multibody.velocities += mj_lambdas;
-                    multibody.integrate_next(params.dt);
+                    multibody.integrate(params.dt);
-                    multibody.forward_kinematics_next(bodies, false);
+                    multibody.forward_kinematics(bodies, false);
 
                     if params.max_stabilization_iterations > 0 {
                         multibody.velocities = prev_vels;

src/pipeline/physics_pipeline.rs

@@ -542,7 +542,7 @@ impl PhysicsPipeline {
             multibody.1.update_root_type(bodies);
             // FIXME: what should we do here? We should not
             //        rely on the next state here.
-            multibody.1.forward_kinematics_next(bodies, true);
+            multibody.1.forward_kinematics(bodies, true);
         }
 
         self.detect_collisions(