orbita3d_kinematics/
jacobian.rs1use nalgebra::{Matrix3, Rotation3, RowVector3};
2
3use crate::Orbita3dKinematicsModel;
4
5impl Orbita3dKinematicsModel {
6 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 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}