Add flexible constructors for states and inputs.

crates/lyapunov-demo/src/main.rs

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

crates/lyapunov/src/controllers/linear_feedback.rs

@@ -1,6 +1,8 @@
 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> {
@@ -30,6 +32,65 @@ 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,10 +10,11 @@ pub struct AccelerationInput {
 }
 
 impl AccelerationInput {
-    pub fn new(acceleration: Acceleration) -> Self {
+    pub fn new<T>(args: T) -> Self
-        Self {
+    where
-            vector: SVector::from_column_slice(&[acceleration.value]),
+        Self: From<T>,
-        }
+    {
+        Self::from(args)
     }
 
     pub fn acceleration(&self) -> Acceleration {
@@ -21,7 +22,15 @@ impl AccelerationInput {
     }
 
     pub fn zero() -> Self {
-        Self::new(Acceleration::new::<meter_per_second_squared>(0.0))
+        Self::from(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,10 +10,11 @@ pub struct CurrentInput {
 }
 
 impl CurrentInput {
-    pub fn new(current: ElectricCurrent) -> Self {
+    pub fn new<T>(args: T) -> Self
-        Self {
+    where
-            vector: SVector::from_column_slice(&[current.value]),
+        Self: From<T>,
-        }
+    {
+        Self::from(args)
     }
 
     pub fn current(&self) -> ElectricCurrent {
@@ -21,4 +22,12 @@ 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,10 +11,11 @@ pub struct VoltageInput {
 }
 
 impl VoltageInput {
-    pub fn new(voltage: Voltage) -> Self {
+    pub fn new<T>(args: T) -> Self
-        Self {
+    where
-            vector: SVector::from_column_slice(&[voltage.value]),
+        Self: From<T>,
-        }
+    {
+        Self::from(args)
     }
 
     pub fn voltage(&self) -> Voltage {
@@ -22,4 +23,12 @@ 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,12 +13,29 @@ pub struct CurrentState {
 }
 
 impl CurrentState {
-    pub fn new(current: ElectricCurrent) -> Self {
+    pub fn new<T>(args: T) -> 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])
     }
@@ -32,6 +49,12 @@ 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,11 +14,25 @@ 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,9 +8,31 @@ pub struct OmegaAlphaState {
 }
 
 impl OmegaAlphaState {
-    pub fn new(
+    pub fn new<T>(args: T) -> Self
-        angular_velocity: AngularVelocity,
+    where
-        angular_acceleration: AngularAcceleration,
+        Self: From<T>,
+    {
+        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(&[
@@ -19,7 +41,9 @@ impl OmegaAlphaState {
             ]),
         }
     }
+}
 
+impl OmegaAlphaState {
     pub fn angular_velocity(&self) -> AngularVelocity {
         AngularVelocity::new::<radian_per_second>(self.vector[0])
     }
@@ -29,6 +53,12 @@ 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,12 +13,37 @@ pub struct PositionVelocityState {
 }
 
 impl PositionVelocityState {
-    pub fn new(position: Length, velocity: Velocity) -> Self {
+    pub fn new<T>(args: T) -> 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])
     }
@@ -44,6 +69,12 @@ 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,12 +13,37 @@ pub struct ThetaOmegaState {
 }
 
 impl ThetaOmegaState {
-    pub fn new(angle: Angle, angular_velocity: AngularVelocity) -> Self {
+    pub fn new<T>(args: T) -> 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])
     }
@@ -44,6 +69,12 @@ 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,12 +8,37 @@ pub struct VelocityAccelerationState {
 }
 
 impl VelocityAccelerationState {
-    pub fn new(velocity: Velocity, acceleration: Acceleration) -> Self {
+    pub fn new<T>(args: T) -> 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])
     }
@@ -23,6 +48,12 @@ 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,6 +40,74 @@ 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 {
@@ -115,10 +183,10 @@ impl System<2, 1> for DCMotorPosition {
         // Angular acceleration.
         let alpha = (torque - (self.friction * omega).value) / self.inertia.value;
 
-        OmegaAlphaState::new(
+        OmegaAlphaState::new((
-            AngularVelocity::new::<radian_per_second>(omega.value),
+            omega,
             AngularAcceleration::new::<radian_per_second_squared>(alpha),
-        )
+        ))
     }
 }
 

crates/lyapunov/src/systems/pendulum.rs

@@ -2,8 +2,10 @@ 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 {
@@ -30,6 +32,35 @@ 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 {
@@ -54,10 +85,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),
-        )
+        ))
     }
 }