From c1858ce6da3d2621d2dae74b42338d1638fe31ba Mon Sep 17 00:00:00 2001 From: Bourumir <127052462+bourumir-wyngs@users.noreply.github.com> Date: Wed, 8 May 2024 20:23:29 +0200 Subject: [PATCH] Switching from serdes-yaml to yaml-rust2 (#4) * Release 1.1.1 - switching from serdes-yaml to yaml-rust2. Also serdes is now no longer required. * Release 1.1.1 - switching from serdes-yaml to yaml-rust2. Also serdes is now no longer required. --- Cargo.lock | 158 ++++++++++++++++++------------------ Cargo.toml | 9 +- src/parameters_from_file.rs | 113 ++++++++++++-------------- src/tests/testcases.rs | 126 ++++++++++++++++++++++------ 4 files changed, 232 insertions(+), 174 deletions(-) diff --git a/Cargo.lock b/Cargo.lock index b698e5a..daab672 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -2,6 +2,18 @@ # It is not intended for manual editing. version = 3 +[[package]] +name = "ahash" +version = "0.8.11" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "e89da841a80418a9b391ebaea17f5c112ffaaa96f621d2c285b5174da76b9011" +dependencies = [ + "cfg-if", + "once_cell", + "version_check", + "zerocopy", +] + [[package]] name = "aho-corasick" version = "1.1.3" @@ -11,6 +23,12 @@ dependencies = [ "memchr", ] +[[package]] +name = "allocator-api2" +version = "0.2.18" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "5c6cb57a04249c6480766f7f7cef5467412af1490f8d1e243141daddada3264f" + [[package]] name = "approx" version = "0.5.1" @@ -20,6 +38,12 @@ dependencies = [ "num-traits", ] +[[package]] +name = "arraydeque" +version = "0.5.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "7d902e3d592a523def97af8f317b08ce16b7ab854c1985a0c671e6f15cebc236" + [[package]] name = "autocfg" version = "1.2.0" @@ -39,10 +63,13 @@ source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "baf1de4339761588bc0619e3cbc0120ee582ebb74b53b4efbf79117bd2da40fd" [[package]] -name = "equivalent" -version = "1.0.1" +name = "encoding_rs" +version = "0.8.34" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "5443807d6dff69373d433ab9ef5378ad8df50ca6298caf15de6e52e24aaf54d5" +checksum = "b45de904aa0b010bce2ab45264d0631681847fa7b6f2eaa7dab7619943bc4f59" +dependencies = [ + "cfg-if", +] [[package]] name = "getrandom" @@ -60,23 +87,20 @@ name = "hashbrown" version = "0.14.3" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "290f1a1d9242c78d09ce40a5e87e7554ee637af1351968159f4952f028f75604" +dependencies = [ + "ahash", + "allocator-api2", +] [[package]] -name = "indexmap" -version = "2.2.6" +name = "hashlink" +version = "0.8.4" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "168fb715dda47215e360912c096649d23d58bf392ac62f73919e831745e40f26" +checksum = "e8094feaf31ff591f651a2664fb9cfd92bba7a60ce3197265e9482ebe753c8f7" dependencies = [ - "equivalent", "hashbrown", ] -[[package]] -name = "itoa" -version = "1.0.11" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "49f1f14873335454500d59611f1cf4a4b0f786f9ac11f4312a78e4cf2566695b" - [[package]] name = "libc" version = "0.2.153" @@ -164,6 +188,12 @@ dependencies = [ "autocfg", ] +[[package]] +name = "once_cell" +version = "1.19.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "3fdb12b2476b595f9358c5161aa467c2438859caa136dec86c26fdd2efe17b92" + [[package]] name = "paste" version = "1.0.14" @@ -261,22 +291,14 @@ checksum = "adad44e29e4c806119491a7f06f03de4d1af22c3a680dd47f1e6e179439d1f56" [[package]] name = "rs-opw-kinematics" -version = "1.1.0" +version = "1.1.1" dependencies = [ "nalgebra", "rand", "regex", - "serde", - "serde_yaml", - "thiserror", + "yaml-rust2", ] -[[package]] -name = "ryu" -version = "1.0.17" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "e86697c916019a8588c99b5fac3cead74ec0b4b819707a682fd4d23fa0ce1ba1" - [[package]] name = "safe_arch" version = "0.7.1" @@ -286,39 +308,6 @@ dependencies = [ "bytemuck", ] -[[package]] -name = "serde" -version = "1.0.197" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "3fb1c873e1b9b056a4dc4c0c198b24c3ffa059243875552b2bd0933b1aee4ce2" -dependencies = [ - "serde_derive", -] - -[[package]] -name = "serde_derive" -version = "1.0.197" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "7eb0b34b42edc17f6b7cac84a52a1c5f0e1bb2227e997ca9011ea3dd34e8610b" -dependencies = [ - "proc-macro2", - "quote", - "syn 2.0.57", -] - -[[package]] -name = "serde_yaml" -version = "0.9.34+deprecated" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "6a8b1a1a2ebf674015cc02edccce75287f1a0130d394307b36743c2f5d504b47" -dependencies = [ - "indexmap", - "itoa", - "ryu", - "serde", - "unsafe-libyaml", -] - [[package]] name = "simba" version = "0.8.1" @@ -354,26 +343,6 @@ dependencies = [ "unicode-ident", ] -[[package]] -name = "thiserror" -version = "1.0.59" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "f0126ad08bff79f29fc3ae6a55cc72352056dfff61e3ff8bb7129476d44b23aa" -dependencies = [ - "thiserror-impl", -] - -[[package]] -name = "thiserror-impl" -version = "1.0.59" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "d1cd413b5d558b4c5bf3680e324a6fa5014e7b7c067a51e69dbdf47eb7148b66" -dependencies = [ - "proc-macro2", - "quote", - "syn 2.0.57", -] - [[package]] name = "typenum" version = "1.17.0" @@ -387,10 +356,10 @@ source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "3354b9ac3fae1ff6755cb6db53683adb661634f67557942dea4facebec0fee4b" [[package]] -name = "unsafe-libyaml" -version = "0.2.11" +name = "version_check" +version = "0.9.4" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "673aac59facbab8a9007c7f6108d11f63b603f7cabff99fabf650fea5c32b861" +checksum = "49874b5167b65d7193b8aba1567f5c7d93d001cafc34600cee003eda787e483f" [[package]] name = "wasi" @@ -407,3 +376,34 @@ dependencies = [ "bytemuck", "safe_arch", ] + +[[package]] +name = "yaml-rust2" +version = "0.8.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "498f4d102a79ea1c9d4dd27573c0fc96ad74c023e8da38484e47883076da25fb" +dependencies = [ + "arraydeque", + "encoding_rs", + "hashlink", +] + +[[package]] +name = "zerocopy" +version = "0.7.34" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "ae87e3fcd617500e5d106f0380cf7b77f3c6092aae37191433159dda23cfb087" +dependencies = [ + "zerocopy-derive", +] + +[[package]] +name = "zerocopy-derive" +version = "0.7.34" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "15e934569e47891f7d9411f1a451d947a60e000ab3bd24fbb970f000387d1b3b" +dependencies = [ + "proc-macro2", + "quote", + "syn 2.0.57", +] diff --git a/Cargo.toml b/Cargo.toml index 951e825..4807a71 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -1,6 +1,6 @@ [package] name = "rs-opw-kinematics" -version = "1.1.0" +version = "1.1.1" edition = "2021" authors = ["Bourumir Wyngs "] description = "Inverse and forward kinematics for 6 axis robots with a parallel base and spherical wrist." @@ -17,13 +17,8 @@ maintenance = { status = "actively-developed" } [dependencies] nalgebra = "0.32.5" -thiserror = "1.0.59" -serde = { version = "1.0", features = ["derive"] } +yaml-rust2 = "0.8.0" - -# Bourumir's own branch currently without any major changes so we can use the old one as well -#serde_yaml = { git = "https://github.com/bourumir-wyngs/serde-yaml-bw", branch = "1.0.0" } -serde_yaml = "0.9.34" regex = "1.10.4" [dev-dependencies] diff --git a/src/parameters_from_file.rs b/src/parameters_from_file.rs index f44e01e..279c410 100644 --- a/src/parameters_from_file.rs +++ b/src/parameters_from_file.rs @@ -1,28 +1,16 @@ use std::fs::File; use std::io::Read; use std::path::Path; -use regex::Regex; -use serde::Deserialize; -use thiserror::Error; + +use yaml_rust2::{Yaml, YamlLoader}; use crate::parameters::opw_kinematics::Parameters; -/// https://github.com/ros-industrial/fanuc/blob/3ea2842baca3184cc621071b785cbf0c588a4046/fanuc_m16ib_support/config/opw_parameters_m16ib20.yaml -/// Defines the parameters loading error -#[derive(Error, Debug)] -pub enum ParametersError { - #[error("failed to read robot definition file")] - FileReadError(#[from] std::io::Error), - #[error("failed to parse YAML in the robot definition file")] - YamlParseError(#[from] serde_yaml::Error), - #[error("failed to process YAML content")] - YamlProcessError(#[from] regex::Error), -} +/// 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: - /// + /// /// # FANUC m16ib20 /// opw_kinematics_geometric_parameters: /// a1: 0.15 @@ -36,58 +24,61 @@ impl Parameters { /// opw_kinematics_joint_sign_corrections: [1, 1, -1, -1, -1, -1] /// /// ROS-Industrial provides many such files for FANUC robots on GitHub - /// (ros-industrial/fanuc, see fanuc_m10ia_support/config/opw_parameters_m10ia.yaml + /// (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)?; + pub fn from_yaml_file>(path: P) -> Result { + let mut file = File::open(path).map_err(|e| e.to_string())?; let mut contents = String::new(); - file.read_to_string(&mut contents)?; + 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]; + + /// 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 processed_contents = preprocess_yaml_contents(&contents)?; + let geometric_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 deserialized: YamlParameters = serde_yaml::from_str(&processed_contents)?; + let offsets: [f64; 6] = offsets_yaml.as_vec().ok_or("Missing offsets array")? + .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()), + }) + .collect::, _>>()? + .try_into() + .map_err(|_| "Incorrect number of offsets, must be 6".to_string())?; + + let sign_corrections: [i8; 6] = sign_corrections_yaml.as_vec().ok_or("Missing sign corrections array")? + .iter() + .map(|item| item.as_i64().ok_or("Sign correction not an integer".to_string()).map(|x| x as i8)) + .collect::, _>>()? + .try_into() + .map_err(|_| "Incorrect number of sign corrections, must be 6".to_string())?; Ok(Parameters { - a1: deserialized.opw_kinematics_geometric_parameters.a1, - a2: deserialized.opw_kinematics_geometric_parameters.a2, - b: deserialized.opw_kinematics_geometric_parameters.b, - c1: deserialized.opw_kinematics_geometric_parameters.c1, - c2: deserialized.opw_kinematics_geometric_parameters.c2, - c3: deserialized.opw_kinematics_geometric_parameters.c3, - c4: deserialized.opw_kinematics_geometric_parameters.c4, - offsets: deserialized.opw_kinematics_joint_offsets, - sign_corrections: deserialized.opw_kinematics_joint_sign_corrections, + 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")?, + offsets, + sign_corrections, }) } } -fn preprocess_yaml_contents(contents: &str) -> Result { - let re = Regex::new(r"deg\(([^)]+)\)")?; - let processed_contents = re.replace_all(contents, |caps: ®ex::Captures| { - format!("{}", deg(caps[1].parse::().unwrap())) - }).to_string(); - - Ok(processed_contents) -} - -fn deg(degree: f64) -> f64 { - degree * std::f64::consts::PI / 180.0 -} - -#[derive(Debug, Deserialize)] -struct YamlParameters { - opw_kinematics_geometric_parameters: GeometricParameters, - opw_kinematics_joint_offsets: [f64; 6], - opw_kinematics_joint_sign_corrections: [i8; 6], -} - -#[derive(Debug, Deserialize)] -struct GeometricParameters { - a1: f64, - a2: f64, - b: f64, - c1: f64, - c2: f64, - c3: f64, - c4: f64, -} diff --git a/src/tests/testcases.rs b/src/tests/testcases.rs index 6cf903a..3c998ad 100644 --- a/src/tests/testcases.rs +++ b/src/tests/testcases.rs @@ -1,20 +1,18 @@ -use serde::{Deserialize, Serialize}; use std::fs::File; use std::io::Read; -use serde_yaml; -#[derive(Debug, Serialize, Deserialize)] +#[derive(Debug)] struct Pose { translation: [f64; 3], quaternion: [f64; 4], // Assuming [x, y, z, w] ordering here } -#[derive(Debug, Serialize, Deserialize)] +#[derive(Debug)] struct Case { id: i32, parameters: String, joints: [f64; 6], - solutions: Vec<[f64; 6]>, + _solutions: Vec<[f64; 6]>, pose: Pose, } @@ -29,12 +27,8 @@ impl Case { } } -#[derive(Debug, Serialize, Deserialize)] -struct Cases { - cases: Vec, -} - use nalgebra::{Isometry3, Quaternion, Translation3, UnitQuaternion}; +use yaml_rust2::{Yaml, YamlLoader}; impl Pose { pub fn to_isometry(&self) -> Isometry3 { @@ -64,11 +58,89 @@ impl Pose { } } -fn load_yaml(filename: &str) -> Result { - let mut file = File::open(filename).expect("Unable to open file"); +fn load_yaml(file_path: &str) -> Result, String> { + let mut file = File::open(file_path).map_err(|e| e.to_string())?; let mut contents = String::new(); - file.read_to_string(&mut contents).expect("Unable to read the file"); - serde_yaml::from_str(&contents) + 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 cases_yaml = &docs[0]["cases"]; + + let mut cases: Vec = Vec::new(); + if let Some(cases_vec) = cases_yaml.as_vec() { + for case_yaml in cases_vec { + let id = case_yaml["id"].as_i64().ok_or("Missing id")? as i32; + let parameters = case_yaml["parameters"].as_str().ok_or("Missing parameters")?.to_string(); + let joints = parse_array::(&case_yaml["joints"])?; + let solutions = parse_solutions(&case_yaml["solutions"])?; + let pose = parse_pose(&case_yaml["pose"])?; + + cases.push(Case { + id, + parameters, + joints, + _solutions: solutions, + pose, + }); + } + } else { + return Err("Expected 'cases' to be a sequence".to_string()); + } + + Ok(cases) +} + +fn parse_array(yaml: &Yaml) -> Result<[T; N], String> + where + ::Err: std::fmt::Display, // Ensure the error type of T's FromStr can be displayed +{ + let vec = yaml.as_vec().ok_or("Expected an array in YAML")?; + + if vec.is_empty() { + return Err("Array in YAML is empty, cannot initialize".to_string()); + } + + // Initialize the array using the first element if it can be parsed, otherwise return an error + let first_value = vec.get(0) + .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)), + _ => Err(format!("First item is not a real or integer value, found: {:?}", item)) + } + })?; + + let mut array: [T; N] = [first_value; N]; // Use the first parsed value to initialize the array + + // 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))?, + _ => return Err(format!("Expected a real or integer value at index {}, found: {:?}", i, item)) + }; + } + + Ok(array) +} + +fn parse_solutions(yaml: &Yaml) -> Result, String> { + let mut solutions = Vec::new(); + for solution_yaml in yaml.as_vec().ok_or("Expected solutions array")? { + solutions.push(parse_array::(solution_yaml)?); + } + Ok(solutions) +} + +fn parse_pose(yaml: &Yaml) -> Result { + let translation = parse_array::(&yaml["translation"])?; + let quaternion = parse_array::(&yaml["quaternion"])?; + + Ok(Pose { + translation, + quaternion, + }) } fn are_isometries_approx_equal(a: &Isometry3, b: &Isometry3, tolerance: f64) -> bool { @@ -125,7 +197,7 @@ mod tests { println!("No matching solution found"); return None; // Explicitly indicate that no matching column was found - } + } #[test] fn test_load_yaml() { @@ -138,22 +210,22 @@ mod tests { assert!(result.is_ok(), "Failed to load or parse the YAML file"); - let cases_struct = result.expect("Expected a valid Cases struct after parsing"); + let cases = result.expect("Expected a valid Cases struct after parsing"); // Example assertion: the list of cases should not be empty. - assert!(!cases_struct.cases.is_empty(), "No cases were loaded from the YAML file"); + assert!(!cases.is_empty(), "No cases were loaded from the YAML file"); } #[test] fn test_forward_ik() { let filename = "src/tests/cases.yaml"; let result = load_yaml(filename); - assert!(result.is_ok(), "Failed to load or parse the YAML file"); + 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"); let all_parameters = create_parameter_map(); - println!("Forward IK: {} test cases", cases.cases.len()); + println!("Forward IK: {} test cases", cases.len()); - for case in cases.cases.iter() { + for case in cases.iter() { let parameters = all_parameters.get(&case.parameters).unwrap_or_else(|| { panic!("Parameters for the robot [{}] are unknown", &case.parameters) }); @@ -182,9 +254,9 @@ mod tests { assert!(result.is_ok(), "Failed to load or parse the YAML file"); let cases = result.expect("Expected a valid Cases struct after parsing"); let all_parameters = create_parameter_map(); - println!("Inverse IK: {} test cases", cases.cases.len()); + println!("Inverse IK: {} test cases", cases.len()); - for case in cases.cases.iter() { + for case in cases.iter() { let parameters = all_parameters.get(&case.parameters).unwrap_or_else(|| { panic!("Parameters for the robot [{}] are unknown", &case.parameters) }); @@ -227,9 +299,9 @@ mod tests { assert!(result.is_ok(), "Failed to load or parse the YAML file"); let cases = result.expect("Expected a valid Cases struct after parsing"); let all_parameters = create_parameter_map(); - println!("Inverse IK: {} test cases", cases.cases.len()); + println!("Inverse IK: {} test cases", cases.len()); - for case in cases.cases.iter() { + for case in cases.iter() { if case.id != 1241 { //continue; } @@ -243,7 +315,7 @@ mod tests { found_joints_approx_equal(&solutions, &case.joints_in_radians(), 0.001_f64.to_radians()); if !matches!(found_matching, Some(0)) { - println!("**** No valid solution: {:?} for case {} on {} ****", + println!("**** No valid solution: {:?} for case {} on {} ****", found_matching, case.id, case.parameters); let joints_str = &case.joints.iter() .map(|&val| format!("{:5.2}", val)) @@ -264,8 +336,8 @@ mod tests { println!("---"); } assert!(matches!(found_matching, Some(0)), - "Fully matching joints must come first. At {}, Expected Some(0), got {:?}", - case.id,found_matching); + "Fully matching joints must come first. At {}, Expected Some(0), got {:?}", + case.id, found_matching); } }