1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
//! Implicit joint springs on a multibody. A spring set on a multibody joint
//! applies a generalized force `-stiffness·(q − rest)` integrated implicitly
//! (its `dt²·k` term lands on the mass-matrix diagonal). These tests pin the
//! two defining properties: it drives the joint to its rest angle, and it does
//! so stably even on a low-inertia link where an explicit motor would diverge.
use rapier3d::prelude::*;
/// Fixed base + one dynamic link on a revolute multibody joint. Sets a spring
/// (rest angle `rest`, the given `stiffness`) on the joint plus a little joint
/// damping so it settles, then returns the link's final rotation about Z after
/// `steps` steps with gravity off.
fn settle_angle(stiffness: Real, rest: Real, damping: Real, inertia: Real, steps: usize) -> Real {
let mut bodies = RigidBodySet::new();
let mut colliders = ColliderSet::new();
let mut impulse_joints = ImpulseJointSet::new();
let mut multibody_joints = MultibodyJointSet::new();
let base = bodies.insert(RigidBodyBuilder::fixed());
// Link rotates in place about the shared origin (COM on the hinge axis), so
// the effective joint inertia is exactly `inertia` — no lever-arm term.
let link = bodies.insert(RigidBodyBuilder::dynamic().additional_mass_properties(
MassProperties::new(Vector::ZERO, 1.0, Vector::new(inertia, inertia, inertia)),
));
let rev = RevoluteJointBuilder::new(Vector::Z);
let handle = multibody_joints.insert(base, link, rev, true).unwrap();
let gravity = Vector::ZERO;
let integration_parameters = IntegrationParameters::default();
let mut pipeline = PhysicsPipeline::new();
let mut islands = IslandManager::new();
let mut broad_phase = DefaultBroadPhase::new();
let mut narrow_phase = NarrowPhase::new();
let mut ccd = CCDSolver::new();
let mut step = |bodies: &mut RigidBodySet, mbj: &mut MultibodyJointSet| {
pipeline.step(
gravity,
&integration_parameters,
&mut islands,
&mut broad_phase,
&mut narrow_phase,
bodies,
&mut colliders,
&mut impulse_joints,
mbj,
&mut ccd,
&(),
&(),
);
};
// Warm up one step so the fixed-base root collapses to 0 DoFs (it starts as
// a 6-DoF free root), leaving the single revolute DoF.
step(&mut bodies, &mut multibody_joints);
{
let (mb, link_id) = multibody_joints.get_mut(handle).unwrap();
let link_mut = mb.links_mut().nth(link_id).unwrap();
// The revolute's free axis is the angular X axis (index DIM = 3).
link_mut.joint.set_spring(3, stiffness, rest);
// A little joint-space damping so the (otherwise conservative) spring
// settles in a finite number of steps.
let n = mb.damping().len();
mb.damping_mut()[n - 1] = damping;
}
for _ in 0..steps {
step(&mut bodies, &mut multibody_joints);
}
bodies[link].rotation().to_scaled_axis().z
}
#[test]
fn implicit_spring_drives_joint_to_rest_angle() {
// Spring rest = +0.6 rad; the joint starts at 0. With ~critical damping
// (d ≈ 2·√(k·I) = 2·√(5·0.05) = 1.0) it settles at the rest angle.
let angle = settle_angle(5.0, 0.6, 1.0, 0.05, 400);
assert!(
(angle - 0.6).abs() < 0.05,
"spring did not settle at its rest angle: angle = {angle}"
);
}
#[test]
fn implicit_spring_is_stable_on_tiny_inertia() {
// The whole point of implicit integration: a stiff spring on a near-massless
// link (like robotiq's follower / flybody's leg segments) stays bounded
// where an explicit position motor diverges to thousands of rad/s.
let angle = settle_angle(50.0, 0.3, 0.01, 1.0e-6, 600);
assert!(angle.is_finite(), "spring blew up on tiny inertia: {angle}");
assert!(
(angle - 0.3).abs() < 0.1,
"spring on tiny inertia didn't reach rest: angle = {angle}"
);
}