More experiments with the way the generic joint is stabilized.

5 files changed, 332 insertions, 25 deletions

diff --git a/src/dynamics/joint/generic_joint.rs b/src/dynamics/joint/generic_joint.rs
index cfea537..2aa9a51 100644
--- a/src/dynamics/joint/generic_joint.rs
+++ b/src/dynamics/joint/generic_joint.rs
@@ -1,4 +1,6 @@
+use crate::dynamics::RevoluteJoint;
 use crate::math::{Isometry, Real, SpacialVector, SPATIAL_DIM};
+use crate::na::{Rotation3, UnitQuaternion};
 
 #[derive(Copy, Clone, Debug)]
 #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
@@ -44,3 +46,19 @@ impl GenericJoint {
         }
     }
 }
+
+impl From<RevoluteJoint> for GenericJoint {
+    fn from(joint: RevoluteJoint) -> Self {
+        let basis1 = [joint.local_axis1, joint.basis1[0], joint.basis1[1]];
+        let basis2 = [joint.local_axis2, joint.basis2[0], joint.basis2[1]];
+        let quat1 = UnitQuaternion::from_basis_unchecked(&basis1[..]);
+        let quat2 = UnitQuaternion::from_basis_unchecked(&basis2[..]);
+        let local_anchor1 = Isometry::from_parts(joint.local_anchor1.coords.into(), quat1);
+        let local_anchor2 = Isometry::from_parts(joint.local_anchor2.coords.into(), quat2);
+
+        let mut result = Self::new(local_anchor1, local_anchor2);
+        result.min_position[3] = -Real::MAX;
+        result.max_position[3] = Real::MAX;
+        result
+    }
+}
diff --git a/src/dynamics/solver/joint_constraint/generic_position_constraint.rs b/src/dynamics/solver/joint_constraint/generic_position_constraint.rs
index f5138ea..1e80311 100644
--- a/src/dynamics/solver/joint_constraint/generic_position_constraint.rs
+++ b/src/dynamics/solver/joint_constraint/generic_position_constraint.rs
@@ -1,4 +1,5 @@
 use super::{GenericVelocityConstraint, GenericVelocityGroundConstraint};
