//! 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 }