Joint API and joint motors improvements

1 files changed, 338 insertions, 67 deletions

diff --git a/src/dynamics/solver/joint_constraint/joint_velocity_constraint_builder.rs b/src/dynamics/solver/joint_constraint/joint_velocity_constraint_builder.rs
index 3aa00f2..95ab288 100644
--- a/src/dynamics/solver/joint_constraint/joint_velocity_constraint_builder.rs
+++ b/src/dynamics/solver/joint_constraint/joint_velocity_constraint_builder.rs
@@ -3,8 +3,8 @@ use crate::dynamics::solver::joint_constraint::joint_velocity_constraint::{
 };
 use crate::dynamics::solver::joint_constraint::SolverBody;
 use crate::dynamics::solver::MotorParameters;
-use crate::dynamics::{IntegrationParameters, JointIndex};
-use crate::math::{Isometry, Matrix, Point, Real, Rotation, Vector, ANG_DIM, DIM};
+use crate::dynamics::{IntegrationParameters, JointAxesMask, JointIndex, JointLimits, JointMotor};
+use crate::math::{AngVector, Isometry, Matrix, Point, Real, Rotation, Vector, ANG_DIM, DIM};
 use crate::utils::{IndexMut2, WCrossMatrix, WDot, WQuat, WReal};
 use na::SMatrix;
 
