Joint API and joint motors improvements

11 files changed, 700 insertions, 275 deletions

diff --git a/src/dynamics/solver/generic_velocity_constraint.rs b/src/dynamics/solver/generic_velocity_constraint.rs
index c1d4134..a304008 100644
--- a/src/dynamics/solver/generic_velocity_constraint.rs
+++ b/src/dynamics/solver/generic_velocity_constraint.rs
@@ -261,7 +261,7 @@ impl GenericVelocityConstraint {
                     let mut rhs_wo_bias = (1.0 + is_bouncy * manifold_point.restitution)
                         * (vel1 - vel2).dot(&force_dir1);
                     rhs_wo_bias += manifold_point.dist.max(0.0) * inv_dt;
-                    rhs_wo_bias *= is_bouncy + is_resting * params.velocity_solve_fraction;
+                    rhs_wo_bias *= is_bouncy + is_resting;
                     let rhs_bias =
                         /* is_resting * */ erp_inv_dt * manifold_point.dist.min(0.0);
 
diff --git a/src/dynamics/solver/generic_velocity_ground_constraint.rs b/src/dynamics/solver/generic_velocity_ground_constraint.rs
index 9ce824e..336954b 100644
--- a/src/dynamics/solver/generic_velocity_ground_constraint.rs
+++ b/src/dynamics/solver/generic_velocity_ground_constraint.rs
@@ -145,7 +145,7 @@ impl GenericVelocityGroundConstraint {
                     let mut rhs_wo_bias = (1.0 + is_bouncy * manifold_point.restitution)
                         * (vel1 - vel2).dot(&force_dir1);
                     rhs_wo_bias += manifold_point.dist.max(0.0) * inv_dt;
-                    rhs_wo_bias *= is_bouncy + is_resting * params.velocity_solve_fraction;
+                    rhs_wo_bias *= is_bouncy + is_resting ;
                     let rhs_bias =
                         /* is_resting * */ erp_inv_dt * manifold_point.dist.min(0.0);
 
diff --git a/src/dynamics/solver/island_solver.rs b/src/dynamics/solver/island_solver.rs
index c31cee1..36c42bd 100644
--- a/src/dynamics/solver/island_solver.rs
+++ b/src/dynamics/solver/island_solver.rs
@@ -1,6 +1,6 @@
 use super::VelocitySolver;
 use crate::counters::Counters;
-use crate::data::{BundleSet, ComponentSet, ComponentSetMut};
+use crate::data::{ComponentSet, ComponentSetMut};
 use crate::dynamics::solver::{
     AnyGenericVelocityConstraint, AnyJointVelocityConstraint, AnyVelocityConstraint,
     SolverConstraints,
diff --git a/src/dynamics/solver/joint_constraint/joint_generic_velocity_constraint.rs b/src/dynamics/solver/joint_constraint/joint_generic_velocity_constraint.rs
index be42b87..4f26a73 100644
--- a/src/dynamics/solver/joint_constraint/joint_generic_velocity_constraint.rs
+++ b/src/dynamics/solver/joint_constraint/joint_generic_velocity_constraint.rs
@@ -1,7 +1,7 @@
 use crate::dynamics::solver::joint_constraint::joint_velocity_constraint::WritebackId;
 use crate::dynamics::solver::joint_constraint::{JointVelocityConstraintBuilder, SolverBody};
 use crate::dynamics::solver::DeltaVel;
-use crate::dynamics::{IntegrationParameters, JointData, JointGraphEdge, JointIndex, Multibody};
+use crate::dynamics::{GenericJoint, IntegrationParameters, JointGraphEdge, JointIndex, Multibody};
 use crate::math::{Isometry, Real, DIM};
 use crate::prelude::SPATIAL_DIM;
 use na::{DVector, DVectorSlice, DVectorSliceMut};
@@ -25,6 +25,8 @@ pub struct JointGenericVelocityConstraint {
     pub inv_lhs: Real,
     pub rhs: Real,
     pub rhs_wo_bias: Real,
+    pub cfm_coeff: Real,
+    pub cfm_gain: Real,
 
     pub writeback_id: WritebackId,
 }
@@ -52,6 +54,8 @@ impl JointGenericVelocityConstraint {
             inv_lhs: 0.0,
             rhs: 0.0,
             rhs_wo_bias: 0.0,
+            cfm_coeff: 0.0,
+            cfm_gain: 0.0,
             writeback_id: WritebackId::Dof(0),
         }
     }
@@ -65,7 +69,7 @@ impl JointGenericVelocityConstraint {
         mb2: Option<(&Multibody, usize)>,
         frame1: &Isometry<Real>,
         frame2: &Isometry<Real>,
-        joint: &JointData,
+        joint: &GenericJoint,
         jacobians: &mut DVector<Real>,
         j_id: &mut usize,
         out: &mut [Self],
@@ -83,26 +87,10 @@ impl JointGenericVelocityConstraint {
             locked_axes,
         );
 
-        for i in 0..DIM {
-            if locked_axes & (1 << i) != 0 {
-                out[len] = builder.lock_linear_generic(
-                    params,
-                    jacobians,
-                    j_id,
-                    joint_id,
-                    body1,
-                    body2,
-                    mb1,
-                    mb2,
-                    i,
-                    WritebackId::Dof(i),
-                );
-                len += 1;
-            }
-        }
+        let start = len;
         for i in DIM..SPATIAL_DIM {
-            if locked_axes & (1 << i) != 0 {
-                out[len] = builder.lock_angular_generic(
+            if (motor_axes >> DIM) & (1 << i) != 0 {
+                out[len] = builder.motor_angular_generic(
                     params,
                     jacobians,
                     j_id,
@@ -112,12 +100,12 @@ impl JointGenericVelocityConstraint {
                     mb1,
                     mb2,
                     i - DIM,
-                    WritebackId::Dof(i),
+                    &joint.motors[i].motor_params(params.dt),
+                    WritebackId::Motor(i),
                 );
                 len += 1;
             }
         }
-
         for i in 0..DIM {
             if motor_axes & (1 << i) != 0 {
                 out[len] = builder.motor_linear_generic(
@@ -137,10 +125,15 @@ impl JointGenericVelocityConstraint {
                 len += 1;
             }
         }
+        JointVelocityConstraintBuilder::finalize_generic_constraints(
+            jacobians,
+            &mut out[start..len],
+        );
 
+        let start = len;
         for i in DIM..SPATIAL_DIM {
-            if (motor_axes >> DIM) & (1 << i) != 0 {
-                out[len] = builder.motor_angular_generic(
+            if locked_axes & (1 << i) != 0 {
+                out[len] = builder.lock_angular_generic(
                     params,
                     jacobians,
                     j_id,
@@ -150,16 +143,14 @@ impl JointGenericVelocityConstraint {
                     mb1,
                     mb2,
                     i - DIM,
-                    &joint.motors[i].motor_params(params.dt),
-                    WritebackId::Motor(i),
+                    WritebackId::Dof(i),
                 );
                 len += 1;
             }
         }
-
         for i in 0..DIM {
-            if limit_axes & (1 << i) != 0 {
-                out[len] = builder.limit_linear_generic(
+            if locked_axes & (1 << i) != 0 {
+                out[len] = builder.lock_linear_generic(
                     params,
                     jacobians,
                     j_id,
@@ -169,8 +160,7 @@ impl JointGenericVelocityConstraint {
                     mb1,
                     mb2,
                     i,
-                    [joint.limits[i].min, joint.limits[i].max],
-                    WritebackId::Limit(i),
+                    WritebackId::Dof(i),
                 );
                 len += 1;
             }
@@ -194,8 +184,29 @@ impl JointGenericVelocityConstraint {
                 len += 1;
             }
         }
+        for i in 0..DIM {
+            if limit_axes & (1 << i) != 0 {
+                out[len] = builder.limit_linear_generic(
+                    params,
+                    jacobians,
+                    j_id,
+                    joint_id,
+                    body1,
+                    body2,
+                    mb1,
+                    mb2,
+                    i,
+                    [joint.limits[i].min, joint.limits[i].max],
+                    WritebackId::Limit(i),
+                );
+                len += 1;
+            }
+        }
 
-        JointVelocityConstraintBuilder::finalize_generic_constraints(jacobians, &mut out[..len]);
+        JointVelocityConstraintBuilder::finalize_generic_constraints(
+            jacobians,
+            &mut out[start..len],
+        );
         len
     }
 
@@ -273,7 +284,7 @@ impl JointGenericVelocityConstraint {
 
         let dvel = self.rhs + (vel2 - vel1);
         let total_impulse = na::clamp(
-            self.impulse + self.inv_lhs * dvel,
+            self.impulse + self.inv_lhs * (dvel - self.cfm_gain * self.impulse),
             self.impulse_bounds[0],
             self.impulse_bounds[1],
         );
@@ -316,6 +327,8 @@ pub struct JointGenericVelocityGroundConstraint {
     pub inv_lhs: Real,
     pub rhs: Real,
     pub rhs_wo_bias: Real,
+    pub cfm_coeff: Real,
+    pub cfm_gain: Real,
 
     pub writeback_id: WritebackId,
 }
@@ -338,6 +351,8 @@ impl JointGenericVelocityGroundConstraint {
             inv_lhs: 0.0,
             rhs: 0.0,
             rhs_wo_bias: 0.0,
+            cfm_coeff: 0.0,
+            cfm_gain: 0.0,
             writeback_id: WritebackId::Dof(0),
         }
     }
@@ -350,7 +365,7 @@ impl JointGenericVelocityGroundConstraint {
         mb2: (&Multibody, usize),
         frame1: &Isometry<Real>,
         frame2: &Isometry<Real>,
-        joint: &JointData,
+        joint: &GenericJoint,
         jacobians: &mut DVector<Real>,
         j_id: &mut usize,
         out: &mut [Self],
@@ -368,32 +383,20 @@ impl JointGenericVelocityGroundConstraint {
             locked_axes,
         );
 
-        for i in 0..DIM {
-            if locked_axes & (1 << i) != 0 {
-                out[len] = builder.lock_linear_generic_ground(
-                    params,
-                    jacobians,
-                    j_id,
-                    joint_id,
-                    body1,
-                    mb2,
-                    i,
-                    WritebackId::Dof(i),
-                );
-                len += 1;
-            }
-        }
+        let start = len;
         for i in DIM..SPATIAL_DIM {
-            if locked_axes & (1 << i) != 0 {
-                out[len] = builder.lock_angular_generic_ground(
+            if (motor_axes >> DIM) & (1 << i) != 0 {
+                out[len] = builder.motor_angular_generic_ground(
                     params,
                     jacobians,
                     j_id,
                     joint_id,
                     body1,
+                    body2,
                     mb2,
                     i - DIM,
-                    WritebackId::Dof(i),
+                    &joint.motors[i].motor_params(params.dt),
+                    WritebackId::Motor(i),
                 );
                 len += 1;
             }
@@ -418,27 +421,30 @@ impl JointGenericVelocityGroundConstraint {
             }
         }
 
+        JointVelocityConstraintBuilder::finalize_generic_constraints_ground(
+            jacobians,
+            &mut out[start..len],
+        );
+
+        let start = len;
         for i in DIM..SPATIAL_DIM {
-            if (motor_axes >> DIM) & (1 << i) != 0 {
-                out[len] = builder.motor_angular_generic_ground(
+            if locked_axes & (1 << i) != 0 {
+                out[len] = builder.lock_angular_generic_ground(
                     params,
                     jacobians,
                     j_id,
                     joint_id,
                     body1,
-                    body2,
                     mb2,
                     i - DIM,
-                    &joint.motors[i].motor_params(params.dt),
-                    WritebackId::Motor(i),
+                    WritebackId::Dof(i),
                 );
                 len += 1;
             }
         }
-
         for i in 0..DIM {
-            if limit_axes & (1 << i) != 0 {
-                out[len] = builder.limit_linear_generic_ground(
+            if locked_axes & (1 << i) != 0 {
+                out[len] = builder.lock_linear_generic_ground(
                     params,
                     jacobians,
                     j_id,
@@ -446,8 +452,7 @@ impl JointGenericVelocityGroundConstraint {
                     body1,
                     mb2,
                     i,
-                    [joint.limits[i].min, joint.limits[i].max],
-                    WritebackId::Limit(i),
+                    WritebackId::Dof(i),
                 );
                 len += 1;
             }
@@ -469,10 +474,26 @@ impl JointGenericVelocityGroundConstraint {
                 len += 1;
             }
         }
+        for i in 0..DIM {
+            if limit_axes & (1 << i) != 0 {
+                out[len] = builder.limit_linear_generic_ground(
+                    params,
+                    jacobians,
+                    j_id,
+                    joint_id,
+                    body1,
+                    mb2,
+                    i,
+                    [joint.limits[i].min, joint.limits[i].max],
+                    WritebackId::Limit(i),
+                );
+                len += 1;
+            }
+        }
 
         JointVelocityConstraintBuilder::finalize_generic_constraints_ground(
             jacobians,
-            &mut out[..len],
+            &mut out[start..len],
         );
         len
     }
@@ -511,7 +532,7 @@ impl JointGenericVelocityGroundConstraint {
 
         let dvel = self.rhs + vel2;
         let total_impulse = na::clamp(
-            self.impulse + self.inv_lhs * dvel,
+            self.impulse + self.inv_lhs * (dvel - self.cfm_gain * self.impulse),
             self.impulse_bounds[0],
             self.impulse_bounds[1],
         );
diff --git a/src/dynamics/solver/joint_constraint/joint_generic_velocity_constraint_builder.rs b/src/dynamics/solver/joint_constraint/joint_generic_velocity_constraint_builder.rs
index 5edc815..510f46f 100644
--- a/src/dynamics/solver/joint_constraint/joint_generic_velocity_constraint_builder.rs
+++ b/src/dynamics/solver/joint_constraint/joint_generic_velocity_constraint_builder.rs
@@ -113,7 +113,7 @@ impl JointVelocityConstraintBuilder<Real> {
             j.copy_from(&wj);
         }
 
-        let rhs_wo_bias = (vel2 - vel1) * params.velocity_solve_fraction;
+        let rhs_wo_bias = vel2 - vel1;
 
         let mj_lambda1 = mb1.map(|m| m.0.solver_id).unwrap_or(body1.mj_lambda[0]);
         let mj_lambda2 = mb2.map(|m| m.0.solver_id).unwrap_or(body2.mj_lambda[0]);
@@ -133,6 +133,8 @@ impl JointVelocityConstraintBuilder<Real> {
             inv_lhs: 0.0,
             rhs: rhs_wo_bias,
             rhs_wo_bias,
+            cfm_coeff: 0.0,
+            cfm_gain: 0.0,
             writeback_id,
         }
     }
@@ -169,7 +171,7 @@ impl JointVelocityConstraintBuilder<Real> {
             ang_jac2,
         );
 
-        let erp_inv_dt = params.erp_inv_dt();
+        let erp_inv_dt = params.joint_erp_inv_dt();
         let rhs_bias = lin_jac.dot(&self.lin_err) * erp_inv_dt;
         c.rhs += rhs_bias;
         c
@@ -212,7 +214,7 @@ impl JointVelocityConstraintBuilder<Real> {
         let min_enabled = dist < limits[0];
         let max_enabled = limits[1] < dist;
 
-        let erp_inv_dt = params.erp_inv_dt();
+        let erp_inv_dt = params.joint_erp_inv_dt();
         let rhs_bias = ((dist - limits[1]).max(0.0) - (limits[0] - dist).max(0.0)) * erp_inv_dt;
         constraint.rhs += rhs_bias;
         constraint.impulse_bounds = [
@@ -265,20 +267,20 @@ impl JointVelocityConstraintBuilder<Real> {
         );
 
         let mut rhs_wo_bias = 0.0;
-        if motor_params.stiffness != 0.0 {
+        if motor_params.erp_inv_dt != 0.0 {
             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 != 0.0 {
-            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 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;
 
         constraint.impulse_bounds = [-motor_params.max_impulse, motor_params.max_impulse];
         constraint.rhs = rhs_wo_bias;
         constraint.rhs_wo_bias = rhs_wo_bias;
+        constraint.cfm_coeff = motor_params.cfm_coeff;
+        constraint.cfm_gain = motor_params.cfm_gain;
         constraint
     }
 
@@ -312,7 +314,7 @@ impl JointVelocityConstraintBuilder<Real> {
             ang_jac,
         );
 
-        let erp_inv_dt = params.erp_inv_dt();
+        let erp_inv_dt = params.joint_erp_inv_dt();
         #[cfg(feature = "dim2")]
         let rhs_bias = self.ang_err.im * erp_inv_dt;
         #[cfg(feature = "dim3")]
@@ -364,7 +366,7 @@ impl JointVelocityConstraintBuilder<Real> {
             max_enabled as u32 as Real * Real::MAX,
         ];
 
-        let erp_inv_dt = params.erp_inv_dt();
+        let erp_inv_dt = params.joint_erp_inv_dt();
         let rhs_bias =
             ((s_ang - s_limits[1]).max(0.0) - (s_limits[0] - s_ang).max(0.0)) * erp_inv_dt;
 
@@ -409,22 +411,22 @@ impl JointVelocityConstraintBuilder<Real> {
         );
 
         let mut rhs_wo_bias = 0.0;
-        if motor_params.stiffness != 0.0 {
+        if motor_params.erp_inv_dt != 0.0 {
             #[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.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 != 0.0 {
-            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;
 
         constraint.rhs_wo_bias = rhs_wo_bias;
         constraint.rhs = rhs_wo_bias;
+        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
     }
@@ -436,6 +438,11 @@ impl JointVelocityConstraintBuilder<Real> {
         // TODO: orthogonalization doesn’t seem to give good results for multibodies?
         const ORTHOGONALIZE: bool = false;
         let len = constraints.len();
+
+        if len == 0 {
+            return;
+        }
+
         let ndofs1 = constraints[0].ndofs1;
         let ndofs2 = constraints[0].ndofs2;
 
@@ -449,8 +456,10 @@ impl JointVelocityConstraintBuilder<Real> {
             let w_jac_j2 = jacobians.rows(c_j.j_id2 + ndofs2, ndofs2);
 
             let dot_jj = jac_j1.dot(&w_jac_j1) + jac_j2.dot(&w_jac_j2);
-            let inv_dot_jj = crate::utils::inv(dot_jj);
-            c_j.inv_lhs = inv_dot_jj; // Don’t forget to update the inv_lhs.
+            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 = 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 != [-Real::MAX, Real::MAX] {
                 // Don't remove constraints with limited forces from the others
@@ -510,7 +519,7 @@ impl JointVelocityConstraintBuilder<Real> {
         let vel2 = mb2
             .fill_jacobians(link_id2, lin_jac, ang_jac2, j_id, jacobians)
             .1;
-        let rhs_wo_bias = (vel2 - vel1) * params.velocity_solve_fraction;
+        let rhs_wo_bias = vel2 - vel1;
 
         let mj_lambda2 = mb2.solver_id;
 
@@ -524,6 +533,8 @@ impl JointVelocityConstraintBuilder<Real> {
             inv_lhs: 0.0,
             rhs: rhs_wo_bias,
             rhs_wo_bias,
+            cfm_coeff: 0.0,
+            cfm_gain: 0.0,
             writeback_id,
         }
     }
@@ -556,7 +567,7 @@ impl JointVelocityConstraintBuilder<Real> {
             ang_jac2,
         );
 
-        let erp_inv_dt = params.erp_inv_dt();
+        let erp_inv_dt = params.joint_erp_inv_dt();
         let rhs_bias = lin_jac.dot(&self.lin_err) * erp_inv_dt;
         c.rhs += rhs_bias;
         c
@@ -595,7 +606,7 @@ impl JointVelocityConstraintBuilder<Real> {
         let min_enabled = dist < limits[0];
         let max_enabled = limits[1] < dist;
 
-        let erp_inv_dt = params.erp_inv_dt();
+        let erp_inv_dt = params.joint_erp_inv_dt();
         let rhs_bias = ((dist - limits[1]).max(0.0) - (limits[0] - dist).max(0.0)) * erp_inv_dt;
         constraint.rhs += rhs_bias;
         constraint.impulse_bounds = [
@@ -645,20 +656,20 @@ impl JointVelocityConstraintBuilder<Real> {
         );
 
         let mut rhs_wo_bias = 0.0;
-        if motor_params.stiffness != 0.0 {
+        if motor_params.erp_inv_dt != 0.0 {
             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 != 0.0 {
-            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 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;
 
         constraint.impulse_bounds = [-motor_params.max_impulse, motor_params.max_impulse];
         constraint.rhs = rhs_wo_bias;
         constraint.rhs_wo_bias = rhs_wo_bias;
+        constraint.cfm_coeff = motor_params.cfm_coeff;
+        constraint.cfm_gain = motor_params.cfm_gain;
         constraint
     }
 
@@ -688,7 +699,7 @@ impl JointVelocityConstraintBuilder<Real> {
             ang_jac,
         );
 
-        let erp_inv_dt = params.erp_inv_dt();
+        let erp_inv_dt = params.joint_erp_inv_dt();
         #[cfg(feature = "dim2")]
         let rhs_bias = self.ang_err.im * erp_inv_dt;
         #[cfg(feature = "dim3")]
@@ -736,7 +747,7 @@ impl JointVelocityConstraintBuilder<Real> {
             max_enabled as u32 as Real * Real::MAX,
         ];
 
-        let erp_inv_dt = params.erp_inv_dt();
+        let erp_inv_dt = params.joint_erp_inv_dt();
         let rhs_bias =
             ((s_ang - s_limits[1]).max(0.0) - (s_limits[0] - s_ang).max(0.0)) * erp_inv_dt;
 
@@ -778,22 +789,22 @@ impl JointVelocityConstraintBuilder<Real> {
         );
 
         let mut rhs = 0.0;
-        if motor_params.stiffness != 0.0 {
+        if motor_params.erp_inv_dt != 0.0 {
             #[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.sin();
-            rhs += (s_ang_dist - s_target_ang) * motor_params.stiffness;
+            rhs += (s_ang_dist - s_target_ang) * motor_params.erp_inv_dt;
         }
 
-        if motor_params.damping != 0.0 {
-            let dvel = ang_jac.gdot(body2.angvel) - ang_jac.gdot(body1.angvel);
-            rhs += (dvel - motor_params.target_vel * ang_jac.norm()) * motor_params.damping;
-        }
+        let dvel = ang_jac.gdot(body2.angvel) - ang_jac.gdot(body1.angvel);
+        rhs += dvel - motor_params.target_vel;
 
         constraint.rhs_wo_bias = rhs;
         constraint.rhs = rhs;
+        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
     }
@@ -805,6 +816,11 @@ impl JointVelocityConstraintBuilder<Real> {
         // TODO: orthogonalization doesn’t seem to give good results for multibodies?
         const ORTHOGONALIZE: bool = false;
         let len = constraints.len();
+
+        if len == 0 {
+            return;
+        }
+
         let ndofs2 = constraints[0].ndofs2;
 
         // Use the modified Gramm-Schmidt orthogonalization.
@@ -815,8 +831,10 @@ impl JointVelocityConstraintBuilder<Real> {
             let w_jac_j2 = jacobians.rows(c_j.j_id2 + ndofs2, ndofs2);
 
             let dot_jj = jac_j2.dot(&w_jac_j2);
-            let inv_dot_jj = crate::utils::inv(dot_jj);
-            c_j.inv_lhs = inv_dot_jj; // Don’t forget to update the inv_lhs.
+            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 = 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 != [-Real::MAX, Real::MAX] {
                 // Don't remove constraints with limited forces from the others
diff --git a/src/dynamics/solver/joint_constraint/joint_velocity_constraint.rs b/src/dynamics/solver/joint_constraint/joint_velocity_constraint.rs
index d264313..f830521 100644
--- a/src/dynamics/solver/joint_constraint/joint_velocity_constraint.rs
+++ b/src/dynamics/solver/joint_constraint/joint_velocity_constraint.rs
@@ -1,6 +1,8 @@
 use crate::dynamics::solver::joint_constraint::JointVelocityConstraintBuilder;
 use crate::dynamics::solver::DeltaVel;
-use crate::dynamics::{IntegrationParameters, JointData, JointGraphEdge, JointIndex};
+use crate::dynamics::{
+    GenericJoint, IntegrationParameters, JointAxesMask, JointGraphEdge, JointIndex,
+};
 use crate::math::{AngVector, AngularInertia, Isometry, Point, Real, Vector, DIM, SPATIAL_DIM};
 use crate::utils::{WDot, WReal};
 
@@ -12,10 +14,9 @@ use {
 
 #[derive(Copy, Clone, PartialEq, Debug)]
 pub struct MotorParameters<N: WReal> {
-    pub stiffness: N,
-    pub damping: N,
-    pub gamma: N,
-    // pub keep_lhs: bool,
+    pub erp_inv_dt: N,
+    pub cfm_coeff: N,
+    pub cfm_gain: N,
     pub target_pos: N,
     pub target_vel: N,
     pub max_impulse: N,
@@ -24,10 +25,9 @@ pub struct MotorParameters<N: WReal> {
 impl<N: WReal> Default for MotorParameters<N> {
     fn default() -> Self {
         Self {
-            stiffness: N::zero(),
-            damping: N::zero(),
-            gamma: N::zero(),
-            // keep_lhs: true,
+            erp_inv_dt: N::zero(),
+            cfm_coeff: N::zero(),
+            cfm_gain: N::zero(),
             target_pos: N::zero(),
             target_vel: N::zero(),
             max_impulse: N::zero(),
@@ -72,6 +72,8 @@ pub struct JointVelocityConstraint<N: WReal, const LANES: usize> {
     pub inv_lhs: N,
     pub rhs: N,
     pub rhs_wo_bias: N,
+    pub cfm_gain: N,
+    pub cfm_coeff: N,
 
     pub im1: Vector<N>,
     pub im2: Vector<N>,
@@ -91,6 +93,8 @@ impl<N: WReal, const LANES: usize> JointVelocityConstraint<N, LANES> {
             ang_jac1: na::zero(),
             ang_jac2: na::zero(),
             inv_lhs: N::zero(),
+            cfm_gain: N::zero(),
+            cfm_coeff: N::zero(),
             rhs: N::zero(),
             rhs_wo_bias: N::zero(),
             im1: na::zero(),
@@ -105,7 +109,7 @@ impl<N: WReal, const LANES: usize> JointVelocityConstraint<N, LANES> {
             self.ang_jac2.gdot(mj_lambda2.angular) - self.ang_jac1.gdot(mj_lambda1.angular);
 
         let rhs = dlinvel + dangvel + self.rhs;
-        let total_impulse = (self.impulse + self.inv_lhs * rhs)
+        let total_impulse = (self.impulse + self.inv_lhs * (rhs - self.cfm_gain * self.impulse))
             .simd_clamp(self.impulse_bounds[0], self.impulse_bounds[1]);
         let delta_impulse = total_impulse - self.impulse;
         self.impulse = total_impulse;
@@ -133,13 +137,14 @@ impl JointVelocityConstraint<Real, 1> {
         body2: &SolverBody<Real, 1>,
         frame1: &Isometry<Real>,
         frame2: &Isometry<Real>,
-        joint: &JointData,
+        joint: &GenericJoint,
         out: &mut [Self],
     ) -> usize {
         let mut len = 0;
         let locked_axes = joint.locked_axes.bits();
-        let motor_axes = joint.motor_axes.bits();
-        let limit_axes = joint.limit_axes.bits();
+        let motor_axes = joint.motor_axes.bits() & !locked_axes;
+        let limit_axes = joint.limit_axes.bits() & !locked_axes;
+        let coupled_axes = joint.coupled_axes.bits();
 
         let builder = JointVelocityConstraintBuilder::new(
             frame1,
@@ -149,78 +154,105 @@ impl JointVelocityConstraint<Real, 1> {
             locked_axes,
         );
 
-        for i in 0..DIM {
-            if locked_axes & (1 << i) != 0 {
-                out[len] =
-                    builder.lock_linear(params, [joint_id], body1, body2, i, WritebackId::Dof(i));
-                len += 1;
-            }
-        }
+        let start = len;
         for i in DIM..SPATIAL_DIM {
-            if locked_axes & (1 << i) != 0 {
-                out[len] = builder.lock_angular(
-                    params,
+            if (motor_axes & !coupled_axes) & (1 << i) != 0 {
+                out[len] = builder.motor_angular(
                     [joint_id],
                     body1,
                     body2,
                     i - DIM,
-                    WritebackId::Dof(i),
+                    &joint.motors[i].motor_params(params.dt),
+                    WritebackId::Motor(i),
                 );
                 len += 1;
             }
         }
-
         for i in 0..DIM {
-            if motor_axes & (1 << i) != 0 {
+            if (motor_axes & !coupled_axes) & (1 << i) != 0 {
+                let limits = if limit_axes & (1 << i) != 0 {
+                    Some([joint.limits[i].min, joint.limits[i].max])
+                } else {
+                    None
+                };
+
                 out[len] = builder.motor_linear(
                     params,
                     [joint_id],
                     body1,
                     body2,
-                    locked_axes >> DIM,
                     i,
                     &joint.motors[i].motor_params(params.dt),
+                    limits,
                     WritebackId::Motor(i),
                 );
                 len += 1;
             }
         }
+
+        if (motor_axes & coupled_axes) & JointAxesMask::ANG_AXES.bits() != 0 {
+            // TODO: coupled angular motor constraint.
+        }
+
+        if (motor_axes & coupled_axes) & JointAxesMask::LIN_AXES.bits() != 0 {
+            // TODO: coupled linear limit constraint.
+            // out[len] = builder.motor_linear_coupled(
+            //     params,
+            //     [joint_id],
+            //     body1,
+            //     body2,
+            //     limit_axes & coupled_axes,
+            //     &joint.limits,
+            //     WritebackId::Limit(0), // TODO: writeback
+            // );
+            // len += 1;
+        }
+        JointVelocityConstraintBuilder::finalize_constraints(&mut out[start..len]);
+
+        let start = len;
         for i in DIM..SPATIAL_DIM {
-            if motor_axes & (1 << i) != 0 {
-                out[len] = builder.motor_angular(
+            if locked_axes & (1 << i) != 0 {
+                out[len] = builder.lock_angular(
+                    params,
                     [joint_id],
                     body1,
                     body2,
                     i - DIM,
-                    &joint.motors[i].motor_params(params.dt),
-                    WritebackId::Motor(i),
+                    WritebackId::Dof(i),
                 );
                 len += 1;
             }
         }
-
         for i in 0..DIM {
-            if limit_axes & (1 << i) != 0 {
-                out[len] = builder.limit_linear(
+            if locked_axes & (1 << i) != 0 {
+                out[len] =
+                    builder.lock_linear(params, [joint_id], body1, body2, i, WritebackId::Dof(i));
+                len += 1;
+            }
+        }
+
+        for i in DIM..SPATIAL_DIM {
+            if (limit_axes & !coupled_axes) & (1 << i) != 0 {
+                out[len] = builder.limit_angular(
                     params,
                     [joint_id],