dynamics_joint/
joint_data.rs

1//! Structure containing the mutable properties of a joint.
2
3use crate::{
4    continuous::JointDataContinuous, fixed::JointDataFixed, joint::JointWrapper,
5    prismatic::JointDataPrismatic, revolute::JointDataRevolute,
6};
7use dynamics_spatial::{configuration::Configuration, motion::SpatialMotion, se3::SE3};
8
9#[derive(Clone, Debug)]
10/// Enum encapsulating different joint data implementations.
11///
12/// It serves as the inner representation for the `JointDataWrapper` struct.
13/// As such, users should interact with joints data through the `JointDataWrapper` interface,
14/// and avoid using this enum directly.
15enum JointDataImpl {
16    Continuous(JointDataContinuous),
17    Prismatic(JointDataPrismatic),
18    Revolute(JointDataRevolute),
19    Fixed(JointDataFixed),
20}
21
22#[derive(Clone, Debug)]
23/// Wrapper struct for different joint data implementations.
24///
25/// This struct provides a unified interface to interact with different joint types
26/// through the `JointData` trait. It serves as the main entry point for users of
27/// the library to work with joints data.
28pub struct JointDataWrapper {
29    inner: JointDataImpl,
30}
31
32/// Trait for joint data, providing methods to access and update joint properties.
33pub trait JointData {
34    /// Returns the joint configuration vector.
35    fn get_joint_q(&self) -> &Configuration;
36
37    /// Returns the joint velocity vector.
38    fn get_joint_v(&self) -> &Configuration;
39
40    /// Returns the placement of the joint in the world frame.
41    fn get_joint_placement(&self) -> SE3;
42
43    /// Updates the joint data with the current position and velocity configurations.
44    fn update(
45        &mut self,
46        joint_model: &JointWrapper,
47        joint_q: &Configuration,
48        joint_v: Option<&Configuration>,
49    );
50
51    /// Returns the joint velocity as a spatial motion.
52    fn get_joint_velocity(&self) -> &SpatialMotion;
53}
54
55impl JointDataWrapper {
56    /// Creates a new `JointDataWrapper` from a `JointDataContinuous`.
57    pub fn continuous(joint_data: JointDataContinuous) -> Self {
58        JointDataWrapper {
59            inner: JointDataImpl::Continuous(joint_data),
60        }
61    }
62
63    /// Creates a new `JointDataWrapper` from a `JointDataPrismatic`.
64    pub fn prismatic(joint_data: JointDataPrismatic) -> Self {
65        JointDataWrapper {
66            inner: JointDataImpl::Prismatic(joint_data),
67        }
68    }
69
70    /// Creates a new `JointDataWrapper` from a `JointDataRevolute`.
71    pub fn revolute(joint_data: JointDataRevolute) -> Self {
72        JointDataWrapper {
73            inner: JointDataImpl::Revolute(joint_data),
74        }
75    }
76
77    /// Creates a new `JointDataWrapper` from a `JointDataFixed`.
78    pub fn fixed(joint_data: JointDataFixed) -> Self {
79        JointDataWrapper {
80            inner: JointDataImpl::Fixed(joint_data),
81        }
82    }
83}
84
85// TODO: use macros to reduce boilerplate
86impl JointData for JointDataWrapper {
87    fn get_joint_q(&self) -> &Configuration {
88        match &self.inner {
89            JointDataImpl::Continuous(joint_data) => joint_data.get_joint_q(),
90            JointDataImpl::Prismatic(joint_data) => joint_data.get_joint_q(),
91            JointDataImpl::Revolute(joint_data) => joint_data.get_joint_q(),
92            JointDataImpl::Fixed(joint_data) => joint_data.get_joint_q(),
93        }
94    }
95
96    fn get_joint_v(&self) -> &Configuration {
97        match &self.inner {
98            JointDataImpl::Continuous(joint_data) => joint_data.get_joint_v(),
99            JointDataImpl::Prismatic(joint_data) => joint_data.get_joint_v(),
100            JointDataImpl::Revolute(joint_data) => joint_data.get_joint_v(),
101            JointDataImpl::Fixed(joint_data) => joint_data.get_joint_v(),
102        }
103    }
104
105    fn get_joint_placement(&self) -> SE3 {
106        match &self.inner {
107            JointDataImpl::Continuous(joint_data) => joint_data.get_joint_placement(),
108            JointDataImpl::Prismatic(joint_data) => joint_data.get_joint_placement(),
109            JointDataImpl::Revolute(joint_data) => joint_data.get_joint_placement(),
110            JointDataImpl::Fixed(joint_data) => joint_data.get_joint_placement(),
111        }
112    }
113
114    fn update(
115        &mut self,
116        joint_model: &JointWrapper,
117        joint_q: &Configuration,
118        joint_v: Option<&Configuration>,
119    ) {
120        match &mut self.inner {
121            JointDataImpl::Continuous(joint_data) => {
122                joint_data.update(joint_model, joint_q, joint_v)
123            }
124            JointDataImpl::Prismatic(joint_data) => {
125                joint_data.update(joint_model, joint_q, joint_v)
126            }
127            JointDataImpl::Revolute(joint_data) => joint_data.update(joint_model, joint_q, joint_v),
128            JointDataImpl::Fixed(joint_data) => joint_data.update(joint_model, joint_q, joint_v),
129        }
130    }
131
132    fn get_joint_velocity(&self) -> &SpatialMotion {
133        match &self.inner {
134            JointDataImpl::Continuous(joint_data) => joint_data.get_joint_velocity(),
135            JointDataImpl::Prismatic(joint_data) => joint_data.get_joint_velocity(),
136            JointDataImpl::Revolute(joint_data) => joint_data.get_joint_velocity(),
137            JointDataImpl::Fixed(joint_data) => joint_data.get_joint_velocity(),
138        }
139    }
140}
dynamics_joint/joint_data.rs

dynamics_joint/
joint_data.rs
