dynamics_spatial/
force.rs

1//! Defines spatial **forces** and related operations.
2
3use std::ops::{Add, AddAssign, Sub, SubAssign};
4
5use nalgebra::{Matrix6, Vector6};
6
7use crate::{
8    se3::{ActSE3, SE3},
9    so3::SO3,
10    vector3d::Vector3D,
11    vector6d::Vector6D,
12};
13
14/// Spatial force vector, combining torque and force components.
15///
16/// A spatial force is represented as a 6-dimensional vector,
17/// which can be decomposed into $\begin{bmatrix} f & \tau \end{bmatrix}$,
18/// where $f$ is the force (translational component) and $\tau$ is the torque (rotational component).
19#[derive(Debug, Clone, PartialEq)]
20pub struct SpatialForce(pub Vector6<f64>);
21
22impl SpatialForce {
23    /// Creates a new [`SpatialForce`] from the given force and torque components.
24    ///
25    /// # Arguments
26    ///
27    /// * `force` - The force component of the spatial force.
28    /// * `torque` - The torque component of the spatial force.
29    ///
30    /// # Returns
31    /// A new [`SpatialForce`] object combining the given torque and force.
32    pub fn from_parts(force: Vector3D, torque: Vector3D) -> Self {
33        let mut data = Vector6::zeros();
34        data.fixed_rows_mut::<3>(0).copy_from(&force.0);
35        data.fixed_rows_mut::<3>(3).copy_from(&torque.0);
36        Self(data)
37    }
38
39    /// Creates a new [`SpatialForce`] with zeros components.
40    pub fn zero() -> Self {
41        Self(Vector6::zeros())
42    }
43
44    /// Creates a new [`SpatialForce`] from the given [`nalgebra::Vector6`]
45    pub fn from_vector6(vector: Vector6<f64>) -> Self {
46        Self(vector)
47    }
48
49    /// Creates a new [`SpatialForce`] from the given [`Vector6D`]
50    pub fn from_vector6d(vector: Vector6D) -> Self {
51        Self(vector.0)
52    }
53
54    /// Returns the force's rotation (last 3 components)
55    pub fn rotation(&self) -> Vector3D {
56        Vector3D(self.0.fixed_rows::<3>(3).into())
57    }
58
59    /// Returns the force's translation (first 3 components)
60    pub fn translation(&self) -> Vector3D {
61        Vector3D(self.0.fixed_rows::<3>(0).into())
62    }
63
64    /// Constructs the cross product matrix for spatial motion vectors.
65    ///
66    /// # Arguments
67    /// * `angular` - The angular velocity component (3D vector).
68    /// * `linear` - The linear velocity component (3D vector).
69    ///
70    /// # Returns
71    /// A 6x6 matrix representing the cross product operation.
72    fn cross_matrix(angular: Vector3D, linear: Vector3D) -> Matrix6<f64> {
73        let mut cross_matrix = Matrix6::zeros();
74        let angular_so3 = SO3::from_vector3d(&angular);
75        let linear_so3 = SO3::from_vector3d(&linear);
76        cross_matrix
77            .view_mut((0, 0), (3, 3))
78            .copy_from(&angular_so3.0);
79        cross_matrix
80            .view_mut((0, 3), (3, 3))
81            .copy_from(&linear_so3.0);
82        cross_matrix
83            .view_mut((3, 3), (3, 3))
84            .copy_from(&angular_so3.0);
85        cross_matrix
86    }
87
88    /// Computes the cross product of two spatial force vectors.
89    ///
90    /// # Arguments
91    /// * `other` - The other spatial force vector to compute the cross product with.
92    ///
93    /// # Returns
94    /// A new `SpatialForce` representing the cross product.
95    #[must_use]
96    pub fn cross(&self, other: &SpatialForce) -> SpatialForce {
97        let angular = self.rotation();
98        let linear = self.translation();
99
100        let cross_matrix = SpatialForce::cross_matrix(angular, linear);
101
102        SpatialForce(cross_matrix * other.0)
103    }
104
105    /// Computes the inner product of two spatial forces.
106    #[must_use]
107    pub fn inner(&self, other: &SpatialForce) -> f64 {
108        self.0.dot(&other.0)
109    }
110}
111
112impl Add for SpatialForce {
113    type Output = SpatialForce;
114
115    fn add(self, rhs: Self) -> Self::Output {
116        SpatialForce(self.0 + rhs.0)
117    }
118}
119
120impl Add<&SpatialForce> for SpatialForce {
121    type Output = SpatialForce;
122
123    fn add(self, rhs: &Self) -> Self::Output {
124        SpatialForce(self.0 + rhs.0)
125    }
126}
127
128impl AddAssign for SpatialForce {
129    fn add_assign(&mut self, rhs: Self) {
130        self.0 += rhs.0;
131    }
132}
133
134impl AddAssign<&SpatialForce> for SpatialForce {
135    fn add_assign(&mut self, rhs: &Self) {
136        self.0 += rhs.0;
137    }
138}
139
140impl Sub for SpatialForce {
141    type Output = SpatialForce;
142
143    fn sub(self, rhs: Self) -> Self::Output {
144        SpatialForce(self.0 - rhs.0)
145    }
146}
147
148impl Sub<&SpatialForce> for SpatialForce {
149    type Output = SpatialForce;
150
151    fn sub(self, rhs: &Self) -> Self::Output {
152        SpatialForce(self.0 - rhs.0)
153    }
154}
155
156impl SubAssign for SpatialForce {
157    fn sub_assign(&mut self, rhs: Self) {
158        self.0 -= rhs.0;
159    }
160}
161
162impl SubAssign<&SpatialForce> for SpatialForce {
163    fn sub_assign(&mut self, rhs: &Self) {
164        self.0 -= rhs.0;
165    }
166}
167
168impl ActSE3 for SpatialForce {
169    fn act(&self, transform: &SE3) -> Self {
170        let rotation = transform.rotation();
171        let translation = transform.translation();
172
173        let force = rotation * &self.translation();
174        let torque = rotation * &self.rotation() + translation.cross(&force);
175
176        SpatialForce::from_parts(force, torque)
177    }
178
179    fn act_inv(&self, _transform: &SE3) -> Self {
180        unimplemented!()
181    }
182}
dynamics_spatial/force.rs

dynamics_spatial/
force.rs
