orbita3d_kinematics/position/
inverse.rs

1use nalgebra::{Matrix2, Matrix3, Rotation3, Vector2, Vector3};
2use std::f64::consts::PI;
3
4const TOLERANCE_ZERO_YAW: f64 = 1e-6; // Define a small tolerance for near-zero values
5
6use crate::{conversion, Orbita3dKinematicsModel};
7
8#[derive(Debug)]
9/// Error that can occur when computing the inverse kinematics of the Orbita3d platform.
10pub enum InverseSolutionErrorKind {
11    /// No solution found.
12    NoSolution(Rotation3<f64>),
13    /// Invalid solution found.
14    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    /// Compute the inverse kinematics of the Orbita3d platform.
34    ///
35    /// Compute the motor angles from the platform orientation.
36    ///
37    /// # Arguments
38    /// * rot - The platform orientation as a rotation matrix.
39    /// # Returns
40    /// * The motor angles as a 3-element array, without the 120° offsets.
41    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        // remove the 120° offsets and put in [-pi ; pi]
55        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)?; // The ik returns a geometric solution with thetas in [-pi; pi] without the "natural" 120° offset (zero position is :[0, 0, 0])
77
78        // let target_rpy_no_offet = [target_rpy[0], target_rpy[1], target_rpy[2] + self.offset]; //FIXME????
79        let mut thetas: [f64; 3] = self.compute_valid_solution(target_rpy, thetas)?; //this does not seems to work correctly with offsets
80
81        log::debug!("valid Thetas {:?}", thetas);
82        // if yaw is more than Pi, we may have to deal with some edge cases
83        let true_yaw = target_rpy[2] + self.offset; //FIXME????
84        if true_yaw.abs() >= std::f64::consts::PI {
85            // Compute the k*2*Pi offset if the yaw target is more than 1 full rotation
86
87            // let nb_turns = (target_rpy[2] / std::f64::consts::TAU).trunc(); //number of full turn
88            let nb_turns = (true_yaw / std::f64::consts::TAU).trunc(); //number of full turn
89            if nb_turns.abs() >= 1.0 {
90                multiturn_offset = std::f64::consts::TAU * (nb_turns);
91            }
92            // also, if yaw.abs().rem_euclid(2.0 * PI) > pi, we might want to consider the 2pi complement
93            // if target_rpy[2].abs().rem_euclid(std::f64::consts::TAU) >= std::f64::consts::PI
94            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        // println!("CHECK GAMMAS: {:?}", gammas);
116        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        // Select the "real world" solution from the geometric one: => disks should not cross each over
134        // For each theta, there is 2 solutions (only one valid):
135        // - The return angle in [-pi, pi]
136        // - The 2pi complement
137        // We should select the one that avoid crossing the other disks and that rotates in the correct yaw direction
138        //
139        // algo:
140        // - generate all possible solutions
141        // - check the validity of the solution (gammas)
142        // - there should be maximum 2 valid set of solutions?
143        // - select the right one (physically feasible) and if there are 2 solutions, select the one with the same yaw sign
144
145        // generate solutions 2^3
146
147        const NBSOLS: i32 = 8;
148        let mut all_solutions = [[0.0_f64; 3]; NBSOLS as usize];
149        // TODO: remove extra conversion?
150        // let target = conversion::matrix_to_intrinsic_roll_pitch_yaw(rot);
151
152        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        // There is either one solution or 2 valid solutions
177        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            // here we have the 2 solutions (both 2pi complement), we chose the one with the same yaw sign
199            let mut yaw_sign = (target_rpy[2] + self.offset).signum();
200            let mut theta_sign = validvec[0][0].signum();
201
202            // If the yaw or thetas are very close to zero, treat them as effectively zero
203            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            // Compare the yaw sign and theta sign
210            // but now accounting for near-zero values
211            if theta_sign == yaw_sign {
212                thetas = validvec[0];
213            } else {
214                thetas = validvec[1];
215            }
216        }
217        // log::debug!("valid Thetas {:?}", thetas);
218        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            // Unique solution
233            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            // Polynome has 2 roots
238            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    // Compute the angle difference between each 2 disks (disks without the 120° offset).
291
292    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        // In the basic condition:
319        //    - disk 0 <--> X
320        //    - so with only pitch (move within Y axis, meaning turn around X), disk 0 should not move
321
322        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(); // thetas are without the 120° offset
335
336        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        // let gammas = compute_gammas(Vector3::from_row_slice(&[
354        //     thetas[0],
355        //     thetas[1] + 120.0_f64.to_radians(),
356        //     thetas[2] - 120.0_f64.to_radians(),
357        // ]));
358        // println!(
359        //     "GAMMAS: {:?} THETAS: {:?} gamma_min: {} gamma_max: {}",
360        //     gammas, thetas, orb.gamma_min, orb.gamma_max
361        // );
362
363        // Thetas are at the extreme values, check should fail
364        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}