hheroux/lyapunov
hheroux/lyapunov
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity Actions

Simulate double integrator with linear state feedback and constant cancelling feedforward.

Browse source
This commit is contained in:
Hayden Heroux 2026-10-29 14:04:29 -04:00
parent 11562e223f
commit fb40960e13
15 changed files with 466 additions and 8 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

235
Cargo.lock generated
View file

@ -2,6 +2,241 @@
# It is not intended for manual editing.
version = 4
[[package]]
name = "approx"
version = "0.5.1"
source = "registry+https://github.com/rust-lang/crates.io-index"
checksum = "cab112f0a86d568ea0e627cc1d6be74a1e9cd55214684db5561995f6dad897c6"
dependencies = [
"num-traits",
]
[[package]]
name = "autocfg"
version = "1.5.1"
source = "registry+https://github.com/rust-lang/crates.io-index"
checksum = "f2032f911046de80f0a198e0901378627c33f59ea0ac00e363d481118bd70a53"
[[package]]
name = "bytemuck"
version = "1.25.2"
source = "registry+https://github.com/rust-lang/crates.io-index"
checksum = "95832e849adfb21180ccb6826a99da14e5d266ae5c2e668e1602cf234f153797"
[[package]]
name = "glam"
version = "0.30.10"
source = "registry+https://github.com/rust-lang/crates.io-index"
checksum = "19fc433e8437a212d1b6f1e68c7824af3aed907da60afa994e7f542d18d12aa9"
[[package]]
name = "glam"
version = "0.31.1"
source = "registry+https://github.com/rust-lang/crates.io-index"
checksum = "556f6b2ea90b8d15a74e0e7bb41671c9bdf38cd9f78c284d750b9ce58a2b5be7"
[[package]]
name = "glam"
version = "0.32.1"
source = "registry+https://github.com/rust-lang/crates.io-index"
checksum = "f70749695b063ecbf6b62949ccccde2e733ec3ecbbd71d467dca4e5c6c97cca0"
[[package]]
name = "glam"
version = "0.33.12"
source = "registry+https://github.com/rust-lang/crates.io-index"
checksum = "37bfe73dc8ec21f54d181e5a60554327ec3f1c600c455a75ccbfc65d2106a6c4"
[[package]]
name = "lyapunov"
version = "0.1.0"
dependencies = [
"nalgebra",
"uom",
]
[[package]]
name = "lyapunov-demo"
version = "0.1.0"
dependencies = [
"lyapunov",
"nalgebra",
"uom",
]
[[package]]
name = "matrixmultiply"
version = "0.3.11"
source = "registry+https://github.com/rust-lang/crates.io-index"
checksum = "3f607c237553f086e7043417a51df26b2eb899d3caff94e6a67592ff992fedc7"
dependencies = [
"autocfg",
"rawpointer",
]
[[package]]
name = "nalgebra"
version = "0.35.0"
source = "registry+https://github.com/rust-lang/crates.io-index"
checksum = "adc43a60c217b0c6ff46e47f26911015ad8d2e5a8be1af668c67e370d99a4346"
dependencies = [
"approx",
"glam 0.30.10",
"glam 0.31.1",
"glam 0.32.1",
"glam 0.33.12",
"matrixmultiply",
"nalgebra-macros",
"num-complex",
"num-rational",
"num-traits",
"simba",
"typenum",
]
[[package]]
name = "nalgebra-macros"
version = "0.3.0"
source = "registry+https://github.com/rust-lang/crates.io-index"
checksum = "973e7178a678cfd059ccec50887658d482ce16b0aa9da3888ddeab5cd5eb4889"
dependencies = [
"proc-macro2",
"quote",
"syn",
]
[[package]]
name = "num-bigint"
version = "0.4.8"
source = "registry+https://github.com/rust-lang/crates.io-index"
checksum = "c89e69e7e0f03bea5ef08013795c25018e101932225a656383bd384495ecc367"
dependencies = [
"num-integer",
"num-traits",
]
[[package]]
name = "num-complex"
version = "0.4.6"
source = "registry+https://github.com/rust-lang/crates.io-index"
checksum = "73f88a1307638156682bada9d7604135552957b7818057dcef22705b4d509495"
dependencies = [
"num-traits",
]
[[package]]
name = "num-integer"
version = "0.1.47"
source = "registry+https://github.com/rust-lang/crates.io-index"
checksum = "7ce2d95d4b3734dc35aa2f45e1aa22cd416814592a4f9d9205e11affd5b8e10b"
dependencies = [
"num-traits",
]
[[package]]
name = "num-rational"
version = "0.4.2"
source = "registry+https://github.com/rust-lang/crates.io-index"
checksum = "f83d14da390562dca69fc84082e73e548e1ad308d24accdedd2720017cb37824"
dependencies = [
"num-bigint",
"num-integer",
"num-traits",
]
[[package]]
name = "num-traits"
version = "0.2.19"
source = "registry+https://github.com/rust-lang/crates.io-index"
checksum = "071dfc062690e90b734c0b2273ce72ad0ffa95f0c74596bc250dcfd960262841"
dependencies = [
"autocfg",
]
[[package]]
name = "proc-macro2"
version = "1.0.107"
source = "registry+https://github.com/rust-lang/crates.io-index"
checksum = "985e7ec9bb745e6ce6535b544d84d6cd6f7ad8bd711c398938ae983b91a766d9"
dependencies = [
"unicode-ident",
]
[[package]]
name = "quote"
version = "1.0.47"
source = "registry+https://github.com/rust-lang/crates.io-index"
checksum = "1fbf4db142a473a8d80c26bbf18454ed458bf8d26c8219c331daecfdbd079001"
dependencies = [
"proc-macro2",
]
[[package]]
name = "rawpointer"
version = "0.2.1"
source = "registry+https://github.com/rust-lang/crates.io-index"
checksum = "60a357793950651c4ed0f3f52338f53b2f809f32d83a07f72909fa13e4c6c1e3"
[[package]]
name = "safe_arch"
version = "1.2.0"
source = "registry+https://github.com/rust-lang/crates.io-index"
checksum = "42c6efa15875e6ecb39ca61fb0b0c1a40b84fac5a5ffe71eef7d1000c8eb3f5f"
dependencies = [
"bytemuck",
]
[[package]]
name = "simba"
version = "0.10.2"
source = "registry+https://github.com/rust-lang/crates.io-index"
checksum = "f1a7200d82ff1c7b4235efd12f11e2537a402c91cf83a5cb97ce80eef787fde7"
dependencies = [
"approx",
"num-complex",
"num-traits",
"wide",
]
[[package]]
name = "syn"
version = "2.0.119"
source = "registry+https://github.com/rust-lang/crates.io-index"
checksum = "872831b642d1a07999a962a351ed35b955ea2cfc8f3862091e2a240a84f17297"
dependencies = [
"proc-macro2",
"quote",
"unicode-ident",
]
[[package]]
name = "typenum"
version = "1.20.1"
source = "registry+https://github.com/rust-lang/crates.io-index"
checksum = "b6f5e870be6c3b371b77fe0ee0bafb859fa4964b4404c27de1d380043c4dda20"
[[package]]
name = "unicode-ident"
version = "1.0.27"
source = "registry+https://github.com/rust-lang/crates.io-index"
checksum = "a2c754d6c33795a1c324727428e5a7dedb5b06195f9890bdbcba760d3e246563"
[[package]]
name = "uom"
version = "0.38.0"
source = "registry+https://github.com/rust-lang/crates.io-index"
checksum = "a739f83872836c82a4f2527d4e54b37007b3de68cafe7edde95fd695968bf4b9"
dependencies = [
"num-traits",
"typenum",
]
[[package]]
name = "wide"
version = "1.7.1"
source = "registry+https://github.com/rust-lang/crates.io-index"
checksum = "d920ac99c3c8edce110cb8d07dbb324d6d026011dce85b1e9355b70f0adacc4f"
dependencies = [
"bytemuck",
"safe_arch",
]

