dynamics_joint/
fixed.rs

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

dynamics_joint/
fixed.rs
