dynamics_joint/
joint_data.rs

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

dynamics_joint/
joint_data.rs