11
Cargo.toml
View file

@ -1,6 +1,7 @@
[package]
name = "lyapunov"
version = "0.1.0"
edition = "2024"
[workspace]
resolver = "2"
members = ["crates/*"]
[dependencies]
[workspace.package]
edition = "2024"
version = "0.1.0"

9
crates/lyapunov-demo/Cargo.toml Normal file
View file

@ -0,0 +1,9 @@
[package]
name = "lyapunov-demo"
version.workspace = true
edition.workspace = true
[dependencies]
lyapunov = { path = "../lyapunov" }
nalgebra = "0.35"
uom = "0.38"

30
crates/lyapunov-demo/src/main.rs Normal file
View file

@ -0,0 +1,30 @@
use lyapunov::{Controller, Integrator, LinearFeedback, LinearSystem, Rk4};
use nalgebra::SVector;
use uom::si::f64::Time;
use uom::si::time::millisecond;
fn main() {
let system = LinearSystem::<2, 1>::double_integrator(1.0, -0.1);
let u_ff = system
.constant_cancelling_input()
.unwrap_or_else(SVector::zeros);
let p = LinearFeedback::<2, 1>::from_gains([2.0, 3.0]);
let mut x = SVector::<f64, 2>::new(10.0, 0.0);
let r = SVector::<f64, 2>::new(0.0, 0.0);
let dt = Time::new::<millisecond>(50.0);
for step in 0..200 {
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);
println!(
"t={:.2}s u={:.3} pos={:.3}m vel={:.3}m/s",
t.value, u[0], x[0], x[1],
);
}
}

