Work on importing robot parameters

from file

Cargo.toml

@@ -12,5 +12,6 @@ nalgebra = "0.32.5"
 thiserror = "1.0.58"
 serde = { version = "1.0", features = ["derive"] }
 serde_yaml = "0.9.34"
+regex = "1.10.4"
 
 

README.md

@@ -60,9 +60,12 @@ let parameters = Parameters {
 Note that the offset of the third joint is -90 degrees, bringing the joint from the upright position to parallel with
 the ground at "zero".
 
-The project already contains definitions for ABB IRB 2400/10, KUKA KR 6 R700 sixx, Fanuc R-2000iB/200R, Stäubli TX40,
+The project already contains definitions for ABB IRB 2400/10, KUKA KR 6 R700 sixx, FANUC R-2000iB/200R, Stäubli TX40,
 ABB IRB 2600-12/1.65, ABB IRB 4600-60/2.05, Stäubli TX2-140, Stäubli TX2-160, Stäubli TX2-160L with various level of
-testing. 
+testing. Many such configurations are provided by robot manufacturers for they robots.
+For instance, FANUC M10IA is described [here](https://github.com/ros-industrial/fanuc/blob/3ea2842baca3184cc621071b785cbf0c588a4046/fanuc_m10ia_support/config/opw_parameters_m10ia.yaml).
+Many other robots are described in [ros-industrial/fanuc](https://github.com/ros-industrial/fanuc) repository.
+We are working in possibility of reading these configuration files directly.
 
 # Example
 Cargo.toml:
@@ -106,7 +109,6 @@ fn main() {
   let solutions = robot.inverse_continuing(&pose, &JOINTS_AT_ZERO);
   dump_solutions(&solutions);
 }
-
 ```
 
 # Testing

src/parameters_from_file.rs

@@ -1,25 +1,51 @@
 use std::fs::File;
 use std::io::Read;
 use std::path::Path;
+use regex::Regex;
 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
+/// Defines the parameters loading error
 #[derive(Error, Debug)]
 pub enum ParametersError {
-    #[error("Failed to read the robot description file")]
+    #[error("failed to read robot definition file")]
     FileReadError(#[from] std::io::Error),
-    #[error("failed to parse YAML in robot description file")]
+    #[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),
 }
 
 impl Parameters {
+
+    ///
+    /// Read the robot configuration from YAML file. YAML file like this is supported:
+    /// 
+    /// # FANUC m16ib20
+    /// opw_kinematics_geometric_parameters:
+    ///   a1: 0.15
+    ///   a2: -0.10
+    ///   b: 0.0
+    ///   c1: 0.525
+    ///   c2: 0.77
+    ///   c3: 0.74
+    ///   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]
+    ///
+    /// FANUC provides may such files for their 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<P: AsRef<Path>>(path: P) -> Result<Self, ParametersError> {
         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)?;
+
+        let processed_contents = preprocess_yaml_contents(&contents)?;
+
+        let deserialized: YamlParameters = serde_yaml::from_str(&processed_contents)?;
 
         Ok(Parameters {
             a1: deserialized.opw_kinematics_geometric_parameters.a1,
@@ -35,7 +61,19 @@ impl Parameters {
     }
 }
 
-/// Helper struct for deserialization
+fn preprocess_yaml_contents(contents: &str) -> Result<String, regex::Error> {
+    let re = Regex::new(r"deg\(([^)]+)\)")?;
+    let processed_contents = re.replace_all(contents, |caps: &regex::Captures| {
+        format!("{}", deg(caps[1].parse::<f64>().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,
@@ -53,8 +91,3 @@ struct GeometricParameters {
     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
-}

src/tests/fanuc_m16ib20.yaml

@@ -0,0 +1,13 @@
+#
+# Test data set for Fanuc m16ib20
+#
+opw_kinematics_geometric_parameters:
+  a1: 0.15
+  a2: -0.10
+  b: 0.0
+  c1: 0.525
+  c2: 0.77
+  c3: 0.74
+  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]

src/tests/testcases.rs

@@ -355,4 +355,34 @@ mod tests {
         let joints = [0.0, 0.1, 0.2, 0.3, 0.4, PI];
         assert_eq!(robot.kinematic_singularity(&joints), None);
     }
+
+    #[test]
+    fn test_parameters_from_yaml() {
+        let filename = "src/tests/fanuc_m16ib20.yaml";        
+        let loaded = 
+            Parameters::from_yaml_file(filename).expect("Failed to load parameters from file");
+
+        let expected = Parameters {
+            a1: 0.15,
+            a2: -0.10,
+            b: 0.0,
+            c1: 0.525,
+            c2: 0.77,
+            c3: 0.74,
+            c4: 0.10,
+            offsets: [0.0, 0.0, -90.0_f64.to_radians(), 0.0, 0.0, 180.0_f64.to_radians()],
+            sign_corrections: [1, 1, -1, -1, -1, -1],
+        };
+
+
+        assert_eq!(expected.a1, loaded.a1);
+        assert_eq!(expected.a2, loaded.a2);
+        assert_eq!(expected.b, loaded.b);
+        assert_eq!(expected.c1, loaded.c1);
+        assert_eq!(expected.c2, loaded.c2);
+        assert_eq!(expected.c3, loaded.c3);
+        assert_eq!(expected.c4, loaded.c4);
+        assert_eq!(expected.offsets, loaded.offsets);
+        assert_eq!(expected.sign_corrections, loaded.sign_corrections);
+    }   
 }