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 diff --git a/src/dynamics/integration_parameters.rs b/src/dynamics/integration_parameters.rs index 99b6fe48d..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. /// @@ -159,23 +146,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 +215,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 @@ -275,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 8d4066d35..034daeb88 100644 --- a/src/dynamics/joint/generic_joint.rs +++ b/src/dynamics/joint/generic_joint.rs @@ -248,6 +248,18 @@ 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 positioning errors 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, } impl Default for GenericJoint { @@ -264,6 +276,8 @@ impl Default for GenericJoint { contacts_enabled: true, enabled: JointEnabled::Enabled, user_data: 0, + 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..1b5159777 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 multibodies_mut(&mut self) -> impl Iterator { + self.multibodies.iter_mut().map(|e| e.1) + } } 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 d6efd12d6..634f7e670 100644 --- a/src/dynamics/joint/multibody_joint/unit_multibody_joint.rs +++ b/src/dynamics/joint/multibody_joint/unit_multibody_joint.rs @@ -20,11 +20,14 @@ pub fn unit_joint_limit_constraint( constraints: &mut [JointGenericOneBodyConstraint], insert_at: &mut usize, ) { + 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]; 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..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,6 +173,8 @@ impl JointTwoBodyConstraintBuilderSimd { &frame1, &frame2, self.locked_axes, + self.joint_natural_frequency, + self.joint_damping_ratio, &mut out[self.constraint_id..], ); } @@ -265,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")] @@ -304,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]); @@ -337,6 +347,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) + self.joint_natural_frequency, + self.joint_damping_ratio, &mut out[self.constraint_id..], ); } @@ -423,17 +437,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 +481,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 +519,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 +557,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 +679,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 +693,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 +731,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 +754,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 +843,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 +852,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 +940,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 +962,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 +997,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 +1034,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 +1195,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 +1207,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 +1288,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 +1312,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 +1347,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 +1357,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 +1443,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 +1469,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 +1506,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 +1532,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..dd8e6b2c0 100644 --- a/src/dynamics/solver/joint_constraint/joint_generic_constraint.rs +++ b/src/dynamics/solver/joint_constraint/joint_generic_constraint.rs @@ -89,6 +89,9 @@ impl JointGenericTwoBodyConstraint { locked_axes, ); + 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 { if motor_axes & (1 << i) != 0 { @@ -141,6 +144,8 @@ impl JointGenericTwoBodyConstraint { mb2, i - DIM, WritebackId::Dof(i), + joint_natural_frequency, + joint_damping_ratio, ); len += 1; } @@ -158,6 +163,8 @@ impl JointGenericTwoBodyConstraint { mb2, i, WritebackId::Dof(i), + joint_natural_frequency, + joint_damping_ratio, ); len += 1; } @@ -177,6 +184,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 +204,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 +388,9 @@ impl JointGenericOneBodyConstraint { locked_axes, ); + 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 { if motor_axes & (1 << i) != 0 { @@ -428,6 +442,8 @@ impl JointGenericOneBodyConstraint { mb2, i - DIM, WritebackId::Dof(i), + joint_natural_frequency, + joint_damping_ratio, ); len += 1; } @@ -443,6 +459,8 @@ impl JointGenericOneBodyConstraint { mb2, i, WritebackId::Dof(i), + joint_natural_frequency, + joint_damping_ratio, ); len += 1; } @@ -460,6 +478,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 +496,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..ada6052d8 100644 --- a/src/dynamics/solver/joint_constraint/joint_velocity_constraint.rs +++ b/src/dynamics/solver/joint_constraint/joint_velocity_constraint.rs @@ -158,6 +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; + 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 = @@ -208,6 +210,8 @@ impl JointTwoBodyConstraint { &joint.motors[i].motor_params(params.dt), limits, WritebackId::Motor(i), + joint_natural_frequency, + joint_damping_ratio, ); len += 1; } @@ -252,14 +256,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 +288,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 +304,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 +324,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 +342,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; } @@ -360,6 +382,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( @@ -373,8 +397,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; } } @@ -388,6 +420,8 @@ impl JointTwoBodyConstraint { body2, i - DIM, WritebackId::Dof(i), + joint_natural_frequency, + joint_damping_ratio, ); len += 1; } @@ -494,6 +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; + 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 = @@ -589,6 +625,8 @@ impl JointOneBodyConstraint { body2, i - DIM, WritebackId::Dof(i), + joint_natural_frequency, + joint_damping_ratio, ); len += 1; } @@ -602,6 +640,8 @@ impl JointOneBodyConstraint { body2, i, WritebackId::Dof(i), + joint_natural_frequency, + joint_damping_ratio, ); len += 1; } @@ -617,6 +657,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 +673,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 +693,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 +711,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; } @@ -699,6 +747,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; @@ -719,6 +769,8 @@ impl JointOneBodyConstraint { body2, i, WritebackId::Dof(i), + joint_natural_frequency, + joint_damping_ratio, ); len += 1; } @@ -732,6 +784,8 @@ impl JointOneBodyConstraint { body2, i - DIM, WritebackId::Dof(i), + joint_natural_frequency, + joint_damping_ratio, ); len += 1; } diff --git a/src_testbed/testbed.rs b/src_testbed/testbed.rs index 37b571013..a6266fc5f 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,22 @@ 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 + .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; + } +} 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"), ); }