8
crates/lyapunov/Cargo.toml Normal file
View file

@ -0,0 +1,8 @@
[package]
name = "lyapunov"
version.workspace = true
edition.workspace = true
[dependencies]
uom = "0.38"
nalgebra = "0.35"

39
crates/lyapunov/src/controllers/linear_feedback.rs Normal file
View file

@ -0,0 +1,39 @@
use nalgebra::{SMatrix, SVector};
use uom::si::f64::Time;
use crate::traits::{Controller, System};
pub struct LinearFeedback<const N: usize, const M: usize> {
pub gain_matrix: SMatrix<f64, M, N>,
}
impl<S, const N: usize, const M: usize> Controller<S, N, M> for LinearFeedback<N, M>
where
S: System<N, M, State = SVector<f64, N>, Input = SVector<f64, M>, Derivative = SVector<f64, N>>,
S::State: Copy + std::ops::Sub<Output = S::State>,
{
fn calculate(&self, _system: &S, _t: Time, x: &S::State, r: &S::State) -> S::Input {
let error = *r - *x;
self.gain_matrix * error
}
}
impl<const N: usize> LinearFeedback<N, 1> {
pub fn from_gains(gains: [f64; N]) -> Self {
let mut gain_matrix = SMatrix::<f64, 1, N>::zeros();
for (j, &g) in gains.iter().enumerate() {
gain_matrix[(0, j)] = g;
}
Self { gain_matrix }
}
}
impl<const N: usize> LinearFeedback<N, N> {
pub fn diagonal(gains: [f64; N]) -> Self {
let mut gain_matrix = SMatrix::<f64, N, N>::zeros();
for i in 0..N {
gain_matrix[(i, i)] = gains[i];
}
Self { gain_matrix }
}
}

1
crates/lyapunov/src/controllers/mod.rs Normal file
View file

@ -0,0 +1 @@
pub mod linear_feedback;

5
crates/lyapunov/src/integrators/mod.rs Normal file
View file

@ -0,0 +1,5 @@
pub mod rk4;
pub mod traits;
pub use rk4::Rk4;
pub use traits::Accumulate;

34
crates/lyapunov/src/integrators/rk4.rs Normal file
View file

@ -0,0 +1,34 @@
use uom::si::f64::Time;
use crate::{
integrators::traits::Accumulate,
traits::{Integrator, System},
};
pub struct Rk4;
impl<S, const N: usize, const M: usize> Integrator<S, N, M> for Rk4
where
S: System<N, M>,
S::State: Clone + Accumulate<S::Derivative>,
{
fn step(&self, system: &S, t: Time, x: &S::State, u: &S::Input, dt: Time) -> S::State {
let half_dt = dt * 0.5;
let k1 = system.derivatives(t, x, u);
let x2 = x.add_scaled(half_dt, &k1);
let k2 = system.derivatives(t + half_dt, &x2, u);
let x3 = x.add_scaled(half_dt, &k2);
let k3 = system.derivatives(t + half_dt, &x3, u);
let x4 = x.add_scaled(dt, &k3);
let k4 = system.derivatives(t + dt, &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)
}
}

