From 144f793f3df491ded05de0393bd725296e5e4630 Mon Sep 17 00:00:00 2001 From: Thierry Berger Date: Tue, 14 Jan 2025 10:27:38 +0100 Subject: [PATCH 1/7] add joint_natural_frequency and joint_damping_ratio to GenericJoint. :warning: I'm not sure about multibodies --- src/dynamics/integration_parameters.rs | 39 +++--- src/dynamics/joint/generic_joint.rs | 22 ++++ .../multibody_joint/unit_multibody_joint.rs | 8 +- .../joint_constraint_builder.rs | 118 +++++++++++++----- .../joint_generic_constraint.rs | 30 +++++ .../joint_generic_constraint_builder.rs | 32 +++-- .../joint_velocity_constraint.rs | 48 ++++++- 7 files changed, 240 insertions(+), 57 deletions(-) diff --git a/src/dynamics/integration_parameters.rs b/src/dynamics/integration_parameters.rs index 99b6fe48d..f93dd012c 100644 --- a/src/dynamics/integration_parameters.rs +++ b/src/dynamics/integration_parameters.rs @@ -159,23 +159,27 @@ impl IntegrationParameters { } /// The joint’s spring angular frequency for constraint regularization. - pub fn joint_angular_frequency(&self) -> Real { - self.joint_natural_frequency * Real::two_pi() + pub fn joint_angular_frequency(joint_natural_frequency: Real) -> Real { + joint_natural_frequency * Real::two_pi() } /// The [`Self::joint_erp`] coefficient, multiplied by the inverse timestep length. - pub fn joint_erp_inv_dt(&self) -> Real { - let ang_freq = self.joint_angular_frequency(); - ang_freq / (self.dt * ang_freq + 2.0 * self.joint_damping_ratio) + pub fn joint_erp_inv_dt( + &self, + joint_natural_frequency: Real, + joint_damping_ratio: Real, + ) -> Real { + let ang_freq = Self::joint_angular_frequency(joint_natural_frequency); + ang_freq / (self.dt * ang_freq + 2.0 * joint_damping_ratio) } /// The effective Error Reduction Parameter applied for calculating regularization forces /// on joints. /// - /// This parameter is computed automatically from [`Self::joint_natural_frequency`], - /// [`Self::joint_damping_ratio`] and the substep length. - pub fn joint_erp(&self) -> Real { - self.dt * self.joint_erp_inv_dt() + /// This parameter is computed automatically from `joint_natural_frequency`, + /// `joint_damping_ratio` and the substep length. + pub fn joint_erp(&self, joint_natural_frequency: Real, joint_damping_ratio: Real) -> Real { + self.dt * self.joint_erp_inv_dt(joint_natural_frequency, joint_damping_ratio) } /// The CFM factor to be used in the constraint resolution. @@ -224,21 +228,22 @@ impl IntegrationParameters { /// The CFM (constraints force mixing) coefficient applied to all joints for constraints regularization. /// - /// This parameter is computed automatically from [`Self::joint_natural_frequency`], - /// [`Self::joint_damping_ratio`] and the substep length. - pub fn joint_cfm_coeff(&self) -> Real { + /// This parameter is computed automatically from `joint_natural_frequency`, + /// `joint_damping_ratio` and the substep length. + pub fn joint_cfm_coeff( + &self, + joint_natural_frequency: Real, + joint_damping_ratio: Real, + ) -> Real { // Compute CFM assuming a critically damped spring multiplied by the damping ratio. // The logic is similar to `Self::contact_cfm_factor`. - let joint_erp = self.joint_erp(); + let joint_erp = self.joint_erp(joint_natural_frequency, joint_damping_ratio); if joint_erp == 0.0 { return 0.0; } let inv_erp_minus_one = 1.0 / joint_erp - 1.0; inv_erp_minus_one * inv_erp_minus_one - / ((1.0 + inv_erp_minus_one) - * 4.0 - * self.joint_damping_ratio - * self.joint_damping_ratio) + / ((1.0 + inv_erp_minus_one) * 4.0 * joint_damping_ratio * joint_damping_ratio) } /// Amount of penetration the engine won’t attempt to correct (default: `0.001` multiplied by diff --git a/src/dynamics/joint/generic_joint.rs b/src/dynamics/joint/generic_joint.rs index 8d4066d35..b8c8a9194 100644 --- a/src/dynamics/joint/generic_joint.rs +++ b/src/dynamics/joint/generic_joint.rs @@ -248,6 +248,26 @@ pub struct GenericJoint { pub enabled: JointEnabled, /// User-defined data associated to this joint. pub user_data: u128, + /// > 0: the natural frequency used by the springs for joint constraint regularization. + /// + /// Increasing this value will make it so that penetrations get fixed more quickly. + /// (default: [`None`]). + /// + /// If [`None`], + /// [`IntegrationParameters::joint_natural_frequency`](crate::prelude::IntegrationParameters::joint_natural_frequency) + /// is used. + pub joint_natural_frequency: Option, + + /// The fraction of critical damping applied to the joint for constraints regularization. + /// + /// Larger values make the constraints more compliant (allowing more joint + /// drift before stabilization). + /// (default [`None`]). + /// + /// If [`None`], + /// [`IntegrationParameters::joint_damping_ratio`](crate::prelude::IntegrationParameters::joint_damping_ratio) + /// is used. + pub joint_damping_ratio: Option, } impl Default for GenericJoint { @@ -264,6 +284,8 @@ impl Default for GenericJoint { contacts_enabled: true, enabled: JointEnabled::Enabled, user_data: 0, + joint_natural_frequency: None, + joint_damping_ratio: None, } } } diff --git a/src/dynamics/joint/multibody_joint/unit_multibody_joint.rs b/src/dynamics/joint/multibody_joint/unit_multibody_joint.rs index d6efd12d6..c1c766810 100644 --- a/src/dynamics/joint/multibody_joint/unit_multibody_joint.rs +++ b/src/dynamics/joint/multibody_joint/unit_multibody_joint.rs @@ -20,11 +20,15 @@ pub fn unit_joint_limit_constraint( constraints: &mut [JointGenericOneBodyConstraint], insert_at: &mut usize, ) { + // TODO: use joint_natural_frequency and joint_damping_ratio from multibodies GenericJoint. + let joint_natural_frequency = params.joint_natural_frequency; + let joint_damping_ratio = params.joint_damping_ratio; + let ndofs = multibody.ndofs(); let min_enabled = curr_pos < limits[0]; let max_enabled = limits[1] < curr_pos; - let erp_inv_dt = params.joint_erp_inv_dt(); - let cfm_coeff = params.joint_cfm_coeff(); + let erp_inv_dt = params.joint_erp_inv_dt(joint_natural_frequency, joint_damping_ratio); + let cfm_coeff = params.joint_cfm_coeff(joint_natural_frequency, joint_damping_ratio); let rhs_bias = ((curr_pos - limits[1]).max(0.0) - (limits[0] - curr_pos).max(0.0)) * erp_inv_dt; let rhs_wo_bias = 0.0; diff --git a/src/dynamics/solver/joint_constraint/joint_constraint_builder.rs b/src/dynamics/solver/joint_constraint/joint_constraint_builder.rs index 9948d3fe9..08956d582 100644 --- a/src/dynamics/solver/joint_constraint/joint_constraint_builder.rs +++ b/src/dynamics/solver/joint_constraint/joint_constraint_builder.rs @@ -423,17 +423,29 @@ impl JointTwoBodyConstraintHelper { limited_axis: usize, limits: [N; 2], writeback_id: WritebackId, + joint_natural_frequency: Real, + joint_damping_ratio: Real, ) -> JointTwoBodyConstraint { let zero = N::zero(); - let mut constraint = - self.lock_linear(params, joint_id, body1, body2, limited_axis, writeback_id); + let mut constraint = self.lock_linear( + params, + joint_id, + body1, + body2, + limited_axis, + writeback_id, + joint_natural_frequency, + joint_damping_ratio, + ); let dist = self.lin_err.dot(&constraint.lin_jac); let min_enabled = dist.simd_le(limits[0]); let max_enabled = limits[1].simd_le(dist); - let erp_inv_dt = N::splat(params.joint_erp_inv_dt()); - let cfm_coeff = N::splat(params.joint_cfm_coeff()); + let erp_inv_dt = + N::splat(params.joint_erp_inv_dt(joint_natural_frequency, joint_damping_ratio)); + let cfm_coeff = + N::splat(params.joint_cfm_coeff(joint_natural_frequency, joint_damping_ratio)); 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; @@ -455,6 +467,8 @@ impl JointTwoBodyConstraintHelper { coupled_axes: u8, limits: [N; 2], writeback_id: WritebackId, + joint_natural_frequency: Real, + joint_damping_ratio: Real, ) -> JointTwoBodyConstraint { let zero = N::zero(); let mut lin_jac = Vector::zeros(); @@ -491,8 +505,10 @@ impl JointTwoBodyConstraintHelper { 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 erp_inv_dt = + N::splat(params.joint_erp_inv_dt(joint_natural_frequency, joint_damping_ratio)); + let cfm_coeff = + N::splat(params.joint_cfm_coeff(joint_natural_frequency, joint_damping_ratio)); let rhs_bias = (dist - limits[1]).simd_max(zero) * erp_inv_dt; let rhs = rhs_wo_bias + rhs_bias; let impulse_bounds = [N::zero(), N::splat(Real::INFINITY)]; @@ -527,10 +543,20 @@ impl JointTwoBodyConstraintHelper { motor_params: &MotorParameters, limits: Option<[N; 2]>, writeback_id: WritebackId, + joint_natural_frequency: Real, + joint_damping_ratio: Real, ) -> JointTwoBodyConstraint { let inv_dt = N::splat(params.inv_dt()); - let mut constraint = - self.lock_linear(params, joint_id, body1, body2, motor_axis, writeback_id); + let mut constraint = self.lock_linear( + params, + joint_id, + body1, + body2, + motor_axis, + writeback_id, + joint_natural_frequency, + joint_damping_ratio, + ); let mut rhs_wo_bias = N::zero(); if motor_params.erp_inv_dt != N::zero() { @@ -639,6 +665,8 @@ impl JointTwoBodyConstraintHelper { body2: &JointSolverBody, locked_axis: usize, writeback_id: WritebackId, + joint_natural_frequency: Real, + joint_damping_ratio: Real, ) -> JointTwoBodyConstraint { let lin_jac = self.basis.column(locked_axis).into_owned(); #[cfg(feature = "dim2")] @@ -651,8 +679,10 @@ impl JointTwoBodyConstraintHelper { let mut ang_jac2 = self.cmat2_basis.column(locked_axis).into_owned(); let rhs_wo_bias = N::zero(); - let erp_inv_dt = N::splat(params.joint_erp_inv_dt()); - let cfm_coeff = N::splat(params.joint_cfm_coeff()); + let erp_inv_dt = + N::splat(params.joint_erp_inv_dt(joint_natural_frequency, joint_damping_ratio)); + let cfm_coeff = + N::splat(params.joint_cfm_coeff(joint_natural_frequency, joint_damping_ratio)); let rhs_bias = lin_jac.dot(&self.lin_err) * erp_inv_dt; ang_jac1 = body1.sqrt_ii * ang_jac1; @@ -687,6 +717,8 @@ impl JointTwoBodyConstraintHelper { _limited_axis: usize, limits: [N; 2], writeback_id: WritebackId, + joint_natural_frequency: Real, + joint_damping_ratio: Real, ) -> JointTwoBodyConstraint { let zero = N::zero(); let half = N::splat(0.5); @@ -708,8 +740,10 @@ impl JointTwoBodyConstraintHelper { #[cfg(feature = "dim3")] let ang_jac = self.ang_basis.column(_limited_axis).into_owned(); let rhs_wo_bias = N::zero(); - let erp_inv_dt = N::splat(params.joint_erp_inv_dt()); - let cfm_coeff = N::splat(params.joint_cfm_coeff()); + let erp_inv_dt = + N::splat(params.joint_erp_inv_dt(joint_natural_frequency, joint_damping_ratio)); + let cfm_coeff = + N::splat(params.joint_cfm_coeff(joint_natural_frequency, joint_damping_ratio)); let rhs_bias = ((s_ang - s_limits[1]).simd_max(zero) - (s_limits[0] - s_ang).simd_max(zero)) * erp_inv_dt; @@ -795,6 +829,8 @@ impl JointTwoBodyConstraintHelper { body2: &JointSolverBody, _locked_axis: usize, writeback_id: WritebackId, + joint_natural_frequency: Real, + joint_damping_ratio: Real, ) -> JointTwoBodyConstraint { #[cfg(feature = "dim2")] let ang_jac = N::one(); @@ -802,8 +838,10 @@ impl JointTwoBodyConstraintHelper { let ang_jac = self.ang_basis.column(_locked_axis).into_owned(); let rhs_wo_bias = N::zero(); - let erp_inv_dt = N::splat(params.joint_erp_inv_dt()); - let cfm_coeff = N::splat(params.joint_cfm_coeff()); + let erp_inv_dt = + N::splat(params.joint_erp_inv_dt(joint_natural_frequency, joint_damping_ratio)); + let cfm_coeff = + N::splat(params.joint_cfm_coeff(joint_natural_frequency, joint_damping_ratio)); #[cfg(feature = "dim2")] let rhs_bias = self.ang_err.im * erp_inv_dt; #[cfg(feature = "dim3")] @@ -888,6 +926,8 @@ impl JointTwoBodyConstraintHelper { limited_axis: usize, limits: [N; 2], writeback_id: WritebackId, + joint_natural_frequency: Real, + joint_damping_ratio: Real, ) -> JointOneBodyConstraint { let zero = N::zero(); let lin_jac = self.basis.column(limited_axis).into_owned(); @@ -908,8 +948,10 @@ impl JointTwoBodyConstraintHelper { let mut ang_jac2 = self.cmat2_basis.column(limited_axis).into_owned(); let rhs_wo_bias = -lin_jac.dot(&body1.linvel) - ang_jac1.gdot(body1.angvel); - let erp_inv_dt = N::splat(params.joint_erp_inv_dt()); - let cfm_coeff = N::splat(params.joint_cfm_coeff()); + let erp_inv_dt = + N::splat(params.joint_erp_inv_dt(joint_natural_frequency, joint_damping_ratio)); + let cfm_coeff = + N::splat(params.joint_cfm_coeff(joint_natural_frequency, joint_damping_ratio)); let rhs_bias = ((dist - limits[1]).simd_max(zero) - (limits[0] - dist).simd_max(zero)) * erp_inv_dt; @@ -941,6 +983,8 @@ impl JointTwoBodyConstraintHelper { coupled_axes: u8, limits: [N; 2], writeback_id: WritebackId, + joint_natural_frequency: Real, + joint_damping_ratio: Real, ) -> JointOneBodyConstraint { let zero = N::zero(); let mut lin_jac = Vector::zeros(); @@ -976,8 +1020,10 @@ impl JointTwoBodyConstraintHelper { 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 erp_inv_dt = + N::splat(params.joint_erp_inv_dt(joint_natural_frequency, joint_damping_ratio)); + let cfm_coeff = + N::splat(params.joint_cfm_coeff(joint_natural_frequency, joint_damping_ratio)); let rhs_bias = (dist - limits[1]).simd_max(zero) * erp_inv_dt; let rhs = rhs_wo_bias + rhs_bias; let impulse_bounds = [N::zero(), N::splat(Real::INFINITY)]; @@ -1135,6 +1181,8 @@ impl JointTwoBodyConstraintHelper { body2: &JointSolverBody, locked_axis: usize, writeback_id: WritebackId, + joint_natural_frequency: Real, + joint_damping_ratio: Real, ) -> JointOneBodyConstraint { let lin_jac = self.basis.column(locked_axis).into_owned(); let ang_jac1 = self.cmat1_basis.column(locked_axis).into_owned(); @@ -1145,8 +1193,10 @@ impl JointTwoBodyConstraintHelper { let rhs_wo_bias = -lin_jac.dot(&body1.linvel) - ang_jac1.gdot(body1.angvel); - let erp_inv_dt = N::splat(params.joint_erp_inv_dt()); - let cfm_coeff = N::splat(params.joint_cfm_coeff()); + let erp_inv_dt = + N::splat(params.joint_erp_inv_dt(joint_natural_frequency, joint_damping_ratio)); + let cfm_coeff = + N::splat(params.joint_cfm_coeff(joint_natural_frequency, joint_damping_ratio)); let rhs_bias = lin_jac.dot(&self.lin_err) * erp_inv_dt; ang_jac2 = body2.sqrt_ii * ang_jac2; @@ -1224,6 +1274,8 @@ impl JointTwoBodyConstraintHelper { _limited_axis: usize, limits: [N; 2], writeback_id: WritebackId, + joint_natural_frequency: Real, + joint_damping_ratio: Real, ) -> JointOneBodyConstraint { let zero = N::zero(); let half = N::splat(0.5); @@ -1246,8 +1298,10 @@ impl JointTwoBodyConstraintHelper { let ang_jac = self.ang_basis.column(_limited_axis).into_owned(); let rhs_wo_bias = -ang_jac.gdot(body1.angvel); - let erp_inv_dt = N::splat(params.joint_erp_inv_dt()); - let cfm_coeff = N::splat(params.joint_cfm_coeff()); + let erp_inv_dt = + N::splat(params.joint_erp_inv_dt(joint_natural_frequency, joint_damping_ratio)); + let cfm_coeff = + N::splat(params.joint_cfm_coeff(joint_natural_frequency, joint_damping_ratio)); let rhs_bias = ((s_ang - s_limits[1]).simd_max(zero) - (s_limits[0] - s_ang).simd_max(zero)) * erp_inv_dt; @@ -1279,6 +1333,8 @@ impl JointTwoBodyConstraintHelper { body2: &JointSolverBody, _locked_axis: usize, writeback_id: WritebackId, + joint_natural_frequency: Real, + joint_damping_ratio: Real, ) -> JointOneBodyConstraint { #[cfg(feature = "dim2")] let ang_jac = N::one(); @@ -1287,8 +1343,10 @@ impl JointTwoBodyConstraintHelper { let rhs_wo_bias = -ang_jac.gdot(body1.angvel); - let erp_inv_dt = N::splat(params.joint_erp_inv_dt()); - let cfm_coeff = N::splat(params.joint_cfm_coeff()); + let erp_inv_dt = + N::splat(params.joint_erp_inv_dt(joint_natural_frequency, joint_damping_ratio)); + let cfm_coeff = + N::splat(params.joint_cfm_coeff(joint_natural_frequency, joint_damping_ratio)); #[cfg(feature = "dim2")] let rhs_bias = self.ang_err.im * erp_inv_dt; #[cfg(feature = "dim3")] @@ -1371,6 +1429,8 @@ impl JointTwoBodyConstraintHelper { coupled_axes: u8, limits: [Real; 2], writeback_id: WritebackId, + joint_natural_frequency: Real, + joint_damping_ratio: Real, ) -> JointTwoBodyConstraint { // NOTE: right now, this only supports exactly 2 coupled axes. let ang_coupled_axes = coupled_axes >> DIM; @@ -1395,8 +1455,8 @@ impl JointTwoBodyConstraintHelper { let rhs_wo_bias = 0.0; - let erp_inv_dt = params.joint_erp_inv_dt(); - let cfm_coeff = params.joint_cfm_coeff(); + let erp_inv_dt = params.joint_erp_inv_dt(joint_natural_frequency, joint_damping_ratio); + let cfm_coeff = params.joint_cfm_coeff(joint_natural_frequency, joint_damping_ratio); let rhs_bias = ((angle - limits[1]).max(0.0) - (limits[0] - angle).max(0.0)) * erp_inv_dt; let ang_jac1 = body1.sqrt_ii * ang_jac; @@ -1432,6 +1492,8 @@ impl JointTwoBodyConstraintHelper { coupled_axes: u8, limits: [Real; 2], writeback_id: WritebackId, + joint_natural_frequency: Real, + joint_damping_ratio: Real, ) -> JointOneBodyConstraint { // NOTE: right now, this only supports exactly 2 coupled axes. let ang_coupled_axes = coupled_axes >> DIM; @@ -1456,8 +1518,8 @@ impl JointTwoBodyConstraintHelper { let rhs_wo_bias = -ang_jac.gdot(body1.angvel); - let erp_inv_dt = params.joint_erp_inv_dt(); - let cfm_coeff = params.joint_cfm_coeff(); + let erp_inv_dt = params.joint_erp_inv_dt(joint_natural_frequency, joint_damping_ratio); + let cfm_coeff = params.joint_cfm_coeff(joint_natural_frequency, joint_damping_ratio); let rhs_bias = ((angle - limits[1]).max(0.0) - (limits[0] - angle).max(0.0)) * erp_inv_dt; let ang_jac2 = body2.sqrt_ii * ang_jac; diff --git a/src/dynamics/solver/joint_constraint/joint_generic_constraint.rs b/src/dynamics/solver/joint_constraint/joint_generic_constraint.rs index d500b48d6..89eb4c472 100644 --- a/src/dynamics/solver/joint_constraint/joint_generic_constraint.rs +++ b/src/dynamics/solver/joint_constraint/joint_generic_constraint.rs @@ -89,6 +89,13 @@ impl JointGenericTwoBodyConstraint { locked_axes, ); + let joint_natural_frequency = joint + .joint_natural_frequency + .unwrap_or(params.joint_natural_frequency); + let joint_damping_ratio = joint + .joint_damping_ratio + .unwrap_or(params.joint_damping_ratio); + let start = len; for i in DIM..SPATIAL_DIM { if motor_axes & (1 << i) != 0 { @@ -141,6 +148,8 @@ impl JointGenericTwoBodyConstraint { mb2, i - DIM, WritebackId::Dof(i), + joint_natural_frequency, + joint_damping_ratio, ); len += 1; } @@ -158,6 +167,8 @@ impl JointGenericTwoBodyConstraint { mb2, i, WritebackId::Dof(i), + joint_natural_frequency, + joint_damping_ratio, ); len += 1; } @@ -177,6 +188,8 @@ impl JointGenericTwoBodyConstraint { i - DIM, [joint.limits[i].min, joint.limits[i].max], WritebackId::Limit(i), + joint_natural_frequency, + joint_damping_ratio, ); len += 1; } @@ -195,6 +208,8 @@ impl JointGenericTwoBodyConstraint { i, [joint.limits[i].min, joint.limits[i].max], WritebackId::Limit(i), + joint_natural_frequency, + joint_damping_ratio, ); len += 1; } @@ -377,6 +392,13 @@ impl JointGenericOneBodyConstraint { locked_axes, ); + let joint_natural_frequency = joint + .joint_natural_frequency + .unwrap_or(params.joint_natural_frequency); + let joint_damping_ratio = joint + .joint_damping_ratio + .unwrap_or(params.joint_damping_ratio); + let start = len; for i in DIM..SPATIAL_DIM { if motor_axes & (1 << i) != 0 { @@ -428,6 +450,8 @@ impl JointGenericOneBodyConstraint { mb2, i - DIM, WritebackId::Dof(i), + joint_natural_frequency, + joint_damping_ratio, ); len += 1; } @@ -443,6 +467,8 @@ impl JointGenericOneBodyConstraint { mb2, i, WritebackId::Dof(i), + joint_natural_frequency, + joint_damping_ratio, ); len += 1; } @@ -460,6 +486,8 @@ impl JointGenericOneBodyConstraint { i - DIM, [joint.limits[i].min, joint.limits[i].max], WritebackId::Limit(i), + joint_natural_frequency, + joint_damping_ratio, ); len += 1; } @@ -476,6 +504,8 @@ impl JointGenericOneBodyConstraint { i, [joint.limits[i].min, joint.limits[i].max], WritebackId::Limit(i), + joint_natural_frequency, + joint_damping_ratio, ); len += 1; } diff --git a/src/dynamics/solver/joint_constraint/joint_generic_constraint_builder.rs b/src/dynamics/solver/joint_constraint/joint_generic_constraint_builder.rs index 987619582..410e7407e 100644 --- a/src/dynamics/solver/joint_constraint/joint_generic_constraint_builder.rs +++ b/src/dynamics/solver/joint_constraint/joint_generic_constraint_builder.rs @@ -573,6 +573,8 @@ impl JointTwoBodyConstraintHelper { mb2: Option<(&Multibody, usize)>, locked_axis: usize, writeback_id: WritebackId, + joint_natural_frequency: Real, + joint_damping_ratio: Real, ) -> JointGenericTwoBodyConstraint { let lin_jac = self.basis.column(locked_axis).into_owned(); let ang_jac1 = self.cmat1_basis.column(locked_axis).into_owned(); @@ -592,7 +594,7 @@ impl JointTwoBodyConstraintHelper { ang_jac2, ); - let erp_inv_dt = params.joint_erp_inv_dt(); + let erp_inv_dt = params.joint_erp_inv_dt(joint_natural_frequency, joint_damping_ratio); let rhs_bias = lin_jac.dot(&self.lin_err) * erp_inv_dt; c.rhs += rhs_bias; c @@ -611,6 +613,8 @@ impl JointTwoBodyConstraintHelper { limited_axis: usize, limits: [Real; 2], writeback_id: WritebackId, + joint_natural_frequency: Real, + joint_damping_ratio: Real, ) -> JointGenericTwoBodyConstraint { let lin_jac = self.basis.column(limited_axis).into_owned(); let ang_jac1 = self.cmat1_basis.column(limited_axis).into_owned(); @@ -634,7 +638,7 @@ impl JointTwoBodyConstraintHelper { let min_enabled = dist <= limits[0]; let max_enabled = limits[1] <= dist; - let erp_inv_dt = params.joint_erp_inv_dt(); + let erp_inv_dt = params.joint_erp_inv_dt(joint_natural_frequency, joint_damping_ratio); 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 = [ @@ -712,6 +716,8 @@ impl JointTwoBodyConstraintHelper { mb2: Option<(&Multibody, usize)>, _locked_axis: usize, writeback_id: WritebackId, + joint_natural_frequency: Real, + joint_damping_ratio: Real, ) -> JointGenericTwoBodyConstraint { #[cfg(feature = "dim2")] let ang_jac = Vector1::new(1.0); @@ -732,7 +738,7 @@ impl JointTwoBodyConstraintHelper { ang_jac, ); - let erp_inv_dt = params.joint_erp_inv_dt(); + let erp_inv_dt = params.joint_erp_inv_dt(joint_natural_frequency, joint_damping_ratio); #[cfg(feature = "dim2")] let rhs_bias = self.ang_err.im * erp_inv_dt; #[cfg(feature = "dim3")] @@ -754,6 +760,8 @@ impl JointTwoBodyConstraintHelper { _limited_axis: usize, limits: [Real; 2], writeback_id: WritebackId, + joint_natural_frequency: Real, + joint_damping_ratio: Real, ) -> JointGenericTwoBodyConstraint { #[cfg(feature = "dim2")] let ang_jac = Vector1::new(1.0); @@ -786,7 +794,7 @@ impl JointTwoBodyConstraintHelper { max_enabled as u32 as Real * Real::MAX, ]; - let erp_inv_dt = params.joint_erp_inv_dt(); + let erp_inv_dt = params.joint_erp_inv_dt(joint_natural_frequency, joint_damping_ratio); let rhs_bias = ((s_ang - s_limits[1]).max(0.0) - (s_limits[0] - s_ang).max(0.0)) * erp_inv_dt; @@ -962,6 +970,8 @@ impl JointTwoBodyConstraintHelper { mb2: (&Multibody, usize), locked_axis: usize, writeback_id: WritebackId, + joint_natural_frequency: Real, + joint_damping_ratio: Real, ) -> JointGenericOneBodyConstraint { let lin_jac = self.basis.column(locked_axis).into_owned(); let ang_jac1 = self.cmat1_basis.column(locked_axis).into_owned(); @@ -979,7 +989,7 @@ impl JointTwoBodyConstraintHelper { ang_jac2, ); - let erp_inv_dt = params.joint_erp_inv_dt(); + let erp_inv_dt = params.joint_erp_inv_dt(joint_natural_frequency, joint_damping_ratio); let rhs_bias = lin_jac.dot(&self.lin_err) * erp_inv_dt; c.rhs += rhs_bias; c @@ -996,6 +1006,8 @@ impl JointTwoBodyConstraintHelper { limited_axis: usize, limits: [Real; 2], writeback_id: WritebackId, + joint_natural_frequency: Real, + joint_damping_ratio: Real, ) -> JointGenericOneBodyConstraint { let lin_jac = self.basis.column(limited_axis).into_owned(); let ang_jac1 = self.cmat1_basis.column(limited_axis).into_owned(); @@ -1017,7 +1029,7 @@ impl JointTwoBodyConstraintHelper { let min_enabled = dist <= limits[0]; let max_enabled = limits[1] <= dist; - let erp_inv_dt = params.joint_erp_inv_dt(); + let erp_inv_dt = params.joint_erp_inv_dt(joint_natural_frequency, joint_damping_ratio); 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 = [ @@ -1090,6 +1102,8 @@ impl JointTwoBodyConstraintHelper { mb2: (&Multibody, usize), _locked_axis: usize, writeback_id: WritebackId, + joint_natural_frequency: Real, + joint_damping_ratio: Real, ) -> JointGenericOneBodyConstraint { #[cfg(feature = "dim2")] let ang_jac = Vector1::new(1.0); @@ -1108,7 +1122,7 @@ impl JointTwoBodyConstraintHelper { ang_jac, ); - let erp_inv_dt = params.joint_erp_inv_dt(); + let erp_inv_dt = params.joint_erp_inv_dt(joint_natural_frequency, joint_damping_ratio); #[cfg(feature = "dim2")] let rhs_bias = self.ang_err.im * erp_inv_dt; #[cfg(feature = "dim3")] @@ -1128,6 +1142,8 @@ impl JointTwoBodyConstraintHelper { _limited_axis: usize, limits: [Real; 2], writeback_id: WritebackId, + joint_natural_frequency: Real, + joint_damping_ratio: Real, ) -> JointGenericOneBodyConstraint { #[cfg(feature = "dim2")] let ang_jac = Vector1::new(1.0); @@ -1158,7 +1174,7 @@ impl JointTwoBodyConstraintHelper { max_enabled as u32 as Real * Real::MAX, ]; - let erp_inv_dt = params.joint_erp_inv_dt(); + let erp_inv_dt = params.joint_erp_inv_dt(joint_natural_frequency, joint_damping_ratio); let rhs_bias = ((s_ang - s_limits[1]).max(0.0) - (s_limits[0] - s_ang).max(0.0)) * erp_inv_dt; diff --git a/src/dynamics/solver/joint_constraint/joint_velocity_constraint.rs b/src/dynamics/solver/joint_constraint/joint_velocity_constraint.rs index 61a882194..a844e48d1 100644 --- a/src/dynamics/solver/joint_constraint/joint_velocity_constraint.rs +++ b/src/dynamics/solver/joint_constraint/joint_velocity_constraint.rs @@ -158,6 +158,12 @@ impl JointTwoBodyConstraint { let limit_axes = joint.limit_axes.bits() & !locked_axes; let coupled_axes = joint.coupled_axes.bits(); + let joint_natural_frequency = joint + .joint_natural_frequency + .unwrap_or(params.joint_natural_frequency); + let joint_damping_ratio = joint + .joint_damping_ratio + .unwrap_or(params.joint_damping_ratio); // The has_lin/ang_coupling test is needed to avoid shl overflow later. let has_lin_coupling = (coupled_axes & JointAxesMask::LIN_AXES.bits()) != 0; let first_coupled_lin_axis_id = @@ -208,6 +214,8 @@ impl JointTwoBodyConstraint { &joint.motors[i].motor_params(params.dt), limits, WritebackId::Motor(i), + joint_natural_frequency, + joint_damping_ratio, ); len += 1; } @@ -252,14 +260,24 @@ impl JointTwoBodyConstraint { body2, i - DIM, WritebackId::Dof(i), + joint_natural_frequency, + joint_damping_ratio, ); len += 1; } } 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)); + out[len] = builder.lock_linear( + params, + [joint_id], + body1, + body2, + i, + WritebackId::Dof(i), + joint_natural_frequency, + joint_damping_ratio, + ); len += 1; } } @@ -274,6 +292,8 @@ impl JointTwoBodyConstraint { i - DIM, [joint.limits[i].min, joint.limits[i].max], WritebackId::Limit(i), + joint_natural_frequency, + joint_damping_ratio, ); len += 1; } @@ -288,6 +308,8 @@ impl JointTwoBodyConstraint { i, [joint.limits[i].min, joint.limits[i].max], WritebackId::Limit(i), + joint_natural_frequency, + joint_damping_ratio, ); len += 1; } @@ -306,6 +328,8 @@ impl JointTwoBodyConstraint { joint.limits[first_coupled_ang_axis_id].max, ], WritebackId::Limit(first_coupled_ang_axis_id), + joint_natural_frequency, + joint_damping_ratio, ); len += 1; } @@ -322,6 +346,8 @@ impl JointTwoBodyConstraint { joint.limits[first_coupled_lin_axis_id].max, ], WritebackId::Limit(first_coupled_lin_axis_id), + joint_natural_frequency, + joint_damping_ratio, ); len += 1; } @@ -494,6 +520,12 @@ impl JointOneBodyConstraint { let limit_axes = joint.limit_axes.bits() & !locked_axes; let coupled_axes = joint.coupled_axes.bits(); + let joint_natural_frequency = joint + .joint_natural_frequency + .unwrap_or(params.joint_natural_frequency); + let joint_damping_ratio = joint + .joint_damping_ratio + .unwrap_or(params.joint_damping_ratio); // The has_lin/ang_coupling test is needed to avoid shl overflow later. let has_lin_coupling = (coupled_axes & JointAxesMask::LIN_AXES.bits()) != 0; let first_coupled_lin_axis_id = @@ -589,6 +621,8 @@ impl JointOneBodyConstraint { body2, i - DIM, WritebackId::Dof(i), + joint_natural_frequency, + joint_damping_ratio, ); len += 1; } @@ -602,6 +636,8 @@ impl JointOneBodyConstraint { body2, i, WritebackId::Dof(i), + joint_natural_frequency, + joint_damping_ratio, ); len += 1; } @@ -617,6 +653,8 @@ impl JointOneBodyConstraint { i - DIM, [joint.limits[i].min, joint.limits[i].max], WritebackId::Limit(i), + joint_natural_frequency, + joint_damping_ratio, ); len += 1; } @@ -631,6 +669,8 @@ impl JointOneBodyConstraint { i, [joint.limits[i].min, joint.limits[i].max], WritebackId::Limit(i), + joint_natural_frequency, + joint_damping_ratio, ); len += 1; } @@ -649,6 +689,8 @@ impl JointOneBodyConstraint { joint.limits[first_coupled_ang_axis_id].max, ], WritebackId::Limit(first_coupled_ang_axis_id), + joint_natural_frequency, + joint_damping_ratio, ); len += 1; } @@ -665,6 +707,8 @@ impl JointOneBodyConstraint { joint.limits[first_coupled_lin_axis_id].max, ], WritebackId::Limit(first_coupled_lin_axis_id), + joint_natural_frequency, + joint_damping_ratio, ); len += 1; } From 6fd192617496db9ae7a491d250d4617aaf547549 Mon Sep 17 00:00:00 2001 From: Thierry Berger Date: Tue, 14 Jan 2025 10:54:05 +0100 Subject: [PATCH 2/7] fix compiling --- .../joint_constraint_builder.rs | 7 ++++++ .../joint_velocity_constraint.rs | 22 +++++++++++++++++-- 2 files changed, 27 insertions(+), 2 deletions(-) diff --git a/src/dynamics/solver/joint_constraint/joint_constraint_builder.rs b/src/dynamics/solver/joint_constraint/joint_constraint_builder.rs index 08956d582..d1245f16d 100644 --- a/src/dynamics/solver/joint_constraint/joint_constraint_builder.rs +++ b/src/dynamics/solver/joint_constraint/joint_constraint_builder.rs @@ -169,6 +169,9 @@ impl JointTwoBodyConstraintBuilderSimd { &frame1, &frame2, self.locked_axes, + // TODO: get those parameters in a joint specific fashion. + params.joint_natural_frequency, + params.joint_damping_ratio, &mut out[self.constraint_id..], ); } @@ -337,6 +340,10 @@ impl JointOneBodyConstraintBuilderSimd { &self.frame1, &frame2, self.locked_axes, + // TODO: Get those parameters in a joint specific fashion + // This should be part of JointOneBodyConstraintBuilderSimd ; added from `generate`, with data from ImpulseJoint (inner GenericJoint) + params.joint_natural_frequency, + params.joint_damping_ratio, &mut out[self.constraint_id..], ); } diff --git a/src/dynamics/solver/joint_constraint/joint_velocity_constraint.rs b/src/dynamics/solver/joint_constraint/joint_velocity_constraint.rs index a844e48d1..c5b7555d7 100644 --- a/src/dynamics/solver/joint_constraint/joint_velocity_constraint.rs +++ b/src/dynamics/solver/joint_constraint/joint_velocity_constraint.rs @@ -386,6 +386,8 @@ impl JointTwoBodyConstraint { frame1: &Isometry, frame2: &Isometry, locked_axes: u8, + joint_natural_frequency: Real, + joint_damping_ratio: Real, out: &mut [Self], ) -> usize { let builder = JointTwoBodyConstraintHelper::new( @@ -399,8 +401,16 @@ impl JointTwoBodyConstraint { let mut len = 0; 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)); + out[len] = builder.lock_linear( + params, + joint_id, + body1, + body2, + i, + WritebackId::Dof(i), + joint_natural_frequency, + joint_damping_ratio, + ); len += 1; } } @@ -414,6 +424,8 @@ impl JointTwoBodyConstraint { body2, i - DIM, WritebackId::Dof(i), + joint_natural_frequency, + joint_damping_ratio, ); len += 1; } @@ -743,6 +755,8 @@ impl JointOneBodyConstraint { frame1: &Isometry, frame2: &Isometry, locked_axes: u8, + joint_natural_frequency: Real, + joint_damping_ratio: Real, out: &mut [Self], ) -> usize { let mut len = 0; @@ -763,6 +777,8 @@ impl JointOneBodyConstraint { body2, i, WritebackId::Dof(i), + joint_natural_frequency, + joint_damping_ratio, ); len += 1; } @@ -776,6 +792,8 @@ impl JointOneBodyConstraint { body2, i - DIM, WritebackId::Dof(i), + joint_natural_frequency, + joint_damping_ratio, ); len += 1; } From b8ed7bb9349e211125755a95e0d7ec076a8b160e Mon Sep 17 00:00:00 2001 From: Thierry Berger Date: Mon, 20 Jan 2025 14:54:14 +0100 Subject: [PATCH 3/7] removed joint_damping_ratio and joint_natural_frequency from integration parameters, in favor of having them directly in GenericJoint --- src/dynamics/integration_parameters.rs | 15 ------ src/dynamics/joint/generic_joint.rs | 20 +++---- .../multibody_joint/multibody_joint_set.rs | 4 ++ .../multibody_joint/unit_multibody_joint.rs | 5 +- .../joint_generic_constraint.rs | 16 ++---- .../joint_velocity_constraint.rs | 16 ++---- src_testbed/testbed.rs | 53 ++++++++++++++++++- src_testbed/ui.rs | 9 ++-- 8 files changed, 76 insertions(+), 62 deletions(-) diff --git a/src/dynamics/integration_parameters.rs b/src/dynamics/integration_parameters.rs index f93dd012c..1bd0f9535 100644 --- a/src/dynamics/integration_parameters.rs +++ b/src/dynamics/integration_parameters.rs @@ -43,19 +43,6 @@ pub struct IntegrationParameters { /// (default: `30.0`). pub contact_natural_frequency: Real, - /// > 0: the natural frequency used by the springs for joint constraint regularization. - /// - /// Increasing this value will make it so that penetrations get fixed more quickly. - /// (default: `1.0e6`). - pub joint_natural_frequency: Real, - - /// The fraction of critical damping applied to the joint for constraints regularization. - /// - /// Larger values make the constraints more compliant (allowing more joint - /// drift before stabilization). - /// (default `1.0`). - pub joint_damping_ratio: Real, - /// The coefficient in `[0, 1]` applied to warmstart impulses, i.e., impulses that are used as the /// initial solution (instead of 0) at the next simulation step. /// @@ -280,8 +267,6 @@ impl IntegrationParameters { min_ccd_dt: 1.0 / 60.0 / 100.0, contact_natural_frequency: 30.0, contact_damping_ratio: 5.0, - joint_natural_frequency: 1.0e6, - joint_damping_ratio: 1.0, warmstart_coefficient: 1.0, num_internal_pgs_iterations: 1, num_internal_stabilization_iterations: 2, diff --git a/src/dynamics/joint/generic_joint.rs b/src/dynamics/joint/generic_joint.rs index b8c8a9194..dfb1fc4d7 100644 --- a/src/dynamics/joint/generic_joint.rs +++ b/src/dynamics/joint/generic_joint.rs @@ -251,23 +251,15 @@ pub struct GenericJoint { /// > 0: the natural frequency used by the springs for joint constraint regularization. /// /// Increasing this value will make it so that penetrations get fixed more quickly. - /// (default: [`None`]). - /// - /// If [`None`], - /// [`IntegrationParameters::joint_natural_frequency`](crate::prelude::IntegrationParameters::joint_natural_frequency) - /// is used. - pub joint_natural_frequency: Option, + /// (default: `1.0e6`). + pub joint_natural_frequency: Real, /// The fraction of critical damping applied to the joint for constraints regularization. /// /// Larger values make the constraints more compliant (allowing more joint /// drift before stabilization). - /// (default [`None`]). - /// - /// If [`None`], - /// [`IntegrationParameters::joint_damping_ratio`](crate::prelude::IntegrationParameters::joint_damping_ratio) - /// is used. - pub joint_damping_ratio: Option, + /// (default `1.0`). + pub joint_damping_ratio: Real, } impl Default for GenericJoint { @@ -284,8 +276,8 @@ impl Default for GenericJoint { contacts_enabled: true, enabled: JointEnabled::Enabled, user_data: 0, - joint_natural_frequency: None, - joint_damping_ratio: None, + joint_natural_frequency: 1.0e6, + joint_damping_ratio: 1.0, } } } diff --git a/src/dynamics/joint/multibody_joint/multibody_joint_set.rs b/src/dynamics/joint/multibody_joint/multibody_joint_set.rs index d31572913..cdfd975ea 100644 --- a/src/dynamics/joint/multibody_joint/multibody_joint_set.rs +++ b/src/dynamics/joint/multibody_joint/multibody_joint_set.rs @@ -464,6 +464,10 @@ impl MultibodyJointSet { pub fn multibodies(&self) -> impl Iterator { self.multibodies.iter().map(|e| e.1) } + /// Iterates through all the multibodies on this set. + pub fn multibody_links_mut(&mut self) -> impl Iterator { + self.multibodies.iter_mut().flat_map(|e| e.1.links_mut()) + } } impl std::ops::Index for MultibodyJointSet { diff --git a/src/dynamics/joint/multibody_joint/unit_multibody_joint.rs b/src/dynamics/joint/multibody_joint/unit_multibody_joint.rs index c1c766810..634f7e670 100644 --- a/src/dynamics/joint/multibody_joint/unit_multibody_joint.rs +++ b/src/dynamics/joint/multibody_joint/unit_multibody_joint.rs @@ -20,9 +20,8 @@ pub fn unit_joint_limit_constraint( constraints: &mut [JointGenericOneBodyConstraint], insert_at: &mut usize, ) { - // TODO: use joint_natural_frequency and joint_damping_ratio from multibodies GenericJoint. - let joint_natural_frequency = params.joint_natural_frequency; - let joint_damping_ratio = params.joint_damping_ratio; + let joint_natural_frequency = link.joint.data.joint_natural_frequency; + let joint_damping_ratio = link.joint.data.joint_damping_ratio; let ndofs = multibody.ndofs(); let min_enabled = curr_pos < limits[0]; diff --git a/src/dynamics/solver/joint_constraint/joint_generic_constraint.rs b/src/dynamics/solver/joint_constraint/joint_generic_constraint.rs index 89eb4c472..dd8e6b2c0 100644 --- a/src/dynamics/solver/joint_constraint/joint_generic_constraint.rs +++ b/src/dynamics/solver/joint_constraint/joint_generic_constraint.rs @@ -89,12 +89,8 @@ impl JointGenericTwoBodyConstraint { locked_axes, ); - let joint_natural_frequency = joint - .joint_natural_frequency - .unwrap_or(params.joint_natural_frequency); - let joint_damping_ratio = joint - .joint_damping_ratio - .unwrap_or(params.joint_damping_ratio); + let joint_natural_frequency = joint.joint_natural_frequency; + let joint_damping_ratio = joint.joint_damping_ratio; let start = len; for i in DIM..SPATIAL_DIM { @@ -392,12 +388,8 @@ impl JointGenericOneBodyConstraint { locked_axes, ); - let joint_natural_frequency = joint - .joint_natural_frequency - .unwrap_or(params.joint_natural_frequency); - let joint_damping_ratio = joint - .joint_damping_ratio - .unwrap_or(params.joint_damping_ratio); + let joint_natural_frequency = joint.joint_natural_frequency; + let joint_damping_ratio = joint.joint_damping_ratio; let start = len; for i in DIM..SPATIAL_DIM { diff --git a/src/dynamics/solver/joint_constraint/joint_velocity_constraint.rs b/src/dynamics/solver/joint_constraint/joint_velocity_constraint.rs index c5b7555d7..ada6052d8 100644 --- a/src/dynamics/solver/joint_constraint/joint_velocity_constraint.rs +++ b/src/dynamics/solver/joint_constraint/joint_velocity_constraint.rs @@ -158,12 +158,8 @@ impl JointTwoBodyConstraint { let limit_axes = joint.limit_axes.bits() & !locked_axes; let coupled_axes = joint.coupled_axes.bits(); - let joint_natural_frequency = joint - .joint_natural_frequency - .unwrap_or(params.joint_natural_frequency); - let joint_damping_ratio = joint - .joint_damping_ratio - .unwrap_or(params.joint_damping_ratio); + let joint_natural_frequency = joint.joint_natural_frequency; + let joint_damping_ratio = joint.joint_damping_ratio; // The has_lin/ang_coupling test is needed to avoid shl overflow later. let has_lin_coupling = (coupled_axes & JointAxesMask::LIN_AXES.bits()) != 0; let first_coupled_lin_axis_id = @@ -532,12 +528,8 @@ impl JointOneBodyConstraint { let limit_axes = joint.limit_axes.bits() & !locked_axes; let coupled_axes = joint.coupled_axes.bits(); - let joint_natural_frequency = joint - .joint_natural_frequency - .unwrap_or(params.joint_natural_frequency); - let joint_damping_ratio = joint - .joint_damping_ratio - .unwrap_or(params.joint_damping_ratio); + let joint_natural_frequency = joint.joint_natural_frequency; + let joint_damping_ratio = joint.joint_damping_ratio; // The has_lin/ang_coupling test is needed to avoid shl overflow later. let has_lin_coupling = (coupled_axes & JointAxesMask::LIN_AXES.bits()) != 0; let first_coupled_lin_axis_id = diff --git a/src_testbed/testbed.rs b/src_testbed/testbed.rs index 37b571013..c634a258f 100644 --- a/src_testbed/testbed.rs +++ b/src_testbed/testbed.rs @@ -125,6 +125,24 @@ pub struct TestbedState { camera_locked: bool, // Used so that the camera can remain the same before and after we change backend or press the restart button. } +#[derive(Resource, Clone, Debug, PartialEq)] +pub struct JointsConfiguration { + /// Sets [`rapier::prelude::GenericJoint::joint_natural_frequency`] for all joints. + pub joint_natural_frequency: Real, + + /// Sets [`rapier::prelude::GenericJoint::joint_damping_ratio`] for all joints. + pub joint_damping_ratio: Real, +} + +impl Default for JointsConfiguration { + fn default() -> Self { + Self { + joint_natural_frequency: 1.0e6, + joint_damping_ratio: 1.0, + } + } +} + #[derive(Resource)] struct SceneBuilders(SimulationBuilders); @@ -442,6 +460,7 @@ impl TestbedApp { ..Default::default() }) .init_resource::() + .init_resource::() .add_plugins(DefaultPlugins.set(window_plugin)) .add_plugins(OrbitCameraPlugin) .add_plugins(WireframePlugin) @@ -462,7 +481,11 @@ impl TestbedApp { .insert_non_send_resource(self.plugins) .add_systems(Update, update_testbed) .add_systems(Update, egui_focus) - .add_systems(Update, track_mouse_state); + .add_systems(Update, track_mouse_state) + .add_systems( + Update, + update_joint_configuration.run_if(resource_changed::), + ); init(&mut app); app.run(); @@ -1184,6 +1207,7 @@ fn update_testbed( builders: ResMut, mut graphics: NonSendMut, mut state: ResMut, + mut joints_configuration: ResMut, mut debug_render: ResMut, mut harness: NonSendMut, #[cfg(feature = "other-backends")] mut other_backends: NonSendMut, @@ -1237,7 +1261,18 @@ fn update_testbed( // Update UI { let harness = &mut *harness; - ui::update_ui(&mut ui_context, &mut state, harness, &mut debug_render); + // Bypass change detection, to avoid changing all joints data each frames. + let mut joints_configuration_cloned = joints_configuration.clone(); + ui::update_ui( + &mut ui_context, + &mut state, + harness, + &mut debug_render, + &mut joints_configuration_cloned, + ); + if joints_configuration_cloned != *joints_configuration { + *joints_configuration = joints_configuration_cloned; + } for plugin in &mut plugins.0 { plugin.update_ui( @@ -1655,3 +1690,17 @@ fn highlight_hovered_body( } } } + +pub fn update_joint_configuration( + joints_configuration: Res, + mut harness: NonSendMut, +) { + for joint in harness.physics.impulse_joints.iter_mut() { + joint.1.data.joint_natural_frequency = joints_configuration.joint_natural_frequency; + joint.1.data.joint_damping_ratio = joints_configuration.joint_damping_ratio; + } + for joint in harness.physics.multibody_joints.multibody_links_mut() { + joint.joint.data.joint_natural_frequency = joints_configuration.joint_natural_frequency; + joint.joint.data.joint_damping_ratio = joints_configuration.joint_damping_ratio; + } +} diff --git a/src_testbed/ui.rs b/src_testbed/ui.rs index 5945c38b2..6c4a22a56 100644 --- a/src_testbed/ui.rs +++ b/src_testbed/ui.rs @@ -6,8 +6,8 @@ use std::num::NonZeroUsize; use crate::debug_render::DebugRenderPipelineResource; use crate::harness::Harness; use crate::testbed::{ - RapierSolverType, RunMode, TestbedActionFlags, TestbedState, TestbedStateFlags, - PHYSX_BACKEND_PATCH_FRICTION, PHYSX_BACKEND_TWO_FRICTION_DIR, + JointsConfiguration, RapierSolverType, RunMode, TestbedActionFlags, TestbedState, + TestbedStateFlags, PHYSX_BACKEND_PATCH_FRICTION, PHYSX_BACKEND_TWO_FRICTION_DIR, }; use crate::PhysicsState; @@ -21,6 +21,7 @@ pub fn update_ui( state: &mut TestbedState, harness: &mut Harness, debug_render: &mut DebugRenderPipelineResource, + joints_configuration: &mut JointsConfiguration, ) { #[cfg(feature = "profiler_ui")] { @@ -231,13 +232,13 @@ pub fn update_ui( ); ui.add( Slider::new( - &mut integration_parameters.joint_natural_frequency, + &mut joints_configuration.joint_natural_frequency, 0.0..=1200000.0, ) .text("joint erp"), ); ui.add( - Slider::new(&mut integration_parameters.joint_damping_ratio, 0.0..=20.0) + Slider::new(&mut joints_configuration.joint_damping_ratio, 0.0..=20.0) .text("joint damping ratio"), ); } From c6230c3c8468e45cb5eb3f2da8c83ce20b390c07 Mon Sep 17 00:00:00 2001 From: Thierry Berger Date: Mon, 20 Jan 2025 14:56:41 +0100 Subject: [PATCH 4/7] add changelog entry --- CHANGELOG.md | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index efd1905b2..31ca72ec6 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -1,3 +1,9 @@ +## Unreleased + +### Modified + +- Removed `IntegrationParameters::joint_natural_frequency` and `IntegrationParameters::joint_damping_ratio` in favor of being in `GenericJoint` (#789). + ## v0.23.0 (08 Jan 2025) ### Fix From d1bbe2b22b751eae9c5621a0573ed84baf552dc3 Mon Sep 17 00:00:00 2001 From: Thierry Berger Date: Mon, 20 Jan 2025 15:20:04 +0100 Subject: [PATCH 5/7] fix simd joint configuration retrieval --- .../joint_constraint_builder.rs | 17 ++++++++++++----- 1 file changed, 12 insertions(+), 5 deletions(-) diff --git a/src/dynamics/solver/joint_constraint/joint_constraint_builder.rs b/src/dynamics/solver/joint_constraint/joint_constraint_builder.rs index d1245f16d..2f45b5a04 100644 --- a/src/dynamics/solver/joint_constraint/joint_constraint_builder.rs +++ b/src/dynamics/solver/joint_constraint/joint_constraint_builder.rs @@ -99,6 +99,8 @@ pub struct JointTwoBodyConstraintBuilderSimd { local_frame2: Isometry, locked_axes: u8, constraint_id: usize, + joint_natural_frequency: Real, + joint_damping_ratio: Real, } #[cfg(feature = "simd-is-enabled")] @@ -139,6 +141,8 @@ impl JointTwoBodyConstraintBuilderSimd { local_frame2: gather![|ii| joint[ii].data.local_frame2].into(), locked_axes: joint[0].data.locked_axes.bits(), constraint_id: *out_constraint_id, + joint_natural_frequency: joint[0].data.joint_natural_frequency, + joint_damping_ratio: joint[0].data.joint_damping_ratio, }; let count = ConstraintsCounts::from_joint(joint[0]); @@ -169,9 +173,8 @@ impl JointTwoBodyConstraintBuilderSimd { &frame1, &frame2, self.locked_axes, - // TODO: get those parameters in a joint specific fashion. - params.joint_natural_frequency, - params.joint_damping_ratio, + self.joint_natural_frequency, + self.joint_damping_ratio, &mut out[self.constraint_id..], ); } @@ -268,6 +271,8 @@ pub struct JointOneBodyConstraintBuilderSimd { local_frame2: Isometry, locked_axes: u8, constraint_id: usize, + joint_natural_frequency: Real, + joint_damping_ratio: Real, } #[cfg(feature = "simd-is-enabled")] @@ -307,6 +312,8 @@ impl JointOneBodyConstraintBuilderSimd { local_frame2: local_frame2.into(), locked_axes: joint[0].data.locked_axes.bits(), constraint_id: *out_constraint_id, + joint_natural_frequency: joint[0].data.joint_natural_frequency, + joint_damping_ratio: joint[0].data.joint_damping_ratio, }; let count = ConstraintsCounts::from_joint(joint[0]); @@ -342,8 +349,8 @@ impl JointOneBodyConstraintBuilderSimd { self.locked_axes, // TODO: Get those parameters in a joint specific fashion // This should be part of JointOneBodyConstraintBuilderSimd ; added from `generate`, with data from ImpulseJoint (inner GenericJoint) - params.joint_natural_frequency, - params.joint_damping_ratio, + self.joint_natural_frequency, + self.joint_damping_ratio, &mut out[self.constraint_id..], ); } From 7814b3215e8ff48d5d99435654e6aa872b10a47a Mon Sep 17 00:00:00 2001 From: Thierry Berger Date: Mon, 27 Jan 2025 09:33:25 +0100 Subject: [PATCH 6/7] Update src/dynamics/joint/generic_joint.rs --- src/dynamics/joint/generic_joint.rs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/dynamics/joint/generic_joint.rs b/src/dynamics/joint/generic_joint.rs index dfb1fc4d7..034daeb88 100644 --- a/src/dynamics/joint/generic_joint.rs +++ b/src/dynamics/joint/generic_joint.rs @@ -250,7 +250,7 @@ pub struct GenericJoint { pub user_data: u128, /// > 0: the natural frequency used by the springs for joint constraint regularization. /// - /// Increasing this value will make it so that penetrations get fixed more quickly. + /// Increasing this value will make it so that positioning errors get fixed more quickly. /// (default: `1.0e6`). pub joint_natural_frequency: Real, From 5f1cb6fceea1dbfa47b546a7332354cb0f4ec535 Mon Sep 17 00:00:00 2001 From: Thierry Berger Date: Mon, 27 Jan 2025 09:43:20 +0100 Subject: [PATCH 7/7] simpler accessor for multibodies_mut --- src/dynamics/joint/multibody_joint/multibody_joint_set.rs | 4 ++-- src_testbed/testbed.rs | 7 ++++++- 2 files changed, 8 insertions(+), 3 deletions(-) diff --git a/src/dynamics/joint/multibody_joint/multibody_joint_set.rs b/src/dynamics/joint/multibody_joint/multibody_joint_set.rs index cdfd975ea..1b5159777 100644 --- a/src/dynamics/joint/multibody_joint/multibody_joint_set.rs +++ b/src/dynamics/joint/multibody_joint/multibody_joint_set.rs @@ -465,8 +465,8 @@ impl MultibodyJointSet { self.multibodies.iter().map(|e| e.1) } /// Iterates through all the multibodies on this set. - pub fn multibody_links_mut(&mut self) -> impl Iterator { - self.multibodies.iter_mut().flat_map(|e| e.1.links_mut()) + pub fn multibodies_mut(&mut self) -> impl Iterator { + self.multibodies.iter_mut().map(|e| e.1) } } diff --git a/src_testbed/testbed.rs b/src_testbed/testbed.rs index c634a258f..a6266fc5f 100644 --- a/src_testbed/testbed.rs +++ b/src_testbed/testbed.rs @@ -1699,7 +1699,12 @@ pub fn update_joint_configuration( joint.1.data.joint_natural_frequency = joints_configuration.joint_natural_frequency; joint.1.data.joint_damping_ratio = joints_configuration.joint_damping_ratio; } - for joint in harness.physics.multibody_joints.multibody_links_mut() { + for joint in harness + .physics + .multibody_joints + .multibodies_mut() + .flat_map(|e| e.links_mut()) + { joint.joint.data.joint_natural_frequency = joints_configuration.joint_natural_frequency; joint.joint.data.joint_damping_ratio = joints_configuration.joint_damping_ratio; }