nichkara/rapier
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Add joint softness per joint (#901)

Browse Source
This commit is contained in:
Dragos Daian
2025-11-21 16:48:52 +01:00
committed by GitHub
parent 5f687b0d29
commit e40d3a9dce
22 changed files with 442 additions and 200 deletions
Download Patch File Download Diff File Expand all files Collapse all files

253
src/dynamics/integration_parameters.rs
View File

@@ -1,8 +1,7 @@
use crate::math::Real;
use na::RealField;
#[cfg(doc)]
use super::RigidBodyActivation;
use crate::math::Real;
use simba::simd::SimdRealField;
// TODO: enabling the block solver in 3d introduces a lot of jitters in
// the 3D domino demo. So for now we dont enable it in 3D.
@@ -30,6 +29,114 @@ pub enum FrictionModel {
Coulomb,
}
#[derive(Copy, Clone, Debug, PartialEq)]
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
// TODO: we should be able to combine this with MotorModel.
/// Coefficients for a spring, typically used for configuring constraint softness for contacts and
/// joints.
pub struct SpringCoefficients<N> {
/// Sets the natural frequency (Hz) of the spring-like constraint.
///
/// Higher values make the constraint stiffer and resolve constraint violations more quickly.
pub natural_frequency: N,
/// Sets the damping ratio for the spring-like constraint.
///
/// Larger values make the joint more compliant (allowing more drift before stabilization).
pub damping_ratio: N,
}
impl<N: SimdRealField<Element = Real> + Copy> SpringCoefficients<N> {
/// Initializes spring coefficients from the spring natural frequency and damping ratio.
pub fn new(natural_frequency: N, damping_ratio: N) -> Self {
Self {
natural_frequency,
damping_ratio,
}
}
/// Default softness coefficients for contacts.
pub fn contact_defaults() -> Self {
Self {
natural_frequency: N::splat(30.0),
damping_ratio: N::splat(5.0),
}
}
/// Default softness coefficients for joints.
pub fn joint_defaults() -> Self {
Self {
natural_frequency: N::splat(1.0e6),
damping_ratio: N::splat(1.0),
}
}
/// The contact’s spring angular frequency for constraints regularization.
pub fn angular_frequency(&self) -> N {
self.natural_frequency * N::simd_two_pi()
}
/// The [`Self::erp`] coefficient, multiplied by the inverse timestep length.
pub fn erp_inv_dt(&self, dt: N) -> N {
let ang_freq = self.angular_frequency();
ang_freq / (dt * ang_freq + N::splat(2.0) * self.damping_ratio)
}
/// The effective Error Reduction Parameter applied for calculating regularization forces.
///
/// This parameter is computed automatically from [`Self::natural_frequency`],
/// [`Self::damping_ratio`] and the substep length.
pub fn erp(&self, dt: N) -> N {
dt * self.erp_inv_dt(dt)
}
/// Compute CFM assuming a critically damped spring multiplied by the damping ratio.
///
/// This coefficient softens the impulse applied at each solver iteration.
pub fn cfm_coeff(&self, dt: N) -> N {
let one = N::one();
let erp = self.erp(dt);
let erp_is_not_zero = erp.simd_ne(N::zero());
let inv_erp_minus_one = one / erp - one;
// let stiffness = 4.0 * damping_ratio * damping_ratio * projected_mass
// / (dt * dt * inv_erp_minus_one * inv_erp_minus_one);
// let damping = 4.0 * damping_ratio * damping_ratio * projected_mass
// / (dt * inv_erp_minus_one);
// let cfm = 1.0 / (dt * dt * stiffness + dt * damping);
// NOTE: This simplifies to cfm = cfm_coeff / projected_mass:
let result = inv_erp_minus_one * inv_erp_minus_one
/ ((one + inv_erp_minus_one) * N::splat(4.0) * self.damping_ratio * self.damping_ratio);
result.select(erp_is_not_zero, N::zero())
}
/// The CFM factor to be used in the constraint resolution.
///
/// This parameter is computed automatically from [`Self::natural_frequency`],
/// [`Self::damping_ratio`] and the substep length.
pub fn cfm_factor(&self, dt: N) -> N {
let one = N::one();
let cfm_coeff = self.cfm_coeff(dt);
// We use this coefficient inside the impulse resolution.
// Surprisingly, several simplifications happen there.
// Let `m` the projected mass of the constraint.
// Let `m’` the projected mass that includes CFM: `m’ = 1 / (1 / m + cfm_coeff / m) = m / (1 + cfm_coeff)`
// We have:
// new_impulse = old_impulse - m’ (delta_vel - cfm * old_impulse)
// = old_impulse - m / (1 + cfm_coeff) * (delta_vel - cfm_coeff / m * old_impulse)
// = old_impulse * (1 - cfm_coeff / (1 + cfm_coeff)) - m / (1 + cfm_coeff) * delta_vel
// = old_impulse / (1 + cfm_coeff) - m * delta_vel / (1 + cfm_coeff)
// = 1 / (1 + cfm_coeff) * (old_impulse - m * delta_vel)
// So, setting cfm_factor = 1 / (1 + cfm_coeff).
// We obtain:
// new_impulse = cfm_factor * (old_impulse - m * delta_vel)
//
// The value returned by this function is this cfm_factor that can be used directly
// in the constraint solver.
one / (one + cfm_coeff)
}
}
/// Configuration parameters that control the physics simulation quality and behavior.
///
/// These parameters affect how the physics engine advances time, resolves collisions, and
@@ -80,34 +187,8 @@ pub struct IntegrationParameters {
/// to numerical instabilities.
pub min_ccd_dt: Real,
/// > 0: the damping ratio used by the springs for contact constraint stabilization.
///
/// Larger values make the constraints more compliant (allowing more visible
/// penetrations before stabilization).
/// (default `5.0`).
pub contact_damping_ratio: Real,
/// > 0: the natural frequency used by the springs for contact constraint regularization.
///
/// Increasing this value will make it so that penetrations get fixed more quickly at the
/// expense of potential jitter effects due to overshooting. In order to make the simulation
/// look stiffer, it is recommended to increase the [`Self::contact_damping_ratio`] instead of this
/// value.
/// (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,
/// Softness coefficients for contact constraints.
pub contact_softness: SpringCoefficients<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.
@@ -193,110 +274,7 @@ impl IntegrationParameters {
}
}
/// The contact’s spring angular frequency for constraints regularization.
pub fn contact_angular_frequency(&self) -> Real {
self.contact_natural_frequency * Real::two_pi()
}
/// The [`Self::contact_erp`] coefficient, multiplied by the inverse timestep length.
pub fn contact_erp_inv_dt(&self) -> Real {
let ang_freq = self.contact_angular_frequency();
ang_freq / (self.dt * ang_freq + 2.0 * self.contact_damping_ratio)
}
/// The effective Error Reduction Parameter applied for calculating regularization forces
/// on contacts.
///
/// This parameter is computed automatically from [`Self::contact_natural_frequency`],
/// [`Self::contact_damping_ratio`] and the substep length.
pub fn contact_erp(&self) -> Real {
self.dt * self.contact_erp_inv_dt()
}
/// The joint’s spring angular frequency for constraint regularization.
pub fn joint_angular_frequency(&self) -> Real {
self.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)
}
/// 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()
}
/// The CFM factor to be used in the constraint resolution.
///
/// This parameter is computed automatically from [`Self::contact_natural_frequency`],
/// [`Self::contact_damping_ratio`] and the substep length.
pub fn contact_cfm_factor(&self) -> Real {
// Compute CFM assuming a critically damped spring multiplied by the damping ratio.
// The logic is similar to [`Self::joint_cfm_coeff`].
let contact_erp = self.contact_erp();
if contact_erp == 0.0 {
return 0.0;
}
let inv_erp_minus_one = 1.0 / contact_erp - 1.0;
// let stiffness = 4.0 * damping_ratio * damping_ratio * projected_mass
// / (dt * dt * inv_erp_minus_one * inv_erp_minus_one);
// let damping = 4.0 * damping_ratio * damping_ratio * projected_mass
// / (dt * inv_erp_minus_one);
// let cfm = 1.0 / (dt * dt * stiffness + dt * damping);
// NOTE: This simplifies to cfm = cfm_coeff / projected_mass:
let cfm_coeff = inv_erp_minus_one * inv_erp_minus_one
/ ((1.0 + inv_erp_minus_one)
* 4.0
* self.contact_damping_ratio
* self.contact_damping_ratio);
// Furthermore, we use this coefficient inside of the impulse resolution.
// Surprisingly, several simplifications happen there.
// Let `m` the projected mass of the constraint.
// Let `m’` the projected mass that includes CFM: `m’ = 1 / (1 / m + cfm_coeff / m) = m / (1 + cfm_coeff)`
// We have:
// new_impulse = old_impulse - m’ (delta_vel - cfm * old_impulse)
// = old_impulse - m / (1 + cfm_coeff) * (delta_vel - cfm_coeff / m * old_impulse)
// = old_impulse * (1 - cfm_coeff / (1 + cfm_coeff)) - m / (1 + cfm_coeff) * delta_vel
// = old_impulse / (1 + cfm_coeff) - m * delta_vel / (1 + cfm_coeff)
// = 1 / (1 + cfm_coeff) * (old_impulse - m * delta_vel)
// So, setting cfm_factor = 1 / (1 + cfm_coeff).
// We obtain:
// new_impulse = cfm_factor * (old_impulse - m * delta_vel)
//
// The value returned by this function is this cfm_factor that can be used directly
// in the constraint solver.
1.0 / (1.0 + cfm_coeff)
}
/// 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 {
// 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();
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)
}
/// Amount of penetration the engine won’t attempt to correct (default: `0.001` multiplied by
/// Amount of penetration the engine won't attempt to correct (default: `0.001` multiplied by
/// [`Self::length_unit`]).
pub fn allowed_linear_error(&self) -> Real {
self.normalized_allowed_linear_error * self.length_unit
@@ -326,10 +304,7 @@ impl Default for IntegrationParameters {
Self {
dt: 1.0 / 60.0,
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,
contact_softness: SpringCoefficients::contact_defaults(),
warmstart_coefficient: 1.0,
num_internal_pgs_iterations: 1,
num_internal_stabilization_iterations: 1,

23
src/dynamics/joint/fixed_joint.rs
View File

@@ -1,3 +1,4 @@
use crate::dynamics::integration_parameters::SpringCoefficients;
use crate::dynamics::{GenericJoint, GenericJointBuilder, JointAxesMask};
use crate::math::{Isometry, Point, Real};
@@ -91,6 +92,19 @@ impl FixedJoint {
self.data.set_local_anchor2(anchor2);
self
}
/// Gets the softness of this joint’s locked degrees of freedom.
#[must_use]
pub fn softness(&self) -> SpringCoefficients<Real> {
self.data.softness
}
/// Sets the softness of this joint’s locked degrees of freedom.
#[must_use]
pub fn set_softness(&mut self, softness: SpringCoefficients<Real>) -> &mut Self {
self.data.softness = softness;
self
}
}
impl From<FixedJoint> for GenericJoint {
@@ -138,13 +152,20 @@ impl FixedJointBuilder {
self
}
/// Sets the joint’s anchor, expressed in the local-space of the second rigid-body.
/// Sets the joint's anchor, expressed in the local-space of the second rigid-body.
#[must_use]
pub fn local_anchor2(mut self, anchor2: Point<Real>) -> Self {
self.0.set_local_anchor2(anchor2);
self
}
/// Sets the softness of this joint’s locked degrees of freedom.
#[must_use]
pub fn softness(mut self, softness: SpringCoefficients<Real>) -> Self {
self.0.data.softness = softness;
self
}
/// Build the fixed joint.
#[must_use]
pub fn build(self) -> FixedJoint {

19
src/dynamics/joint/generic_joint.rs
View File

@@ -1,5 +1,7 @@
#![allow(clippy::bad_bit_mask)] // Clippy will complain about the bitmasks due to JointAxesMask::FREE_FIXED_AXES being 0.
#![allow(clippy::unnecessary_cast)] // Casts are needed for switching between f32/f64.
use crate::dynamics::integration_parameters::SpringCoefficients;
use crate::dynamics::solver::MotorParameters;
use crate::dynamics::{
FixedJoint, MotorModel, PrismaticJoint, RevoluteJoint, RigidBody, RopeJoint,
@@ -282,6 +284,8 @@ pub struct GenericJoint {
/// For coupled degrees of freedoms (DoF), only the first linear (resp. angular) coupled DoF motor and `motor_axes`
/// bitmask is applied to the coupled linear (resp. angular) axes.
pub motors: [JointMotor; SPATIAL_DIM],
/// The coefficients controlling the joint constraints’ softness.
pub softness: SpringCoefficients<Real>,
/// Are contacts between the attached rigid-bodies enabled?
pub contacts_enabled: bool,
/// Whether the joint is enabled.
@@ -301,6 +305,7 @@ impl Default for GenericJoint {
coupled_axes: JointAxesMask::empty(),
limits: [JointLimits::default(); SPATIAL_DIM],
motors: [JointMotor::default(); SPATIAL_DIM],
softness: SpringCoefficients::joint_defaults(),
contacts_enabled: true,
enabled: JointEnabled::Enabled,
user_data: 0,
@@ -440,6 +445,13 @@ impl GenericJoint {
self
}
/// Sets the spring coefficients controlling this joint constraint’s softness.
#[must_use]
pub fn set_softness(&mut self, softness: SpringCoefficients<Real>) -> &mut Self {
self.softness = softness;
self
}
/// The joint limits along the specified axis.
#[must_use]
pub fn limits(&self, axis: JointAxis) -> Option<&JointLimits<Real>> {
@@ -767,6 +779,13 @@ impl GenericJointBuilder {
self
}
/// Sets the softness of this joint’s locked degrees of freedom.
#[must_use]
pub fn softness(mut self, softness: SpringCoefficients<Real>) -> Self {
self.0.softness = softness;
self
}
/// An arbitrary user-defined 128-bit integer associated to the joints built by this builder.
pub fn user_data(mut self, data: u128) -> Self {
self.0.user_data = data;

2
src/dynamics/joint/multibody_joint/multibody_joint.rs
View File

@@ -314,6 +314,7 @@ impl MultibodyJoint {
jacobians,
constraints,
&mut num_constraints,
self.data.softness,
);
}
curr_free_dof += 1;
@@ -349,6 +350,7 @@ impl MultibodyJoint {
jacobians,
constraints,
&mut num_constraints,
self.data.softness,
);
Some(limits)
} else {

9
src/dynamics/joint/multibody_joint/unit_multibody_joint.rs
View File

@@ -1,5 +1,6 @@
#![allow(missing_docs)] // For downcast.
use crate::dynamics::integration_parameters::SpringCoefficients;
use crate::dynamics::joint::MultibodyLink;
use crate::dynamics::solver::{GenericJointConstraint, WritebackId};
use crate::dynamics::{IntegrationParameters, JointMotor, Multibody};
@@ -19,12 +20,16 @@ pub fn unit_joint_limit_constraint(
jacobians: &mut DVector<Real>,
constraints: &mut [GenericJointConstraint],
insert_at: &mut usize,
softness: SpringCoefficients<Real>,
) {
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();
// Compute per-joint ERP and CFM
let erp_inv_dt = softness.erp_inv_dt(params.dt);
let cfm_coeff = softness.cfm_coeff(params.dt);
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;

23
src/dynamics/joint/pin_slot_joint.rs
View File

@@ -1,6 +1,7 @@
#[cfg(feature = "dim2")]
use crate::dynamics::joint::{GenericJointBuilder, JointAxesMask};
use crate::dynamics::integration_parameters::SpringCoefficients;
use crate::dynamics::joint::GenericJoint;
use crate::dynamics::{JointAxis, MotorModel};
use crate::math::{Point, Real, UnitVector};
@@ -155,6 +156,19 @@ impl PinSlotJoint {
self.data.set_limits(JointAxis::LinX, limits);
self
}
/// Gets the softness of this joint’s locked degrees of freedom.
#[must_use]
pub fn softness(&self) -> SpringCoefficients<Real> {
self.data.softness
}
/// Sets the softness of this joint’s locked degrees of freedom.
#[must_use]
pub fn set_softness(&mut self, softness: SpringCoefficients<Real>) -> &mut Self {
self.data.softness = softness;
self
}
}
impl From<PinSlotJoint> for GenericJoint {
@@ -255,13 +269,20 @@ impl PinSlotJointBuilder {
self
}
/// Sets the `[min,max]` limit distances attached bodies can translate along the joint’s principal axis.
/// Sets the `[min,max]` limit distances attached bodies can translate along the joint's principal axis.
#[must_use]
pub fn limits(mut self, limits: [Real; 2]) -> Self {
self.0.set_limits(limits);
self
}
/// Sets the softness of this joint’s locked degrees of freedom.
#[must_use]
pub fn softness(mut self, softness: SpringCoefficients<Real>) -> Self {
self.0.data.softness = softness;
self
}
/// Builds the pin slot joint.
#[must_use]
pub fn build(self) -> PinSlotJoint {

23
src/dynamics/joint/prismatic_joint.rs
View File

@@ -1,3 +1,4 @@
use crate::dynamics::integration_parameters::SpringCoefficients;
use crate::dynamics::joint::{GenericJoint, GenericJointBuilder, JointAxesMask};
use crate::dynamics::{JointAxis, MotorModel};
use crate::math::{Point, Real, UnitVector};
@@ -176,6 +177,19 @@ impl PrismaticJoint {
self.data.set_limits(JointAxis::LinX, limits);
self
}
/// Gets the softness of this joint’s locked degrees of freedom.
#[must_use]
pub fn softness(&self) -> SpringCoefficients<Real> {
self.data.softness
}
/// Sets the softness of this joint.
#[must_use]
pub fn set_softness(&mut self, softness: SpringCoefficients<Real>) -> &mut Self {
self.data.softness = softness;
self
}
}
impl From<PrismaticJoint> for GenericJoint {
@@ -275,13 +289,20 @@ impl PrismaticJointBuilder {
self
}
/// Sets the `[min,max]` limit distances attached bodies can translate along the joint’s principal axis.
/// Sets the `[min,max]` limit distances attached bodies can translate along the joint's principal axis.
#[must_use]
pub fn limits(mut self, limits: [Real; 2]) -> Self {
self.0.set_limits(limits);
self
}
/// Sets the softness of this joint’s locked degrees of freedom.
#[must_use]
pub fn softness(mut self, softness: SpringCoefficients<Real>) -> Self {
self.0.data.softness = softness;
self
}
/// Builds the prismatic joint.
#[must_use]
pub fn build(self) -> PrismaticJoint {

23
src/dynamics/joint/revolute_joint.rs
View File

@@ -1,3 +1,4 @@
use crate::dynamics::integration_parameters::SpringCoefficients;
use crate::dynamics::joint::{GenericJoint, GenericJointBuilder, JointAxesMask};
use crate::dynamics::{JointAxis, JointLimits, JointMotor, MotorModel};
use crate::math::{Point, Real, Rotation};
@@ -203,6 +204,19 @@ impl RevoluteJoint {
self.data.set_limits(JointAxis::AngX, limits);
self
}
/// Gets the softness of this joint’s locked degrees of freedom.
#[must_use]
pub fn softness(&self) -> SpringCoefficients<Real> {
self.data.softness
}
/// Sets the softness of this joint’s locked degrees of freedom.
#[must_use]
pub fn set_softness(&mut self, softness: SpringCoefficients<Real>) -> &mut Self {
self.data.softness = softness;
self
}
}
impl From<RevoluteJoint> for GenericJoint {
@@ -296,13 +310,20 @@ impl RevoluteJointBuilder {
self
}
/// Sets the `[min,max]` limit angles attached bodies can rotate along the joint’s principal axis.
/// Sets the `[min,max]` limit angles attached bodies can rotate along the joint's principal axis.
#[must_use]
pub fn limits(mut self, limits: [Real; 2]) -> Self {
self.0.set_limits(limits);
self
}
/// Sets the softness of this joint’s locked degrees of freedom.
#[must_use]
pub fn softness(mut self, softness: SpringCoefficients<Real>) -> Self {
self.0.data.softness = softness;
self
}
/// Builds the revolute joint.
#[must_use]
pub fn build(self) -> RevoluteJoint {

21
src/dynamics/joint/rope_joint.rs
View File

@@ -1,3 +1,4 @@
use crate::dynamics::integration_parameters::SpringCoefficients;
use crate::dynamics::joint::{GenericJoint, GenericJointBuilder, JointAxesMask};
use crate::dynamics::{JointAxis, MotorModel};
use crate::math::{Point, Real};
@@ -153,6 +154,19 @@ impl RopeJoint {
self.data.set_limits(JointAxis::LinX, [0.0, max_dist]);
self
}
/// Gets the softness of this joint’s locked degrees of freedom.
#[must_use]
pub fn softness(&self) -> SpringCoefficients<Real> {
self.data.softness
}
/// Sets the softness of this joint’s locked degrees of freedom.
#[must_use]
pub fn set_softness(&mut self, softness: SpringCoefficients<Real>) -> &mut Self {
self.data.softness = softness;
self
}
}
impl From<RopeJoint> for GenericJoint {
@@ -245,6 +259,13 @@ impl RopeJointBuilder {
self
}
/// Sets the softness of this joint’s locked degrees of freedom.
#[must_use]
pub fn softness(mut self, softness: SpringCoefficients<Real>) -> Self {
self.0.data.softness = softness;
self
}
/// Builds the rope joint.
#[must_use]
pub fn build(self) -> RopeJoint {

21
src/dynamics/joint/spherical_joint.rs
View File

@@ -1,3 +1,4 @@
use crate::dynamics::integration_parameters::SpringCoefficients;
use crate::dynamics::joint::{GenericJoint, GenericJointBuilder, JointAxesMask};
use crate::dynamics::{JointAxis, JointMotor, MotorModel};
use crate::math::{Isometry, Point, Real};
@@ -192,6 +193,19 @@ impl SphericalJoint {
self.data.set_limits(axis, limits);
self
}
/// Gets the softness of this joint’s locked degrees of freedom.
#[must_use]
pub fn softness(&self) -> SpringCoefficients<Real> {
self.data.softness
}
/// Sets the softness of this joint’s locked degrees of freedom.
#[must_use]
pub fn set_softness(&mut self, softness: SpringCoefficients<Real>) -> &mut Self {
self.data.softness = softness;
self
}
}
impl From<SphericalJoint> for GenericJoint {
@@ -305,6 +319,13 @@ impl SphericalJointBuilder {
self
}
/// Sets the softness of this joint’s locked degrees of freedom.
#[must_use]
pub fn softness(mut self, softness: SpringCoefficients<Real>) -> Self {
self.0.data.softness = softness;
self
}
/// Builds the spherical joint.
#[must_use]
pub fn build(self) -> SphericalJoint {

2
src/dynamics/mod.rs
View File

@@ -2,7 +2,7 @@
pub use self::ccd::CCDSolver;
pub use self::coefficient_combine_rule::CoefficientCombineRule;
pub use self::integration_parameters::IntegrationParameters;
pub use self::integration_parameters::{IntegrationParameters, SpringCoefficients};
pub use self::island_manager::IslandManager;
#[cfg(feature = "dim3")]

4
src/dynamics/solver/contact_constraint/contact_with_coulomb_friction.rs
View File

@@ -254,10 +254,10 @@ impl ContactWithCoulombFrictionBuilder {
_multibodies: &MultibodyJointSet,
constraint: &mut ContactWithCoulombFriction,
) {
let cfm_factor = SimdReal::splat(params.contact_cfm_factor());
let cfm_factor = SimdReal::splat(params.contact_softness.cfm_factor(params.dt));
let inv_dt = SimdReal::splat(params.inv_dt());
let allowed_lin_err = SimdReal::splat(params.allowed_linear_error());
let erp_inv_dt = SimdReal::splat(params.contact_erp_inv_dt());
let erp_inv_dt = SimdReal::splat(params.contact_softness.erp_inv_dt(params.dt));
let max_corrective_velocity = SimdReal::splat(params.max_corrective_velocity());
let warmstart_coeff = SimdReal::splat(params.warmstart_coefficient);

4
src/dynamics/solver/contact_constraint/contact_with_twist_friction.rs
View File

@@ -261,10 +261,10 @@ impl ContactWithTwistFrictionBuilder {
_multibodies: &MultibodyJointSet,
constraint: &mut ContactWithTwistFriction,
) {
let cfm_factor = SimdReal::splat(params.contact_cfm_factor());
let cfm_factor = SimdReal::splat(params.contact_softness.cfm_factor(params.dt));
let inv_dt = SimdReal::splat(params.inv_dt());
let allowed_lin_err = SimdReal::splat(params.allowed_linear_error());
let erp_inv_dt = SimdReal::splat(params.contact_erp_inv_dt());
let erp_inv_dt = SimdReal::splat(params.contact_softness.erp_inv_dt(params.dt));
let max_corrective_velocity = SimdReal::splat(params.max_corrective_velocity());
let warmstart_coeff = SimdReal::splat(params.warmstart_coefficient);

4
src/dynamics/solver/contact_constraint/generic_contact_constraint.rs
View File

@@ -346,9 +346,9 @@ impl GenericContactConstraintBuilder {
multibodies: &MultibodyJointSet,
constraint: &mut GenericContactConstraint,
) {
let cfm_factor = params.contact_cfm_factor();
let cfm_factor = params.contact_softness.cfm_factor(params.dt);
let inv_dt = params.inv_dt();
let erp_inv_dt = params.contact_erp_inv_dt();
let erp_inv_dt = params.contact_softness.erp_inv_dt(params.dt);
// We don’t update jacobians so the update is mostly identical to the non-generic velocity constraint.
let pose1 = multibodies

4
src/dynamics/solver/joint_constraint/generic_joint_constraint.rs
View File

@@ -140,6 +140,7 @@ impl GenericJointConstraint {
mb1,
mb2,
i - DIM,
joint.softness,
WritebackId::Dof(i),
);
len += 1;
@@ -157,6 +158,7 @@ impl GenericJointConstraint {
mb1,
mb2,
i,
joint.softness,
WritebackId::Dof(i),
);
len += 1;
@@ -176,6 +178,7 @@ impl GenericJointConstraint {
mb2,
i - DIM,
[joint.limits[i].min, joint.limits[i].max],
joint.softness,
WritebackId::Limit(i),
);
len += 1;
@@ -194,6 +197,7 @@ impl GenericJointConstraint {
mb2,
i,
[joint.limits[i].min, joint.limits[i].max],
joint.softness,
WritebackId::Limit(i),
);
len += 1;

14
src/dynamics/solver/joint_constraint/generic_joint_constraint_builder.rs
View File

@@ -11,6 +11,7 @@ use crate::utils;
use crate::utils::IndexMut2;
use na::{DVector, SVector};
use crate::dynamics::integration_parameters::SpringCoefficients;
use crate::dynamics::solver::ConstraintsCounts;
use crate::dynamics::solver::solver_body::SolverBodies;
#[cfg(feature = "dim3")]
@@ -406,6 +407,7 @@ impl JointConstraintHelper<Real> {
mb1: LinkOrBodyRef,
mb2: LinkOrBodyRef,
locked_axis: usize,
softness: SpringCoefficients<Real>,
writeback_id: WritebackId,
) -> GenericJointConstraint {
let lin_jac = self.basis.column(locked_axis).into_owned();
@@ -426,7 +428,7 @@ impl JointConstraintHelper<Real> {
ang_jac2,
);
let erp_inv_dt = params.joint_erp_inv_dt();
let erp_inv_dt = softness.erp_inv_dt(params.dt);
let rhs_bias = lin_jac.dot(&self.lin_err) * erp_inv_dt;
c.rhs += rhs_bias;
c
@@ -444,6 +446,7 @@ impl JointConstraintHelper<Real> {
mb2: LinkOrBodyRef,
limited_axis: usize,
limits: [Real; 2],
softness: SpringCoefficients<Real>,
writeback_id: WritebackId,
) -> GenericJointConstraint {
let lin_jac = self.basis.column(limited_axis).into_owned();
@@ -468,7 +471,8 @@ impl JointConstraintHelper<Real> {
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 = softness.erp_inv_dt(params.dt);
let rhs_bias = ((dist - limits[1]).max(0.0) - (limits[0] - dist).max(0.0)) * erp_inv_dt;
constraint.rhs += rhs_bias;
constraint.impulse_bounds = [
@@ -545,6 +549,7 @@ impl JointConstraintHelper<Real> {
mb1: LinkOrBodyRef,
mb2: LinkOrBodyRef,
_locked_axis: usize,
softness: SpringCoefficients<Real>,
writeback_id: WritebackId,
) -> GenericJointConstraint {
#[cfg(feature = "dim2")]
@@ -566,7 +571,7 @@ impl JointConstraintHelper<Real> {
ang_jac,
);
let erp_inv_dt = params.joint_erp_inv_dt();
let erp_inv_dt = softness.erp_inv_dt(params.dt);
#[cfg(feature = "dim2")]
let rhs_bias = self.ang_err.im * erp_inv_dt;
#[cfg(feature = "dim3")]
@@ -587,6 +592,7 @@ impl JointConstraintHelper<Real> {
mb2: LinkOrBodyRef,
_limited_axis: usize,
limits: [Real; 2],
softness: SpringCoefficients<Real>,
writeback_id: WritebackId,
) -> GenericJointConstraint {
#[cfg(feature = "dim2")]
@@ -620,7 +626,7 @@ impl JointConstraintHelper<Real> {
max_enabled as u32 as Real * Real::MAX,
];
let erp_inv_dt = params.joint_erp_inv_dt();
let erp_inv_dt = softness.erp_inv_dt(params.dt);
let rhs_bias =
((s_ang - s_limits[1]).max(0.0) - (s_limits[0] - s_ang).max(0.0)) * erp_inv_dt;

69
src/dynamics/solver/joint_constraint/joint_constraint_builder.rs
View File

@@ -15,7 +15,10 @@ use crate::utils::{SimdBasis, SimdQuat};
use na::SMatrix;
#[cfg(feature = "simd-is-enabled")]
use crate::math::{SIMD_WIDTH, SimdReal};
use {
crate::dynamics::SpringCoefficients,
crate::math::{SIMD_WIDTH, SimdReal},
};
pub struct JointConstraintBuilder {
body1: u32,
@@ -103,6 +106,7 @@ pub struct JointConstraintBuilderSimd {
local_frame1: Isometry<SimdReal>,
local_frame2: Isometry<SimdReal>,
locked_axes: u8,
softness: SpringCoefficients<SimdReal>,
constraint_id: usize,
}
@@ -149,6 +153,10 @@ impl JointConstraintBuilderSimd {
local_frame1,
local_frame2,
locked_axes: joint[0].data.locked_axes.bits(),
softness: SpringCoefficients {
natural_frequency: array![|ii| joint[ii].data.softness.natural_frequency].into(),
damping_ratio: array![|ii| joint[ii].data.softness.damping_ratio].into(),
},
constraint_id: *out_constraint_id,
};
@@ -191,6 +199,7 @@ impl JointConstraintBuilderSimd {
&frame1,
&frame2,
self.locked_axes,
self.softness,
&mut out[self.constraint_id..],
);
}
@@ -277,17 +286,25 @@ impl<N: SimdRealCopy> JointConstraintHelper<N> {
limited_axis: usize,
limits: [N; 2],
writeback_id: WritebackId,
erp_inv_dt: N,
cfm_coeff: N,
) -> JointConstraint<N, LANES> {
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,
erp_inv_dt,
cfm_coeff,
);
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 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;
@@ -309,6 +326,8 @@ impl<N: SimdRealCopy> JointConstraintHelper<N> {
coupled_axes: u8,
limits: [N; 2],
writeback_id: WritebackId,
erp_inv_dt: N,
cfm_coeff: N,
) -> JointConstraint<N, LANES> {
let zero = N::zero();
let mut lin_jac = Vector::zeros();
@@ -345,8 +364,6 @@ impl<N: SimdRealCopy> JointConstraintHelper<N> {
let ii_ang_jac1 = body1.ii * ang_jac1;
let ii_ang_jac2 = body2.ii * ang_jac2;
let erp_inv_dt = N::splat(params.joint_erp_inv_dt());
let cfm_coeff = N::splat(params.joint_cfm_coeff());
let rhs_bias = (dist - limits[1]).simd_max(zero) * erp_inv_dt;
let rhs = rhs_wo_bias + rhs_bias;
let impulse_bounds = [N::zero(), N::splat(Real::INFINITY)];
@@ -385,8 +402,18 @@ impl<N: SimdRealCopy> JointConstraintHelper<N> {
writeback_id: WritebackId,
) -> JointConstraint<N, LANES> {
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,
// Set regularization factors to zero.
// The motor impl. will overwrite them after.
N::zero(),
N::zero(),
);
let mut rhs_wo_bias = N::zero();
if motor_params.erp_inv_dt != N::zero() {
@@ -491,12 +518,14 @@ impl<N: SimdRealCopy> JointConstraintHelper<N> {
pub fn lock_linear<const LANES: usize>(
&self,
params: &IntegrationParameters,
_params: &IntegrationParameters,
joint_id: [JointIndex; LANES],
body1: &JointSolverBody<N, LANES>,
body2: &JointSolverBody<N, LANES>,
locked_axis: usize,
writeback_id: WritebackId,
erp_inv_dt: N,
cfm_coeff: N,
) -> JointConstraint<N, LANES> {
let lin_jac = self.basis.column(locked_axis).into_owned();
#[cfg(feature = "dim2")]
@@ -509,8 +538,6 @@ impl<N: SimdRealCopy> JointConstraintHelper<N> {
let 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 rhs_bias = lin_jac.dot(&self.lin_err) * erp_inv_dt;
let ii_ang_jac1 = body1.ii * ang_jac1;
@@ -540,13 +567,15 @@ impl<N: SimdRealCopy> JointConstraintHelper<N> {
pub fn limit_angular<const LANES: usize>(
&self,
params: &IntegrationParameters,
_params: &IntegrationParameters,
joint_id: [JointIndex; LANES],
body1: &JointSolverBody<N, LANES>,
body2: &JointSolverBody<N, LANES>,
_limited_axis: usize,
limits: [N; 2],
writeback_id: WritebackId,
erp_inv_dt: N,
cfm_coeff: N,
) -> JointConstraint<N, LANES> {
let zero = N::zero();
let half = N::splat(0.5);
@@ -568,8 +597,6 @@ impl<N: SimdRealCopy> JointConstraintHelper<N> {
#[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 rhs_bias = ((s_ang - s_limits[1]).simd_max(zero)
- (s_limits[0] - s_ang).simd_max(zero))
* erp_inv_dt;
@@ -663,12 +690,14 @@ impl<N: SimdRealCopy> JointConstraintHelper<N> {
pub fn lock_angular<const LANES: usize>(
&self,
params: &IntegrationParameters,
_params: &IntegrationParameters,
joint_id: [JointIndex; LANES],
body1: &JointSolverBody<N, LANES>,
body2: &JointSolverBody<N, LANES>,
_locked_axis: usize,
writeback_id: WritebackId,
erp_inv_dt: N,
cfm_coeff: N,
) -> JointConstraint<N, LANES> {
#[cfg(feature = "dim2")]
let ang_jac = N::one();
@@ -676,8 +705,6 @@ impl<N: SimdRealCopy> JointConstraintHelper<N> {
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());
#[cfg(feature = "dim2")]
let rhs_bias = self.ang_err.im * erp_inv_dt;
#[cfg(feature = "dim3")]
@@ -760,13 +787,15 @@ impl JointConstraintHelper<Real> {
#[cfg(feature = "dim3")]
pub fn limit_angular_coupled(
&self,
params: &IntegrationParameters,
_params: &IntegrationParameters,
joint_id: [JointIndex; 1],
body1: &JointSolverBody<Real, 1>,
body2: &JointSolverBody<Real, 1>,
coupled_axes: u8,
limits: [Real; 2],
writeback_id: WritebackId,
erp_inv_dt: Real,
cfm_coeff: Real,
) -> JointConstraint<Real, 1> {
// NOTE: right now, this only supports exactly 2 coupled axes.
let ang_coupled_axes = coupled_axes >> DIM;
@@ -791,8 +820,6 @@ impl JointConstraintHelper<Real> {
let rhs_wo_bias = 0.0;
let erp_inv_dt = params.joint_erp_inv_dt();
let cfm_coeff = params.joint_cfm_coeff();
let rhs_bias = ((angle - limits[1]).max(0.0) - (limits[0] - angle).max(0.0)) * erp_inv_dt;
let ii_ang_jac1 = body1.ii * ang_jac;

47
src/dynamics/solver/joint_constraint/joint_velocity_constraint.rs
View File

@@ -11,6 +11,8 @@ use crate::dynamics::solver::solver_body::SolverBodies;
use crate::math::{SIMD_WIDTH, SimdReal};
#[cfg(feature = "dim2")]
use crate::num::Zero;
#[cfg(feature = "simd-is-enabled")]
use na::SimdValue;
#[derive(Copy, Clone, PartialEq, Debug)]
pub struct MotorParameters<N: SimdRealCopy> {
@@ -142,6 +144,10 @@ impl JointConstraint<Real, 1> {
let limit_axes = joint.limit_axes.bits() & !locked_axes;
let coupled_axes = joint.coupled_axes.bits();
// Compute per-joint ERP and CFM coefficients
let erp_inv_dt = joint.softness.erp_inv_dt(params.dt);
let cfm_coeff = joint.softness.cfm_coeff(params.dt);
// 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 =
@@ -236,14 +242,24 @@ impl JointConstraint<Real, 1> {
body2,
i - DIM,
WritebackId::Dof(i),
erp_inv_dt,
cfm_coeff,
);
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),
erp_inv_dt,
cfm_coeff,
);
len += 1;
}
}
@@ -258,6 +274,8 @@ impl JointConstraint<Real, 1> {
i - DIM,
[joint.limits[i].min, joint.limits[i].max],
WritebackId::Limit(i),
erp_inv_dt,
cfm_coeff,
);
len += 1;
}
@@ -272,6 +290,8 @@ impl JointConstraint<Real, 1> {
i,
[joint.limits[i].min, joint.limits[i].max],
WritebackId::Limit(i),
erp_inv_dt,
cfm_coeff,
);
len += 1;
}
@@ -290,6 +310,8 @@ impl JointConstraint<Real, 1> {
joint.limits[first_coupled_ang_axis_id].max,
],
WritebackId::Limit(first_coupled_ang_axis_id),
erp_inv_dt,
cfm_coeff,
);
len += 1;
}
@@ -306,6 +328,8 @@ impl JointConstraint<Real, 1> {
joint.limits[first_coupled_lin_axis_id].max,
],
WritebackId::Limit(first_coupled_lin_axis_id),
erp_inv_dt,
cfm_coeff,
);
len += 1;
}
@@ -344,8 +368,13 @@ impl JointConstraint<SimdReal, SIMD_WIDTH> {
frame1: &Isometry<SimdReal>,
frame2: &Isometry<SimdReal>,
locked_axes: u8,
softness: crate::dynamics::SpringCoefficients<SimdReal>,
out: &mut [Self],
) -> usize {
let dt = SimdReal::splat(params.dt);
let erp_inv_dt = softness.erp_inv_dt(dt);
let cfm_coeff = softness.cfm_coeff(dt);
let builder = JointConstraintHelper::new(
frame1,
frame2,
@@ -357,8 +386,16 @@ impl JointConstraint<SimdReal, SIMD_WIDTH> {
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),
erp_inv_dt,
cfm_coeff,
);
len += 1;
}
}
@@ -372,6 +409,8 @@ impl JointConstraint<SimdReal, SIMD_WIDTH> {
body2,
i - DIM,
WritebackId::Dof(i),
erp_inv_dt,
cfm_coeff,
);
len += 1;
}