rs_opw_kinematics/
parameters_from_file.rs

1//! Supports extracting OPW parameters from YAML file (optional)
2
3use std::path::Path;
4
5use garde::Validate;
6use serde::Deserialize;
7use serde_saphyr::Options;
8
9use crate::parameter_error::ParameterError;
10use crate::parameters::opw_kinematics::Parameters;
11
12fn default_offsets() -> Vec<f64> { vec![0.0; 6] }
13fn default_sign_corrections() -> Vec<i8> { vec![1; 6] }
14
15fn opw_geometry_parameter(v: &f64, _ctx: &()) -> garde::Result {
16    if !v.is_finite() {
17        return Err(garde::Error::new("must be finite"));
18    }
19    Ok(())
20}
21
22fn joint_offset(v: &f64, _ctx: &()) -> garde::Result {
23    if !v.is_finite() {
24        return Err(garde::Error::new("must be finite"));
25    }
26    let limit = 2.0 * std::f64::consts::PI;
27    if *v < -limit || *v > limit {
28        return Err(garde::Error::new("must be within [-2*PI, 2*PI]"));
29    }
30    Ok(())
31}
32
33fn sign_correction(v: &i8, _ctx: &()) -> garde::Result {
34    if *v != -1 && *v != 1 {
35        return Err(garde::Error::new("must be -1 or 1"));
36    }
37    Ok(())
38}
39
40#[derive(Deserialize, Validate)]
41struct GeometricParameters {
42    #[garde(custom(opw_geometry_parameter))]
43    pub a1: f64,
44    #[garde(custom(opw_geometry_parameter))]
45    pub a2: f64,
46    #[garde(custom(opw_geometry_parameter))]
47    pub b: f64,
48    #[garde(custom(opw_geometry_parameter))]
49    pub c1: f64,
50    #[garde(custom(opw_geometry_parameter))]
51    pub c2: f64,
52    #[garde(custom(opw_geometry_parameter))]
53    pub c3: f64,
54    #[garde(custom(opw_geometry_parameter))]
55    pub c4: f64,
56    /// Optional here; top-level `dof` overrides if also present
57    #[serde(default)]
58    #[garde(range(min = 5, max = 6))]
59    pub dof: Option<i8>,
60}
61
62#[derive(Deserialize, Validate)]
63struct Root {
64    #[garde(dive)]
65    pub opw_kinematics_geometric_parameters: GeometricParameters,
66    #[serde(default = "default_offsets")]
67    #[garde(length(min = 5, max = 6), inner(custom(joint_offset)))]
68    pub opw_kinematics_joint_offsets: Vec<f64>,
69    #[serde(default = "default_sign_corrections")]
70    #[garde(length(min = 5, max = 6), inner(custom(sign_correction)))]
71    pub opw_kinematics_joint_sign_corrections: Vec<i8>,
72    /// Optional; overrides opw_kinematics_geometric_parameters.dof if present
73    #[serde(default)]
74    #[garde(range(min = 5, max = 6))]
75    pub dof: Option<i8>,
76}
77
78impl Parameters {
79    /// Read the robot configuration from YAML file. YAML file like this is supported:
80    /// ```yaml
81    /// # FANUC m16ib20
82    /// opw_kinematics_geometric_parameters:
83    ///   a1: 0.15
84    ///   a2: -0.10
85    ///   b: 0.0
86    ///   c1: 0.525
87    ///   c2: 0.77
88    ///   c3: 0.74
89    ///   c4: 0.10
90    /// opw_kinematics_joint_offsets: [0.0, 0.0, deg(-90.0), 0.0, 0.0, deg(180.0)]
91    /// opw_kinematics_joint_sign_corrections: [1, 1, -1, -1, -1, -1]
92    /// dof: 6
93    /// ```
94    /// Offsets, sign corrections, and DOF are optional.
95    ///
96    /// YAML extension to parse the deg(angle) function is supported (serde_saphyr).
97    ///
98    /// See e.g. ROS-Industrial fanuc_m16ib_support opw yaml.
99    pub fn from_yaml_file<P: AsRef<Path>>(path: P) -> Result<Self, ParameterError> {
100        let contents = std::fs::read_to_string(path)?;
101        let root: Root = serde_saphyr::from_str_with_options_valid(
102            &contents,
103            Options { angle_conversions: true, ..Default::default() }
104        ).map_err(|e| ParameterError::ParseError(format!("{}", e)))?;
105
106        // DOF precedence:
107        // - If both present and different -> error.
108        // - Else prefer top-level; else gp; else default 6.
109        let dof = match (root.dof, root.opw_kinematics_geometric_parameters.dof) {
110            (Some(top), Some(inner)) if top != inner => {
111                return Err(ParameterError::ParseError(format!(
112                    "dof appears at top-level ({}) and under geometric parameters ({}) with conflicting values",
113                    top, inner
114                )));
115            }
116            (Some(top), _) => top,
117            (None, Some(inner)) => inner,
118            (None, None) => 6,
119        };
120
121        // Sign corrections: allow 5 (pad with 1) or 6.
122        // Value/length validation is handled by garde on `Root`.
123        let mut sign_corrections = vec_to_six(root.opw_kinematics_joint_sign_corrections, 1i8)?;
124
125        // Offsets: allow 5 (pad with 0.0) or 6.
126        // Value/length validation is handled by garde on `Root`.
127        let mut offsets = vec_to_six(root.opw_kinematics_joint_offsets, 0.0f64)?;
128
129        // 5-DOF normalization: lock joint 6 to 0 (both sign correction and offset)
130        if dof == 5 {
131            if sign_corrections[5] != 0 {
132                // Normalize to 0 to match "blocked J6"
133                sign_corrections[5] = 0;
134            }
135            if offsets[5] != 0.0 {
136                // Offset on a locked joint is misleading; normalize to 0
137                offsets[5] = 0.0;
138            }
139        }
140
141        let gp = &root.opw_kinematics_geometric_parameters;
142
143        Ok(Parameters {
144            a1: gp.a1,
145            a2: gp.a2,
146            b: gp.b,
147            c1: gp.c1,
148            c2: gp.c2,
149            c3: gp.c3,
150            c4: gp.c4,
151            dof,
152            offsets,
153            sign_corrections,
154        })
155    }
156}
157
158/// Convert a vector to a 6-element array:
159/// - If length is 5, pad with `pad`.
160/// - If length is 6, pass-through.
161/// - Otherwise, error.
162fn vec_to_six<T: Copy>(
163    mut v: Vec<T>,
164    pad: T,
165) -> Result<[T; 6], ParameterError> {
166    if v.len() == 5 {
167        v.push(pad);
168    }
169    v.try_into()
170        .map_err(|v: Vec<T>| ParameterError::InvalidLength { expected: 6, found: v.len() })
171}
rs_opw_kinematics/parameters_from_file.rs

rs_opw_kinematics/
parameters_from_file.rs
