From 31dc86cfee049b2b15d66cb7509933b1573c578b Mon Sep 17 00:00:00 2001 From: Bourumir Wyngs Date: Sun, 21 Apr 2024 14:06:35 +0200 Subject: [PATCH 01/21] Increment numbers in Cargo.toml and README.md. Started work on version 1.1.0. --- Cargo.toml | 2 +- README.md | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/Cargo.toml b/Cargo.toml index 1e85ea1..269c9c9 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -1,6 +1,6 @@ [package] name = "rs-opw-kinematics" -version = "1.0.2" +version = "1.1.0" edition = "2021" authors = ["Bourumir Wyngs "] description = "Inverse and forward kinematics for 6 axis robots with a parallel base and spherical wrist." diff --git a/README.md b/README.md index 1ce4593..006ddc4 100644 --- a/README.md +++ b/README.md @@ -66,7 +66,7 @@ the ground at "zero." Cargo.toml: ```toml [dependencies] -rs-opw-kinematics = "1.0.2" +rs-opw-kinematics = "1.1.0" ``` main.rs: From 8d354b2840f6a75796db1e3ce8c116633d1cf0a5 Mon Sep 17 00:00:00 2001 From: Bourumir Wyngs Date: Mon, 22 Apr 2024 20:28:39 +0200 Subject: [PATCH 02/21] Implementation of constraints.rs --- Cargo.lock | 2 +- src/constraints.rs | 115 ++++++++++++++++++++++++++++++++++++++++ src/kinematic_traits.rs | 5 ++ src/kinematics_impl.rs | 27 ++++++++-- src/lib.rs | 2 + 5 files changed, 146 insertions(+), 5 deletions(-) create mode 100644 src/constraints.rs diff --git a/Cargo.lock b/Cargo.lock index 4b68bfb..3b6fbca 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -202,7 +202,7 @@ checksum = "adad44e29e4c806119491a7f06f03de4d1af22c3a680dd47f1e6e179439d1f56" [[package]] name = "rs-opw-kinematics" -version = "1.0.2" +version = "1.1.0" dependencies = [ "nalgebra", "regex", diff --git a/src/constraints.rs b/src/constraints.rs new file mode 100644 index 0000000..cefcd45 --- /dev/null +++ b/src/constraints.rs @@ -0,0 +1,115 @@ +use std::f64::consts::PI; + +pub struct Constraints { + /// Normalized lower limit. If more than upper limit, the range wraps-around through 0 + pub from: [f64; 6], + + /// Normalized upper limit. If less than lower limit, the range wraps-around through 0 + pub to: [f64; 6], +} + +impl Constraints { + pub fn new(from: [f64; 6], to: [f64; 6]) -> Self { + let two_pi = 2.0 * PI; + let from_normalized: [f64; 6] = from.map(|f| ((f % two_pi) + two_pi) % two_pi); + let to_normalized: [f64; 6] = to.map(|t| ((t % two_pi) + two_pi) % two_pi); + + Constraints { + from: from_normalized, + to: to_normalized, + } + } + + pub fn compliant(&self, angles: &[f64; 6]) -> bool { + let two_pi = 2.0 * PI; + for i in 0..6 { + if self.from[i] == self.to[i] { + continue; // Joint without constraints, from == to + } + let angle = ((angles[i] % two_pi) + two_pi) % two_pi; + if self.from[i] <= self.to[i] { + if !(angle >= self.from[i] && angle <= self.to[i]) { + return false; + } + } else { + if !(angle >= self.from[i] || angle <= self.to[i]) { + return false; + } + } + } + true + } + + pub fn filter(&self, angles: &Vec<[f64; 6]>) -> Vec<[f64; 6]> { + angles.into_iter() + .filter(|angle_array| self.compliant(&angle_array)) + .cloned() + .collect() + } +} + +#[cfg(test)] +mod tests { + use super::*; + + #[test] + fn test_no_wrap_around() { + let angles = [0.1 * PI, 0.2 * PI, 0.3 * PI, 0.4 * PI, 0.5 * PI, 0.6 * PI]; + let from = [0.0, 0.15 * PI, 0.25 * PI, 0.35 * PI, 0.45 * PI, 0.55 * PI]; + let to = [0.2 * PI, 0.3 * PI, 0.4 * PI, 0.5 * PI, 0.6 * PI, 0.7 * PI]; + let limits = Constraints::new(from, to); + assert!(limits.compliant(&angles)); + } + + #[test] + fn test_with_wrap_around() { + let angles = [0.9 * PI, 1.9 * PI, 0.05 * PI, 1.05 * PI, 1.95 * PI, 0.95 * PI]; + let from = [0.8 * PI, 1.8 * PI, 0.0, 1.0 * PI, 1.9 * PI, 0.9 * PI]; + let to = [0.1 * PI, 1.1 * PI, 0.2 * PI, 1.2 * PI, 0.0, 1.0 * PI]; + let limits = Constraints::new(from, to); + assert!(limits.compliant(&angles)); + } + + #[test] + fn test_full_circle() { + let angles = [0.0, 1.0 * PI, 0.5 * PI, 1.5 * PI, 0.25 * PI, 0.75 * PI]; + let from = [0.0; 6]; + let to = [2.0 * PI; 6]; + let limits = Constraints::new(from, to); + assert!(limits.compliant(&angles)); + } + + #[test] + fn test_invalid_angles_no_wrap_around() { + let angles = [0.15 * PI, 0.25 * PI, 0.55 * PI, 0.65 * PI, 0.75 * PI, 0.85 * PI]; + let from = [0.2 * PI, 0.3 * PI, 0.6 * PI, 0.7 * PI, 0.8 * PI, 0.9 * PI]; + let to = [0.1 * PI, 0.2 * PI, 0.5 * PI, 0.6 * PI, 0.7 * PI, 0.8 * PI]; + let limits = Constraints::new(from, to); + assert!(!limits.compliant(&angles)); + } + + #[test] + fn test_invalid_angles_with_wrap_around() { + let angles = [0.8 * PI, 1.8 * PI, 1.0 * PI, 0.0, 2.1 * PI, 1.1 * PI]; + let from = [0.9 * PI, 2.0 * PI, 0.1 * PI, 0.2 * PI, 2.2 * PI, 1.2 * PI]; + let to = [0.0, 1.0 * PI, 0.05 * PI, 0.1 * PI, 2.0 * PI, 1.0 * PI]; + let limits = Constraints::new(from, to); + assert!(!limits.compliant(&angles)); + } + + #[test] + fn test_filter_angles() { + let from = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]; + let to = [PI / 2.0, PI / 2.0, PI / 2.0, PI / 2.0, PI / 2.0, PI / 2.0]; + let angles = vec![ + [PI / 3.0, PI / 4.0, PI / 6.0, PI / 3.0, PI / 4.0, PI / 6.0], // Should be retained + [PI, 2.0 * PI, PI, PI, PI, PI], // Should be removed + ]; + + let limits = Constraints::new(from, to); + let filtered_angles = limits.filter(&angles); + assert_eq!(filtered_angles.len(), 1); + assert_eq!(filtered_angles[0], [PI / 3.0, PI / 4.0, PI / 6.0, PI / 3.0, PI / 4.0, PI / 6.0]); + } +} + diff --git a/src/kinematic_traits.rs b/src/kinematic_traits.rs index af08a0d..a52751b 100644 --- a/src/kinematic_traits.rs +++ b/src/kinematic_traits.rs @@ -1,6 +1,7 @@ extern crate nalgebra as na; use na::{Isometry3}; +use crate::constraints::Constraints; /// Pose is used a pose of the robot tcp. It contains both Cartesian position and rotation quaternion /// ``` @@ -59,5 +60,9 @@ pub trait Kinematics { /// Detect the singularity. Returns either A type singlularity or None if /// no singularity detected. fn kinematic_singularity(&self, qs: &Joints) -> Option; + + /// Set the constraints. See the documentation of this class on how to create them. + /// If constraints are set, all solutions returned are constraint compliant. + fn constraints(&mut self, constraints: Option); } diff --git a/src/kinematics_impl.rs b/src/kinematics_impl.rs index 64ea32e..bb283ef 100644 --- a/src/kinematics_impl.rs +++ b/src/kinematics_impl.rs @@ -4,12 +4,14 @@ use crate::parameters::opw_kinematics::{Parameters}; use crate::utils::opw_kinematics::{is_valid}; use nalgebra::{Isometry3, Matrix3, OVector, Rotation3, Translation3, U3, Unit, UnitQuaternion, Vector3}; +use crate::constraints::Constraints; const DEBUG: bool = false; pub struct OPWKinematics { parameters: Parameters, unit_z: Unit>, + constraints: Option, } impl OPWKinematics { @@ -19,6 +21,7 @@ impl OPWKinematics { OPWKinematics { parameters, unit_z: Unit::new_normalize(Vector3::z_axis().into_inner()), + constraints: None, } } } @@ -310,7 +313,7 @@ impl Kinematics for OPWKinematics { } } - result + self.constraints_compliant(result) } // Replaces singularity with correct solution @@ -372,9 +375,12 @@ impl Kinematics for OPWKinematics { // Check last time if the pose is ok let check_pose = self.forward(&now); if compare_poses(&pose, &check_pose, DISTANCE_TOLERANCE, ANGULAR_TOLERANCE) { - solutions.push(now); - // We only expect one singularity case hence once we found, we can end - break 'shifts; + // Guard against the case our solution is out of constraints. + if self.constraints.as_ref().map_or(true, |c| c.compliant(&now)) { + solutions.push(now); + // We only expect one singularity case hence once we found, we can end + break 'shifts; + } } } @@ -456,6 +462,19 @@ impl Kinematics for OPWKinematics { None } } + + fn constraints(&mut self, constraints: Option) { + self.constraints = constraints; + } +} + +impl OPWKinematics { + fn constraints_compliant(&self, solutions: Solutions) -> Solutions { + match &self.constraints { + Some(constraints) => constraints.filter(&solutions), + None => solutions + } + } } // Adjusted helper function to check for n*pi where n is any integer diff --git a/src/lib.rs b/src/lib.rs index 3178f3b..167834c 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -6,6 +6,8 @@ pub mod utils; pub mod kinematic_traits; pub mod kinematics_impl; +pub mod constraints; + #[cfg(test)] mod tests; From 65249c32b0ed65d4690653ec5c1c69b34212f0d0 Mon Sep 17 00:00:00 2001 From: Bourumir Wyngs Date: Mon, 22 Apr 2024 20:45:07 +0200 Subject: [PATCH 03/21] Implementation of constraints.rs --- src/constraints.rs | 1 + src/kinematic_traits.rs | 2 +- src/kinematics_impl.rs | 26 ++++++++++++++++---------- 3 files changed, 18 insertions(+), 11 deletions(-) diff --git a/src/constraints.rs b/src/constraints.rs index cefcd45..b729e0c 100644 --- a/src/constraints.rs +++ b/src/constraints.rs @@ -1,5 +1,6 @@ use std::f64::consts::PI; +#[derive(Clone)] pub struct Constraints { /// Normalized lower limit. If more than upper limit, the range wraps-around through 0 pub from: [f64; 6], diff --git a/src/kinematic_traits.rs b/src/kinematic_traits.rs index a52751b..47dcc2c 100644 --- a/src/kinematic_traits.rs +++ b/src/kinematic_traits.rs @@ -63,6 +63,6 @@ pub trait Kinematics { /// Set the constraints. See the documentation of this class on how to create them. /// If constraints are set, all solutions returned are constraint compliant. - fn constraints(&mut self, constraints: Option); + fn constraints(&mut self, constraints: &Constraints); } diff --git a/src/kinematics_impl.rs b/src/kinematics_impl.rs index bb283ef..6f15d54 100644 --- a/src/kinematics_impl.rs +++ b/src/kinematics_impl.rs @@ -313,7 +313,7 @@ impl Kinematics for OPWKinematics { } } - self.constraints_compliant(result) + self.filter_constraints_compliant(result) } // Replaces singularity with correct solution @@ -374,13 +374,12 @@ impl Kinematics for OPWKinematics { // Check last time if the pose is ok let check_pose = self.forward(&now); - if compare_poses(&pose, &check_pose, DISTANCE_TOLERANCE, ANGULAR_TOLERANCE) { + if compare_poses(&pose, &check_pose, DISTANCE_TOLERANCE, ANGULAR_TOLERANCE) && + self.constraints_compliant(now) { // Guard against the case our solution is out of constraints. - if self.constraints.as_ref().map_or(true, |c| c.compliant(&now)) { - solutions.push(now); - // We only expect one singularity case hence once we found, we can end - break 'shifts; - } + solutions.push(now); + // We only expect one singularity case hence once we found, we can end + break 'shifts; } } @@ -463,18 +462,25 @@ impl Kinematics for OPWKinematics { } } - fn constraints(&mut self, constraints: Option) { - self.constraints = constraints; + fn constraints(&mut self, constraints: &Constraints) { + self.constraints = Some(constraints.clone()); } } impl OPWKinematics { - fn constraints_compliant(&self, solutions: Solutions) -> Solutions { + fn filter_constraints_compliant(&self, solutions: Solutions) -> Solutions { match &self.constraints { Some(constraints) => constraints.filter(&solutions), None => solutions } } + + fn constraints_compliant(&self, solution: Joints) -> bool { + match &self.constraints { + Some(constraints) => constraints.compliant(&solution), + None => true + } + } } // Adjusted helper function to check for n*pi where n is any integer From d8ef496d6c283addb855457302114a3ac1dfbe9b Mon Sep 17 00:00:00 2001 From: Bourumir Wyngs Date: Mon, 22 Apr 2024 20:49:44 +0200 Subject: [PATCH 04/21] Set the constraints better in constructor. --- src/kinematic_traits.rs | 5 ----- src/kinematics_impl.rs | 14 ++++++++++---- 2 files changed, 10 insertions(+), 9 deletions(-) diff --git a/src/kinematic_traits.rs b/src/kinematic_traits.rs index 47dcc2c..af08a0d 100644 --- a/src/kinematic_traits.rs +++ b/src/kinematic_traits.rs @@ -1,7 +1,6 @@ extern crate nalgebra as na; use na::{Isometry3}; -use crate::constraints::Constraints; /// Pose is used a pose of the robot tcp. It contains both Cartesian position and rotation quaternion /// ``` @@ -60,9 +59,5 @@ pub trait Kinematics { /// Detect the singularity. Returns either A type singlularity or None if /// no singularity detected. fn kinematic_singularity(&self, qs: &Joints) -> Option; - - /// Set the constraints. See the documentation of this class on how to create them. - /// If constraints are set, all solutions returned are constraint compliant. - fn constraints(&mut self, constraints: &Constraints); } diff --git a/src/kinematics_impl.rs b/src/kinematics_impl.rs index 6f15d54..6d34d98 100644 --- a/src/kinematics_impl.rs +++ b/src/kinematics_impl.rs @@ -24,6 +24,16 @@ impl OPWKinematics { constraints: None, } } + + /// Create a new instance that takes also Constraints. + /// If constraints are set, all solutions returned by this solver are constraint compliant. + pub fn new_with_constraints(parameters: Parameters, constraints: Constraints) -> Self { + OPWKinematics { + parameters, + unit_z: Unit::new_normalize(Vector3::z_axis().into_inner()), + constraints: Some(constraints), + } + } } const MM: f64 = 0.001; @@ -461,10 +471,6 @@ impl Kinematics for OPWKinematics { None } } - - fn constraints(&mut self, constraints: &Constraints) { - self.constraints = Some(constraints.clone()); - } } impl OPWKinematics { From a468c2e71a1d2befc8ebbd5c333383787167e7d1 Mon Sep 17 00:00:00 2001 From: Bourumir Wyngs Date: Tue, 23 Apr 2024 23:00:36 +0200 Subject: [PATCH 05/21] Implemented sorting weight sorting of results, compromising between sorting weight and constraint centers. --- src/constraints.rs | 51 +++++++++++++++++++++++----- src/kinematic_traits.rs | 10 ++++++ src/kinematics_impl.rs | 74 ++++++++++++++++++++++++++++++++--------- src/utils.rs | 6 ++++ 4 files changed, 116 insertions(+), 25 deletions(-) diff --git a/src/constraints.rs b/src/constraints.rs index b729e0c..a1cef59 100644 --- a/src/constraints.rs +++ b/src/constraints.rs @@ -7,20 +7,51 @@ pub struct Constraints { /// Normalized upper limit. If less than lower limit, the range wraps-around through 0 pub to: [f64; 6], + + // Constraint centers. Used in distance from constraints based sorting. + pub centers: [f64; 6], + + /// Used when sorting the solutions by both middle values of the constraints and the previous + /// values of the joints. 1.0 gives the absolute priority to constraints, 0.0. gives + /// absolute priority for joints. + pub sorting_weight: f64, } +/// When sorting solutions, give absolute priority to constraints (the closer to the midrange +/// of constraints, the better) +pub const BY_CONSTRAINS: f64 = 1.0; +/// When sorting solutions, give absolute priority to previous joints values (the closer to the +/// previous value, the better). +pub const BY_PREV: f64 = 0.0; + impl Constraints { - pub fn new(from: [f64; 6], to: [f64; 6]) -> Self { + /// Create constraints that restrict the joint rotations between 'from' to 'to' values. + /// Wrapping arround is supported so order is important. For instance, + /// from = 0.1 and to = 0.2 (radians) means the joint + /// is allowed to rotate to 0.11, 0.12 ... 1.99, 0.2. + /// from = 0.2 ant to = 0.1 is also valid but means the joint is allowed to rotate + /// to 0.21, 0.22, 0.99, 2 * PI or 0.0 (wrapping around), then to 0.09 and finally 0.1, + /// so the other side of the circle. The sorting_weight parameter influences sorting of the + /// results: 0.0 (or BY_PREV) gives absolute priority to the previous values of the joints, + /// 1.0 (or BY_CONSTRAINTS) gives absolute priority to the middle values of constraints. + /// Intermediate values like 0.5 provide the weighted compromise. + pub fn new(from: [f64; 6], to: [f64; 6], sorting_weight: f64) -> Self { let two_pi = 2.0 * PI; - let from_normalized: [f64; 6] = from.map(|f| ((f % two_pi) + two_pi) % two_pi); - let to_normalized: [f64; 6] = to.map(|t| ((t % two_pi) + two_pi) % two_pi); + let from_normalized = from.map(|f| ((f % two_pi) + two_pi) % two_pi); + let to_normalized = to.map(|t| ((t % two_pi) + two_pi) % two_pi); + let centers = + std::array::from_fn(|i| (from_normalized[i] + to_normalized[i]) / 2.0) + .map(|t| ((t % two_pi) + two_pi) % two_pi); Constraints { from: from_normalized, to: to_normalized, + centers: centers, + sorting_weight: sorting_weight, } } + /// Checks if all values in the given vector or angles satisfy these constraints. pub fn compliant(&self, angles: &[f64; 6]) -> bool { let two_pi = 2.0 * PI; for i in 0..6 { @@ -41,6 +72,8 @@ impl Constraints { true } + /// Return new vector of angle arrays, removing all that have members not satisfying these + /// constraints. pub fn filter(&self, angles: &Vec<[f64; 6]>) -> Vec<[f64; 6]> { angles.into_iter() .filter(|angle_array| self.compliant(&angle_array)) @@ -58,7 +91,7 @@ mod tests { let angles = [0.1 * PI, 0.2 * PI, 0.3 * PI, 0.4 * PI, 0.5 * PI, 0.6 * PI]; let from = [0.0, 0.15 * PI, 0.25 * PI, 0.35 * PI, 0.45 * PI, 0.55 * PI]; let to = [0.2 * PI, 0.3 * PI, 0.4 * PI, 0.5 * PI, 0.6 * PI, 0.7 * PI]; - let limits = Constraints::new(from, to); + let limits = Constraints::new(from, to, BY_CONSTRAINS); assert!(limits.compliant(&angles)); } @@ -67,7 +100,7 @@ mod tests { let angles = [0.9 * PI, 1.9 * PI, 0.05 * PI, 1.05 * PI, 1.95 * PI, 0.95 * PI]; let from = [0.8 * PI, 1.8 * PI, 0.0, 1.0 * PI, 1.9 * PI, 0.9 * PI]; let to = [0.1 * PI, 1.1 * PI, 0.2 * PI, 1.2 * PI, 0.0, 1.0 * PI]; - let limits = Constraints::new(from, to); + let limits = Constraints::new(from, to, BY_CONSTRAINS); assert!(limits.compliant(&angles)); } @@ -76,7 +109,7 @@ mod tests { let angles = [0.0, 1.0 * PI, 0.5 * PI, 1.5 * PI, 0.25 * PI, 0.75 * PI]; let from = [0.0; 6]; let to = [2.0 * PI; 6]; - let limits = Constraints::new(from, to); + let limits = Constraints::new(from, to, BY_CONSTRAINS); assert!(limits.compliant(&angles)); } @@ -85,7 +118,7 @@ mod tests { let angles = [0.15 * PI, 0.25 * PI, 0.55 * PI, 0.65 * PI, 0.75 * PI, 0.85 * PI]; let from = [0.2 * PI, 0.3 * PI, 0.6 * PI, 0.7 * PI, 0.8 * PI, 0.9 * PI]; let to = [0.1 * PI, 0.2 * PI, 0.5 * PI, 0.6 * PI, 0.7 * PI, 0.8 * PI]; - let limits = Constraints::new(from, to); + let limits = Constraints::new(from, to, BY_CONSTRAINS); assert!(!limits.compliant(&angles)); } @@ -94,7 +127,7 @@ mod tests { let angles = [0.8 * PI, 1.8 * PI, 1.0 * PI, 0.0, 2.1 * PI, 1.1 * PI]; let from = [0.9 * PI, 2.0 * PI, 0.1 * PI, 0.2 * PI, 2.2 * PI, 1.2 * PI]; let to = [0.0, 1.0 * PI, 0.05 * PI, 0.1 * PI, 2.0 * PI, 1.0 * PI]; - let limits = Constraints::new(from, to); + let limits = Constraints::new(from, to, BY_CONSTRAINS); assert!(!limits.compliant(&angles)); } @@ -107,7 +140,7 @@ mod tests { [PI, 2.0 * PI, PI, PI, PI, PI], // Should be removed ]; - let limits = Constraints::new(from, to); + let limits = Constraints::new(from, to, BY_CONSTRAINS); let filtered_angles = limits.filter(&angles); assert_eq!(filtered_angles.len(), 1); assert_eq!(filtered_angles[0], [PI / 3.0, PI / 4.0, PI / 6.0, PI / 3.0, PI / 4.0, PI / 6.0]); diff --git a/src/kinematic_traits.rs b/src/kinematic_traits.rs index af08a0d..3c21e17 100644 --- a/src/kinematic_traits.rs +++ b/src/kinematic_traits.rs @@ -1,5 +1,6 @@ extern crate nalgebra as na; +use std::f64::NAN; use na::{Isometry3}; /// Pose is used a pose of the robot tcp. It contains both Cartesian position and rotation quaternion @@ -35,6 +36,12 @@ pub type Joints = [f64; 6]; #[allow(dead_code)] pub const JOINTS_AT_ZERO: Joints = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]; +/// Special value that can be used with inverse_continuing, indicating that there +/// are not previous joint value, but returned joints must be sorted to be as +/// close as possible to the centers of the constraints. If no constraitns are set, +/// zeroes are assumed. +pub const CONSTRAINT_CENTERED: Joints = [NAN, 0.0, 0.0, 0.0, 0.0, 0.0]; + /// For providing solutions. As invalid solutions are discarded, /// this is a variable length vector (may be empty if robot cannot reach the /// given point). @@ -51,6 +58,9 @@ pub trait Kinematics { /// 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 + /// Use CONSTRAINT_CENTERED as previous if there is no previous position but we prefer + /// to be as close to the center of constraints (or zeroes if not set) as + /// possible. fn inverse_continuing(&self, pose: &Pose, previous: &Joints) -> Solutions; /// Find forward kinematics (pose from joint positions). diff --git a/src/kinematics_impl.rs b/src/kinematics_impl.rs index 6d34d98..1a075ee 100644 --- a/src/kinematics_impl.rs +++ b/src/kinematics_impl.rs @@ -1,17 +1,19 @@ use std::f64::{consts::PI}; -use crate::kinematic_traits::{Kinematics, Solutions, Pose, Singularity, Joints}; +use crate::kinematic_traits::{Kinematics, Solutions, Pose, Singularity, Joints, JOINTS_AT_ZERO}; use crate::parameters::opw_kinematics::{Parameters}; use crate::utils::opw_kinematics::{is_valid}; use nalgebra::{Isometry3, Matrix3, OVector, Rotation3, Translation3, U3, Unit, UnitQuaternion, Vector3}; -use crate::constraints::Constraints; +use crate::constraints::{BY_CONSTRAINS, BY_PREV, Constraints}; const DEBUG: bool = false; pub struct OPWKinematics { + /// The parameters that were used to construct this solver. parameters: Parameters, - unit_z: Unit>, constraints: Option, + + unit_z: Unit>, } impl OPWKinematics { @@ -33,7 +35,7 @@ impl OPWKinematics { unit_z: Unit::new_normalize(Vector3::z_axis().into_inner()), constraints: Some(constraints), } - } + } } const MM: f64 = 0.001; @@ -327,7 +329,15 @@ impl Kinematics for OPWKinematics { } // Replaces singularity with correct solution - fn inverse_continuing(&self, pose: &Pose, previous: &Joints) -> Solutions { + fn inverse_continuing(&self, pose: &Pose, prev: &Joints) -> Solutions { + let previous; + if prev[0].is_nan() { + // Special value CONSTRAINT_CENTERED has been used + previous = self.constraint_centers(); + } else { + previous = prev; + } + const SINGULARITY_SHIFT: f64 = DISTANCE_TOLERANCE / 8.; const SINGULARITY_SHIFTS: [[f64; 3]; 4] = [[0., 0., 0., ], [SINGULARITY_SHIFT, 0., 0.], @@ -404,7 +414,7 @@ impl Kinematics for OPWKinematics { normalize_near(&mut solutions[s_idx][joint_idx], previous[joint_idx]); } } - sort_by_closeness(&mut solutions, &previous); + self.sort_by_closeness(&mut solutions, &previous); solutions } @@ -487,6 +497,48 @@ impl OPWKinematics { None => true } } + + /// Sorts the solutions vector by closeness to the `previous` joint. + /// Joints must be pre-normalized to be as close as possible, not away by 360 degrees + fn sort_by_closeness(&self, solutions: &mut Solutions, previous: &Joints) { + let sorting_weight = self.constraints.as_ref() + .map_or(BY_PREV, |c| c.sorting_weight); + if sorting_weight == BY_PREV { + // If no constraints or they weight is zero, use simpler version + solutions.sort_by(|a, b| { + let distance_a = calculate_distance(a, previous); + let distance_b = calculate_distance(b, previous); + distance_a.partial_cmp(&distance_b).unwrap_or(std::cmp::Ordering::Equal) + }); + } else { + let constraints = self.constraints.as_ref().unwrap(); + solutions.sort_by(|a, b| { + let prev_a; + let prev_b; + if sorting_weight == BY_CONSTRAINS { + prev_a = calculate_distance(a, previous); + prev_b = calculate_distance(b, previous); + } else { + // Do not calculate unneeded distances if these values are to be ignored. + prev_a = 0.0; + prev_b = 0.0; + } + + let constr_a = calculate_distance(a, &constraints.centers); + let constr_b = calculate_distance(b, &constraints.centers); + + let distance_a = prev_a * sorting_weight + constr_a * (1.0 - sorting_weight); + let distance_b = prev_b * sorting_weight + constr_b * (1.0 - sorting_weight); + distance_a.partial_cmp(&distance_b).unwrap_or(std::cmp::Ordering::Equal) + }); + } + } + + /// Get constraint centers in case we have the already constructed instance of the + fn constraint_centers(&self) -> &Joints { + self.constraints.as_ref() + .map_or(&JOINTS_AT_ZERO, |c| &c.centers) + } } // Adjusted helper function to check for n*pi where n is any integer @@ -543,16 +595,6 @@ fn calculate_distance(joint1: &Joints, joint2: &Joints) -> f64 { .sum() } -/// Sorts the solutions vector by closeness to the `previous` joint. -/// Joints must be pre-normalized to be as close as possible, not away by 360 degrees -fn sort_by_closeness(solutions: &mut Solutions, previous: &Joints) { - solutions.sort_by(|a, b| { - let distance_a = calculate_distance(a, previous); - let distance_b = calculate_distance(b, previous); - distance_a.partial_cmp(&distance_b).unwrap_or(std::cmp::Ordering::Equal) - }); -} - // Compare two poses with the given tolerance. fn compare_poses(ta: &Isometry3, tb: &Isometry3, distance_tolerance: f64, angular_tolerance: f64) -> bool { diff --git a/src/utils.rs b/src/utils.rs index c8a5d46..b6b8098 100644 --- a/src/utils.rs +++ b/src/utils.rs @@ -35,6 +35,12 @@ pub fn dump_joints(joints: &Joints) { println!("[{}]", row_str.trim_end()); } +/// Allows to specify joint values in degrees (converts to radians) +#[allow(dead_code)] +fn as_radians(degrees: [i32; 6]) -> Joints { + std::array::from_fn(|i| (degrees[i] as f64).to_radians()) +} + #[cfg(test)] mod tests { use std::f64::consts::PI; From 9f146c1a64f6f052d3f0bc0b01f953ffe2aff09e Mon Sep 17 00:00:00 2001 From: Bourumir Wyngs Date: Fri, 26 Apr 2024 23:42:09 +0200 Subject: [PATCH 06/21] All tests pass, more tests will be needed --- src/constraints.rs | 98 ++++++++--- src/kinematics_impl.rs | 322 ++++++++++++++++++----------------- src/tests/constraint_test.rs | 74 ++++++++ src/tests/mod.rs | 3 +- src/tests/testcases.rs | 61 +++---- src/utils.rs | 20 ++- 6 files changed, 363 insertions(+), 215 deletions(-) create mode 100644 src/tests/constraint_test.rs diff --git a/src/constraints.rs b/src/constraints.rs index a1cef59..3ba54bd 100644 --- a/src/constraints.rs +++ b/src/constraints.rs @@ -1,4 +1,7 @@ use std::f64::consts::PI; +use std::f64::{INFINITY, NAN}; +use crate::kinematic_traits::{Joints, JOINTS_AT_ZERO}; +use crate::utils::dump_joints; #[derive(Clone)] pub struct Constraints { @@ -11,6 +14,9 @@ pub struct Constraints { // Constraint centers. Used in distance from constraints based sorting. pub centers: [f64; 6], + // How far from the center the value could be + pub tolerances: [f64; 6], + /// Used when sorting the solutions by both middle values of the constraints and the previous /// values of the joints. 1.0 gives the absolute priority to constraints, 0.0. gives /// absolute priority for joints. @@ -24,7 +30,10 @@ pub const BY_CONSTRAINS: f64 = 1.0; /// previous value, the better). pub const BY_PREV: f64 = 0.0; +const TWO_PI: f64 = 2.0 * PI; + impl Constraints { + /// Create constraints that restrict the joint rotations between 'from' to 'to' values. /// Wrapping arround is supported so order is important. For instance, /// from = 0.1 and to = 0.2 (radians) means the joint @@ -35,41 +44,62 @@ impl Constraints { /// results: 0.0 (or BY_PREV) gives absolute priority to the previous values of the joints, /// 1.0 (or BY_CONSTRAINTS) gives absolute priority to the middle values of constraints. /// Intermediate values like 0.5 provide the weighted compromise. - pub fn new(from: [f64; 6], to: [f64; 6], sorting_weight: f64) -> Self { - let two_pi = 2.0 * PI; - let from_normalized = from.map(|f| ((f % two_pi) + two_pi) % two_pi); - let to_normalized = to.map(|t| ((t % two_pi) + two_pi) % two_pi); - let centers = - std::array::from_fn(|i| (from_normalized[i] + to_normalized[i]) / 2.0) - .map(|t| ((t % two_pi) + two_pi) % two_pi); + pub fn new(from: Joints, to: Joints, sorting_weight: f64) -> Self { + + let mut centers: Joints = JOINTS_AT_ZERO; + let mut tolerances: Joints = JOINTS_AT_ZERO; + + for j_idx in 0..6 { + let a = from[j_idx]; + let mut b = to[j_idx]; + if a == b { + tolerances[j_idx] = INFINITY; // No constraint, not checked + } else if a < b { + // Values do not wrap arround + centers[j_idx] = (a + b) / 2.0; + tolerances[j_idx] = (b - a) / 2.0; + } else { + // Values wrap arround. Move b forward by period till it gets ahead. + while b < a { + b = b + TWO_PI; + } + centers[j_idx] = (a + b) / 2.0; + tolerances[j_idx] = (b - a) / 2.0; + } + } Constraints { - from: from_normalized, - to: to_normalized, + from: from, + to: to, centers: centers, + tolerances: tolerances, sorting_weight: sorting_weight, } } + fn inside_bounds(angle1: f64, angle2: f64, tolerance: f64) -> bool { + if tolerance.is_infinite() { + return false; + } + let mut difference = (angle1 - angle2).abs(); + difference = difference % TWO_PI; + if difference > PI { + difference = TWO_PI - difference; + } + difference <= tolerance + } + /// Checks if all values in the given vector or angles satisfy these constraints. pub fn compliant(&self, angles: &[f64; 6]) -> bool { - let two_pi = 2.0 * PI; - for i in 0..6 { - if self.from[i] == self.to[i] { - continue; // Joint without constraints, from == to - } - let angle = ((angles[i] % two_pi) + two_pi) % two_pi; - if self.from[i] <= self.to[i] { - if !(angle >= self.from[i] && angle <= self.to[i]) { - return false; - } - } else { - if !(angle >= self.from[i] || angle <= self.to[i]) { - return false; - } - } - } - true + let ok = angles.iter().enumerate().all(|(i, &angle)| { + // '!' is used to negate the condition from 'out_of_bounds' directly in the 'all' call. + Self::inside_bounds(angle, self.centers[i], self.tolerances[i]) + }); + + println!("Questioning - {}", ok); + dump_joints(angles); + + ok } /// Return new vector of angle arrays, removing all that have members not satisfying these @@ -84,8 +114,24 @@ impl Constraints { #[cfg(test)] mod tests { + use crate::kinematic_traits::Solutions; + use crate::utils::{as_radians, dump_joints}; use super::*; + #[test] + fn test_historical_failure_1() { + let from = as_radians([9, 18, 28, 38, -5, 55]); + let angles = as_radians([10, 20, 30, 40, 0, 60]); + let to = as_radians([11, 22, 33, 44, 5, 65]); + + let limits = Constraints::new(from, to, BY_CONSTRAINS); + + let sols: Solutions = vec![angles]; + assert!(limits.filter(&sols).len() == 1); + + assert!(limits.compliant(&angles)); + } + #[test] fn test_no_wrap_around() { let angles = [0.1 * PI, 0.2 * PI, 0.3 * PI, 0.4 * PI, 0.5 * PI, 0.6 * PI]; diff --git a/src/kinematics_impl.rs b/src/kinematics_impl.rs index 1a075ee..f14319c 100644 --- a/src/kinematics_impl.rs +++ b/src/kinematics_impl.rs @@ -61,7 +61,170 @@ const J5: usize = 4; const J6: usize = 5; impl Kinematics for OPWKinematics { + + /// Return the solution that is constraint compliant anv values are valid + /// (no NaNs, etc) but otherwise not sorted. fn inverse(&self, pose: &Pose) -> Solutions { + self.filter_constraints_compliant(self.inverse_intern(&pose)) + } + + // Replaces singularity with correct solution + fn inverse_continuing(&self, pose: &Pose, prev: &Joints) -> Solutions { + let previous; + if prev[0].is_nan() { + // Special value CONSTRAINT_CENTERED has been used + previous = self.constraint_centers(); + } else { + previous = prev; + } + + const SINGULARITY_SHIFT: f64 = DISTANCE_TOLERANCE / 8.; + const SINGULARITY_SHIFTS: [[f64; 3]; 4] = + [[0., 0., 0., ], [SINGULARITY_SHIFT, 0., 0.], + [0., SINGULARITY_SHIFT, 0.], [0., 0., SINGULARITY_SHIFT]]; + + let mut solutions: Vec = Vec::with_capacity(9); + let pt = pose.translation; + + let rotation = pose.rotation; + 'shifts: for d in SINGULARITY_SHIFTS { + let shifted = Pose::from_parts( + Translation3::new(pt.x + d[0], pt.y + d[1], pt.z + d[2]), rotation); + let ik = self.inverse_intern(&shifted); + // Self::dump_shifted_solutions(d, &ik); + if solutions.is_empty() { + // Unshifted version that comes first is always included into results + solutions.extend(&ik); + } + + for s_idx in 0..ik.len() { + let singularity = + self.kinematic_singularity(&ik[s_idx]); + if singularity.is_some() && is_valid(&ik[s_idx]) { + let s; + let s_n; + if let Some(Singularity::A) = singularity { + let mut now = ik[s_idx]; + if are_angles_close(now[J5], 0.) { + // J5 = 0 singlularity, J4 and J6 rotate same direction + s = previous[J4] + previous[J6]; + s_n = now[J4] + now[J6]; + } else { + // J5 = -180 or 180 singularity, even if the robot would need + // specific design to rotate J5 to this angle without self-colliding. + // J4 and J6 rotate in opposite directions + s = previous[J4] - previous[J6]; + s_n = now[J4] - now[J6]; + + // Fix J5 sign to match the previous + normalize_near(&mut now[J5], previous[J5]); + } + + let mut angle = s_n - s; + while angle > PI { + angle -= 2.0 * PI; + } + while angle < -PI { + angle += 2.0 * PI; + } + let j_d = angle / 2.0; + + now[J4] = previous[J4] + j_d; + now[J6] = previous[J6] + j_d; + + // Check last time if the pose is ok + let check_pose = self.forward(&now); + if compare_poses(&pose, &check_pose, DISTANCE_TOLERANCE, ANGULAR_TOLERANCE) && + self.constraints_compliant(now) { + // Guard against the case our solution is out of constraints. + solutions.push(now); + // We only expect one singularity case hence once we found, we can end + break 'shifts; + } + } + + break; + } + } + } + // Before any sorting, normalize all angles to be close to + // 'previous' + for s_idx in 0..solutions.len() { + for joint_idx in 0..6 { + normalize_near(&mut solutions[s_idx][joint_idx], previous[joint_idx]); + } + } + self.sort_by_closeness(&mut solutions, &previous); + self.filter_constraints_compliant(solutions) + } + + fn forward(&self, joints: &Joints) -> Pose { + let p = &self.parameters; + + let q1 = joints[0] * p.sign_corrections[0] as f64 - p.offsets[0]; + let q2 = joints[1] * p.sign_corrections[1] as f64 - p.offsets[1]; + let q3 = joints[2] * p.sign_corrections[2] as f64 - p.offsets[2]; + let q4 = joints[3] * p.sign_corrections[3] as f64 - p.offsets[3]; + let q5 = joints[4] * p.sign_corrections[4] as f64 - p.offsets[4]; + let q6 = joints[5] * p.sign_corrections[5] as f64 - p.offsets[5]; + + let psi3 = f64::atan2(p.a2, p.c3); + let k = f64::sqrt(p.a2 * p.a2 + p.c3 * p.c3); + + let cx1 = p.c2 * f64::sin(q2) + k * f64::sin(q2 + q3 + psi3) + p.a1; + let cy1 = p.b; + let cz1 = p.c2 * f64::cos(q2) + k * f64::cos(q2 + q3 + psi3); + + let cx0 = cx1 * f64::cos(q1) - cy1 * f64::sin(q1); + let cy0 = cx1 * f64::sin(q1) + cy1 * f64::cos(q1); + let cz0 = cz1 + p.c1; + + let s1 = f64::sin(q1); + let s2 = f64::sin(q2); + let s3 = f64::sin(q3); + let s4 = f64::sin(q4); + let s5 = f64::sin(q5); + let s6 = f64::sin(q6); + + let c1 = f64::cos(q1); + let c2 = f64::cos(q2); + let c3 = f64::cos(q3); + let c4 = f64::cos(q4); + let c5 = f64::cos(q5); + let c6 = f64::cos(q6); + + let r_0c = Matrix3::new( + c1 * c2 * c3 - c1 * s2 * s3, -s1, c1 * c2 * s3 + c1 * s2 * c3, + s1 * c2 * c3 - s1 * s2 * s3, c1, s1 * c2 * s3 + s1 * s2 * c3, + -s2 * c3 - c2 * s3, 0.0, -s2 * s3 + c2 * c3, + ); + + let r_ce = Matrix3::new( + c4 * c5 * c6 - s4 * s6, -c4 * c5 * s6 - s4 * c6, c4 * s5, + s4 * c5 * c6 + c4 * s6, -s4 * c5 * s6 + c4 * c6, s4 * s5, + -s5 * c6, s5 * s6, c5, + ); + + let r_oe = r_0c * r_ce; + + let translation = Vector3::new(cx0, cy0, cz0) + p.c4 * r_oe * *self.unit_z; + let rotation = Rotation3::from_matrix_unchecked(r_oe); + + Pose::from_parts(Translation3::from(translation), + UnitQuaternion::from_rotation_matrix(&rotation)) + } + + fn kinematic_singularity(&self, joints: &Joints) -> Option { + if is_close_to_multiple_of_pi(joints[J5], SINGULARITY_ANGLE_THR) { + Some(Singularity::A) + } else { + None + } + } +} + +impl OPWKinematics { + fn inverse_intern(&self, pose: &Pose) -> Solutions { let params = &self.parameters; // Adjust to wrist center @@ -325,165 +488,8 @@ impl Kinematics for OPWKinematics { } } - self.filter_constraints_compliant(result) + result } - - // Replaces singularity with correct solution - fn inverse_continuing(&self, pose: &Pose, prev: &Joints) -> Solutions { - let previous; - if prev[0].is_nan() { - // Special value CONSTRAINT_CENTERED has been used - previous = self.constraint_centers(); - } else { - previous = prev; - } - - const SINGULARITY_SHIFT: f64 = DISTANCE_TOLERANCE / 8.; - const SINGULARITY_SHIFTS: [[f64; 3]; 4] = - [[0., 0., 0., ], [SINGULARITY_SHIFT, 0., 0.], - [0., SINGULARITY_SHIFT, 0.], [0., 0., SINGULARITY_SHIFT]]; - - let mut solutions: Vec = Vec::with_capacity(9); - let pt = pose.translation; - - let rotation = pose.rotation; - 'shifts: for d in SINGULARITY_SHIFTS { - let shifted = Pose::from_parts( - Translation3::new(pt.x + d[0], pt.y + d[1], pt.z + d[2]), rotation); - let ik = self.inverse(&shifted); - // Self::dump_shifted_solutions(d, &ik); - if solutions.is_empty() { - // Unshifted version that comes first is always included into results - solutions.extend(&ik); - } - - for s_idx in 0..ik.len() { - let singularity = - self.kinematic_singularity(&ik[s_idx]); - if singularity.is_some() && is_valid(&ik[s_idx]) { - let s; - let s_n; - if let Some(Singularity::A) = singularity { - let mut now = ik[s_idx]; - if are_angles_close(now[J5], 0.) { - // J5 = 0 singlularity, J4 and J6 rotate same direction - s = previous[J4] + previous[J6]; - s_n = now[J4] + now[J6]; - } else { - // J5 = -180 or 180 singularity, even if the robot would need - // specific design to rotate J5 to this angle without self-colliding. - // J4 and J6 rotate in opposite directions - s = previous[J4] - previous[J6]; - s_n = now[J4] - now[J6]; - - // Fix J5 sign to match the previous - normalize_near(&mut now[J5], previous[J5]); - } - - let mut angle = s_n - s; - while angle > PI { - angle -= 2.0 * PI; - } - while angle < -PI { - angle += 2.0 * PI; - } - let j_d = angle / 2.0; - - now[J4] = previous[J4] + j_d; - now[J6] = previous[J6] + j_d; - - // Check last time if the pose is ok - let check_pose = self.forward(&now); - if compare_poses(&pose, &check_pose, DISTANCE_TOLERANCE, ANGULAR_TOLERANCE) && - self.constraints_compliant(now) { - // Guard against the case our solution is out of constraints. - solutions.push(now); - // We only expect one singularity case hence once we found, we can end - break 'shifts; - } - } - - break; - } - } - } - // Before any sorting, normalize all angles to be close to - // 'previous' - for s_idx in 0..solutions.len() { - for joint_idx in 0..6 { - normalize_near(&mut solutions[s_idx][joint_idx], previous[joint_idx]); - } - } - self.sort_by_closeness(&mut solutions, &previous); - solutions - } - - fn forward(&self, joints: &Joints) -> Pose { - let p = &self.parameters; - - let q1 = joints[0] * p.sign_corrections[0] as f64 - p.offsets[0]; - let q2 = joints[1] * p.sign_corrections[1] as f64 - p.offsets[1]; - let q3 = joints[2] * p.sign_corrections[2] as f64 - p.offsets[2]; - let q4 = joints[3] * p.sign_corrections[3] as f64 - p.offsets[3]; - let q5 = joints[4] * p.sign_corrections[4] as f64 - p.offsets[4]; - let q6 = joints[5] * p.sign_corrections[5] as f64 - p.offsets[5]; - - let psi3 = f64::atan2(p.a2, p.c3); - let k = f64::sqrt(p.a2 * p.a2 + p.c3 * p.c3); - - let cx1 = p.c2 * f64::sin(q2) + k * f64::sin(q2 + q3 + psi3) + p.a1; - let cy1 = p.b; - let cz1 = p.c2 * f64::cos(q2) + k * f64::cos(q2 + q3 + psi3); - - let cx0 = cx1 * f64::cos(q1) - cy1 * f64::sin(q1); - let cy0 = cx1 * f64::sin(q1) + cy1 * f64::cos(q1); - let cz0 = cz1 + p.c1; - - let s1 = f64::sin(q1); - let s2 = f64::sin(q2); - let s3 = f64::sin(q3); - let s4 = f64::sin(q4); - let s5 = f64::sin(q5); - let s6 = f64::sin(q6); - - let c1 = f64::cos(q1); - let c2 = f64::cos(q2); - let c3 = f64::cos(q3); - let c4 = f64::cos(q4); - let c5 = f64::cos(q5); - let c6 = f64::cos(q6); - - let r_0c = Matrix3::new( - c1 * c2 * c3 - c1 * s2 * s3, -s1, c1 * c2 * s3 + c1 * s2 * c3, - s1 * c2 * c3 - s1 * s2 * s3, c1, s1 * c2 * s3 + s1 * s2 * c3, - -s2 * c3 - c2 * s3, 0.0, -s2 * s3 + c2 * c3, - ); - - let r_ce = Matrix3::new( - c4 * c5 * c6 - s4 * s6, -c4 * c5 * s6 - s4 * c6, c4 * s5, - s4 * c5 * c6 + c4 * s6, -s4 * c5 * s6 + c4 * c6, s4 * s5, - -s5 * c6, s5 * s6, c5, - ); - - let r_oe = r_0c * r_ce; - - let translation = Vector3::new(cx0, cy0, cz0) + p.c4 * r_oe * *self.unit_z; - let rotation = Rotation3::from_matrix_unchecked(r_oe); - - Pose::from_parts(Translation3::from(translation), - UnitQuaternion::from_rotation_matrix(&rotation)) - } - - fn kinematic_singularity(&self, joints: &Joints) -> Option { - if is_close_to_multiple_of_pi(joints[J5], SINGULARITY_ANGLE_THR) { - Some(Singularity::A) - } else { - None - } - } -} - -impl OPWKinematics { fn filter_constraints_compliant(&self, solutions: Solutions) -> Solutions { match &self.constraints { Some(constraints) => constraints.filter(&solutions), diff --git a/src/tests/constraint_test.rs b/src/tests/constraint_test.rs new file mode 100644 index 0000000..444f908 --- /dev/null +++ b/src/tests/constraint_test.rs @@ -0,0 +1,74 @@ +#[cfg(test)] +mod tests { + use std::f64::consts::PI; + use crate::constraints::{BY_CONSTRAINS, Constraints}; + use crate::kinematic_traits::{Kinematics, Solutions}; + use crate::kinematics_impl::OPWKinematics; + use crate::parameters::opw_kinematics::Parameters; + use crate::utils::{as_radians, dump_solutions, dump_solutions_degrees}; + + #[test] + fn test_constraints() { + // This robot has both A and B type singularity + // B type singularity two angles, maestro + let parameters = Parameters::staubli_tx2_160l(); + + let not_below = as_radians([9, 18, 28, 38, -5, 55]); + let joints = as_radians( [10, 20, 30, 40, 0, 60]); + let not_above = as_radians([11, 22, 33, 44, 5, 65]); + + + //let robot = OPWKinematics::new_with_constraints(parameters, + // Constraints::new(not_below, not_above, BY_CONSTRAINS)); + let robot = OPWKinematics::new(parameters.clone()); + let pose = robot.forward(&joints); + + let solutions = robot.inverse_continuing(&pose, &joints); + verify(&solutions, vec!( + [10.00, 20.00, 30.00, 40.00, 0.00, 60.00], + [10.00, 51.75, -30.00, -0.00, 28.25, 100.00], + [10.00, 51.75, -30.00, 180.00, -28.25, -80.00] + )); + + let constraints = Constraints::new(not_below, not_above, BY_CONSTRAINS); + assert!(constraints.compliant(&joints)); + let robot = OPWKinematics::new_with_constraints( + parameters.clone(), constraints); + let solutions = robot.inverse_continuing(&pose, &joints); + verify(&solutions, vec!( + [10.00, 20.00, 30.00, 40.00, 0.00, 60.00] + )); + } + + fn verify(actual: &Solutions, expected: Solutions) { + let tolerance = 1.0; + let mut solution_matches = true; + + if actual.len() != expected.len() { + solution_matches = false; + } else { + for sol_idx in 0..expected.len() { + for joint_idx in 0..6 { + let computed = actual[sol_idx][joint_idx].to_degrees(); + let asserted = expected[sol_idx][joint_idx]; + + let diff = (computed - asserted).abs(); + if diff >= tolerance && (diff - 2. * PI).abs() > tolerance { + // For angles, 360 degree difference means the same angle. + solution_matches = false; + break; + } + } + } + } + + if !solution_matches { + println!("Solutions do not match"); + println!("Expected:"); + dump_solutions_degrees(&expected); + println!("Actual"); + dump_solutions(&actual); + panic!("Solutions do not match"); + } + } +} \ No newline at end of file diff --git a/src/tests/mod.rs b/src/tests/mod.rs index 828a647..b8c8ced 100644 --- a/src/tests/mod.rs +++ b/src/tests/mod.rs @@ -1 +1,2 @@ -mod testcases; \ No newline at end of file +mod testcases; +mod constraint_test; \ No newline at end of file diff --git a/src/tests/testcases.rs b/src/tests/testcases.rs index 2e9e89f..6cf903a 100644 --- a/src/tests/testcases.rs +++ b/src/tests/testcases.rs @@ -91,11 +91,42 @@ fn are_isometries_approx_equal(a: &Isometry3, b: &Isometry3, tolerance mod tests { use std::collections::HashMap; use std::f64::consts::PI; - use crate::kinematic_traits::{Kinematics, Singularity, Solutions}; + use crate::kinematic_traits::{Joints, Kinematics, Singularity, Solutions}; use crate::parameters::opw_kinematics::Parameters; use crate::kinematics_impl::OPWKinematics; use super::*; + /// Check if 'expected' exists in the given vector of solutions. This function is also + /// used by other tests. + pub fn found_joints_approx_equal(solutions: &Solutions, expected: &Joints, tolerance: f64) -> Option { + for sol_idx in 0..solutions.len() { + // println!("Checking solution at index {}", sol_idx); + + let mut solution_matches = true; + for joint_idx in 0..6 { + let computed = solutions[sol_idx][joint_idx]; + let asserted = expected[joint_idx]; + + let diff = (computed - asserted).abs(); + //println!("Column value: {}, Expected value: {}, Difference: {}", + // computed, asserted, diff); + + if diff >= tolerance && (diff - 2. * PI).abs() > tolerance { + // For angles, 360 degree difference means the same angle. + solution_matches = false; + break; + } + } + + if solution_matches { + return Some(sol_idx as i32); // Return the index of the matching solution + } + } + + println!("No matching solution found"); + return None; // Explicitly indicate that no matching column was found + } + #[test] fn test_load_yaml() { let filename = "src/tests/cases.yaml"; @@ -287,34 +318,6 @@ mod tests { "Fully matching joints must come first. Expected Some(0), got {:?}", found_matching); } - fn found_joints_approx_equal(solutions: &Solutions, expected: &[f64; 6], tolerance: f64) -> Option { - for sol_idx in 0..solutions.len() { - // println!("Checking solution at index {}", sol_idx); - - let mut solution_matches = true; - for joint_idx in 0..6 { - let computed = solutions[sol_idx][joint_idx]; - let asserted = expected[joint_idx]; - - let diff = (computed - asserted).abs(); - //println!("Column value: {}, Expected value: {}, Difference: {}", - // computed, asserted, diff); - - if diff >= tolerance && (diff - 2. * PI).abs() > tolerance { - // For angles, 360 degree difference means the same angle. - solution_matches = false; - break; - } - } - - if solution_matches { - return Some(sol_idx as i32); // Return the index of the matching solution - } - } - - println!("No matching solution found"); - return None; // Explicitly indicate that no matching column was found - } fn create_parameter_map() -> HashMap { // Create map to get actual parameters that are not in the yaml file (maybe should be?) diff --git a/src/utils.rs b/src/utils.rs index b6b8098..abc3fb1 100644 --- a/src/utils.rs +++ b/src/utils.rs @@ -14,6 +14,9 @@ pub(crate) mod opw_kinematics { /// Print joint values for all solutions, converting radianst to degrees. #[allow(dead_code)] pub fn dump_solutions(solutions: &Solutions) { + if solutions.is_empty() { + println!("No solutions"); + } for sol_idx in 0..solutions.len() { let mut row_str = String::new(); for joint_idx in 0..6 { @@ -24,6 +27,21 @@ pub fn dump_solutions(solutions: &Solutions) { } } +#[allow(dead_code)] +pub fn dump_solutions_degrees(solutions: &Solutions) { + if solutions.is_empty() { + println!("No solutions"); + } + 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]; + row_str.push_str(&format!("{:5.2} ", computed)); + } + println!("[{}]", row_str.trim_end()); + } +} + /// Print joint values, converting radianst to degrees. #[allow(dead_code)] pub fn dump_joints(joints: &Joints) { @@ -37,7 +55,7 @@ pub fn dump_joints(joints: &Joints) { /// Allows to specify joint values in degrees (converts to radians) #[allow(dead_code)] -fn as_radians(degrees: [i32; 6]) -> Joints { +pub fn as_radians(degrees: [i32; 6]) -> Joints { std::array::from_fn(|i| (degrees[i] as f64).to_radians()) } From f79bf925df8242d369a93bd7cec3aaa19a3a259e Mon Sep 17 00:00:00 2001 From: Bourumir Wyngs Date: Mon, 29 Apr 2024 22:24:49 +0200 Subject: [PATCH 07/21] New great test for constraints written and now fails it. --- Cargo.lock | 87 +++++++++++++++++++++++++ Cargo.toml | 1 + src/constraints.rs | 2 +- src/tests/constraint_test_auto.rs | 104 ++++++++++++++++++++++++++++++ src/tests/mod.rs | 3 +- 5 files changed, 195 insertions(+), 2 deletions(-) create mode 100644 src/tests/constraint_test_auto.rs diff --git a/Cargo.lock b/Cargo.lock index 3b6fbca..314730b 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -32,12 +32,29 @@ version = "1.15.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "5d6d68c57235a3a081186990eca2867354726650f42f7516ca50c28d6281fd15" +[[package]] +name = "cfg-if" +version = "1.0.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "baf1de4339761588bc0619e3cbc0120ee582ebb74b53b4efbf79117bd2da40fd" + [[package]] name = "equivalent" version = "1.0.1" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "5443807d6dff69373d433ab9ef5378ad8df50ca6298caf15de6e52e24aaf54d5" +[[package]] +name = "getrandom" +version = "0.2.14" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "94b22e06ecb0110981051723910cbf0b5f5e09a2062dd7663334ee79a9d1286c" +dependencies = [ + "cfg-if", + "libc", + "wasi", +] + [[package]] name = "hashbrown" version = "0.14.3" @@ -60,6 +77,12 @@ version = "1.0.11" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "49f1f14873335454500d59611f1cf4a4b0f786f9ac11f4312a78e4cf2566695b" +[[package]] +name = "libc" +version = "0.2.153" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "9c198f91728a82281a64e1f4f9eeb25d82cb32a5de251c6bd1b5154d63a8e7bd" + [[package]] name = "matrixmultiply" version = "0.3.8" @@ -147,6 +170,12 @@ version = "1.0.14" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "de3145af08024dea9fa9914f381a17b8fc6034dfb00f3a84013f7ff43f29ed4c" +[[package]] +name = "ppv-lite86" +version = "0.2.17" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "5b40af805b3121feab8a3c29f04d8ad262fa8e0561883e7653e024ae4479e6de" + [[package]] name = "proc-macro2" version = "1.0.79" @@ -165,6 +194,37 @@ dependencies = [ "proc-macro2", ] +[[package]] +name = "rand" +version = "0.9.0-alpha.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "8d31e63ea85be51c423e52ba8f2e68a3efd53eed30203ee029dd09947333693e" +dependencies = [ + "rand_chacha", + "rand_core", + "zerocopy", +] + +[[package]] +name = "rand_chacha" +version = "0.9.0-alpha.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "78674ef918c19451dbd250f8201f8619b494f64c9aa6f3adb28fd8a0f1f6da46" +dependencies = [ + "ppv-lite86", + "rand_core", +] + +[[package]] +name = "rand_core" +version = "0.9.0-alpha.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "cc89dffba8377c5ec847d12bb41492bda235dba31a25e8b695cd0fe6589eb8c9" +dependencies = [ + "getrandom", + "zerocopy", +] + [[package]] name = "rawpointer" version = "0.2.1" @@ -205,6 +265,7 @@ name = "rs-opw-kinematics" version = "1.1.0" dependencies = [ "nalgebra", + "rand", "regex", "serde", "serde_yaml", @@ -332,6 +393,12 @@ version = "0.2.11" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "673aac59facbab8a9007c7f6108d11f63b603f7cabff99fabf650fea5c32b861" +[[package]] +name = "wasi" +version = "0.11.0+wasi-snapshot-preview1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "9c8d87e72b64a3b4db28d11ce29237c246188f4f51057d65a7eab63b7987e423" + [[package]] name = "wide" version = "0.7.15" @@ -341,3 +408,23 @@ dependencies = [ "bytemuck", "safe_arch", ] + +[[package]] +name = "zerocopy" +version = "0.8.0-alpha.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "db678a6ee512bd06adf35c35be471cae2f9c82a5aed2b5d15e03628c98bddd57" +dependencies = [ + "zerocopy-derive", +] + +[[package]] +name = "zerocopy-derive" +version = "0.8.0-alpha.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "201585ea96d37ee69f2ac769925ca57160cef31acb137c16f38b02b76f4c1e62" +dependencies = [ + "proc-macro2", + "quote", + "syn 2.0.57", +] diff --git a/Cargo.toml b/Cargo.toml index 500adc5..ea6f5e7 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -16,5 +16,6 @@ thiserror = "1.0.59" serde = { version = "1.0", features = ["derive"] } serde_yaml = "0.9.34" regex = "1.10.4" +rand = "0.9.0-alpha.1" diff --git a/src/constraints.rs b/src/constraints.rs index 3ba54bd..0aa2a07 100644 --- a/src/constraints.rs +++ b/src/constraints.rs @@ -1,5 +1,5 @@ use std::f64::consts::PI; -use std::f64::{INFINITY, NAN}; +use std::f64::INFINITY; use crate::kinematic_traits::{Joints, JOINTS_AT_ZERO}; use crate::utils::dump_joints; diff --git a/src/tests/constraint_test_auto.rs b/src/tests/constraint_test_auto.rs new file mode 100644 index 0000000..1c72a20 --- /dev/null +++ b/src/tests/constraint_test_auto.rs @@ -0,0 +1,104 @@ +#[cfg(test)] +mod tests { + extern crate rand; + + use rand::{Rng, SeedableRng}; + use rand::rngs::StdRng; + use crate::constraints::{BY_CONSTRAINS, Constraints}; + use crate::utils::as_radians; + + // Define the data structure for a test case + struct TestCase { + id: usize, + from_angles: [i32; 6], + to_angles: [i32; 6], + check_angles: [i32; 6], + expected_results: [bool; 6], + passing: bool, + } + + impl TestCase { + pub fn generate(id: usize, rng: &mut StdRng) -> Self { + let mut from_angles = [0; 6]; + let mut to_angles = [0; 6]; + let mut check_angles = [0; 6]; + let mut expected_results = [false; 6]; + + let mut all_passing = true; + for i in 0..6 { + let inside_range = rng.gen::(); + let a = rng.gen_range(1..359); + let b = rng.gen_range(1..359); + if inside_range { + from_angles[i] = a.min(b); + to_angles[i] = a.max(b); + } else { + from_angles[i] = a.max(b); + to_angles[i] = a.min(b); + } + + expected_results[i] = rng.gen_bool(0.95); + if expected_results[i] { + if from_angles[i] <= to_angles[i] { + check_angles[i] = rng.gen_range(from_angles[i]..=to_angles[i]); + } else { + check_angles[i] = if rng.gen::() { + rng.gen_range(from_angles[i]..360) + } else { + rng.gen_range(0..=to_angles[i]) + }; + } + } else { + if from_angles[i] <= to_angles[i] { + check_angles[i] = if rng.gen::() { + rng.gen_range(0..from_angles[i]) + } else { + rng.gen_range(to_angles[i] + 1..360) + }; + } else { + // Handle wrap-around failure properly + if to_angles[i] != 359 { + check_angles[i] = rng.gen_range(to_angles[i] + 1..360); + } else { + check_angles[i] = rng.gen_range(0..from_angles[i]); + } + all_passing = false; + } + } + } + + TestCase { + id, + from_angles, + to_angles, + check_angles, + expected_results, + passing: all_passing + } + } + } + + #[test] + fn test_generated_constraints() { + let seed = [0u8; 32]; + let mut rng = StdRng::from_seed(seed); + + // Generate multiple test cases + for id in 0..2048 { + let case = TestCase::generate(id, &mut rng); + let constraints = Constraints::new( + as_radians(case.from_angles), + as_radians(case.to_angles), + BY_CONSTRAINS, + ); + let actual = constraints.compliant(&as_radians(case.check_angles)); + if actual != case.passing { + println!("Case mimatch: expected {}, actual {}", case.passing, actual); + println!("ID: {}, From: {:?}, To: {:?}, Check: {:?}, Result: {:?} Passing {:?}", + case.id, case.from_angles, case.to_angles, case.check_angles, + case.expected_results, case.passing); + panic!("Test case {} failed", case.id); + } + } + } +} \ No newline at end of file diff --git a/src/tests/mod.rs b/src/tests/mod.rs index b8c8ced..7fadbdd 100644 --- a/src/tests/mod.rs +++ b/src/tests/mod.rs @@ -1,2 +1,3 @@ mod testcases; -mod constraint_test; \ No newline at end of file +mod constraint_test; +mod constraint_test_auto; \ No newline at end of file From 2a9b6f60a97ee960663f90a479db95c63ceec988 Mon Sep 17 00:00:00 2001 From: Bourumir Wyngs Date: Wed, 1 May 2024 20:13:51 +0200 Subject: [PATCH 08/21] Autogenerated test cases 0 .. 360 pass --- src/constraints.rs | 4 +- src/tests/constraint_test_auto.rs | 92 +++++++++++++++++++------------ 2 files changed, 59 insertions(+), 37 deletions(-) diff --git a/src/constraints.rs b/src/constraints.rs index 0aa2a07..c639697 100644 --- a/src/constraints.rs +++ b/src/constraints.rs @@ -96,7 +96,7 @@ impl Constraints { Self::inside_bounds(angle, self.centers[i], self.tolerances[i]) }); - println!("Questioning - {}", ok); + //println!("Questioning - {}", ok); dump_joints(angles); ok @@ -115,7 +115,7 @@ impl Constraints { #[cfg(test)] mod tests { use crate::kinematic_traits::Solutions; - use crate::utils::{as_radians, dump_joints}; + use crate::utils::{as_radians}; use super::*; #[test] diff --git a/src/tests/constraint_test_auto.rs b/src/tests/constraint_test_auto.rs index 1c72a20..d050393 100644 --- a/src/tests/constraint_test_auto.rs +++ b/src/tests/constraint_test_auto.rs @@ -2,6 +2,7 @@ mod tests { extern crate rand; + use std::mem; use rand::{Rng, SeedableRng}; use rand::rngs::StdRng; use crate::constraints::{BY_CONSTRAINS, Constraints}; @@ -24,47 +25,53 @@ mod tests { let mut check_angles = [0; 6]; let mut expected_results = [false; 6]; - let mut all_passing = true; for i in 0..6 { - let inside_range = rng.gen::(); - let a = rng.gen_range(1..359); - let b = rng.gen_range(1..359); - if inside_range { - from_angles[i] = a.min(b); - to_angles[i] = a.max(b); - } else { - from_angles[i] = a.max(b); - to_angles[i] = a.min(b); + // Generate two angles that are not equal + let mut a: i32 = rng.gen_range(0..360); + let mut b: i32 = rng.gen_range(0..360); + + while (a - b).abs() < 2 { // Leave at least on value in between + b = rng.gen_range(0..360); // generate again } - expected_results[i] = rng.gen_bool(0.95); - if expected_results[i] { - if from_angles[i] <= to_angles[i] { - check_angles[i] = rng.gen_range(from_angles[i]..=to_angles[i]); - } else { - check_angles[i] = if rng.gen::() { - rng.gen_range(from_angles[i]..360) - } else { - rng.gen_range(0..=to_angles[i]) - }; - } + // Make sure b > a + if b < a { + mem::swap(&mut a, &mut b); + } + let c; // the value to check + + // Decide if the case should pass + let pass = rng.gen_bool(0.95); + + // ordinary case, a < c < b to pass + if pass { + // Generate c within the range from a to b boundaries exclusive + c = rng.gen_range(a + 1..b); } else { - if from_angles[i] <= to_angles[i] { - check_angles[i] = if rng.gen::() { - rng.gen_range(0..from_angles[i]) - } else { - rng.gen_range(to_angles[i] + 1..360) + c = loop { + // Generate outside the range, either below a or above b: + if rng.gen_bool(0.5) && a > 0 { + // below a + break rng.gen_range(0..a); + } else if b < 360 - 2 { // 360 and 359 would not generate as expected + // above b + break rng.gen_range(b + 1..360); }; - } else { - // Handle wrap-around failure properly - if to_angles[i] != 359 { - check_angles[i] = rng.gen_range(to_angles[i] + 1..360); - } else { - check_angles[i] = rng.gen_range(0..from_angles[i]); - } - all_passing = false; } } + + // Decide if we are doing the "wrap arround 360 or 0 case" or ordinary case + if rng.gen_bool(0.5) { + expected_results[i] = pass; + from_angles[i] = a; + to_angles[i] = b; + check_angles[i] = c; + } else { + expected_results[i] = !pass; + from_angles[i] = b; + to_angles[i] = a; + check_angles[i] = c; + } } TestCase { @@ -73,7 +80,7 @@ mod tests { to_angles, check_angles, expected_results, - passing: all_passing + passing: expected_results.iter().all(|&val| val), } } } @@ -97,6 +104,21 @@ mod tests { println!("ID: {}, From: {:?}, To: {:?}, Check: {:?}, Result: {:?} Passing {:?}", case.id, case.from_angles, case.to_angles, case.check_angles, case.expected_results, case.passing); + println!("Deep check"); + + // To make analysis of the glitch easier, we set contraints and all angles + // as one + for p in 0..6 { + let focused_constraints = + Constraints::new( + as_radians([case.from_angles[p]; 6]), + as_radians([case.to_angles[p]; 6]), + BY_CONSTRAINS); + let joints = as_radians([case.check_angles[p]; 6]); + println!("{}: {} .. {} : {} ? = {}", p, + case.from_angles[p], case.to_angles[p], case.check_angles[p], + focused_constraints.compliant(&joints)); + } panic!("Test case {} failed", case.id); } } From 9e31790ca1ee3001711722cf8f404c37f48b347c Mon Sep 17 00:00:00 2001 From: Bourumir Wyngs Date: Wed, 1 May 2024 20:38:45 +0200 Subject: [PATCH 09/21] Autogenerated test cases -180 .. 180 pass --- src/tests/constraint_test_auto.rs | 52 +++++++++++++++++++++++++++++-- 1 file changed, 49 insertions(+), 3 deletions(-) diff --git a/src/tests/constraint_test_auto.rs b/src/tests/constraint_test_auto.rs index d050393..75ee97a 100644 --- a/src/tests/constraint_test_auto.rs +++ b/src/tests/constraint_test_auto.rs @@ -65,12 +65,12 @@ mod tests { expected_results[i] = pass; from_angles[i] = a; to_angles[i] = b; - check_angles[i] = c; + check_angles[i] = c; } else { expected_results[i] = !pass; from_angles[i] = b; to_angles[i] = a; - check_angles[i] = c; + check_angles[i] = c; } } @@ -123,4 +123,50 @@ mod tests { } } } -} \ No newline at end of file + + #[test] + /// This tests checks the "shifted" case where both angles and constraints + /// are in the range from -180 to 180 + fn test_generated_constraints_shifted() { + fn shift(angle: i32) -> i32 { + // 0 .. 360 to - 180 .. 180 + angle - 180 + } + + let seed = [0u8; 32]; + let mut rng = StdRng::from_seed(seed); + + // Generate multiple test cases + for id in 0..2048 { + let case = TestCase::generate(id, &mut rng); + let constraints = Constraints::new( + as_radians(case.from_angles), + as_radians(case.to_angles), + BY_CONSTRAINS, + ); + let actual = constraints.compliant(&as_radians(case.check_angles)); + if actual != case.passing { + println!("Case mimatch: expected {}, actual {}", case.passing, actual); + println!("ID: {}, From: {:?}, To: {:?}, Check: {:?}, Result: {:?} Passing {:?}", + case.id, case.from_angles, case.to_angles, case.check_angles, + case.expected_results, case.passing); + println!("Deep check"); + + // To make analysis of the glitch easier, we set contraints and all angles + // as one + for p in 0..6 { + let focused_constraints = + Constraints::new( + as_radians([shift(case.from_angles[p]); 6]), + as_radians([shift(case.to_angles[p]); 6]), + BY_CONSTRAINS); + let joints = as_radians([shift(case.check_angles[p]); 6]); + println!("{}: {} .. {} : {} ? = {}", p, + case.from_angles[p], case.to_angles[p], shift(case.check_angles[p]), + focused_constraints.compliant(&joints)); + } + panic!("Test case {} failed", case.id); + } + } + } +} From f7ba8f767f45566401ded05d2b3aa5cf70d365bc Mon Sep 17 00:00:00 2001 From: Bourumir Wyngs Date: Wed, 1 May 2024 22:14:07 +0200 Subject: [PATCH 10/21] Almost done but still need to finish example in README.md --- README.md | 38 +++++ src/constraints.rs | 17 +- src/tests/constraint_test_auto.rs | 172 ------------------- src/tests/constraint_test_various.rs | 240 +++++++++++++++++++++++++++ src/tests/mod.rs | 2 +- 5 files changed, 284 insertions(+), 185 deletions(-) delete mode 100644 src/tests/constraint_test_auto.rs create mode 100644 src/tests/constraint_test_various.rs diff --git a/README.md b/README.md index 006ddc4..9e211e2 100644 --- a/README.md +++ b/README.md @@ -106,6 +106,44 @@ fn main() { } ``` +```Rust +use rs_opw_kinematics::kinematic_traits::{Joints, Kinematics, Pose, CONSTRAINTS_CENTERED}; +use rs_opw_kinematics::kinematics_impl::OPWKinematics; +use rs_opw_kinematics::parameters::opw_kinematics::Parameters; +use rs_opw_kinematics::utils::{dump_joints, dump_solutions}; + +fn main() { + // TODO this example is unfinished and wrong! + let robot = OPWKinematics::new(Parameters::irb2400_10()); + let joints: Joints = [0.0, 0.1, 0.2, 0.3, 0.0, 0.5]; // Joints are alias of [f64; 6] + + let when_continuing_from: [f64; 6] = [0.0, 0.11, 0.22, 0.3, 0.1, 0.5]; + println!("No constraints"); + let solutions = robot.inverse_continuing(&pose, &when_continuing_from); + dump_solutions(&solutions); + + println!("With constraints, sorted by proximity to the previous pose"); + let when_continuing_from_j6_0: [f64; 6] = [0.0, 0.11, 0.22, 0.8, 0.1, 0.0]; + let solutions = robot.inverse_continuing(&pose, &when_continuing_from_j6_0); + dump_solutions(&solutions); + + println!("With constraints, sorted by proximity to the center of constraints"); + let when_continuing_from_j6_0: [f64; 6] = [0.0, 0.11, 0.22, 0.8, 0.1, 0.0]; + let solutions = robot.inverse_continuing(&pose, &when_continuing_from_j6_0); + dump_solutions(&solutions); + + println!("With constraints, somewhat intermediate"); + let when_continuing_from_j6_0: [f64; 6] = [0.0, 0.11, 0.22, 0.8, 0.1, 0.0]; + let solutions = robot.inverse_continuing(&pose, &when_continuing_from_j6_0); + dump_solutions(&solutions); +} +``` + +# Constraints +Since 1.1.0, it is possible to set constraints for the joints. Robot poses where any of the joints are outside +the specified constraint range are not included into returned list of solutions. It is also possible to +influence the sorting of the result list by giving some preference to the center of constratints. + # Configuring the solver for your robot The project contains built-in definitions for ABB IRB 2400/10, IRB 2600-12/1.65, IRB 4600-60/2.05; KUKA KR 6 R700 sixx, FANUC R-2000iB/200R; Stäubli TX40, TX2-140, TX2-160 and TX2-160L with various levels of diff --git a/src/constraints.rs b/src/constraints.rs index c639697..a8d5601 100644 --- a/src/constraints.rs +++ b/src/constraints.rs @@ -1,7 +1,6 @@ use std::f64::consts::PI; use std::f64::INFINITY; use crate::kinematic_traits::{Joints, JOINTS_AT_ZERO}; -use crate::utils::dump_joints; #[derive(Clone)] pub struct Constraints { @@ -33,7 +32,6 @@ pub const BY_PREV: f64 = 0.0; const TWO_PI: f64 = 2.0 * PI; impl Constraints { - /// Create constraints that restrict the joint rotations between 'from' to 'to' values. /// Wrapping arround is supported so order is important. For instance, /// from = 0.1 and to = 0.2 (radians) means the joint @@ -45,7 +43,6 @@ impl Constraints { /// 1.0 (or BY_CONSTRAINTS) gives absolute priority to the middle values of constraints. /// Intermediate values like 0.5 provide the weighted compromise. pub fn new(from: Joints, to: Joints, sorting_weight: f64) -> Self { - let mut centers: Joints = JOINTS_AT_ZERO; let mut tolerances: Joints = JOINTS_AT_ZERO; @@ -54,7 +51,7 @@ impl Constraints { let mut b = to[j_idx]; if a == b { tolerances[j_idx] = INFINITY; // No constraint, not checked - } else if a < b { + } else if a < b { // Values do not wrap arround centers[j_idx] = (a + b) / 2.0; tolerances[j_idx] = (b - a) / 2.0; @@ -64,7 +61,7 @@ impl Constraints { b = b + TWO_PI; } centers[j_idx] = (a + b) / 2.0; - tolerances[j_idx] = (b - a) / 2.0; + tolerances[j_idx] = (b - a) / 2.0; } } @@ -87,18 +84,14 @@ impl Constraints { difference = TWO_PI - difference; } difference <= tolerance - } - + } + /// Checks if all values in the given vector or angles satisfy these constraints. pub fn compliant(&self, angles: &[f64; 6]) -> bool { let ok = angles.iter().enumerate().all(|(i, &angle)| { // '!' is used to negate the condition from 'out_of_bounds' directly in the 'all' call. Self::inside_bounds(angle, self.centers[i], self.tolerances[i]) }); - - //println!("Questioning - {}", ok); - dump_joints(angles); - ok } @@ -128,7 +121,7 @@ mod tests { let sols: Solutions = vec![angles]; assert!(limits.filter(&sols).len() == 1); - + assert!(limits.compliant(&angles)); } diff --git a/src/tests/constraint_test_auto.rs b/src/tests/constraint_test_auto.rs deleted file mode 100644 index 75ee97a..0000000 --- a/src/tests/constraint_test_auto.rs +++ /dev/null @@ -1,172 +0,0 @@ -#[cfg(test)] -mod tests { - extern crate rand; - - use std::mem; - use rand::{Rng, SeedableRng}; - use rand::rngs::StdRng; - use crate::constraints::{BY_CONSTRAINS, Constraints}; - use crate::utils::as_radians; - - // Define the data structure for a test case - struct TestCase { - id: usize, - from_angles: [i32; 6], - to_angles: [i32; 6], - check_angles: [i32; 6], - expected_results: [bool; 6], - passing: bool, - } - - impl TestCase { - pub fn generate(id: usize, rng: &mut StdRng) -> Self { - let mut from_angles = [0; 6]; - let mut to_angles = [0; 6]; - let mut check_angles = [0; 6]; - let mut expected_results = [false; 6]; - - for i in 0..6 { - // Generate two angles that are not equal - let mut a: i32 = rng.gen_range(0..360); - let mut b: i32 = rng.gen_range(0..360); - - while (a - b).abs() < 2 { // Leave at least on value in between - b = rng.gen_range(0..360); // generate again - } - - // Make sure b > a - if b < a { - mem::swap(&mut a, &mut b); - } - let c; // the value to check - - // Decide if the case should pass - let pass = rng.gen_bool(0.95); - - // ordinary case, a < c < b to pass - if pass { - // Generate c within the range from a to b boundaries exclusive - c = rng.gen_range(a + 1..b); - } else { - c = loop { - // Generate outside the range, either below a or above b: - if rng.gen_bool(0.5) && a > 0 { - // below a - break rng.gen_range(0..a); - } else if b < 360 - 2 { // 360 and 359 would not generate as expected - // above b - break rng.gen_range(b + 1..360); - }; - } - } - - // Decide if we are doing the "wrap arround 360 or 0 case" or ordinary case - if rng.gen_bool(0.5) { - expected_results[i] = pass; - from_angles[i] = a; - to_angles[i] = b; - check_angles[i] = c; - } else { - expected_results[i] = !pass; - from_angles[i] = b; - to_angles[i] = a; - check_angles[i] = c; - } - } - - TestCase { - id, - from_angles, - to_angles, - check_angles, - expected_results, - passing: expected_results.iter().all(|&val| val), - } - } - } - - #[test] - fn test_generated_constraints() { - let seed = [0u8; 32]; - let mut rng = StdRng::from_seed(seed); - - // Generate multiple test cases - for id in 0..2048 { - let case = TestCase::generate(id, &mut rng); - let constraints = Constraints::new( - as_radians(case.from_angles), - as_radians(case.to_angles), - BY_CONSTRAINS, - ); - let actual = constraints.compliant(&as_radians(case.check_angles)); - if actual != case.passing { - println!("Case mimatch: expected {}, actual {}", case.passing, actual); - println!("ID: {}, From: {:?}, To: {:?}, Check: {:?}, Result: {:?} Passing {:?}", - case.id, case.from_angles, case.to_angles, case.check_angles, - case.expected_results, case.passing); - println!("Deep check"); - - // To make analysis of the glitch easier, we set contraints and all angles - // as one - for p in 0..6 { - let focused_constraints = - Constraints::new( - as_radians([case.from_angles[p]; 6]), - as_radians([case.to_angles[p]; 6]), - BY_CONSTRAINS); - let joints = as_radians([case.check_angles[p]; 6]); - println!("{}: {} .. {} : {} ? = {}", p, - case.from_angles[p], case.to_angles[p], case.check_angles[p], - focused_constraints.compliant(&joints)); - } - panic!("Test case {} failed", case.id); - } - } - } - - #[test] - /// This tests checks the "shifted" case where both angles and constraints - /// are in the range from -180 to 180 - fn test_generated_constraints_shifted() { - fn shift(angle: i32) -> i32 { - // 0 .. 360 to - 180 .. 180 - angle - 180 - } - - let seed = [0u8; 32]; - let mut rng = StdRng::from_seed(seed); - - // Generate multiple test cases - for id in 0..2048 { - let case = TestCase::generate(id, &mut rng); - let constraints = Constraints::new( - as_radians(case.from_angles), - as_radians(case.to_angles), - BY_CONSTRAINS, - ); - let actual = constraints.compliant(&as_radians(case.check_angles)); - if actual != case.passing { - println!("Case mimatch: expected {}, actual {}", case.passing, actual); - println!("ID: {}, From: {:?}, To: {:?}, Check: {:?}, Result: {:?} Passing {:?}", - case.id, case.from_angles, case.to_angles, case.check_angles, - case.expected_results, case.passing); - println!("Deep check"); - - // To make analysis of the glitch easier, we set contraints and all angles - // as one - for p in 0..6 { - let focused_constraints = - Constraints::new( - as_radians([shift(case.from_angles[p]); 6]), - as_radians([shift(case.to_angles[p]); 6]), - BY_CONSTRAINS); - let joints = as_radians([shift(case.check_angles[p]); 6]); - println!("{}: {} .. {} : {} ? = {}", p, - case.from_angles[p], case.to_angles[p], shift(case.check_angles[p]), - focused_constraints.compliant(&joints)); - } - panic!("Test case {} failed", case.id); - } - } - } -} diff --git a/src/tests/constraint_test_various.rs b/src/tests/constraint_test_various.rs new file mode 100644 index 0000000..a16d8d0 --- /dev/null +++ b/src/tests/constraint_test_various.rs @@ -0,0 +1,240 @@ +#[cfg(test)] +mod tests { + extern crate rand; + + use std::mem; + use rand::{Rng, SeedableRng}; + use rand::rngs::StdRng; + use crate::constraints::{BY_CONSTRAINS, Constraints}; + use crate::utils::as_radians; + + // Define the data structure for a test case + struct TestCase { + id: usize, + from_angles: [i32; 6], + to_angles: [i32; 6], + check_angles: [i32; 6], + expected_results: [bool; 6], + passing: bool, + } + + impl TestCase { + pub fn generate(id: usize, rng: &mut StdRng) -> Self { + let mut from_angles = [0; 6]; + let mut to_angles = [0; 6]; + let mut check_angles = [0; 6]; + let mut expected_results = [false; 6]; + + for i in 0..6 { + // Generate two angles that are not equal + let mut a: i32 = rng.gen_range(0..360); + let mut b: i32 = rng.gen_range(0..360); + + while (a - b).abs() < 2 { // Leave at least on value in between + b = rng.gen_range(0..360); // generate again + } + + // Make sure b > a + if b < a { + mem::swap(&mut a, &mut b); + } + let c; // the value to check + + // Decide if the case should pass + let pass = rng.gen_bool(0.95); + + // ordinary case, a < c < b to pass + if pass { + // Generate c within the range from a to b boundaries exclusive + c = rng.gen_range(a + 1..b); + } else { + c = loop { + // Generate outside the range, either below a or above b: + if rng.gen_bool(0.5) && a > 0 { + // below a + break rng.gen_range(0..a); + } else if b < 360 - 2 { // 360 and 359 would not generate as expected + // above b + break rng.gen_range(b + 1..360); + }; + } + } + + // Decide if we are doing the "wrap arround 360 or 0 case" or ordinary case + if rng.gen_bool(0.5) { + expected_results[i] = pass; + from_angles[i] = a; + to_angles[i] = b; + check_angles[i] = c; + } else { + expected_results[i] = !pass; + from_angles[i] = b; + to_angles[i] = a; + check_angles[i] = c; + } + } + + TestCase { + id, + from_angles, + to_angles, + check_angles, + expected_results, + passing: expected_results.iter().all(|&val| val), + } + } + } + + #[test] + fn test_generated_constraints_0_360() { + test_generated_constraints_shifted_by(0); + } + + #[test] + /// This tests checks the "shifted" case where both angles and constraints + /// are in the range from -180 to 180 + fn test_generated_constraints_180_180() { + test_generated_constraints_shifted_by(-180); + } + + #[test] + fn test_special_cases() { + let cases: Vec = vec![ + TestCase { + id: 0, + from_angles: [-20; 6], + to_angles: [190; 6], + check_angles: [-19, -5, 0, 20, 100, 189], + expected_results: [true; 6], + passing: true, + }, + TestCase { + id: 1, + from_angles: [-50; 6], + to_angles: [260; 6], + check_angles: [-49, -5, 0, 20, 200, 259], + expected_results: [true; 6], + passing: true, + }, + TestCase { + id: 2, + from_angles: [-50; 6], + to_angles: [260; 6], + check_angles: [-51, -5, 0, 20, 200, 259], + expected_results: [false, true, true, true, true, true], + passing: false, + }, + TestCase { + id: 2, + from_angles: [-50; 6], + to_angles: [260; 6], + check_angles: [261, -5, 0, 20, 200, 259], + expected_results: [false, true, true, true, true, true], + passing: false, + }, + TestCase { + id: 3, + from_angles: [260; 6], + to_angles: [-50; 6], + check_angles: [-49, -5, 0, 20, 200, 259], + expected_results: [false; 6], + passing: false, + }, + ]; + + for case in cases { + test_case(&case, 0); + } + } + + #[test] + fn test_boundary_cases() { + let cases: Vec = vec![ + TestCase { + id: 0, + from_angles: [-60; 6], + to_angles: [60; 6], + check_angles: [-60, 60, -59, 59, 0, 1], + expected_results: [true; 6], + passing: true, + }, + TestCase { + id: 1, + from_angles: [-60; 6], + to_angles: [60; 6], + check_angles: [-61, 61, -69, 69, 180, 181], + expected_results: [false; 6], + passing: false, + }, + + ]; + + for case in cases { + test_case(&case, 0); + } + } + + #[test] + fn test_over_360() { + let cases: Vec = vec![ + TestCase { + id: 0, + from_angles: [-60; 6], + to_angles: [60; 6], + check_angles: [-60 + 360, 60, -59 + 360, 59, 0, 0], + expected_results: [true; 6], + passing: true, + }, + ]; + + for case in cases { + test_case(&case, 0); + } + } + + + fn test_generated_constraints_shifted_by(offset: i32) { + let seed = [0u8; 32]; + let mut rng = StdRng::from_seed(seed); + + // Generate multiple test cases + for id in 0..2048 { + let case = TestCase::generate(id, &mut rng); + test_case(&case, offset); + } + } + + fn test_case(case: &TestCase, offset: i32) { + let constraints = Constraints::new( + as_radians(case.from_angles), + as_radians(case.to_angles), + BY_CONSTRAINS, + ); + let actual = constraints.compliant(&as_radians(case.check_angles)); + if actual != case.passing { + println!("Case mimatch: expected {}, actual {}", case.passing, actual); + println!("ID: {}, From: {:?}, To: {:?}, Check: {:?}, Result: {:?} Passing {:?}", + case.id, case.from_angles, case.to_angles, case.check_angles, + case.expected_results, case.passing); + println!("Deep check"); + + // To make analysis of the glitch easier, we set contraints and all angles + // as one + for p in 0..6 { + let from = case.to_angles[p]; + let to = case.from_angles[p]; + let focused_constraints = + Constraints::new( + as_radians([to + offset; 6]), + as_radians([from + offset; 6]), + BY_CONSTRAINS); + let angle = case.check_angles[p]; + let joints = as_radians([angle + offset; 6]); + println!("{}: {} .. {} : {} ? = {}", p, + case.from_angles[p], case.to_angles[p], angle + offset, + focused_constraints.compliant(&joints)); + } + panic!("Test case {} failed", case.id); + } + } +} diff --git a/src/tests/mod.rs b/src/tests/mod.rs index 7fadbdd..285a1ac 100644 --- a/src/tests/mod.rs +++ b/src/tests/mod.rs @@ -1,3 +1,3 @@ mod testcases; mod constraint_test; -mod constraint_test_auto; \ No newline at end of file +mod constraint_test_various; \ No newline at end of file From 85bb073c0268c7abf8e1c3148570831ee3faca2c Mon Sep 17 00:00:00 2001 From: Bourumir Wyngs Date: Thu, 2 May 2024 22:32:48 +0200 Subject: [PATCH 11/21] Preparations for 1.1.0-rc1 --- Cargo.toml | 2 +- README.md | 55 +++++++++++++++++++----------------------- src/kinematics_impl.rs | 6 ++--- src/main.rs | 22 +++++++++++++++++ 4 files changed, 51 insertions(+), 34 deletions(-) diff --git a/Cargo.toml b/Cargo.toml index ea6f5e7..4534972 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -1,6 +1,6 @@ [package] name = "rs-opw-kinematics" -version = "1.1.0" +version = "1.1.0-rc1" edition = "2021" authors = ["Bourumir Wyngs "] description = "Inverse and forward kinematics for 6 axis robots with a parallel base and spherical wrist." diff --git a/README.md b/README.md index 9e211e2..777e831 100644 --- a/README.md +++ b/README.md @@ -62,20 +62,30 @@ let parameters = Parameters { Note that the offset of the third joint is -90 degrees, bringing the joint from the upright position to parallel with the ground at "zero." +# Constraints +Since 1.1.0, it is possible to set constraints for the joints. Robot poses where any of the joints are outside +the specified constraint range are not included into returned list of solutions. It is also possible to +influence the sorting of the result list by giving some preference to the center of constraints. + +Constraints are tested for the range from -2π to 2π, but beware angles repeat with period of 2π so +constraint from -π to π already permits free rotation, covering any angle. + # Example Cargo.toml: ```toml [dependencies] -rs-opw-kinematics = "1.1.0" +rs-opw-kinematics = "1.1.0-rc1" ``` main.rs: ```Rust +use std::f64::consts::PI; use rs_opw_kinematics::kinematic_traits::{Joints, Kinematics, Pose, JOINTS_AT_ZERO}; use rs_opw_kinematics::kinematics_impl::OPWKinematics; use rs_opw_kinematics::parameters::opw_kinematics::Parameters; use rs_opw_kinematics::utils::{dump_joints, dump_solutions}; +use rs_opw_kinematics::constraints::{BY_CONSTRAINS, BY_PREV, Constraints}; fn main() { let robot = OPWKinematics::new(Parameters::irb2400_10()); @@ -103,46 +113,31 @@ fn main() { println!("The solution appears and the needed rotation is now equally distributed between J4 and J6."); let solutions = robot.inverse_continuing(&pose, &JOINTS_AT_ZERO); dump_solutions(&solutions); -} -``` - -```Rust -use rs_opw_kinematics::kinematic_traits::{Joints, Kinematics, Pose, CONSTRAINTS_CENTERED}; -use rs_opw_kinematics::kinematics_impl::OPWKinematics; -use rs_opw_kinematics::parameters::opw_kinematics::Parameters; -use rs_opw_kinematics::utils::{dump_joints, dump_solutions}; - -fn main() { - // TODO this example is unfinished and wrong! - let robot = OPWKinematics::new(Parameters::irb2400_10()); - let joints: Joints = [0.0, 0.1, 0.2, 0.3, 0.0, 0.5]; // Joints are alias of [f64; 6] - - let when_continuing_from: [f64; 6] = [0.0, 0.11, 0.22, 0.3, 0.1, 0.5]; - println!("No constraints"); - let solutions = robot.inverse_continuing(&pose, &when_continuing_from); - dump_solutions(&solutions); + let robot = OPWKinematics::new_with_constraints( + Parameters::irb2400_10(), Constraints::new( + [-0.1, 0.0, 0.0, 0.0, -PI, -PI], + [ PI, PI, 2.0*PI, PI, PI, PI], + BY_PREV, + )); println!("With constraints, sorted by proximity to the previous pose"); - let when_continuing_from_j6_0: [f64; 6] = [0.0, 0.11, 0.22, 0.8, 0.1, 0.0]; let solutions = robot.inverse_continuing(&pose, &when_continuing_from_j6_0); dump_solutions(&solutions); + let robot = OPWKinematics::new_with_constraints( + Parameters::irb2400_10(), Constraints::new( + [-0.1, 0.0, 0.0, 0.0, -PI, -PI], + [ PI, PI, 2.0*PI, PI, PI, PI], + BY_CONSTRAINS, + )); println!("With constraints, sorted by proximity to the center of constraints"); - let when_continuing_from_j6_0: [f64; 6] = [0.0, 0.11, 0.22, 0.8, 0.1, 0.0]; - let solutions = robot.inverse_continuing(&pose, &when_continuing_from_j6_0); - dump_solutions(&solutions); - - println!("With constraints, somewhat intermediate"); - let when_continuing_from_j6_0: [f64; 6] = [0.0, 0.11, 0.22, 0.8, 0.1, 0.0]; let solutions = robot.inverse_continuing(&pose, &when_continuing_from_j6_0); dump_solutions(&solutions); } ``` -# Constraints -Since 1.1.0, it is possible to set constraints for the joints. Robot poses where any of the joints are outside -the specified constraint range are not included into returned list of solutions. It is also possible to -influence the sorting of the result list by giving some preference to the center of constratints. +The constants _BY_PREV_ ( = 0.0) and _BY_CONSTRAINTS_ ( = 1.0) are for convenience only. Intermediate values like +0.6 can also be specified and result in weighted sorting. # Configuring the solver for your robot The project contains built-in definitions for ABB IRB 2400/10, IRB 2600-12/1.65, IRB 4600-60/2.05; KUKA KR 6 R700 sixx, diff --git a/src/kinematics_impl.rs b/src/kinematics_impl.rs index f14319c..8563f50 100644 --- a/src/kinematics_impl.rs +++ b/src/kinematics_impl.rs @@ -521,7 +521,7 @@ impl OPWKinematics { solutions.sort_by(|a, b| { let prev_a; let prev_b; - if sorting_weight == BY_CONSTRAINS { + if sorting_weight != BY_CONSTRAINS { prev_a = calculate_distance(a, previous); prev_b = calculate_distance(b, previous); } else { @@ -533,8 +533,8 @@ impl OPWKinematics { let constr_a = calculate_distance(a, &constraints.centers); let constr_b = calculate_distance(b, &constraints.centers); - let distance_a = prev_a * sorting_weight + constr_a * (1.0 - sorting_weight); - let distance_b = prev_b * sorting_weight + constr_b * (1.0 - sorting_weight); + let distance_a = prev_a * (1.0 - sorting_weight) + constr_a * sorting_weight; + let distance_b = prev_b * (1.0 - sorting_weight) + constr_b * sorting_weight; distance_a.partial_cmp(&distance_b).unwrap_or(std::cmp::Ordering::Equal) }); } diff --git a/src/main.rs b/src/main.rs index 397f233..c9e9c44 100644 --- a/src/main.rs +++ b/src/main.rs @@ -1,3 +1,5 @@ +use std::f64::consts::PI; +use rs_opw_kinematics::constraints::{BY_CONSTRAINS, BY_PREV, Constraints}; use rs_opw_kinematics::kinematic_traits::{Joints, Kinematics, Pose, JOINTS_AT_ZERO}; use rs_opw_kinematics::kinematics_impl::OPWKinematics; use rs_opw_kinematics::parameters::opw_kinematics::Parameters; @@ -29,4 +31,24 @@ fn main() { println!("The solution appears and the needed rotation is now equally distributed between J4 and J6."); let solutions = robot.inverse_continuing(&pose, &JOINTS_AT_ZERO); dump_solutions(&solutions); + + let robot = OPWKinematics::new_with_constraints( + Parameters::irb2400_10(), Constraints::new( + [-0.1, 0.0, 0.0, 0.0, -PI, -PI], + [ PI, PI, 2.0*PI, PI, PI, PI], + BY_PREV, + )); + println!("With constraints, sorted by proximity to the previous pose"); + let solutions = robot.inverse_continuing(&pose, &when_continuing_from_j6_0); + dump_solutions(&solutions); + + let robot = OPWKinematics::new_with_constraints( + Parameters::irb2400_10(), Constraints::new( + [-0.1, 0.0, 0.0, 0.0, -PI, -PI], + [ PI, PI, 2.0*PI, PI, PI, PI], + BY_CONSTRAINS, + )); + println!("With constraints, sorted by proximity to the center of constraints"); + let solutions = robot.inverse_continuing(&pose, &when_continuing_from_j6_0); + dump_solutions(&solutions); } \ No newline at end of file From d48eba63f62545c0886e9a04d6d28e3eed183b0c Mon Sep 17 00:00:00 2001 From: Bourumir Wyngs Date: Thu, 2 May 2024 22:34:47 +0200 Subject: [PATCH 12/21] Preparations for 1.1.0-rc1 --- README.md | 7 ++----- 1 file changed, 2 insertions(+), 5 deletions(-) diff --git a/README.md b/README.md index 777e831..c9feba8 100644 --- a/README.md +++ b/README.md @@ -146,13 +146,10 @@ testing. Robot manufacturers may provide such configurations for the robots they For instance, FANUC M10IA is described [here](https://github.com/ros-industrial/fanuc/blob/3ea2842baca3184cc621071b785cbf0c588a4046/fanuc_m10ia_support/config/opw_parameters_m10ia.yaml). Many other robots are described in [ros-industrial/fanuc](https://github.com/ros-industrial/fanuc) repository. This project contains the code for reading such configurations directly, including support for *deg(angle)* -function that sometimes occurs there. - -Is it possible to read YAML parameter files directly, including parsing of the deg(angle) -function that sometimes occurs there. +function that sometimes occurs there: ```Rust - let parameters = Parameters::from_yaml_file(filename).expect("Failed to load parameters from file"); + let parameters = Parameters::from_yaml_file(filename).expect("Failed to load parameters"); let robot = OPWKinematics::new(parameters); ``` From 3a9880fc03b1ab4e16b2aa4737332f39bcd56ab2 Mon Sep 17 00:00:00 2001 From: Bourumir Wyngs Date: Thu, 2 May 2024 22:46:42 +0200 Subject: [PATCH 13/21] Use non-alpha release of rand. --- Cargo.lock | 37 ++++++++----------------------------- Cargo.toml | 4 +++- 2 files changed, 11 insertions(+), 30 deletions(-) diff --git a/Cargo.lock b/Cargo.lock index 314730b..8e89a01 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -196,20 +196,20 @@ dependencies = [ [[package]] name = "rand" -version = "0.9.0-alpha.1" +version = "0.8.5" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "8d31e63ea85be51c423e52ba8f2e68a3efd53eed30203ee029dd09947333693e" +checksum = "34af8d1a0e25924bc5b7c43c079c942339d8f0a8b57c39049bef581b46327404" dependencies = [ + "libc", "rand_chacha", "rand_core", - "zerocopy", ] [[package]] name = "rand_chacha" -version = "0.9.0-alpha.1" +version = "0.3.1" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "78674ef918c19451dbd250f8201f8619b494f64c9aa6f3adb28fd8a0f1f6da46" +checksum = "e6c10a63a0fa32252be49d21e7709d4d4baf8d231c2dbce1eaa8141b9b127d88" dependencies = [ "ppv-lite86", "rand_core", @@ -217,12 +217,11 @@ dependencies = [ [[package]] name = "rand_core" -version = "0.9.0-alpha.1" +version = "0.6.4" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "cc89dffba8377c5ec847d12bb41492bda235dba31a25e8b695cd0fe6589eb8c9" +checksum = "ec0be4795e2f6a28069bec0b5ff3e2ac9bafc99e6a9a7dc3547996c5c816922c" dependencies = [ "getrandom", - "zerocopy", ] [[package]] @@ -262,7 +261,7 @@ checksum = "adad44e29e4c806119491a7f06f03de4d1af22c3a680dd47f1e6e179439d1f56" [[package]] name = "rs-opw-kinematics" -version = "1.1.0" +version = "1.1.0-rc1" dependencies = [ "nalgebra", "rand", @@ -408,23 +407,3 @@ dependencies = [ "bytemuck", "safe_arch", ] - -[[package]] -name = "zerocopy" -version = "0.8.0-alpha.6" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "db678a6ee512bd06adf35c35be471cae2f9c82a5aed2b5d15e03628c98bddd57" -dependencies = [ - "zerocopy-derive", -] - -[[package]] -name = "zerocopy-derive" -version = "0.8.0-alpha.6" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "201585ea96d37ee69f2ac769925ca57160cef31acb137c16f38b02b76f4c1e62" -dependencies = [ - "proc-macro2", - "quote", - "syn 2.0.57", -] diff --git a/Cargo.toml b/Cargo.toml index 4534972..0e78de5 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -16,6 +16,8 @@ thiserror = "1.0.59" serde = { version = "1.0", features = ["derive"] } serde_yaml = "0.9.34" regex = "1.10.4" -rand = "0.9.0-alpha.1" + +[dev-dependencies] +rand = "0.8.5" From d1d34dd6a894865ed3d788b5adb05db2b6e862ef Mon Sep 17 00:00:00 2001 From: Bourumir Wyngs Date: Thu, 2 May 2024 23:02:26 +0200 Subject: [PATCH 14/21] Small documentation update --- README.md | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/README.md b/README.md index c9feba8..7d7a663 100644 --- a/README.md +++ b/README.md @@ -67,6 +67,11 @@ Since 1.1.0, it is possible to set constraints for the joints. Robot poses where the specified constraint range are not included into returned list of solutions. It is also possible to influence the sorting of the result list by giving some preference to the center of constraints. +Constraints are specified by providing two angles, _from_ and to, for every _joint_. If _from_ < _to_, the valid range +spans between from and to. If _from_ > _to_, the valid range spans over the 0, wrapping arround. For instance, +if _from_ = 5 and _to_ = 15, values 6, 8 and 11 are valid, while values like 90, 180 are not. If _from_ = 15 and +_to_ = 5 (the opposite), values 16, 17, 100, 180, 359, 0, 1, 3, 4 are valid while 6, 8 and 11 are not. + Constraints are tested for the range from -2π to 2π, but beware angles repeat with period of 2π so constraint from -π to π already permits free rotation, covering any angle. From ca1921d84032dcd3bad1d1063c373b54b2799e9a Mon Sep 17 00:00:00 2001 From: Bourumir Wyngs Date: Thu, 2 May 2024 23:03:45 +0200 Subject: [PATCH 15/21] Small documentation update --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index 7d7a663..db54282 100644 --- a/README.md +++ b/README.md @@ -72,7 +72,7 @@ spans between from and to. If _from_ > _to_, the valid range spans over the 0, w if _from_ = 5 and _to_ = 15, values 6, 8 and 11 are valid, while values like 90, 180 are not. If _from_ = 15 and _to_ = 5 (the opposite), values 16, 17, 100, 180, 359, 0, 1, 3, 4 are valid while 6, 8 and 11 are not. -Constraints are tested for the range from -2π to 2π, but beware angles repeat with period of 2π so +Constraints are tested for the range from -2π to 2π, but as angles repeat with period of 2π, the constraint from -π to π already permits free rotation, covering any angle. # Example From ff7eecb39c5c4db60e18ebc2f1eabb91ed0ff291 Mon Sep 17 00:00:00 2001 From: Bourumir Wyngs Date: Fri, 3 May 2024 21:47:30 +0200 Subject: [PATCH 16/21] Small documentation update --- README.md | 1 + 1 file changed, 1 insertion(+) diff --git a/README.md b/README.md index db54282..a0cbb3f 100644 --- a/README.md +++ b/README.md @@ -21,6 +21,7 @@ This paper can be found # Features - rs-opw-kinematics is written entirely in Rust (not a C++ binding) and could be deployed using Cargo. - all returned solutions are valid, normalized, and cross-checked with forward kinematics. +- joint angles can be checked against constraints, returning only compliant solutions. - to generate a trajectory of the robot (sequence of poses), it is possible to use "previous joint positions" as additional input. - if the previous joint positions are provided, the solutions are sorted by proximity to them (closest first) - for kinematic singularity at J5 = 0° or J5 = ±180° positions this solver provides reasonable J4 and J6 From 5b4f84af848adfe13890ccdccc633508467fb59a Mon Sep 17 00:00:00 2001 From: Bourumir Wyngs Date: Fri, 3 May 2024 21:53:50 +0200 Subject: [PATCH 17/21] Small documentation update --- README.md | 15 ++++++++------- 1 file changed, 8 insertions(+), 7 deletions(-) diff --git a/README.md b/README.md index a0cbb3f..68754a9 100644 --- a/README.md +++ b/README.md @@ -19,14 +19,15 @@ This paper can be found [here](https://www.researchgate.net/profile/Mathias-Brandstoetter/publication/264212870_An_Analytical_Solution_of_the_Inverse_Kinematics_Problem_of_Industrial_Serial_Manipulators_with_an_Ortho-parallel_Basis_and_a_Spherical_Wrist/links/53d2417e0cf2a7fbb2e98b09/An-Analytical-Solution-of-the-Inverse-Kinematics-Problem-of-Industrial-Serial-Manipulators-with-an-Ortho-parallel-Basis-and-a-Spherical-Wrist.pdf). # Features -- rs-opw-kinematics is written entirely in Rust (not a C++ binding) and could be deployed using Cargo. -- all returned solutions are valid, normalized, and cross-checked with forward kinematics. -- joint angles can be checked against constraints, returning only compliant solutions. -- to generate a trajectory of the robot (sequence of poses), it is possible to use "previous joint positions" as additional input. -- if the previous joint positions are provided, the solutions are sorted by proximity to them (closest first) -- for kinematic singularity at J5 = 0° or J5 = ±180° positions this solver provides reasonable J4 and J6 +- rs-opw-kinematics is written entirely in Rust (not a C++ binding) and deployable via Cargo. +- All returned solutions are valid, normalized, and cross-checked with forward kinematics. +- Joint angles can be checked against constraints, ensuring only compliant solutions are returned. +- To generate a trajectory of the robot (sequence of poses), it is possible to use "previous joint positions" as additional input. +- If the previous joint positions are provided, the solutions are sorted by proximity to them (closest first). + It is also possible to prioritize proximity to the center of constraints. +- For kinematic singularity at J5 = 0° or J5 = ±180° positions this solver provides reasonable J4 and J6 values close to the previous positions of these joints (and not arbitrary that may result in a large jerk of the real robot) -- use zeros to get the possible solution of singularity case with J4 and J6 close to zero rotation. +- Use zeros to get the possible solution of singularity case with J4 and J6 close to zero rotation. - The solver currently uses 64-bit floats (Rust f64), providing the positional accuracy below 1µm for the two robots tested. From 1d8b5454d29f89e73897329bf522cc96d0816f1d Mon Sep 17 00:00:00 2001 From: Bourumir Wyngs Date: Fri, 3 May 2024 22:03:25 +0200 Subject: [PATCH 18/21] Changing into 1.1.0 version, preparing for release. --- Cargo.lock | 2 +- Cargo.toml | 2 +- README.md | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/Cargo.lock b/Cargo.lock index 8e89a01..b698e5a 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -261,7 +261,7 @@ checksum = "adad44e29e4c806119491a7f06f03de4d1af22c3a680dd47f1e6e179439d1f56" [[package]] name = "rs-opw-kinematics" -version = "1.1.0-rc1" +version = "1.1.0" dependencies = [ "nalgebra", "rand", diff --git a/Cargo.toml b/Cargo.toml index 0e78de5..b8f7975 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -1,6 +1,6 @@ [package] name = "rs-opw-kinematics" -version = "1.1.0-rc1" +version = "1.1.0" edition = "2021" authors = ["Bourumir Wyngs "] description = "Inverse and forward kinematics for 6 axis robots with a parallel base and spherical wrist." diff --git a/README.md b/README.md index 68754a9..7cc3319 100644 --- a/README.md +++ b/README.md @@ -81,7 +81,7 @@ constraint from -π to π already permits free rotation, covering any angle. Cargo.toml: ```toml [dependencies] -rs-opw-kinematics = "1.1.0-rc1" +rs-opw-kinematics = "1.1.0" ``` main.rs: From 514197f0978641f99502db6bffa82159634bfe48 Mon Sep 17 00:00:00 2001 From: Bourumir Wyngs Date: Sat, 4 May 2024 11:28:31 +0200 Subject: [PATCH 19/21] Some improving of documentation workding. --- README.md | 158 ++++++++++++++++++++++++++++-------------------------- 1 file changed, 81 insertions(+), 77 deletions(-) diff --git a/README.md b/README.md index 7cc3319..4a8cb7d 100644 --- a/README.md +++ b/README.md @@ -1,5 +1,6 @@ Rust implementation of inverse and forward kinematic solutions for six-axis industrial robots with a parallel base -and spherical wrist. Hardened against the J5 = 0° or ± 180° singularity and optimized for trajectory planning. +and spherical wrist. Hardened against the J5 = 0° or ± 180° singularity and optimized for trajectory +planning. [![GitHub](https://img.shields.io/badge/GitHub-777777)](https://github.com/bourumir-wyngs/rs-opw-kinematics) [![crates.io](https://img.shields.io/crates/v/rs-opw-kinematics.svg)](https://crates.io/crates/rs-opw-kinematics) @@ -10,30 +11,32 @@ and spherical wrist. Hardened against the J5 = 0° or ± 180° singu # Intro -This work is based on the 2014 year paper `An Analytical Solution of the Inverse Kinematics Problem -of Industrial Serial Manipulators with an Ortho-parallel Basis and a Spherical Wrist` by -Mathias Brandstötter, Arthur Angerer, and Michael Hofbaur. It is also inspired by the similar -C++ project [Jmeyer1292/opw_kinematics](https://github.com/Jmeyer1292/opw_kinematics) (that was used as a reference -implementation to generate data for the test suite; also, this documentation uses the robot diagram from there). -This paper can be found -[here](https://www.researchgate.net/profile/Mathias-Brandstoetter/publication/264212870_An_Analytical_Solution_of_the_Inverse_Kinematics_Problem_of_Industrial_Serial_Manipulators_with_an_Ortho-parallel_Basis_and_a_Spherical_Wrist/links/53d2417e0cf2a7fbb2e98b09/An-Analytical-Solution-of-the-Inverse-Kinematics-Problem-of-Industrial-Serial-Manipulators-with-an-Ortho-parallel-Basis-and-a-Spherical-Wrist.pdf). +This work builds upon the 2014 paper titled _An Analytical Solution of the Inverse Kinematics Problem of Industrial +Serial Manipulators with an Ortho-parallel Basis and a Spherical Wrist_, authored by Mathias Brandstötter, Arthur +Angerer, and Michael Hofbaur. The paper is available [here](https://www.researchgate.net/profile/Mathias-Brandstoetter/publication/264212870_An_Analytical_Solution_of_the_Inverse_Kinematics_Problem_of_Industrial_Serial_Manipulators_with_an_Ortho-parallel_Basis_and_a_Spherical_Wrist/links/53d2417e0cf2a7fbb2e98b09/An-Analytical-Solution-of-the-Inverse-Kinematics-Problem-of-Industrial-Serial-Manipulators-with-an-Ortho-parallel-Basis-and-a-Spherical-Wrist.pdf). Additionally, it draws inspiration from the similar C++ +project, [Jmeyer1292/opw_kinematics](https://github.com/Jmeyer1292/opw_kinematics), which served as a reference implementation for generating data for the test suite. +This documentation also incorporates the robot diagram from that project. # Features + - rs-opw-kinematics is written entirely in Rust (not a C++ binding) and deployable via Cargo. - All returned solutions are valid, normalized, and cross-checked with forward kinematics. - Joint angles can be checked against constraints, ensuring only compliant solutions are returned. -- To generate a trajectory of the robot (sequence of poses), it is possible to use "previous joint positions" as additional input. -- If the previous joint positions are provided, the solutions are sorted by proximity to them (closest first). +- To generate a trajectory of the robot (sequence of poses), it is possible to use "previous joint positions" as + additional input. +- If the previous joint positions are provided, the solutions are sorted by proximity to them (closest first). It is also possible to prioritize proximity to the center of constraints. - For kinematic singularity at J5 = 0° or J5 = ±180° positions this solver provides reasonable J4 and J6 - values close to the previous positions of these joints (and not arbitrary that may result in a large jerk of the real robot) + values close to the previous positions of these joints (and not arbitrary that may result in a large jerk of the real + robot) - Use zeros to get the possible solution of singularity case with J4 and J6 close to zero rotation. -- The solver currently uses 64-bit floats (Rust f64), providing the positional accuracy below 1µm for - the two robots tested. + +The solver currently uses 64-bit floats (Rust f64), providing the positional accuracy below 1µm for the two +robots tested. # Parameters -This library uses seven kinematic parameters (a1, a2, b, c1, c2, c3, and c4). This solver assumes that the arm is +This library uses seven kinematic parameters (_a1, a2, b, c1, c2, c3_, and _c4_). This solver assumes that the arm is at zero when all joints stick straight up in the air, as seen in the image below. It also assumes that all rotations are positive about the base axis of the robot. No other setup is required. @@ -49,36 +52,33 @@ For example, the ABB IRB2400 has the following values: ```Rust let parameters = Parameters { - a1: 0.100, - a2: - 0.135, - b: 0.000, - c1: 0.615, - c2: 0.705, - c3: 0.755, - c4: 0.085, - offsets: [0.0, 0.0, -std::f64::consts::PI / 2.0, 0.0, 0.0, 0.0], - sign_corrections: [1; 6], + a1: 0.100, a2: - 0.135, b: 0.000, c1: 0.615, c2: 0.705, c3: 0.755, c4: 0.085, + offsets: [0.0, 0.0, -std::f64::consts::PI / 2.0, 0.0, 0.0, 0.0], + sign_corrections: [1; 6], } ``` -Note that the offset of the third joint is -90 degrees, bringing the joint from the upright position to parallel with +Note that the offset of the third joint is -90°, bringing the joint from the upright position to parallel with the ground at "zero." # Constraints + Since 1.1.0, it is possible to set constraints for the joints. Robot poses where any of the joints are outside the specified constraint range are not included into returned list of solutions. It is also possible to -influence the sorting of the result list by giving some preference to the center of constraints. +influence the sorting of the result list by giving some preference to the center of constraints. Constraints are specified by providing two angles, _from_ and to, for every _joint_. If _from_ < _to_, the valid range spans between from and to. If _from_ > _to_, the valid range spans over the 0, wrapping arround. For instance, -if _from_ = 5 and _to_ = 15, values 6, 8 and 11 are valid, while values like 90, 180 are not. If _from_ = 15 and +if _from_ = 5 and _to_ = 15, values 6, 8 and 11 are valid, while values like 90, 180 are not. If _from_ = 15 and _to_ = 5 (the opposite), values 16, 17, 100, 180, 359, 0, 1, 3, 4 are valid while 6, 8 and 11 are not. -Constraints are tested for the range from -2π to 2π, but as angles repeat with period of 2π, the +Constraints are tested for the range from -2π to 2π, but as angles repeat with period of 2π, the constraint from -π to π already permits free rotation, covering any angle. # Example + Cargo.toml: + ```toml [dependencies] rs-opw-kinematics = "1.1.0" @@ -95,75 +95,79 @@ use rs_opw_kinematics::utils::{dump_joints, dump_solutions}; use rs_opw_kinematics::constraints::{BY_CONSTRAINS, BY_PREV, Constraints}; fn main() { - let robot = OPWKinematics::new(Parameters::irb2400_10()); - let joints: Joints = [0.0, 0.1, 0.2, 0.3, 0.0, 0.5]; // Joints are alias of [f64; 6] - println!("Initial joints with singularity J5 = 0: "); - dump_joints(&joints); - - println!("Solutions (original angle set is lacking due singularity there: "); - let pose: Pose = robot.forward(&joints); // Pose is alias of nalgebra::Isometry3 - - let solutions = robot.inverse(&pose); // Solutions is alias of Vec - dump_solutions(&solutions); - - println!("Solutions assuming we continue from somewhere close. The 'lost solution' returns"); - let when_continuing_from: [f64; 6] = [0.0, 0.11, 0.22, 0.3, 0.1, 0.5]; - let solutions = robot.inverse_continuing(&pose, &when_continuing_from); - dump_solutions(&solutions); - - println!("Same pose, all J4+J6 rotation assumed to be previously concentrated on J4 only"); - let when_continuing_from_j6_0: [f64; 6] = [0.0, 0.11, 0.22, 0.8, 0.1, 0.0]; - let solutions = robot.inverse_continuing(&pose, &when_continuing_from_j6_0); - dump_solutions(&solutions); - - println!("If we do not have the previous position, we can assume we want J4, J6 close to 0.0"); - println!("The solution appears and the needed rotation is now equally distributed between J4 and J6."); - let solutions = robot.inverse_continuing(&pose, &JOINTS_AT_ZERO); - dump_solutions(&solutions); - - let robot = OPWKinematics::new_with_constraints( - Parameters::irb2400_10(), Constraints::new( - [-0.1, 0.0, 0.0, 0.0, -PI, -PI], - [ PI, PI, 2.0*PI, PI, PI, PI], - BY_PREV, - )); - println!("With constraints, sorted by proximity to the previous pose"); - let solutions = robot.inverse_continuing(&pose, &when_continuing_from_j6_0); - dump_solutions(&solutions); - - let robot = OPWKinematics::new_with_constraints( - Parameters::irb2400_10(), Constraints::new( - [-0.1, 0.0, 0.0, 0.0, -PI, -PI], - [ PI, PI, 2.0*PI, PI, PI, PI], - BY_CONSTRAINS, - )); - println!("With constraints, sorted by proximity to the center of constraints"); - let solutions = robot.inverse_continuing(&pose, &when_continuing_from_j6_0); - dump_solutions(&solutions); + let robot = OPWKinematics::new(Parameters::irb2400_10()); + let joints: Joints = [0.0, 0.1, 0.2, 0.3, 0.0, 0.5]; // Joints are alias of [f64; 6] + println!("Initial joints with singularity J5 = 0: "); + dump_joints(&joints); + + println!("Solutions (original angle set is lacking due singularity there: "); + let pose: Pose = robot.forward(&joints); // Pose is alias of nalgebra::Isometry3 + + let solutions = robot.inverse(&pose); // Solutions is alias of Vec + dump_solutions(&solutions); + + println!("Solutions assuming we continue from somewhere close. The 'lost solution' returns"); + let when_continuing_from: [f64; 6] = [0.0, 0.11, 0.22, 0.3, 0.1, 0.5]; + let solutions = robot.inverse_continuing(&pose, &when_continuing_from); + dump_solutions(&solutions); + + println!("Same pose, all J4+J6 rotation assumed to be previously concentrated on J4 only"); + let when_continuing_from_j6_0: [f64; 6] = [0.0, 0.11, 0.22, 0.8, 0.1, 0.0]; + let solutions = robot.inverse_continuing(&pose, &when_continuing_from_j6_0); + dump_solutions(&solutions); + + println!("If we do not have the previous position, we can assume we want J4, J6 close to 0.0"); + println!("The solution appears and the needed rotation is now equally distributed between J4 and J6."); + let solutions = robot.inverse_continuing(&pose, &JOINTS_AT_ZERO); + dump_solutions(&solutions); + + let robot = OPWKinematics::new_with_constraints( + Parameters::irb2400_10(), Constraints::new( + [-0.1, 0.0, 0.0, 0.0, -PI, -PI], + [PI, PI, 2.0 * PI, PI, PI, PI], + BY_PREV, + )); + println!("With constraints, sorted by proximity to the previous pose"); + let solutions = robot.inverse_continuing(&pose, &when_continuing_from_j6_0); + dump_solutions(&solutions); + + let robot = OPWKinematics::new_with_constraints( + Parameters::irb2400_10(), Constraints::new( + [-0.1, 0.0, 0.0, 0.0, -PI, -PI], + [PI, PI, 2.0 * PI, PI, PI, PI], + BY_CONSTRAINS, + )); + println!("With constraints, sorted by proximity to the center of constraints"); + let solutions = robot.inverse_continuing(&pose, &when_continuing_from_j6_0); + dump_solutions(&solutions); } ``` -The constants _BY_PREV_ ( = 0.0) and _BY_CONSTRAINTS_ ( = 1.0) are for convenience only. Intermediate values like +The constants _BY_PREV_ ( = 0.0) and _BY_CONSTRAINTS_ ( = 1.0) are for convenience only. Intermediate values like 0.6 can also be specified and result in weighted sorting. # Configuring the solver for your robot -The project contains built-in definitions for ABB IRB 2400/10, IRB 2600-12/1.65, IRB 4600-60/2.05; KUKA KR 6 R700 sixx, + +The project contains built-in definitions for ABB IRB 2400/10, IRB 2600-12/1.65, IRB 4600-60/2.05; KUKA KR 6 R700 sixx, FANUC R-2000iB/200R; Stäubli TX40, TX2-140, TX2-160 and TX2-160L with various levels of testing. Robot manufacturers may provide such configurations for the robots they make. -For instance, FANUC M10IA is described [here](https://github.com/ros-industrial/fanuc/blob/3ea2842baca3184cc621071b785cbf0c588a4046/fanuc_m10ia_support/config/opw_parameters_m10ia.yaml). +For instance, FANUC M10IA is +described [here](https://github.com/ros-industrial/fanuc/blob/3ea2842baca3184cc621071b785cbf0c588a4046/fanuc_m10ia_support/config/opw_parameters_m10ia.yaml). Many other robots are described in [ros-industrial/fanuc](https://github.com/ros-industrial/fanuc) repository. This project contains the code for reading such configurations directly, including support for *deg(angle)* function that sometimes occurs there: ```Rust let parameters = Parameters::from_yaml_file(filename).expect("Failed to load parameters"); - let robot = OPWKinematics::new(parameters); +let robot = OPWKinematics::new(parameters); ``` # Testing + The code of this project is tested against the test set (cases.yaml, 2048 cases per robot) that is believed to be correct for the two robots, KUKA KR 6 R700 sixx and ABB IRB 2400/10. It has been produced -using independent C++ implementation by [Jmeyer1292/opw_kinematics](https://github.com/Jmeyer1292/opw_kinematics). The testing suite checks if the solutions +using independent C++ implementation by [Jmeyer1292/opw_kinematics](https://github.com/Jmeyer1292/opw_kinematics). The +testing suite checks if the solutions match. From 175d0934b95845235be50a44ee21e5f616efe493 Mon Sep 17 00:00:00 2001 From: Bourumir Wyngs Date: Sat, 4 May 2024 11:31:19 +0200 Subject: [PATCH 20/21] Some improving of documentation workding. --- README.md | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/README.md b/README.md index 4a8cb7d..578cc8a 100644 --- a/README.md +++ b/README.md @@ -13,7 +13,8 @@ planning. This work builds upon the 2014 paper titled _An Analytical Solution of the Inverse Kinematics Problem of Industrial Serial Manipulators with an Ortho-parallel Basis and a Spherical Wrist_, authored by Mathias Brandstötter, Arthur -Angerer, and Michael Hofbaur. The paper is available [here](https://www.researchgate.net/profile/Mathias-Brandstoetter/publication/264212870_An_Analytical_Solution_of_the_Inverse_Kinematics_Problem_of_Industrial_Serial_Manipulators_with_an_Ortho-parallel_Basis_and_a_Spherical_Wrist/links/53d2417e0cf2a7fbb2e98b09/An-Analytical-Solution-of-the-Inverse-Kinematics-Problem-of-Industrial-Serial-Manipulators-with-an-Ortho-parallel-Basis-and-a-Spherical-Wrist.pdf). Additionally, it draws inspiration from the similar C++ +Angerer, and Michael Hofbaur. The paper is [available in ResearchGate](https://www.researchgate.net/profile/Mathias-Brandstoetter/publication/264212870_An_Analytical_Solution_of_the_Inverse_Kinematics_Problem_of_Industrial_Serial_Manipulators_with_an_Ortho-parallel_Basis_and_a_Spherical_Wrist/links/53d2417e0cf2a7fbb2e98b09/An-Analytical-Solution-of-the-Inverse-Kinematics-Problem-of-Industrial-Serial-Manipulators-with-an-Ortho-parallel-Basis-and-a-Spherical-Wrist.pdf) +. Additionally, it draws inspiration from the similar C++ project, [Jmeyer1292/opw_kinematics](https://github.com/Jmeyer1292/opw_kinematics), which served as a reference implementation for generating data for the test suite. This documentation also incorporates the robot diagram from that project. From 4cf28de67d2ec98f9333354276d41032e3903c25 Mon Sep 17 00:00:00 2001 From: Bourumir Wyngs Date: Sat, 4 May 2024 11:51:05 +0200 Subject: [PATCH 21/21] Some improving of documentation wording. --- README.md | 100 +++++++++++++++++++++++++++++------------------------- 1 file changed, 54 insertions(+), 46 deletions(-) diff --git a/README.md b/README.md index 578cc8a..5d2e6a5 100644 --- a/README.md +++ b/README.md @@ -89,58 +89,66 @@ main.rs: ```Rust use std::f64::consts::PI; -use rs_opw_kinematics::kinematic_traits::{Joints, Kinematics, Pose, JOINTS_AT_ZERO}; +use rs_opw_kinematics::kinematic_traits::{Joints, Kinematics, Pose, + JOINTS_AT_ZERO, CONSTRAINT_CENTERED}; use rs_opw_kinematics::kinematics_impl::OPWKinematics; use rs_opw_kinematics::parameters::opw_kinematics::Parameters; use rs_opw_kinematics::utils::{dump_joints, dump_solutions}; use rs_opw_kinematics::constraints::{BY_CONSTRAINS, BY_PREV, Constraints}; fn main() { - let robot = OPWKinematics::new(Parameters::irb2400_10()); - let joints: Joints = [0.0, 0.1, 0.2, 0.3, 0.0, 0.5]; // Joints are alias of [f64; 6] - println!("Initial joints with singularity J5 = 0: "); - dump_joints(&joints); - - println!("Solutions (original angle set is lacking due singularity there: "); - let pose: Pose = robot.forward(&joints); // Pose is alias of nalgebra::Isometry3 - - let solutions = robot.inverse(&pose); // Solutions is alias of Vec - dump_solutions(&solutions); - - println!("Solutions assuming we continue from somewhere close. The 'lost solution' returns"); - let when_continuing_from: [f64; 6] = [0.0, 0.11, 0.22, 0.3, 0.1, 0.5]; - let solutions = robot.inverse_continuing(&pose, &when_continuing_from); - dump_solutions(&solutions); - - println!("Same pose, all J4+J6 rotation assumed to be previously concentrated on J4 only"); - let when_continuing_from_j6_0: [f64; 6] = [0.0, 0.11, 0.22, 0.8, 0.1, 0.0]; - let solutions = robot.inverse_continuing(&pose, &when_continuing_from_j6_0); - dump_solutions(&solutions); - - println!("If we do not have the previous position, we can assume we want J4, J6 close to 0.0"); - println!("The solution appears and the needed rotation is now equally distributed between J4 and J6."); - let solutions = robot.inverse_continuing(&pose, &JOINTS_AT_ZERO); - dump_solutions(&solutions); - - let robot = OPWKinematics::new_with_constraints( - Parameters::irb2400_10(), Constraints::new( - [-0.1, 0.0, 0.0, 0.0, -PI, -PI], - [PI, PI, 2.0 * PI, PI, PI, PI], - BY_PREV, - )); - println!("With constraints, sorted by proximity to the previous pose"); - let solutions = robot.inverse_continuing(&pose, &when_continuing_from_j6_0); - dump_solutions(&solutions); - - let robot = OPWKinematics::new_with_constraints( - Parameters::irb2400_10(), Constraints::new( - [-0.1, 0.0, 0.0, 0.0, -PI, -PI], - [PI, PI, 2.0 * PI, PI, PI, PI], - BY_CONSTRAINS, - )); - println!("With constraints, sorted by proximity to the center of constraints"); - let solutions = robot.inverse_continuing(&pose, &when_continuing_from_j6_0); - dump_solutions(&solutions); + let robot = OPWKinematics::new(Parameters::irb2400_10()); + let joints: Joints = [0.0, 0.1, 0.2, 0.3, 0.0, 0.5]; // Joints are alias of [f64; 6] + println!("Initial joints with singularity J5 = 0: "); + dump_joints(&joints); + + println!("Solutions (original angle set is lacking due singularity there: "); + let pose: Pose = robot.forward(&joints); // Pose is alias of nalgebra::Isometry3 + + let solutions = robot.inverse(&pose); // Solutions is alias of Vec + dump_solutions(&solutions); + + println!("Solutions assuming we continue from somewhere close. The 'lost solution' returns"); + let when_continuing_from: [f64; 6] = [0.0, 0.11, 0.22, 0.3, 0.1, 0.5]; + let solutions = robot.inverse_continuing(&pose, &when_continuing_from); + dump_solutions(&solutions); + + println!("Same pose, all J4+J6 rotation assumed to be previously concentrated on J4 only"); + let when_continuing_from_j6_0: [f64; 6] = [0.0, 0.11, 0.22, 0.8, 0.1, 0.0]; + let solutions = robot.inverse_continuing(&pose, &when_continuing_from_j6_0); + dump_solutions(&solutions); + + println!("If we do not have the previous position, we can assume we want J4, J6 close to 0.0 \ + The solution appears and the needed rotation is now equally distributed between J4 and J6."); + let solutions = robot.inverse_continuing(&pose, &JOINTS_AT_ZERO); + dump_solutions(&solutions); + + let robot = OPWKinematics::new_with_constraints( + Parameters::irb2400_10(), Constraints::new( + [-0.1, 0.0, 0.0, 0.0, -PI, -PI], + [ PI, PI, 2.0*PI, PI, PI, PI], + BY_PREV, + )); + + println!("If we do not have the previous pose yet, we can now ask to prever the pose \ + closer to the center of constraints."); + let solutions = robot.inverse_continuing(&pose, &CONSTRAINT_CENTERED); + dump_solutions(&solutions); + + + println!("With constraints, sorted by proximity to the previous pose"); + let solutions = robot.inverse_continuing(&pose, &when_continuing_from_j6_0); + dump_solutions(&solutions); + + let robot = OPWKinematics::new_with_constraints( + Parameters::irb2400_10(), Constraints::new( + [-0.1, 0.0, 0.0, 0.0, -PI, -PI], + [ PI, PI, 2.0*PI, PI, PI, PI], + BY_CONSTRAINS, + )); + println!("With constraints, sorted by proximity to the center of constraints"); + let solutions = robot.inverse_continuing(&pose, &when_continuing_from_j6_0); + dump_solutions(&solutions); } ```