orbita3d_kinematics/
velocity.rs

1use nalgebra::{Matrix3, Vector3};
2
3use crate::Orbita3dKinematicsModel;
4
5impl Orbita3dKinematicsModel {
6    /// Compute the forward velocity
7    ///
8    /// Compute the output velocity (platform oriented velocity) from the input velocity (motors velocity) and the motor angles.
9    ///
10    /// # Arguments
11    /// * thetas - The motor angles as a 3-element array.
12    /// * input_velocity - The input velocity as a 3-element array (motors velocity).
13    /// # Returns
14    /// * The output velocity as a 3d rotation. This rotation is a axis-angle rotation vector representing the velocity pseudo vector.
15    pub fn compute_output_velocity(
16        &self,
17        thetas: [f64; 3],
18        input_velocity: [f64; 3],
19    ) -> Vector3<f64> {
20        let rot = self.compute_forward_kinematics(thetas);
21
22        let j_inv = self.jacobian_inverse(rot, thetas);
23        self.compute_output_velocity_from_j_inv(j_inv, input_velocity.into())
24    }
25
26    /// Compute the inverse velocity
27    ///
28    /// Compute the input velocity (motors velocity) from the output velocity (platform oriented velocity) and the motor angles.
29    ///
30    /// # Arguments
31    /// * thetas - The motor angles as a 3-element array.
32    /// * output_velocity - The output velocity as a 3d rotation axis-angle velocity pseudo-vector.
33    /// # Returns
34    /// * The input velocity as a 3-element array.
35    pub fn compute_input_velocity(
36        &self,
37        thetas: [f64; 3],
38        output_velocity: Vector3<f64>,
39    ) -> [f64; 3] {
40        let rot = self.compute_forward_kinematics([thetas[0], thetas[1], thetas[2]]);
41        let j_inv = self.jacobian_inverse(rot, thetas);
42        self.compute_input_velocity_from_j_inv(j_inv, output_velocity)
43            .into()
44    }
45
46    fn compute_input_velocity_from_j_inv(
47        &self,
48        j_inv: Matrix3<f64>,
49        output_vel: Vector3<f64>,
50    ) -> Vector3<f64> {
51        let j = j_inv.pseudo_inverse(1.0e-6).unwrap();
52        j * output_vel
53    }
54
55    fn compute_output_velocity_from_j_inv(
56        &self,
57        j_inv: Matrix3<f64>,
58        input_vel: Vector3<f64>,
59    ) -> Vector3<f64> {
60        j_inv * input_vel
61    }
62}
63
64#[cfg(test)]
65mod tests {
66    use crate::{conversion, Orbita3dKinematicsModel};
67
68    use rand::Rng;
69
70    const ROLL_RANGE: f64 = 30.0;
71    const PITCH_RANGE: f64 = 30.0;
72    const YAW_RANGE: f64 = 90.0;
73
74    fn random_rpy() -> [f64; 3] {
75        let mut rng = rand::thread_rng();
76
77        let roll = rng.gen_range(-ROLL_RANGE..ROLL_RANGE).to_radians();
78        let pitch = rng.gen_range(-PITCH_RANGE..PITCH_RANGE).to_radians();
79        let yaw = rng.gen_range(-YAW_RANGE..YAW_RANGE).to_radians();
80
81        [roll, pitch, yaw]
82    }
83
84    fn check_inverse_forward(thetas: [f64; 3], input_velocity: [f64; 3]) {
85        let orb = Orbita3dKinematicsModel::default();
86
87        let output_velocity = orb.compute_output_velocity(thetas, input_velocity);
88        let reconstructed = orb.compute_input_velocity(thetas, output_velocity);
89
90        for i in 0..3 {
91            assert!(
92                (input_velocity[i] - reconstructed[i]).abs() < 1e-2,
93                "Fail for\n thetas: {:?}\n input velocity: {:?}\n rec: {:?}\n",
94                thetas,
95                input_velocity,
96                reconstructed
97            );
98        }
99    }
100
101    #[test]
102    fn inverse_forward_vel() {
103        // Using fixed value 1
104        let thetas = [
105            0.147376526054817,
106            -0.0063153266133482155,
107            0.29099962984161976,
108        ];
109        let input_velocity = [0.6696758700667225, 0.1914613976070494, -0.3389136179061003];
110        check_inverse_forward(thetas, input_velocity);
111
112        // Using fixed value 2
113        let thetas = [
114            -0.6799726966192987,
115            -1.1128173034407476,
116            -0.8489251256361031,
117        ];
118        let input_velocity = [0.7810543324281887, -0.4502710350767902, 0.6821691832152244];
119        check_inverse_forward(thetas, input_velocity);
120
121        // // Using random values
122        let orb = Orbita3dKinematicsModel::default();
123
124        let rpy: [f64; 3] = random_rpy();
125
126        let rot = conversion::intrinsic_roll_pitch_yaw_to_matrix(rpy[0], rpy[1], rpy[2]);
127        let thetas = orb.compute_inverse_kinematics(rot).unwrap();
128
129        let mut rng = rand::thread_rng();
130        let input_velocity = [
131            rng.gen_range(-1.0..1.0),
132            rng.gen_range(-1.0..1.0),
133            rng.gen_range(-1.0..1.0),
134        ];
135        check_inverse_forward(thetas, input_velocity);
136    }
137}
orbita3d_kinematics/velocity.rs

orbita3d_kinematics/
velocity.rs
