diff --git a/Cargo.lock b/Cargo.lock index 8548bd1..4f05e92 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -163,6 +163,7 @@ dependencies = [ "nalgebra", "serde", "serde_yaml", + "thiserror", ] [[package]] @@ -248,6 +249,26 @@ dependencies = [ "unicode-ident", ] +[[package]] +name = "thiserror" +version = "1.0.58" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "03468839009160513471e86a034bb2c5c0e4baae3b43f79ffc55c4a5427b3297" +dependencies = [ + "thiserror-impl", +] + +[[package]] +name = "thiserror-impl" +version = "1.0.58" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "c61f3ba182994efc43764a46c018c347bc492c79f024e705f46567b418f6d4f7" +dependencies = [ + "proc-macro2", + "quote", + "syn 2.0.57", +] + [[package]] name = "typenum" version = "1.17.0" diff --git a/Cargo.toml b/Cargo.toml index ab0dcea..fbae963 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -9,8 +9,7 @@ license = "BSD-3-Clause" [dependencies] nalgebra = "0.32.5" - -[dev-dependencies] +thiserror = "1.0.58" serde = { version = "1.0", features = ["derive"] } serde_yaml = "0.9.34" diff --git a/src/lib.rs b/src/lib.rs index de464ed..3178f3b 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -1,5 +1,7 @@ pub mod parameters; pub mod parameters_robots; +pub mod parameters_from_file; + pub mod utils; pub mod kinematic_traits; pub mod kinematics_impl; diff --git a/src/parameters_from_file.rs b/src/parameters_from_file.rs new file mode 100644 index 0000000..d96f913 --- /dev/null +++ b/src/parameters_from_file.rs @@ -0,0 +1,60 @@ +use std::fs::File; +use std::io::Read; +use std::path::Path; +use serde::Deserialize; +use thiserror::Error; +use crate::parameters::opw_kinematics::Parameters; + +/// https://github.com/ros-industrial/fanuc/blob/3ea2842baca3184cc621071b785cbf0c588a4046/fanuc_m16ib_support/config/opw_parameters_m16ib20.yaml +#[derive(Error, Debug)] +pub enum ParametersError { + #[error("Failed to read the robot description file")] + FileReadError(#[from] std::io::Error), + #[error("failed to parse YAML in robot description file")] + YamlParseError(#[from] serde_yaml::Error), +} + +impl Parameters { + 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)?; + let deserialized: YamlParameters = serde_yaml::from_str(&contents)?; + + 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, + }) + } +} + +/// Helper struct for deserialization +#[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, +} + +// Utility function for degree to radian conversion, if needed +fn deg(degree: f64) -> f64 { + degree * std::f64::consts::PI / 180.0 +} diff --git a/src/parameters_robots.rs b/src/parameters_robots.rs index 8cc6850..e6456b1 100644 --- a/src/parameters_robots.rs +++ b/src/parameters_robots.rs @@ -19,7 +19,7 @@ pub mod opw_kinematics { sign_corrections: [1; 6], } } - + pub fn irb2400_10() -> Self { Parameters { a1: 0.100,