multicalc 0.10.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
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81

//! Pure-pursuit path-following law.

use crate::error::ControlError;
use crate::kinematics::BodyTwist;
use crate::linear_algebra::Vector2D;
use crate::scalar::Numeric;
use crate::spatial::SE2;

/// A path curvature, the reciprocal of the turning radius, in units of 1/m. Positive curves left.
#[derive(Debug, Clone, Copy, PartialEq)]
pub struct Curvature<T: Numeric = f64> {
    value: T,
}

impl<T: Numeric> Curvature<T> {
    /// Wraps a curvature value.
    #[inline]
    #[must_use]
    pub fn new(value: T) -> Self {
        Self { value }
    }

    /// The curvature value in 1/m.
    #[inline]
    #[must_use]
    pub fn value(self) -> T {
        self.value
    }

    /// The body twist that follows this curvature at a given forward speed: linear speed forward and
    /// angular rate `forward_speed * curvature`.
    #[inline]
    #[must_use]
    pub fn to_body_twist(self, forward_speed: T) -> BodyTwist<T> {
        BodyTwist::new(forward_speed, forward_speed * self.value)
    }
}

/// The pure-pursuit curvature that steers a robot toward a lookahead point.
///
/// `pose` is the robot pose in the world frame and `lookahead_point` is the target in the world
/// frame. `lookahead_distance` is the pursuit distance `L_d`. The result is the exact
/// `κ = 2·sin(α)/L_d` written in body-frame coordinates, which holds when `L_d` equals the distance
/// from the robot to the target point.
///
/// Returns [`ControlError::NonPositiveLookaheadDistance`] if `lookahead_distance` is not finite or not
/// strictly positive, or [`ControlError::NonFinite`] if the lookahead point is not finite.
///
/// ```
/// use multicalc::control::pure_pursuit_curvature;
/// use multicalc::spatial::SE2;
/// use multicalc::linear_algebra::Vector;
///
/// let pose = SE2::identity();
/// let lookahead_distance = 2.0;
///
/// // A point straight ahead needs no turn.
/// let straight_ahead = Vector::new([2.0_f64, 0.0]);
/// let ahead = pure_pursuit_curvature(pose, straight_ahead, lookahead_distance).unwrap();
/// assert!(ahead.value().abs() < 1e-12);
///
/// // A point to the left curves left (positive curvature).
/// let off_to_the_left = Vector::new([2.0_f64, 1.0]);
/// let left = pure_pursuit_curvature(pose, off_to_the_left, lookahead_distance).unwrap();
/// assert!(left.value() > 0.0);
/// ```
pub fn pure_pursuit_curvature<T: Numeric>(
    pose: SE2<T>,
    lookahead_point: Vector2D<T>,
    lookahead_distance: T,
) -> Result<Curvature<T>, ControlError> {
    if !lookahead_distance.is_finite() || lookahead_distance <= T::ZERO {
        return Err(ControlError::NonPositiveLookaheadDistance);
    }
    if !lookahead_point.is_finite() {
        return Err(ControlError::NonFinite);
    }
    let [_forward, lateral] = pose.inverse().act(lookahead_point).into_array();
    let curvature = (T::TWO * lateral) / (lookahead_distance * lookahead_distance);
    Ok(Curvature::new(curvature))
}