dynibo 0.5.2

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

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

/// Runtime state of a floating 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 {
    pub(crate) fn stationary_unchecked(frame: Frame) -> Self {
        Self {
            frame,
            velocity: Twist::zeros(),
            acceleration: Twist::zeros(),
        }
    }

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

    /// 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
    }
}

pub(crate) 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",
        })
    }
}