rapier3d/dynamics/joint/multibody_joint/
unit_multibody_joint.rs

1#![allow(missing_docs)] // For downcast.
2
3use crate::dynamics::joint::MultibodyLink;
4use crate::dynamics::solver::{JointGenericOneBodyConstraint, WritebackId};
5use crate::dynamics::{IntegrationParameters, JointMotor, Multibody};
6use crate::math::Real;
7use na::DVector;
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<Real>,
20    constraints: &mut [JointGenericOneBodyConstraint],
21    insert_at: &mut usize,
22) {
23    let ndofs = multibody.ndofs();
24    let min_enabled = curr_pos < limits[0];
25    let max_enabled = limits[1] < curr_pos;
26    let erp_inv_dt = params.joint_erp_inv_dt();
27    let cfm_coeff = params.joint_cfm_coeff();
28    let rhs_bias = ((curr_pos - limits[1]).max(0.0) - (limits[0] - curr_pos).max(0.0)) * erp_inv_dt;
29    let rhs_wo_bias = 0.0;
30
31    let dof_j_id = *j_id + dof_id + link.assembly_id;
32    jacobians.rows_mut(*j_id, ndofs * 2).fill(0.0);
33    jacobians[dof_j_id] = 1.0;
34    jacobians[dof_j_id + ndofs] = 1.0;
35    multibody
36        .inv_augmented_mass()
37        .solve_mut(&mut jacobians.rows_mut(*j_id + ndofs, ndofs));
38
39    let lhs = jacobians[dof_j_id + ndofs]; // = J^t * M^-1 J
40    let impulse_bounds = [
41        min_enabled as u32 as Real * -Real::MAX,
42        max_enabled as u32 as Real * Real::MAX,
43    ];
44
45    let constraint = JointGenericOneBodyConstraint {
46        solver_vel2: multibody.solver_id,
47        ndofs2: ndofs,
48        j_id2: *j_id,
49        joint_id: usize::MAX,
50        impulse: 0.0,
51        impulse_bounds,
52        inv_lhs: crate::utils::inv(lhs),
53        rhs: rhs_wo_bias + rhs_bias,
54        rhs_wo_bias,
55        cfm_coeff,
56        cfm_gain: 0.0,
57        writeback_id: WritebackId::Limit(dof_id),
58    };
59
60    constraints[*insert_at] = constraint;
61    *insert_at += 1;
62
63    *j_id += 2 * ndofs;
64}
65
66/// Initializes and generate the velocity constraints applicable to the multibody links attached
67/// to this multibody_joint.
68pub fn unit_joint_motor_constraint(
69    params: &IntegrationParameters,
70    multibody: &Multibody,
71    link: &MultibodyLink,
72    motor: &JointMotor,
73    curr_pos: Real,
74    limits: Option<[Real; 2]>,
75    dof_id: usize,
76    j_id: &mut usize,
77    jacobians: &mut DVector<Real>,
78    constraints: &mut [JointGenericOneBodyConstraint],
79    insert_at: &mut usize,
80) {
81    let inv_dt = params.inv_dt();
82    let ndofs = multibody.ndofs();
83    let motor_params = motor.motor_params(params.dt);
84
85    let dof_j_id = *j_id + dof_id + link.assembly_id;
86    jacobians.rows_mut(*j_id, ndofs * 2).fill(0.0);
87    jacobians[dof_j_id] = 1.0;
88    jacobians[dof_j_id + ndofs] = 1.0;
89    multibody
90        .inv_augmented_mass()
91        .solve_mut(&mut jacobians.rows_mut(*j_id + ndofs, ndofs));
92
93    let lhs = jacobians[dof_j_id + ndofs]; // = J^t * M^-1 J
94    let impulse_bounds = [-motor_params.max_impulse, motor_params.max_impulse];
95
96    let mut rhs_wo_bias = 0.0;
97    if motor_params.erp_inv_dt != 0.0 {
98        rhs_wo_bias += (curr_pos - motor_params.target_pos) * motor_params.erp_inv_dt;
99    }
100
101    let mut target_vel = motor_params.target_vel;
102    if let Some(limits) = limits {
103        target_vel = target_vel.clamp(
104            (limits[0] - curr_pos) * inv_dt,
105            (limits[1] - curr_pos) * inv_dt,
106        );
107    };
108
109    rhs_wo_bias += -target_vel;
110
111    let constraint = JointGenericOneBodyConstraint {
112        solver_vel2: multibody.solver_id,
113        ndofs2: ndofs,
114        j_id2: *j_id,
115        joint_id: usize::MAX,
116        impulse: 0.0,
117        impulse_bounds,
118        cfm_coeff: motor_params.cfm_coeff,
119        cfm_gain: motor_params.cfm_gain,
120        inv_lhs: crate::utils::inv(lhs),
121        rhs: rhs_wo_bias,
122        rhs_wo_bias,
123        writeback_id: WritebackId::Limit(dof_id),
124    };
125
126    constraints[*insert_at] = constraint;
127    *insert_at += 1;
128
129    *j_id += 2 * ndofs;
130}
rapier3d/dynamics/joint/multibody_joint/unit_multibody_joint.rs

rapier3d/dynamics/joint/multibody_joint/
unit_multibody_joint.rs
