Add joint_natural_frequency and joint_damping_ratio to GenericJoint.

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

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,

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 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,
 }
 
 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,
         }
     }
 }

src/dynamics/joint/multibody_joint/multibody_joint_set.rs

@@ -464,6 +464,10 @@ impl MultibodyJointSet {
     pub fn multibodies(&self) -> impl Iterator<Item = &Multibody> {
         self.multibodies.iter().map(|e| e.1)
     }
+    /// Iterates through all the multibodies on this set.
+    pub fn multibody_links_mut(&mut self) -> impl Iterator<Item = &mut MultibodyLink> {
+        self.multibodies.iter_mut().flat_map(|e| e.1.links_mut())
+    }
 }
 
 impl std::ops::Index<MultibodyIndex> for MultibodyJointSet {

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;