dynamics_joint/
fixed.rs

1//! Fixed joint, without any degree of freedom.
2
3use crate::{
4    joint::{JointModel, JointType, JointWrapper},
5    joint_data::{JointData, JointDataWrapper, JointError},
6};
7use dynamics_spatial::{
8    configuration::Configuration, force::SpatialForce, motion::SpatialMotion, se3::SE3,
9};
10use rand::rngs::ThreadRng;
11
12/// Model of a fixed joint.
13#[derive(Clone, Debug, Default)]
14pub struct JointModelFixed {}
15
16impl JointModel for JointModelFixed {
17    fn get_joint_type(&self) -> JointType {
18        JointType::Fixed
19    }
20
21    fn nq(&self) -> usize {
22        0
23    }
24
25    fn nv(&self) -> usize {
26        0
27    }
28
29    fn neutral(&self) -> Configuration {
30        Configuration::zeros(0)
31    }
32
33    fn create_joint_data(&self) -> JointDataWrapper {
34        JointDataWrapper::fixed(JointDataFixed::new(self))
35    }
36
37    fn random_configuration(&self, _rng: &mut ThreadRng) -> Configuration {
38        Configuration::zeros(0)
39    }
40
41    fn get_axis(&self) -> Vec<SpatialMotion> {
42        Vec::new()
43    }
44
45    fn subspace(&self, v: &Configuration) -> SpatialMotion {
46        assert_eq!(v.len(), 0, "Fixed joint model expects no velocity.");
47        SpatialMotion::zero() // TODO: check
48    }
49
50    fn subspace_dual(&self, f: &SpatialForce) -> Configuration {
51        debug_assert_eq!(
52            f,
53            &SpatialForce::zero(),
54            "Fixed joint model expects zero force."
55        );
56        Configuration::zeros(0)
57    }
58
59    fn bias(&self) -> SpatialMotion {
60        SpatialMotion::zero()
61    }
62}
63
64/// Data structure containing the mutable properties of a fixed joint.
65#[derive(Debug, Clone)]
66pub struct JointDataFixed {
67    /// The joint configuration vector (always empty).
68    pub joint_q: Configuration,
69    /// The joint velocity vector (always empty).
70    pub joint_v: Configuration,
71    /// The placement of the joint in the local frame.
72    pub placement: SE3,
73    /// The joint velocity as a spatial motion.
74    pub joint_velocity: SpatialMotion,
75}
76
77impl JointDataFixed {
78    /// Creates a new [`JointDataFixed`] from given joint model.
79    ///
80    /// # Arguments
81    ///
82    /// * `joint_model` - The fixed joint model.
83    ///
84    /// # Returns
85    /// A new [`JointDataFixed`] object.
86    #[must_use]
87    pub fn new(_joint_model: &JointModelFixed) -> Self {
88        JointDataFixed {
89            joint_q: Configuration::zeros(0),
90            joint_v: Configuration::zeros(0),
91            placement: SE3::identity(),
92            joint_velocity: SpatialMotion::zero(),
93        }
94    }
95}
96
97impl JointData for JointDataFixed {
98    fn get_joint_q(&self) -> &Configuration {
99        &self.joint_q
100    }
101
102    fn get_joint_v(&self) -> &Configuration {
103        &self.joint_v
104    }
105
106    fn get_joint_placement(&self) -> SE3 {
107        self.placement
108    }
109
110    fn update(
111        &mut self,
112        _joint_model: &JointWrapper,
113        _joint_q: &Configuration,
114        _joint_v: Option<&Configuration>,
115    ) -> Result<(), JointError> {
116        Ok(())
117    }
118
119    fn get_joint_velocity(&self) -> &SpatialMotion {
120        &self.joint_velocity
121    }
122}
dynamics_joint/fixed.rs

dynamics_joint/
fixed.rs
