Work on singularities

src/kinematics_impl.rs

@@ -309,7 +309,7 @@ impl Kinematics for OPWKinematics {
                 ];
                 let check_pose = self.forward(&solution);
                 if !compare_poses(&pose, &check_pose, 1e-3) {
-                    println!("********** Pose Failure *********");
+                    println!("********** Pose Failure sol {} *********", si);
                     // Kill the entry making the failed solution invalid.
                     solutions[(0, si)] = f64::NAN;
                 }
@@ -377,7 +377,12 @@ impl Kinematics for OPWKinematics {
     }
 
     fn kinematic_singularity(&self, joints: &[f64; 6]) -> Option<Singularity> {
-        let b = self.parameters.b == 0.0 && is_close_to_multiple_of_pi(joints[1]);
+        let b =
+            self.parameters.b == 0.0 &&
+            self.parameters.a1 == 0.0 &&
+            self.parameters.a2 == 0.0 &&
+            is_close_to_multiple_of_pi(joints[1]) &&
+            is_close_to_multiple_of_pi(joints[2]);
         let a = is_close_to_multiple_of_pi(joints[4]);
 
         if a && b {

src/parameters_robots.rs

@@ -33,6 +33,21 @@ pub mod opw_kinematics {
             }
         }
 
+        // Hypothetical robot with b = 0 and a1=a2 so prone to A type singularity
+        pub fn singularities_robot() -> Self {
+            Parameters {
+                a1: 0.,
+                a2: 0.,
+                b: 0.000, // axis aligned
+                c1: 0.550,
+                c2: 0.825,
+                c3: 0.925,
+                c4: 0.110,
+                offsets: [0.0; 6],
+                ..Self::new()
+            }
+        }
+
 
         // See https://www.staubli.com/content/dam/robotics/products/robots/tx2/TX2-140-160-datasheet-EN.pdf.
         // These three Staubli robots have spherical wrist and mostly identical plan, with only

src/tests/testcases.rs

@@ -154,9 +154,6 @@ mod tests {
         println!("Inverse IK: {} test cases", cases.cases.len());
 
         for case in cases.cases.iter() {
-            if case.id != 81 {
-                // continue;
-            }
             let parameters = all_parameters.get(&case.parameters).unwrap_or_else(|| {
                 panic!("Parameters for the robot [{}] are unknown", &case.parameters)
             });
@@ -189,6 +186,63 @@ mod tests {
         }
     }
 
+    #[test]
+    fn test_singularity_ik_1() {
+        // This robot has both A and B type singularity
+        // B type singularity two angles, maestro
+        let parameters = Parameters::staubli_tx2_160l();
+        let kinematics = OPWKinematics::new(parameters.clone());
+        investigate_singularity(&kinematics, [10, 20, 30, 40, 50, 60]);
+        investigate_singularity(&kinematics, [10, 20, 30, 40, 0, 60]);
+        investigate_singularity(&kinematics, [10, 20, 30, 40, 180, 60]);
+        investigate_singularity(&kinematics, [10, 20, 30, 40, -180, 60]);
+        investigate_singularity(&kinematics, [10, 20, 30, 41, 0, 59]);
+        investigate_singularity(&kinematics, [15, 25, 25, 39, 50, 60]);
+    }
+
+    #[test]
+    fn test_singularity_ik_2() {
+        // This robot has both A and B type singularity
+        // B type singularity two angles, maestro
+        let parameters = Parameters::singularities_robot();
+        let kinematics = OPWKinematics::new(parameters.clone());
+        investigate_singularity(&kinematics, [10, 20, 30, 40, 50, 60]);
+        investigate_singularity(&kinematics, [10, 0, 0, 40, 50, 60]);
+        investigate_singularity(&kinematics, [10, -90, 90, 40, 50, 60]);
+        investigate_singularity(&kinematics, [10, 90,  0, 40, 50, 60]);
+        investigate_singularity(&kinematics, [10, -90, 0, 40, 50, 60]);
+    }
+
+
+    // Investigate singularity (research function, input in degrees)
+    fn investigate_singularity(kinematics: &dyn Kinematics, joints: [i32; 6]) {
+        let mut joints_in_radians: [f64; 6] = [0.0; 6];
+        for (i, &deg) in joints.iter().enumerate() {
+            joints_in_radians[i] = deg as f64 * std::f64::consts::PI / 180.0;
+        }
+        let ik = kinematics.forward(&joints_in_radians);
+        let solutions = kinematics.inverse(&ik);
+
+        println!();
+        println!("**** Singularity case ****");
+        let joints_str = &joints.iter()
+            .map(|&val| format!("{:5}", val))
+            .collect::<Vec<String>>()
+            .join(" ");
+        println!("Joints joints: [{}]", joints_str);
+
+        println!("Solutions Matrix:");
+        for sol_idx in 0..8 {
+            let mut row_str = String::new();
+            for joint_idx in 0..6 {
+                let computed = solutions[(joint_idx, sol_idx)];
+                row_str.push_str(&format!("{:5.2} ", computed.to_degrees()));
+            }
+            println!("[{}]", row_str.trim_end()); // Trim trailing space for aesthetics
+            println!("---");
+        }
+    }
+
     fn found_joints_approx_equal(solutions: &Solutions, expected: &[f64; 6], tolerance: f64) -> Option<i32> {
         for sol_idx in 0..8 {
             // println!("Checking solution at index {}", sol_idx);
@@ -253,7 +307,7 @@ mod tests {
 
     #[test]
     fn test_singularity_b() {
-        let robot_vulnerable = OPWKinematics::new(Parameters::irb2400_10());
+        let robot_vulnerable = OPWKinematics::new(Parameters::singularities_robot());
         let robot_immune = OPWKinematics::new(Parameters::staubli_tx40());
 
         // Assuming joint[1] close to 0 triggers B type singularity if b = 0
@@ -268,7 +322,7 @@ mod tests {
 
     #[test]
     fn test_singularity_ab() {
-        let robot_vulnerable = OPWKinematics::new(Parameters::irb2400_10());
+        let robot_vulnerable = OPWKinematics::new(Parameters::singularities_robot());
         let robot_immune = OPWKinematics::new(Parameters::staubli_tx40());
 
         // These joints have both A and (for vulnerable robot) B singularity