From be35da9e57f12f9362370f019c6596e418285a57 Mon Sep 17 00:00:00 2001 From: Avi Weinstock Date: Sat, 29 May 2021 14:45:46 -0400 Subject: [PATCH 1/3] Implement PID controllers and use them to stabilize Agent airship flight. --- CHANGELOG.md | 1 + common/src/comp/agent.rs | 93 +++++++++++++++++++++++++++- common/src/comp/body/ship.rs | 7 +++ common/src/comp/mod.rs | 2 +- server/src/cmd.rs | 17 ++++- server/src/events/entity_creation.rs | 10 ++- server/src/sys/agent.rs | 21 +++++-- 7 files changed, 138 insertions(+), 13 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index e80e286d45..3f620a930d 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -143,6 +143,7 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0 - Fixed terrain clipping with glider - Fixed an issue where prices weren't properly making their way from econsim to the actual trade values. - Fixed entities with voxel colliders being off by one physics tick for collision. +- Airships no longer oscillate dramatically into the sky due to mistaking velocity for acceleration. ## [0.9.0] - 2021-03-20 diff --git a/common/src/comp/agent.rs b/common/src/comp/agent.rs index 0bb02f01ec..7b780a80e1 100644 --- a/common/src/comp/agent.rs +++ b/common/src/comp/agent.rs @@ -7,7 +7,7 @@ use crate::{ }; use specs::{Component, Entity as EcsEntity}; use specs_idvs::IdvStorage; -use std::collections::VecDeque; +use std::{collections::VecDeque, fmt}; use vek::*; use super::dialogue::Subject; @@ -300,6 +300,7 @@ pub struct Target { pub selected_at: f64, } +#[allow(clippy::type_complexity)] #[derive(Clone, Debug, Default)] pub struct Agent { pub rtsim_controller: RtSimController, @@ -313,6 +314,7 @@ pub struct Agent { pub bearing: Vec2, pub sounds_heard: Vec, pub awareness: f32, + pub pid_controller: Option, Vec3) -> f32, 16>>, } #[derive(Clone, Debug, Default)] @@ -336,6 +338,15 @@ impl Agent { self } + #[allow(clippy::type_complexity)] + pub fn with_pid_controller( + mut self, + pid: PidController, Vec3) -> f32, 16>, + ) -> Self { + self.pid_controller = Some(pid); + self + } + pub fn new( patrol_origin: Option>, body: &Body, @@ -385,3 +396,83 @@ mod tests { assert!(b.can(BehaviorCapability::SPEAK)); } } + +#[derive(Clone)] +pub struct PidController, Vec3) -> f32, const NUM_SAMPLES: usize> { + pub kp: f32, + pub ki: f32, + pub kd: f32, + pub sp: Vec3, + pv_samples: [(f64, Vec3); NUM_SAMPLES], + pv_idx: usize, + e: F, +} + +impl, Vec3) -> f32, const NUM_SAMPLES: usize> fmt::Debug + for PidController +{ + fn fmt(&self, f: &mut fmt::Formatter) -> fmt::Result { + f.debug_struct("PidController") + .field("kp", &self.kp) + .field("ki", &self.ki) + .field("kd", &self.kd) + .field("sp", &self.sp) + .field("pv_samples", &self.pv_samples) + .field("pv_idx", &self.pv_idx) + .finish() + } +} + +impl, Vec3) -> f32, const NUM_SAMPLES: usize> PidController { + pub fn new(kp: f32, ki: f32, kd: f32, sp: Vec3, time: f64, e: F) -> Self { + Self { + kp, + ki, + kd, + sp, + pv_samples: [(time, Vec3::zero()); NUM_SAMPLES], + pv_idx: 0, + e, + } + } + + pub fn add_measurement(&mut self, time: f64, pv: Vec3) { + self.pv_idx += 1; + self.pv_idx %= NUM_SAMPLES; + self.pv_samples[self.pv_idx] = (time, pv); + } + + /// https://en.wikipedia.org/wiki/PID_controller#Mathematical_form + pub fn calc_err(&self) -> f32 { + self.kp * self.proportional_err() + + self.ki * self.integral_err() + + self.kd * self.derivative_err() + } + + pub fn proportional_err(&self) -> f32 { (self.e)(self.sp, self.pv_samples[self.pv_idx].1) } + + /// https://en.wikipedia.org/wiki/Trapezoidal_rule#Uniform_grid + pub fn integral_err(&self) -> f32 { + let f = |x| (self.e)(self.sp, x); + let (a, x0) = self.pv_samples[(self.pv_idx + 1) % NUM_SAMPLES]; + let (b, xn) = self.pv_samples[self.pv_idx]; + let dx = (b - a) / NUM_SAMPLES as f64; + let mut err = 0.0; + for i in 1..=NUM_SAMPLES - 1 { + let xk = self.pv_samples[(self.pv_idx + 1 + i) % NUM_SAMPLES].1; + err += f(xk); + } + err += (f(xn) - f(x0)) / 2.0; + err *= dx as f32; + err + } + + /// https://en.wikipedia.org/wiki/Numerical_differentiation#Finite_differences + pub fn derivative_err(&self) -> f32 { + let f = |x| (self.e)(self.sp, x); + let (a, x0) = self.pv_samples[(self.pv_idx + NUM_SAMPLES - 1) % NUM_SAMPLES]; + let (b, x1) = self.pv_samples[self.pv_idx]; + let h = b - a; + (f(x1) - f(x0)) / h as f32 + } +} diff --git a/common/src/comp/body/ship.rs b/common/src/comp/body/ship.rs index f880683d47..ccedd285b5 100644 --- a/common/src/comp/body/ship.rs +++ b/common/src/comp/body/ship.rs @@ -52,6 +52,13 @@ impl Body { pub fn density(&self) -> Density { Density(AIR_DENSITY) } pub fn mass(&self) -> Mass { Mass((self.hull_vol() + self.balloon_vol()) * self.density().0) } + + pub fn pid_coefficients(&self) -> (f32, f32, f32) { + let kp = 1.0; + let ki = 1.0; + let kd = 1.0; + (kp, ki, kd) + } } /// Terrain is 11.0 scale relative to small-scale voxels, diff --git a/common/src/comp/mod.rs b/common/src/comp/mod.rs index bde3d3c3ee..f120798be7 100644 --- a/common/src/comp/mod.rs +++ b/common/src/comp/mod.rs @@ -46,7 +46,7 @@ pub mod visual; pub use self::{ ability::{CharacterAbility, CharacterAbilityType}, admin::{Admin, AdminRole}, - agent::{Agent, Alignment, Behavior, BehaviorCapability, BehaviorState}, + agent::{Agent, Alignment, Behavior, BehaviorCapability, BehaviorState, PidController}, aura::{Aura, AuraChange, AuraKind, Auras}, beam::{Beam, BeamSegment}, body::{ diff --git a/server/src/cmd.rs b/server/src/cmd.rs index 1338707477..b93550d1fe 100644 --- a/server/src/cmd.rs +++ b/server/src/cmd.rs @@ -1152,9 +1152,10 @@ fn handle_spawn_airship( 200.0, ) }); + let ship = comp::ship::Body::DefaultAirship; let mut builder = server .state - .create_ship(pos, comp::ship::Body::DefaultAirship, true) + .create_ship(pos, ship, true) .with(LightEmitter { col: Rgb::new(1.0, 0.65, 0.2), strength: 2.0, @@ -1162,7 +1163,19 @@ fn handle_spawn_airship( animated: true, }); if let Some(pos) = destination { - builder = builder.with(comp::Agent::default().with_destination(pos)) + let (kp, ki, kd) = ship.pid_coefficients(); + fn pure_z(sp: Vec3, pv: Vec3) -> f32 { (sp - pv).z } + let agent = comp::Agent::default() + .with_destination(pos) + .with_pid_controller(comp::PidController::new( + kp, + ki, + kd, + Vec3::zero(), + 0.0, + pure_z, + )); + builder = builder.with(agent); } builder.build(); diff --git a/server/src/events/entity_creation.rs b/server/src/events/entity_creation.rs index 1a59e6096c..1e659477e7 100644 --- a/server/src/events/entity_creation.rs +++ b/server/src/events/entity_creation.rs @@ -8,8 +8,8 @@ use common::{ buff::{BuffCategory, BuffData, BuffKind, BuffSource}, inventory::loadout::Loadout, shockwave, Agent, Alignment, Body, Health, HomeChunk, Inventory, Item, ItemDrop, - LightEmitter, Object, Ori, Poise, Pos, Projectile, Scale, SkillSet, Stats, Vel, - WaypointArea, + LightEmitter, Object, Ori, PidController, Poise, Pos, Projectile, Scale, SkillSet, Stats, + Vel, WaypointArea, }, outcome::Outcome, rtsim::RtSimEntity, @@ -146,7 +146,11 @@ pub fn handle_create_ship( rtsim_entity: Option, ) { let mut entity = server.state.create_ship(pos, ship, mountable); - if let Some(agent) = agent { + if let Some(mut agent) = agent { + let (kp, ki, kd) = ship.pid_coefficients(); + fn pure_z(sp: Vec3, pv: Vec3) -> f32 { (sp - pv).z } + agent = + agent.with_pid_controller(PidController::new(kp, ki, kd, Vec3::zero(), 0.0, pure_z)); entity = entity.with(agent); } if let Some(rtsim_entity) = rtsim_entity { diff --git a/server/src/sys/agent.rs b/server/src/sys/agent.rs index fa01525b93..1b407e4ee6 100644 --- a/server/src/sys/agent.rs +++ b/server/src/sys/agent.rs @@ -271,6 +271,10 @@ impl<'a> System<'a> for Sys { Some(CharacterState::GlideWield) | Some(CharacterState::Glide(_)) ) && !physics_state.on_ground; + if let Some(pid) = agent.pid_controller.as_mut() { + pid.add_measurement(read_data.time.0, pos.0); + } + // This controls how picky NPCs are about their pathfinding. Giants are larger // and so can afford to be less precise when trying to move around // the world (especially since they would otherwise get stuck on @@ -800,7 +804,7 @@ impl<'a> AgentData<'a> { controller.inputs.climb = Some(comp::Climb::Up); //.filter(|_| bearing.z > 0.1 || self.physics_state.in_liquid().is_some()); - controller.inputs.move_z = bearing.z + let height_offset = bearing.z + if self.traversal_config.can_fly { // NOTE: costs 4 us (imbris) let obstacle_ahead = read_data @@ -820,14 +824,14 @@ impl<'a> AgentData<'a> { .body .map(|body| { #[cfg(feature = "worldgen")] - let height_approx = self.pos.0.y + let height_approx = self.pos.0.z - read_data .world .sim() .get_alt_approx(self.pos.0.xy().map(|x: f32| x as i32)) .unwrap_or(0.0); #[cfg(not(feature = "worldgen"))] - let height_approx = self.pos.0.y; + let height_approx = self.pos.0.z; height_approx < body.flying_height() }) @@ -859,14 +863,19 @@ impl<'a> AgentData<'a> { } if obstacle_ahead || ground_too_close { - 1.0 //fly up when approaching obstacles + 5.0 //fly up when approaching obstacles } else { - -0.1 + -2.0 } //flying things should slowly come down from the stratosphere } else { 0.05 //normal land traveller offset }; - + if let Some(pid) = agent.pid_controller.as_mut() { + pid.sp = self.pos.0.z + height_offset * Vec3::unit_z(); + controller.inputs.move_z = pid.calc_err(); + } else { + controller.inputs.move_z = height_offset; + } // Put away weapon if thread_rng().gen_bool(0.1) && matches!( From b2f88af26e8b6adbd33e389f05efad2bb2c0b3db Mon Sep 17 00:00:00 2001 From: Avi Weinstock Date: Sat, 29 May 2021 22:50:09 -0400 Subject: [PATCH 2/3] Add comments to the PID controller code. --- common/src/comp/agent.rs | 31 +++++++++++++++++++++++++++++-- 1 file changed, 29 insertions(+), 2 deletions(-) diff --git a/common/src/comp/agent.rs b/common/src/comp/agent.rs index 7b780a80e1..4b0a9150dd 100644 --- a/common/src/comp/agent.rs +++ b/common/src/comp/agent.rs @@ -397,14 +397,24 @@ mod tests { } } +/// PID controllers are used for automatically adapting nonlinear controls (like +/// buoyancy for airships) to target specific outcomes (i.e. a specific height) #[derive(Clone)] pub struct PidController, Vec3) -> f32, const NUM_SAMPLES: usize> { + /// The coefficient of the proportional term pub kp: f32, + /// The coefficient of the integral term pub ki: f32, + /// The coefficient of the derivative term pub kd: f32, + /// The setpoint that the process has as its goal pub sp: Vec3, + /// A ring buffer of the last NUM_SAMPLES measured process variables pv_samples: [(f64, Vec3); NUM_SAMPLES], + /// The index into the ring buffer of process variables pv_idx: usize, + /// The error function, to change how the difference between the setpoint + /// and process variables are calculated e: F, } @@ -424,6 +434,8 @@ impl, Vec3) -> f32, const NUM_SAMPLES: usize> fmt::Debug } impl, Vec3) -> f32, const NUM_SAMPLES: usize> PidController { + /// Constructs a PidController with the specified weights, setpoint, + /// starting time, and error function pub fn new(kp: f32, ki: f32, kd: f32, sp: Vec3, time: f64, e: F) -> Self { Self { kp, @@ -436,12 +448,15 @@ impl, Vec3) -> f32, const NUM_SAMPLES: usize> PidController } } + /// Adds a measurement of the process variable to the ringbuffer pub fn add_measurement(&mut self, time: f64, pv: Vec3) { self.pv_idx += 1; self.pv_idx %= NUM_SAMPLES; self.pv_samples[self.pv_idx] = (time, pv); } + /// The amount to set the control variable to is a weighed sum of the + /// proportional error, the integral error, and the derivative error. /// https://en.wikipedia.org/wiki/PID_controller#Mathematical_form pub fn calc_err(&self) -> f32 { self.kp * self.proportional_err() @@ -449,8 +464,13 @@ impl, Vec3) -> f32, const NUM_SAMPLES: usize> PidController + self.kd * self.derivative_err() } + /// The proportional error is the error function applied to the set point + /// and the most recent process variable measurement pub fn proportional_err(&self) -> f32 { (self.e)(self.sp, self.pv_samples[self.pv_idx].1) } + /// The integral error is the error function integrated over the last + /// NUM_SAMPLES values. The trapezoid rule for numerical integration was + /// chosen because it's fairly easy to calculate and sufficiently accurate. /// https://en.wikipedia.org/wiki/Trapezoidal_rule#Uniform_grid pub fn integral_err(&self) -> f32 { let f = |x| (self.e)(self.sp, x); @@ -458,15 +478,22 @@ impl, Vec3) -> f32, const NUM_SAMPLES: usize> PidController let (b, xn) = self.pv_samples[self.pv_idx]; let dx = (b - a) / NUM_SAMPLES as f64; let mut err = 0.0; - for i in 1..=NUM_SAMPLES - 1 { - let xk = self.pv_samples[(self.pv_idx + 1 + i) % NUM_SAMPLES].1; + // \Sigma_{k=1}^{N-1} f(x_k) + for k in 1..=NUM_SAMPLES - 1 { + let xk = self.pv_samples[(self.pv_idx + 1 + k) % NUM_SAMPLES].1; err += f(xk); } + // (\Sigma_{k=1}^{N-1} f(x_k)) + \frac{f(x_N) + f(x_0)}{2} err += (f(xn) - f(x0)) / 2.0; + // \Delta x * ((\Sigma_{k=1}^{N-1} f(x_k)) + \frac{f(x_N) + f(x_0)}{2}) err *= dx as f32; err } + /// The derivative error is the numerical derivative of the error function + /// based on the most recent 2 samples. Using more than 2 samples might + /// improve the accuracy of the estimate of the derivative, but it would be + /// an estimate of the derivative error further in the past. /// https://en.wikipedia.org/wiki/Numerical_differentiation#Finite_differences pub fn derivative_err(&self) -> f32 { let f = |x| (self.e)(self.sp, x); From 7aac77288b9b07dc3923b16e2a5d0cf97f42d0be Mon Sep 17 00:00:00 2001 From: Avi Weinstock Date: Sun, 30 May 2021 11:38:47 -0400 Subject: [PATCH 3/3] Address MR 2356 comments. --- common/src/comp/agent.rs | 23 +++++++++++++++++++---- common/src/comp/body/ship.rs | 7 ------- server/src/cmd.rs | 4 ++-- server/src/events/entity_creation.rs | 13 ++++++++++--- server/src/sys/agent.rs | 4 ++-- 5 files changed, 33 insertions(+), 18 deletions(-) diff --git a/common/src/comp/agent.rs b/common/src/comp/agent.rs index 4b0a9150dd..3fade71b8c 100644 --- a/common/src/comp/agent.rs +++ b/common/src/comp/agent.rs @@ -1,5 +1,5 @@ use crate::{ - comp::{humanoid, quadruped_low, quadruped_medium, quadruped_small, Body}, + comp::{humanoid, quadruped_low, quadruped_medium, quadruped_small, ship, Body}, path::Chaser, rtsim::RtSimController, trade::{PendingTrade, ReducedInventory, SiteId, SitePrices, TradeId, TradeResult}, @@ -314,7 +314,7 @@ pub struct Agent { pub bearing: Vec2, pub sounds_heard: Vec, pub awareness: f32, - pub pid_controller: Option, Vec3) -> f32, 16>>, + pub position_pid_controller: Option, Vec3) -> f32, 16>>, } #[derive(Clone, Debug, Default)] @@ -339,11 +339,11 @@ impl Agent { } #[allow(clippy::type_complexity)] - pub fn with_pid_controller( + pub fn with_position_pid_controller( mut self, pid: PidController, Vec3) -> f32, 16>, ) -> Self { - self.pid_controller = Some(pid); + self.position_pid_controller = Some(pid); self } @@ -503,3 +503,18 @@ impl, Vec3) -> f32, const NUM_SAMPLES: usize> PidController (f(x1) - f(x0)) / h as f32 } } + +/// Get the PID coefficients associated with some Body, since it will likely +/// need to be tuned differently for each body type +pub fn pid_coefficients(body: &Body) -> (f32, f32, f32) { + match body { + Body::Ship(ship::Body::DefaultAirship) => { + let kp = 1.0; + let ki = 1.0; + let kd = 1.0; + (kp, ki, kd) + }, + // default to a pure-proportional controller, which is the first step when tuning + _ => (1.0, 0.0, 0.0), + } +} diff --git a/common/src/comp/body/ship.rs b/common/src/comp/body/ship.rs index ccedd285b5..f880683d47 100644 --- a/common/src/comp/body/ship.rs +++ b/common/src/comp/body/ship.rs @@ -52,13 +52,6 @@ impl Body { pub fn density(&self) -> Density { Density(AIR_DENSITY) } pub fn mass(&self) -> Mass { Mass((self.hull_vol() + self.balloon_vol()) * self.density().0) } - - pub fn pid_coefficients(&self) -> (f32, f32, f32) { - let kp = 1.0; - let ki = 1.0; - let kd = 1.0; - (kp, ki, kd) - } } /// Terrain is 11.0 scale relative to small-scale voxels, diff --git a/server/src/cmd.rs b/server/src/cmd.rs index b93550d1fe..b350359a88 100644 --- a/server/src/cmd.rs +++ b/server/src/cmd.rs @@ -1163,11 +1163,11 @@ fn handle_spawn_airship( animated: true, }); if let Some(pos) = destination { - let (kp, ki, kd) = ship.pid_coefficients(); + let (kp, ki, kd) = comp::agent::pid_coefficients(&comp::Body::Ship(ship)); fn pure_z(sp: Vec3, pv: Vec3) -> f32 { (sp - pv).z } let agent = comp::Agent::default() .with_destination(pos) - .with_pid_controller(comp::PidController::new( + .with_position_pid_controller(comp::PidController::new( kp, ki, kd, diff --git a/server/src/events/entity_creation.rs b/server/src/events/entity_creation.rs index 1e659477e7..c66317cf0e 100644 --- a/server/src/events/entity_creation.rs +++ b/server/src/events/entity_creation.rs @@ -3,6 +3,7 @@ use common::{ character::CharacterId, comp::{ self, + agent::pid_coefficients, aura::{Aura, AuraKind, AuraTarget}, beam, buff::{BuffCategory, BuffData, BuffKind, BuffSource}, @@ -147,10 +148,16 @@ pub fn handle_create_ship( ) { let mut entity = server.state.create_ship(pos, ship, mountable); if let Some(mut agent) = agent { - let (kp, ki, kd) = ship.pid_coefficients(); + let (kp, ki, kd) = pid_coefficients(&Body::Ship(ship)); fn pure_z(sp: Vec3, pv: Vec3) -> f32 { (sp - pv).z } - agent = - agent.with_pid_controller(PidController::new(kp, ki, kd, Vec3::zero(), 0.0, pure_z)); + agent = agent.with_position_pid_controller(PidController::new( + kp, + ki, + kd, + Vec3::zero(), + 0.0, + pure_z, + )); entity = entity.with(agent); } if let Some(rtsim_entity) = rtsim_entity { diff --git a/server/src/sys/agent.rs b/server/src/sys/agent.rs index 1b407e4ee6..7f191c6405 100644 --- a/server/src/sys/agent.rs +++ b/server/src/sys/agent.rs @@ -271,7 +271,7 @@ impl<'a> System<'a> for Sys { Some(CharacterState::GlideWield) | Some(CharacterState::Glide(_)) ) && !physics_state.on_ground; - if let Some(pid) = agent.pid_controller.as_mut() { + if let Some(pid) = agent.position_pid_controller.as_mut() { pid.add_measurement(read_data.time.0, pos.0); } @@ -870,7 +870,7 @@ impl<'a> AgentData<'a> { } else { 0.05 //normal land traveller offset }; - if let Some(pid) = agent.pid_controller.as_mut() { + if let Some(pid) = agent.position_pid_controller.as_mut() { pid.sp = self.pos.0.z + height_offset * Vec3::unit_z(); controller.inputs.move_z = pid.calc_err(); } else {