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

rapier2d/dynamics/joint/multibody_joint/
unit_multibody_joint.rs
