From c8b15c949fe69654c2c9b5b74a68572702e366c5 Mon Sep 17 00:00:00 2001 From: Hayden Heroux Date: Fri, 30 Oct 2026 19:06:15 -0400 Subject: [PATCH] Add flexible constructors for states and inputs. --- crates/lyapunov-demo/src/main.rs | 28 ++----- .../src/controllers/linear_feedback.rs | 61 +++++++++++++++ crates/lyapunov/src/inputs/acceleration.rs | 19 +++-- crates/lyapunov/src/inputs/current.rs | 17 ++++- crates/lyapunov/src/inputs/voltage.rs | 17 ++++- crates/lyapunov/src/states/current.rs | 25 ++++++- crates/lyapunov/src/states/current_rate.rs | 14 ++++ crates/lyapunov/src/states/omega_alpha.rs | 36 ++++++++- .../lyapunov/src/states/position_velocity.rs | 33 ++++++++- crates/lyapunov/src/states/theta_omega.rs | 33 ++++++++- .../src/states/velocity_acceleration.rs | 33 ++++++++- crates/lyapunov/src/systems/dc_motor.rs | 74 ++++++++++++++++++- crates/lyapunov/src/systems/pendulum.rs | 28 ++++++- 13 files changed, 371 insertions(+), 47 deletions(-) diff --git a/crates/lyapunov-demo/src/main.rs b/crates/lyapunov-demo/src/main.rs index e5f863d..f806946 100644 --- a/crates/lyapunov-demo/src/main.rs +++ b/crates/lyapunov-demo/src/main.rs @@ -4,42 +4,26 @@ use lyapunov::units::*; use lyapunov::{Controller, DCMotor, Integrator, LinearFeedback, Pendulum, Rk4}; fn main() { - let qube_servo_2 = DCMotor::new( - ElectricalResistance::new::(8.4), - Inductance::new::(0.00116), - lyapunov::TorqueConstant::from( - Torque::new::(0.042) / ElectricCurrent::new::(1.0), - ), - ElectricCurrent::new::(2.0), - Voltage::new::(10.0), - ); - let system = Pendulum::new( Mass::new::(0.024), Length::new::(0.129), MomentOfInertia::new::(3.33 * 0.00001), Acceleration::new::(9.81), - qube_servo_2, + DCMotor::qube_servo_2(), ); - // Gains are in A/rad and A/(rad/s). - let feedback = LinearFeedback::<2, 1>::from_gains([0.02, 0.008]); + let r = ThetaOmegaState::new(Angle::new::(std::f64::consts::PI)); + + let feedback = LinearFeedback::<2, 1>::pendulum(&system); let rk4 = Rk4; // Start at the downward (stable) equilibrium to demonstrate swing-up. - let mut x = ThetaOmegaState::new( - Angle::new::(0.0), - AngularVelocity::new::(0.0), - ); - let r = ThetaOmegaState::new( - Angle::new::(3.14159), - AngularVelocity::new::(0.0), - ); + let mut x = ThetaOmegaState::zero(); let dt = Time::new::(10.0); - for step in 0..1_000 { + for step in 0..350 { let t = dt * (step as f64); let u_fb = feedback.calculate(&system, t, &x, &r); diff --git a/crates/lyapunov/src/controllers/linear_feedback.rs b/crates/lyapunov/src/controllers/linear_feedback.rs index f461e1f..4b16937 100644 --- a/crates/lyapunov/src/controllers/linear_feedback.rs +++ b/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 { @@ -30,6 +32,65 @@ impl LinearFeedback { } } +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::(); + Self::from_linear_system_with(system, omega_n, 1.0) + } +} + impl LinearFeedback { pub fn diagonal(gains: [f64; N]) -> Self { let mut gain_matrix = SMatrix::::zeros(); diff --git a/crates/lyapunov/src/inputs/acceleration.rs b/crates/lyapunov/src/inputs/acceleration.rs index a968436..4ca5417 100644 --- a/crates/lyapunov/src/inputs/acceleration.rs +++ b/crates/lyapunov/src/inputs/acceleration.rs @@ -10,10 +10,11 @@ pub struct AccelerationInput { } impl AccelerationInput { - pub fn new(acceleration: Acceleration) -> Self { - Self { - vector: SVector::from_column_slice(&[acceleration.value]), - } + pub fn new(args: T) -> Self + where + Self: From, + { + Self::from(args) } pub fn acceleration(&self) -> Acceleration { @@ -21,7 +22,15 @@ impl AccelerationInput { } pub fn zero() -> Self { - Self::new(Acceleration::new::(0.0)) + Self::from(Acceleration::new::(0.0)) + } +} + +impl From for AccelerationInput { + fn from(acceleration: Acceleration) -> Self { + Self { + vector: SVector::from_column_slice(&[acceleration.value]), + } } } diff --git a/crates/lyapunov/src/inputs/current.rs b/crates/lyapunov/src/inputs/current.rs index dadd6e5..34aaaec 100644 --- a/crates/lyapunov/src/inputs/current.rs +++ b/crates/lyapunov/src/inputs/current.rs @@ -10,10 +10,11 @@ pub struct CurrentInput { } impl CurrentInput { - pub fn new(current: ElectricCurrent) -> Self { - Self { - vector: SVector::from_column_slice(&[current.value]), - } + pub fn new(args: T) -> Self + where + Self: From, + { + Self::from(args) } pub fn current(&self) -> ElectricCurrent { @@ -21,4 +22,12 @@ impl CurrentInput { } } +impl From for CurrentInput { + fn from(current: ElectricCurrent) -> Self { + Self { + vector: SVector::from_column_slice(&[current.value]), + } + } +} + impl_has_vector!(CurrentInput, 1); diff --git a/crates/lyapunov/src/inputs/voltage.rs b/crates/lyapunov/src/inputs/voltage.rs index b10404b..f87aacd 100644 --- a/crates/lyapunov/src/inputs/voltage.rs +++ b/crates/lyapunov/src/inputs/voltage.rs @@ -11,10 +11,11 @@ pub struct VoltageInput { } impl VoltageInput { - pub fn new(voltage: Voltage) -> Self { - Self { - vector: SVector::from_column_slice(&[voltage.value]), - } + pub fn new(args: T) -> Self + where + Self: From, + { + Self::from(args) } pub fn voltage(&self) -> Voltage { @@ -22,4 +23,12 @@ impl VoltageInput { } } +impl From for VoltageInput { + fn from(voltage: Voltage) -> Self { + Self { + vector: SVector::from_column_slice(&[voltage.value]), + } + } +} + impl_has_vector!(VoltageInput, 1); diff --git a/crates/lyapunov/src/states/current.rs b/crates/lyapunov/src/states/current.rs index 63abdf8..ca0bd81 100644 --- a/crates/lyapunov/src/states/current.rs +++ b/crates/lyapunov/src/states/current.rs @@ -13,12 +13,29 @@ pub struct CurrentState { } impl CurrentState { - pub fn new(current: ElectricCurrent) -> Self { + pub fn new(args: T) -> Self + where + Self: From, + { + Self::from(args) + } + + pub fn zero() -> Self { + Self { + vector: SVector::zeros(), + } + } +} + +impl From for CurrentState { + fn from(current: ElectricCurrent) -> Self { Self { vector: SVector::from_column_slice(&[current.value]), } } +} +impl CurrentState { pub fn current(&self) -> ElectricCurrent { ElectricCurrent::new::(self.vector[0]) } @@ -32,6 +49,12 @@ impl Accumulate for CurrentState { } } +impl Default for CurrentState { + fn default() -> Self { + Self::zero() + } +} + impl_has_vector!(CurrentState, 1); impl TimeDerivative for CurrentState { diff --git a/crates/lyapunov/src/states/current_rate.rs b/crates/lyapunov/src/states/current_rate.rs index 1cf2fa0..1462b6d 100644 --- a/crates/lyapunov/src/states/current_rate.rs +++ b/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::(self.vector[0]) / Time::new::(1.0) } } +impl Default for CurrentRateState { + fn default() -> Self { + Self::zero() + } +} + impl From> for CurrentRateState { fn from(vector: SVector) -> Self { Self { vector } diff --git a/crates/lyapunov/src/states/omega_alpha.rs b/crates/lyapunov/src/states/omega_alpha.rs index b50d819..a1023f3 100644 --- a/crates/lyapunov/src/states/omega_alpha.rs +++ b/crates/lyapunov/src/states/omega_alpha.rs @@ -8,9 +8,31 @@ pub struct OmegaAlphaState { } impl OmegaAlphaState { - pub fn new( - angular_velocity: AngularVelocity, - angular_acceleration: AngularAcceleration, + pub fn new(args: T) -> Self + where + Self: From, + { + Self::from(args) + } + + pub fn zero() -> Self { + Self { + vector: SVector::zeros(), + } + } +} + +impl From 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::(self.vector[0]) } @@ -29,6 +53,12 @@ impl OmegaAlphaState { } } +impl Default for OmegaAlphaState { + fn default() -> Self { + Self::zero() + } +} + impl From> for OmegaAlphaState { fn from(vector: SVector) -> Self { Self { vector } diff --git a/crates/lyapunov/src/states/position_velocity.rs b/crates/lyapunov/src/states/position_velocity.rs index 460aa5c..bdba60b 100644 --- a/crates/lyapunov/src/states/position_velocity.rs +++ b/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(args: T) -> Self + where + Self: From, + { + Self::from(args) + } + + pub fn zero() -> Self { + Self { + vector: SVector::zeros(), + } + } +} + +impl From 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::(self.vector[0]) } @@ -44,6 +69,12 @@ impl Accumulate for PositionVelocityState { } } +impl Default for PositionVelocityState { + fn default() -> Self { + Self::zero() + } +} + impl_has_vector!(PositionVelocityState, 2); impl TimeDerivative for PositionVelocityState { diff --git a/crates/lyapunov/src/states/theta_omega.rs b/crates/lyapunov/src/states/theta_omega.rs index 3aac5e5..8b787e5 100644 --- a/crates/lyapunov/src/states/theta_omega.rs +++ b/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(args: T) -> Self + where + Self: From, + { + Self::from(args) + } + + pub fn zero() -> Self { + Self { + vector: SVector::zeros(), + } + } +} + +impl From 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::(self.vector[0]) } @@ -44,6 +69,12 @@ impl Accumulate for ThetaOmegaState { } } +impl Default for ThetaOmegaState { + fn default() -> Self { + Self::zero() + } +} + impl_has_vector!(ThetaOmegaState, 2); impl TimeDerivative for ThetaOmegaState { diff --git a/crates/lyapunov/src/states/velocity_acceleration.rs b/crates/lyapunov/src/states/velocity_acceleration.rs index 925601a..a999e80 100644 --- a/crates/lyapunov/src/states/velocity_acceleration.rs +++ b/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(args: T) -> Self + where + Self: From, + { + Self::from(args) + } + + pub fn zero() -> Self { + Self { + vector: SVector::zeros(), + } + } +} + +impl From 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::(self.vector[0]) } @@ -23,6 +48,12 @@ impl VelocityAccelerationState { } } +impl Default for VelocityAccelerationState { + fn default() -> Self { + Self::zero() + } +} + impl From> for VelocityAccelerationState { fn from(vector: SVector) -> Self { Self { vector } diff --git a/crates/lyapunov/src/systems/dc_motor.rs b/crates/lyapunov/src/systems/dc_motor.rs index 216b2c1..5e71ff0 100644 --- a/crates/lyapunov/src/systems/dc_motor.rs +++ b/crates/lyapunov/src/systems/dc_motor.rs @@ -40,6 +40,74 @@ impl DCMotor { .clamp(-self.max_current.value, self.max_current.value); ElectricCurrent::new::(clamped) } + + pub fn qube_servo_2() -> Self { + Self::new( + ElectricalResistance::new::(8.4), + Inductance::new::(0.00116), + Torque::new::(0.042) / ElectricCurrent::new::(1.0), + ElectricCurrent::new::(2.0), + Voltage::new::(10.0), + ) + } + + pub fn kraken_x60() -> Self { + Self::new( + ElectricalResistance::new::(0.0328), + Inductance::new::(0.0001), + Torque::new::(0.0194) / ElectricCurrent::new::(1.0), + ElectricCurrent::new::(366.0), + Voltage::new::(12.0), + ) + } + + pub fn kraken_x44() -> Self { + Self::new( + ElectricalResistance::new::(0.0430), + Inductance::new::(0.0001), + Torque::new::(0.0147) / ElectricCurrent::new::(1.0), + ElectricCurrent::new::(279.0), + Voltage::new::(12.0), + ) + } + + pub fn neo() -> Self { + Self::new( + ElectricalResistance::new::(0.114), + Inductance::new::(0.0001), + Torque::new::(0.0248) / ElectricCurrent::new::(1.0), + ElectricCurrent::new::(105.0), + Voltage::new::(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::(v); + Self::new( + ElectricalResistance::new::(r), + Inductance::new::(l), + Torque::new::(kt) / ElectricCurrent::new::(1.0), + ElectricCurrent::new::(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( - AngularVelocity::new::(omega.value), + OmegaAlphaState::new(( + omega, AngularAcceleration::new::(alpha), - ) + )) } } diff --git a/crates/lyapunov/src/systems/pendulum.rs b/crates/lyapunov/src/systems/pendulum.rs index 47aa800..d3d9bba 100644 --- a/crates/lyapunov/src/systems/pendulum.rs +++ b/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 { @@ -37,6 +39,28 @@ impl Pendulum { / self.motor.torque_constant.value; CurrentInput::new(ElectricCurrent::new::(current)) } + + pub fn linearize(&self) -> LinearSystem<2, 1> { + let input_gain = self.motor.torque_constant.value / self.inertia.value; + LinearSystem { + 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::::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::::from_row_slice(&[0.0, 1.0, stiffness, 0.0]), + input_matrix: SMatrix::::from_column_slice(&[0.0, input_gain]), + constant: SVector::::zeros(), + } + } } impl System<2, 1> for Pendulum { @@ -61,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::(omega), AngularAcceleration::new::(alpha), - ) + )) } }