diff --git a/crates/lyapunov-demo/src/main.rs b/crates/lyapunov-demo/src/main.rs index 9674ae3..118dfb2 100644 --- a/crates/lyapunov-demo/src/main.rs +++ b/crates/lyapunov-demo/src/main.rs @@ -1,18 +1,30 @@ -use lyapunov::{Controller, Integrator, LinearFeedback, LinearSystem, Rk4}; -use nalgebra::SVector; -use uom::si::f64::Time; +use lyapunov::inputs::AccelerationInput; +use lyapunov::states::PositionVelocityState; +use lyapunov::{Controller, DoubleIntegrator, Integrator, LinearFeedback, Rk4}; +use uom::si::acceleration::meter_per_second_squared; +use uom::si::f64::{Acceleration, Length, Time, Velocity}; +use uom::si::length::meter; use uom::si::time::millisecond; +use uom::si::velocity::meter_per_second; fn main() { - let system = LinearSystem::<2, 1>::double_integrator(1.0, -0.1); + let system = DoubleIntegrator::new(1.0, Acceleration::new::(-0.1)); + let u_ff = system .constant_cancelling_input() - .unwrap_or_else(SVector::zeros); + .unwrap_or_else(AccelerationInput::zero); let p = LinearFeedback::<2, 1>::from_gains([2.0, 3.0]); - let mut x = SVector::::new(10.0, 0.0); - let r = SVector::::new(0.0, 0.0); + let mut x = PositionVelocityState::new( + Length::new::(10.0), + Velocity::new::(0.0), + ); + let r = PositionVelocityState::new( + Length::new::(0.0), + Velocity::new::(0.0), + ); + let dt = Time::new::(50.0); for step in 0..200 { @@ -23,8 +35,11 @@ fn main() { x = Rk4.step(&system, t, &x, &u, dt); println!( - "t={:.2}s u={:.3} pos={:.3}m vel={:.3}m/s", - t.value, u[0], x[0], x[1], + "t={:.2}s u={:.3?} pos={:.3?} vel={:.3?}", + t.value, + u.acceleration(), + x.position(), + x.velocity(), ); } } diff --git a/crates/lyapunov/src/controllers/linear_feedback.rs b/crates/lyapunov/src/controllers/linear_feedback.rs index 5f7040c..f686155 100644 --- a/crates/lyapunov/src/controllers/linear_feedback.rs +++ b/crates/lyapunov/src/controllers/linear_feedback.rs @@ -1,7 +1,7 @@ use nalgebra::{SMatrix, SVector}; use uom::si::f64::Time; -use crate::traits::{Controller, System}; +use crate::traits::{Controller, System, VectorState}; pub struct LinearFeedback { pub gain_matrix: SMatrix, @@ -9,12 +9,14 @@ pub struct LinearFeedback { impl Controller for LinearFeedback where - S: System, Input = SVector, Derivative = SVector>, - S::State: Copy + std::ops::Sub, + S: System, + S::State: Copy + std::ops::Sub + VectorState, + S::Input: From>, { fn calculate(&self, _system: &S, _t: Time, x: &S::State, r: &S::State) -> S::Input { let error = *r - *x; - self.gain_matrix * error + let error_vector = error.to_vector(); + (self.gain_matrix * error_vector).into() } } diff --git a/crates/lyapunov/src/inputs/acceleration_input.rs b/crates/lyapunov/src/inputs/acceleration_input.rs new file mode 100644 index 0000000..9a02ddd --- /dev/null +++ b/crates/lyapunov/src/inputs/acceleration_input.rs @@ -0,0 +1,68 @@ +use nalgebra::SVector; +use std::convert::From; +use std::fmt; +use std::ops::Add; +use uom::si::f64::Acceleration; + +use crate::traits::VectorInput; + +#[derive(Clone, Copy, PartialEq)] +pub struct AccelerationInput { + pub vector: SVector, +} + +impl AccelerationInput { + pub fn new(acceleration: Acceleration) -> Self { + Self { + vector: SVector::from_column_slice(&[acceleration.value]), + } + } + + pub fn acceleration(&self) -> Acceleration { + Acceleration::new::(self.vector[0]) + } + + pub fn zero() -> Self { + Self::new(Acceleration::new::< + uom::si::acceleration::meter_per_second_squared, + >(0.0)) + } +} + +impl From> for AccelerationInput { + fn from(vector: SVector) -> Self { + Self { vector } + } +} + +impl From for SVector { + fn from(input: AccelerationInput) -> Self { + input.vector + } +} + +impl VectorInput<1> for AccelerationInput { + fn to_vector(&self) -> SVector { + self.vector + } +} + +impl Add for AccelerationInput { + type Output = Self; + + fn add(self, rhs: Self) -> Self::Output { + Self { + vector: self.vector + rhs.vector, + } + } +} + +impl fmt::Debug for AccelerationInput { + fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result { + write!( + f, + "AccelerationInput {{ acceleration: {:?} }}", + self.acceleration() + ) + } +} diff --git a/crates/lyapunov/src/inputs/mod.rs b/crates/lyapunov/src/inputs/mod.rs new file mode 100644 index 0000000..af297bf --- /dev/null +++ b/crates/lyapunov/src/inputs/mod.rs @@ -0,0 +1,3 @@ +pub mod acceleration_input; + +pub use acceleration_input::AccelerationInput; diff --git a/crates/lyapunov/src/lib.rs b/crates/lyapunov/src/lib.rs index c2cdb72..466af41 100644 --- a/crates/lyapunov/src/lib.rs +++ b/crates/lyapunov/src/lib.rs @@ -1,9 +1,16 @@ pub mod controllers; +pub mod inputs; pub mod integrators; +pub mod states; pub mod systems; pub mod traits; +pub use inputs::AccelerationInput; +pub use states::{PositionVelocityState, VelocityAccelerationState}; +pub use systems::DoubleIntegrator; +pub use traits::VectorInput; + pub use controllers::linear_feedback::LinearFeedback; pub use integrators::rk4::Rk4; pub use systems::linear_system::LinearSystem; -pub use traits::{Controller, Integrator, System}; +pub use traits::{Controller, Integrator, System, VectorState}; diff --git a/crates/lyapunov/src/states/mod.rs b/crates/lyapunov/src/states/mod.rs new file mode 100644 index 0000000..8a35011 --- /dev/null +++ b/crates/lyapunov/src/states/mod.rs @@ -0,0 +1,5 @@ +pub mod position_velocity; +pub mod velocity_acceleration; + +pub use position_velocity::PositionVelocityState; +pub use velocity_acceleration::VelocityAccelerationState; diff --git a/crates/lyapunov/src/states/position_velocity.rs b/crates/lyapunov/src/states/position_velocity.rs new file mode 100644 index 0000000..83cf041 --- /dev/null +++ b/crates/lyapunov/src/states/position_velocity.rs @@ -0,0 +1,96 @@ +use nalgebra::SVector; +use std::convert::From; +use std::fmt; +use uom::si::f64::{Length, Velocity}; + +use crate::integrators::traits::Accumulate; +use crate::states::VelocityAccelerationState; +use crate::traits::VectorState; +use uom::si::f64::Time; + +#[derive(Clone, Copy, PartialEq)] +pub struct PositionVelocityState { + pub vector: SVector, +} + +impl PositionVelocityState { + pub fn new(position: Length, velocity: Velocity) -> Self { + Self { + vector: SVector::from_column_slice(&[position.value, velocity.value]), + } + } + + pub fn position(&self) -> Length { + Length::new::(self.vector[0]) + } + + pub fn velocity(&self) -> Velocity { + Velocity::new::(self.vector[1]) + } + + pub fn set_position(&mut self, position: Length) { + self.vector[0] = position.value; + } + + pub fn set_velocity(&mut self, velocity: Velocity) { + self.vector[1] = velocity.value; + } + + pub fn position_clamped(&self, min: Length, max: Length) -> Self { + let position = self.position(); + if position > max { + Self::new(max, self.velocity()) + } else if position < min { + Self::new(min, self.velocity()) + } else { + *self + } + } +} + +impl From> for PositionVelocityState { + fn from(vector: SVector) -> Self { + Self { vector } + } +} + +impl std::ops::Sub for PositionVelocityState { + type Output = Self; + + fn sub(self, rhs: Self) -> Self::Output { + Self { + vector: self.vector - rhs.vector, + } + } +} + +impl From for SVector { + fn from(state: PositionVelocityState) -> Self { + state.vector + } +} + +impl Accumulate for PositionVelocityState { + fn add_scaled(&self, dt: Time, deriv: &VelocityAccelerationState) -> Self { + Self { + vector: self.vector + dt.value * deriv.vector, + } + } +} + +impl VectorState<2> for PositionVelocityState { + fn to_vector(&self) -> SVector { + self.vector + } +} + +impl fmt::Debug for PositionVelocityState { + fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result { + write!( + f, + "PositionVelocityState {{ position: {:?}, velocity: {:?} }}", + self.position(), + self.velocity() + ) + } +} diff --git a/crates/lyapunov/src/states/velocity_acceleration.rs b/crates/lyapunov/src/states/velocity_acceleration.rs new file mode 100644 index 0000000..584637a --- /dev/null +++ b/crates/lyapunov/src/states/velocity_acceleration.rs @@ -0,0 +1,36 @@ +use nalgebra::SVector; +use std::convert::From; +use uom::si::f64::{Acceleration, Velocity}; + +#[derive(Clone, Copy, Debug, PartialEq)] +pub struct VelocityAccelerationState { + pub vector: SVector, +} + +impl VelocityAccelerationState { + pub fn new(velocity: Velocity, acceleration: Acceleration) -> Self { + Self { + vector: SVector::from_column_slice(&[velocity.value, acceleration.value]), + } + } + + pub fn velocity(&self) -> Velocity { + Velocity::new::(self.vector[0]) + } + + pub fn acceleration(&self) -> Acceleration { + Acceleration::new::(self.vector[1]) + } +} + +impl From> for VelocityAccelerationState { + fn from(vector: SVector) -> Self { + Self { vector } + } +} + +impl From for SVector { + fn from(state: VelocityAccelerationState) -> Self { + state.vector + } +} diff --git a/crates/lyapunov/src/systems/linear_system.rs b/crates/lyapunov/src/systems/linear_system.rs index feb16b3..5d51b12 100644 --- a/crates/lyapunov/src/systems/linear_system.rs +++ b/crates/lyapunov/src/systems/linear_system.rs @@ -1,6 +1,8 @@ -use crate::traits::System; +use crate::inputs::AccelerationInput; +use crate::states::{PositionVelocityState, VelocityAccelerationState}; +use crate::traits::{System, VectorInput}; use nalgebra::{DMatrix, DVector, SMatrix, SVector}; -use uom::si::f64::Time; +use uom::si::f64::{Acceleration, Time}; pub struct LinearSystem { pub state_matrix: SMatrix, @@ -45,3 +47,49 @@ impl LinearSystem<2, 1> { } } } + +#[derive(Clone, Copy)] +pub struct DoubleIntegrator { + pub state_matrix: SMatrix, + pub input_matrix: SMatrix, + pub constant: SVector, +} + +impl DoubleIntegrator { + pub fn new(input_gain: f64, disturbance: Acceleration) -> Self { + Self { + state_matrix: SMatrix::::from_row_slice(&[0.0, 1.0, 0.0, 0.0]), + input_matrix: SMatrix::::from_column_slice(&[0.0, input_gain]), + constant: SVector::::from_column_slice(&[0.0, -disturbance.value]), + } + } + + pub fn constant_cancelling_input(&self) -> Option { + let constant_cancel = -self.constant; + + let input_matrix_dyn = DMatrix::from_column_slice(2, 1, self.input_matrix.as_slice()); + let constant_cancel_dyn = DVector::from_column_slice(constant_cancel.as_slice()); + + let svd = input_matrix_dyn.svd(true, true); + let input_dyn = svd.solve(&constant_cancel_dyn, 1e-10).ok()?; + + let mut input = SVector::::zeros(); + for i in 0..1 { + input[i] = input_dyn[i]; + } + Some(AccelerationInput::from(input)) + } +} + +impl System<2, 1> for DoubleIntegrator { + type State = PositionVelocityState; + type Input = AccelerationInput; + type Derivative = VelocityAccelerationState; + + fn derivatives(&self, _t: Time, x: &Self::State, u: &Self::Input) -> Self::Derivative { + let u_vector = u.to_vector(); + let dx = self.state_matrix * x.vector + self.input_matrix * u_vector + self.constant; + + VelocityAccelerationState { vector: dx } + } +} diff --git a/crates/lyapunov/src/systems/mod.rs b/crates/lyapunov/src/systems/mod.rs index 69683f8..3b49972 100644 --- a/crates/lyapunov/src/systems/mod.rs +++ b/crates/lyapunov/src/systems/mod.rs @@ -1,3 +1,3 @@ pub mod linear_system; -pub use linear_system::LinearSystem; +pub use linear_system::{DoubleIntegrator, LinearSystem}; diff --git a/crates/lyapunov/src/traits.rs b/crates/lyapunov/src/traits.rs index dc3c815..45abd1a 100644 --- a/crates/lyapunov/src/traits.rs +++ b/crates/lyapunov/src/traits.rs @@ -1,3 +1,4 @@ +use nalgebra::SVector; use uom::si::f64::*; // Represents a dynamical system with state dimension N and input dimension M. @@ -26,3 +27,25 @@ where { fn calculate(&self, system: &S, t: Time, x: &S::State, r: &S::State) -> S::Input; } + +/// A state type that can be viewed as an `N`-dimensional vector. +pub trait VectorState: Copy { + fn to_vector(&self) -> SVector; +} + +impl VectorState for SVector { + fn to_vector(&self) -> SVector { + *self + } +} + +/// An input type that can be viewed as an `M`-dimensional vector. +pub trait VectorInput: Copy { + fn to_vector(&self) -> SVector; +} + +impl VectorInput for SVector { + fn to_vector(&self) -> SVector { + *self + } +}