From ad730c2a1e897b52f86339c016cb1f33405b00a9 Mon Sep 17 00:00:00 2001
From: Bourumir Wyngs <bourumir.wyngs@gmail.com>
Date: Sat, 13 Apr 2024 22:22:53 +0200
Subject: [PATCH] Solutions is now a variable size array without inconsistent
 values.

---
 src/kinematic_traits.rs |  2 +-
 src/kinematics_impl.rs  | 29 ++++++++---------------------
 src/tests/testcases.rs  | 12 ++++++------
 3 files changed, 15 insertions(+), 28 deletions(-)

diff --git a/src/kinematic_traits.rs b/src/kinematic_traits.rs
index 700942f..8477bb2 100644
--- a/src/kinematic_traits.rs
+++ b/src/kinematic_traits.rs
@@ -39,7 +39,7 @@ pub mod kinematics_traits {
     /// Joints that take arbitrary angles will take angles as close to 0 as possible
     pub(crate) const ZERO_JOINTS: Joints = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0];
 
-    pub(crate) type Solutions = [Joints; 8];
+    pub(crate) type Solutions = Vec<Joints>;
 
     pub trait Kinematics {
         /// Find inverse kinematics (joint position) for this pose
diff --git a/src/kinematics_impl.rs b/src/kinematics_impl.rs
index 2249977..ac180b9 100644
--- a/src/kinematics_impl.rs
+++ b/src/kinematics_impl.rs
@@ -295,6 +295,8 @@ impl Kinematics for OPWKinematics {
             }
         }
 
+        let mut result: Solutions = Vec::with_capacity(8);
+
         // Debug check. Solution failing cross-verification is flagged
         // as invalid. This loop also normalizes valid solutions to 0
         for si in 0..sols.len() {
@@ -316,15 +318,15 @@ impl Kinematics for OPWKinematics {
             };
             if valid {
                 let check_pose = self.forward(&sols[si]);
-                if !compare_poses(&pose, &check_pose, DISTANCE_TOLERANCE, ANGULAR_TOLERANCE) {
+                if compare_poses(&pose, &check_pose, DISTANCE_TOLERANCE, ANGULAR_TOLERANCE) {
+                    result.push(sols[si]);
+                } else {
                     println!("********** Pose Failure sol {} *********", si);
-                    // Kill the entry making the failed solution invalid.
-                    sols[si][0] = f64::NAN;
                 }
             }
         }
 
-        sols
+        result
     }
 
     // Replaces one invalid or clearly singular case with "shifted" version
@@ -335,21 +337,6 @@ impl Kinematics for OPWKinematics {
             [[SINGULARITY_SHIFT, 0., 0.], [0., SINGULARITY_SHIFT, 0.], [0., 0., SINGULARITY_SHIFT]];
 
         let mut sols = self.inverse(pose);
-        let mut problematic: Option<usize> = None;
-
-        // Find free slot to put the alternative solution
-        for s_idx in 0..sols.len() {
-            if !is_valid(&sols[s_idx]) ||
-                self.kinematic_singularity(&sols[s_idx]).is_some() {
-                problematic = Some(s_idx);
-                break;
-            }
-        }
-
-        // All solutions are valid with no singularity suspected - there can be any problems!
-        if problematic.is_none() {
-            return sols;
-        }
 
         println!("Main solutions");
         for sol_idx in 0..sols.len() {
@@ -403,8 +390,8 @@ impl Kinematics for OPWKinematics {
                             // Check last time if the pose is ok
                             let check_pose_2 = self.forward(&ik[s_idx]);
                             if compare_poses(&pose, &check_pose_2, DISTANCE_TOLERANCE, ANGULAR_TOLERANCE) {
-                                println!("A type singularity {} substituted at {:?}", s_idx, problematic);
-                                sols[problematic.unwrap()] = ik[s_idx];
+                                println!("A type singularity {} added", s_idx);
+                                sols.push(ik[s_idx]);
                                 break 'shifts;
                             }
                         }
diff --git a/src/tests/testcases.rs b/src/tests/testcases.rs
index 625f2d0..a2737dd 100644
--- a/src/tests/testcases.rs
+++ b/src/tests/testcases.rs
@@ -171,7 +171,7 @@ mod tests {
                 println!("Expected joints: [{}]", joints_str);
 
                 println!("Solutions Matrix:");
-                for sol_idx in 0..8 {
+                for sol_idx in 0..solutions.len() {
                     let mut row_str = String::new();
                     for joint_idx in 0..6 {
                         let computed = solutions[sol_idx][joint_idx];
@@ -261,7 +261,7 @@ mod tests {
         println!("Joints joints: [{}]", joints_str);
 
         println!("Solutions Matrix:");
-        for sol_idx in 0..8 {
+        for sol_idx in 0..solutions.len() {
             let mut row_str = String::new();
             for joint_idx in 0..6 {
                 let computed = solutions[sol_idx][joint_idx];
@@ -317,7 +317,7 @@ mod tests {
         println!("Joints joints: [{}]", joints_str);
 
         println!("Solutions Matrix:");
-        for sol_idx in 0..8 {
+        for sol_idx in 0..solutions.len() {
             let mut row_str = String::new();
             for joint_idx in 0..6 {
                 let computed = solutions[sol_idx][joint_idx];
@@ -332,7 +332,7 @@ mod tests {
         let solutions = kinematics.inverse(&ik2);
 
         println!("Solutions Matrix IK2:");
-        for sol_idx in 0..8 {
+        for sol_idx in 0..solutions.len() {
             let mut row_str = String::new();
             for joint_idx in 0..6 {
                 let computed = solutions[sol_idx][joint_idx];
@@ -347,7 +347,7 @@ mod tests {
         let solutions = kinematics.inverse(&ik2);
 
         println!("Solutions Matrix IK2:");
-        for sol_idx in 0..8 {
+        for sol_idx in 0..solutions.len() {
             let mut row_str = String::new();
             for joint_idx in 0..6 {
                 let computed = solutions[sol_idx][joint_idx];
@@ -360,7 +360,7 @@ mod tests {
     }
 
     fn found_joints_approx_equal(solutions: &Solutions, expected: &[f64; 6], tolerance: f64) -> Option<i32> {
-        for sol_idx in 0..8 {
+        for sol_idx in 0..solutions.len() {
             // println!("Checking solution at index {}", sol_idx);
 
             let mut solution_matches = true;