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

dynamics_spatial/
force.rs
