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        // has to be trasponsed here - not sure why (otherwise v.row(0) takes the frist column rather than row)
17        // TODO verify furhter and find why this happens
18        let v = self.platform_unit_vectors_from_mat(rot).transpose();
19
20        let sa1 = self.alpha.sin();
21        let ca1 = self.alpha.cos();
22        let st1 = thetas[0].sin();
23        let ct1 = thetas[0].cos();
24        // these thetas need to have 120 degrees added or subtracted
25        let st2 = (thetas[1] + 120.0_f64.to_radians()).sin();
26        let ct2 = (thetas[1] + 120.0_f64.to_radians()).cos();
27        let st3 = (thetas[2] - 120.0_f64.to_radians()).sin();
28        let ct3 = (thetas[2] - 120.0_f64.to_radians()).cos();
29
30        let row1_denom = -sa1 * st1 * v.row(0)[0] + sa1 * ct1 * v.row(0)[1];
31        let row2_denom = -sa1 * st2 * v.row(1)[0] + sa1 * ct2 * v.row(1)[1];
32        let row3_denom = -sa1 * st3 * v.row(2)[0] + sa1 * ct3 * v.row(2)[1];
33
34        let row1 = RowVector3::new(
35            (sa1 * st1 * v.row(0)[2] + ca1 * v.row(0)[1]) / row1_denom,
36            (-sa1 * ct1 * v.row(0)[2] - ca1 * v.row(0)[0]) / row1_denom,
37            1.0,
38        );
39        let row2 = RowVector3::new(
40            (sa1 * st2 * v.row(1)[2] + ca1 * v.row(1)[1]) / row2_denom,
41            (-sa1 * ct2 * v.row(1)[2] - ca1 * v.row(1)[0]) / row2_denom,
42            1.0,
43        );
44        let row3 = RowVector3::new(
45            (sa1 * st3 * v.row(2)[2] + ca1 * v.row(2)[1]) / row3_denom,
46            (-sa1 * ct3 * v.row(2)[2] - ca1 * v.row(2)[0]) / row3_denom,
47            1.0,
48        );
49
50        Matrix3::from_rows(&[row1, row2, row3])
51    }
52
53    /// Compute the jacobian
54    ///
55    /// The jacobian is computed by inverting the inverse jacobian.
56    ///
57    /// # Arguments
58    /// * rot - The platform orientation as a rotation matrix.
59    /// * thetas - The motor angles as a 3-element array.
60    /// # Returns
61    /// * The jacobian as a 3x3 matrix.
62    pub fn jacobian(&self, rot: Rotation3<f64>, thetas: [f64; 3]) -> Matrix3<f64> {
63        let j_inv = self.jacobian_inverse(rot, thetas);
64        j_inv.try_inverse().unwrap()
65    }
66}
orbita3d_kinematics/jacobian.rs

orbita3d_kinematics/
jacobian.rs
