Release 1.1.0 (#3)

Release 1.1.0. Constraint support and documentation updates.

Cargo.toml

@@ -1,6 +1,6 @@
 [package]
 name = "rs-opw-kinematics"
-version = "1.0.2"
+version = "1.1.0"
 edition = "2021"
 authors = ["Bourumir Wyngs <bourumir.wyngs@gmail.com>"]
 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"
+
 

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&pi; to 2&pi;, but as angles repeat with period of 2&pi;, the
+constraint from -&pi; to &pi; 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.