From 9b158ffff6ba5fb1477418f4e35fe32fa88a9ca4 Mon Sep 17 00:00:00 2001 From: Bourumir Wyngs Date: Fri, 5 Apr 2024 22:43:55 +0200 Subject: [PATCH] Work on singularities --- src/kinematics_impl.rs | 9 ++++-- src/parameters_robots.rs | 15 ++++++++++ src/tests/testcases.rs | 64 ++++++++++++++++++++++++++++++++++++---- 3 files changed, 81 insertions(+), 7 deletions(-) diff --git a/src/kinematics_impl.rs b/src/kinematics_impl.rs index dc5b4ee..2687fc8 100644 --- a/src/kinematics_impl.rs +++ b/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 { - 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 { diff --git a/src/parameters_robots.rs b/src/parameters_robots.rs index c8962cc..1131ee2 100644 --- a/src/parameters_robots.rs +++ 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 diff --git a/src/tests/testcases.rs b/src/tests/testcases.rs index 109faaa..24f6784 100644 --- a/src/tests/testcases.rs +++ b/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, °) 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::>() + .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 { 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