rapier2d/dynamics/joint/multibody_joint/
unit_multibody_joint.rs

1#![allow(missing_docs)] // For downcast.
2
3use crate::dynamics::integration_parameters::SpringCoefficients;
4use crate::dynamics::joint::MultibodyLink;
5use crate::dynamics::solver::{GenericJointConstraint, WritebackId};
6use crate::dynamics::{IntegrationParameters, JointMotor, Multibody};
7use crate::math::{DVector, Real};
8
9/// Initializes and generate the velocity constraints applicable to the multibody links attached
10/// to this multibody_joint.
11pub fn unit_joint_limit_constraint(
12    params: &IntegrationParameters,
13    multibody: &Multibody,
14    link: &MultibodyLink,
15    limits: [Real; 2],
16    curr_pos: Real,
17    dof_id: usize,
18    j_id: &mut usize,
19    jacobians: &mut DVector,
20    constraints: &mut [GenericJointConstraint],
21    insert_at: &mut usize,
22    softness: SpringCoefficients<Real>,
23) {
24    let ndofs = multibody.ndofs();
25    let min_enabled = curr_pos < limits[0];
26    let max_enabled = limits[1] < curr_pos;
27
28    // Compute per-joint ERP and CFM
29    let erp_inv_dt = softness.erp_inv_dt(params.dt);
30    let cfm_coeff = softness.cfm_coeff(params.dt);
31
32    let rhs_bias = ((curr_pos - limits[1]).max(0.0) - (limits[0] - curr_pos).max(0.0)) * erp_inv_dt;
33    let rhs_wo_bias = 0.0;
34
35    let dof_j_id = *j_id + dof_id + link.assembly_id;
36    jacobians.rows_mut(*j_id, ndofs * 2).fill(0.0);
37    jacobians[dof_j_id] = 1.0;
38    jacobians[dof_j_id + ndofs] = 1.0;
39    multibody
40        .inv_augmented_mass()
41        .solve_mut(&mut jacobians.rows_mut(*j_id + ndofs, ndofs));
42
43    let lhs = jacobians[dof_j_id + ndofs]; // = J^t * M^-1 J
44    let impulse_bounds = [
45        min_enabled as u32 as Real * -Real::MAX,
46        max_enabled as u32 as Real * Real::MAX,
47    ];
48
49    let constraint = GenericJointConstraint {
50        is_rigid_body1: false,
51        solver_vel1: u32::MAX,
52        ndofs1: 0,
53        j_id1: 0,
54
55        is_rigid_body2: false,
56        solver_vel2: multibody.solver_id,
57        ndofs2: ndofs,
58        j_id2: *j_id,
59        joint_id: usize::MAX, // TODO: we don’t support impulse writeback for internal constraints yet.
60        impulse: 0.0,
61        impulse_bounds,
62        inv_lhs: crate::utils::inv(lhs),
63        rhs: rhs_wo_bias + rhs_bias,
64        rhs_wo_bias,
65        cfm_coeff,
66        cfm_gain: 0.0,
67        writeback_id: WritebackId::Limit(dof_id),
68    };
69
70    constraints[*insert_at] = constraint;
71    *insert_at += 1;
72
73    *j_id += 2 * ndofs;
74}
75
76/// Initializes and generate the velocity constraints applicable to the multibody links attached
77/// to this multibody_joint.
78pub fn unit_joint_motor_constraint(
79    params: &IntegrationParameters,
80    multibody: &Multibody,
81    link: &MultibodyLink,
82    motor: &JointMotor,
83    curr_pos: Real,
84    limits: Option<[Real; 2]>,
85    dof_id: usize,
86    j_id: &mut usize,
87    jacobians: &mut DVector,
88    constraints: &mut [GenericJointConstraint],
89    insert_at: &mut usize,
90) {
91    let inv_dt = params.inv_dt();
92    let ndofs = multibody.ndofs();
93    let motor_params = motor.motor_params(params.dt);
94
95    let dof_j_id = *j_id + dof_id + link.assembly_id;
96    jacobians.rows_mut(*j_id, ndofs * 2).fill(0.0);
97    jacobians[dof_j_id] = 1.0;
98    jacobians[dof_j_id + ndofs] = 1.0;
99    multibody
100        .inv_augmented_mass()
101        .solve_mut(&mut jacobians.rows_mut(*j_id + ndofs, ndofs));
102
103    let lhs = jacobians[dof_j_id + ndofs]; // = J^t * M^-1 J
104    let impulse_bounds = [-motor_params.max_impulse, motor_params.max_impulse];
105
106    let mut rhs_wo_bias = 0.0;
107    if motor_params.erp_inv_dt != 0.0 {
108        rhs_wo_bias += (curr_pos - motor_params.target_pos) * motor_params.erp_inv_dt;
109    }
110
111    let mut target_vel = motor_params.target_vel;
112    if let Some(limits) = limits {
113        target_vel = target_vel.clamp(
114            (limits[0] - curr_pos) * inv_dt,
115            (limits[1] - curr_pos) * inv_dt,
116        );
117    };
118
119    rhs_wo_bias += -target_vel;
120
121    let constraint = GenericJointConstraint {
122        is_rigid_body1: false,
123        solver_vel1: u32::MAX,
124        ndofs1: 0,
125        j_id1: 0,
126
127        is_rigid_body2: false,
128        solver_vel2: multibody.solver_id,
129        ndofs2: ndofs,
130        j_id2: *j_id,
131        joint_id: usize::MAX, // TODO: we don’t support impulse writeback for internal constraints yet.
132        impulse: 0.0,
133        impulse_bounds,
134        cfm_coeff: motor_params.cfm_coeff,
135        cfm_gain: motor_params.cfm_gain,
136        inv_lhs: crate::utils::inv(lhs),
137        rhs: rhs_wo_bias,
138        rhs_wo_bias,
139        writeback_id: WritebackId::Limit(dof_id),
140    };
141
142    constraints[*insert_at] = constraint;
143    *insert_at += 1;
144
145    *j_id += 2 * ndofs;
146}
rapier2d/dynamics/joint/multibody_joint/unit_multibody_joint.rs

rapier2d/dynamics/joint/multibody_joint/
unit_multibody_joint.rs
