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_generic_velocity_constraint.rs | 151 ++++---- .../joint_generic_velocity_constraint_builder.rs | 98 +++-- .../joint_constraint/joint_velocity_constraint.rs | 253 +++++++++---- .../joint_velocity_constraint_builder.rs | 405 +++++++++++++++++---- 4 files changed, 662 insertions(+), 245 deletions(-) (limited to 'src/dynamics/solver/joint_constraint') 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, frame2: &Isometry, - joint: &JointData, + joint: &GenericJoint, jacobians: &mut DVector, 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, frame2: &Isometry, - joint: &JointData, + joint: &GenericJoint, jacobians: &mut DVector, 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 { 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 { 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 { 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 { 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 { ); 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 { 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 { 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 { ); 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 { // 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 { 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 { 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 { 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 { 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 { 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 { ); 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 { 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 { 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 { ); 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 { // 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 { 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 { - 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 { impl Default for MotorParameters { 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 { pub inv_lhs: N, pub rhs: N, pub rhs_wo_bias: N, + pub cfm_gain: N, + pub cfm_coeff: N, pub im1: Vector, pub im2: Vector, @@ -91,6 +93,8 @@ impl JointVelocityConstraint { 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 JointVelocityConstraint { 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 { body2: &SolverBody, frame1: &Isometry, frame2: &Isometry, - 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 { 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], body1, body2, - i, + i - DIM, [joint.limits[i].min, joint.limits[i].max], WritebackId::Limit(i), ); len += 1; } } - for i in DIM..SPATIAL_DIM { - if limit_axes & (1 << i) != 0 { - out[len] = builder.limit_angular( + for i in 0..DIM { + if (limit_axes & !coupled_axes) & (1 << i) != 0 { + out[len] = builder.limit_linear( params, [joint_id], body1, body2, - i - DIM, + i, [joint.limits[i].min, joint.limits[i].max], WritebackId::Limit(i), ); @@ -228,7 +260,25 @@ impl JointVelocityConstraint { } } - JointVelocityConstraintBuilder::finalize_constraints(&mut out[..len]); + if (limit_axes & coupled_axes) & JointAxesMask::ANG_AXES.bits() != 0 { + // TODO: coupled angular limit constraint. + } + + if (limit_axes & coupled_axes) & JointAxesMask::LIN_AXES.bits() != 0 { + // TODO: coupled linear limit constraint. + out[len] = builder.limit_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]); + len } @@ -349,6 +399,8 @@ pub struct JointVelocityGroundConstraint { pub ang_jac2: AngVector, pub inv_lhs: N, + pub cfm_coeff: N, + pub cfm_gain: N, pub rhs: N, pub rhs_wo_bias: N, @@ -367,6 +419,8 @@ impl JointVelocityGroundConstraint { lin_jac: Vector::zeros(), ang_jac2: na::zero(), inv_lhs: N::zero(), + cfm_coeff: N::zero(), + cfm_gain: N::zero(), rhs: N::zero(), rhs_wo_bias: N::zero(), im2: na::zero(), @@ -379,7 +433,7 @@ impl JointVelocityGroundConstraint { let dangvel = mj_lambda2.angular; let dvel = self.lin_jac.dot(&dlinvel) + self.ang_jac2.gdot(dangvel) + self.rhs; - let total_impulse = (self.impulse + self.inv_lhs * dvel) + let total_impulse = (self.impulse + self.inv_lhs * (dvel - 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; @@ -404,13 +458,14 @@ impl JointVelocityGroundConstraint { body2: &SolverBody, frame1: &Isometry, frame2: &Isometry, - joint: &JointData, + joint: &GenericJoint, out: &mut [Self], ) -> usize { let mut len = 0; - let locked_axes = joint.locked_axes.bits() as u8; - let motor_axes = joint.motor_axes.bits() as u8; - let limit_axes = joint.limit_axes.bits() as u8; + let locked_axes = joint.locked_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, @@ -420,19 +475,68 @@ impl JointVelocityGroundConstraint { locked_axes, ); + let start = len; + for i in DIM..SPATIAL_DIM { + if (motor_axes & !coupled_axes) & (1 << i) != 0 { + out[len] = builder.motor_angular_ground( + [joint_id], + body1, + body2, + i - DIM, + &joint.motors[i].motor_params(params.dt), + WritebackId::Motor(i), + ); + len += 1; + } + } for i in 0..DIM { - if locked_axes & (1 << i) != 0 { - out[len] = builder.lock_linear_ground( + 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_ground( params, [joint_id], body1, body2, i, - WritebackId::Dof(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 motor constraint. + out[len] = builder.motor_linear_coupled_ground( + params, + [joint_id], + body1, + body2, + motor_axes & coupled_axes, + &joint.motors, + limit_axes & coupled_axes, + &joint.limits, + WritebackId::Limit(0), // TODO: writeback + ); + len += 1; + */ + todo!() + } + + JointVelocityConstraintBuilder::finalize_ground_constraints(&mut out[start..len]); + + let start = len; for i in DIM..SPATIAL_DIM { if locked_axes & (1 << i) != 0 { out[len] = builder.lock_angular_ground( @@ -446,36 +550,36 @@ impl JointVelocityGroundConstraint { len += 1; } } - for i in 0..DIM { - if motor_axes & (1 << i) != 0 { - out[len] = builder.motor_linear_ground( + if locked_axes & (1 << i) != 0 { + out[len] = builder.lock_linear_ground( + params, [joint_id], body1, body2, i, - &joint.motors[i].motor_params(params.dt), - WritebackId::Motor(i), + WritebackId::Dof(i), ); len += 1; } } + for i in DIM..SPATIAL_DIM { - if motor_axes & (1 << i) != 0 { - out[len] = builder.motor_angular_ground( + if (limit_axes & !coupled_axes) & (1 << i) != 0 { + out[len] = builder.limit_angular_ground( + params, [joint_id], body1, body2, i - DIM, - &joint.motors[i].motor_params(params.dt), - WritebackId::Motor(i), + [joint.limits[i].min, joint.limits[i].max], + WritebackId::Limit(i), ); len += 1; } } - for i in 0..DIM { - if limit_axes & (1 << i) != 0 { + if (limit_axes & !coupled_axes) & (1 << i) != 0 { out[len] = builder.limit_linear_ground( params, [joint_id], @@ -488,22 +592,25 @@ impl JointVelocityGroundConstraint { len += 1; } } - for i in DIM..SPATIAL_DIM { - if limit_axes & (1 << i) != 0 { - out[len] = builder.limit_angular_ground( - params, - [joint_id], - body1, - body2, - i - DIM, - [joint.limits[i].min, joint.limits[i].max], - WritebackId::Limit(i), - ); - len += 1; - } + + if (limit_axes & coupled_axes) & JointAxesMask::ANG_AXES.bits() != 0 { + // TODO: coupled angular limit constraint. } - JointVelocityConstraintBuilder::finalize_ground_constraints(&mut out[..len]); + if (limit_axes & coupled_axes) & JointAxesMask::LIN_AXES.bits() != 0 { + out[len] = builder.limit_linear_coupled_ground( + params, + [joint_id], + body1, + body2, + limit_axes & coupled_axes, + &joint.limits, + WritebackId::Limit(0), // TODO: writeback + ); + len += 1; + } + JointVelocityConstraintBuilder::finalize_ground_constraints(&mut out[start..len]); + len } 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 @@ im