Add pendulum demo.

crates/lyapunov-demo/src/main.rs

@@ -1,45 +1,57 @@
-use lyapunov::inputs::AccelerationInput;
+use lyapunov::states::ThetaOmegaState;
-use lyapunov::states::PositionVelocityState;
+use lyapunov::traits::Saturate;
-use lyapunov::{Controller, DoubleIntegrator, Integrator, LinearFeedback, Rk4};
+use lyapunov::units::*;
-use uom::si::acceleration::meter_per_second_squared;
+use lyapunov::{Controller, DCMotor, Integrator, LinearFeedback, Pendulum, Rk4};
-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 = DoubleIntegrator::new(1.0, Acceleration::new::<meter_per_second_squared>(-0.1));
+    let motor = DCMotor::new(
-
+        ElectricalResistance::new::<ohm>(1.0),
-    let u_ff = system
+        Inductance::new::<henry>(0.5),
-        .constant_cancelling_input()
+        lyapunov::TorqueConstant::from(
-        .unwrap_or_else(AccelerationInput::zero);
+            Torque::new::<newton_meter>(0.5) / ElectricCurrent::new::<ampere>(1.0),
-
+        ),
-    let p = LinearFeedback::<2, 1>::from_gains([2.0, 3.0]);
+        ElectricCurrent::new::<ampere>(2.0),
-
+        ElectricPotential::new::<volt>(12.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);
+    let system = Pendulum::new(
+        Mass::new::<kilogram>(1.0),
+        Length::new::<meter>(1.0),
+        MomentOfInertia::new::<kilogram_square_meter>(1.0),
+        Acceleration::new::<meter_per_second_squared>(9.81),
+        motor,
+    );
 
-    for step in 0..200 {
+    // Gains are in A/rad and A/(rad/s).
+    let feedback = LinearFeedback::<2, 1>::from_gains([8.0, 2.0]);
+
+    let rk4 = Rk4;
+
+    // Start displaced from the downward equilibrium.
+    let mut x = ThetaOmegaState::new(
+        Angle::new::<radian>(0.5),
+        AngularVelocity::new::<radian_per_second>(0.0),
+    );
+    let r = ThetaOmegaState::new(
+        Angle::new::<radian>(0.0),
+        AngularVelocity::new::<radian_per_second>(0.0),
+    );
+
+    let dt = Time::new::<millisecond>(10.0);
+
+    for step in 0..300 {
         let t = dt * (step as f64);
 
-        let u_fb = p.calculate(&system, t, &x, &r);
+        let u_fb = feedback.calculate(&system, t, &x, &r);
-        let u = u_fb + u_ff;
+        let u = u_fb.saturate(&system);
-        x = Rk4.step(&system, t, &x, &u, dt);
+        x = rk4.step(&system, t, &x, &u, dt);
 
         println!(
-            "t={:.2}s  u={:.3?}  pos={:.3?}  vel={:.3?}",
+            "t={:.2}s  theta={:.3?}  omega={:.3?}  current={:.3?}",
             t.value,
-            u.acceleration(),
+            x.angle(),
-            x.position(),
+            x.angular_velocity(),
-            x.velocity(),
+            u.current(),
         );
     }
 }

crates/lyapunov/src/controllers/linear_feedback.rs

@@ -1,7 +1,7 @@
-use nalgebra::{SMatrix, SVector};
+use crate::units::*;
-use uom::si::f64::Time;
+use nalgebra::SMatrix;
 
-use crate::traits::{Controller, System, VectorState};
+use crate::traits::{Controller, System, VectorValue};
 
 pub struct LinearFeedback<const N: usize, const M: usize> {
     pub gain_matrix: SMatrix<f64, M, N>,
@@ -10,8 +10,8 @@ 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>,
-    S::State: Copy + std::ops::Sub<Output = S::State> + VectorState<N>,
+    S::State: VectorValue<N>,
-    S::Input: From<SVector<f64, M>>,
+    S::Input: VectorValue<M>,
 {
     fn calculate(&self, _system: &S, _t: Time, x: &S::State, r: &S::State) -> S::Input {
         let error = *r - *x;

crates/lyapunov/src/inputs/acceleration.rs

@@ -0,0 +1,28 @@
+use crate::units::*;
+use nalgebra::SVector;
+use std::convert::From;
+
+use crate::impl_has_vector;
+
+#[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::<meter_per_second_squared>(self.vector[0])
+    }
+
+    pub fn zero() -> Self {
+        Self::new(Acceleration::new::<meter_per_second_squared>(0.0))
+    }
+}
+
+impl_has_vector!(AccelerationInput, 1);

crates/lyapunov/src/inputs/acceleration_input.rs

@@ -1,68 +0,0 @@
-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/current.rs

@@ -0,0 +1,24 @@
+use crate::units::*;
+use nalgebra::SVector;
+use std::convert::From;
+
+use crate::impl_has_vector;
+
+#[derive(Clone, Copy, PartialEq)]
+pub struct CurrentInput {
+    pub vector: SVector<f64, 1>,
+}
+
+impl CurrentInput {
+    pub fn new(current: ElectricCurrent) -> Self {
+        Self {
+            vector: SVector::from_column_slice(&[current.value]),
+        }
+    }
+
+    pub fn current(&self) -> ElectricCurrent {
+        ElectricCurrent::new::<ampere>(self.vector[0])
+    }
+}
+
+impl_has_vector!(CurrentInput, 1);

crates/lyapunov/src/inputs/mod.rs

@@ -1,3 +1,7 @@
-pub mod acceleration_input;
+pub mod acceleration;
+pub mod current;
+pub mod voltage;
 
-pub use acceleration_input::AccelerationInput;
+pub use acceleration::AccelerationInput;
+pub use current::CurrentInput;
+pub use voltage::VoltageInput;

crates/lyapunov/src/inputs/voltage.rs

@@ -0,0 +1,25 @@
+use crate::units::Voltage;
+use crate::units::*;
+use nalgebra::SVector;
+use std::convert::From;
+
+use crate::impl_has_vector;
+
+#[derive(Clone, Copy, PartialEq)]
+pub struct VoltageInput {
+    pub vector: SVector<f64, 1>,
+}
+
+impl VoltageInput {
+    pub fn new(voltage: Voltage) -> Self {
+        Self {
+            vector: SVector::from_column_slice(&[voltage.value]),
+        }
+    }
+
+    pub fn voltage(&self) -> Voltage {
+        Voltage::new::<volt>(self.vector[0])
+    }
+}
+
+impl_has_vector!(VoltageInput, 1);

crates/lyapunov/src/integrators/rk4.rs

@@ -1,8 +1,8 @@
-use uom::si::f64::Time;
+use crate::units::*;
 
 use crate::{
     integrators::traits::Accumulate,
-    traits::{Integrator, System},
+    traits::{Integrator, System, TimeDerivative},
 };
 
 pub struct Rk4;
@@ -10,7 +10,7 @@ pub struct Rk4;
 impl<S, const N: usize, const M: usize> Integrator<S, N, M> for Rk4
 where
     S: System<N, M>,
-    S::State: Clone + Accumulate<S::Derivative>,
+    S::State: Clone + TimeDerivative + Accumulate<<S::State as TimeDerivative>::Derivative>,
 {
     fn step(&self, system: &S, t: Time, x: &S::State, u: &S::Input, dt: Time) -> S::State {
         let half_dt = dt * 0.5;

crates/lyapunov/src/integrators/traits.rs

@@ -1,5 +1,5 @@
+use crate::units::*;
 use nalgebra::SVector;
-use uom::si::f64::Time;
 
 pub trait Accumulate<D> {
     fn add_scaled(&self, dt: Time, derivative: &D) -> Self;
@@ -7,6 +7,6 @@ pub trait Accumulate<D> {
 
 impl<const N: usize> Accumulate<SVector<f64, N>> for SVector<f64, N> {
     fn add_scaled(&self, dt: Time, derivative: &SVector<f64, N>) -> Self {
-        self + dt.get::<uom::si::time::second>() * derivative
+        self + dt.get::<second>() * derivative
     }
 }

crates/lyapunov/src/lib.rs

@@ -4,13 +4,18 @@ pub mod integrators;
 pub mod states;
 pub mod systems;
 pub mod traits;
+pub mod units;
 
-pub use inputs::AccelerationInput;
+pub use inputs::{AccelerationInput, CurrentInput, VoltageInput};
-pub use states::{PositionVelocityState, VelocityAccelerationState};
+pub use states::{
-pub use systems::DoubleIntegrator;
+    CurrentRateState, CurrentState, OmegaAlphaState, PositionVelocityState, ThetaOmegaState,
-pub use traits::VectorInput;
+    VelocityAccelerationState,
+};
+pub use systems::{DCMotor, DCMotorPosition, DoubleIntegrator, Pendulum};
+pub use traits::{Saturate, VectorValue};
+pub use units::{BackEmfConstant, CurrentRate, FrictionCoefficient, TorqueConstant, Voltage};
 
 pub use controllers::linear_feedback::LinearFeedback;
 pub use integrators::rk4::Rk4;
 pub use systems::linear_system::LinearSystem;
-pub use traits::{Controller, Integrator, System, VectorState};
+pub use traits::{Controller, Integrator, System, TimeDerivative};

crates/lyapunov/src/states/current.rs

@@ -0,0 +1,39 @@
+use crate::units::*;
+use nalgebra::SVector;
+use std::convert::From;
+
+use crate::impl_has_vector;
+use crate::integrators::traits::Accumulate;
+use crate::states::CurrentRateState;
+use crate::traits::TimeDerivative;
+
+#[derive(Clone, Copy, PartialEq)]
+pub struct CurrentState {
+    pub vector: SVector<f64, 1>,
+}
+
+impl CurrentState {
+    pub fn new(current: ElectricCurrent) -> Self {
+        Self {
+            vector: SVector::from_column_slice(&[current.value]),
+        }
+    }
+
+    pub fn current(&self) -> ElectricCurrent {
+        ElectricCurrent::new::<ampere>(self.vector[0])
+    }
+}
+
+impl Accumulate<CurrentRateState> for CurrentState {
+    fn add_scaled(&self, dt: Time, deriv: &CurrentRateState) -> Self {
+        Self {
+            vector: self.vector + dt.value * deriv.vector,
+        }
+    }
+}
+
+impl_has_vector!(CurrentState, 1);
+
+impl TimeDerivative for CurrentState {
+    type Derivative = CurrentRateState;
+}

crates/lyapunov/src/states/current_rate.rs

@@ -0,0 +1,32 @@
+use crate::units::*;
+use nalgebra::SVector;
+use std::convert::From;
+
+#[derive(Clone, Copy, PartialEq)]
+pub struct CurrentRateState {
+    pub vector: SVector<f64, 1>,
+}
+
+impl CurrentRateState {
+    pub fn new(rate: CurrentRate) -> Self {
+        Self {
+            vector: SVector::from_column_slice(&[rate.value]),
+        }
+    }
+
+    pub fn rate(&self) -> CurrentRate {
+        ElectricCurrent::new::<ampere>(self.vector[0]) / Time::new::<second>(1.0)
+    }
+}
+
+impl From<SVector<f64, 1>> for CurrentRateState {
+    fn from(vector: SVector<f64, 1>) -> Self {
+        Self { vector }
+    }
+}
+
+impl From<CurrentRateState> for SVector<f64, 1> {
+    fn from(state: CurrentRateState) -> Self {
+        state.vector
+    }
+}

crates/lyapunov/src/states/mod.rs

@@ -1,5 +1,13 @@
+pub mod current;
+pub mod current_rate;
+pub mod omega_alpha;
 pub mod position_velocity;
+pub mod theta_omega;
 pub mod velocity_acceleration;
 
+pub use current::CurrentState;
+pub use current_rate::CurrentRateState;
+pub use omega_alpha::OmegaAlphaState;
 pub use position_velocity::PositionVelocityState;
+pub use theta_omega::ThetaOmegaState;
 pub use velocity_acceleration::VelocityAccelerationState;

crates/lyapunov/src/states/omega_alpha.rs

@@ -0,0 +1,42 @@
+use crate::units::*;
+use nalgebra::SVector;
+use std::convert::From;
+
+#[derive(Clone, Copy, Debug, PartialEq)]
+pub struct OmegaAlphaState {
+    pub vector: SVector<f64, 2>,
+}
+
+impl OmegaAlphaState {
+    pub fn new(
+        angular_velocity: AngularVelocity,
+        angular_acceleration: AngularAcceleration,
+    ) -> Self {
+        Self {
+            vector: SVector::from_column_slice(&[
+                angular_velocity.value,
+                angular_acceleration.value,
+            ]),
+        }
+    }
+
+    pub fn angular_velocity(&self) -> AngularVelocity {
+        AngularVelocity::new::<radian_per_second>(self.vector[0])
+    }
+
+    pub fn angular_acceleration(&self) -> AngularAcceleration {
+        AngularAcceleration::new::<radian_per_second_squared>(self.vector[1])
+    }
+}
+
+impl From<SVector<f64, 2>> for OmegaAlphaState {
+    fn from(vector: SVector<f64, 2>) -> Self {
+        Self { vector }
+    }
+}
+
+impl From<OmegaAlphaState> for SVector<f64, 2> {
+    fn from(state: OmegaAlphaState) -> Self {
+        state.vector
+    }
+}

crates/lyapunov/src/states/position_velocity.rs

@@ -1,12 +1,11 @@
+use crate::units::*;
 use nalgebra::SVector;
 use std::convert::From;
-use std::fmt;
-use uom::si::f64::{Length, Velocity};
 
+use crate::impl_has_vector;
 use crate::integrators::traits::Accumulate;
 use crate::states::VelocityAccelerationState;
-use crate::traits::VectorState;
+use crate::traits::TimeDerivative;
-use uom::si::f64::Time;
 
 #[derive(Clone, Copy, PartialEq)]
 pub struct PositionVelocityState {
@@ -21,11 +20,11 @@ impl PositionVelocityState {
     }
 
     pub fn position(&self) -> Length {
-        Length::new::<uom::si::length::meter>(self.vector[0])
+        Length::new::<meter>(self.vector[0])
     }
 
     pub fn velocity(&self) -> Velocity {
-        Velocity::new::<uom::si::velocity::meter_per_second>(self.vector[1])
+        Velocity::new::<meter_per_second>(self.vector[1])
     }
 
     pub fn set_position(&mut self, position: Length) {
@@ -35,39 +34,6 @@ impl PositionVelocityState {
     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 {
@@ -78,19 +44,8 @@ impl Accumulate<VelocityAccelerationState> for PositionVelocityState {
     }
 }
 
-impl VectorState<2> for PositionVelocityState {
+impl_has_vector!(PositionVelocityState, 2);
-    fn to_vector(&self) -> SVector<f64, 2> {
-        self.vector
-    }
-}
 
-impl fmt::Debug for PositionVelocityState {
+impl TimeDerivative for PositionVelocityState {
-    fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
+    type Derivative = VelocityAccelerationState;
-        write!(
-            f,
-            "PositionVelocityState {{ position: {:?}, velocity: {:?} }}",
-            self.position(),
-            self.velocity()
-        )
-    }
 }

crates/lyapunov/src/states/theta_omega.rs

@@ -0,0 +1,51 @@
+use crate::units::*;
+use nalgebra::SVector;
+use std::convert::From;
+
+use crate::impl_has_vector;
+use crate::integrators::traits::Accumulate;
+use crate::states::OmegaAlphaState;
+use crate::traits::TimeDerivative;
+
+#[derive(Clone, Copy, Debug, PartialEq)]
+pub struct ThetaOmegaState {
+    pub vector: SVector<f64, 2>,
+}
+
+impl ThetaOmegaState {
+    pub fn new(angle: Angle, angular_velocity: AngularVelocity) -> Self {
+        Self {
+            vector: SVector::from_column_slice(&[angle.value, angular_velocity.value]),
+        }
+    }
+
+    pub fn angle(&self) -> Angle {
+        Angle::new::<radian>(self.vector[0])
+    }
+
+    pub fn angular_velocity(&self) -> AngularVelocity {
+        AngularVelocity::new::<radian_per_second>(self.vector[1])
+    }
+
+    pub fn set_angle(&mut self, angle: Angle) {
+        self.vector[0] = angle.value;
+    }
+
+    pub fn set_angular_velocity(&mut self, angular_velocity: AngularVelocity) {
+        self.vector[1] = angular_velocity.value;
+    }
+}
+
+impl Accumulate<OmegaAlphaState> for ThetaOmegaState {
+    fn add_scaled(&self, dt: Time, deriv: &OmegaAlphaState) -> Self {
+        Self {
+            vector: self.vector + dt.value * deriv.vector,
+        }
+    }
+}
+
+impl_has_vector!(ThetaOmegaState, 2);
+
+impl TimeDerivative for ThetaOmegaState {
+    type Derivative = OmegaAlphaState;
+}

crates/lyapunov/src/states/velocity_acceleration.rs

@@ -1,6 +1,6 @@
+use crate::units::*;
 use nalgebra::SVector;
 use std::convert::From;
-use uom::si::f64::{Acceleration, Velocity};
 
 #[derive(Clone, Copy, Debug, PartialEq)]
 pub struct VelocityAccelerationState {
@@ -15,11 +15,11 @@ impl VelocityAccelerationState {
     }
 
     pub fn velocity(&self) -> Velocity {
-        Velocity::new::<uom::si::velocity::meter_per_second>(self.vector[0])
+        Velocity::new::<meter_per_second>(self.vector[0])
     }
 
     pub fn acceleration(&self) -> Acceleration {
-        Acceleration::new::<uom::si::acceleration::meter_per_second_squared>(self.vector[1])
+        Acceleration::new::<meter_per_second_squared>(self.vector[1])
     }
 }
 

crates/lyapunov/src/systems/dc_motor.rs

@@ -0,0 +1,149 @@
+use crate::units::*;
+
+use crate::inputs::{CurrentInput, VoltageInput};
+use crate::states::{CurrentRateState, CurrentState, OmegaAlphaState, ThetaOmegaState};
+use crate::traits::{Saturate, System, TimeDerivative};
+
+#[derive(Clone, Copy, Debug)]
+pub struct DCMotor {
+    pub resistance: ElectricalResistance,
+    pub inductance: Inductance,
+    pub torque_constant: TorqueConstant,
+    pub max_current: ElectricCurrent,
+    pub nominal_voltage: ElectricPotential,
+}
+
+impl DCMotor {
+    pub fn new(
+        resistance: ElectricalResistance,
+        inductance: Inductance,
+        torque_constant: TorqueConstant,
+        max_current: ElectricCurrent,
+        nominal_voltage: ElectricPotential,
+    ) -> Self {
+        Self {
+            resistance,
+            inductance,
+            torque_constant,
+            max_current,
+            nominal_voltage,
+        }
+    }
+
+    pub fn torque(&self, current: ElectricCurrent) -> Torque {
+        Torque::new::<newton_meter>((self.torque_constant * current).value)
+    }
+
+    pub fn saturate_current(&self, current: ElectricCurrent) -> ElectricCurrent {
+        let clamped = current
+            .value
+            .clamp(-self.max_current.value, self.max_current.value);
+        ElectricCurrent::new::<ampere>(clamped)
+    }
+}
+
+impl System<1, 1> for DCMotor {
+    type State = CurrentState;
+    type Input = VoltageInput;
+
+    fn derivatives(
+        &self,
+        _t: Time,
+        x: &Self::State,
+        u: &Self::Input,
+    ) -> <Self::State as TimeDerivative>::Derivative {
+        let current = x.current().value;
+        let voltage = u.voltage().value;
+
+        // Simplified electrical dynamics: L di/dt = V - R i
+        let di_dt = (voltage - self.resistance.value * current) / self.inductance.value;
+
+        CurrentRateState::new(ElectricCurrent::new::<ampere>(di_dt) / Time::new::<second>(1.0))
+    }
+}
+
+#[derive(Clone, Copy, Debug)]
+pub struct DCMotorPosition {
+    pub inertia: MomentOfInertia,
+    pub torque_constant: TorqueConstant,
+    pub back_emf_constant: BackEmfConstant,
+    pub resistance: ElectricalResistance,
+    pub friction: FrictionCoefficient,
+    pub nominal_voltage: ElectricPotential,
+}
+
+impl DCMotorPosition {
+    pub fn new(
+        inertia: MomentOfInertia,
+        torque_constant: TorqueConstant,
+        back_emf_constant: BackEmfConstant,
+        resistance: ElectricalResistance,
+        friction: FrictionCoefficient,
+        nominal_voltage: ElectricPotential,
+    ) -> Self {
+        Self {
+            inertia,
+            torque_constant,
+            back_emf_constant,
+            resistance,
+            friction,
+            nominal_voltage,
+        }
+    }
+}
+
+impl System<2, 1> for DCMotorPosition {
+    type State = ThetaOmegaState;
+    type Input = VoltageInput;
+
+    fn derivatives(
+        &self,
+        _t: Time,
+        x: &Self::State,
+        u: &Self::Input,
+    ) -> <Self::State as TimeDerivative>::Derivative {
+        let omega = x.angular_velocity();
+        let voltage = u.voltage().value;
+
+        // Instantaneous current: I = (V - K_e * omega) / R
+        let back_emf = (self.back_emf_constant * omega).value;
+        let current = (voltage - back_emf) / self.resistance.value;
+
+        // Motor torque: tau = K_t * I
+        let torque = (self.torque_constant * ElectricCurrent::new::<ampere>(current)).value;
+
+        // Angular acceleration.
+        let alpha = (torque - (self.friction * omega).value) / self.inertia.value;
+
+        OmegaAlphaState::new(
+            AngularVelocity::new::<radian_per_second>(omega.value),
+            AngularAcceleration::new::<radian_per_second_squared>(alpha),
+        )
+    }
+}
+
+impl Saturate<DCMotor> for VoltageInput {
+    fn saturate(&self, system: &DCMotor) -> Self {
+        let clamped = self
+            .voltage()
+            .value
+            .clamp(-system.nominal_voltage.value, system.nominal_voltage.value);
+        VoltageInput::new(ElectricPotential::new::<volt>(clamped))
+    }
+}
+
+impl Saturate<DCMotorPosition> for VoltageInput {
+    fn saturate(&self, system: &DCMotorPosition) -> Self {
+        let clamped = self
+            .voltage()
+            .value
+            .clamp(-system.nominal_voltage.value, system.nominal_voltage.value);
+        VoltageInput::new(ElectricPotential::new::<volt>(clamped))
+    }
+}
+
+impl Saturate<DCMotor> for CurrentInput {
+    fn saturate(&self, system: &DCMotor) -> Self {
+        CurrentInput::new(system.saturate_current(self.current()))
+    }
+}

crates/lyapunov/src/systems/linear_system.rs

@@ -1,8 +1,8 @@
 use crate::inputs::AccelerationInput;
 use crate::states::{PositionVelocityState, VelocityAccelerationState};
-use crate::traits::{System, VectorInput};
+use crate::traits::{System, TimeDerivative, VectorValue};
+use crate::units::*;
 use nalgebra::{DMatrix, DVector, SMatrix, SVector};
-use uom::si::f64::{Acceleration, Time};
 
 pub struct LinearSystem<const N: usize, const M: usize> {
     pub state_matrix: SMatrix<f64, N, N>,
@@ -31,9 +31,13 @@ impl<const N: usize, const M: usize> LinearSystem<N, M> {
 impl<const N: usize, const M: usize> System<N, M> for LinearSystem<N, M> {
     type State = SVector<f64, N>;
     type Input = SVector<f64, M>;
-    type Derivative = SVector<f64, N>;
 
-    fn derivatives(&self, _t: Time, x: &Self::State, u: &Self::Input) -> Self::Derivative {
+    fn derivatives(
+        &self,
+        _t: Time,
+        x: &Self::State,
+        u: &Self::Input,
+    ) -> <Self::State as TimeDerivative>::Derivative {
         self.state_matrix * x + self.input_matrix * u + self.constant
     }
 }
@@ -84,9 +88,13 @@ impl DoubleIntegrator {
 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 {
+    fn derivatives(
+        &self,
+        _t: Time,
+        x: &Self::State,
+        u: &Self::Input,
+    ) -> <Self::State as TimeDerivative>::Derivative {
         let u_vector = u.to_vector();
         let dx = self.state_matrix * x.vector + self.input_matrix * u_vector + self.constant;
 

crates/lyapunov/src/systems/mod.rs

@@ -1,3 +1,7 @@
+pub mod dc_motor;
 pub mod linear_system;
+pub mod pendulum;
 
+pub use dc_motor::{DCMotor, DCMotorPosition};
 pub use linear_system::{DoubleIntegrator, LinearSystem};
+pub use pendulum::Pendulum;

crates/lyapunov/src/systems/pendulum.rs

@@ -0,0 +1,68 @@
+use crate::units::*;
+
+use crate::inputs::CurrentInput;
+use crate::states::{OmegaAlphaState, ThetaOmegaState};
+use crate::systems::dc_motor::DCMotor;
+use crate::traits::{Saturate, System, TimeDerivative};
+
+#[derive(Clone, Copy, Debug)]
+pub struct Pendulum {
+    pub mass: Mass,
+    pub length: Length,
+    pub inertia: MomentOfInertia,
+    pub gravity: Acceleration,
+    pub motor: DCMotor,
+}
+
+impl Pendulum {
+    pub fn new(
+        mass: Mass,
+        length: Length,
+        inertia: MomentOfInertia,
+        gravity: Acceleration,
+        motor: DCMotor,
+    ) -> Self {
+        Self {
+            mass,
+            length,
+            inertia,
+            gravity,
+            motor,
+        }
+    }
+}
+
+impl System<2, 1> for Pendulum {
+    type State = ThetaOmegaState;
+    type Input = CurrentInput;
+
+    fn derivatives(
+        &self,
+        _t: Time,
+        x: &Self::State,
+        u: &Self::Input,
+    ) -> <Self::State as TimeDerivative>::Derivative {
+        let theta = x.angle().value;
+        let omega = x.angular_velocity().value;
+        let current = u.current().value;
+
+        let torque = self
+            .motor
+            .torque(ElectricCurrent::new::<ampere>(current))
+            .value;
+        let alpha = (torque
+            - self.mass.value * self.gravity.value * self.length.value * theta.sin())
+            / self.inertia.value;
+
+        OmegaAlphaState::new(
+            AngularVelocity::new::<radian_per_second>(omega),
+            AngularAcceleration::new::<radian_per_second_squared>(alpha),
+        )
+    }
+}
+
+impl Saturate<Pendulum> for CurrentInput {
+    fn saturate(&self, system: &Pendulum) -> Self {
+        self.saturate(&system.motor)
+    }
+}

crates/lyapunov/src/traits.rs

@@ -1,15 +1,29 @@
+use crate::units::*;
 use nalgebra::SVector;
-use uom::si::f64::*;
+
+// Indicates the time-derivative type of a state.
+pub trait TimeDerivative {
+    type Derivative;
+}
+
+// `SVector` is its own derivative type.
+impl<const N: usize> TimeDerivative for SVector<f64, N> {
+    type Derivative = SVector<f64, N>;
+}
 
 // Represents a dynamical system with state dimension N and input dimension M.
 pub trait System<const N: usize, const M: usize> {
-    type State;
+    type State: TimeDerivative;
     type Input;
-    type Derivative;
 
     // Evaluate the derivative vector x_dot at (t, x, u)
     // TODO Create time-invariant system trait?
-    fn derivatives(&self, t: Time, x: &Self::State, u: &Self::Input) -> Self::Derivative;
+    fn derivatives(
+        &self,
+        t: Time,
+        x: &Self::State,
+        u: &Self::Input,
+    ) -> <Self::State as TimeDerivative>::Derivative;
 }
 
 // Computes the evolution of a system over (t, x, u)
@@ -28,24 +42,91 @@ 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 HasVector<const D: usize> {
-pub trait VectorState<const N: usize>: Copy {
+    fn vector(&self) -> &SVector<f64, D>;
-    fn to_vector(&self) -> SVector<f64, N>;
+    fn vector_mut(&mut self) -> &mut SVector<f64, D>;
 }
 
-impl<const N: usize> VectorState<N> for SVector<f64, N> {
+impl<const D: usize> HasVector<D> for SVector<f64, D> {
-    fn to_vector(&self) -> SVector<f64, N> {
+    fn vector(&self) -> &SVector<f64, D> {
-        *self
+        self
+    }
+
+    fn vector_mut(&mut self) -> &mut SVector<f64, D> {
+        self
     }
 }
 
-/// An input type that can be viewed as an `M`-dimensional vector.
+pub trait VectorValue<const D: usize>:
-pub trait VectorInput<const M: usize>: Copy {
+    Copy
-    fn to_vector(&self) -> SVector<f64, M>;
+    + From<SVector<f64, D>>
-}
+    + Into<SVector<f64, D>>
-
+    + std::ops::Add<Output = Self>
-impl<const M: usize> VectorInput<M> for SVector<f64, M> {
+    + std::ops::Sub<Output = Self>
-    fn to_vector(&self) -> SVector<f64, M> {
+    + HasVector<D>
-        *self
+{
+    fn to_vector(&self) -> SVector<f64, D> {
+        *self.vector()
     }
 }
+
+impl<T, const D: usize> VectorValue<D> for T where
+    T: Copy
+        + From<SVector<f64, D>>
+        + Into<SVector<f64, D>>
+        + std::ops::Add<Output = T>
+        + std::ops::Sub<Output = T>
+        + HasVector<D>
+{
+}
+
+#[macro_export]
+macro_rules! impl_has_vector {
+    ($type:ty, $dim:expr) => {
+        impl $crate::traits::HasVector<$dim> for $type {
+            fn vector(&self) -> &nalgebra::SVector<f64, $dim> {
+                &self.vector
+            }
+
+            fn vector_mut(&mut self) -> &mut nalgebra::SVector<f64, $dim> {
+                &mut self.vector
+            }
+        }
+
+        impl From<nalgebra::SVector<f64, $dim>> for $type {
+            fn from(vector: nalgebra::SVector<f64, $dim>) -> Self {
+                Self { vector }
+            }
+        }
+
+        impl From<$type> for nalgebra::SVector<f64, $dim> {
+            fn from(value: $type) -> Self {
+                value.vector
+            }
+        }
+
+        impl std::ops::Add for $type {
+            type Output = Self;
+
+            fn add(self, rhs: Self) -> Self::Output {
+                Self {
+                    vector: self.vector + rhs.vector,
+                }
+            }
+        }
+
+        impl std::ops::Sub for $type {
+            type Output = Self;
+
+            fn sub(self, rhs: Self) -> Self::Output {
+                Self {
+                    vector: self.vector - rhs.vector,
+                }
+            }
+        }
+    };
+}
+
+pub trait Saturate<S> {
+    fn saturate(&self, system: &S) -> Self;
+}

crates/lyapunov/src/units.rs

@@ -0,0 +1,26 @@
+pub use uom::si::f64::*;
+
+pub use uom::si::acceleration::meter_per_second_squared;
+pub use uom::si::angle::radian;
+pub use uom::si::angular_acceleration::radian_per_second_squared;
+pub use uom::si::angular_velocity::radian_per_second;
+pub use uom::si::electric_current::ampere;
+pub use uom::si::electric_potential::volt;
+pub use uom::si::electrical_resistance::ohm;
+pub use uom::si::inductance::henry;
+pub use uom::si::length::meter;
+pub use uom::si::mass::kilogram;
+pub use uom::si::moment_of_inertia::kilogram_square_meter;
+pub use uom::si::time::millisecond;
+pub use uom::si::time::second;
+pub use uom::si::torque::newton_meter;
+pub use uom::si::velocity::meter_per_second;
+
+use std::ops::Div;
+
+pub type Voltage = ElectricPotential;
+
+pub type TorqueConstant = <Torque as Div<ElectricCurrent>>::Output;
+pub type BackEmfConstant = <ElectricPotential as Div<AngularVelocity>>::Output;
+pub type FrictionCoefficient = <Torque as Div<AngularVelocity>>::Output;
+pub type CurrentRate = <ElectricCurrent as Div<Time>>::Output;