12
crates/lyapunov/src/integrators/traits.rs Normal file
View file

@ -0,0 +1,12 @@
use nalgebra::SVector;
use uom::si::f64::Time;
pub trait Accumulate<D> {
fn add_scaled(&self, dt: Time, derivative: &D) -> Self;
}
impl<const N: usize> Accumulate<SVector<f64, N>> for SVector<f64, N> {
fn add_scaled(&self, dt: Time, derivative: &SVector<f64, N>) -> Self {
self + dt.get::<uom::si::time::second>() * derivative
}
}

9
crates/lyapunov/src/lib.rs Normal file
View file

@ -0,0 +1,9 @@
pub mod controllers;
pub mod integrators;
pub mod systems;
pub mod traits;
pub use controllers::linear_feedback::LinearFeedback;
pub use integrators::rk4::Rk4;
pub use systems::linear_system::LinearSystem;
pub use traits::{Controller, Integrator, System};

47
crates/lyapunov/src/systems/linear_system.rs Normal file
View file

@ -0,0 +1,47 @@
use crate::traits::System;
use nalgebra::{DMatrix, DVector, SMatrix, SVector};
use uom::si::f64::Time;
pub struct LinearSystem<const N: usize, const M: usize> {
pub state_matrix: SMatrix<f64, N, N>,
pub input_matrix: SMatrix<f64, N, M>,
pub constant: SVector<f64, N>,
}
impl<const N: usize, const M: usize> LinearSystem<N, M> {
pub fn constant_cancelling_input(&self) -> Option<SVector<f64, M>> {
let constant_cancel = -self.constant;
let input_matrix_dyn = DMatrix::from_column_slice(N, M, self.input_matrix.as_slice());
let constant_cancel_dyn = DVector::from_column_slice(constant_cancel.as_slice());
let svd = input_matrix_dyn.svd(true, true);
let input_dyn = svd.solve(&constant_cancel_dyn, 1e-10).ok()?;
let mut input = SVector::<f64, M>::zeros();
for i in 0..M {
input[i] = input_dyn[i];
}
Some(input)
}
}
impl<const N: usize, const M: usize> System<N, M> for LinearSystem<N, M> {
type State = SVector<f64, N>;
type Input = SVector<f64, M>;
type Derivative = SVector<f64, N>;
fn derivatives(&self, _t: Time, x: &Self::State, u: &Self::Input) -> Self::Derivative {
self.state_matrix * x + self.input_matrix * u + self.constant
}
}
impl LinearSystem<2, 1> {
pub fn double_integrator(input_gain: f64, disturbance: f64) -> Self {
Self {
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>::from_column_slice(&[0.0, -disturbance]),
}
}
}

3
crates/lyapunov/src/systems/mod.rs Normal file
View file

@ -0,0 +1,3 @@
pub mod linear_system;
pub use linear_system::LinearSystem;

28
crates/lyapunov/src/traits.rs Normal file
View file

@ -0,0 +1,28 @@
use uom::si::f64::*;
// Represents a dynamical system with state dimension N and input dimension M.
pub trait System<const N: usize, const M: usize> {
type State;
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;
}
// Computes the evolution of a system over (t, x, u)
pub trait Integrator<S, const N: usize, const M: usize>
where
S: System<N, M>,
{
// Performs a single simulation step of size dt for the system
fn step(&self, system: &S, t: Time, x: &S::State, u: &S::Input, dt: Time) -> S::State;
}
pub trait Controller<S, const N: usize, const M: usize>
where
S: System<N, M>,
{
fn calculate(&self, system: &S, t: Time, x: &S::State, r: &S::State) -> S::Input;
}

3
src/main.rs
View file

@ -1,3 +0,0 @@
fn main() {
println!("Hello, world!");
}