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.
150 lines
3.9 KiB
Rust
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
|
|
}
|