crates/lyapunov-demo/src/main.rs

@@ -4,43 +4,54 @@ use lyapunov::units::*;
 use lyapunov::{Controller, DCMotor, Integrator, LinearFeedback, Pendulum, Rk4};
 
 fn main() {
-    let system = Pendulum::new(
+    let motor = DCMotor::new(
-        Mass::new::<kilogram>(0.024),
+        ElectricalResistance::new::<ohm>(1.0),
-        Length::new::<meter>(0.129),
+        Inductance::new::<henry>(0.5),
-        MomentOfInertia::new::<kilogram_square_meter>(3.33 * 0.00001),
+        lyapunov::TorqueConstant::from(
-        Acceleration::new::<meter_per_second_squared>(9.81),
+            Torque::new::<newton_meter>(0.5) / ElectricCurrent::new::<ampere>(1.0),
-        DCMotor::qube_servo_2(),
+        ),
+        ElectricCurrent::new::<ampere>(2.0),
+        ElectricPotential::new::<volt>(12.0),
     );
 
-    let r = ThetaOmegaState::new(Angle::new::<radian>(std::f64::consts::PI));
+    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,
+    );
 
-    let feedback = LinearFeedback::<2, 1>::pendulum(&system);
+    // Gains are in A/rad and A/(rad/s).
+    let feedback = LinearFeedback::<2, 1>::from_gains([8.0, 2.0]);
 
     let rk4 = Rk4;
 
-    // Start at the downward (stable) equilibrium to demonstrate swing-up.
+    // Start displaced from the downward equilibrium.
-    let mut x = ThetaOmegaState::zero();
+    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..350 {
+    for step in 0..300 {
         let t = dt * (step as f64);
 
         let u_fb = feedback.calculate(&system, t, &x, &r);
-        let u_ff = system.gravity_cancelling_input(&x);
+        let u = u_fb.saturate(&system);
-        let u_total = u_fb + u_ff;
-        let u = u_total.saturate(&system);
-
         x = rk4.step(&system, t, &x, &u, dt);
 
         println!(
-            "t={:.2?}  theta={:.3?}  omega={:.3?}  current={:.3?}  u_fb={:.3?}  u_ff={:.3?}",
+            "t={:.2}s  theta={:.3?}  omega={:.3?}  current={:.3?}",
-            t,
+            t.value,
             x.angle(),
             x.angular_velocity(),
             u.current(),
-            u_fb.current(),
-            u_ff.current()
         );
     }
 }

crates/lyapunov/src/controllers/linear_feedback.rs

@@ -1,8 +1,6 @@
 use crate::units::*;
 use nalgebra::SMatrix;
 
-use crate::states::ThetaOmegaState;
-use crate::systems::{LinearSystem, Pendulum};
 use crate::traits::{Controller, System, VectorValue};
 
 pub struct LinearFeedback<const N: usize, const M: usize> {
@@ -32,65 +30,6 @@ impl<const N: usize> LinearFeedback<N, 1> {
     }
 }
 
-impl LinearFeedback<2, 1> {
-    pub fn pendulum(pendulum: &Pendulum) -> Self {
-        Self::pendulum_with(pendulum, 5.0, 1.0)
-    }
-
-    pub fn local_pendulum(pendulum: &Pendulum, reference: &ThetaOmegaState) -> Self {
-        Self::local_pendulum_with(pendulum, reference, 5.0, 1.0)
-    }
-
-    pub fn pendulum_with(pendulum: &Pendulum, omega_n: f64, zeta: f64) -> Self {
-        let linearized = pendulum.linearize();
-        Self::from_linear_system_with(&linearized, omega_n, zeta)
-    }
-
-    pub fn local_pendulum_with(
-        pendulum: &Pendulum,
-        reference: &ThetaOmegaState,
-        omega_n: f64,
-        zeta: f64,
-    ) -> Self {
-        let linearized = pendulum.local_linearize(reference);
-        Self::from_linear_system_with(&linearized, omega_n, zeta)
-    }
-
-    pub fn pendulum_with_settling_time(pendulum: &Pendulum, settling_time: Time) -> Self {
-        let linearized = pendulum.linearize();
-        Self::from_linear_system_with_settling_time(&linearized, settling_time)
-    }
-
-    pub fn local_pendulum_with_settling_time(
-        pendulum: &Pendulum,
-        reference: &ThetaOmegaState,
-        settling_time: Time,
-    ) -> Self {
-        let linearized = pendulum.local_linearize(reference);
-        Self::from_linear_system_with_settling_time(&linearized, settling_time)
-    }
-
-    pub fn from_linear_system(system: &LinearSystem<2, 1>) -> Self {
-        Self::from_linear_system_with(system, 5.0, 1.0)
-    }
-
-    pub fn from_linear_system_with(system: &LinearSystem<2, 1>, omega_n: f64, zeta: f64) -> Self {
-        let stiffness = system.state_matrix[(1, 0)];
-        let input_gain = system.input_matrix[(1, 0)];
-        let k_d = 2.0 * zeta * omega_n / input_gain;
-        let k_p = (omega_n * omega_n + stiffness) / input_gain;
-        Self::from_gains([k_p, k_d])
-    }
-
-    pub fn from_linear_system_with_settling_time(
-        system: &LinearSystem<2, 1>,
-        settling_time: Time,
-    ) -> Self {
-        let omega_n = 4.0 / settling_time.get::<second>();
-        Self::from_linear_system_with(system, omega_n, 1.0)
-    }
-}
-
 impl<const N: usize> LinearFeedback<N, N> {
     pub fn diagonal(gains: [f64; N]) -> Self {
         let mut gain_matrix = SMatrix::<f64, N, N>::zeros();

crates/lyapunov/src/inputs/acceleration.rs

@@ -10,11 +10,10 @@ pub struct AccelerationInput {
 }
 
 impl AccelerationInput {
-    pub fn new<T>(args: T) -> Self
+    pub fn new(acceleration: Acceleration) -> Self {
-    where
+        Self {
-        Self: From<T>,
+            vector: SVector::from_column_slice(&[acceleration.value]),
-    {
+        }
-        Self::from(args)
     }
 
     pub fn acceleration(&self) -> Acceleration {
@@ -22,15 +21,7 @@ impl AccelerationInput {
     }
 
     pub fn zero() -> Self {
-        Self::from(Acceleration::new::<meter_per_second_squared>(0.0))
+        Self::new(Acceleration::new::<meter_per_second_squared>(0.0))
-    }
-}
-
-impl From<Acceleration> for AccelerationInput {
-    fn from(acceleration: Acceleration) -> Self {
-        Self {
-            vector: SVector::from_column_slice(&[acceleration.value]),
-        }
     }
 }
 

crates/lyapunov/src/inputs/current.rs

@@ -10,11 +10,10 @@ pub struct CurrentInput {
 }
 
 impl CurrentInput {
-    pub fn new<T>(args: T) -> Self
+    pub fn new(current: ElectricCurrent) -> Self {
-    where
+        Self {
-        Self: From<T>,
+            vector: SVector::from_column_slice(&[current.value]),
-    {
+        }
-        Self::from(args)
     }
 
     pub fn current(&self) -> ElectricCurrent {
@@ -22,12 +21,4 @@ impl CurrentInput {
     }
 }
 
-impl From<ElectricCurrent> for CurrentInput {
-    fn from(current: ElectricCurrent) -> Self {
-        Self {
-            vector: SVector::from_column_slice(&[current.value]),
-        }
-    }
-}
-
 impl_has_vector!(CurrentInput, 1);

crates/lyapunov/src/inputs/voltage.rs

@@ -11,11 +11,10 @@ pub struct VoltageInput {
 }
 
 impl VoltageInput {
-    pub fn new<T>(args: T) -> Self
+    pub fn new(voltage: Voltage) -> Self {
-    where
+        Self {
-        Self: From<T>,
+            vector: SVector::from_column_slice(&[voltage.value]),
-    {
+        }
-        Self::from(args)
     }
 
     pub fn voltage(&self) -> Voltage {
@@ -23,12 +22,4 @@ impl VoltageInput {
     }
 }
 
-impl From<Voltage> for VoltageInput {
-    fn from(voltage: Voltage) -> Self {
-        Self {
-            vector: SVector::from_column_slice(&[voltage.value]),
-        }
-    }
-}
-
 impl_has_vector!(VoltageInput, 1);

crates/lyapunov/src/states/current.rs

@@ -13,29 +13,12 @@ pub struct CurrentState {
 }
 
 impl CurrentState {
-    pub fn new<T>(args: T) -> Self
+    pub fn new(current: ElectricCurrent) -> Self {
-    where
-        Self: From<T>,
-    {
-        Self::from(args)
-    }
-
-    pub fn zero() -> Self {
-        Self {
-            vector: SVector::zeros(),
-        }
-    }
-}
-
-impl From<ElectricCurrent> for CurrentState {
-    fn from(current: ElectricCurrent) -> Self {
         Self {
             vector: SVector::from_column_slice(&[current.value]),
         }
     }
-}
 
-impl CurrentState {
     pub fn current(&self) -> ElectricCurrent {
         ElectricCurrent::new::<ampere>(self.vector[0])
     }
@@ -49,12 +32,6 @@ impl Accumulate<CurrentRateState> for CurrentState {
     }
 }
 
-impl Default for CurrentState {
-    fn default() -> Self {
-        Self::zero()
-    }
-}
-
 impl_has_vector!(CurrentState, 1);
 
 impl TimeDerivative for CurrentState {

crates/lyapunov/src/states/current_rate.rs

@@ -14,25 +14,11 @@ impl CurrentRateState {
         }
     }
 
-    pub fn zero() -> Self {
-        Self {
-            vector: SVector::zeros(),
-        }
-    }
-}
-
-impl CurrentRateState {
     pub fn rate(&self) -> CurrentRate {
         ElectricCurrent::new::<ampere>(self.vector[0]) / Time::new::<second>(1.0)
     }
 }
 
-impl Default for CurrentRateState {
-    fn default() -> Self {
-        Self::zero()
-    }
-}
-
 impl From<SVector<f64, 1>> for CurrentRateState {
     fn from(vector: SVector<f64, 1>) -> Self {
         Self { vector }

crates/lyapunov/src/states/omega_alpha.rs

@@ -8,31 +8,9 @@ pub struct OmegaAlphaState {
 }
 
 impl OmegaAlphaState {
-    pub fn new<T>(args: T) -> Self
+    pub fn new(
-    where
+        angular_velocity: AngularVelocity,
-        Self: From<T>,
+        angular_acceleration: AngularAcceleration,
-    {
-        Self::from(args)
-    }
-
-    pub fn zero() -> Self {
-        Self {
-            vector: SVector::zeros(),
-        }
-    }
-}
-
-impl From<AngularVelocity> for OmegaAlphaState {
-    fn from(angular_velocity: AngularVelocity) -> Self {
-        Self {
-            vector: SVector::from_column_slice(&[angular_velocity.value, 0.0]),
-        }
-    }
-}
-
-impl From<(AngularVelocity, AngularAcceleration)> for OmegaAlphaState {
-    fn from(
-        (angular_velocity, angular_acceleration): (AngularVelocity, AngularAcceleration),
     ) -> Self {
         Self {
             vector: SVector::from_column_slice(&[
@@ -41,9 +19,7 @@ impl From<(AngularVelocity, AngularAcceleration)> for OmegaAlphaState {
             ]),
         }
     }
-}
 
-impl OmegaAlphaState {
     pub fn angular_velocity(&self) -> AngularVelocity {
         AngularVelocity::new::<radian_per_second>(self.vector[0])
     }
@@ -53,12 +29,6 @@ impl OmegaAlphaState {
     }
 }
 
-impl Default for OmegaAlphaState {
-    fn default() -> Self {
-        Self::zero()
-    }
-}
-
 impl From<SVector<f64, 2>> for OmegaAlphaState {
     fn from(vector: SVector<f64, 2>) -> Self {
         Self { vector }

crates/lyapunov/src/states/position_velocity.rs

@@ -13,37 +13,12 @@ pub struct PositionVelocityState {
 }
 
 impl PositionVelocityState {
-    pub fn new<T>(args: T) -> Self
+    pub fn new(position: Length, velocity: Velocity) -> Self {
-    where
-        Self: From<T>,
-    {
-        Self::from(args)
-    }
-
-    pub fn zero() -> Self {
-        Self {
-            vector: SVector::zeros(),
-        }
-    }
-}
-
-impl From<Length> for PositionVelocityState {
-    fn from(position: Length) -> Self {
-        Self {
-            vector: SVector::from_column_slice(&[position.value, 0.0]),
-        }
-    }
-}
-
-impl From<(Length, Velocity)> for PositionVelocityState {
-    fn from((position, velocity): (Length, Velocity)) -> Self {
         Self {
             vector: SVector::from_column_slice(&[position.value, velocity.value]),
         }
     }
-}
 
-impl PositionVelocityState {
     pub fn position(&self) -> Length {
         Length::new::<meter>(self.vector[0])
     }
@@ -69,12 +44,6 @@ impl Accumulate<VelocityAccelerationState> for PositionVelocityState {
     }
 }
 
-impl Default for PositionVelocityState {
-    fn default() -> Self {
-        Self::zero()
-    }
-}
-
 impl_has_vector!(PositionVelocityState, 2);
 
 impl TimeDerivative for PositionVelocityState {

crates/lyapunov/src/states/theta_omega.rs

@@ -13,37 +13,12 @@ pub struct ThetaOmegaState {
 }
 
 impl ThetaOmegaState {
-    pub fn new<T>(args: T) -> Self
+    pub fn new(angle: Angle, angular_velocity: AngularVelocity) -> Self {
-    where
-        Self: From<T>,
-    {
-        Self::from(args)
-    }
-
-    pub fn zero() -> Self {
-        Self {
-            vector: SVector::zeros(),
-        }
-    }
-}
-
-impl From<Angle> for ThetaOmegaState {
-    fn from(angle: Angle) -> Self {
-        Self {
-            vector: SVector::from_column_slice(&[angle.value, 0.0]),
-        }
-    }
-}
-
-impl From<(Angle, AngularVelocity)> for ThetaOmegaState {
-    fn from((angle, angular_velocity): (Angle, AngularVelocity)) -> Self {
         Self {
             vector: SVector::from_column_slice(&[angle.value, angular_velocity.value]),
         }
     }
-}
 
-impl ThetaOmegaState {
     pub fn angle(&self) -> Angle {
         Angle::new::<radian>(self.vector[0])
     }
@@ -69,12 +44,6 @@ impl Accumulate<OmegaAlphaState> for ThetaOmegaState {
     }
 }
 
-impl Default for ThetaOmegaState {
-    fn default() -> Self {
-        Self::zero()
-    }
-}
-
 impl_has_vector!(ThetaOmegaState, 2);
 
 impl TimeDerivative for ThetaOmegaState {

crates/lyapunov/src/states/velocity_acceleration.rs

@@ -8,37 +8,12 @@ pub struct VelocityAccelerationState {
 }
 
 impl VelocityAccelerationState {
-    pub fn new<T>(args: T) -> Self
+    pub fn new(velocity: Velocity, acceleration: Acceleration) -> Self {
-    where
-        Self: From<T>,
-    {
-        Self::from(args)
-    }
-
-    pub fn zero() -> Self {
-        Self {
-            vector: SVector::zeros(),
-        }
-    }
-}
-
-impl From<Velocity> for VelocityAccelerationState {
-    fn from(velocity: Velocity) -> Self {
-        Self {
-            vector: SVector::from_column_slice(&[velocity.value, 0.0]),
-        }
-    }
-}
-
-impl From<(Velocity, Acceleration)> for VelocityAccelerationState {
-    fn from((velocity, acceleration): (Velocity, Acceleration)) -> Self {
         Self {
             vector: SVector::from_column_slice(&[velocity.value, acceleration.value]),
         }
     }
-}
 
-impl VelocityAccelerationState {
     pub fn velocity(&self) -> Velocity {
         Velocity::new::<meter_per_second>(self.vector[0])
     }
@@ -48,12 +23,6 @@ impl VelocityAccelerationState {
     }
 }
 
-impl Default for VelocityAccelerationState {
-    fn default() -> Self {
-        Self::zero()
-    }
-}
-
 impl From<SVector<f64, 2>> for VelocityAccelerationState {
     fn from(vector: SVector<f64, 2>) -> Self {
         Self { vector }

crates/lyapunov/src/systems/dc_motor.rs

@@ -40,74 +40,6 @@ impl DCMotor {
             .clamp(-self.max_current.value, self.max_current.value);
         ElectricCurrent::new::<ampere>(clamped)
     }
-
-    pub fn qube_servo_2() -> Self {
-        Self::new(
-            ElectricalResistance::new::<ohm>(8.4),
-            Inductance::new::<henry>(0.00116),
-            Torque::new::<newton_meter>(0.042) / ElectricCurrent::new::<ampere>(1.0),
-            ElectricCurrent::new::<ampere>(2.0),
-            Voltage::new::<volt>(10.0),
-        )
-    }
-
-    pub fn kraken_x60() -> Self {
-        Self::new(
-            ElectricalResistance::new::<ohm>(0.0328),
-            Inductance::new::<henry>(0.0001),
-            Torque::new::<newton_meter>(0.0194) / ElectricCurrent::new::<ampere>(1.0),
-            ElectricCurrent::new::<ampere>(366.0),
-            Voltage::new::<volt>(12.0),
-        )
-    }
-
-    pub fn kraken_x44() -> Self {
-        Self::new(
-            ElectricalResistance::new::<ohm>(0.0430),
-            Inductance::new::<henry>(0.0001),
-            Torque::new::<newton_meter>(0.0147) / ElectricCurrent::new::<ampere>(1.0),
-            ElectricCurrent::new::<ampere>(279.0),
-            Voltage::new::<volt>(12.0),
-        )
-    }
-
-    pub fn neo() -> Self {
-        Self::new(
-            ElectricalResistance::new::<ohm>(0.114),
-            Inductance::new::<henry>(0.0001),
-            Torque::new::<newton_meter>(0.0248) / ElectricCurrent::new::<ampere>(1.0),
-            ElectricCurrent::new::<ampere>(105.0),
-            Voltage::new::<volt>(12.0),
-        )
-    }
-
-    pub fn gearbox(motors: &[Self]) -> Self {
-        assert!(!motors.is_empty(), "gearbox requires at least one motor");
-        let mut inv_r = 0.0;
-        let mut inv_l = 0.0;
-        let mut kt = 0.0;
-        let mut i_max = 0.0;
-        for m in motors {
-            inv_r += 1.0 / m.resistance.value;
-            inv_l += 1.0 / m.inductance.value;
-            kt += m.torque_constant.value;
-            i_max += m.max_current.value;
-        }
-        let r = 1.0 / inv_r;
-        let l = 1.0 / inv_l;
-        let v = motors
-            .iter()
-            .map(|m| m.nominal_voltage.value)
-            .fold(f64::INFINITY, f64::min);
-        let v = Voltage::new::<volt>(v);
-        Self::new(
-            ElectricalResistance::new::<ohm>(r),
-            Inductance::new::<henry>(l),
-            Torque::new::<newton_meter>(kt) / ElectricCurrent::new::<ampere>(1.0),
-            ElectricCurrent::new::<ampere>(i_max),
-            v,
-        )
-    }
 }
 
 impl System<1, 1> for DCMotor {
@@ -183,10 +115,10 @@ impl System<2, 1> for DCMotorPosition {
         // Angular acceleration.
         let alpha = (torque - (self.friction * omega).value) / self.inertia.value;
 
-        OmegaAlphaState::new((
+        OmegaAlphaState::new(
-            omega,
+            AngularVelocity::new::<radian_per_second>(omega.value),
             AngularAcceleration::new::<radian_per_second_squared>(alpha),
-        ))
+        )
     }
 }
 

crates/lyapunov/src/systems/pendulum.rs

@@ -2,10 +2,8 @@ use crate::units::*;
 
 use crate::inputs::CurrentInput;
 use crate::states::{OmegaAlphaState, ThetaOmegaState};
-use crate::systems::LinearSystem;
 use crate::systems::dc_motor::DCMotor;
 use crate::traits::{Saturate, System, TimeDerivative};
-use nalgebra::{SMatrix, SVector};
 
 #[derive(Clone, Copy, Debug)]
 pub struct Pendulum {
@@ -32,35 +30,6 @@ impl Pendulum {
             motor,
         }
     }
-
-    pub fn gravity_cancelling_input(&self, x: &ThetaOmegaState) -> CurrentInput {
-        let theta = x.angle().value;
-        let current = self.mass.value * self.gravity.value * self.length.value * theta.sin()
-            / self.motor.torque_constant.value;
-        CurrentInput::new(ElectricCurrent::new::<ampere>(current))
-    }
-
-    pub fn linearize(&self) -> LinearSystem<2, 1> {
-        let input_gain = self.motor.torque_constant.value / self.inertia.value;
-        LinearSystem {
-            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>::zeros(),
-        }
-    }
-
-    pub fn local_linearize(&self, reference: &ThetaOmegaState) -> LinearSystem<2, 1> {
-        let theta0 = reference.angle().value;
-        let cos = theta0.cos();
-        let input_gain = self.motor.torque_constant.value / self.inertia.value;
-        let stiffness =
-            -(self.mass.value * self.gravity.value * self.length.value / self.inertia.value) * cos;
-        LinearSystem {
-            state_matrix: SMatrix::<f64, 2, 2>::from_row_slice(&[0.0, 1.0, stiffness, 0.0]),
-            input_matrix: SMatrix::<f64, 2, 1>::from_column_slice(&[0.0, input_gain]),
-            constant: SVector::<f64, 2>::zeros(),
-        }
-    }
 }
 
 impl System<2, 1> for Pendulum {
@@ -85,10 +54,10 @@ impl System<2, 1> for Pendulum {
             - self.mass.value * self.gravity.value * self.length.value * theta.sin())
             / self.inertia.value;
 
-        OmegaAlphaState::new((
+        OmegaAlphaState::new(
             AngularVelocity::new::<radian_per_second>(omega),
             AngularAcceleration::new::<radian_per_second_squared>(alpha),
-        ))
+        )
     }
 }