orbita3d_kinematics/
jacobian.rs

1use nalgebra::{Matrix3, Rotation3, RowVector3};
2
3use crate::Orbita3dKinematicsModel;
4
5impl Orbita3dKinematicsModel {
6    /// Compute the inverse jacobian
7    ///
8    /// Compute the inverse jacobian from the platform orientation and the motor angles.
9    ///
10    /// # Arguments
11    /// * rot - The platform orientation as a rotation matrix.
12    /// * thetas - The motor angles as a 3-element array.
13    /// # Returns
14    /// * The inverse jacobian as a 3x3 matrix.
15    pub fn jacobian_inverse(&self, rot: Rotation3<f64>, thetas: [f64; 3]) -> Matrix3<f64> {
16        let v = self.platform_unit_vectors_from_mat(rot);
17
18        let sa1 = self.alpha.sin();
19        let ca1 = self.alpha.cos();
20        let st1 = thetas[0].sin();
21        let ct1 = thetas[0].cos();
22        let st2 = thetas[1].sin();
23        let ct2 = thetas[1].cos();
24        let st3 = thetas[2].sin();
25        let ct3 = thetas[2].cos();
26
27        let row1_denom = -sa1 * st1 * v.row(0)[0] + sa1 * ct1 * v.row(0)[1];
28        let row2_denom = -sa1 * st2 * v.row(1)[0] + sa1 * ct2 * v.row(1)[1];
29        let row3_denom = -sa1 * st3 * v.row(2)[0] + sa1 * ct3 * v.row(2)[1];
30
31        let row1 = RowVector3::new(
32            (sa1 * st1 * v.row(0)[2] + ca1 * v.row(0)[1]) / row1_denom,
33            (-sa1 * ct1 * v.row(0)[2] - ca1 * v.row(0)[0]) / row1_denom,
34            1.0,
35        );
36        let row2 = RowVector3::new(
37            (sa1 * st2 * v.row(1)[2] + ca1 * v.row(1)[1]) / row2_denom,
38            (-sa1 * ct2 * v.row(1)[2] - ca1 * v.row(1)[0]) / row2_denom,
39            1.0,
40        );
41        let row3 = RowVector3::new(
42            (sa1 * st3 * v.row(2)[2] + ca1 * v.row(2)[1]) / row3_denom,
43            (-sa1 * ct3 * v.row(2)[2] - ca1 * v.row(2)[0]) / row3_denom,
44            1.0,
45        );
46
47        Matrix3::from_rows(&[row1, row2, row3])
48    }
49
50    /// Compute the jacobian
51    ///
52    /// The jacobian is computed by inverting the inverse jacobian.
53    ///
54    /// # Arguments
55    /// * rot - The platform orientation as a rotation matrix.
56    /// * thetas - The motor angles as a 3-element array.
57    /// # Returns
58    /// * The jacobian as a 3x3 matrix.
59    pub fn jacobian(&self, rot: Rotation3<f64>, thetas: [f64; 3]) -> Matrix3<f64> {
60        let j_inv = self.jacobian_inverse(rot, thetas);
61        j_inv.try_inverse().unwrap()
62    }
63}
orbita3d_kinematics/jacobian.rs

orbita3d_kinematics/
jacobian.rs
