dynamics_spatial/
jacobian.rs

1//! Defines a **Jacobian** structure and related operations.
2
3use std::ops::Mul;
4
5use nalgebra::{DMatrix, Matrix6x1};
6
7use crate::{force::SpatialForce, vector6d::Vector6D};
8
9/// Jacobian matrix, relating joint velocities to end-effector velocities.
10///
11/// The Jacobian is a matrix of size $6 \times n_v$ where $n_v$ is the number of joints in the robot model.
12/// Each column corresponds to the basis of the spatial velocity
13/// of the joint in the origin of the inertial frame.
14#[derive(Debug, Clone, PartialEq)]
15pub struct Jacobian(pub(crate) DMatrix<f64>);
16
17impl Jacobian {
18    /// Creates a zero Jacobian matrix with the given number of columns.
19    ///
20    /// # Arguments
21    //// * `cols` - The number of columns of the Jacobian matrix, which corresponds to the number of joints in the robot model.
22    ///
23    /// # Returns
24    /// * A zero Jacobian matrix of size $6 \times cols$.
25    pub fn zero(cols: usize) -> Self {
26        Self(DMatrix::zeros(6, cols))
27    }
28
29    /// Updates the specified column of the Jacobian matrix with the given data.
30    ///
31    /// # Arguments
32    /// * `v_offset` - The index of the column to update, which corresponds to the joint index in the robot model.
33    /// * `column_data` - The data to update the column with, which should be a 6-dimensional vector representing the spatial velocity basis of the joint.
34    pub fn update_column(&mut self, v_offset: usize, column_data: &[f64; 6]) {
35        self.0
36            .fixed_columns_mut::<1>(v_offset)
37            .copy_from(&DMatrix::from_column_slice(6, 1, column_data))
38    }
39
40    /// Returns a specified column of the Jacobian matrix as a `JacobianColumn` structure.
41    ///
42    /// # Arguments
43    /// * `v_offset` - The index of the column to retrieve, which corresponds to the joint index in the robot model.
44    /// # Returns
45    /// * A `JacobianColumn` structure containing the specified column of the Jacobian matrix, which represents the spatial velocity basis of the joint.
46    pub fn column(&self, v_offset: usize) -> JacobianColumn {
47        JacobianColumn(Matrix6x1::from_column_slice(
48            self.0.column(v_offset).as_slice(),
49        ))
50    }
51}
52
53/// A column of the Jacobian matrix, representing the spatial velocity basis of a joint.
54pub struct JacobianColumn(pub Matrix6x1<f64>);
55
56impl Mul<&SpatialForce> for &JacobianColumn {
57    type Output = f64;
58
59    fn mul(self, rhs: &SpatialForce) -> Self::Output {
60        self.0.dot(&rhs.0)
61    }
62}
63
64impl Mul<&Vector6D> for &JacobianColumn {
65    type Output = f64;
66
67    fn mul(self, rhs: &Vector6D) -> Self::Output {
68        self.0.dot(&rhs.0)
69    }
70}
dynamics_spatial/jacobian.rs

dynamics_spatial/
jacobian.rs
