diff --git a/Cargo.lock b/Cargo.lock index 4b68bfb..b698e5a 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,36 @@ dependencies = [ "proc-macro2", ] +[[package]] +name = "rand" +version = "0.8.5" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "34af8d1a0e25924bc5b7c43c079c942339d8f0a8b57c39049bef581b46327404" +dependencies = [ + "libc", + "rand_chacha", + "rand_core", +] + +[[package]] +name = "rand_chacha" +version = "0.3.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "e6c10a63a0fa32252be49d21e7709d4d4baf8d231c2dbce1eaa8141b9b127d88" +dependencies = [ + "ppv-lite86", + "rand_core", +] + +[[package]] +name = "rand_core" +version = "0.6.4" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "ec0be4795e2f6a28069bec0b5ff3e2ac9bafc99e6a9a7dc3547996c5c816922c" +dependencies = [ + "getrandom", +] + [[package]] name = "rawpointer" version = "0.2.1" @@ -202,9 +261,10 @@ checksum = "adad44e29e4c806119491a7f06f03de4d1af22c3a680dd47f1e6e179439d1f56" [[package]] name = "rs-opw-kinematics" -version = "1.0.2" +version = "1.1.0" dependencies = [ "nalgebra", + "rand", "regex", "serde", "serde_yaml", @@ -332,6 +392,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" diff --git a/Cargo.toml b/Cargo.toml index d2b9afd..b8f7975 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." @@ -17,4 +17,7 @@ serde = { version = "1.0", features = ["derive"] } serde_yaml = "0.9.34" regex = "1.10.4" +[dev-dependencies] +rand = "0.8.5" + diff --git a/README.md b/README.md index 1ce4593..5d2e6a5 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,28 +11,33 @@ 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 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. # 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. -- 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 - 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. + +- 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. + +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. @@ -47,35 +53,48 @@ 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. + +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 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.0.2" +rs-opw-kinematics = "1.1.0" ``` main.rs: ```Rust -use rs_opw_kinematics::kinematic_traits::{Joints, Kinematics, Pose, JOINTS_AT_ZERO}; +use std::f64::consts::PI; +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()); @@ -99,34 +118,65 @@ fn main() { 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."); + 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); } ``` +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. - -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 robot = OPWKinematics::new(parameters); + let parameters = Parameters::from_yaml_file(filename).expect("Failed to load 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. diff --git a/src/constraints.rs b/src/constraints.rs new file mode 100644 index 0000000..a8d5601 --- /dev/null +++ b/src/constraints.rs @@ -0,0 +1,188 @@ +use std::f64::consts::PI; +use std::f64::INFINITY; +use crate::kinematic_traits::{Joints, JOINTS_AT_ZERO}; + +#[derive(Clone)] +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], + + // 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. + 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; + +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 + /// 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: 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, + 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 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]) + }); + ok + } + + /// 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)) + .cloned() + .collect() + } +} + +#[cfg(test)] +mod tests { + use crate::kinematic_traits::Solutions; + use crate::utils::{as_radians}; + 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]; + 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, BY_CONSTRAINS); + 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, BY_CONSTRAINS); + 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, BY_CONSTRAINS); + 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, BY_CONSTRAINS); + 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, BY_CONSTRAINS); + 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, 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 64ea32e..8563f50 100644 --- a/src/kinematics_impl.rs +++ b/src/kinematics_impl.rs @@ -1,14 +1,18 @@ 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::{BY_CONSTRAINS, BY_PREV, Constraints}; const DEBUG: bool = false; pub struct OPWKinematics { + /// The parameters that were used to construct this solver. parameters: Parameters, + constraints: Option, + unit_z: Unit>, } @@ -19,6 +23,17 @@ impl OPWKinematics { OPWKinematics { parameters, unit_z: Unit::new_normalize(Vector3::z_axis().into_inner()), + 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), } } } @@ -46,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 @@ -312,149 +490,60 @@ impl Kinematics for OPWKinematics { result } - - // Replaces singularity with correct solution - fn inverse_continuing(&self, pose: &Pose, previous: &Joints) -> Solutions { - 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) { - solutions.push(now); - // We only expect one singularity case hence once we found, we can end - break 'shifts; - } - } - - break; - } - } + fn filter_constraints_compliant(&self, solutions: Solutions) -> Solutions { + match &self.constraints { + Some(constraints) => constraints.filter(&solutions), + None => solutions } - // 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]); - } - } - 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, - ); + fn constraints_compliant(&self, solution: Joints) -> bool { + match &self.constraints { + Some(constraints) => constraints.compliant(&solution), + None => true + } + } - let r_oe = r_0c * r_ce; + /// 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 translation = Vector3::new(cx0, cy0, cz0) + p.c4 * r_oe * *self.unit_z; - let rotation = Rotation3::from_matrix_unchecked(r_oe); + let constr_a = calculate_distance(a, &constraints.centers); + let constr_b = calculate_distance(b, &constraints.centers); - Pose::from_parts(Translation3::from(translation), - UnitQuaternion::from_rotation_matrix(&rotation)) + 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) + }); + } } - fn kinematic_singularity(&self, joints: &Joints) -> Option { - if is_close_to_multiple_of_pi(joints[J5], SINGULARITY_ANGLE_THR) { - Some(Singularity::A) - } else { - None - } + /// 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) } } @@ -512,16 +601,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/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; 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 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/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 828a647..285a1ac 100644 --- a/src/tests/mod.rs +++ b/src/tests/mod.rs @@ -1 +1,3 @@ -mod testcases; \ No newline at end of file +mod testcases; +mod constraint_test; +mod constraint_test_various; \ 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 c8a5d46..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) { @@ -35,6 +53,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)] +pub 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;