From caef61c078a6e9903d8182b81bd49d79b46e3f0b Mon Sep 17 00:00:00 2001 From: Bourumir Wyngs Date: Tue, 9 Apr 2024 21:06:16 +0200 Subject: [PATCH] Starting the work on singularity hardening --- src/kinematic_traits.rs | 10 ++++++++++ src/kinematics_impl.rs | 40 +++++++++++++++++++++++++++++++++------- src/utils.rs | 41 +++-------------------------------------- 3 files changed, 46 insertions(+), 45 deletions(-) diff --git a/src/kinematic_traits.rs b/src/kinematic_traits.rs index 8ced1ee..383242c 100644 --- a/src/kinematic_traits.rs +++ b/src/kinematic_traits.rs @@ -38,7 +38,17 @@ pub mod kinematics_traits { pub(crate) type Solutions = [Joints; 8]; pub trait Kinematics { + /// Find inverse kinematics (joint position) for this pose + /// This function is faster but does not handle the singularity J5 = 0 well. fn inverse(&self, pose: &Pose) -> Solutions; + + + /// Find inverse kinematics (joint position) for this pose + /// This function handles the singularity J5 = 0 by keeping the previous values + /// the values J4 and J6 from the previous solution + fn inverse_continuing(&self, pose: &Pose, previous: &Joints) -> Solutions; + + /// Find forward kinematics (pose from joint positions). fn forward(&self, qs: &Joints) -> Pose; /// Detect the singularity. Returns either A, B type singlularity or None if diff --git a/src/kinematics_impl.rs b/src/kinematics_impl.rs index 2c00909..40a1218 100644 --- a/src/kinematics_impl.rs +++ b/src/kinematics_impl.rs @@ -1,6 +1,7 @@ use std::f64::{consts::PI}; use crate::kinematic_traits::kinematics_traits::{Kinematics, Solutions, Pose, Singularity, Joints}; -use crate::parameters::opw_kinematics::Parameters; +use crate::parameters::opw_kinematics::{Parameters}; +use crate::utils::opw_kinematics::{is_valid}; use nalgebra::{Isometry3, Matrix3, OVector, Rotation3, Translation3, U3, Unit, UnitQuaternion, Vector3}; @@ -20,16 +21,17 @@ impl OPWKinematics { } // Compare two poses with the given tolerance. -fn compare_poses(ta: &Isometry3, tb: &Isometry3, tolerance: f64) -> bool { +fn compare_poses(ta: &Isometry3, tb: &Isometry3, + distance_tolerance: f64, angular_tolerance: f64) -> bool { let translation_distance = (ta.translation.vector - tb.translation.vector).norm(); let angular_distance = ta.rotation.angle_to(&tb.rotation); - if translation_distance.abs() > tolerance { + if translation_distance.abs() > distance_tolerance { println!("Translation Error: {}", translation_distance); return false; } - if angular_distance.abs() > tolerance { + if angular_distance.abs() > angular_distance { println!("Angular Error: {}", angular_distance); return false; } @@ -37,6 +39,10 @@ fn compare_poses(ta: &Isometry3, tb: &Isometry3, tolerance: f64) -> bo } +const DISTANCE_TOLERANCE: f64 = 1E-6; +const ANGULAR_TOLERANCE: f64 = 1E-4; +const SINGULARITY_THRESHOLD: f64 = 0.01 * PI / 180.0; + impl Kinematics for OPWKinematics { fn inverse(&self, pose: &Pose) -> Solutions { let params = &self.parameters; @@ -290,7 +296,7 @@ impl Kinematics for OPWKinematics { }; if valid { let check_pose = self.forward(&sols[si]); - if !compare_poses(&pose, &check_pose, 1e-3) { + if !compare_poses(&pose, &check_pose, DISTANCE_TOLERANCE, ANGULAR_TOLERANCE) { println!("********** Pose Failure sol {} *********", si); // Kill the entry making the failed solution invalid. sols[si][0] = f64::NAN; @@ -301,6 +307,28 @@ impl Kinematics for OPWKinematics { sols } + fn inverse_continuing(&self, pose: &Pose, previous: &Joints) -> Solutions { + let mut sols = self.inverse(pose); + let mut problematic: Option = None; + + 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; + } + + // TODO this is not finished. + + sols + } + fn forward(&self, joints: &Joints) -> Pose { let p = &self.parameters; @@ -380,8 +408,6 @@ impl Kinematics for OPWKinematics { // Adjusted helper function to check for n*pi where n is any integer fn is_close_to_multiple_of_pi(joint_value: f64) -> bool { - const SINGULARITY_THRESHOLD: f64 = 0.01 * PI / 180.0; - // Normalize angle within [0, 2*PI) let normalized_angle = joint_value.rem_euclid(2.0 * PI); // Check if the normalized angle is close to 0 or PI diff --git a/src/utils.rs b/src/utils.rs index c8e3803..3219848 100644 --- a/src/utils.rs +++ b/src/utils.rs @@ -1,22 +1,11 @@ -mod opw_kinematics { +pub(crate) mod opw_kinematics { use std::f64::consts::PI; + use crate::kinematic_traits::kinematics_traits::{Joints, Solutions}; /// Checks if all elements in the array are finite - pub fn is_valid(qs: &[f64; 6]) -> bool { + pub fn is_valid(qs: &Joints) -> bool { qs.iter().all(|&q| q.is_finite()) } - - /// Adjusts the elements of the array to be within the range [-π, π] - pub fn harmonize_toward_zero(qs: &mut [f64; 6]) { - for q in qs.iter_mut() { - while *q > PI { - *q -= 2.0 * PI; - } - while *q < -PI { - *q += 2.0 * PI; - } - } - } } #[cfg(test)] @@ -41,28 +30,4 @@ mod tests { let qs = [0.0, f64::INFINITY, 1.0, -1.0, 0.5, -0.5]; assert!(!is_valid(&qs)); } - - #[test] - fn test_harmonize_toward_zero_within_range() { - let mut qs = [0.0, -PI / 2.0, PI / 2.0, -0.1, 0.1, PI - 0.1]; - harmonize_toward_zero(&mut qs); - let expected = [0.0, -PI / 2.0, PI / 2.0, -0.1, 0.1, PI - 0.1]; - assert_eq!(qs, expected); - } - - #[test] - fn test_harmonize_toward_zero_above_pi() { - let mut qs = [2.0 * PI, 3.0 * PI, -3.0 * PI, 4.0 * PI, -4.0 * PI, 5.0 * PI]; - harmonize_toward_zero(&mut qs); - let expected = [0.0, PI, -PI, 0.0, 0.0, PI]; - assert_eq!(qs, expected); - } - - #[test] - fn test_harmonize_toward_zero_below_minus_pi() { - let mut qs = [-2.0 * PI, -3.0 * PI, 3.0 * PI, -4.0 * PI, 4.0 * PI, -5.0 * PI]; - harmonize_toward_zero(&mut qs); - let expected = [0.0, -PI, PI, 0.0, 0.0, -PI]; - assert_eq!(qs, expected); - } } \ No newline at end of file