dynamics_spatial/
motion.rs

1//! Defines spatial **motion** and related operations.
2
3use nalgebra::{Matrix6, Rotation3, Vector6};
4
5use crate::{
6    force::SpatialForce,
7    se3::{ActSE3, SE3},
8    vector3d::Vector3D,
9    vector6d::Vector6D,
10};
11use std::{
12    fmt::Display,
13    ops::{Add, AddAssign, Mul},
14};
15
16#[derive(Clone, Debug, PartialEq, Default)]
17/// Spatial motion vector, combining linear and angular velocity components.
18///
19/// The first three elements represent linear velocity, and the last three represent angular velocity.
20pub struct SpatialMotion(pub(crate) Vector6<f64>);
21// TODO: rewrite this with Vector6D to avoid double implementation
22
23impl SpatialMotion {
24    /// Creates a new `SpatialMotion` from a 3D rotational axis (for revolute and continuous joints).
25    #[must_use]
26    pub fn from_rotational_axis(axis: &Vector3D) -> Self {
27        let mut v = Vector6::zeros();
28        v.fixed_rows_mut::<3>(3).copy_from(&axis.0);
29        Self(v)
30    }
31
32    /// Creates a new `SpatialMotion` from a 3D translational axis (for prismatic joints).
33    #[must_use]
34    pub fn from_translational_axis(axis: &Vector3D) -> Self {
35        let mut v = Vector6::zeros();
36        v.fixed_rows_mut::<3>(0).copy_from(&axis.0);
37        Self(v)
38    }
39
40    /// Extracts the rotation (angular velocity) component of the spatial motion.
41    #[must_use]
42    pub fn rotation(&self) -> Vector3D {
43        Vector3D(self.0.fixed_rows::<3>(3).into())
44    }
45
46    /// Extracts the translation (linear velocity) component of the spatial motion.
47    #[must_use]
48    pub fn translation(&self) -> Vector3D {
49        Vector3D(self.0.fixed_rows::<3>(0).into())
50    }
51
52    /// Zero spatial motion (no motion).
53    #[must_use]
54    pub fn zero() -> Self {
55        Self(Vector6::zeros())
56    }
57
58    /// Creates a `SpatialMotion` from linear and angular components.
59    ///
60    /// # Arguments
61    /// * `linear` - The linear velocity component (3D vector).
62    /// * `angular` - The angular velocity component (3D vector).
63    #[must_use]
64    pub fn from_parts(linear: Vector3D, angular: Vector3D) -> Self {
65        let mut v = Vector6::zeros();
66        v.fixed_rows_mut::<3>(0).copy_from(&linear.0);
67        v.fixed_rows_mut::<3>(3).copy_from(&angular.0);
68        Self(v)
69    }
70
71    /// Creates a `SpatialMotion` from a 6D vector.
72    ///
73    /// # Arguments
74    /// * `v` - The 6D vector representing spatial motion; the first three elements are linear, the last three are angular.
75    #[must_use]
76    pub fn from_vector6d(v: Vector6D) -> Self {
77        Self(v.0)
78    }
79
80    /// Constructs the cross product matrix for spatial motion vectors.
81    ///
82    /// # Arguments
83    /// * `angular` - The angular velocity component (3D vector).
84    /// * `linear` - The linear velocity component (3D vector).
85    ///
86    /// # Returns
87    /// A 6x6 matrix representing the cross product operation.
88    fn cross_matrix(angular: Vector3D, linear: Vector3D) -> Matrix6<f64> {
89        let mut cross_matrix = Matrix6::zeros();
90        let angular_so3 = crate::so3::SO3::from_vector3d(&angular);
91        let linear_so3 = crate::so3::SO3::from_vector3d(&linear);
92        cross_matrix
93            .view_mut((0, 0), (3, 3))
94            .copy_from(&angular_so3.0);
95        cross_matrix
96            .view_mut((0, 3), (3, 3))
97            .copy_from(&linear_so3.0);
98        cross_matrix
99            .view_mut((3, 3), (3, 3))
100            .copy_from(&angular_so3.0);
101        cross_matrix
102    }
103
104    /// Computes the cross product of two spatial motion vectors.
105    ///
106    /// # Arguments
107    /// * `other` - The other spatial motion vector to compute the cross product with.
108    ///
109    /// # Returns
110    /// A new `SpatialMotion` representing the cross product.
111    #[must_use]
112    pub fn cross(&self, other: &SpatialMotion) -> SpatialMotion {
113        let angular_1 = self.rotation();
114        let linear_1 = self.translation();
115
116        let angular_2 = other.rotation();
117        let linear_2 = other.translation();
118
119        let cross_linear = angular_1.cross(&linear_2) + linear_1.cross(&angular_2);
120        let cross_angular = angular_1.cross(&angular_2);
121
122        SpatialMotion::from_parts(cross_linear, cross_angular)
123    }
124
125    /// Computes the cross product of two spatial motion vectors.
126    ///
127    /// # Arguments
128    /// * `other` - The other spatial motion vector to compute the cross product with.
129    ///
130    /// # Returns
131    /// A new `SpatialMotion` representing the cross product.
132    #[must_use]
133    pub fn cross_force(&self, other: &SpatialForce) -> SpatialForce {
134        let m_linear = self.translation();
135        let m_angular = self.rotation();
136
137        let f_linear = other.translation();
138        let f_angular = other.rotation();
139
140        let cross_linear = m_angular.cross(&f_linear);
141        let cross_angular = m_angular.cross(&f_angular) + m_linear.cross(&f_linear);
142
143        SpatialForce::from_parts(cross_linear, cross_angular)
144    }
145
146    /// Computes the dual cross product of two spatial motion vectors.
147    ///
148    /// # Arguments
149    /// * `other` - The other spatial motion vector to compute the dual cross product with.
150    ///
151    /// # Returns
152    /// A new `SpatialMotion` representing the dual cross product.
153    #[must_use]
154    pub fn cross_star(&self, other: &SpatialMotion) -> SpatialMotion {
155        let angular = self.rotation();
156        let linear = self.translation();
157
158        let cross_matrix = SpatialMotion::cross_matrix(angular, linear);
159        let dual_cross_matrix = -cross_matrix.transpose();
160
161        SpatialMotion(dual_cross_matrix * other.0)
162    }
163
164    /// Computes the inner product of two spatial motion vectors.
165    #[must_use]
166    pub fn inner(&self, other: &SpatialMotion) -> f64 {
167        self.0.dot(&other.0)
168    }
169}
170
171impl Add for SpatialMotion {
172    type Output = SpatialMotion;
173
174    fn add(self, rhs: Self) -> Self::Output {
175        SpatialMotion(self.0 + rhs.0)
176    }
177}
178
179impl Add<&SpatialMotion> for SpatialMotion {
180    type Output = SpatialMotion;
181
182    fn add(self, rhs: &Self) -> Self::Output {
183        SpatialMotion(self.0 + rhs.0)
184    }
185}
186
187impl AddAssign for SpatialMotion {
188    fn add_assign(&mut self, rhs: Self) {
189        self.0 += rhs.0;
190    }
191}
192
193impl Mul<f64> for SpatialMotion {
194    type Output = SpatialMotion;
195
196    fn mul(self, rhs: f64) -> Self::Output {
197        SpatialMotion(self.0 * rhs)
198    }
199}
200
201impl Mul<f64> for &SpatialMotion {
202    type Output = SpatialMotion;
203
204    fn mul(self, rhs: f64) -> Self::Output {
205        SpatialMotion(self.0 * rhs)
206    }
207}
208
209impl Mul<SpatialMotion> for f64 {
210    type Output = SpatialMotion;
211
212    fn mul(self, rhs: SpatialMotion) -> Self::Output {
213        SpatialMotion(rhs.0 * self)
214    }
215}
216
217impl ActSE3 for SpatialMotion {
218    fn act(&self, se3: &SE3) -> Self {
219        let angular = se3.rotation() * &self.rotation();
220        let linear = se3.rotation() * &self.translation() + se3.translation().cross(&angular);
221        SpatialMotion::from_parts(linear, angular)
222    }
223
224    fn act_inv(&self, se3: &SE3) -> Self {
225        let angular = se3.rotation().transpose() * &self.rotation();
226        let linear = se3.rotation().transpose()
227            * &(self.translation() - se3.translation().cross(&self.rotation()));
228        SpatialMotion::from_parts(linear, angular)
229    }
230}
231
232impl Display for SpatialMotion {
233    fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
234        write!(
235            f,
236            "SpatialMotion(linear: [{:.4}, {:.4}, {:.4}], angular: [{:.4}, {:.4}, {:.4}])",
237            self.translation().0[0],
238            self.translation().0[1],
239            self.translation().0[2],
240            self.rotation().0[0],
241            self.rotation().0[1],
242            self.rotation().0[2],
243        )
244    }
245}
246
247#[derive(Clone, Copy, Debug, PartialEq)]
248/// Spatial rotation, represented as a 3x3 rotation matrix.
249pub struct SpatialRotation(pub(crate) Rotation3<f64>);
250
251impl SpatialRotation {
252    /// Creates a new `SpatialRotation` from a 3D axis and an angle (for revolute joints).
253    #[must_use]
254    pub fn from_axis_angle(axis: &Vector3D, angle: f64) -> Self {
255        let rot = Rotation3::from_axis_angle(&nalgebra::Unit::new_normalize(axis.0), angle);
256        Self(rot)
257    }
258
259    /// Converts the rotation to an [[[`SE3`]]] with the given translation.
260    #[must_use]
261    pub fn to_se3(&self, translation: &Vector3D) -> SE3 {
262        SE3::from_parts(*translation, *self)
263    }
264
265    /// Returns the identity rotation.
266    #[must_use]
267    pub fn identity() -> Self {
268        Self(Rotation3::identity())
269    }
270
271    /// Returns the angle of rotation in radians.
272    #[must_use]
273    pub fn angle(&self) -> f64 {
274        self.0.angle()
275    }
276
277    #[must_use]
278    pub fn from_euler_angles(roll: f64, pitch: f64, yaw: f64) -> Self {
279        Self(Rotation3::from_euler_angles(roll, pitch, yaw))
280    }
281
282    /// Returns the transpose of the rotation matrix.
283    pub fn transpose(&self) -> Self {
284        Self(self.0.transpose())
285    }
286}
287
288impl Mul<&Vector3D> for SpatialRotation {
289    type Output = Vector3D;
290
291    fn mul(self, rhs: &Vector3D) -> Self::Output {
292        Vector3D(self.0 * rhs.0)
293    }
294}
295
296#[cfg(test)]
297mod tests {
298    use crate::so3::SO3;
299    use approx::assert_relative_eq;
300    use nalgebra::{Matrix3, Matrix6};
301
302    use super::*;
303
304    #[test]
305    fn test_spatial_motion_zero() {
306        let zero = SpatialMotion::zero();
307        assert_eq!(zero.0, Vector6::zeros());
308    }
309
310    #[test]
311    fn test_spatial_rotation_identity() {
312        let identity = SpatialRotation::identity();
313        assert_eq!(identity.0, Rotation3::identity());
314    }
315
316    #[test]
317    fn test_spatial_rotation_pi_2() {
318        let z = Vector3D::new(0.0, 0.0, 1.0);
319        let rotation = SpatialRotation::from_axis_angle(&z, std::f64::consts::PI / 2.0);
320        let expected = Matrix3::new(0.0, -1.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0);
321        assert_relative_eq!(rotation.0.matrix(), &expected);
322    }
323
324    #[test]
325    fn test_cross() {
326        let angular1 = Vector3D::new(1.0, 2.0, 3.0);
327        let linear1 = Vector3D::new(4.0, 5.0, 6.0);
328        let motion1 = SpatialMotion::from_parts(linear1, angular1);
329
330        let angular2 = Vector3D::new(7.0, 8.0, 9.0);
331        let linear2 = Vector3D::new(10.0, 11.0, 12.0);
332        let motion2 = SpatialMotion::from_parts(angular2, linear2);
333
334        let mut matrix = Matrix6::zeros();
335        let angular_so3 = SO3::from_vector3d(&angular1);
336        let linear_so3 = SO3::from_vector3d(&linear1);
337        matrix.view_mut((0, 0), (3, 3)).copy_from(&angular_so3.0);
338        matrix.view_mut((0, 3), (3, 3)).copy_from(&linear_so3.0);
339        matrix.view_mut((3, 3), (3, 3)).copy_from(&angular_so3.0);
340
341        let expected_cross = SpatialMotion(matrix * motion2.0);
342        let result_cross = motion1.cross(&motion2);
343        let expected_cross_star = SpatialMotion(-matrix.transpose() * motion2.0);
344        let result_cross_star = motion1.cross_star(&motion2);
345
346        assert_relative_eq!(result_cross.0, expected_cross.0);
347        assert_relative_eq!(result_cross_star.0, expected_cross_star.0);
348    }
349}
dynamics_spatial/motion.rs

dynamics_spatial/
motion.rs
