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