From fb20d72ee29de9311a81aec6eb9f02fd2aa35fc4 Mon Sep 17 00:00:00 2001 From: Sébastien Crozet Date: Sun, 20 Feb 2022 12:55:00 +0100 Subject: Joint API and joint motors improvements --- .../joint_velocity_constraint_builder.rs | 405 +++++++++++++++++---- 1 file changed, 338 insertions(+), 67 deletions(-) (limited to 'src/dynamics/solver/joint_constraint/joint_velocity_constraint_builder.rs') 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 JointVelocityConstraintBuilder { 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 JointVelocityConstraintBuilder { constraint } + pub fn limit_linear_coupled( + &self, + params: &IntegrationParameters, + joint_id: [JointIndex; LANES], + body1: &SolverBody, + body2: &SolverBody, + limited_coupled_axes: u8, + limits: &[JointLimits], + writeback_id: WritebackId, + ) -> JointVelocityConstraint { + let zero = N::zero(); + let mut lin_jac = Vector::zeros(); + let mut ang_jac1: AngVector = na::zero(); + let mut ang_jac2: AngVector = 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( &self, params: &IntegrationParameters, joint_id: [JointIndex; LANES], body1: &SolverBody, body2: &SolverBody, - _locked_ang_axes: u8, motor_axis: usize, motor_params: &MotorParameters, + limits: Option<[N; 2]>, writeback_id: WritebackId, ) -> JointVelocityConstraint { + 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 JointVelocityConstraintBuilder { 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 JointVelocityConstraintBuilder { 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 JointVelocityConstraintBuilder { #[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 JointVelocityConstraintBuilder { 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 JointVelocityConstraintBuilder { 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 JointVelocityConstraintBuilder { 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 JointVelocityConstraintBuilder { 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 JointVelocityConstraintBuilder { 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 JointVelocityConstraintBuilder { constraints: &mut [JointVelocityConstraint], ) { 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 JointVelocityConstraintBuilder { 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 JointVelocityConstraintBuilder { 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 JointVelocityConstraintBuilder { 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 JointVelocityConstraintBuilder { 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 JointVelocityConstraintBuilder { 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( + &self, + params: &IntegrationParameters, + joint_id: [JointIndex; LANES], + body1: &SolverBody, + body2: &SolverBody, + limited_coupled_axes: u8, + limits: &[JointLimits], + writeback_id: WritebackId, + ) -> JointVelocityGroundConstraint { + let zero = N::zero(); + let mut lin_jac = Vector::zeros(); + let mut ang_jac1: AngVector = na::zero(); + let mut ang_jac2: AngVector = 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( &self, + params: &IntegrationParameters, joint_id: [JointIndex; LANES], body1: &SolverBody, body2: &SolverBody, motor_axis: usize, motor_params: &MotorParameters, + limits: Option<[N; 2]>, writeback_id: WritebackId, ) -> JointVelocityGroundConstraint { + 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 JointVelocityConstraintBuilder { 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 JointVelocityConstraintBuilder { 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( + &self, + params: &IntegrationParameters, + joint_id: [JointIndex; LANES], + body1: &SolverBody, + body2: &SolverBody, + motor_coupled_axes: u8, + motors: &[MotorParameters], + limited_coupled_axes: u8, + limits: &[JointLimits], + writeback_id: WritebackId, + ) -> JointVelocityGroundConstraint { + todo!() + /* + let zero = N::zero(); + let mut lin_jac = Vector::zeros(); + let mut ang_jac1: AngVector = na::zero(); + let mut ang_jac2: AngVector = 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( &self, params: &IntegrationParameters, @@ -498,10 +751,11 @@ impl JointVelocityConstraintBuilder { 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 JointVelocityConstraintBuilder { 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 JointVelocityConstraintBuilder { 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 JointVelocityConstraintBuilder { 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 JointVelocityConstraintBuilder { #[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 JointVelocityConstraintBuilder { 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 JointVelocityConstraintBuilder { #[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 JointVelocityConstraintBuilder { 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 JointVelocityConstraintBuilder { constraints: &mut [JointVelocityGroundConstraint], ) { let len = constraints.len(); + + if len == 0 { + return; + } + let imsum = constraints[0].im2; // Use the modified Gram-Schmidt orthogonalization. @@ -673,18 +939,23 @@ impl JointVelocityConstraintBuilder { 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; -- cgit