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