Add unit-safe vectors.

Matrix and vector math is not unit-safe.

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::<meter_per_second_squared>(-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::<f64, 2>::new(10.0, 0.0);
-    let r = SVector::<f64, 2>::new(0.0, 0.0);
+    let mut x = PositionVelocityState::new(
+        Length::new::<meter>(10.0),
+        Velocity::new::<meter_per_second>(0.0),
+    );
+    let r = PositionVelocityState::new(
+        Length::new::<meter>(0.0),
+        Velocity::new::<meter_per_second>(0.0),
+    );
+
     let dt = Time::new::<millisecond>(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(),
         );
     }
 }

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<const N: usize, const M: usize> {
     pub gain_matrix: SMatrix<f64, M, N>,
@@ -9,12 +9,14 @@ pub struct LinearFeedback<const N: usize, const M: usize> {
 
 impl<S, const N: usize, const M: usize> Controller<S, N, M> for LinearFeedback<N, M>
 where
-    S: System<N, M, State = SVector<f64, N>, Input = SVector<f64, M>, Derivative = SVector<f64, N>>,
-    S::State: Copy + std::ops::Sub<Output = S::State>,
+    S: System<N, M>,
+    S::State: Copy + std::ops::Sub<Output = S::State> + VectorState<N>,
+    S::Input: From<SVector<f64, M>>,
 {
     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()
     }
 }
 

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<f64, 1>,
+}
+
+impl AccelerationInput {
+    pub fn new(acceleration: Acceleration) -> Self {
+        Self {
+            vector: SVector::from_column_slice(&[acceleration.value]),
+        }
+    }
+
+    pub fn acceleration(&self) -> Acceleration {
+        Acceleration::new::<uom::si::acceleration::meter_per_second_squared>(self.vector[0])
+    }
+
+    pub fn zero() -> Self {
+        Self::new(Acceleration::new::<
+            uom::si::acceleration::meter_per_second_squared,
+        >(0.0))
+    }
+}
+
+impl From<SVector<f64, 1>> for AccelerationInput {
+    fn from(vector: SVector<f64, 1>) -> Self {
+        Self { vector }
+    }
+}
+
+impl From<AccelerationInput> for SVector<f64, 1> {
+    fn from(input: AccelerationInput) -> Self {
+        input.vector
+    }
+}
+
+impl VectorInput<1> for AccelerationInput {
+    fn to_vector(&self) -> SVector<f64, 1> {
+        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()
+        )
+    }
+}

crates/lyapunov/src/inputs/mod.rs

@@ -0,0 +1,3 @@
+pub mod acceleration_input;
+
+pub use acceleration_input::AccelerationInput;

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};

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;

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<f64, 2>,
+}
+
+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::<uom::si::length::meter>(self.vector[0])
+    }
+
+    pub fn velocity(&self) -> Velocity {
+        Velocity::new::<uom::si::velocity::meter_per_second>(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<SVector<f64, 2>> for PositionVelocityState {
+    fn from(vector: SVector<f64, 2>) -> 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<PositionVelocityState> for SVector<f64, 2> {
+    fn from(state: PositionVelocityState) -> Self {
+        state.vector
+    }
+}
+
+impl Accumulate<VelocityAccelerationState> 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<f64, 2> {
+        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()
+        )
+    }
+}

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<f64, 2>,
+}
+
+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::<uom::si::velocity::meter_per_second>(self.vector[0])
+    }
+
+    pub fn acceleration(&self) -> Acceleration {
+        Acceleration::new::<uom::si::acceleration::meter_per_second_squared>(self.vector[1])
+    }
+}
+
+impl From<SVector<f64, 2>> for VelocityAccelerationState {
+    fn from(vector: SVector<f64, 2>) -> Self {
+        Self { vector }
+    }
+}
+
+impl From<VelocityAccelerationState> for SVector<f64, 2> {
+    fn from(state: VelocityAccelerationState) -> Self {
+        state.vector
+    }
+}

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<const N: usize, const M: usize> {
     pub state_matrix: SMatrix<f64, N, N>,
@@ -45,3 +47,49 @@ impl LinearSystem<2, 1> {
         }
     }
 }
+
+#[derive(Clone, Copy)]
+pub struct DoubleIntegrator {
+    pub state_matrix: SMatrix<f64, 2, 2>,
+    pub input_matrix: SMatrix<f64, 2, 1>,
+    pub constant: SVector<f64, 2>,
+}
+
+impl DoubleIntegrator {
+    pub fn new(input_gain: f64, disturbance: Acceleration) -> Self {
+        Self {
+            state_matrix: SMatrix::<f64, 2, 2>::from_row_slice(&[0.0, 1.0, 0.0, 0.0]),
+            input_matrix: SMatrix::<f64, 2, 1>::from_column_slice(&[0.0, input_gain]),
+            constant: SVector::<f64, 2>::from_column_slice(&[0.0, -disturbance.value]),
+        }
+    }
+
+    pub fn constant_cancelling_input(&self) -> Option<AccelerationInput> {
+        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::<f64, 1>::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 }
+    }
+}

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};

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<const N: usize>: Copy {
+    fn to_vector(&self) -> SVector<f64, N>;
+}
+
+impl<const N: usize> VectorState<N> for SVector<f64, N> {
+    fn to_vector(&self) -> SVector<f64, N> {
+        *self
+    }
+}
+
+/// An input type that can be viewed as an `M`-dimensional vector.
+pub trait VectorInput<const M: usize>: Copy {
+    fn to_vector(&self) -> SVector<f64, M>;
+}
+
+impl<const M: usize> VectorInput<M> for SVector<f64, M> {
+    fn to_vector(&self) -> SVector<f64, M> {
+        *self
+    }
+}