From 8b20175b6e599ca1f791bd0386b29f96a92c85b7 Mon Sep 17 00:00:00 2001 From: Avi Weinstock Date: Sat, 29 May 2021 14:45:46 -0400 Subject: [PATCH] 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!(