@@ -92,10 +92,12 @@ impl<N: WReal> JointVelocityConstraintBuilder<N> {
         let min_enabled = dist.simd_lt(limits[0]);
         let max_enabled = limits[1].simd_lt(dist);
 
-        let erp_inv_dt = N::splat(params.erp_inv_dt());
+        let erp_inv_dt = N::splat(params.joint_erp_inv_dt());
+        let cfm_coeff = N::splat(params.joint_cfm_coeff());
         let rhs_bias =
             ((dist - limits[1]).simd_max(zero) - (limits[0] - dist).simd_max(zero)) * erp_inv_dt;
         constraint.rhs = constraint.rhs_wo_bias + rhs_bias;
+        constraint.cfm_coeff = cfm_coeff;
         constraint.impulse_bounds = [
             N::splat(-Real::INFINITY).select(min_enabled, zero),
             N::splat(Real::INFINITY).select(max_enabled, zero),
@@ -104,39 +106,114 @@ impl<N: WReal> JointVelocityConstraintBuilder<N> {
         constraint
     }
 
+    pub fn limit_linear_coupled<const LANES: usize>(
+        &self,
+        params: &IntegrationParameters,
+        joint_id: [JointIndex; LANES],
+        body1: &SolverBody<N, LANES>,
+        body2: &SolverBody<N, LANES>,
+        limited_coupled_axes: u8,
+        limits: &[JointLimits<N>],
+        writeback_id: WritebackId,
+    ) -> JointVelocityConstraint<N, LANES> {
+        let zero = N::zero();
+        let mut lin_jac = Vector::zeros();
+        let mut ang_jac1: AngVector<N> = na::zero();
+        let mut ang_jac2: AngVector<N> = na::zero();
+        let mut limit = N::zero();
+
+        for i in 0..DIM {
+            if limited_coupled_axes & (1 << i) != 0 {
+                let coeff = self.basis.column(i).dot(&self.lin_err);
+                lin_jac += self.basis.column(i) * coeff;
+                #[cfg(feature = "dim2")]
+                {
+                    ang_jac1 += self.cmat1_basis[i] * coeff;
+                    ang_jac2 += self.cmat2_basis[i] * coeff;
+                }
+                #[cfg(feature = "dim3")]
+                {
+                    ang_jac1 += self.cmat1_basis.column(i) * coeff;
+                    ang_jac2 += self.cmat2_basis.column(i) * coeff;
+                }
+                limit += limits[i].max * limits[i].max;
+            }
+        }
+
+        limit = limit.simd_sqrt();
+        let dist = lin_jac.norm();
+        let inv_dist = crate::utils::simd_inv(dist);
+        lin_jac *= inv_dist;
+        ang_jac1 *= inv_dist;
+        ang_jac2 *= inv_dist;
+
+        let dvel = lin_jac.dot(&(body2.linvel - body1.linvel))
+            + (ang_jac2.gdot(body2.angvel) - ang_jac1.gdot(body1.angvel));
+        let rhs_wo_bias = dvel + (dist - limit).simd_min(zero) * N::splat(params.inv_dt());
+
+        ang_jac1 = body1.sqrt_ii * ang_jac1;
+        ang_jac2 = body2.sqrt_ii * ang_jac2;
+
+        let erp_inv_dt = N::splat(params.joint_erp_inv_dt());
+        let cfm_coeff = N::splat(params.joint_cfm_coeff());
+        let rhs_bias = (dist - limit).simd_max(zero) * erp_inv_dt;
+        let rhs = rhs_wo_bias + rhs_bias;
+        let impulse_bounds = [N::zero(), N::splat(Real::INFINITY)];
+
+        JointVelocityConstraint {
+            joint_id,
+            mj_lambda1: body1.mj_lambda,
+            mj_lambda2: body2.mj_lambda,
+            im1: body1.im,
+            im2: body2.im,
+            impulse: N::zero(),
+            impulse_bounds,
+            lin_jac,
+            ang_jac1,
+            ang_jac2,
+            inv_lhs: N::zero(), // Will be set during ortogonalization.
+            cfm_coeff,
+            cfm_gain: N::zero(),
+            rhs,
+            rhs_wo_bias,
+            writeback_id,
+        }
+    }
+
     pub fn motor_linear<const LANES: usize>(
         &self,
         params: &IntegrationParameters,
         joint_id: [JointIndex; LANES],
         body1: &SolverBody<N, LANES>,
         body2: &SolverBody<N, LANES>,
-        _locked_ang_axes: u8,
         motor_axis: usize,
         motor_params: &MotorParameters<N>,
+        limits: Option<[N; 2]>,
         writeback_id: WritebackId,
     ) -> JointVelocityConstraint<N, LANES> {
+        let inv_dt = N::splat(params.inv_dt());
         let mut constraint =
             self.lock_linear(params, joint_id, body1, body2, motor_axis, writeback_id);
 
-        // if locked_ang_axes & (1 << motor_axis) != 0 {
-        //     // FIXME: check that this also works for cases
-        //     // when not all the angular axes are locked.
-        //     constraint.ang_jac1 = na::zero();
-        //     constraint.ang_jac2 = na::zero();
-        // }
-
         let mut rhs_wo_bias = N::zero();
-        if motor_params.stiffness != N::zero() {
+        if motor_params.erp_inv_dt != N::zero() {
             let dist = self.lin_err.dot(&constraint.lin_jac);
-            rhs_wo_bias += (dist - motor_params.target_pos) * motor_params.stiffness;
+            rhs_wo_bias += (dist - motor_params.target_pos) * motor_params.erp_inv_dt;
         }
 
-        if motor_params.damping != N::zero() {
-            let dvel = constraint.lin_jac.dot(&(body2.linvel - body1.linvel))
-                + (constraint.ang_jac2.gdot(body2.angvel) - constraint.ang_jac1.gdot(body1.angvel));
-            rhs_wo_bias += (dvel - motor_params.target_vel) * motor_params.damping;
-        }
+        let mut target_vel = motor_params.target_vel;
+        if let Some(limits) = limits {
+            let dist = self.lin_err.dot(&constraint.lin_jac);
+            target_vel =
+                target_vel.simd_clamp((limits[0] - dist) * inv_dt, (limits[1] - dist) * inv_dt);
+        };
 
+        let dvel = constraint.lin_jac.dot(&(body2.linvel - body1.linvel))
+            + (constraint.ang_jac2.gdot(body2.angvel) - constraint.ang_jac1.gdot(body1.angvel));
+        rhs_wo_bias += dvel - target_vel;
+
+        constraint.cfm_coeff = motor_params.cfm_coeff;
+        constraint.cfm_gain = motor_params.cfm_gain;
         constraint.impulse_bounds = [-motor_params.max_impulse, motor_params.max_impulse];
         constraint.rhs = rhs_wo_bias;
         constraint.rhs_wo_bias = rhs_wo_bias;
@@ -164,10 +241,11 @@ impl<N: WReal> JointVelocityConstraintBuilder<N> {
 
         let dvel = lin_jac.dot(&(body2.linvel - body1.linvel))
             + (ang_jac2.gdot(body2.angvel) - ang_jac1.gdot(body1.angvel));
-        let rhs_wo_bias = dvel * N::splat(params.velocity_solve_fraction);
+        let rhs_wo_bias = dvel;
 
-        let erp_inv_dt = params.erp_inv_dt();
-        let rhs_bias = lin_jac.dot(&self.lin_err) * N::splat(erp_inv_dt);
+        let erp_inv_dt = N::splat(params.joint_erp_inv_dt());
+        let cfm_coeff = N::splat(params.joint_cfm_coeff());
+        let rhs_bias = lin_jac.dot(&self.lin_err) * erp_inv_dt;
 
         ang_jac1 = body1.sqrt_ii * ang_jac1;
         ang_jac2 = body2.sqrt_ii * ang_jac2;
@@ -184,6 +262,8 @@ impl<N: WReal> JointVelocityConstraintBuilder<N> {
             ang_jac1,
             ang_jac2,
             inv_lhs: N::zero(), // Will be set during ortogonalization.
+            cfm_coeff,
+            cfm_gain: N::zero(),
             rhs: rhs_wo_bias + rhs_bias,
             rhs_wo_bias,
             writeback_id,
@@ -220,12 +300,13 @@ impl<N: WReal> JointVelocityConstraintBuilder<N> {
         #[cfg(feature = "dim3")]
         let ang_jac = self.ang_basis.column(limited_axis).into_owned();
         let dvel = ang_jac.gdot(body2.angvel) - ang_jac.gdot(body1.angvel);
-        let rhs_wo_bias = dvel * N::splat(params.velocity_solve_fraction);
+        let rhs_wo_bias = dvel;
 
-        let erp_inv_dt = params.erp_inv_dt();
+        let erp_inv_dt = N::splat(params.joint_erp_inv_dt());
+        let cfm_coeff = N::splat(params.joint_cfm_coeff());
         let rhs_bias = ((s_ang - s_limits[1]).simd_max(zero)
             - (s_limits[0] - s_ang).simd_max(zero))
-            * N::splat(erp_inv_dt);
+            * erp_inv_dt;
 
         let ang_jac1 = body1.sqrt_ii * ang_jac;
         let ang_jac2 = body2.sqrt_ii * ang_jac;
@@ -242,6 +323,8 @@ impl<N: WReal> JointVelocityConstraintBuilder<N> {
             ang_jac1,
             ang_jac2,
             inv_lhs: N::zero(), // Will be set during ortogonalization.
+            cfm_coeff,
+            cfm_gain: N::zero(),
             rhs: rhs_wo_bias + rhs_bias,
             rhs_wo_bias,
             writeback_id,
@@ -264,20 +347,17 @@ impl<N: WReal> JointVelocityConstraintBuilder<N> {
         let ang_jac = self.basis.column(_motor_axis).into_owned();
 
         let mut rhs_wo_bias = N::zero();
-        if motor_params.stiffness != N::zero() {
+        if motor_params.erp_inv_dt != N::zero() {
             #[cfg(feature = "dim2")]
             let s_ang_dist = self.ang_err.im;
             #[cfg(feature = "dim3")]
             let s_ang_dist = self.ang_err.imag()[_motor_axis];
             let s_target_ang = motor_params.target_pos.simd_sin();
-            rhs_wo_bias += (s_ang_dist - s_target_ang) * motor_params.stiffness;
+            rhs_wo_bias += (s_ang_dist - s_target_ang) * motor_params.erp_inv_dt;
         }
 
-        if motor_params.damping != N::zero() {
-            let dvel = ang_jac.gdot(body2.angvel) - ang_jac.gdot(body1.angvel);
-            rhs_wo_bias +=
-                (dvel - motor_params.target_vel/* * ang_jac.norm() */) * motor_params.damping;
-        }
+        let dvel = ang_jac.gdot(body2.angvel) - ang_jac.gdot(body1.angvel);
+        rhs_wo_bias += dvel - motor_params.target_vel;
 
         let ang_jac1 = body1.sqrt_ii * ang_jac;
         let ang_jac2 = body2.sqrt_ii * ang_jac;
@@ -294,6 +374,8 @@ impl<N: WReal> JointVelocityConstraintBuilder<N> {
             ang_jac1,
             ang_jac2,
             inv_lhs: N::zero(), // Will be set during ortogonalization.
+            cfm_coeff: motor_params.cfm_coeff,
+            cfm_gain: motor_params.cfm_gain,
             rhs: rhs_wo_bias,
             rhs_wo_bias,
             writeback_id,
@@ -315,13 +397,14 @@ impl<N: WReal> JointVelocityConstraintBuilder<N> {
         let ang_jac = self.ang_basis.column(locked_axis).into_owned();
 
         let dvel = ang_jac.gdot(body2.angvel) - ang_jac.gdot(body1.angvel);
-        let rhs_wo_bias = dvel * N::splat(params.velocity_solve_fraction);
+        let rhs_wo_bias = dvel;
 
-        let erp_inv_dt = params.erp_inv_dt();
+        let erp_inv_dt = N::splat(params.joint_erp_inv_dt());
+        let cfm_coeff = N::splat(params.joint_cfm_coeff());
         #[cfg(feature = "dim2")]
-        let rhs_bias = self.ang_err.im * N::splat(erp_inv_dt);
+        let rhs_bias = self.ang_err.im * erp_inv_dt;
         #[cfg(feature = "dim3")]
-        let rhs_bias = self.ang_err.imag()[locked_axis] * N::splat(erp_inv_dt);
+        let rhs_bias = self.ang_err.imag()[locked_axis] * erp_inv_dt;
 
         let ang_jac1 = body1.sqrt_ii * ang_jac;
         let ang_jac2 = body2.sqrt_ii * ang_jac;
@@ -338,6 +421,8 @@ impl<N: WReal> JointVelocityConstraintBuilder<N> {
             ang_jac1,
             ang_jac2,
             inv_lhs: N::zero(), // Will be set during ortogonalization.
+            cfm_coeff,
+            cfm_gain: N::zero(),
             rhs: rhs_wo_bias + rhs_bias,
             rhs_wo_bias,
             writeback_id,
@@ -349,6 +434,11 @@ impl<N: WReal> JointVelocityConstraintBuilder<N> {
         constraints: &mut [JointVelocityConstraint<N, LANES>],
     ) {
         let len = constraints.len();
+
+        if len == 0 {
+            return;
+        }
+
         let imsum = constraints[0].im1 + constraints[0].im2;
 
         // Use the modified Gram-Schmidt orthogonalization.
@@ -357,8 +447,10 @@ impl<N: WReal> JointVelocityConstraintBuilder<N> {
             let dot_jj = c_j.lin_jac.dot(&imsum.component_mul(&c_j.lin_jac))
                 + c_j.ang_jac1.gdot(c_j.ang_jac1)
                 + c_j.ang_jac2.gdot(c_j.ang_jac2);
+            let cfm_gain = dot_jj * c_j.cfm_coeff + c_j.cfm_gain;
             let inv_dot_jj = crate::utils::simd_inv(dot_jj);
-            c_j.inv_lhs = inv_dot_jj; // Don’t forget to update the inv_lhs.
+            c_j.inv_lhs = crate::utils::simd_inv(dot_jj + cfm_gain); // Don’t forget to update the inv_lhs.
+            c_j.cfm_gain = cfm_gain;
 
             if c_j.impulse_bounds != [-N::splat(Real::MAX), N::splat(Real::MAX)] {
                 // Don't remove constraints with limited forces from the others
@@ -369,6 +461,7 @@ impl<N: WReal> JointVelocityConstraintBuilder<N> {
 
             for i in (j + 1)..len {
                 let (c_i, c_j) = constraints.index_mut_const(i, j);
+
                 let dot_ij = c_i.lin_jac.dot(&imsum.component_mul(&c_j.lin_jac))
                     + c_i.ang_jac1.gdot(c_j.ang_jac1)
                     + c_i.ang_jac2.gdot(c_j.ang_jac2);
@@ -396,6 +489,7 @@ impl<N: WReal> JointVelocityConstraintBuilder<N> {
         let zero = N::zero();
         let lin_jac = self.basis.column(limited_axis).into_owned();
         let dist = self.lin_err.dot(&lin_jac);
+
         let min_enabled = dist.simd_lt(limits[0]);
         let max_enabled = limits[1].simd_lt(dist);
 
@@ -412,11 +506,12 @@ impl<N: WReal> JointVelocityConstraintBuilder<N> {
 
         let dvel = lin_jac.dot(&(body2.linvel - body1.linvel))
             + (ang_jac2.gdot(body2.angvel) - ang_jac1.gdot(body1.angvel));
-        let rhs_wo_bias = dvel * N::splat(params.velocity_solve_fraction);
+        let rhs_wo_bias = dvel;
 
-        let erp_inv_dt = params.erp_inv_dt();
-        let rhs_bias = ((dist - limits[1]).simd_max(zero) - (limits[0] - dist).simd_max(zero))
-            * N::splat(erp_inv_dt);
+        let erp_inv_dt = N::splat(params.joint_erp_inv_dt());
+        let cfm_coeff = N::splat(params.joint_cfm_coeff());
+        let rhs_bias =
+            ((dist - limits[1]).simd_max(zero) - (limits[0] - dist).simd_max(zero)) * erp_inv_dt;
 
         ang_jac2 = body2.sqrt_ii * ang_jac2;
 
@@ -429,21 +524,97 @@ impl<N: WReal> JointVelocityConstraintBuilder<N> {
             lin_jac,
             ang_jac2,
             inv_lhs: zero, // Will be set during ortogonalization.
+            cfm_coeff,
+            cfm_gain: N::zero(),
             rhs: rhs_wo_bias + rhs_bias,
             rhs_wo_bias,
             writeback_id,
         }
     }
 
+    pub fn limit_linear_coupled_ground<const LANES: usize>(
+        &self,
+        params: &IntegrationParameters,
+        joint_id: [JointIndex; LANES],
+        body1: &SolverBody<N, LANES>,
+        body2: &SolverBody<N, LANES>,
+        limited_coupled_axes: u8,
+        limits: &[JointLimits<N>],
+        writeback_id: WritebackId,
+    ) -> JointVelocityGroundConstraint<N, LANES> {
+        let zero = N::zero();
+        let mut lin_jac = Vector::zeros();
+        let mut ang_jac1: AngVector<N> = na::zero();
+        let mut ang_jac2: AngVector<N> = na::zero();
+        let mut limit = N::zero();
+
+        for i in 0..DIM {
+            if limited_coupled_axes & (1 << i) != 0 {
+                let coeff = self.basis.column(i).dot(&self.lin_err);
+                lin_jac += self.basis.column(i) * coeff;
+                #[cfg(feature = "dim2")]
+                {
+                    ang_jac1 += self.cmat1_basis[i] * coeff;
+                    ang_jac2 += self.cmat2_basis[i] * coeff;
+                }
+                #[cfg(feature = "dim3")]
+                {
+                    ang_jac1 += self.cmat1_basis.column(i) * coeff;
+                    ang_jac2 += self.cmat2_basis.column(i) * coeff;
+                }
+                limit += limits[i].max * limits[i].max;
+            }
+        }
+
+        limit = limit.simd_sqrt();
+        let dist = lin_jac.norm();
+        let inv_dist = crate::utils::simd_inv(dist);
+        lin_jac *= inv_dist;
+        ang_jac1 *= inv_dist;
+        ang_jac2 *= inv_dist;
+
+        let dvel = lin_jac.dot(&(body2.linvel - body1.linvel))
+            + (ang_jac2.gdot(body2.angvel) - ang_jac1.gdot(body1.angvel));
+        let rhs_wo_bias = dvel + (dist - limit).simd_min(zero) * N::splat(params.inv_dt());
+
+        ang_jac2 = body2.sqrt_ii * ang_jac2;
+
+        let erp_inv_dt = N::splat(params.joint_erp_inv_dt());
+        let cfm_coeff = N::splat(params.joint_cfm_coeff());
+        let rhs_bias = (dist - limit).simd_max(zero) * erp_inv_dt;
+        let rhs = rhs_wo_bias + rhs_bias;
+        let impulse_bounds = [N::zero(), N::splat(Real::INFINITY)];
+
+        JointVelocityGroundConstraint {
+            joint_id,
+            mj_lambda2: body2.mj_lambda,
+            im2: body2.im,
+            impulse: N::zero(),
+            impulse_bounds,
+            lin_jac,
+            ang_jac2,
+            inv_lhs: N::zero(), // Will be set during ortogonalization.
+            cfm_coeff,
+            cfm_gain: N::zero(),
+            rhs,
+            rhs_wo_bias,
+            writeback_id,
+        }
+    }
+
     pub fn motor_linear_ground<const LANES: usize>(
         &self,
+        params: &IntegrationParameters,
         joint_id: [JointIndex; LANES],
         body1: &SolverBody<N, LANES>,
         body2: &SolverBody<N, LANES>,
         motor_axis: usize,
         motor_params: &MotorParameters<N>,
+        limits: Option<[N; 2]>,
         writeback_id: WritebackId,
     ) -> JointVelocityGroundConstraint<N, LANES> {
+        let inv_dt = N::splat(params.inv_dt());
+
         let lin_jac = self.basis.column(motor_axis).into_owned();
         let ang_jac1 = self.cmat1_basis.column(motor_axis).into_owned();
         #[cfg(feature = "dim2")]
@@ -452,16 +623,21 @@ impl<N: WReal> JointVelocityConstraintBuilder<N> {
         let mut ang_jac2 = self.cmat2_basis.column(motor_axis).into_owned();
 
         let mut rhs_wo_bias = N::zero();
-        if motor_params.stiffness != N::zero() {
+        if motor_params.erp_inv_dt != N::zero() {
             let dist = self.lin_err.dot(&lin_jac);
-            rhs_wo_bias += (dist - motor_params.target_pos) * motor_params.stiffness;
+            rhs_wo_bias += (dist - motor_params.target_pos) * motor_params.erp_inv_dt;
         }
 
-        if motor_params.damping != N::zero() {
-            let dvel = lin_jac.dot(&(body2.linvel - body1.linvel))
-                + (ang_jac2.gdot(body2.angvel) - ang_jac1.gdot(body1.angvel));
-            rhs_wo_bias += (dvel - motor_params.target_vel) * motor_params.damping;
-        }
+        let mut target_vel = motor_params.target_vel;
+        if let Some(limits) = limits {
+            let dist = self.lin_err.dot(&lin_jac);
+            target_vel =
+                target_vel.simd_clamp((limits[0] - dist) * inv_dt, (limits[1] - dist) * inv_dt);
+        };
+
+        let dvel = lin_jac.dot(&(body2.linvel - body1.linvel))
+            + (ang_jac2.gdot(body2.angvel) - ang_jac1.gdot(body1.angvel));
+        rhs_wo_bias += dvel - target_vel;
 
         ang_jac2 = body2.sqrt_ii * ang_jac2;
 
@@ -474,12 +650,89 @@ impl<N: WReal> JointVelocityConstraintBuilder<N> {
             lin_jac,
             ang_jac2,
             inv_lhs: N::zero(), // Will be set during ortogonalization.
+            cfm_coeff: motor_params.cfm_coeff,
+            cfm_gain: motor_params.cfm_gain,
             rhs: rhs_wo_bias,
             rhs_wo_bias,
             writeback_id,
         }
     }
 
+    pub fn motor_linear_coupled_ground<const LANES: usize>(
+        &self,
+        params: &IntegrationParameters,
+        joint_id: [JointIndex; LANES],
+        body1: &SolverBody<N, LANES>,
+        body2: &SolverBody<N, LANES>,
+        motor_coupled_axes: u8,
+        motors: &[MotorParameters<N>],
+        limited_coupled_axes: u8,
+        limits: &[JointLimits<N>],
+        writeback_id: WritebackId,
+    ) -> JointVelocityGroundConstraint<N, LANES> {
+        todo!()
+        /*
+        let zero = N::zero();
+        let mut lin_jac = Vector::zeros();
+        let mut ang_jac1: AngVector<N> = na::zero();
+        let mut ang_jac2: AngVector<N> = na::zero();
+        let mut limit = N::zero();
+
+        for i in 0..DIM {
+            if limited_coupled_axes & (1 << i) != 0 {
+                let coeff = self.basis.column(i).dot(&self.lin_err);
+                lin_jac += self.basis.column(i) * coeff;
+                #[cfg(feature = "dim2")]
+                {
+                    ang_jac1 += self.cmat1_basis[i] * coeff;
+                    ang_jac2 += self.cmat2_basis[i] * coeff;
+                }
+                #[cfg(feature = "dim3")]
+                {
+                    ang_jac1 += self.cmat1_basis.column(i) * coeff;
+                    ang_jac2 += self.cmat2_basis.column(i) * coeff;
+                }
+                limit += limits[i].max * limits[i].max;
+            }
+        }
+
+        limit = limit.simd_sqrt();
+        let dist = lin_jac.norm();
+        let inv_dist = crate::utils::simd_inv(dist);
+        lin_jac *= inv_dist;
+        ang_jac1 *= inv_dist;
+        ang_jac2 *= inv_dist;
+
+        let dvel = lin_jac.dot(&(body2.linvel - body1.linvel))
+            + (ang_jac2.gdot(body2.angvel) - ang_jac1.gdot(body1.angvel));
+        let rhs_wo_bias = dvel + (dist - limit).simd_min(zero) * N::splat(params.inv_dt());
+
+        ang_jac2 = body2.sqrt_ii * ang_jac2;
+
+        let erp_inv_dt = N::splat(params.joint_erp_inv_dt());
+        let cfm_coeff = N::splat(params.joint_cfm_coeff());
+        let rhs_bias = (dist - limit).simd_max(zero) * erp_inv_dt;
+        let rhs = rhs_wo_bias + rhs_bias;
+        let impulse_bounds = [N::zero(), N::splat(Real::INFINITY)];
+
+        JointVelocityGroundConstraint {
+            joint_id,
+            mj_lambda2: body2.mj_lambda,
+            im2: body2.im,
+            impulse: N::zero(),
+            impulse_bounds,
+            lin_jac,
+            ang_jac2,
+            inv_lhs: N::zero(), // Will be set during ortogonalization.
+            cfm_coeff,
+            cfm_gain: N::zero(),
+            rhs,
+            rhs_wo_bias,
+            writeback_id,
+        }
+        */
+    }
+
     pub fn lock_linear_ground<const LANES: usize>(
         &self,
         params: &IntegrationParameters,
@@ -498,10 +751,11 @@ impl<N: WReal> JointVelocityConstraintBuilder<N> {
 
         let dvel = lin_jac.dot(&(body2.linvel - body1.linvel))
             + (ang_jac2.gdot(body2.angvel) - ang_jac1.gdot(body1.angvel));
-        let rhs_wo_bias = dvel * N::splat(params.velocity_solve_fraction);
+        let rhs_wo_bias = dvel;
 
-        let erp_inv_dt = params.erp_inv_dt();
-        let rhs_bias = lin_jac.dot(&self.lin_err) * N::splat(erp_inv_dt);
+        let erp_inv_dt = N::splat(params.joint_erp_inv_dt());
+        let cfm_coeff = N::splat(params.joint_cfm_coeff());
+        let rhs_bias = lin_jac.dot(&self.lin_err) * erp_inv_dt;
 
         ang_jac2 = body2.sqrt_ii * ang_jac2;
 
@@ -514,6 +768,8 @@ impl<N: WReal> JointVelocityConstraintBuilder<N> {
             lin_jac,
             ang_jac2,
             inv_lhs: N::zero(), // Will be set during ortogonalization.
+            cfm_coeff,
+            cfm_gain: N::zero(),
             rhs: rhs_wo_bias + rhs_bias,
             rhs_wo_bias,
             writeback_id,
@@ -536,20 +792,17 @@ impl<N: WReal> JointVelocityConstraintBuilder<N> {
         let ang_jac = self.basis.column(_motor_axis).into_owned();
 
         let mut rhs_wo_bias = N::zero();
-        if motor_params.stiffness != N::zero() {
+        if motor_params.erp_inv_dt != N::zero() {
             #[cfg(feature = "dim2")]
             let s_ang_dist = self.ang_err.im;
             #[cfg(feature = "dim3")]
             let s_ang_dist = self.ang_err.imag()[_motor_axis];
             let s_target_ang = motor_params.target_pos.simd_sin();
-            rhs_wo_bias += (s_ang_dist - s_target_ang) * motor_params.stiffness;
+            rhs_wo_bias += (s_ang_dist - s_target_ang) * motor_params.erp_inv_dt;
         }
 
-        if motor_params.damping != N::zero() {
-            let dvel = ang_jac.gdot(body2.angvel) - ang_jac.gdot(body1.angvel);
-            rhs_wo_bias +=
-                (dvel - motor_params.target_vel/* * ang_jac.norm() */) * motor_params.damping;
-        }
+        let dvel = ang_jac.gdot(body2.angvel) - ang_jac.gdot(body1.angvel);
+        rhs_wo_bias += dvel - motor_params.target_vel;
 
         let ang_jac2 = body2.sqrt_ii * ang_jac;
 
@@ -562,6 +815,8 @@ impl<N: WReal> JointVelocityConstraintBuilder<N> {
             lin_jac: na::zero(),
             ang_jac2,
             inv_lhs: N::zero(), // Will be set during ortogonalization.
+            cfm_coeff: motor_params.cfm_coeff,
+            cfm_gain: motor_params.cfm_gain,
             rhs: rhs_wo_bias,
             rhs_wo_bias,
             writeback_id,
@@ -598,12 +853,13 @@ impl<N: WReal> JointVelocityConstraintBuilder<N> {
         #[cfg(feature = "dim3")]
         let ang_jac = self.ang_basis.column(limited_axis).into_owned();
         let dvel = ang_jac.gdot(body2.angvel) - ang_jac.gdot(body1.angvel);
-        let rhs_wo_bias = dvel * N::splat(params.velocity_solve_fraction);
+        let rhs_wo_bias = dvel;
 
-        let erp_inv_dt = params.erp_inv_dt();
+        let erp_inv_dt = N::splat(params.joint_erp_inv_dt());
+        let cfm_coeff = N::splat(params.joint_cfm_coeff());
         let rhs_bias = ((s_ang - s_limits[1]).simd_max(zero)
             - (s_limits[0] - s_ang).simd_max(zero))
-            * N::splat(erp_inv_dt);
+            * erp_inv_dt;
 
         let ang_jac2 = body2.sqrt_ii * ang_jac;
 
@@ -616,6 +872,8 @@ impl<N: WReal> JointVelocityConstraintBuilder<N> {
             lin_jac: na::zero(),
             ang_jac2,
             inv_lhs: zero, // Will be set during ortogonalization.
+            cfm_coeff,
+            cfm_gain: N::zero(),
             rhs: rhs_wo_bias + rhs_bias,
             rhs_wo_bias,
             writeback_id,
@@ -636,13 +894,14 @@ impl<N: WReal> JointVelocityConstraintBuilder<N> {
         #[cfg(feature = "dim3")]
         let ang_jac = self.ang_basis.column(locked_axis).into_owned();
         let dvel = ang_jac.gdot(body2.angvel) - ang_jac.gdot(body1.angvel);
-        let rhs_wo_bias = dvel * N::splat(params.velocity_solve_fraction);
+        let rhs_wo_bias = dvel;
 
-        let erp_inv_dt = params.erp_inv_dt();
+        let erp_inv_dt = N::splat(params.joint_erp_inv_dt());
+        let cfm_coeff = N::splat(params.joint_cfm_coeff());
         #[cfg(feature = "dim2")]
-        let rhs_bias = self.ang_err.im * N::splat(erp_inv_dt);
+        let rhs_bias = self.ang_err.im * erp_inv_dt;
         #[cfg(feature = "dim3")]
-        let rhs_bias = self.ang_err.imag()[locked_axis] * N::splat(erp_inv_dt);
+        let rhs_bias = self.ang_err.imag()[locked_axis] * erp_inv_dt;
 
         let ang_jac2 = body2.sqrt_ii * ang_jac;
 
@@ -655,6 +914,8 @@ impl<N: WReal> JointVelocityConstraintBuilder<N> {
             lin_jac: na::zero(),
             ang_jac2,
             inv_lhs: N::zero(), // Will be set during ortogonalization.
+            cfm_coeff,
+            cfm_gain: N::zero(),
             rhs: rhs_wo_bias + rhs_bias,
             rhs_wo_bias,
             writeback_id,
@@ -666,6 +927,11 @@ impl<N: WReal> JointVelocityConstraintBuilder<N> {
         constraints: &mut [JointVelocityGroundConstraint<N, LANES>],
     ) {
         let len = constraints.len();
+
+        if len == 0 {
+            return;
+        }
+
         let imsum = constraints[0].im2;
 
         // Use the modified Gram-Schmidt orthogonalization.
@@ -673,18 +939,23 @@ impl<N: WReal> JointVelocityConstraintBuilder<N> {
             let c_j = &mut constraints[j];
             let dot_jj = c_j.lin_jac.dot(&imsum.component_mul(&c_j.lin_jac))
                 + c_j.ang_jac2.gdot(c_j.ang_jac2);
-            let inv_dot_jj = crate::utils::simd_inv(dot_jj);
+            let cfm_gain = dot_jj * c_j.cfm_coeff + c_j.cfm_gain;
+            let inv_dot_jj = crate::utils::simd_inv(dot_jj + cfm_gain);
             c_j.inv_lhs = inv_dot_jj; // Don’t forget to update the inv_lhs.
+            c_j.cfm_gain = cfm_gain;
 
-            if c_j.impulse_bounds != [-N::splat(Real::MAX), N::splat(Real::MAX)] {
+            if c_j.impulse_bounds != [-N::splat(Real::MAX), N::splat(Real::MAX)]
+                || c_j.cfm_gain != N::zero()
+            {
                 // Don't remove constraints with limited forces from the others
                 // because they may not deliver the necessary forces to fulfill
                 // the removed parts of other constraints.
                 continue;
             }
 
-            for i in (j + 1)..len {
+            for i in j + 1..len {
                 let (c_i, c_j) = constraints.index_mut_const(i, j);
+
                 let dot_ij = c_i.lin_jac.dot(&imsum.component_mul(&c_j.lin_jac))
                     + c_i.ang_jac2.gdot(c_j.ang_jac2);
                 let coeff = dot_ij * inv_dot_jj;