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
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132

use crate::{Frame, Result};
use std::{path::Path, sync::Arc};

mod dynamics;
mod kinematics;
mod loads;
mod model;
mod topology;
mod workspace;

pub use kinematics::InverseKinematicsOptions;
pub use loads::{IndexedLoad, LoadBuffer};
use model::Model;
pub use model::RobotModel;
pub use topology::LinkId;
use workspace::Workspace;

const FLOATING_BASE_DOF: usize = 6;

#[derive(Clone, Copy, Debug, PartialEq, Eq)]
pub(super) enum RootMode {
    Fixed,
    Floating,
}

/// A fixed-base robot model with reusable, instance-local calculation storage.
#[derive(Debug)]
pub struct Robot {
    model: Arc<Model>,
    workspace: Workspace,
    world_from_root: Frame,
}

/// A floating-base robot model with reusable, instance-local calculation storage.
#[derive(Debug)]
pub struct FloatingRobot {
    model: Arc<Model>,
    workspace: Workspace,
}

const fn base_dof_count(base_mode: RootMode) -> usize {
    match base_mode {
        RootMode::Fixed => 0,
        RootMode::Floating => FLOATING_BASE_DOF,
    }
}

fn generalized_count(model: &Model, base_mode: RootMode) -> usize {
    base_dof_count(base_mode) + model.joint_count()
}

impl Robot {
    /// Loads and validates a tree robot model from a URDF file.
    ///
    /// # Errors
    ///
    /// Returns an error if the file cannot be parsed or its graph is invalid.
    pub fn from_urdf(path: impl AsRef<Path>) -> Result<Self> {
        Ok(RobotModel::from_urdf(path)?.robot())
    }

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

    /// Replaces the fixed root-link pose in the world frame.
    pub fn set_base_frame(&mut self, frame: Frame) -> Result<()> {
        crate::base::validate_frame(&frame)?;
        self.world_from_root = frame;
        Ok(())
    }

    /// Shares the immutable model without allocating calculation storage.
    pub fn model(&self) -> RobotModel {
        RobotModel {
            model: Arc::clone(&self.model),
        }
    }

    /// Creates another calculation instance sharing this robot's immutable model.
    ///
    /// The returned robot allocates fresh calculation storage, so both instances
    /// can be used concurrently without locking.
    pub fn fork(&self) -> Self {
        let model = Arc::clone(&self.model);
        let workspace = Workspace::new(model.as_ref());
        Self {
            model,
            workspace,
            world_from_root: self.world_from_root,
        }
    }

    /// Returns the runtime generalized-vector size for this robot.
    ///
    /// Floating-base generalized vectors are ordered `[base angular, base
    /// linear, joints]`; fixed-base vectors contain only non-fixed joint entries.
    pub fn generalized_count(&self) -> usize {
        self.model.joint_count()
    }
}

impl FloatingRobot {
    /// Loads a tree robot model with a six-degree-of-freedom floating root.
    pub fn from_urdf(path: impl AsRef<Path>) -> Result<Self> {
        RobotModel::from_urdf(path)?.floating_robot()
    }

    /// Shares the immutable model without allocating calculation storage.
    pub fn model(&self) -> RobotModel {
        RobotModel {
            model: Arc::clone(&self.model),
        }
    }

    /// Creates another calculation instance sharing this robot's immutable model.
    pub fn fork(&self) -> Self {
        let model = Arc::clone(&self.model);
        let workspace = Workspace::new(model.as_ref());
        Self { model, workspace }
    }

    /// Returns the runtime generalized-vector size (six base coordinates plus joints).
    pub fn generalized_count(&self) -> usize {
        generalized_count(self.model.as_ref(), RootMode::Floating)
    }
}

#[cfg(test)]
#[cfg_attr(coverage_nightly, coverage(off))]
mod tests;