orbita3d_kinematics/
torque.rs

1use nalgebra::{Matrix3, Vector3};
2
3use crate::Orbita3dKinematicsModel;
4
5impl Orbita3dKinematicsModel {
6    /// Compute the forward static torque
7    ///
8    /// Compute the output static torque (platform oriented torque) from the input torque (motors torque) and the motor angles.
9    ///
10    /// # Arguments
11    /// * thetas - The motor angles as a 3-element array.
12    /// * input_torque - The input torque as a 3-element array.
13    /// # Returns
14    /// * The output torque as a 3d pseudo vector.
15    pub fn compute_output_torque(&self, thetas: [f64; 3], input_torque: [f64; 3]) -> Vector3<f64> {
16        let rot = self.compute_forward_kinematics(thetas);
17
18        let j_inv = self.jacobian_inverse(rot, thetas);
19        self.compute_output_torque_from_j_inv(j_inv, input_torque.into())
20    }
21
22    /// Compute the inverse static torque
23    ///
24    /// Compute the input torque (motors torque) from the output torque (platform oriented torque) and the motor angles.
25    ///
26    /// # Arguments
27    /// * thetas - The motor angles as a 3-element array.
28    /// * output_torque - The output torque as a 3d pseudo-vector.
29    /// # Returns
30    /// * The input torque as a 3-element array.
31    pub fn compute_input_torque(&self, thetas: [f64; 3], output_torque: Vector3<f64>) -> [f64; 3] {
32        let rot = self.compute_forward_kinematics(thetas);
33        let j_inv = self.jacobian_inverse(rot, thetas);
34
35        self.compute_input_torque_from_j_inv(j_inv, output_torque)
36            .into()
37    }
38
39    fn compute_output_torque_from_j_inv(
40        &self,
41        j_inv: Matrix3<f64>,
42        input_torque: Vector3<f64>,
43    ) -> Vector3<f64> {
44        j_inv.transpose() * input_torque
45    }
46
47    fn compute_input_torque_from_j_inv(
48        &self,
49        j_inv: Matrix3<f64>,
50        output_torque: Vector3<f64>,
51    ) -> Vector3<f64> {
52        let j = j_inv.pseudo_inverse(0.000001).unwrap();
53        j.transpose() * output_torque
54    }
55}
56
57#[cfg(test)]
58mod tests {
59    use crate::{conversion::intrinsic_roll_pitch_yaw_to_matrix, Orbita3dKinematicsModel};
60
61    use rand::Rng;
62
63    const ROLL_RANGE: f64 = 30.0;
64    const PITCH_RANGE: f64 = 30.0;
65    const YAW_RANGE: f64 = 90.0;
66
67    fn random_rpy() -> [f64; 3] {
68        let mut rng = rand::thread_rng();
69
70        let roll = rng.gen_range(-ROLL_RANGE..ROLL_RANGE).to_radians();
71        let pitch = rng.gen_range(-PITCH_RANGE..PITCH_RANGE).to_radians();
72        let yaw = rng.gen_range(-YAW_RANGE..YAW_RANGE).to_radians();
73
74        [roll, pitch, yaw]
75    }
76
77    #[test]
78    fn inverse_forward_torque() {
79        let orb = Orbita3dKinematicsModel::default();
80
81        let rpy = random_rpy();
82        let rot = intrinsic_roll_pitch_yaw_to_matrix(rpy[0], rpy[1], rpy[2]);
83
84        let thetas = orb.compute_inverse_kinematics(rot).unwrap();
85
86        let mut rng = rand::thread_rng();
87        let input_torque = [
88            rng.gen_range(-1.0..1.0),
89            rng.gen_range(-1.0..1.0),
90            rng.gen_range(-1.0..1.0),
91        ];
92
93        let output_torque = orb.compute_output_torque(thetas, input_torque);
94        let reconstructed = orb.compute_input_torque(thetas, output_torque);
95
96        assert!(
97            (input_torque[0] - reconstructed[0]).abs() < 1e-2,
98            "Fail for\n thetas: {:?}\n input torque: {:?}\n rec: {:?}\n",
99            thetas,
100            input_torque,
101            reconstructed
102        );
103        assert!(
104            (input_torque[1] - reconstructed[1]).abs() < 1e-2,
105            "Fail for\n thetas: {:?}\n input torque: {:?}\n rec: {:?}\n",
106            thetas,
107            input_torque,
108            reconstructed
109        );
110        assert!(
111            (input_torque[2] - reconstructed[2]).abs() < 1e-2,
112            "Fail for\n thetas: {:?}\n input torque: {:?}\n rec: {:?}\n",
113            thetas,
114            input_torque,
115            reconstructed
116        );
117    }
118}
orbita3d_kinematics/torque.rs

orbita3d_kinematics/
torque.rs
