From b00113ed2f9e4824a254027b57c9b4a07c4c2307 Mon Sep 17 00:00:00 2001 From: Sébastien Crozet Date: Sat, 23 Mar 2024 10:15:07 +0100 Subject: fix: implement linear-coupled-motor constraint between two dynamic bodies Fix #602 --- .../contact_constraint/contact_constraints_set.rs | 1 + .../joint_constraint/joint_constraint_builder.rs | 76 ++++++++++++++++++++++ .../joint_constraint/joint_velocity_constraint.rs | 43 ++++++------ 3 files changed, 98 insertions(+), 22 deletions(-) (limited to 'src') diff --git a/src/dynamics/solver/contact_constraint/contact_constraints_set.rs b/src/dynamics/solver/contact_constraint/contact_constraints_set.rs index b5fdb6d..4d88949 100644 --- a/src/dynamics/solver/contact_constraint/contact_constraints_set.rs +++ b/src/dynamics/solver/contact_constraint/contact_constraints_set.rs @@ -25,6 +25,7 @@ use { crate::math::SIMD_WIDTH, }; +#[derive(Debug)] pub struct ConstraintsCounts { pub num_constraints: usize, pub num_jacobian_lines: usize, diff --git a/src/dynamics/solver/joint_constraint/joint_constraint_builder.rs b/src/dynamics/solver/joint_constraint/joint_constraint_builder.rs index 00bead1..44716b2 100644 --- a/src/dynamics/solver/joint_constraint/joint_constraint_builder.rs +++ b/src/dynamics/solver/joint_constraint/joint_constraint_builder.rs @@ -551,6 +551,82 @@ impl JointTwoBodyConstraintHelper { constraint } + pub fn motor_linear_coupled( + &self, + params: &IntegrationParameters, + joint_id: [JointIndex; LANES], + body1: &JointSolverBody, + body2: &JointSolverBody, + coupled_axes: u8, + motor_params: &MotorParameters, + limits: Option<[N; 2]>, + writeback_id: WritebackId, + ) -> JointTwoBodyConstraint { + let inv_dt = N::splat(params.inv_dt()); + + let mut lin_jac = Vector::zeros(); + let mut ang_jac1: AngVector = na::zero(); + let mut ang_jac2: AngVector = na::zero(); + + for i in 0..DIM { + if 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; + } + } + } + + 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 mut rhs_wo_bias = N::zero(); + if motor_params.erp_inv_dt != N::zero() { + rhs_wo_bias += (dist - motor_params.target_pos) * motor_params.erp_inv_dt; + } + + let mut target_vel = motor_params.target_vel; + if let Some(limits) = limits { + target_vel = + target_vel.simd_clamp((limits[0] - dist) * inv_dt, (limits[1] - dist) * inv_dt); + }; + + rhs_wo_bias += -target_vel; + + ang_jac1 = body1.sqrt_ii * ang_jac1; + ang_jac2 = body2.sqrt_ii * ang_jac2; + + JointTwoBodyConstraint { + joint_id, + solver_vel1: body1.solver_vel, + solver_vel2: body2.solver_vel, + im1: body1.im, + im2: body2.im, + impulse: N::zero(), + impulse_bounds: [-motor_params.max_impulse, motor_params.max_impulse], + lin_jac, + 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, + } + } + pub fn lock_linear( &self, params: &IntegrationParameters, diff --git a/src/dynamics/solver/joint_constraint/joint_velocity_constraint.rs b/src/dynamics/solver/joint_constraint/joint_velocity_constraint.rs index e184e3d..60c42d3 100644 --- a/src/dynamics/solver/joint_constraint/joint_velocity_constraint.rs +++ b/src/dynamics/solver/joint_constraint/joint_velocity_constraint.rs @@ -217,28 +217,26 @@ impl JointTwoBodyConstraint { } if (motor_axes & coupled_axes) & JointAxesMask::LIN_AXES.bits() != 0 { - // if (motor_axes & !coupled_axes) & (1 << first_coupled_lin_axis_id) != 0 { - // let limits = if limit_axes & (1 << first_coupled_lin_axis_id) != 0 { - // Some([ - // joint.limits[first_coupled_lin_axis_id].min, - // joint.limits[first_coupled_lin_axis_id].max, - // ]) - // } else { - // None - // }; - // - // out[len] = builder.motor_linear_coupled - // params, - // [joint_id], - // body1, - // body2, - // coupled_axes, - // &joint.motors[first_coupled_lin_axis_id].motor_params(params.dt), - // limits, - // WritebackId::Motor(first_coupled_lin_axis_id), - // ); - // len += 1; - // } + let limits = if (limit_axes & (1 << first_coupled_lin_axis_id)) != 0 { + Some([ + joint.limits[first_coupled_lin_axis_id].min, + joint.limits[first_coupled_lin_axis_id].max, + ]) + } else { + None + }; + + out[len] = builder.motor_linear_coupled( + params, + [joint_id], + body1, + body2, + coupled_axes, + &joint.motors[first_coupled_lin_axis_id].motor_params(params.dt), + limits, + WritebackId::Motor(first_coupled_lin_axis_id), + ); + len += 1; } JointTwoBodyConstraintHelper::finalize_constraints(&mut out[start..len]); @@ -350,6 +348,7 @@ impl JointTwoBodyConstraint { } } } + #[cfg(feature = "simd-is-enabled")] impl JointTwoBodyConstraint { pub fn lock_axes( -- cgit