multicalc 0.9.0

Math for real-time embedded systems, in stable no_std Rust: state estimation, control, kinematics, Lie groups, autodiff, and linear algebra — from 64-bit servers to bare-metal microcontrollers
Documentation 
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50

//! The unicycle plant as an ODE right-hand side.

use crate::kinematics::differential_drive::BodyTwist;
use crate::linear_algebra::Vector;
use crate::scalar::Numeric;

/// The unicycle plant at a held body twist: `f(t, [x, y, θ]) = [v cosθ, v sinθ, ω]`.
///
/// Time-invariant; `t` is present to match the [`Rk4`](crate::Rk4) and [`Rk45`](crate::Rk45)
/// closure shape.
///
/// ```
/// use multicalc::kinematics::{BodyTwist, Unicycle};
/// use multicalc::linear_algebra::Vector;
/// use multicalc::ode::Rk4;
/// let command = BodyTwist::new(1.0_f64, 0.0);   // 1 m/s forward, no turn
/// let plant = Unicycle::new(command);
///
/// let start_time = 0.0;
/// let start_pose = Vector::new([0.0, 0.0, 0.0]);
/// let timestep = 0.1;
/// let state = Rk4::step(&plant.field(), start_time, &start_pose, timestep);
/// assert!((state[0] - 0.1).abs() < 1e-12);
/// ```
#[derive(Debug, Clone, Copy, PartialEq)]
pub struct Unicycle<T: Numeric = f64> {
    twist: BodyTwist<T>,
}

impl<T: Numeric> Unicycle<T> {
    /// A plant holding `twist`.
    #[inline]
    pub fn new(twist: BodyTwist<T>) -> Self {
        Unicycle { twist }
    }

    /// The state derivative at `state = [x, y, θ]`.
    #[inline]
    pub fn derivative(self, state: &Vector<3, T>) -> Vector<3, T> {
        let v = self.twist.linear();
        let theta = state[2];
        Vector::new([v * theta.cos(), v * theta.sin(), self.twist.angular()])
    }

    /// The derivative as an [`Rk4`](crate::Rk4)/[`Rk45`](crate::Rk45) closure.
    #[inline]
    pub fn field(self) -> impl Fn(T, &Vector<3, T>) -> Vector<3, T> {
        move |_t, y| self.derivative(y)
    }
}