orbita3d_kinematics/position/
inverse.rs1use nalgebra::{Matrix2, Matrix3, Rotation3, Vector2, Vector3};
2use std::f64::consts::PI;
3
4const TOLERANCE_ZERO_YAW: f64 = 1e-6; use crate::{conversion, Orbita3dKinematicsModel};
7
8#[derive(Debug)]
9pub enum InverseSolutionErrorKind {
11 NoSolution(Rotation3<f64>),
13 InvalidSolution(Rotation3<f64>, Vector3<f64>),
15}
16impl std::fmt::Display for InverseSolutionErrorKind {
17 fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
18 match self {
19 InverseSolutionErrorKind::NoSolution(rot) => {
20 write!(f, "No solution found for rotation matrix: {}", rot)
21 }
22 InverseSolutionErrorKind::InvalidSolution(rot, gammas) => write!(
23 f,
24 "Invalid solution found for rotation matrix: {} (gammas: {}) => disk angles are out of bounds.",
25 rot, gammas
26 ),
27 }
28 }
29}
30impl std::error::Error for InverseSolutionErrorKind {}
31
32impl Orbita3dKinematicsModel {
33 pub fn compute_inverse_kinematics(
42 &self,
43 rot: Rotation3<f64>,
44 ) -> Result<[f64; 3], InverseSolutionErrorKind> {
45 let v = self.platform_unit_vectors_from_mat(rot).transpose();
46
47 let thetas = self.find_thetas_from_v(v);
48 for t in thetas.iter() {
49 if t.is_nan() {
50 return Err(InverseSolutionErrorKind::NoSolution(rot));
51 }
52 }
53
54 let mut d1 = thetas[0];
56 let mut d2 = thetas[1] - 120.0_f64.to_radians();
57 let mut d3 = thetas[2] + 120.0_f64.to_radians();
58 d1 = d1.sin().atan2(d1.cos());
59 d2 = d2.sin().atan2(d2.cos());
60 d3 = d3.sin().atan2(d3.cos());
61
62 Ok([d1, d2, d3])
63 }
64
65 pub fn compute_inverse_kinematics_rpy_multiturn(
66 &self,
67 target_rpy: [f64; 3],
68 ) -> Result<[f64; 3], InverseSolutionErrorKind> {
69 let rot = conversion::intrinsic_roll_pitch_yaw_to_matrix(
70 target_rpy[0],
71 target_rpy[1],
72 target_rpy[2],
73 );
74 let mut multiturn_offset: f64 = 0.0;
75
76 let thetas = self.compute_inverse_kinematics(rot)?; let mut thetas: [f64; 3] = self.compute_valid_solution(target_rpy, thetas)?; log::debug!("valid Thetas {:?}", thetas);
82 let true_yaw = target_rpy[2] + self.offset; if true_yaw.abs() >= std::f64::consts::PI {
85 let nb_turns = (true_yaw / std::f64::consts::TAU).trunc(); if nb_turns.abs() >= 1.0 {
90 multiturn_offset = std::f64::consts::TAU * (nb_turns);
91 }
92 if true_yaw.abs().rem_euclid(std::f64::consts::TAU) >= std::f64::consts::PI
95 && !(thetas[0].signum() == thetas[1].signum()
96 && thetas[1].signum() == thetas[2].signum())
97 {
98 multiturn_offset += target_rpy[2].signum() * std::f64::consts::TAU
99 }
100
101 log::debug!("Yaw more than Pi, nb full turns: {nb_turns}, yaw%2pi: {:?} offset: {multiturn_offset} theta before: {:?}",true_yaw.abs().rem_euclid(std::f64::consts::TAU),thetas);
102
103 log::debug!("thetas {:?}", thetas);
104
105 thetas.iter_mut().for_each(|x| *x += multiturn_offset);
106
107 log::debug!("Thetas after offset: {:?}", thetas);
108 }
109
110 Ok(thetas)
111 }
112
113 pub fn check_gammas(&self, thetas: Vector3<f64>) -> Result<(), Box<dyn std::error::Error>> {
114 let gammas = compute_gammas(thetas);
115 for g in gammas.iter() {
117 if !((*g > self.gamma_min) && (*g < self.gamma_max)) {
118 let msg = format!(
119 "Gammas out of range: ! {:?} < {:?} < {:?} (thetas {:?})",
120 self.gamma_min, gammas, self.gamma_max, thetas
121 );
122 return Err((msg).into());
123 }
124 }
125 Ok(())
126 }
127
128 pub fn compute_valid_solution(
129 &self,
130 target_rpy: [f64; 3],
131 mut thetas: [f64; 3],
132 ) -> Result<[f64; 3], InverseSolutionErrorKind> {
133 const NBSOLS: i32 = 8;
148 let mut all_solutions = [[0.0_f64; 3]; NBSOLS as usize];
149 for i in 0..NBSOLS {
153 for j in 0..3 {
154 let val = NBSOLS * j + i;
155 let ret = 1 & (val >> j);
156 if ret != 0 {
157 all_solutions[i as usize][j as usize] = thetas[j as usize];
158 } else {
159 all_solutions[i as usize][j as usize] =
160 thetas[j as usize] - thetas[j as usize].signum() * std::f64::consts::TAU;
161 }
162 }
163 }
164 let mut validvec = Vec::new();
165 for sol in all_solutions {
166 match self.check_gammas(sol.into()) {
167 Ok(()) => validvec.push(sol),
168 Err(_) => continue,
169 }
170 }
171 log::debug!(
172 "all solutions: {:?}\nvalid solutions: {:?}",
173 all_solutions,
174 validvec
175 );
176 if validvec.len() == 1 {
178 thetas = validvec[0];
179 } else {
180 if validvec.is_empty() {
181 log::debug!(
182 "NO VALID SOLUTION! target: {:?}\n thetas: {:?}\nall_solutions: {:?}",
183 target_rpy,
184 thetas,
185 all_solutions
186 );
187 let rot = conversion::intrinsic_roll_pitch_yaw_to_matrix(
188 target_rpy[0],
189 target_rpy[1],
190 target_rpy[2],
191 );
192 return Err(InverseSolutionErrorKind::InvalidSolution(
193 rot,
194 compute_gammas(thetas.into()),
195 ));
196 }
197
198 let mut yaw_sign = (target_rpy[2] + self.offset).signum();
200 let mut theta_sign = validvec[0][0].signum();
201
202 if (target_rpy[2] + self.offset).abs() < TOLERANCE_ZERO_YAW {
204 yaw_sign = 0.0;
205 }
206 if validvec[0][0].abs() < TOLERANCE_ZERO_YAW {
207 theta_sign = 0.0;
208 }
209 if theta_sign == yaw_sign {
212 thetas = validvec[0];
213 } else {
214 thetas = validvec[1];
215 }
216 }
217 Ok(thetas)
219 }
220
221 fn find_thetas_from_v(&self, v: Matrix3<f64>) -> Vector3<f64> {
222 let mut thetas = Vector3::zeros();
223 for i in 0..3 {
224 let a_i =
225 -self.alpha.sin() * v.row(i)[0] - self.alpha.cos() * v.row(i)[2] - self.beta.cos();
226 let b_i = self.alpha.sin() * v.row(i)[1];
227 let c_i =
228 self.alpha.sin() * v.row(i)[0] - self.alpha.cos() * v.row(i)[2] - self.beta.cos();
229
230 let mut solutions_theta;
231
232 if a_i.abs() <= 1.5 * f64::EPSILON {
234 let unique_sol = -c_i / (2.0 * b_i);
235 solutions_theta = [unique_sol.atan() * 2.0, PI];
236 }
237 else {
239 let d_i = b_i.powi(2) - a_i * c_i;
240 if d_i < 0.0 {
241 thetas[i] = f64::NAN;
242 continue;
243 }
244
245 let dual_sol = [(-b_i + d_i.sqrt()) / a_i, (-b_i - d_i.sqrt()) / a_i];
246
247 solutions_theta = dual_sol.map(|v| v.atan() * 2.0);
248 }
249
250 solutions_theta = solutions_theta.map(|v| v.rem_euclid(2.0 * std::f64::consts::PI));
251
252 if solutions_theta[0].is_nan() && solutions_theta[1].is_nan() {
253 thetas[i] = f64::NAN;
254 continue;
255 }
256
257 let v_theta = Matrix2::from_columns(&[
258 Vector2::from_row_slice(&[solutions_theta[0].cos(), solutions_theta[0].sin()]),
259 Vector2::from_row_slice(&[solutions_theta[1].cos(), solutions_theta[1].sin()]),
260 ]);
261
262 let mut theta = 0.0;
263
264 let v_i = Vector2::from_iterator(v.row(i).columns(0, 2).transpose().iter().cloned());
265
266 if self.passiv_arms_direct {
267 for (j, &sol) in solutions_theta.iter().enumerate() {
268 let v_theta_ij = Vector2::from_iterator(v_theta.columns(j, 1).iter().cloned());
269
270 if v_theta_ij.perp(&v_i) >= 0.0 {
271 theta = sol;
272 }
273 }
274 } else {
275 for (j, &sol) in solutions_theta.iter().enumerate() {
276 let v_theta_ij = Vector2::from_iterator(v_theta.columns(j, 1).iter().cloned());
277 if v_theta_ij.perp(&v_i) < 0.0 {
278 theta = sol;
279 }
280 }
281 }
282 thetas[i] = theta;
283 }
284
285 thetas
286 }
287}
288
289fn compute_gammas(thetas: Vector3<f64>) -> Vector3<f64> {
290 Vector3::from_row_slice(&[
293 120.0_f64.to_radians() + (thetas[1] - thetas[0]),
294 120.0_f64.to_radians() + (thetas[2] - thetas[1]),
295 120.0_f64.to_radians() + (thetas[0] - thetas[2]),
296 ])
297}
298
299#[cfg(test)]
300mod tests {
301 use crate::conversion::intrinsic_roll_pitch_yaw_to_matrix;
302
303 use super::*;
304
305 #[test]
306 fn ik_zero() {
307 let orb = Orbita3dKinematicsModel::default();
308
309 let rot = intrinsic_roll_pitch_yaw_to_matrix(0.0, 0.0, 0.0);
310 let thetas = orb.compute_inverse_kinematics(rot).unwrap();
311 assert!(thetas[0].abs() < 1e-4);
312 assert!(thetas[1].abs() < 1e-4);
313 assert!(thetas[2].abs() < 1e-4);
314 }
315
316 #[test]
317 fn pitch_only() {
318 let orb = Orbita3dKinematicsModel::default();
323 let rot = intrinsic_roll_pitch_yaw_to_matrix(0.0, 0.3, 0.0);
324 let thetas = orb.compute_inverse_kinematics(rot).unwrap();
325 assert!(thetas[0].abs() < 1e-4);
326 assert!(thetas[1].abs() > 1e-4);
327 assert!(thetas[2].abs() > 1e-4);
328 }
329 #[test]
330 fn gammas() {
331 let orb = Orbita3dKinematicsModel::default();
332
333 let rot = intrinsic_roll_pitch_yaw_to_matrix(0.0, 0.0, 0.0);
334 let thetas = orb.compute_inverse_kinematics(rot).unwrap(); let gammas = compute_gammas(Vector3::from_row_slice(&[thetas[0], thetas[1], thetas[2]]));
337
338 assert!((gammas[0] - 120.0_f64.to_radians()).abs() < 1e-4);
339 assert!((gammas[1] - 120.0_f64.to_radians()).abs() < 1e-4);
340 assert!((gammas[2] - 120.0_f64.to_radians()).abs() < 1e-4);
341 }
342
343 #[test]
344 fn gammas_range() {
345 let orb = Orbita3dKinematicsModel::default();
346
347 let thetas: [f64; 3] = [
348 0.0_f64.to_radians(),
349 -120.0_f64.to_radians() + orb.gamma_min,
350 120.0_f64.to_radians() + orb.gamma_max,
351 ];
352
353 if let Ok(()) =
365 orb.check_gammas(Vector3::from_row_slice(&[thetas[0], thetas[1], thetas[2]]))
366 {
367 panic!()
368 }
369 }
370
371 #[test]
372 fn valid_close_to_zero() {
373 let orb = Orbita3dKinematicsModel::default();
374
375 let rpy = [
376 -1.638_039_316_827_991_8e-8,
377 -6.741_158_650_578_781e-9,
378 -1.220_747_654_483_793_3e-9,
379 ];
380 let rot = intrinsic_roll_pitch_yaw_to_matrix(rpy[0], rpy[1], rpy[2]);
381 let thetas = orb.compute_inverse_kinematics(rot).unwrap();
382
383 let valid_thetas = orb.compute_valid_solution(rpy, thetas).unwrap();
384
385 assert!(valid_thetas[0].abs() < 1e-4);
386 assert!(valid_thetas[1].abs() < 1e-4);
387 assert!(valid_thetas[2].abs() < 1e-4);
388 }
389}