dynibo 0.3.0

Tree-structured robot kinematics and dynamics with runtime-size workspace APIs
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
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119

use crate::{Error, Frame, Result, Twist};

/// Whether the URDF root link is fixed to the world or has six free degrees of freedom.
#[derive(Clone, Copy, Debug, Default, PartialEq, Eq)]
pub enum BaseMode {
    /// The root link has a fixed world pose and zero velocity and acceleration.
    #[default]
    Fixed,
    /// The root link has a world pose and an independently prescribed spatial motion.
    /// Its generalized-vector prefix is `[angular_x, angular_y, angular_z,
    /// linear_x, linear_y, linear_z]`, all expressed in the world frame.
    Floating,
}

/// Runtime state of the URDF root link.
///
/// Velocity and acceleration are angular-first classical quantities expressed
/// in the world frame at the root-link origin.
#[derive(Clone, Copy, Debug, PartialEq)]
pub struct BaseState {
    frame: Frame,
    velocity: Twist,
    acceleration: Twist,
}

impl BaseState {
    /// Creates the zero state of a fixed base at the world origin.
    pub fn fixed() -> Self {
        Self {
            frame: Frame::identity(),
            velocity: Twist::zeros(),
            acceleration: Twist::zeros(),
        }
    }

    /// Creates a stationary fixed base at a prescribed world pose.
    ///
    /// # Errors
    ///
    /// Returns an error if `frame` contains a non-finite value.
    pub fn fixed_at(frame: Frame) -> Result<Self> {
        validate_frame(&frame)?;
        Ok(Self {
            frame,
            velocity: Twist::zeros(),
            acceleration: Twist::zeros(),
        })
    }

    /// Creates a base state expressed in the world frame.
    ///
    /// # Errors
    ///
    /// Returns an error if any component is non-finite.
    pub fn new(frame: Frame, velocity: Twist, acceleration: Twist) -> Result<Self> {
        validate_frame(&frame)?;
        validate_twist(velocity, "velocity")?;
        validate_twist(acceleration, "acceleration")?;
        Ok(Self {
            frame,
            velocity,
            acceleration,
        })
    }

    /// Returns the root-link pose in the world frame.
    pub const fn frame(&self) -> &Frame {
        &self.frame
    }

    /// Returns the root-origin classical velocity expressed in the world frame.
    pub const fn velocity(&self) -> Twist {
        self.velocity
    }

    /// Returns the root-origin classical acceleration expressed in the world frame.
    pub const fn acceleration(&self) -> Twist {
        self.acceleration
    }
}

impl Default for BaseState {
    fn default() -> Self {
        Self::fixed()
    }
}

fn validate_frame(frame: &Frame) -> Result<()> {
    if frame
        .translation
        .vector
        .iter()
        .chain(frame.rotation.coords.iter())
        .all(|value| value.is_finite())
    {
        Ok(())
    } else {
        Err(Error::InvalidBaseState {
            field: "frame",
            reason: "must contain only finite values",
        })
    }
}

fn validate_twist(value: Twist, field: &'static str) -> Result<()> {
    if value
        .angular
        .iter()
        .chain(value.linear.iter())
        .all(|value| value.is_finite())
    {
        Ok(())
    } else {
        Err(Error::InvalidBaseState {
            field,
            reason: "must contain only finite values",
        })
    }
}