+use crate::dynamics::solver::DeltaVel;
 use crate::dynamics::{GenericJoint, IntegrationParameters, RigidBody};
 use crate::math::{
     AngDim, AngVector, AngularInertia, Dim, Isometry, Point, Real, Rotation, SpatialVector, Vector,
@@ -22,9 +23,6 @@ pub(crate) struct GenericPositionConstraint {
     ii2: AngularInertia<Real>,
 
     joint: GenericJoint,
-
-    lin_impulse: Cell<Vector3<Real>>,
-    ang_impulse: Cell<Vector3<Real>>,
 }
 
 impl GenericPositionConstraint {
@@ -58,7 +56,11 @@ impl GenericPositionConstraint {
         let r1 = Point::from(anchor1.translation.vector) - position1 * self.local_com1;
         let r2 = Point::from(anchor2.translation.vector) - position2 * self.local_com2;
 
-        let delta_pos = anchor1.inverse() * anchor2;
+        let delta_pos = Isometry::from_parts(
+            anchor2.translation * anchor1.translation.inverse(),
+            anchor2.rotation * anchor1.rotation.inverse(),
+        );
+
         let mass_matrix = GenericVelocityConstraint::compute_mass_matrix(
             &self.joint,
             self.im1,
@@ -70,11 +72,15 @@ impl GenericPositionConstraint {
             false,
         );
 
-        let lin_err = delta_pos.translation.vector * params.joint_erp;
-        let ang_err = delta_pos.rotation.scaled_axis() * params.joint_erp;
-        let err = Vector6::new(
-            lin_err.x, lin_err.y, lin_err.z, ang_err.x, ang_err.y, ang_err.z,
+        let lin_dpos = delta_pos.translation.vector;
+        let ang_dpos = delta_pos.rotation.scaled_axis();
+        let dpos = Vector6::new(
+            lin_dpos.x, lin_dpos.y, lin_dpos.z, ang_dpos.x, ang_dpos.y, ang_dpos.z,
         );
+        let err = dpos
+            - dpos
+                .sup(&self.joint.min_position)
+                .inf(&self.joint.max_position);
         let impulse = mass_matrix * err;
         let lin_impulse = impulse.xyz();
         let ang_impulse = Vector3::new(impulse[3], impulse[4], impulse[5]);
@@ -94,6 +100,15 @@ impl GenericPositionConstraint {
         positions[self.position1 as usize] = position1;
         positions[self.position2 as usize] = position2;
     }
+
+    pub fn solve2(
+        &self,
+        params: &IntegrationParameters,
+        positions: &mut [Isometry<Real>],
+        dpos: &mut [DeltaVel<Real>],
+    ) {
+        return;
+    }
 }
 
 #[derive(Debug)]
@@ -142,7 +157,10 @@ impl GenericPositionGroundConstraint {
         let anchor2 = position2 * self.local_anchor2;
         let r2 = Point::from(anchor2.translation.vector) - position2 * self.local_com2;
 
-        let delta_pos = self.anchor1.inverse() * anchor2;
+        let delta_pos = Isometry::from_parts(
+            anchor2.translation * self.anchor1.translation.inverse(),
+            anchor2.rotation * self.anchor1.rotation.inverse(),
+        );
         let mass_matrix = GenericVelocityGroundConstraint::compute_mass_matrix(
             &self.joint,
             self.im2,
@@ -151,11 +169,15 @@ impl GenericPositionGroundConstraint {
             false,
         );
 
-        let lin_err = delta_pos.translation.vector * params.joint_erp;
-        let ang_err = Vector3::zeros(); // delta_pos.rotation.scaled_axis() * params.joint_erp;
-        let err = Vector6::new(
-            lin_err.x, lin_err.y, lin_err.z, ang_err.x, ang_err.y, ang_err.z,
+        let lin_dpos = delta_pos.translation.vector;
+        let ang_dpos = delta_pos.rotation.scaled_axis();
+        let dpos = Vector6::new(
+            lin_dpos.x, lin_dpos.y, lin_dpos.z, ang_dpos.x, ang_dpos.y, ang_dpos.z,
         );
+        let err = dpos
+            - dpos
+                .sup(&self.joint.min_position)
+                .inf(&self.joint.max_position);
         let impulse = mass_matrix * err;
         let lin_impulse = impulse.xyz();
         let ang_impulse = Vector3::new(impulse[3], impulse[4], impulse[5]);
@@ -168,4 +190,13 @@ impl GenericPositionGroundConstraint {
 
         positions[self.position2 as usize] = position2;
     }
+
+    pub fn solve2(
+        &self,
+        params: &IntegrationParameters,
+        positions: &mut [Isometry<Real>],
+        dpos: &mut [DeltaVel<Real>],
+    ) {
+        return;
+    }
 }
diff --git a/src/dynamics/solver/joint_constraint/generic_velocity_constraint.rs b/src/dynamics/solver/joint_constraint/generic_velocity_constraint.rs
index 6f501d2..db8010b 100644
--- a/src/dynamics/solver/joint_constraint/generic_velocity_constraint.rs
+++ b/src/dynamics/solver/joint_constraint/generic_velocity_constraint.rs
@@ -2,8 +2,8 @@ use crate::dynamics::solver::DeltaVel;
 use crate::dynamics::{
     GenericJoint, IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RigidBody,
 };
-use crate::math::{AngularInertia, Dim, Real, SpacialVector, Vector};
-use crate::parry::math::SpatialVector;
+use crate::math::{AngularInertia, Dim, Isometry, Real, SpacialVector, Vector, DIM};
+use crate::parry::math::{AngDim, SpatialVector};
 use crate::utils::{WAngularInertia, WCross, WCrossMatrix};
 #[cfg(feature = "dim2")]
 use na::{Matrix3, Vector3};
@@ -18,6 +18,8 @@ pub(crate) struct GenericVelocityConstraint {
     joint_id: JointIndex,
 
     impulse: SpacialVector<Real>,
+    pos_impulse: SpacialVector<Real>,
+
     max_positive_impulse: SpatialVector<Real>,
     max_negative_impulse: SpatialVector<Real>,
 
@@ -31,6 +33,8 @@ pub(crate) struct GenericVelocityConstraint {
     #[cfg(feature = "dim2")]
     rhs: Vector3<Real>,
 
+    pos_rhs: Vector6<Real>,
+
     im1: Real,
     im2: Real,
 
@@ -88,16 +92,11 @@ impl GenericVelocityConstraint {
                         || joint.max_positive_impulse[i] < Real::MAX
                     {
                         let diag = lhs[(i, i)];
+                        lhs.column_mut(i).fill(0.0);
                         lhs.row_mut(i).fill(0.0);
                         lhs[(i, i)] = diag;
                     }
                 }
-            } else {
-                for i in 0..6 {
-                    let diag = lhs[(i, i)];
-                    lhs.row_mut(i).fill(0.0);
-                    lhs[(i, i)] = diag;
-                }
             }
         }
 
@@ -144,13 +143,38 @@ impl GenericVelocityConstraint {
         let inv_lhs = Self::compute_mass_matrix(joint, im1, im2, ii1, ii2, r1, r2, true);
 
         #[cfg(feature = "dim2")]
-        let rhs = Vector3::new(lin_dvel.x, lin_dvel.y, ang_dvel);
+        let dvel = Vector3::new(lin_dvel.x, lin_dvel.y, ang_dvel);
 
         #[cfg(feature = "dim3")]
-        let rhs = Vector6::new(
+        let dvel = Vector6::new(
             lin_dvel.x, lin_dvel.y, lin_dvel.z, ang_dvel.x, ang_dvel.y, ang_dvel.z,
         );
 
+        let target_linvel = anchor2 * joint.target_velocity.xyz();
+        let target_angvel = anchor2 * joint.target_velocity.fixed_rows::<AngDim>(DIM).into_owned();
+        let target_vel = Vector6::new(
+            target_linvel.x,
+            target_linvel.y,
+            target_linvel.z,
+            target_angvel.x,
+            target_angvel.y,
+            target_angvel.z,
+        );
+
+        let rhs = dvel - dvel.sup(&target_vel).inf(&target_vel);
+
+        let delta_pos = Isometry::from_parts(
+            anchor2.translation * anchor1.translation.inverse(),
+            anchor2.rotation * anchor1.rotation.inverse(),
+        );
+        let lin_dpos = delta_pos.translation.vector;
+        let ang_dpos = delta_pos.rotation.scaled_axis();
+        let dpos = Vector6::new(
+            lin_dpos.x, lin_dpos.y, lin_dpos.z, ang_dpos.x, ang_dpos.y, ang_dpos.z,
+        );
+        let err = dpos - dpos.sup(&joint.min_position).inf(&joint.max_position);
+        let pos_rhs = err * params.inv_dt() * params.joint_erp;
+
         let impulse = (joint.impulse * params.warmstart_coeff)
             .inf(&joint.max_positive_impulse)
             .sup(&joint.max_negative_impulse);
@@ -166,12 +190,14 @@ impl GenericVelocityConstraint {
             ii1_sqrt: rb1.effective_world_inv_inertia_sqrt,
             ii2_sqrt: rb2.effective_world_inv_inertia_sqrt,
             impulse,
+            pos_impulse: na::zero(),
             max_positive_impulse: joint.max_positive_impulse,
             max_negative_impulse: joint.max_negative_impulse,
             inv_lhs,
             r1,
             r2,
             rhs,
+            pos_rhs,
         }
     }
 
@@ -200,9 +226,64 @@ impl GenericVelocityConstraint {
     }
 
     pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<Real>]) {
+        return;
+        let mut mj_lambda1 = mj_lambdas[self.mj_lambda1 as usize];
+        let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize];
+
+        let ang_vel1 = self.ii1_sqrt.transform_vector(mj_lambda1.angular);
+        let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular);
+
+        let dlinvel = -mj_lambda1.linear - ang_vel1.gcross(self.r1)
+            + mj_lambda2.linear
+            + ang_vel2.gcross(self.r2);
+        let dangvel = -ang_vel1 + ang_vel2;
+
+        #[cfg(feature = "dim2")]
+        let rhs = Vector3::new(dlinvel.x, dlinvel.y, dangvel) + self.rhs;
+        #[cfg(feature = "dim3")]
+        let dvel = Vector6::new(
+            dlinvel.x, dlinvel.y, dlinvel.z, dangvel.x, dangvel.y, dangvel.z,
+        ) + self.rhs;
+
+        let new_impulse = (self.impulse + self.inv_lhs * dvel)
+            .sup(&self.max_negative_impulse)
+            .inf(&self.max_positive_impulse);
+        let effective_impulse = new_impulse - self.impulse;
+        self.impulse = new_impulse;
+
+        let lin_impulse = effective_impulse.fixed_rows::<Dim>(0).into_owned();
+        #[cfg(feature = "dim2")]
+        let ang_impulse = effective_impulse[2];
+        #[cfg(feature = "dim3")]
+        let ang_impulse = effective_impulse.fixed_rows::<U3>(3).into_owned();
+
+        mj_lambda1.linear += self.im1 * lin_impulse;
+        mj_lambda1.angular += self
+            .ii1_sqrt
+            .transform_vector(ang_impulse + self.r1.gcross(lin_impulse));
+
+        mj_lambda2.linear -= self.im2 * lin_impulse;
+        mj_lambda2.angular -= self
+            .ii2_sqrt
+            .transform_vector(ang_impulse + self.r2.gcross(lin_impulse));
+
+        mj_lambdas[self.mj_lambda1 as usize] = mj_lambda1;
+        mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2;
+    }
+
+    pub fn solve2(
+        &mut self,
+        mj_lambdas: &mut [DeltaVel<Real>],
+        mj_lambdas_pos: &mut [DeltaVel<Real>],
+    ) {
         let mut mj_lambda1 = mj_lambdas[self.mj_lambda1 as usize];
         let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize];
+        let mut mj_lambda_pos1 = mj_lambdas_pos[self.mj_lambda1 as usize];
+        let mut mj_lambda_pos2 = mj_lambdas_pos[self.mj_lambda2 as usize];
 
+        /*
+         * Solve velocity.
+         */
         let ang_vel1 = self.ii1_sqrt.transform_vector(mj_lambda1.angular);
         let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular);
 
@@ -240,8 +321,47 @@ impl GenericVelocityConstraint {
             .ii2_sqrt
             .transform_vector(ang_impulse + self.r2.gcross(lin_impulse));
 
+        /*
+         * Solve positions.
+         */
+
+        let ang_pos1 = self.ii1_sqrt.transform_vector(mj_lambda_pos1.angular);
+        let ang_pos2 = self.ii2_sqrt.transform_vector(mj_lambda_pos2.angular);
+
+        let dlinpos = -mj_lambda_pos1.linear - ang_pos1.gcross(self.r1)
+            + mj_lambda_pos2.linear
+            + ang_pos2.gcross(self.r2);
+        let dangpos = -ang_pos1 + ang_pos2;
+
+        #[cfg(feature = "dim3")]
+        let dpos = Vector6::new(
+            dlinpos.x, dlinpos.y, dlinpos.z, dangpos.x, dangpos.y, dangpos.z,
+        ) + self.pos_rhs;
+
+        let new_impulse = self.pos_impulse + self.inv_lhs * dpos;
+        let effective_impulse = new_impulse - self.pos_impulse;
+        self.pos_impulse = new_impulse;
+
+        let lin_impulse = effective_impulse.fixed_rows::<Dim>(0).into_owned();
+        #[cfg(feature = "dim2")]
+        let ang_impulse = effective_impulse[2];
+        #[cfg(feature = "dim3")]
+        let ang_impulse = effective_impulse.fixed_rows::<U3>(3).into_owned();
+
+        mj_lambda_pos1.linear += self.im1 * lin_impulse;
+        mj_lambda_pos1.angular += self
+            .ii1_sqrt
+            .transform_vector(ang_impulse + self.r1.gcross(lin_impulse));
+
+        mj_lambda_pos2.linear -= self.im2 * lin_impulse;
+        mj_lambda_pos2.angular -= self
+            .ii2_sqrt
+            .transform_vector(ang_impulse + self.r2.gcross(lin_impulse));
+
         mj_lambdas[self.mj_lambda1 as usize] = mj_lambda1;
         mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2;
+        mj_lambdas_pos[self.mj_lambda1 as usize] = mj_lambda_pos1;
+        mj_lambdas_pos[self.mj_lambda2 as usize] = mj_lambda_pos2;
     }
 
     pub fn writeback_impulses(&self, joints_all: &mut [JointGraphEdge]) {
@@ -259,6 +379,8 @@ pub(crate) struct GenericVelocityGroundConstraint {
     joint_id: JointIndex,
 
     impulse: SpacialVector<Real>,
+    pos_impulse: SpacialVector<Real>,
+
     max_positive_impulse: SpatialVector<Real>,
     max_negative_impulse: SpatialVector<Real>,
 
@@ -272,6 +394,8 @@ pub(crate) struct GenericVelocityGroundConstraint {
     #[cfg(feature = "dim2")]
     rhs: Vector3<Real>,
 
+    pos_rhs: Vector6<Real>,
+
     im2: Real,
     ii2: AngularInertia<Real>,
     ii2_sqrt: AngularInertia<Real>,
@@ -317,6 +441,7 @@ impl GenericVelocityGroundConstraint {
                         || joint.max_positive_impulse[i] < Real::MAX
                     {
                         let diag = lhs[(i, i)];
+                        lhs.column_mut(i).fill(0.0);
                         lhs.row_mut(i).fill(0.0);
                         lhs[(i, i)] = diag;
                     }
@@ -374,11 +499,35 @@ impl GenericVelocityGroundConstraint {
         let ang_dvel = rb2.angvel - rb1.angvel;
 
         #[cfg(feature = "dim2")]
-        let rhs = Vector3::new(lin_dvel.x, lin_dvel.y, ang_dvel);
+        let dvel = Vector3::new(lin_dvel.x, lin_dvel.y, ang_dvel);
         #[cfg(feature = "dim3")]
-        let rhs = Vector6::new(
+        let dvel = Vector6::new(
             lin_dvel.x, lin_dvel.y, lin_dvel.z, ang_dvel.x, ang_dvel.y, ang_dvel.z,
         );
+        let target_linvel = anchor2 * joint.target_velocity.xyz();
+        let target_angvel = anchor2 * joint.target_velocity.fixed_rows::<AngDim>(DIM).into_owned();
+        let target_vel = Vector6::new(
+            target_linvel.x,
+            target_linvel.y,
+            target_linvel.z,
+            target_angvel.x,
+            target_angvel.y,
+            target_angvel.z,
+        );
+
+        let mut rhs = dvel - dvel.sup(&target_vel).inf(&target_vel);
+
+        let delta_pos = Isometry::from_parts(
+            anchor2.translation * anchor1.translation.inverse(),
+            anchor2.rotation * anchor1.rotation.inverse(),
+        );
+        let lin_dpos = delta_pos.translation.vector;
+        let ang_dpos = delta_pos.rotation.scaled_axis();
+        let dpos = Vector6::new(
+            lin_dpos.x, lin_dpos.y, lin_dpos.z, ang_dpos.x, ang_dpos.y, ang_dpos.z,
+        );
+        let err = dpos - dpos.sup(&joint.min_position).inf(&joint.max_position);
+        let pos_rhs = err * params.inv_dt() * params.joint_erp;
 
         let impulse = (joint.impulse * params.warmstart_coeff)
             .inf(&joint.max_positive_impulse)
@@ -391,11 +540,13 @@ impl GenericVelocityGroundConstraint {
             ii2,
             ii2_sqrt: rb2.effective_world_inv_inertia_sqrt,
             impulse,
+            pos_impulse: na::zero(),
             max_positive_impulse: joint.max_positive_impulse,
             max_negative_impulse: joint.max_negative_impulse,
             inv_lhs,
             r2,
             rhs,
+            pos_rhs,
         }
     }
 
@@ -417,8 +568,51 @@ impl GenericVelocityGroundConstraint {
     }
 
     pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<Real>]) {
+        return;
+        let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize];
+
+        let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular);
+
+        let dlinvel = mj_lambda2.linear + ang_vel2.gcross(self.r2);
+        let dangvel = ang_vel2;
+        #[cfg(feature = "dim2")]
+        let rhs = Vector3::new(dlinvel.x, dlinvel.y, dangvel) + self.rhs;
+        #[cfg(feature = "dim3")]
+        let dvel = Vector6::new(
+            dlinvel.x, dlinvel.y, dlinvel.z, dangvel.x, dangvel.y, dangvel.z,
+        ) + self.rhs;
+
+        let new_impulse = (self.impulse + self.inv_lhs * dvel)
+            .sup(&self.max_negative_impulse)
+            .inf(&self.max_positive_impulse);
+        let effective_impulse = new_impulse - self.impulse;
+        self.impulse = new_impulse;
+
+        let lin_impulse = effective_impulse.fixed_rows::<Dim>(0).into_owned();
+        #[cfg(feature = "dim2")]
+        let ang_impulse = effective_impulse[2];
+        #[cfg(feature = "dim3")]
+        let ang_impulse = effective_impulse.fixed_rows::<U3>(3).into_owned();
+
+        mj_lambda2.linear -= self.im2 * lin_impulse;
+        mj_lambda2.angular -= self
+            .ii2_sqrt
+            .transform_vector(ang_impulse + self.r2.gcross(lin_impulse));
+
+        mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2;
+    }
+
+    pub fn solve2(
+        &mut self,
+        mj_lambdas: &mut [DeltaVel<Real>],
+        mj_lambdas_pos: &mut [DeltaVel<Real>],
+    ) {
         let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize];
+        let mut mj_lambda_pos2 = mj_lambdas_pos[self.mj_lambda2 as usize];
 
+        /*
+         * Solve velocities.
+         */
         let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular);
 
         let dlinvel = mj_lambda2.linear + ang_vel2.gcross(self.r2);
@@ -447,7 +641,37 @@ impl GenericVelocityGroundConstraint {
             .ii2_sqrt
             .transform_vector(ang_impulse + self.r2.gcross(lin_impulse));
 
+        /*
+         * Solve positions.
+         */
+        let ang_pos2 = self.ii2_sqrt.transform_vector(mj_lambda_pos2.angular);
+
+        let dlinpos = mj_lambda_pos2.linear + ang_pos2.gcross(self.r2);
+        let dangpos = ang_pos2;
+        #[cfg(feature = "dim2")]
+        let rhs = Vector3::new(dlinpos.x, dlinpos.y, dangpos) + self.rhs;
+        #[cfg(feature = "dim3")]
+        let dpos = Vector6::new(
+            dlinpos.x, dlinpos.y, dlinpos.z, dangpos.x, dangpos.y, dangpos.z,
+        ) + self.pos_rhs;
+
+        let new_impulse = self.pos_impulse + self.inv_lhs * dpos;
+        let effective_impulse = new_impulse - self.pos_impulse;
+        self.pos_impulse = new_impulse;
+
+        let lin_impulse = effective_impulse.fixed_rows::<Dim>(0).into_owned();
+        #[cfg(feature = "dim2")]
+        let ang_impulse = effective_impulse[2];
+        #[cfg(feature = "dim3")]
+        let ang_impulse = effective_impulse.fixed_rows::<U3>(3).into_owned();
+
+        mj_lambda_pos2.linear -= self.im2 * lin_impulse;
+        mj_lambda_pos2.angular -= self
+            .ii2_sqrt
+            .transform_vector(ang_impulse + self.r2.gcross(lin_impulse));
+
         mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2;
+        mj_lambdas_pos[self.mj_lambda2 as usize] = mj_lambda_pos2;
     }
 
     // FIXME: duplicated code with the non-ground constraint.
diff --git a/src/dynamics/solver/joint_constraint/joint_constraint.rs b/src/dynamics/solver/joint_constraint/joint_constraint.rs
index 12b8f77..78332e8 100644
--- a/src/dynamics/solver/joint_constraint/joint_constraint.rs
+++ b/src/dynamics/solver/joint_constraint/joint_constraint.rs
@@ -333,6 +333,23 @@ impl AnyJointVelocityConstraint {
         }
     }
 
+    pub fn solve2(
+        &mut self,
+        mj_lambdas: &mut [DeltaVel<Real>],
+        mj_lambdas_pos: &mut [DeltaVel<Real>],
+    ) {
+        match self {
+            AnyJointVelocityConstraint::GenericConstraint(c) => {
+                c.solve2(mj_lambdas, mj_lambdas_pos)
+            }
+            AnyJointVelocityConstraint::GenericGroundConstraint(c) => {
+                c.solve2(mj_lambdas, mj_lambdas_pos)
+            }
+            AnyJointVelocityConstraint::Empty => unreachable!(),
+            _ => {}
+        }
+    }
+
     pub fn writeback_impulses(&self, joints_all: &mut [JointGraphEdge]) {
         match self {
             AnyJointVelocityConstraint::BallConstraint(c) => c.writeback_impulses(joints_all),
diff --git a/src/dynamics/solver/joint_constraint/joint_position_constraint.rs b/src/dynamics/solver/joint_constraint/joint_position_constraint.rs
index 29acac6..0c35883 100644
--- a/src/dynamics/solver/joint_constraint/joint_position_constraint.rs
+++ b/src/dynamics/solver/joint_constraint/joint_position_constraint.rs
@@ -14,6 +14,7 @@ use super::{
     WFixedPositionGroundConstraint, WGenericPositionConstraint, WGenericPositionGroundConstraint,
     WPrismaticPositionConstraint, WPrismaticPositionGroundConstraint,
 };
+use crate::dynamics::solver::DeltaVel;
 use crate::dynamics::{IntegrationParameters, Joint, JointParams, RigidBodySet};
 #[cfg(feature = "simd-is-enabled")]
 use crate::math::SIMD_WIDTH;
@@ -241,4 +242,20 @@ impl AnyJointPositionConstraint {
             AnyJointPositionConstraint::Empty => unreachable!(),
         }
     }
+
+    pub fn solve2(
+        &self,
+        params: &IntegrationParameters,
+        positions: &mut [Isometry<Real>],
+        dpos: &mut [DeltaVel<Real>],
+    ) {
+        match self {
+            AnyJointPositionConstraint::GenericJoint(c) => c.solve2(params, positions, dpos),
+            AnyJointPositionConstraint::GenericGroundConstraint(c) => {
+                c.solve2(params, positions, dpos)
+            }
+            _ => {}
+            AnyJointPositionConstraint::Empty => unreachable!(),
+        }
+    }
 }