diff --git a/.github/workflows/rust.yml b/.github/workflows/rust.yml index 31000a2..6e1c604 100644 --- a/.github/workflows/rust.yml +++ b/.github/workflows/rust.yml @@ -20,3 +20,15 @@ jobs: run: cargo build --verbose - name: Run tests run: cargo test --verbose + - name: Run URDF extractor + run: cargo run src/tests/data/fanuc/lrmate200ib_macro.xacro + - name: Build without features + run: cargo build --no-default-features --verbose + - name: Run tests without features + run: cargo test --no-default-features --verbose + - name: Run URDF extractor once more + run: cargo run + + + + diff --git a/.gitignore b/.gitignore index 19c012f..27288d2 100644 --- a/.gitignore +++ b/.gitignore @@ -1,3 +1,4 @@ /target .idea +/src/tests/data/tmp/ diff --git a/Cargo.lock b/Cargo.lock index 028976f..77291ab 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -14,12 +14,70 @@ dependencies = [ "zerocopy", ] +[[package]] +name = "aho-corasick" +version = "1.1.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "8e60d3430d3a69478ad0993f19238d2df97c507009a52b3c10addcd7f6bcb916" +dependencies = [ + "memchr", +] + [[package]] name = "allocator-api2" version = "0.2.18" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "5c6cb57a04249c6480766f7f7cef5467412af1490f8d1e243141daddada3264f" +[[package]] +name = "anstream" +version = "0.6.14" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "418c75fa768af9c03be99d17643f93f79bbba589895012a80e3452a19ddda15b" +dependencies = [ + "anstyle", + "anstyle-parse", + "anstyle-query", + "anstyle-wincon", + "colorchoice", + "is_terminal_polyfill", + "utf8parse", +] + +[[package]] +name = "anstyle" +version = "1.0.7" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "038dfcf04a5feb68e9c60b21c9625a54c2c0616e79b72b0fd87075a056ae1d1b" + +[[package]] +name = "anstyle-parse" +version = "0.2.4" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "c03a11a9034d92058ceb6ee011ce58af4a9bf61491aa7e1e59ecd24bd40d22d4" +dependencies = [ + "utf8parse", +] + +[[package]] +name = "anstyle-query" +version = "1.0.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "a64c907d4e79225ac72e2a354c9ce84d50ebb4586dee56c82b3ee73004f537f5" +dependencies = [ + "windows-sys", +] + +[[package]] +name = "anstyle-wincon" +version = "3.0.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "61a38449feb7068f52bb06c12759005cf459ee52bb4adc1d5a7c4322d716fb19" +dependencies = [ + "anstyle", + "windows-sys", +] + [[package]] name = "approx" version = "0.5.1" @@ -53,6 +111,52 @@ version = "1.0.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "baf1de4339761588bc0619e3cbc0120ee582ebb74b53b4efbf79117bd2da40fd" +[[package]] +name = "clap" +version = "4.5.4" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "90bc066a67923782aa8515dbaea16946c5bcc5addbd668bb80af688e53e548a0" +dependencies = [ + "clap_builder", + "clap_derive", +] + +[[package]] +name = "clap_builder" +version = "4.5.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "ae129e2e766ae0ec03484e609954119f123cc1fe650337e155d03b022f24f7b4" +dependencies = [ + "anstream", + "anstyle", + "clap_lex", + "strsim", +] + +[[package]] +name = "clap_derive" +version = "4.5.4" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "528131438037fd55894f62d6e9f068b8f45ac57ffa77517819645d10aed04f64" +dependencies = [ + "heck", + "proc-macro2", + "quote", + "syn 2.0.57", +] + +[[package]] +name = "clap_lex" +version = "0.7.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "98cc8fbded0c607b7ba9dd60cd98df59af97e84d24e49c8557331cfc26d301ce" + +[[package]] +name = "colorchoice" +version = "1.0.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "0b6a852b24ab71dffc585bcb46eaf7959d175cb865a7152e35b348d1b2960422" + [[package]] name = "encoding_rs" version = "0.8.34" @@ -64,9 +168,9 @@ dependencies = [ [[package]] name = "getrandom" -version = "0.2.14" +version = "0.2.15" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "94b22e06ecb0110981051723910cbf0b5f5e09a2062dd7663334ee79a9d1286c" +checksum = "c4567c8db10ae91089c99af84c68c38da3ec2f087c3f82960bcdbf3656b6f4d7" dependencies = [ "cfg-if", "libc", @@ -92,11 +196,23 @@ dependencies = [ "hashbrown", ] +[[package]] +name = "heck" +version = "0.5.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "2304e00983f87ffb38b55b444b5e3b60a884b5d30c0fca7d82fe33449bbe55ea" + +[[package]] +name = "is_terminal_polyfill" +version = "1.70.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "f8478577c03552c21db0e2724ffb8986a5ce7af88107e6be5d2ee6e158c12800" + [[package]] name = "libc" -version = "0.2.153" +version = "0.2.154" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "9c198f91728a82281a64e1f4f9eeb25d82cb32a5de251c6bd1b5154d63a8e7bd" +checksum = "ae743338b92ff9146ce83992f766a31066a91a8c84a45e0e9f21e7cf6de6d346" [[package]] name = "matrixmultiply" @@ -108,6 +224,12 @@ dependencies = [ "rawpointer", ] +[[package]] +name = "memchr" +version = "2.7.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "6c8640c5d730cb13ebd907d8d04b52f55ac9a2eec55b440c8892f40d56c76c1d" + [[package]] name = "nalgebra" version = "0.32.5" @@ -185,6 +307,12 @@ version = "1.0.14" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "de3145af08024dea9fa9914f381a17b8fc6034dfb00f3a84013f7ff43f29ed4c" +[[package]] +name = "peresil" +version = "0.3.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "f658886ed52e196e850cfbbfddab9eaa7f6d90dd0929e264c31e5cec07e09e57" + [[package]] name = "ppv-lite86" version = "0.2.17" @@ -245,12 +373,44 @@ version = "0.2.1" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "60a357793950651c4ed0f3f52338f53b2f809f32d83a07f72909fa13e4c6c1e3" +[[package]] +name = "regex" +version = "1.10.4" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "c117dbdfde9c8308975b6a18d71f3f385c89461f7b3fb054288ecf2a2058ba4c" +dependencies = [ + "aho-corasick", + "memchr", + "regex-automata", + "regex-syntax", +] + +[[package]] +name = "regex-automata" +version = "0.4.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "86b83b8b9847f9bf95ef68afb0b8e6cdb80f498442f5179a29fad448fcc1eaea" +dependencies = [ + "aho-corasick", + "memchr", + "regex-syntax", +] + +[[package]] +name = "regex-syntax" +version = "0.8.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "adad44e29e4c806119491a7f06f03de4d1af22c3a680dd47f1e6e179439d1f56" + [[package]] name = "rs-opw-kinematics" -version = "1.1.2" +version = "1.2.0" dependencies = [ + "clap", "nalgebra", "rand", + "regex", + "sxd-document", "yaml-rust2", ] @@ -276,6 +436,22 @@ dependencies = [ "wide", ] +[[package]] +name = "strsim" +version = "0.11.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "7da8b5736845d9f2fcb837ea5d9e2628564b3b043a70948a3f0b778838c5fb4f" + +[[package]] +name = "sxd-document" +version = "0.3.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "94d82f37be9faf1b10a82c4bd492b74f698e40082f0f40de38ab275f31d42078" +dependencies = [ + "peresil", + "typed-arena", +] + [[package]] name = "syn" version = "1.0.109" @@ -298,6 +474,12 @@ dependencies = [ "unicode-ident", ] +[[package]] +name = "typed-arena" +version = "1.7.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "a9b2228007eba4120145f785df0f6c92ea538f5a3635a612ecf4e334c8c1446d" + [[package]] name = "typenum" version = "1.17.0" @@ -310,6 +492,12 @@ version = "1.0.12" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "3354b9ac3fae1ff6755cb6db53683adb661634f67557942dea4facebec0fee4b" +[[package]] +name = "utf8parse" +version = "0.2.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "711b9620af191e0cdc7468a8d14e709c3dcdb115b36f838e601583af800a370a" + [[package]] name = "version_check" version = "0.9.4" @@ -332,6 +520,79 @@ dependencies = [ "safe_arch", ] +[[package]] +name = "windows-sys" +version = "0.52.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "282be5f36a8ce781fad8c8ae18fa3f9beff57ec1b52cb3de0789201425d9a33d" +dependencies = [ + "windows-targets", +] + +[[package]] +name = "windows-targets" +version = "0.52.5" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "6f0713a46559409d202e70e28227288446bf7841d3211583a4b53e3f6d96e7eb" +dependencies = [ + "windows_aarch64_gnullvm", + "windows_aarch64_msvc", + "windows_i686_gnu", + "windows_i686_gnullvm", + "windows_i686_msvc", + "windows_x86_64_gnu", + "windows_x86_64_gnullvm", + "windows_x86_64_msvc", +] + +[[package]] +name = "windows_aarch64_gnullvm" +version = "0.52.5" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "7088eed71e8b8dda258ecc8bac5fb1153c5cffaf2578fc8ff5d61e23578d3263" + +[[package]] +name = "windows_aarch64_msvc" +version = "0.52.5" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "9985fd1504e250c615ca5f281c3f7a6da76213ebd5ccc9561496568a2752afb6" + +[[package]] +name = "windows_i686_gnu" +version = "0.52.5" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "88ba073cf16d5372720ec942a8ccbf61626074c6d4dd2e745299726ce8b89670" + +[[package]] +name = "windows_i686_gnullvm" +version = "0.52.5" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "87f4261229030a858f36b459e748ae97545d6f1ec60e5e0d6a3d32e0dc232ee9" + +[[package]] +name = "windows_i686_msvc" +version = "0.52.5" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "db3c2bf3d13d5b658be73463284eaf12830ac9a26a90c717b7f771dfe97487bf" + +[[package]] +name = "windows_x86_64_gnu" +version = "0.52.5" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "4e4246f76bdeff09eb48875a0fd3e2af6aada79d409d33011886d3e1581517d9" + +[[package]] +name = "windows_x86_64_gnullvm" +version = "0.52.5" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "852298e482cd67c356ddd9570386e2862b5673c85bd5f88df9ab6802b334c596" + +[[package]] +name = "windows_x86_64_msvc" +version = "0.52.5" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "bec47e5bfd1bff0eeaf6d8b485cc1074891a197ab4225d504cb7a1ab88b02bf0" + [[package]] name = "yaml-rust2" version = "0.8.0" diff --git a/Cargo.toml b/Cargo.toml index 264c889..84b3abb 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -1,6 +1,6 @@ [package] name = "rs-opw-kinematics" -version = "1.1.2" +version = "1.2.0" edition = "2021" authors = ["Bourumir Wyngs "] description = "Inverse and forward kinematics for 6 axis robots with a parallel base and spherical wrist." @@ -17,9 +17,20 @@ maintenance = { status = "actively-developed" } [dependencies] nalgebra = "0.32.5" -yaml-rust2 = "0.8.0" + +# Others are only needed to read YAML or convert from URDF +yaml-rust2 = { version = "0.8.0", optional = true } +sxd-document = { version = "0.3", optional = true } +regex = { version = "1.10.4", optional = true } +clap = { version = "4.5.4", features = ["derive"], optional = true } + +[features] +default = ["allow_filesystem"] +allow_filesystem = ["yaml-rust2", "sxd-document", "regex", "clap"] + +# To disable filesystem: +#rs-opw-kinematics = { version = "1.2.0", default-features = false } [dev-dependencies] rand = "0.8.5" - diff --git a/README.md b/README.md index 0e968dc..a86b895 100644 --- a/README.md +++ b/README.md @@ -30,7 +30,7 @@ This documentation also incorporates the robot diagram from that project. - 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. +- Experimental support for parameter extraction from URDF. The solver currently uses 64-bit floats (Rust f64), providing the positional accuracy below 1µm for the two robots tested. @@ -168,9 +168,34 @@ function that sometimes occurs there: ```Rust let parameters = Parameters::from_yaml_file(filename).expect("Failed to load parameters"); -let robot = OPWKinematics::new(parameters); + println!("Reading:\n{}", ¶meters.to_yaml()); + let robot = OPWKinematics::new(parameters); ``` +Since version 1.2.0, parameters and constraints can also be directly extracted from URDF file: +```Rust + let robot = rs_opw_kinematics::urdf::from_urdf_file("/path/to/robot.urdf"); + println!("Reading:\n{}", ¶meters.to_yaml()); +``` + +There is also more advanced function [rs_opw_kinematics::urdf::from_urdf](https://docs.rs/rs-opw-kinematics/1.2.0/rs_opw_kinematics/urdf/fn.from_urdf.html) +that takes URDF string rather than the file, provides error handling and much more control over how the solver +is constructed from the extracted values. + +**Important:** The URDF reader assumes a robot with parallel base and spherical wrist and not an arbitrary robot. +You can easily check this in the robot documentation or simply looking into the drawing. If the robot appears OPW +compliant yet parameters are not extracted correctly, please submit a bug report, providing URDF file and expected +values. In general, always test in simulator before feeding the output of any software to the physical robot. + +# Disabling YAML and URDF readers +For security and performance, some users prefer smaller libraries with less dependencies. If YAML and URDF readers +are not in use, the filesystem access can be completely disabled in your Cargo.toml, importing the library like: + +rs-opw-kinematics = { version = ">=1.2.0, <2.0.0", default-features = false } + +In this case, import of URDF and YAML files will be unaccessible, and used dependencies +will be limited to the single _nalgebra_ crate. + # Testing The code of this project is tested against the test set (cases.yaml, 2048 cases per robot) that is diff --git a/src/constraints.rs b/src/constraints.rs index a8d5601..3b0a376 100644 --- a/src/constraints.rs +++ b/src/constraints.rs @@ -1,8 +1,11 @@ +//! Joint limit support + use std::f64::consts::PI; use std::f64::INFINITY; use crate::kinematic_traits::{Joints, JOINTS_AT_ZERO}; +use crate::utils::deg; -#[derive(Clone)] +#[derive(Clone, Debug)] pub struct Constraints { /// Normalized lower limit. If more than upper limit, the range wraps-around through 0 pub from: [f64; 6], @@ -43,6 +46,18 @@ 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 (centers, tolerances) = Self::compute_centers(from, to); + + Constraints { + from: from, + to: to, + centers: centers, + tolerances: tolerances, + sorting_weight: sorting_weight, + } + } + + fn compute_centers(from: Joints, to: Joints) -> (Joints, Joints) { let mut centers: Joints = JOINTS_AT_ZERO; let mut tolerances: Joints = JOINTS_AT_ZERO; @@ -64,14 +79,17 @@ impl Constraints { tolerances[j_idx] = (b - a) / 2.0; } } + (centers, tolerances) + } - Constraints { - from: from, - to: to, - centers: centers, - tolerances: tolerances, - sorting_weight: sorting_weight, - } + pub fn update_range(& mut self, from: Joints, to: Joints) { + let (centers, tolerances) = Self::compute_centers(from, to); + + self.from = from; + self.to = to; + self.centers = centers; + self.tolerances = tolerances; + // This method does not change the sorting weight. } fn inside_bounds(angle1: f64, angle2: f64, tolerance: f64) -> bool { @@ -103,6 +121,19 @@ impl Constraints { .cloned() .collect() } + + + pub fn to_yaml(&self) -> String { + format!( + "constraints:\n \ + from: [{}]\n \ + to: [{}]\n", + self.from.iter().map(|x| deg(x)) + .collect::>().join(", "), + self.to.iter().map(|x| deg(x)) + .collect::>().join(", ") + ) + } } #[cfg(test)] @@ -120,7 +151,7 @@ mod tests { let limits = Constraints::new(from, to, BY_CONSTRAINS); let sols: Solutions = vec![angles]; - assert!(limits.filter(&sols).len() == 1); + assert_eq!(limits.filter(&sols).len(), 1); assert!(limits.compliant(&angles)); } diff --git a/src/kinematic_traits.rs b/src/kinematic_traits.rs index 3c21e17..f68560d 100644 --- a/src/kinematic_traits.rs +++ b/src/kinematic_traits.rs @@ -1,3 +1,5 @@ +//! Defines traits for direct and inverse kinematics. + extern crate nalgebra as na; use std::f64::NAN; diff --git a/src/kinematics_impl.rs b/src/kinematics_impl.rs index 8563f50..bfc122b 100644 --- a/src/kinematics_impl.rs +++ b/src/kinematics_impl.rs @@ -1,3 +1,5 @@ +//! Provides implementation of inverse and direct kinematics. + use std::f64::{consts::PI}; use crate::kinematic_traits::{Kinematics, Solutions, Pose, Singularity, Joints, JOINTS_AT_ZERO}; use crate::parameters::opw_kinematics::{Parameters}; @@ -8,6 +10,7 @@ use crate::constraints::{BY_CONSTRAINS, BY_PREV, Constraints}; const DEBUG: bool = false; +#[derive(Debug)] pub struct OPWKinematics { /// The parameters that were used to construct this solver. parameters: Parameters, diff --git a/src/lib.rs b/src/lib.rs index 167834c..7b6c8bb 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -1,5 +1,10 @@ +//! Rust implementation of inverse and forward kinematic solutions for six-axis industrial robots +//! with a parallel base and spherical wrist + pub mod parameters; pub mod parameters_robots; + +#[cfg(feature = "allow_filesystem")] pub mod parameters_from_file; pub mod utils; @@ -8,8 +13,19 @@ pub mod kinematics_impl; pub mod constraints; +#[cfg(feature = "allow_filesystem")] +pub mod urdf; +#[cfg(feature = "allow_filesystem")] +pub mod parameter_error; + +#[cfg(feature = "allow_filesystem")] +mod simplify_joint_name; + #[cfg(test)] +#[cfg(feature = "allow_filesystem")] mod tests; + + diff --git a/src/main.rs b/src/main.rs index c9e9c44..1b88e2d 100644 --- a/src/main.rs +++ b/src/main.rs @@ -1,54 +1,66 @@ -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; -use rs_opw_kinematics::utils::{dump_joints, dump_solutions}; +const VERSION: &str = "1.2.0"; +#[cfg(feature = "allow_filesystem")] 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); -} \ No newline at end of file + use clap::{Parser}; + use std::fs::File; + use std::io::{self, Read}; + use rs_opw_kinematics::constraints::BY_PREV; + use rs_opw_kinematics::kinematic_traits::JOINTS_AT_ZERO; + use rs_opw_kinematics::urdf; + + fn print_usage() { + println!("This command line utility extracts OPW parameters \ + \nfor OPW robots from URDF or XACRO files. \ + \n\nIf XACRO file is used, it must contain joint descriptions directly, file \ + \ninclusions are not followed. The function ${{radians(degrees)}} is supported. \ + \nWhile both parameters and constraints are printed out, constraints are not \ + \npart of the OPW parameters. \ + \n\nThis tool is Free software under BSD 3, hosted in repository \ + \nhttps://github.com/bourumir-wyngs/rs-opw-kinematics\n"); + println!("Usage: rs-opw-kinematics urdf_file.urdf"); + } + + fn read_file(file_name: &str) -> io::Result { + let mut file = File::open(file_name)?; + let mut content = String::new(); + file.read_to_string(&mut content)?; + Ok(content) + } + + #[derive(Parser)] + #[command(author, version, about, long_about = None)] + struct Cli { + /// The file to read + file: Option, + } + + println!("rs-opw-kinematics URDF extractor {VERSION} by Bourumir Wyngs.\n"); + + let cli = Cli::parse(); + + if let Some(file_name) = cli.file { + match read_file(&file_name) { + Ok(content) => { + match urdf::from_urdf(content, &None) { + Ok(opw) => { + println!("OPW parameters:\n{}\nJoint limits:\n{}", + &opw.parameters(&JOINTS_AT_ZERO).to_yaml(), + opw.constraints(BY_PREV).to_yaml()); + + }, + Err(e) => println!("Error: {}", e), + } + }, + Err(e) => println!("Error reading file: {}", e), + } + } else { + print_usage(); + } +} + +#[cfg(not(feature = "allow_filesystem"))] +fn main() { + println!("rs-opw-kinematics {VERSION}, CLI not built."); +} + diff --git a/src/parameter_error.rs b/src/parameter_error.rs new file mode 100644 index 0000000..a081d1b --- /dev/null +++ b/src/parameter_error.rs @@ -0,0 +1,45 @@ +//! Error handling for parameter extractors + +use std::io; + +/// Unified error to report failures during both YAML and URDF/XACRO parsing. +#[derive(Debug)] +pub enum ParameterError { + IoError(io::Error), + ParseError(String), + MissingField(String), + WrongAngle(String), + InvalidLength { expected: usize, found: usize }, + XmlProcessingError(String), + ParameterPopulationError(String), + KinematicsConfigurationError(String), +} + +impl std::fmt::Display for ParameterError { + fn fmt(&self, f: &mut std::fmt::Formatter) -> std::fmt::Result { + match *self { + ParameterError::IoError(ref err) => + write!(f, "IO Error: {}", err), + ParameterError::ParseError(ref msg) => + write!(f, "Parse Error: {}", msg), + ParameterError::WrongAngle(ref msg) => + write!(f, "Wrong angle representation: {}", msg), + ParameterError::MissingField(ref field) => + write!(f, "Missing Field: {}", field), + ParameterError::InvalidLength { expected, found } => + write!(f, "Invalid Length: expected {}, found {}", expected, found), + ParameterError::XmlProcessingError(ref err) => + write!(f, "XML Processing Error: {}", err), + ParameterError::ParameterPopulationError(ref err) => + write!(f, "Parameter Population Error: {}", err), + ParameterError::KinematicsConfigurationError(ref err) => + write!(f, "Kinematics Configuration Error: {}", err), + } + } +} + +impl From for ParameterError { + fn from(err: io::Error) -> Self { + ParameterError::IoError(err) + } +} diff --git a/src/parameters.rs b/src/parameters.rs index 966f456..d656927 100644 --- a/src/parameters.rs +++ b/src/parameters.rs @@ -1,4 +1,7 @@ +//! Defines the OPW parameter data structure + pub mod opw_kinematics { + use crate::utils::deg; /// Parameters for the robot. See parameters_robots.rs for examples for concrete robot models. #[derive(Debug, Clone)] @@ -13,4 +16,33 @@ pub mod opw_kinematics { pub offsets: [f64; 6], pub sign_corrections: [i8; 6], } + + impl Parameters { + /// Convert to string yaml representation (quick viewing, etc). + pub fn to_yaml(&self) -> String { + format!( + "opw_kinematics_geometric_parameters:\n \ + a1: {}\n \ + a2: {}\n \ + b: {}\n \ + c1: {}\n \ + c2: {}\n \ + c3: {}\n \ + c4: {}\n\ + opw_kinematics_joint_offsets: [{}]\n\ + opw_kinematics_joint_sign_corrections: [{}]\n", + self.a1, + self.a2, + self.b, + self.c1, + self.c2, + self.c3, + self.c4, + self.offsets.iter().map(|x| deg(x)) + .collect::>().join(","), + self.sign_corrections.iter().map(|x| x.to_string()) + .collect::>().join(",") + ) + } + } } diff --git a/src/parameters_from_file.rs b/src/parameters_from_file.rs index 279c410..2ca45c1 100644 --- a/src/parameters_from_file.rs +++ b/src/parameters_from_file.rs @@ -1,12 +1,14 @@ -use std::fs::File; -use std::io::Read; -use std::path::Path; +//! Supports extracting OPW parameters from YAML file (optional) +use std::{ + fs::File, + io::Read, + path::Path, +}; use yaml_rust2::{Yaml, YamlLoader}; +use crate::parameter_error::ParameterError; use crate::parameters::opw_kinematics::Parameters; -/// See https://github.com/ros-industrial/fanuc/blob/3ea2842baca3184cc621071b785cbf0c588a4046/fanuc_m16ib_support/config/opw_parameters_m16ib20.yaml - impl Parameters { /// /// Read the robot configuration from YAML file. YAML file like this is supported: @@ -25,60 +27,77 @@ impl Parameters { /// /// ROS-Industrial provides many such files for FANUC robots on GitHub /// (ros-industrial/fanuc, see fanuc_m10ia_support/config/opw_parameters_m10ia.yaml) - /// YAML extension to parse the deg(angle) function is supported. - pub fn from_yaml_file>(path: P) -> Result { - let mut file = File::open(path).map_err(|e| e.to_string())?; + /// YAML extension to parse the deg(angle) function is supported. + /// + /// See https://github.com/ros-industrial/fanuc/blob/3ea2842baca3184cc621071b785cbf0c588a4046/fanuc_m16ib_support/config/opw_parameters_m16ib20.yaml + pub fn from_yaml_file>(path: P) -> Result { + let mut file = File::open(path)?; let mut contents = String::new(); - file.read_to_string(&mut contents).map_err(|e| e.to_string())?; - let docs = YamlLoader::load_from_str(&contents).map_err(|e| e.to_string())?; - let doc = &docs[0]; + file.read_to_string(&mut contents)?; - /// We support the deg(angle) function, even if it is not a standard YAML. It is - /// found in the files we need to parse, so what? - fn parse_degrees(s: &str) -> Result { - if s.starts_with("deg(") && s.ends_with(")") { - let len = s.len(); - s[4..len - 1].trim().parse::() - .map_err(|_| format!("Failed to parse degrees from {}", s)) - .map(|deg| deg.to_radians()) - } else { - s.parse::().map_err(|_| format!("Failed to parse deg(x) argument from {}", s)) - } - } + let docs = YamlLoader::load_from_str(&contents).map_err( + |e| ParameterError::ParseError(e.to_string()))?; + let doc = &docs[0]; - let geometric_params = &doc["opw_kinematics_geometric_parameters"]; + let params = &doc["opw_kinematics_geometric_parameters"]; let offsets_yaml = &doc["opw_kinematics_joint_offsets"]; let sign_corrections_yaml = &doc["opw_kinematics_joint_sign_corrections"]; - let offsets: [f64; 6] = offsets_yaml.as_vec().ok_or("Missing offsets array")? + let offsets: [f64; 6] = offsets_yaml.as_vec() + .ok_or_else(|| ParameterError::MissingField("offsets array".into()))? .iter() .map(|item| match item { - Yaml::String(s) if s.starts_with("deg(") => parse_degrees(s), - Yaml::Real(s) | Yaml::String(s) => s.parse::().map_err(|_| "Failed to parse angle".to_string()), - _ => Err("Offset entry is not a number or deg() function".to_string()), + Yaml::String(s) => Self::parse_degrees(s), + Yaml::Real(s) => s.parse::() + .map_err(|_| ParameterError::ParseError("Failed to parse angle".into())), + _ => Err(ParameterError::ParseError( + "Offset entry is not a number or deg() function".into())), }) .collect::, _>>()? .try_into() - .map_err(|_| "Incorrect number of offsets, must be 6".to_string())?; + .map_err(|_| ParameterError::InvalidLength { + expected: 6, + found: offsets_yaml.as_vec() + .unwrap().len(), + })?; - let sign_corrections: [i8; 6] = sign_corrections_yaml.as_vec().ok_or("Missing sign corrections array")? + let sign_corrections: [i8; 6] = sign_corrections_yaml.as_vec() + .ok_or_else(|| ParameterError::MissingField("sign corrections array".into()))? .iter() - .map(|item| item.as_i64().ok_or("Sign correction not an integer".to_string()).map(|x| x as i8)) + .map(|item| item.as_i64().ok_or( + ParameterError::ParseError("Sign correction not an integer".into())) + .map(|x| x as i8)) .collect::, _>>()? .try_into() - .map_err(|_| "Incorrect number of sign corrections, must be 6".to_string())?; + .map_err(|_| ParameterError::InvalidLength { + expected: 6, + found: sign_corrections_yaml.as_vec().unwrap().len(), + })?; Ok(Parameters { - a1: geometric_params["a1"].as_f64().ok_or("Missing field: a1")?, - a2: geometric_params["a2"].as_f64().ok_or("Missing field: a2")?, - b: geometric_params["b"].as_f64().ok_or("Missing field: b")?, - c1: geometric_params["c1"].as_f64().ok_or("Missing field: c1")?, - c2: geometric_params["c2"].as_f64().ok_or("Missing field: c2")?, - c3: geometric_params["c3"].as_f64().ok_or("Missing field: c3")?, - c4: geometric_params["c4"].as_f64().ok_or("Missing field: c4")?, + a1: params["a1"].as_f64().ok_or_else(|| ParameterError::MissingField("a1".into()))?, + a2: params["a2"].as_f64().ok_or_else(|| ParameterError::MissingField("a2".into()))?, + b: params["b"].as_f64().ok_or_else(|| ParameterError::MissingField("b".into()))?, + c1: params["c1"].as_f64().ok_or_else(|| ParameterError::MissingField("c1".into()))?, + c2: params["c2"].as_f64().ok_or_else(|| ParameterError::MissingField("c2".into()))?, + c3: params["c3"].as_f64().ok_or_else(|| ParameterError::MissingField("c3".into()))?, + c4: params["c4"].as_f64().ok_or_else(|| ParameterError::MissingField("c4".into()))?, offsets, sign_corrections, }) } -} + /// Parses angles from strings in degrees format or plain floats. + fn parse_degrees(s: &str) -> Result { + if let Some(angle) = s.strip_prefix("deg(") + .and_then(|s| s.strip_suffix(")")) { + angle.trim().parse::() + .map_err( + |_| ParameterError::ParseError(format!("Failed to parse degrees from {}", s))) + .map(|deg| deg.to_radians()) + } else { + s.parse::().map_err( + |_| ParameterError::ParseError(format!("Failed to parse degrees from {}", s))) + } + } +} diff --git a/src/parameters_robots.rs b/src/parameters_robots.rs index e6456b1..4050c9e 100644 --- a/src/parameters_robots.rs +++ b/src/parameters_robots.rs @@ -1,3 +1,5 @@ +//! Hardcoded OPW parameters for a few robots + pub mod opw_kinematics { use std::f64::consts::PI; use crate::parameters::opw_kinematics::Parameters; diff --git a/src/simplify_joint_name.rs b/src/simplify_joint_name.rs new file mode 100644 index 0000000..73370e8 --- /dev/null +++ b/src/simplify_joint_name.rs @@ -0,0 +1,61 @@ +//! "User friendly" pre-processing of the joint names. + +use regex::Regex; + +/// "User friendly" pre-processing of the joint names. If this does not work as expected, +/// the user should use the advanced function taking the joint names explicitly. +/// +/// Simplify joint name: remove macro construct in the prefix, +/// underscores and non-word characters, set lowercase. +pub fn preprocess_joint_name(joint_name: &str) -> String { + // Create a regex to find the ${prefix} pattern + let re_prefix = Regex::new(r"\$\{[^}]+\}").unwrap(); + // Replace the pattern with an empty string, effectively removing it + let processed_name = re_prefix.replace_all(joint_name, ""); + + // Create a regex to remove all non-alphanumeric characters, including underscores + let re_non_alphanumeric = Regex::new(r"[^\w]|_").unwrap(); + let clean_name = re_non_alphanumeric.replace_all(&processed_name, ""); + + let processed_name = discard_non_digit_joint_chars(clean_name.to_string()); + remove_before_joint(processed_name.to_lowercase()) +} + +fn discard_non_digit_joint_chars(input: String) -> String { + // Define the regular expression + let re = Regex::new(r".*joint(\D*)\d.*").unwrap(); + + // Find the first match of the pattern + if let Some(captures) = re.captures(&input) { + // Get the part marked as \D* which is the first capture group + if let Some(non_digit_part) = captures.get(1) { + let non_digit_str = non_digit_part.as_str(); + return input.replace(non_digit_str, ""); + } + } + input +} + +fn remove_before_joint(s: String) -> String { + // Use a case-insensitive search for the word 'joint' + if let Some(pos) = s.to_lowercase().find("joint") { + // If 'joint' is found, return the substring starting from 'joint' + s[pos..].to_string() + } else { + // If 'joint' is not found, return the original string + s + } +} + +#[cfg(test)] +mod tests { + use super::*; + + #[test] + fn test_name_simplification() { + assert_eq!(preprocess_joint_name("joint1"), "joint1", " simplification incorrect"); + assert_eq!(preprocess_joint_name("JOINT2"), "joint2", "JOINT_2 simplification incorrect"); + assert_eq!(preprocess_joint_name("leftJOINT_2!"), "joint2", "leftJOINT_2! simplification incorrect"); + } +} + diff --git a/src/tests/cases.yaml b/src/tests/data/cases.yaml similarity index 100% rename from src/tests/cases.yaml rename to src/tests/data/cases.yaml diff --git a/src/tests/data/fanuc/LICENSE.txt b/src/tests/data/fanuc/LICENSE.txt new file mode 100644 index 0000000..83fba49 --- /dev/null +++ b/src/tests/data/fanuc/LICENSE.txt @@ -0,0 +1,34 @@ +The files in this folder are taken from https://github.com/ros-industrial/fanuc/blob/melodic-devel + +Software License Agreement (BSD License) + +Copyright (c) 2012-2015, TU Delft Robotics Institute +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions +are met: + + * Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. + * Redistributions in binary form must reproduce the above + copyright notice, this list of conditions and the following + disclaimer in the documentation and/or other materials provided + with the distribution. + * Neither the name of the TU Delft Robotics Institute nor the names + of its contributors may be used to endorse or promote products + derived from this software without specific prior written + permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +POSSIBILITY OF SUCH DAMAGE. diff --git a/src/tests/fanuc_m16ib20.yaml b/src/tests/data/fanuc/fanuc_m16ib20.yaml similarity index 100% rename from src/tests/fanuc_m16ib20.yaml rename to src/tests/data/fanuc/fanuc_m16ib20.yaml diff --git a/src/tests/data/fanuc/lrmate200ib_macro.xacro b/src/tests/data/fanuc/lrmate200ib_macro.xacro new file mode 100644 index 0000000..1c1c5af --- /dev/null +++ b/src/tests/data/fanuc/lrmate200ib_macro.xacro @@ -0,0 +1,181 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/tests/data/fanuc/m10ia_macro.xacro b/src/tests/data/fanuc/m10ia_macro.xacro new file mode 100644 index 0000000..a36c593 --- /dev/null +++ b/src/tests/data/fanuc/m10ia_macro.xacro @@ -0,0 +1,216 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/tests/data/fanuc/m6ib_macro.xacro b/src/tests/data/fanuc/m6ib_macro.xacro new file mode 100644 index 0000000..8168691 --- /dev/null +++ b/src/tests/data/fanuc/m6ib_macro.xacro @@ -0,0 +1,181 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/tests/data/kuka/LICENSE.txt b/src/tests/data/kuka/LICENSE.txt new file mode 100644 index 0000000..673da6a --- /dev/null +++ b/src/tests/data/kuka/LICENSE.txt @@ -0,0 +1,203 @@ +The files in this folder are taken from https://github.com/ros-industrial/kuka_experimental/tree/melodic-devel/kuka_kr6_support + +Apache License + Version 2.0, January 2004 + http://www.apache.org/licenses/ + + TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION + + 1. Definitions. + + "License" shall mean the terms and conditions for use, reproduction, + and distribution as defined by Sections 1 through 9 of this document. + + "Licensor" shall mean the copyright owner or entity authorized by + the copyright owner that is granting the License. + + "Legal Entity" shall mean the union of the acting entity and all + other entities that control, are controlled by, or are under common + control with that entity. For the purposes of this definition, + "control" means (i) the power, direct or indirect, to cause the + direction or management of such entity, whether by contract or + otherwise, or (ii) ownership of fifty percent (50%) or more of the + outstanding shares, or (iii) beneficial ownership of such entity. + + "You" (or "Your") shall mean an individual or Legal Entity + exercising permissions granted by this License. + + "Source" form shall mean the preferred form for making modifications, + including but not limited to software source code, documentation + source, and configuration files. + + "Object" form shall mean any form resulting from mechanical + transformation or translation of a Source form, including but + not limited to compiled object code, generated documentation, + and conversions to other media types. + + "Work" shall mean the work of authorship, whether in Source or + Object form, made available under the License, as indicated by a + copyright notice that is included in or attached to the work + (an example is provided in the Appendix below). + + "Derivative Works" shall mean any work, whether in Source or Object + form, that is based on (or derived from) the Work and for which the + editorial revisions, annotations, elaborations, or other modifications + represent, as a whole, an original work of authorship. For the purposes + of this License, Derivative Works shall not include works that remain + separable from, or merely link (or bind by name) to the interfaces of, + the Work and Derivative Works thereof. + + "Contribution" shall mean any work of authorship, including + the original version of the Work and any modifications or additions + to that Work or Derivative Works thereof, that is intentionally + submitted to Licensor for inclusion in the Work by the copyright owner + or by an individual or Legal Entity authorized to submit on behalf of + the copyright owner. For the purposes of this definition, "submitted" + means any form of electronic, verbal, or written communication sent + to the Licensor or its representatives, including but not limited to + communication on electronic mailing lists, source code control systems, + and issue tracking systems that are managed by, or on behalf of, the + Licensor for the purpose of discussing and improving the Work, but + excluding communication that is conspicuously marked or otherwise + designated in writing by the copyright owner as "Not a Contribution." + + "Contributor" shall mean Licensor and any individual or Legal Entity + on behalf of whom a Contribution has been received by Licensor and + subsequently incorporated within the Work. + + 2. Grant of Copyright License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + copyright license to reproduce, prepare Derivative Works of, + publicly display, publicly perform, sublicense, and distribute the + Work and such Derivative Works in Source or Object form. + + 3. Grant of Patent License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + (except as stated in this section) patent license to make, have made, + use, offer to sell, sell, import, and otherwise transfer the Work, + where such license applies only to those patent claims licensable + by such Contributor that are necessarily infringed by their + Contribution(s) alone or by combination of their Contribution(s) + with the Work to which such Contribution(s) was submitted. If You + institute patent litigation against any entity (including a + cross-claim or counterclaim in a lawsuit) alleging that the Work + or a Contribution incorporated within the Work constitutes direct + or contributory patent infringement, then any patent licenses + granted to You under this License for that Work shall terminate + as of the date such litigation is filed. + + 4. Redistribution. You may reproduce and distribute copies of the + Work or Derivative Works thereof in any medium, with or without + modifications, and in Source or Object form, provided that You + meet the following conditions: + + (a) You must give any other recipients of the Work or + Derivative Works a copy of this License; and + + (b) You must cause any modified files to carry prominent notices + stating that You changed the files; and + + (c) You must retain, in the Source form of any Derivative Works + that You distribute, all copyright, patent, trademark, and + attribution notices from the Source form of the Work, + excluding those notices that do not pertain to any part of + the Derivative Works; and + + (d) If the Work includes a "NOTICE" text file as part of its + distribution, then any Derivative Works that You distribute must + include a readable copy of the attribution notices contained + within such NOTICE file, excluding those notices that do not + pertain to any part of the Derivative Works, in at least one + of the following places: within a NOTICE text file distributed + as part of the Derivative Works; within the Source form or + documentation, if provided along with the Derivative Works; or, + within a display generated by the Derivative Works, if and + wherever such third-party notices normally appear. The contents + of the NOTICE file are for informational purposes only and + do not modify the License. You may add Your own attribution + notices within Derivative Works that You distribute, alongside + or as an addendum to the NOTICE text from the Work, provided + that such additional attribution notices cannot be construed + as modifying the License. + + You may add Your own copyright statement to Your modifications and + may provide additional or different license terms and conditions + for use, reproduction, or distribution of Your modifications, or + for any such Derivative Works as a whole, provided Your use, + reproduction, and distribution of the Work otherwise complies with + the conditions stated in this License. + + 5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. + + 6. Trademarks. This License does not grant permission to use the trade + names, trademarks, service marks, or product names of the Licensor, + except as required for reasonable and customary use in describing the + origin of the Work and reproducing the content of the NOTICE file. + + 7. Disclaimer of Warranty. Unless required by applicable law or + agreed to in writing, Licensor provides the Work (and each + Contributor provides its Contributions) on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or + implied, including, without limitation, any warranties or conditions + of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A + PARTICULAR PURPOSE. You are solely responsible for determining the + appropriateness of using or redistributing the Work and assume any + risks associated with Your exercise of permissions under this License. + + 8. Limitation of Liability. In no event and under no legal theory, + whether in tort (including negligence), contract, or otherwise, + unless required by applicable law (such as deliberate and grossly + negligent acts) or agreed to in writing, shall any Contributor be + liable to You for damages, including any direct, indirect, special, + incidental, or consequential damages of any character arising as a + result of this License or out of the use or inability to use the + Work (including but not limited to damages for loss of goodwill, + work stoppage, computer failure or malfunction, or any and all + other commercial damages or losses), even if such Contributor + has been advised of the possibility of such damages. + + 9. Accepting Warranty or Additional Liability. While redistributing + the Work or Derivative Works thereof, You may choose to offer, + and charge a fee for, acceptance of support, warranty, indemnity, + or other liability obligations and/or rights consistent with this + License. However, in accepting such obligations, You may act only + on Your own behalf and on Your sole responsibility, not on behalf + of any other Contributor, and only if You agree to indemnify, + defend, and hold each Contributor harmless for any liability + incurred by, or claims asserted against, such Contributor by reason + of your accepting any such warranty or additional liability. + + END OF TERMS AND CONDITIONS + + APPENDIX: How to apply the Apache License to your work. + + To apply the Apache License to your work, attach the following + boilerplate notice, with the fields enclosed by brackets "{}" + replaced with your own identifying information. (Don't include + the brackets!) The text should be enclosed in the appropriate + comment syntax for the file format. We also recommend that a + file or class name and description of purpose be included on the + same "printed page" as the copyright notice for easier + identification within third-party archives. + + Copyright {yyyy} {name of copyright owner} + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. \ No newline at end of file diff --git a/src/tests/data/kuka/kr6r700sixx_macro.xacro b/src/tests/data/kuka/kr6r700sixx_macro.xacro new file mode 100644 index 0000000..de0ee04 --- /dev/null +++ b/src/tests/data/kuka/kr6r700sixx_macro.xacro @@ -0,0 +1,179 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/tests/mod.rs b/src/tests/mod.rs index 285a1ac..bb5ba11 100644 --- a/src/tests/mod.rs +++ b/src/tests/mod.rs @@ -1,3 +1,4 @@ mod testcases; mod constraint_test; -mod constraint_test_various; \ No newline at end of file +mod constraint_test_various; +mod urdf_extractor; diff --git a/src/tests/testcases.rs b/src/tests/testcases.rs index 3c998ad..848d677 100644 --- a/src/tests/testcases.rs +++ b/src/tests/testcases.rs @@ -105,8 +105,10 @@ fn parse_array(yaml: &Yaml) -> Resu .ok_or_else(|| "Array is non-empty but no first item found".to_string()) .and_then(|item| { match item { - Yaml::Real(s) => s.parse::().map_err(|e| format!("Failed to parse first item as real: {}, found: '{}'", e, s)), - Yaml::Integer(i) => i.to_string().parse::().map_err(|e| format!("Failed to parse first item as integer: {}, found: '{}'", e, i)), + Yaml::Real(s) => s.parse::() + .map_err(|e| format!("Failed to parse first item as real: {}, found: '{}'", e, s)), + Yaml::Integer(i) => i.to_string().parse::() + .map_err(|e| format!("Failed to parse first item as integer: {}, found: '{}'", e, i)), _ => Err(format!("First item is not a real or integer value, found: {:?}", item)) } })?; @@ -116,8 +118,10 @@ fn parse_array(yaml: &Yaml) -> Resu // Parse each element in the vector and fill the array for (i, item) in vec.iter().enumerate() { array[i] = match item { - Yaml::Real(s) => s.parse::().map_err(|e| format!("Error parsing real at index {}: {}, found: '{}'", i, e, s))?, - Yaml::Integer(i) => i.to_string().parse::().map_err(|e| format!("Error parsing integer at index {}: {}, found: '{}'", i, e, i))?, + Yaml::Real(s) => s.parse::() + .map_err(|e| format!("Error parsing real at index {}: {}, found: '{}'", i, e, s))?, + Yaml::Integer(i) => i.to_string() + .parse::().map_err(|e| format!("Error parsing integer at index {}: {}, found: '{}'", i, e, i))?, _ => return Err(format!("Expected a real or integer value at index {}, found: {:?}", i, item)) }; } @@ -201,7 +205,7 @@ mod tests { #[test] fn test_load_yaml() { - let filename = "src/tests/cases.yaml"; + let filename = "src/tests/data/cases.yaml"; let result = load_yaml(filename); if let Err(e) = &result { @@ -218,7 +222,7 @@ mod tests { #[test] fn test_forward_ik() { - let filename = "src/tests/cases.yaml"; + let filename = "src/tests/data/cases.yaml"; let result = load_yaml(filename); assert!(result.is_ok(), "Failed to load or parse the YAML file: {}", result.unwrap_err()); let cases = result.expect("Expected a valid Cases struct after parsing"); @@ -249,7 +253,7 @@ mod tests { #[test] fn test_inverse_ik() { - let filename = "src/tests/cases.yaml"; + let filename = "src/tests/data/cases.yaml"; let result = load_yaml(filename); assert!(result.is_ok(), "Failed to load or parse the YAML file"); let cases = result.expect("Expected a valid Cases struct after parsing"); @@ -294,7 +298,7 @@ mod tests { #[test] fn test_inverse_ik_continuing() { - let filename = "src/tests/cases.yaml"; + let filename = "src/tests/data/cases.yaml"; let result = load_yaml(filename); assert!(result.is_ok(), "Failed to load or parse the YAML file"); let cases = result.expect("Expected a valid Cases struct after parsing"); @@ -433,7 +437,7 @@ mod tests { #[test] fn test_parameters_from_yaml() { - let filename = "src/tests/fanuc_m16ib20.yaml"; + let filename = "src/tests/data/fanuc/fanuc_m16ib20.yaml"; let loaded = Parameters::from_yaml_file(filename).expect("Failed to load parameters from file"); diff --git a/src/tests/urdf_extractor.rs b/src/tests/urdf_extractor.rs new file mode 100644 index 0000000..b054433 --- /dev/null +++ b/src/tests/urdf_extractor.rs @@ -0,0 +1,134 @@ +use std::fs::read_to_string; +use crate::kinematic_traits::{Joints}; +use crate::parameters::opw_kinematics::Parameters; +use crate::urdf; +use crate::urdf::URDFParameters; + +fn read_urdf(path: &str) -> URDFParameters { + let opw_parameters = urdf::from_urdf(read_to_string(path) + .expect("Failed to read test data file"), &None).expect("Faile to interpret URDF"); + // Output the results or further process + println!("{:?}", opw_parameters); + opw_parameters +} + +#[test] +fn test_extraction_m10ia() { + let opw_parameters= read_urdf("src/tests/data/fanuc/m10ia_macro.xacro"); + + // opw_kinematics_geometric_parameters: + // a1: 0.15 + // a2: -0.20 + // b: 0.0 + // c1: 0.45 + // c2: 0.60 + // c3: 0.64 + // c4: 0.10 + // opw_kinematics_joint_offsets: [0.0, 0.0, deg(-90.0), 0.0, 0.0, deg(180.0)] + // opw_kinematics_joint_sign_corrections: [1, 1, -1, -1, -1, -1] + + assert_eq!(opw_parameters.a1, 0.15, "a1 parameter mismatch"); + assert_eq!(opw_parameters.a2, -0.2, "a2 parameter mismatch"); + assert_eq!(opw_parameters.b, 0.0, "b parameter mismatch"); + assert_eq!(opw_parameters.c1, 0.45, "c1 parameter mismatch"); + assert_eq!(opw_parameters.c2, 0.6, "c2 parameter mismatch"); + assert_eq!(opw_parameters.c3, 0.64, "c3 parameter mismatch"); + assert_eq!(opw_parameters.c4, 0.1, "c4 parameter mismatch"); + + let expected_sign_corrections: [i32; 6] = [1, 1, -1, -1, -1, -1]; + let expected_from: Joints = [-3.14, -1.57, -3.14, -3.31, -3.31, -6.28]; + let expected_to: Joints = [3.14, 2.79, 4.61, 3.31, 3.31, 6.28]; + + for (i, &val) in expected_sign_corrections.iter().enumerate() { + assert_eq!(opw_parameters.sign_corrections[i], val as i8, + "Mismatch in sign_corrections at index {}", i); + } + + for (i, &val) in expected_from.iter().enumerate() { + assert_eq!(opw_parameters.from[i], val, "Mismatch in from at index {}", i); + } + + for (i, &val) in expected_to.iter().enumerate() { + assert_eq!(opw_parameters.to[i], val, "Mismatch in to at index {}", i); + } +} +#[test] +fn test_extraction_lrmate200ib() { + let opw_parameters= read_urdf("src/tests/data/fanuc/lrmate200ib_macro.xacro"); + + // opw_kinematics_geometric_parameters: + // a1: 0.15 + // a2: -0.075 + // b: 0.0 + // c1: 0.35 + // c2: 0.25 + // c3: 0.290 + // c4: 0.08 + // opw_kinematics_joint_offsets: [0.0, 0.0, deg(-90.0), 0.0, 0.0, deg(180.0)] + // opw_kinematics_joint_sign_corrections: [1, 1, -1, -1, -1, -1] + + assert_eq!(opw_parameters.a1, 0.15, "a1 parameter mismatch"); + assert_eq!(opw_parameters.a2, -0.075, "a2 parameter mismatch"); + assert_eq!(opw_parameters.b, 0.0, "b parameter mismatch"); + assert_eq!(opw_parameters.c1, 0.35, "c1 parameter mismatch"); + assert_eq!(opw_parameters.c2, 0.25, "c2 parameter mismatch"); + assert_eq!(opw_parameters.c3, 0.290, "c3 parameter mismatch"); + assert_eq!(opw_parameters.c4, 0.08, "c4 parameter mismatch"); + + let expected_sign_corrections: [i32; 6] = [1, 1, -1, -1, -1, -1]; + let expected_from: Joints = [-2.7925, -0.5759, -2.6145, -3.3161, -2.0943, -6.2831]; + let expected_to: Joints = [2.7925, 2.6529, 2.8797, 3.3161, 2.0943, 6.2831]; + + for (i, &val) in expected_sign_corrections.iter().enumerate() { + assert_eq!(opw_parameters.sign_corrections[i], val as i8, + "Mismatch in sign_corrections at index {}", i); + } + + for (i, &val) in expected_from.iter().enumerate() { + assert_eq!(opw_parameters.from[i], val, "Mismatch in constraints from at index {}", i); + } + + for (i, &val) in expected_to.iter().enumerate() { + assert_eq!(opw_parameters.to[i], val, "Mismatch in constraints to at index {}", i); + } +} + +#[test] +fn test_extraction_m6ib() { + let opw_parameters= read_urdf("src/tests/data/fanuc/m6ib_macro.xacro"); + + // opw_kinematics_geometric_parameters: + // a1: 0.15 + // a2: -0.10 + // b: 0.0 + // c1: 0.45 + // c2: 0.600 + // c3: 0.615 + // c4: 0.10 + + assert_eq!(opw_parameters.a1, 0.15, "a1 parameter mismatch"); + assert_eq!(opw_parameters.a2, -0.10, "a2 parameter mismatch"); + assert_eq!(opw_parameters.b, 0.0, "b parameter mismatch"); + assert_eq!(opw_parameters.c1, 0.45, "c1 parameter mismatch"); + assert_eq!(opw_parameters.c2, 0.6, "c2 parameter mismatch"); + assert_eq!(opw_parameters.c3, 0.615, "c3 parameter mismatch"); + assert_eq!(opw_parameters.c4, 0.10, "c4 parameter mismatch"); +} + +#[test] +fn test_extraction_kr6r700sixx() { + let opw_parameters= + read_urdf("src/tests/data/kuka/kr6r700sixx_macro.xacro"); + + let params = Parameters::kuka_kr6_r700_sixx(); + + assert_eq!(opw_parameters.a1, params.a1, "a1 parameter mismatch"); + assert_eq!(opw_parameters.a2, params.a2, "a2 parameter mismatch"); + assert_eq!(opw_parameters.b, params.b, "b parameter mismatch"); + assert_eq!(opw_parameters.c1, params.c1, "c1 parameter mismatch"); + assert_eq!(opw_parameters.c2, params.c2, "c2 parameter mismatch"); + assert_eq!(opw_parameters.c3, params.c3, "c3 parameter mismatch"); + assert_eq!(opw_parameters.c4, params.c4, "c4 parameter mismatch"); +} + + diff --git a/src/urdf.rs b/src/urdf.rs new file mode 100644 index 0000000..96e95d0 --- /dev/null +++ b/src/urdf.rs @@ -0,0 +1,621 @@ +//! Supports extracting OPW parameters from URDF (optional) + +extern crate sxd_document; + +use crate::simplify_joint_name::preprocess_joint_name; +use std::collections::HashMap; +use sxd_document::{parser, dom, QName}; +use std::error::Error; +use std::fs::read_to_string; +use std::path::Path; +use regex::Regex; +use crate::constraints::{BY_PREV, Constraints}; +use crate::kinematic_traits::{Joints, JOINTS_AT_ZERO}; +use crate::kinematics_impl::OPWKinematics; +use crate::parameter_error::ParameterError; +use crate::parameters::opw_kinematics::Parameters; + +/// Simplified reading from URDF file. This function assumes sorting of results by closest to +/// previous (BY_PREV) and no joint offsets (zero offsets). URDF file is expected in the input +/// but XACRO file may also work. Robot joints must be named joint1 to joint6 in the file +/// (as macro prefix, underscore and non-word chars is dropped, it can also be something like +/// `${prefix}JOINT_1`). It may be more than one robot described in URDF file but they must all +/// be identical. +/// +/// # Parameters +/// - `path`: the location of URDF or XACRO file to load from. +/// +/// # Returns +/// - Returns an instance of `OPWKinematics`, which contains the kinematic parameters +/// extracted from the specified URDF file, including constraints as defined there. +/// +/// # Example +/// ``` +/// let kinematics = rs_opw_kinematics::urdf::from_urdf_file("src/tests/data/fanuc/m6ib_macro.xacro"); +/// println!("{:?}", kinematics); +/// ``` +/// +/// # Errors +/// - The function might panic if the file cannot be found, is not accessible, or is incorrectly +/// formatted. Users should ensure the file path is correct and the file is properly formatted as +/// URDF or XACRO file. +pub fn from_urdf_file>(path: P) -> OPWKinematics { + let xml_content = read_to_string(path).expect("Failed to read xacro/urdf file"); + + let joint_data = process_joints(&xml_content, &None) + .expect("Failed to process XML joints"); + + let opw_parameters = populate_opw_parameters(joint_data, &None) + .expect("Failed to read OpwParameters"); + + opw_parameters.to_robot(BY_PREV, &JOINTS_AT_ZERO) +} + +/// Parses URDF XML content to construct OPW kinematics parameters for a robot. +/// This function provides detailed error handling through the `ParameterError` type, +/// and returns intermediate type from where both Parameters and Constraints can be taken +/// and inspected or modified if so required. +/// +/// # Parameters +/// - `xml_content`: A `String` containing the XML data of the URDF file. +/// - `joint_names`: An optional array containing joint names. This may be required if +/// names do not follow typical naming convention, or there are multiple +/// robots defined in URDF. +/// +/// # Returns +/// - Returns a `Result`. On success, it contains the OPW kinematics +/// configuration for the robot. On failure, it returns a detailed error. +/// Ust to_robot method to convert OpwParameters directly to the robot instance. +/// +/// # Example showing full control over how the inverse kinematics solver is constructed: +/// ``` +/// use std::f64::consts::PI; +/// use rs_opw_kinematics::constraints::BY_PREV; +/// use rs_opw_kinematics::kinematic_traits::{Joints, JOINTS_AT_ZERO, Kinematics}; +/// use rs_opw_kinematics::kinematics_impl::OPWKinematics; +/// use rs_opw_kinematics::urdf::from_urdf; +/// // Exactly this string would fail. Working URDF fragment would be too long for this example. +/// let xml_data = String::from(""); +/// +/// // Let's assume the joint names have prefix and the joints are zero base numbered. +/// let joints = ["lf_joint_0", "lf_joint_1", "lf_joint_2", "lf_joint_3", "lf_joint_4", "lf_joint_5"]; +/// let offsets = [ 0., PI, 0., 0.,0.,0.]; +/// let opw_params = from_urdf(xml_data, &Some(joints)); +/// match opw_params { +/// Ok(opw_params) => { +/// println!("Building the IK solver {:?}", opw_params); +/// let parameters = opw_params.parameters(&JOINTS_AT_ZERO); // Zero joint offsets +/// let constraints =opw_params.constraints(BY_PREV); +/// let robot = OPWKinematics::new_with_constraints(parameters, constraints); +/// // let joints = robot.inverse( ... ) +/// +/// } +/// Err(e) => println!("Error processing URDF: {}", e), +/// } +/// ``` +pub fn from_urdf(xml_content: String, joint_names: &Option<[&str; 6]>) -> Result { + let joint_data = process_joints(&xml_content, joint_names) + .map_err(|e| + ParameterError::XmlProcessingError(format!("Failed to process XML joints: {}", e)))?; + + let opw_parameters = populate_opw_parameters(joint_data, joint_names) + .map_err(|e| + ParameterError::ParameterPopulationError(format!("Failed to interpret robot model: {}", e)))?; + + Ok(opw_parameters) +} + + +#[derive(Debug, Default, PartialEq)] +struct Vector3 { + x: f64, + y: f64, + z: f64, +} + +impl Vector3 { + pub fn non_zero(&self) -> Result { + let mut non_zero_values = vec![]; + + if self.x != 0.0 { + non_zero_values.push(self.x); + } + if self.y != 0.0 { + non_zero_values.push(self.y); + } + if self.z != 0.0 { + non_zero_values.push(self.z); + } + + match non_zero_values.len() { + 0 => Ok(0.0), + 1 => Ok(non_zero_values[0]), + _ => Err(format!("More than one non-zero value in URDF offset {:?}", self).to_string()), + } + } +} + +#[derive(Debug, PartialEq)] +struct JointData { + name: String, + vector: Vector3, + sign_correction: i32, + from: f64, + to: f64, +} + +fn process_joints(xml: &str, joint_names: &Option<[&str; 6]>) -> Result, Box> { + let package = parser::parse(xml)?; + let document = package.as_document(); + + // Access the root element + let root_element = document.root().children().into_iter() + .find_map(|e| e.element()) + .ok_or("No root element found")?; + + // Collect all joint data + let mut joints = Vec::new(); + collect_joints(root_element, &mut joints, joint_names)?; + + convert_to_map(joints) +} + +// Recursive function to collect joint data +fn collect_joints(element: dom::Element, joints: &mut Vec, joint_names: &Option<[&str; 6]>) -> Result<(), Box> { + let origin_tag = QName::new("origin"); + let joint_tag = QName::new("joint"); + let axis_tag = QName::new("axis"); + let limit_tag = QName::new("limit"); + + for child in element.children().into_iter().filter_map(|e| e.element()) { + if child.name() == joint_tag { + let name; + let urdf_name = &child.attribute("name") + .map(|attr| attr.value().to_string()) + .unwrap_or_else(|| "Unnamed".to_string()); + if joint_names.is_some() { + // If joint names are explicitly given, they are expected to be as they are. + name = urdf_name.clone(); + } else { + // Otherwise effort is done to "simplify" the names into joint1 to joint6 + name = preprocess_joint_name(urdf_name); + } + let axis_element = child.children().into_iter() + .find_map(|e| e.element().filter(|el| el.name() == axis_tag)); + let origin_element = child.children().into_iter() + .find_map(|e| e.element().filter(|el| el.name() == origin_tag)); + let limit_element = child.children().into_iter() + .find_map(|e| e.element().filter(|el| el.name() == limit_tag)); + + let mut joint_data = JointData { + name, + vector: origin_element.map_or_else(|| Ok(Vector3::default()), get_xyz_from_origin)?, + sign_correction: axis_element.map_or(Ok(1), get_axis_sign)?, + from: 0., + to: 0., // 0 to 0 in our notation is 'full circle' + }; + + match limit_element.map(get_limits).transpose() { + Ok(Some((from, to))) => { + joint_data.from = from; + joint_data.to = to; + } + Ok(None) => {} + Err(e) => { + println!("Joint limits defined but not not readable for {}: {}", + joint_data.name, e.to_string()); + } + } + + joints.push(joint_data); + } + + collect_joints(child, joints, joint_names)?; + } + + Ok(()) +} + + +fn get_xyz_from_origin(element: dom::Element) -> Result> { + let xyz_attr = element.attribute("xyz").ok_or("xyz attribute not found")?; + let coords: Vec = xyz_attr.value().split_whitespace() + .map(str::parse) + .collect::>()?; + + if coords.len() != 3 { + return Err("XYZ attribute does not contain exactly three values".into()); + } + + Ok(Vector3 { + x: coords[0], + y: coords[1], + z: coords[2], + }) +} + +fn get_axis_sign(axis_element: dom::Element) -> Result> { + // Fetch the 'xyz' attribute, assuming the element is correctly passed + let axis_attr = axis_element.attribute("xyz").ok_or_else(|| { + "'xyz' attribute not found in element supposed to represent the axis" + })?; + + // Parse the xyz attribute to determine the sign corrections + let axis_values: Vec = axis_attr.value().split_whitespace() + .map(str::parse) + .collect::>()?; + + // Filter and count non-zero values, ensuring exactly one non-zero which must be -1 or 1 + let non_zero_values: Vec = axis_values.iter() + .filter(|&&v| v != 0.0) + .map(|&v| if v < 0.0 { -1 } else { 1 }) + .collect(); + + // Check that exactly one non-zero value exists and it is either -1 or 1 + if non_zero_values.len() == 1 && (non_zero_values[0] == -1 || non_zero_values[0] == 1) { + Ok(non_zero_values[0]) + } else { + Err("Invalid axis direction, must have exactly one non-zero value \ + that is either -1 or 1".into()) + } +} + +fn parse_angle(attr_value: &str) -> Result { + // Regular expression to match the ${radians()} format that is common in xacro + let re = Regex::new(r"^\$\{radians\((-?\d+(\.\d+)?)\)\}$") + .map_err(|_| ParameterError::ParseError("Invalid regex pattern".to_string()))?; + + // Check if the input matches the special format + if let Some(caps) = re.captures(attr_value) { + let degrees_str = caps.get(1) + .ok_or(ParameterError::WrongAngle(format!("Bad representation: {}", + attr_value).to_string()))?.as_str(); + let degrees: f64 = degrees_str.parse() + .map_err(|_| ParameterError::WrongAngle(attr_value.to_string()))?; + Ok(degrees.to_radians()) + } else { + // Try to parse the input as a plain number in that case it is in radians + let radians: f64 = attr_value.parse() + .map_err(|_| ParameterError::WrongAngle(attr_value.to_string()))?; + Ok(radians) + } +} + +fn get_limits(element: dom::Element) -> Result<(f64, f64), ParameterError> { + let lower_attr = element.attribute("lower") + .ok_or_else(|| ParameterError::MissingField("lower limit not found".into()))? + .value(); + let lower_limit = parse_angle(lower_attr)?; + + let upper_attr = element.attribute("upper") + .ok_or_else(|| ParameterError::MissingField("upper limit not found".into()))? + .value(); + let upper_limit = parse_angle(upper_attr)?; + + Ok((lower_limit, upper_limit)) +} + +fn convert_to_map(joints: Vec) -> Result, Box> { + let mut map: HashMap = HashMap::new(); + + for joint in joints { + if let Some(existing) = map.get(&joint.name) { + // Check if the existing entry is different from the new one + if existing != &joint { + return Err(Box::new(std::io::Error::new(std::io::ErrorKind::InvalidData, + format!("Duplicate joint name with different data found: {}", joint.name)))); + } + } else { + map.insert(joint.name.clone(), joint); + } + } + + Ok(map) +} + +/// OPW parameters as extrancted from URDF file, including constraints +/// (joint offsets are not directly defined in URDF). This structure +/// can provide robot parameters, constraints and sign corrections, +/// or alterntively can be converted to the robot directly. +#[derive(Default, Debug, Clone, Copy)] +pub struct URDFParameters { + pub a1: f64, + pub a2: f64, + pub b: f64, + pub c1: f64, + pub c2: f64, + pub c3: f64, + pub c4: f64, + pub sign_corrections: [i8; 6], + pub from: Joints, // Array to store the lower limits + pub to: Joints, // Array to store the upper limits +} + +impl URDFParameters { + pub fn to_robot(self, sorting_weight: f64, offsets: &Joints) -> OPWKinematics { + OPWKinematics::new_with_constraints( + Parameters { + a1: self.a1, + a2: self.a2, + b: self.b, + c1: self.c1, + c2: self.c2, + c3: self.c3, + c4: self.c4, + sign_corrections: self.sign_corrections, + offsets: *offsets, + }, + Constraints::new( + self.from, + self.to, + sorting_weight, + ), + ) + } + + /// Return extracted constraints. + pub fn constraints(self, sorting_weight: f64) -> Constraints { + Constraints::new( + self.from, + self.to, + sorting_weight, + ) + } + + /// Return extracted parameters + pub fn parameters(self, offsets: &Joints) -> Parameters { + Parameters { + a1: self.a1, + a2: self.a2, + b: self.b, + c1: self.c1, + c2: self.c2, + c3: self.c3, + c4: self.c4, + sign_corrections: self.sign_corrections, + offsets: *offsets, + } + } +} + +fn populate_opw_parameters(joint_map: HashMap, joint_names: &Option<[&str; 6]>) + -> Result { + let mut opw_parameters = URDFParameters::default(); + + let names = joint_names.unwrap_or_else( + || ["joint1", "joint2", "joint3", "joint4", "joint5", "joint6"]); + + for j in 0..6 { + let joint = joint_map + .get(names[j]).ok_or_else(|| format!("Joint {} not found: {}", j, names[j]))?; + + opw_parameters.sign_corrections[j] = joint.sign_correction as i8; + opw_parameters.from[j] = joint.from; + opw_parameters.to[j] = joint.to; + + match j + 1 { // Joint number 1 to 6 inclusive + 1 => { + opw_parameters.c1 = joint.vector.non_zero()?; + } + 2 => { + opw_parameters.a1 = joint.vector.non_zero()?; + } + 3 => { + // There is more divergence here. + match joint.vector.non_zero() { + Ok(value) => { + // If there is only one value, it is value for c2. Most of the + // modern robots we tested do follow this design. + opw_parameters.c2 = value; + opw_parameters.b = 0.0; + } + Err(_err) => { + pub fn non_zero(a: f64, b: f64) -> Result { + match (a, b) { + (0.0, 0.0) => Ok(0.0), // assuming c2 = 0.0 + (0.0, b) => Ok(b), + (a, 0.0) => Ok(a), + (_, _) => Err(String::from("Both potential c2 values are non-zero")), + } + } + // If there are multiple values, we assume b is given here as y. + // c2 is given either in z or in x, other being 0. + opw_parameters.c2 = non_zero(joint.vector.x, joint.vector.z)?; + opw_parameters.b = joint.vector.y; + } + } + } + 4 => { + opw_parameters.a2 = -joint.vector.non_zero()?; + } + 5 => { + opw_parameters.c3 = joint.vector.non_zero()?; + } + 6 => { + opw_parameters.c4 = joint.vector.non_zero()?; + } + _ => { + // Other joints not in use + } + } + } + + Ok(opw_parameters) +} + +#[allow(dead_code)] +// This function is not in use and exists for references only (old version) +fn populate_opw_parameters_explicit(joint_map: HashMap, joint_names: &Option<[&str; 6]>) + -> Result { + let mut opw_parameters = URDFParameters::default(); + + opw_parameters.b = 0.0; // We only support robots with b = 0 so far. + + let names = joint_names.unwrap_or_else( + || ["joint1", "joint2", "joint3", "joint4", "joint5", "joint6"]); + + for j in 0..6 { + let joint = joint_map + .get(names[j]).ok_or_else(|| format!("Joint {} not found: {}", j, names[j]))?; + + opw_parameters.sign_corrections[j] = joint.sign_correction as i8; + opw_parameters.from[j] = joint.from; + opw_parameters.to[j] = joint.to; + + match j + 1 { // Joint number 1 to 6 inclusive + 1 => { + opw_parameters.c1 = joint.vector.z; + } + 2 => { + opw_parameters.a1 = joint.vector.x; + } + 3 => { + opw_parameters.c2 = joint.vector.z; + opw_parameters.b = joint.vector.y; + //opw_parameters.c2 = joint.vector.x; // Kuka + } + 4 => { + opw_parameters.a2 = -joint.vector.z; + } + 5 => { + opw_parameters.c3 = joint.vector.x; + opw_parameters.c3 = joint.vector.z; // TX40 + } + 6 => { + opw_parameters.c4 = joint.vector.x; + opw_parameters.c4 = joint.vector.z; // TX40 + } + _ => { + // Other joints not in use + } + } + } + + Ok(opw_parameters) +} + + +#[cfg(test)] +mod tests { + use super::*; + + #[test] + fn test_process_joints() { + let xml = r#" + + + + + + + + + + + + + "#; + + let joint_data = process_joints(xml, &None) + .expect("Failed to process XML joints"); + + assert_eq!(joint_data.len(), 2, "Should have extracted two joints"); + + let j1 = &joint_data["joint1"]; + let j2 = &joint_data["joint2"]; + + // Tests for joint1 + assert_eq!(j1.name, "joint1", "Joint1 name incorrect"); + assert_eq!(j1.vector.x, 1.0, "Joint1 X incorrect"); + assert_eq!(j1.vector.y, 2.0, "Joint1 Y incorrect"); + assert_eq!(j1.vector.z, 3.0, "Joint1 Z incorrect"); + assert_eq!(j1.sign_correction, -1, "Joint1 sign correction incorrect"); + assert_eq!(j1.from, -3.14, "Joint1 lower limit incorrect"); + assert_eq!(j1.to, 4.61, "Joint1 upper limit incorrect"); + + // Tests for joint2 + assert_eq!(j2.name, "joint2", "Joint2 name incorrect"); + assert_eq!(j2.vector.x, 4.0, "Joint2 X incorrect"); + assert_eq!(j2.vector.y, 5.0, "Joint2 Y incorrect"); + assert_eq!(j2.vector.z, 6.0, "Joint2 Z incorrect"); + assert_eq!(j2.sign_correction, 1, "Joint2 sign correction incorrect"); + assert_eq!(j2.from, -3.15, "Joint2 lower limit incorrect"); + assert_eq!(j2.to, 4.62, "Joint2 upper limit incorrect"); + } + + #[test] + fn test_populate_named_joints() { + // Let's say they are zero-base numbered and have the side prefix in front. + // Also, there are joints for another robot with different prefix (multiple robots in URDF). + // This test shows that multiple robots are supported (if URDF defines a multi-robot cell) + let xml = r#" + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + "#; + + let joints = ["left_joint_0", "left_joint_1", "left_joint_2", + "left_joint_3", "left_joint_4", "left_joint_5"]; + + let opw_parameters = + from_urdf(xml.to_string(), &Some(joints)).expect("Failed to parse parameters"); + + assert_eq!(opw_parameters.a1, 0.15, "a1 parameter mismatch"); + assert_eq!(opw_parameters.a2, -0.10, "a2 parameter mismatch"); + assert_eq!(opw_parameters.b, 0.0, "b parameter mismatch"); + assert_eq!(opw_parameters.c1, 0.45, "c1 parameter mismatch"); + assert_eq!(opw_parameters.c2, 0.6, "c2 parameter mismatch"); + assert_eq!(opw_parameters.c3, 0.615, "c3 parameter mismatch"); + assert_eq!(opw_parameters.c4, 0.10, "c4 parameter mismatch"); + } +} + + diff --git a/src/utils.rs b/src/utils.rs index abc3fb1..1c8b744 100644 --- a/src/utils.rs +++ b/src/utils.rs @@ -1,3 +1,5 @@ +//! Helper functions + use crate::kinematic_traits::{Joints, Solutions}; /// Checks the solution for validity. This is only internally needed as all returned @@ -59,6 +61,14 @@ pub fn as_radians(degrees: [i32; 6]) -> Joints { std::array::from_fn(|i| (degrees[i] as f64).to_radians()) } +/// formatting for YAML output +pub(crate) fn deg(x: &f64) -> String { + if *x == 0.0 { + return "0".to_string(); + } + format!("deg({:.4})", x.to_degrees()) +} + #[cfg(test)] mod tests { use std::f64::consts::PI;