fabrilis/crates/modes/tests/joint_control.rs
loki5512344 190ef9cc4b Add design persistence, world rendering, metrics overlay and logs
M3: STL/OBJ import with mesh validation, mass/COM/inertia, material database, RON scene with content-addressed blobs and v0->v1 migration. M4: deterministic heightmap terrain, trimesh colliders, wgpu renderer with instancing and depth, winit app with 1 kHz physics, fly camera, on-screen metrics overlay and per-frame CSV/dev logs. Docs: UI, logging, network and headless CLI plans.
2026-11-16 23:40:39 +01:00

150 lines
3.9 KiB
Rust

//! M2 closed loop: a link on a revolute joint is lifted by a PD controller that reads
//! a delayed encoder instead of the ground-truth state.
#![allow(
clippy::unwrap_used,
clippy::expect_used,
reason = "test helper: a failure should abort the test"
)]
use fabrilis_core::snapshot::hash::StateHasher;
use fabrilis_core::time::SimDelta;
use fabrilis_core::units::{Kilograms, Radians};
use fabrilis_core::vec::{Quat, Vec3};
use fabrilis_physics::backend::{BodyDesc, PhysicsBackend, ShapeDesc};
use fabrilis_physics::joints::RevoluteDesc;
use fabrilis_physics::{BodyId, JointId, RapierBackend};
use fabrilis_sensors::encoder::{Encoder, EncoderConfig};
const KP: f64 = 400.0;
const KD: f64 = 25.0;
const TARGET: f64 = 0.0;
const STEPS: usize = 3_000;
const GOLDEN: u64 = 0x014e_e966_1ea1_d0ce;
struct Rig {
world: RapierBackend,
encoder: Encoder,
link: BodyId,
joint: JointId,
dt: SimDelta,
previous_raw: f64,
continuous: f64,
}
impl Rig {
fn new(start_degrees: f64) -> Self {
let mut world = RapierBackend::new();
let base = world.add_body(BodyDesc::fixed(
ShapeDesc::Cuboid {
half_extents: Vec3::new(0.05, 0.05, 0.05),
},
Vec3::ZERO,
));
let start = Quat::from_axis_angle(Vec3::Y, Radians::from_degrees(start_degrees))
.expect("the Y axis is non-zero");
let mut link_desc = BodyDesc::dynamic(
ShapeDesc::Cuboid {
half_extents: Vec3::new(0.5, 0.05, 0.05),
},
Kilograms::new(1.0),
start.rotate(Vec3::new(0.5, 0.0, 0.0)),
);
link_desc.rotation = start;
let link = world.add_body(link_desc);
let joint = world.add_revolute(RevoluteDesc::new(
base,
link,
Vec3::Y,
Vec3::ZERO,
Vec3::new(-0.5, 0.0, 0.0),
));
let mut encoder = Encoder::new(
EncoderConfig {
resolution_bits: 16,
noise_sigma: 0.0,
bias: 0.0,
delay_steps: 1,
},
1,
)
.expect("the encoder configuration is valid");
let dt = SimDelta::from_hz(1_000).expect("1 kHz divides a second");
let first_truth = world.joint_angle(joint).expect("the joint exists");
let _primed = encoder.sample(first_truth);
let previous_raw = encoder.sample(first_truth).get();
Self {
world,
encoder,
link,
joint,
dt,
previous_raw,
continuous: wrap_to_pi(previous_raw),
}
}
fn control_step(&mut self) -> Radians {
let truth = self
.world
.joint_angle(self.joint)
.expect("the joint exists");
let raw = self.encoder.sample(truth).get();
let delta = wrap_to_pi(raw - self.previous_raw);
self.continuous += delta;
self.previous_raw = raw;
let rate = delta / self.dt.as_seconds().get();
let torque = KP * (TARGET - self.continuous) - KD * rate;
self.world.apply_torque(self.link, Vec3::Y * torque);
self.world.step(self.dt);
self.world
.joint_angle(self.joint)
.expect("the joint exists")
}
fn state(&self) -> (f64, f64) {
let transform = self
.world
.body_transform(self.link)
.expect("the link exists");
(transform.position.z, transform.rotation.y)
}
}
fn run() -> (f64, f64, u64) {
let mut rig = Rig::new(60.0);
let mut hasher = StateHasher::new();
let mut final_angle = 0.0;
let mut tail_max: f64 = 0.0;
for step in 0..STEPS {
let angle = rig.control_step();
let (z, qy) = rig.state();
hasher.write_u64(step as u64);
hasher.write_f64(angle.get());
hasher.write_f64(z);
hasher.write_f64(qy);
final_angle = angle.get();
if step > 1_000 {
tail_max = tail_max.max((angle.get() - TARGET).abs());
}
}
(final_angle, tail_max, hasher.finish())
}
#[test]
fn pd_controller_lifts_the_link_to_the_target_angle() {
let (final_angle, tail_max, _) = run();
assert!(final_angle.abs() < 0.02, "final angle = {final_angle}");
assert!(tail_max < 0.02, "tail maximum error = {tail_max}");
}
#[test]
fn the_closed_loop_is_deterministic() {
let (_, _, hash) = run();
assert_eq!(hash, GOLDEN, "new hash: {hash:#018x}");
}
fn wrap_to_pi(angle: f64) -> f64 {
let two_pi = fabrilis_core::math::TAU;
(angle + core::f64::consts::PI).rem_euclid(two_pi) - core::f64::consts::PI
}