From cae2d2fb45a07e00610fea9541299dbcf47448c2 Mon Sep 17 00:00:00 2001 From: Hayden Heroux Date: Thu, 29 Oct 2026 16:47:07 -0400 Subject: [PATCH] Add pendulum demo. --- crates/lyapunov-demo/src/main.rs | 76 +++++---- .../src/controllers/linear_feedback.rs | 10 +- crates/lyapunov/src/inputs/acceleration.rs | 28 ++++ .../lyapunov/src/inputs/acceleration_input.rs | 68 -------- crates/lyapunov/src/inputs/current.rs | 24 +++ crates/lyapunov/src/inputs/mod.rs | 8 +- crates/lyapunov/src/inputs/voltage.rs | 25 +++ crates/lyapunov/src/integrators/rk4.rs | 6 +- crates/lyapunov/src/integrators/traits.rs | 4 +- crates/lyapunov/src/lib.rs | 15 +- crates/lyapunov/src/states/current.rs | 39 +++++ crates/lyapunov/src/states/current_rate.rs | 32 ++++ crates/lyapunov/src/states/mod.rs | 8 + crates/lyapunov/src/states/omega_alpha.rs | 42 +++++ .../lyapunov/src/states/position_velocity.rs | 61 +------ crates/lyapunov/src/states/theta_omega.rs | 51 ++++++ .../src/states/velocity_acceleration.rs | 6 +- crates/lyapunov/src/systems/dc_motor.rs | 149 ++++++++++++++++++ crates/lyapunov/src/systems/linear_system.rs | 20 ++- crates/lyapunov/src/systems/mod.rs | 4 + crates/lyapunov/src/systems/pendulum.rs | 68 ++++++++ crates/lyapunov/src/traits.rs | 117 +++++++++++--- crates/lyapunov/src/units.rs | 26 +++ 23 files changed, 690 insertions(+), 197 deletions(-) create mode 100644 crates/lyapunov/src/inputs/acceleration.rs delete mode 100644 crates/lyapunov/src/inputs/acceleration_input.rs create mode 100644 crates/lyapunov/src/inputs/current.rs create mode 100644 crates/lyapunov/src/inputs/voltage.rs create mode 100644 crates/lyapunov/src/states/current.rs create mode 100644 crates/lyapunov/src/states/current_rate.rs create mode 100644 crates/lyapunov/src/states/omega_alpha.rs create mode 100644 crates/lyapunov/src/states/theta_omega.rs create mode 100644 crates/lyapunov/src/systems/dc_motor.rs create mode 100644 crates/lyapunov/src/systems/pendulum.rs create mode 100644 crates/lyapunov/src/units.rs diff --git a/crates/lyapunov-demo/src/main.rs b/crates/lyapunov-demo/src/main.rs index 118dfb2..5d9adcd 100644 --- a/crates/lyapunov-demo/src/main.rs +++ b/crates/lyapunov-demo/src/main.rs @@ -1,45 +1,57 @@ -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; +use lyapunov::states::ThetaOmegaState; +use lyapunov::traits::Saturate; +use lyapunov::units::*; +use lyapunov::{Controller, DCMotor, Integrator, LinearFeedback, Pendulum, Rk4}; fn main() { - let system = DoubleIntegrator::new(1.0, Acceleration::new::(-0.1)); - - let u_ff = system - .constant_cancelling_input() - .unwrap_or_else(AccelerationInput::zero); - - let p = LinearFeedback::<2, 1>::from_gains([2.0, 3.0]); - - let mut x = PositionVelocityState::new( - Length::new::(10.0), - Velocity::new::(0.0), - ); - let r = PositionVelocityState::new( - Length::new::(0.0), - Velocity::new::(0.0), + let motor = DCMotor::new( + ElectricalResistance::new::(1.0), + Inductance::new::(0.5), + lyapunov::TorqueConstant::from( + Torque::new::(0.5) / ElectricCurrent::new::(1.0), + ), + ElectricCurrent::new::(2.0), + ElectricPotential::new::(12.0), ); - let dt = Time::new::(50.0); + let system = Pendulum::new( + Mass::new::(1.0), + Length::new::(1.0), + MomentOfInertia::new::(1.0), + Acceleration::new::(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::(0.5), + AngularVelocity::new::(0.0), + ); + let r = ThetaOmegaState::new( + Angle::new::(0.0), + AngularVelocity::new::(0.0), + ); + + let dt = Time::new::(10.0); + + for step in 0..300 { let t = dt * (step as f64); - let u_fb = p.calculate(&system, t, &x, &r); - let u = u_fb + u_ff; - x = Rk4.step(&system, t, &x, &u, dt); + let u_fb = feedback.calculate(&system, t, &x, &r); + let u = u_fb.saturate(&system); + 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.position(), - x.velocity(), + x.angle(), + x.angular_velocity(), + u.current(), ); } } diff --git a/crates/lyapunov/src/controllers/linear_feedback.rs b/crates/lyapunov/src/controllers/linear_feedback.rs index f686155..f461e1f 100644 --- a/crates/lyapunov/src/controllers/linear_feedback.rs +++ b/crates/lyapunov/src/controllers/linear_feedback.rs @@ -1,7 +1,7 @@ -use nalgebra::{SMatrix, SVector}; -use uom::si::f64::Time; +use crate::units::*; +use nalgebra::SMatrix; -use crate::traits::{Controller, System, VectorState}; +use crate::traits::{Controller, System, VectorValue}; pub struct LinearFeedback { pub gain_matrix: SMatrix, @@ -10,8 +10,8 @@ pub struct LinearFeedback { impl Controller for LinearFeedback where S: System, - S::State: Copy + std::ops::Sub + VectorState, - S::Input: From>, + S::State: VectorValue, + S::Input: VectorValue, { fn calculate(&self, _system: &S, _t: Time, x: &S::State, r: &S::State) -> S::Input { let error = *r - *x; diff --git a/crates/lyapunov/src/inputs/acceleration.rs b/crates/lyapunov/src/inputs/acceleration.rs new file mode 100644 index 0000000..a968436 --- /dev/null +++ b/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, +} + +impl AccelerationInput { + pub fn new(acceleration: Acceleration) -> Self { + Self { + vector: SVector::from_column_slice(&[acceleration.value]), + } + } + + pub fn acceleration(&self) -> Acceleration { + Acceleration::new::(self.vector[0]) + } + + pub fn zero() -> Self { + Self::new(Acceleration::new::(0.0)) + } +} + +impl_has_vector!(AccelerationInput, 1); diff --git a/crates/lyapunov/src/inputs/acceleration_input.rs b/crates/lyapunov/src/inputs/acceleration_input.rs deleted file mode 100644 index 9a02ddd..0000000 --- a/crates/lyapunov/src/inputs/acceleration_input.rs +++ /dev/null @@ -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, -} - -impl AccelerationInput { - pub fn new(acceleration: Acceleration) -> Self { - Self { - vector: SVector::from_column_slice(&[acceleration.value]), - } - } - - pub fn acceleration(&self) -> Acceleration { - Acceleration::new::(self.vector[0]) - } - - pub fn zero() -> Self { - Self::new(Acceleration::new::< - uom::si::acceleration::meter_per_second_squared, - >(0.0)) - } -} - -impl From> for AccelerationInput { - fn from(vector: SVector) -> Self { - Self { vector } - } -} - -impl From for SVector { - fn from(input: AccelerationInput) -> Self { - input.vector - } -} - -impl VectorInput<1> for AccelerationInput { - fn to_vector(&self) -> SVector { - 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() - ) - } -} diff --git a/crates/lyapunov/src/inputs/current.rs b/crates/lyapunov/src/inputs/current.rs new file mode 100644 index 0000000..dadd6e5 --- /dev/null +++ b/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, +} + +impl CurrentInput { + pub fn new(current: ElectricCurrent) -> Self { + Self { + vector: SVector::from_column_slice(&[current.value]), + } + } + + pub fn current(&self) -> ElectricCurrent { + ElectricCurrent::new::(self.vector[0]) + } +} + +impl_has_vector!(CurrentInput, 1); diff --git a/crates/lyapunov/src/inputs/mod.rs b/crates/lyapunov/src/inputs/mod.rs index af297bf..58ffe14 100644 --- a/crates/lyapunov/src/inputs/mod.rs +++ b/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; diff --git a/crates/lyapunov/src/inputs/voltage.rs b/crates/lyapunov/src/inputs/voltage.rs new file mode 100644 index 0000000..b10404b --- /dev/null +++ b/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, +} + +impl VoltageInput { + pub fn new(voltage: Voltage) -> Self { + Self { + vector: SVector::from_column_slice(&[voltage.value]), + } + } + + pub fn voltage(&self) -> Voltage { + Voltage::new::(self.vector[0]) + } +} + +impl_has_vector!(VoltageInput, 1); diff --git a/crates/lyapunov/src/integrators/rk4.rs b/crates/lyapunov/src/integrators/rk4.rs index e87f08b..6f68680 100644 --- a/crates/lyapunov/src/integrators/rk4.rs +++ b/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 Integrator for Rk4 where S: System, - S::State: Clone + Accumulate, + S::State: Clone + TimeDerivative + Accumulate<::Derivative>, { fn step(&self, system: &S, t: Time, x: &S::State, u: &S::Input, dt: Time) -> S::State { let half_dt = dt * 0.5; diff --git a/crates/lyapunov/src/integrators/traits.rs b/crates/lyapunov/src/integrators/traits.rs index 5e77074..a2db9c6 100644 --- a/crates/lyapunov/src/integrators/traits.rs +++ b/crates/lyapunov/src/integrators/traits.rs @@ -1,5 +1,5 @@ +use crate::units::*; use nalgebra::SVector; -use uom::si::f64::Time; pub trait Accumulate { fn add_scaled(&self, dt: Time, derivative: &D) -> Self; @@ -7,6 +7,6 @@ pub trait Accumulate { impl Accumulate> for SVector { fn add_scaled(&self, dt: Time, derivative: &SVector) -> Self { - self + dt.get::() * derivative + self + dt.get::() * derivative } } diff --git a/crates/lyapunov/src/lib.rs b/crates/lyapunov/src/lib.rs index 466af41..572d9d6 100644 --- a/crates/lyapunov/src/lib.rs +++ b/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 states::{PositionVelocityState, VelocityAccelerationState}; -pub use systems::DoubleIntegrator; -pub use traits::VectorInput; +pub use inputs::{AccelerationInput, CurrentInput, VoltageInput}; +pub use states::{ + CurrentRateState, CurrentState, OmegaAlphaState, PositionVelocityState, ThetaOmegaState, + 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}; diff --git a/crates/lyapunov/src/states/current.rs b/crates/lyapunov/src/states/current.rs new file mode 100644 index 0000000..63abdf8 --- /dev/null +++ b/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, +} + +impl CurrentState { + pub fn new(current: ElectricCurrent) -> Self { + Self { + vector: SVector::from_column_slice(&[current.value]), + } + } + + pub fn current(&self) -> ElectricCurrent { + ElectricCurrent::new::(self.vector[0]) + } +} + +impl Accumulate 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; +} diff --git a/crates/lyapunov/src/states/current_rate.rs b/crates/lyapunov/src/states/current_rate.rs new file mode 100644 index 0000000..1cf2fa0 --- /dev/null +++ b/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, +} + +impl CurrentRateState { + pub fn new(rate: CurrentRate) -> Self { + Self { + vector: SVector::from_column_slice(&[rate.value]), + } + } + + pub fn rate(&self) -> CurrentRate { + ElectricCurrent::new::(self.vector[0]) / Time::new::(1.0) + } +} + +impl From> for CurrentRateState { + fn from(vector: SVector) -> Self { + Self { vector } + } +} + +impl From for SVector { + fn from(state: CurrentRateState) -> Self { + state.vector + } +} diff --git a/crates/lyapunov/src/states/mod.rs b/crates/lyapunov/src/states/mod.rs index 8a35011..308131a 100644 --- a/crates/lyapunov/src/states/mod.rs +++ b/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; diff --git a/crates/lyapunov/src/states/omega_alpha.rs b/crates/lyapunov/src/states/omega_alpha.rs new file mode 100644 index 0000000..b50d819 --- /dev/null +++ b/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, +} + +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::(self.vector[0]) + } + + pub fn angular_acceleration(&self) -> AngularAcceleration { + AngularAcceleration::new::(self.vector[1]) + } +} + +impl From> for OmegaAlphaState { + fn from(vector: SVector) -> Self { + Self { vector } + } +} + +impl From for SVector { + fn from(state: OmegaAlphaState) -> Self { + state.vector + } +} diff --git a/crates/lyapunov/src/states/position_velocity.rs b/crates/lyapunov/src/states/position_velocity.rs index 83cf041..460aa5c 100644 --- a/crates/lyapunov/src/states/position_velocity.rs +++ b/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 uom::si::f64::Time; +use crate::traits::TimeDerivative; #[derive(Clone, Copy, PartialEq)] pub struct PositionVelocityState { @@ -21,11 +20,11 @@ impl PositionVelocityState { } pub fn position(&self) -> Length { - Length::new::(self.vector[0]) + Length::new::(self.vector[0]) } pub fn velocity(&self) -> Velocity { - Velocity::new::(self.vector[1]) + Velocity::new::(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> for PositionVelocityState { - fn from(vector: SVector) -> 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 for SVector { - fn from(state: PositionVelocityState) -> Self { - state.vector - } } impl Accumulate for PositionVelocityState { @@ -78,19 +44,8 @@ impl Accumulate for PositionVelocityState { } } -impl VectorState<2> for PositionVelocityState { - fn to_vector(&self) -> SVector { - self.vector - } -} +impl_has_vector!(PositionVelocityState, 2); -impl fmt::Debug for PositionVelocityState { - fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result { - write!( - f, - "PositionVelocityState {{ position: {:?}, velocity: {:?} }}", - self.position(), - self.velocity() - ) - } +impl TimeDerivative for PositionVelocityState { + type Derivative = VelocityAccelerationState; } diff --git a/crates/lyapunov/src/states/theta_omega.rs b/crates/lyapunov/src/states/theta_omega.rs new file mode 100644 index 0000000..3aac5e5 --- /dev/null +++ b/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, +} + +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::(self.vector[0]) + } + + pub fn angular_velocity(&self) -> AngularVelocity { + AngularVelocity::new::(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 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; +} diff --git a/crates/lyapunov/src/states/velocity_acceleration.rs b/crates/lyapunov/src/states/velocity_acceleration.rs index 584637a..925601a 100644 --- a/crates/lyapunov/src/states/velocity_acceleration.rs +++ b/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::(self.vector[0]) + Velocity::new::(self.vector[0]) } pub fn acceleration(&self) -> Acceleration { - Acceleration::new::(self.vector[1]) + Acceleration::new::(self.vector[1]) } } diff --git a/crates/lyapunov/src/systems/dc_motor.rs b/crates/lyapunov/src/systems/dc_motor.rs new file mode 100644 index 0000000..216b2c1 --- /dev/null +++ b/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::((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::(clamped) + } +} + +impl System<1, 1> for DCMotor { + type State = CurrentState; + type Input = VoltageInput; + + fn derivatives( + &self, + _t: Time, + x: &Self::State, + u: &Self::Input, + ) -> ::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::(di_dt) / Time::new::(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, + ) -> ::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::(current)).value; + + // Angular acceleration. + let alpha = (torque - (self.friction * omega).value) / self.inertia.value; + + OmegaAlphaState::new( + AngularVelocity::new::(omega.value), + AngularAcceleration::new::(alpha), + ) + } +} + +impl Saturate 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::(clamped)) + } +} + +impl Saturate 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::(clamped)) + } +} + +impl Saturate for CurrentInput { + fn saturate(&self, system: &DCMotor) -> Self { + CurrentInput::new(system.saturate_current(self.current())) + } +} diff --git a/crates/lyapunov/src/systems/linear_system.rs b/crates/lyapunov/src/systems/linear_system.rs index 5d51b12..5fe9410 100644 --- a/crates/lyapunov/src/systems/linear_system.rs +++ b/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 { pub state_matrix: SMatrix, @@ -31,9 +31,13 @@ impl LinearSystem { impl System for LinearSystem { type State = SVector; type Input = SVector; - type Derivative = SVector; - fn derivatives(&self, _t: Time, x: &Self::State, u: &Self::Input) -> Self::Derivative { + fn derivatives( + &self, + _t: Time, + x: &Self::State, + u: &Self::Input, + ) -> ::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, + ) -> ::Derivative { let u_vector = u.to_vector(); let dx = self.state_matrix * x.vector + self.input_matrix * u_vector + self.constant; diff --git a/crates/lyapunov/src/systems/mod.rs b/crates/lyapunov/src/systems/mod.rs index 3b49972..74b7c18 100644 --- a/crates/lyapunov/src/systems/mod.rs +++ b/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; diff --git a/crates/lyapunov/src/systems/pendulum.rs b/crates/lyapunov/src/systems/pendulum.rs new file mode 100644 index 0000000..c81515b --- /dev/null +++ b/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, + ) -> ::Derivative { + let theta = x.angle().value; + let omega = x.angular_velocity().value; + let current = u.current().value; + + let torque = self + .motor + .torque(ElectricCurrent::new::(current)) + .value; + let alpha = (torque + - self.mass.value * self.gravity.value * self.length.value * theta.sin()) + / self.inertia.value; + + OmegaAlphaState::new( + AngularVelocity::new::(omega), + AngularAcceleration::new::(alpha), + ) + } +} + +impl Saturate for CurrentInput { + fn saturate(&self, system: &Pendulum) -> Self { + self.saturate(&system.motor) + } +} diff --git a/crates/lyapunov/src/traits.rs b/crates/lyapunov/src/traits.rs index 45abd1a..d6b534e 100644 --- a/crates/lyapunov/src/traits.rs +++ b/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 TimeDerivative for SVector { + type Derivative = SVector; +} // Represents a dynamical system with state dimension N and input dimension M. pub trait System { - 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, + ) -> ::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 VectorState: Copy { - fn to_vector(&self) -> SVector; +pub trait HasVector { + fn vector(&self) -> &SVector; + fn vector_mut(&mut self) -> &mut SVector; } -impl VectorState for SVector { - fn to_vector(&self) -> SVector { - *self +impl HasVector for SVector { + fn vector(&self) -> &SVector { + self + } + + fn vector_mut(&mut self) -> &mut SVector { + self } } -/// An input type that can be viewed as an `M`-dimensional vector. -pub trait VectorInput: Copy { - fn to_vector(&self) -> SVector; -} - -impl VectorInput for SVector { - fn to_vector(&self) -> SVector { - *self +pub trait VectorValue: + Copy + + From> + + Into> + + std::ops::Add + + std::ops::Sub + + HasVector +{ + fn to_vector(&self) -> SVector { + *self.vector() } } + +impl VectorValue for T where + T: Copy + + From> + + Into> + + std::ops::Add + + std::ops::Sub + + HasVector +{ +} + +#[macro_export] +macro_rules! impl_has_vector { + ($type:ty, $dim:expr) => { + impl $crate::traits::HasVector<$dim> for $type { + fn vector(&self) -> &nalgebra::SVector { + &self.vector + } + + fn vector_mut(&mut self) -> &mut nalgebra::SVector { + &mut self.vector + } + } + + impl From> for $type { + fn from(vector: nalgebra::SVector) -> Self { + Self { vector } + } + } + + impl From<$type> for nalgebra::SVector { + 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 { + fn saturate(&self, system: &S) -> Self; +} diff --git a/crates/lyapunov/src/units.rs b/crates/lyapunov/src/units.rs new file mode 100644 index 0000000..511ec3f --- /dev/null +++ b/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 = >::Output; +pub type BackEmfConstant = >::Output; +pub type FrictionCoefficient = >::Output; +pub type CurrentRate = >::Output;