diff --git a/crates/lyapunov/src/integrators/rk4.rs b/crates/lyapunov/src/integrators/rk4.rs index 6f68680..1678a72 100644 --- a/crates/lyapunov/src/integrators/rk4.rs +++ b/crates/lyapunov/src/integrators/rk4.rs @@ -2,7 +2,7 @@ use crate::units::*; use crate::{ integrators::traits::Accumulate, - traits::{Integrator, System, TimeDerivative}, + traits::{Integrator, System, TimeDerivative, TimeInvariantSystem}, }; pub struct Rk4; @@ -32,3 +32,35 @@ where .add_scaled(dt / 6.0, &k4) } } + +impl Rk4 { + pub fn step_ti( + &self, + system: &S, + x: &S::State, + u: &S::Input, + dt: Time, + ) -> S::State + where + S: TimeInvariantSystem, + S::State: Clone + TimeDerivative + Accumulate<::Derivative>, + { + let half_dt = dt * 0.5; + + let k1 = system.derivatives(x, u); + + let x2 = x.add_scaled(half_dt, &k1); + let k2 = system.derivatives(&x2, u); + + let x3 = x.add_scaled(half_dt, &k2); + let k3 = system.derivatives(&x3, u); + + let x4 = x.add_scaled(dt, &k3); + let k4 = system.derivatives(&x4, u); + + x.add_scaled(dt / 6.0, &k1) + .add_scaled(dt / 3.0, &k2) + .add_scaled(dt / 3.0, &k3) + .add_scaled(dt / 6.0, &k4) + } +} diff --git a/crates/lyapunov/src/lib.rs b/crates/lyapunov/src/lib.rs index 572d9d6..a5954c5 100644 --- a/crates/lyapunov/src/lib.rs +++ b/crates/lyapunov/src/lib.rs @@ -18,4 +18,4 @@ pub use units::{BackEmfConstant, CurrentRate, FrictionCoefficient, TorqueConstan pub use controllers::linear_feedback::LinearFeedback; pub use integrators::rk4::Rk4; pub use systems::linear_system::LinearSystem; -pub use traits::{Controller, Integrator, System, TimeDerivative}; +pub use traits::{Controller, Integrator, System, TimeDerivative, TimeInvariantSystem}; diff --git a/crates/lyapunov/src/systems/dc_motor.rs b/crates/lyapunov/src/systems/dc_motor.rs index 5e71ff0..8ea0f30 100644 --- a/crates/lyapunov/src/systems/dc_motor.rs +++ b/crates/lyapunov/src/systems/dc_motor.rs @@ -2,7 +2,7 @@ use crate::units::*; use crate::inputs::{CurrentInput, VoltageInput}; use crate::states::{CurrentRateState, CurrentState, OmegaAlphaState, ThetaOmegaState}; -use crate::traits::{Saturate, System, TimeDerivative}; +use crate::traits::{Saturate, TimeDerivative, TimeInvariantSystem}; #[derive(Clone, Copy, Debug)] pub struct DCMotor { @@ -110,13 +110,12 @@ impl DCMotor { } } -impl System<1, 1> for DCMotor { +impl TimeInvariantSystem<1, 1> for DCMotor { type State = CurrentState; type Input = VoltageInput; fn derivatives( &self, - _t: Time, x: &Self::State, u: &Self::Input, ) -> ::Derivative { @@ -160,13 +159,12 @@ impl DCMotorPosition { } } -impl System<2, 1> for DCMotorPosition { +impl TimeInvariantSystem<2, 1> for DCMotorPosition { type State = ThetaOmegaState; type Input = VoltageInput; fn derivatives( &self, - _t: Time, x: &Self::State, u: &Self::Input, ) -> ::Derivative { diff --git a/crates/lyapunov/src/systems/linear_system.rs b/crates/lyapunov/src/systems/linear_system.rs index 5fe9410..55f0ace 100644 --- a/crates/lyapunov/src/systems/linear_system.rs +++ b/crates/lyapunov/src/systems/linear_system.rs @@ -1,6 +1,6 @@ use crate::inputs::AccelerationInput; use crate::states::{PositionVelocityState, VelocityAccelerationState}; -use crate::traits::{System, TimeDerivative, VectorValue}; +use crate::traits::{TimeDerivative, TimeInvariantSystem, VectorValue}; use crate::units::*; use nalgebra::{DMatrix, DVector, SMatrix, SVector}; @@ -28,13 +28,12 @@ impl LinearSystem { } } -impl System for LinearSystem { +impl TimeInvariantSystem for LinearSystem { type State = SVector; type Input = SVector; fn derivatives( &self, - _t: Time, x: &Self::State, u: &Self::Input, ) -> ::Derivative { @@ -85,13 +84,12 @@ impl DoubleIntegrator { } } -impl System<2, 1> for DoubleIntegrator { +impl TimeInvariantSystem<2, 1> for DoubleIntegrator { type State = PositionVelocityState; type Input = AccelerationInput; fn derivatives( &self, - _t: Time, x: &Self::State, u: &Self::Input, ) -> ::Derivative { diff --git a/crates/lyapunov/src/systems/pendulum.rs b/crates/lyapunov/src/systems/pendulum.rs index d3d9bba..2519bf9 100644 --- a/crates/lyapunov/src/systems/pendulum.rs +++ b/crates/lyapunov/src/systems/pendulum.rs @@ -4,7 +4,7 @@ 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 crate::traits::{Saturate, TimeDerivative, TimeInvariantSystem}; use nalgebra::{SMatrix, SVector}; #[derive(Clone, Copy, Debug)] @@ -63,13 +63,12 @@ impl Pendulum { } } -impl System<2, 1> for Pendulum { +impl TimeInvariantSystem<2, 1> for Pendulum { type State = ThetaOmegaState; type Input = CurrentInput; fn derivatives( &self, - _t: Time, x: &Self::State, u: &Self::Input, ) -> ::Derivative { diff --git a/crates/lyapunov/src/traits.rs b/crates/lyapunov/src/traits.rs index d6b534e..3f34786 100644 --- a/crates/lyapunov/src/traits.rs +++ b/crates/lyapunov/src/traits.rs @@ -17,7 +17,6 @@ pub trait System { type Input; // Evaluate the derivative vector x_dot at (t, x, u) - // TODO Create time-invariant system trait? fn derivatives( &self, t: Time, @@ -26,6 +25,34 @@ pub trait System { ) -> ::Derivative; } +pub trait TimeInvariantSystem { + type State: TimeDerivative; + type Input; + + fn derivatives( + &self, + x: &Self::State, + u: &Self::Input, + ) -> ::Derivative; +} + +impl System for T +where + T: TimeInvariantSystem, +{ + type State = T::State; + type Input = T::Input; + + fn derivatives( + &self, + _t: Time, + x: &Self::State, + u: &Self::Input, + ) -> ::Derivative { + self.derivatives(x, u) + } +} + // Computes the evolution of a system over (t, x, u) pub trait Integrator where