rs_opw_kinematics/
urdf.rs

1//! Supports extracting OPW parameters from URDF (optional)
2
3extern crate sxd_document;
4
5use crate::simplify_joint_name::preprocess_joint_name;
6use std::collections::HashMap;
7use sxd_document::{parser, dom, QName};
8use std::error::Error;
9use std::fs::read_to_string;
10use std::path::Path;
11use regex::Regex;
12use crate::constraints::{BY_PREV, Constraints};
13use crate::kinematic_traits::{Joints, JOINTS_AT_ZERO};
14use crate::kinematics_impl::OPWKinematics;
15use crate::parameter_error::ParameterError;
16use crate::parameters::opw_kinematics::Parameters;
17
18/// Simplified reading from URDF file. This function assumes sorting of results by closest to
19/// previous (BY_PREV) and no joint offsets (zero offsets). URDF file is expected in the input
20/// but XACRO file may also work. Robot joints must be named joint1 to joint6 in the file
21/// (as macro prefix, underscore and non-word chars is dropped, it can also be something like
22/// `${prefix}JOINT_1`). It may be more than one robot described in URDF file but they must all
23/// be identical.
24///
25/// # Parameters
26/// - `path`: the location of URDF or XACRO file to load from.
27///
28/// # Returns
29/// - Returns an instance of `OPWKinematics`, which contains the kinematic parameters
30///   extracted from the specified URDF file, including constraints as defined there.
31///
32/// # Example
33/// ```
34/// let kinematics = rs_opw_kinematics::urdf::from_urdf_file("src/tests/data/fanuc/m6ib_macro.xacro");
35/// println!("{:?}", kinematics);
36/// ```
37///
38/// # Errors
39/// - The function might panic if the file cannot be found, is not accessible, or is incorrectly 
40///   formatted. Users should ensure the file path is correct and the file is properly formatted as 
41///   URDF or XACRO file.
42pub fn from_urdf_file<P: AsRef<Path>>(path: P) -> OPWKinematics {
43    let xml_content = read_to_string(path).expect("Failed to read xacro/urdf file");
44
45    let joint_data = process_joints(&xml_content, &None)
46        .expect("Failed to process XML joints");
47
48    let opw_parameters = populate_opw_parameters(joint_data, &None)
49        .expect("Failed to read OpwParameters");
50
51    opw_parameters.to_robot(BY_PREV, &JOINTS_AT_ZERO)
52}
53
54/// Parses URDF XML content to construct OPW kinematics parameters for a robot.
55/// This function provides detailed error handling through the `ParameterError` type,
56/// and returns intermediate type from where both Parameters and Constraints can be taken
57/// and inspected or modified if so required.
58///
59/// # Parameters
60/// - `xml_content`: A `String` containing the XML data of the URDF file.
61/// - `joint_names`: An optional array containing joint names. This may be required if
62///                  names do not follow typical naming convention, or there are multiple
63///                  robots defined in URDF. 
64///                  For 5 DOF robots, use the name of the tool center point instead of "joint6"
65///
66/// # Returns
67/// - Returns a `Result<URDFParameters, ParameterError>`. On success, it contains the OPW kinematics
68///   configuration for the robot. On failure, it returns a detailed error.
69///   Ust to_robot method to convert OpwParameters directly to the robot instance.
70///
71/// # Example showing full control over how the inverse kinematics solver is constructed:
72/// ```
73/// use std::f64::consts::PI;
74/// use rs_opw_kinematics::constraints::BY_PREV;
75/// use rs_opw_kinematics::kinematic_traits::{Joints, JOINTS_AT_ZERO, Kinematics};
76/// use rs_opw_kinematics::kinematics_impl::OPWKinematics;
77/// use rs_opw_kinematics::urdf::from_urdf;
78/// // Exactly this string would fail. Working URDF fragment would be too long for this example. 
79/// let xml_data = String::from("<robot><joint ...></joint></robot>");
80///
81/// // Let's assume the joint names have prefix and the joints are zero base numbered. 
82/// let joints = ["lf_joint_0", "lf_joint_1", "lf_joint_2", "lf_joint_3", "lf_joint_4", "lf_joint_5"];
83/// let offsets = [ 0., PI, 0., 0.,0.,0.];
84/// let opw_params = from_urdf(xml_data, &Some(joints));
85/// match opw_params {
86///     Ok(opw_params) => {
87///         println!("Building the IK solver {:?}", opw_params);
88///         let parameters = opw_params.parameters(&JOINTS_AT_ZERO); // Zero joint offsets
89///         let constraints =opw_params.constraints(BY_PREV); 
90///         let robot = OPWKinematics::new_with_constraints(parameters, constraints);
91///         // let joints = robot.inverse( ... )    
92///
93///     }
94///     Err(e) => println!("Error processing URDF: {}", e),
95/// }
96/// ```
97pub fn from_urdf(xml_content: String, joint_names: &Option<[&str; 6]>) -> Result<URDFParameters, ParameterError> {
98    let joint_data = process_joints(&xml_content, joint_names)
99        .map_err(|e|
100            ParameterError::XmlProcessingError(format!("Failed to process XML joints: {}", e)))?;
101
102    let opw_parameters = populate_opw_parameters(joint_data, joint_names)
103        .map_err(|e|
104            ParameterError::ParameterPopulationError(format!("Failed to interpret robot model: {}", e)))?;
105
106    Ok(opw_parameters)
107}
108
109
110#[derive(Debug, Default, PartialEq)]
111struct Vector3 {
112    x: f64,
113    y: f64,
114    z: f64,
115}
116
117impl Vector3 {
118    pub fn non_zero(&self) -> Result<f64, String> {
119        let mut non_zero_values = vec![];
120
121        if self.x != 0.0 {
122            non_zero_values.push(self.x);
123        }
124        if self.y != 0.0 {
125            non_zero_values.push(self.y);
126        }
127        if self.z != 0.0 {
128            non_zero_values.push(self.z);
129        }
130
131        match non_zero_values.len() {
132            0 => Ok(0.0),
133            1 => Ok(non_zero_values[0]),
134            _ => Err(format!("More than one non-zero value in URDF offset {:?}", self).to_string()),
135        }
136    }
137}
138
139/// Helper that returns the non-zero value from a pair of numbers if exactly one
140/// of them is non-zero. If both are zero, `Ok(0.0)` is returned. Returns an
141/// error when both values are non-zero.
142fn non_zero_pair(a: f64, b: f64) -> Result<f64, String> {
143    match (a, b) {
144        (0.0, 0.0) => Ok(0.0),
145        (0.0, b) => Ok(b),
146        (a, 0.0) => Ok(a),
147        (_, _) => Err(String::from("Both potential values are non-zero")),
148    }
149}
150
151#[derive(Debug, PartialEq)]
152struct JointData {
153    name: String,
154    vector: Vector3,
155    sign_correction: i32,
156    from: f64,
157    to: f64,
158}
159
160fn process_joints(xml: &str, joint_names: &Option<[&str; 6]>) -> Result<HashMap<String, JointData>, Box<dyn Error>> {
161    let package = parser::parse(xml)?;
162    let document = package.as_document();
163
164    // Access the root element
165    let root_element = document.root().children().into_iter()
166        .find_map(|e| e.element())
167        .ok_or("No root element found")?;
168
169    // Collect all joint data
170    let mut joints = Vec::new();
171    collect_joints(root_element, &mut joints, joint_names)?;
172
173    convert_to_map(joints)
174}
175
176// Recursive function to collect joint data
177fn collect_joints(element: dom::Element, joints: &mut Vec<JointData>, joint_names: &Option<[&str; 6]>) -> Result<(), Box<dyn Error>> {
178    let origin_tag = QName::new("origin");
179    let joint_tag = QName::new("joint");
180    let axis_tag = QName::new("axis");
181    let limit_tag = QName::new("limit");
182
183    for child in element.children().into_iter().filter_map(|e| e.element()) {
184        if child.name() == joint_tag {
185            let name;
186            let urdf_name = &child.attribute("name")
187                .map(|attr| attr.value().to_string())
188                .unwrap_or_else(|| "Unnamed".to_string());
189            if joint_names.is_some() {
190                // If joint names are explicitly given, they are expected to be as they are.
191                name = urdf_name.clone();
192            } else {
193                // Otherwise effort is done to "simplify" the names into joint1 to joint6
194                name = preprocess_joint_name(urdf_name);
195            }
196            let axis_element = child.children().into_iter()
197                .find_map(|e| e.element().filter(|el| el.name() == axis_tag));
198            let origin_element = child.children().into_iter()
199                .find_map(|e| e.element().filter(|el| el.name() == origin_tag));
200            let limit_element = child.children().into_iter()
201                .find_map(|e| e.element().filter(|el| el.name() == limit_tag));
202
203            let mut joint_data = JointData {
204                name,
205                vector: origin_element.map_or_else(|| Ok(Vector3::default()), get_xyz_from_origin)?,
206                sign_correction: axis_element.map_or(Ok(1), get_axis_sign)?,
207                from: 0.,
208                to: 0., // 0 to 0 in our notation is 'full circle'
209            };
210
211            match limit_element.map(get_limits).transpose() {
212                Ok(Some((from, to))) => {
213                    joint_data.from = from;
214                    joint_data.to = to;
215                }
216                Ok(None) => {}
217                Err(e) => {
218                    println!("Joint limits defined but not not readable for {}: {}",
219                             joint_data.name, e.to_string());
220                }
221            }
222
223            joints.push(joint_data);
224        }
225
226        collect_joints(child, joints, joint_names)?;
227    }
228
229    Ok(())
230}
231
232
233fn get_xyz_from_origin(element: dom::Element) -> Result<Vector3, Box<dyn Error>> {
234    let xyz_attr = element.attribute("xyz").ok_or("xyz attribute not found")?;
235    let coords: Vec<f64> = xyz_attr.value().split_whitespace()
236        .map(str::parse)
237        .collect::<Result<_, _>>()?;
238
239    if coords.len() != 3 {
240        return Err("XYZ attribute does not contain exactly three values".into());
241    }
242
243    Ok(Vector3 {
244        x: coords[0],
245        y: coords[1],
246        z: coords[2],
247    })
248}
249
250fn get_axis_sign(axis_element: dom::Element) -> Result<i32, Box<dyn Error>> {
251    // Fetch the 'xyz' attribute, assuming the element is correctly passed
252    let axis_attr = axis_element.attribute("xyz").ok_or_else(|| {
253        "'xyz' attribute not found in element supposed to represent the axis"
254    })?;
255
256    // Parse the xyz attribute to determine the sign corrections
257    let axis_values: Vec<f64> = axis_attr.value().split_whitespace()
258        .map(str::parse)
259        .collect::<Result<_, _>>()?;
260
261    // Filter and count non-zero values, ensuring exactly one non-zero which must be -1 or 1
262    let non_zero_values: Vec<i32> = axis_values.iter()
263        .filter(|&&v| v != 0.0)
264        .map(|&v| if v < 0.0 { -1 } else { 1 })
265        .collect();
266
267    // Check that exactly one non-zero value exists and it is either -1 or 1
268    if non_zero_values.len() == 1 && (non_zero_values[0] == -1 || non_zero_values[0] == 1) {
269        Ok(non_zero_values[0])
270    } else {
271        Ok(0) // This is a fixed joint
272    }
273}
274
275fn parse_angle(attr_value: &str) -> Result<f64, ParameterError> {
276    // Regular expression to match the ${radians(<number>)} format that is common in xacro
277    let re = Regex::new(r"^\$\{radians\((-?\d+(\.\d+)?)\)\}$")
278        .map_err(|_| ParameterError::ParseError("Invalid regex pattern".to_string()))?;
279
280    // Check if the input matches the special format
281    if let Some(caps) = re.captures(attr_value) {
282        let degrees_str = caps.get(1)
283            .ok_or(ParameterError::WrongAngle(format!("Bad representation: {}",
284                                                      attr_value).to_string()))?.as_str();
285        let degrees: f64 = degrees_str.parse()
286            .map_err(|_| ParameterError::WrongAngle(attr_value.to_string()))?;
287        Ok(degrees.to_radians())
288    } else {
289        // Try to parse the input as a plain number in that case it is in radians
290        let radians: f64 = attr_value.parse()
291            .map_err(|_| ParameterError::WrongAngle(attr_value.to_string()))?;
292        Ok(radians)
293    }
294}
295
296fn get_limits(element: dom::Element) -> Result<(f64, f64), ParameterError> {
297    let lower_attr = element.attribute("lower")
298        .ok_or_else(|| ParameterError::MissingField("lower limit not found".into()))?
299        .value();
300    let lower_limit = parse_angle(lower_attr)?;
301
302    let upper_attr = element.attribute("upper")
303        .ok_or_else(|| ParameterError::MissingField("upper limit not found".into()))?
304        .value();
305    let upper_limit = parse_angle(upper_attr)?;
306
307    Ok((lower_limit, upper_limit))
308}
309
310fn convert_to_map(joints: Vec<JointData>) -> Result<HashMap<String, JointData>, Box<dyn Error>> {
311    let mut map: HashMap<String, JointData> = HashMap::new();
312
313    for joint in joints {
314        if let Some(existing) = map.get(&joint.name) {
315            // Check if the existing entry is different from the new one
316            if existing != &joint {
317                return Err(Box::new(std::io::Error::new(std::io::ErrorKind::InvalidData,
318                                                        format!("Duplicate joint name with different data found: {}", joint.name))));
319            }
320        } else {
321            map.insert(joint.name.clone(), joint);
322        }
323    }
324
325    Ok(map)
326}
327
328/// OPW parameters as extrancted from URDF file, including constraints 
329/// (joint offsets are not directly defined in URDF). This structure
330/// can provide robot parameters, constraints and sign corrections,
331/// or alterntively can be converted to the robot directly. 
332#[derive(Default, Debug, Clone, Copy)]
333pub struct URDFParameters {
334    pub a1: f64,
335    pub a2: f64,
336    pub b: f64,
337    pub c1: f64,
338    pub c2: f64,
339    pub c3: f64,
340    pub c4: f64,
341    pub sign_corrections: [i8; 6],
342    pub from: Joints, // Array to store the lower limits
343    pub to: Joints,   // Array to store the upper limits
344    pub dof: i8
345}
346
347impl URDFParameters {
348    pub fn to_robot(self, sorting_weight: f64, offsets: &Joints) -> OPWKinematics {
349        OPWKinematics::new_with_constraints(
350            Parameters {
351                a1: self.a1,
352                a2: self.a2,
353                b: self.b,
354                c1: self.c1,
355                c2: self.c2,
356                c3: self.c3,
357                c4: self.c4,
358                sign_corrections: self.sign_corrections,
359                offsets: *offsets,
360                dof: self.dof
361            },
362            Constraints::new(
363                self.from,
364                self.to,
365                sorting_weight,
366            ),
367        )
368    }
369
370    /// Return extracted constraints.
371    pub fn constraints(self, sorting_weight: f64) -> Constraints {
372        Constraints::new(
373            self.from,
374            self.to,
375            sorting_weight,
376        )
377    }
378
379    /// Return extracted parameters
380    pub fn parameters(self, offsets: &Joints) -> Parameters {
381        Parameters {
382            a1: self.a1,
383            a2: self.a2,
384            b: self.b,
385            c1: self.c1,
386            c2: self.c2,
387            c3: self.c3,
388            c4: self.c4,
389            sign_corrections: self.sign_corrections,
390            offsets: *offsets,
391            dof: self.dof
392        }
393    }
394}
395
396fn populate_opw_parameters(joint_map: HashMap<String, JointData>, joint_names: &Option<[&str; 6]>)
397                           -> Result<URDFParameters, String> {
398    let mut opw_parameters = URDFParameters::default();
399
400    let names = joint_names.unwrap_or_else(
401        || ["joint1", "joint2", "joint3", "joint4", "joint5", "joint6"]);
402    
403    let is_six_dof = joint_map.contains_key(names[5]);
404    opw_parameters.c4 = 0.0; // joint6 would be something like "tcp" for the 5 DOF robot. Otherwise, 0 is assumed.
405
406    for j in 0..6 {
407        let joint = joint_map
408            .get(names[j]).ok_or_else(|| format!("Joint {} not found: {}", j, names[j]))?;
409
410        opw_parameters.sign_corrections[j] = joint.sign_correction as i8;
411        opw_parameters.from[j] = joint.from;
412        opw_parameters.to[j] = joint.to;
413
414        match j + 1 { // Joint number 1 to 6 inclusive
415            1 => {
416                opw_parameters.c1 = joint.vector.non_zero()?;
417            }
418            2 => {
419                opw_parameters.a1 = joint.vector.non_zero()?;
420            }
421            3 => {
422                // There is more divergence here. 
423                match joint.vector.non_zero() {
424                    Ok(value) => {
425                        // If there is only one value, it is value for c2. Most of the 
426                        // modern robots we tested do follow this design.
427                        opw_parameters.c2 = value;
428                        opw_parameters.b = 0.0;
429                    }
430                    Err(_err) => {
431                        // If there are multiple values, we assume b is given here as y.
432                        // c2 is given either in z or in x, other being 0.
433                        opw_parameters.c2 = non_zero_pair(joint.vector.x, joint.vector.z)?;
434                        opw_parameters.b = joint.vector.y;
435                    }
436                }
437            }
438            4 => {
439                match joint.vector.non_zero() {
440                    Ok(value) => {
441                        opw_parameters.a2 = -value;
442                    }
443                    Err(_err) => {
444                        // If there are multiple values, we assume a2 is given here as z.
445                        // c3 is given either in y or in x, other being 0.
446                        opw_parameters.a2 = -joint.vector.z;
447                        if opw_parameters.c3 != 0.0 {
448                            return Err(String::from("C3 seems defined twice (J4)"));
449                        }
450                        opw_parameters.c3 = non_zero_pair(joint.vector.x, joint.vector.y)?;
451                    }
452                }
453            }
454            5 => {
455                let candidate = joint.vector.non_zero()?;
456                if candidate != 0.0 {
457                    if opw_parameters.c3 != 0.0 {
458                        return Err(String::from("C3 seems defined twice (J5)"));
459                    } else {
460                        opw_parameters.c3 = candidate;
461                    }
462                }
463            }
464            6 => {
465                opw_parameters.c4 = joint.vector.non_zero()?;
466            }
467            _ => {
468                // Other joints not in use
469            }
470        }
471    }
472
473    if is_six_dof {
474        opw_parameters.dof = 6    
475    } else {
476        opw_parameters.dof = 5;
477        
478        // Set reasonable values for non-existing joint 6.
479        // Constraint checker will still be checking this range.
480        opw_parameters.sign_corrections[5] = 0; // Always suppress
481        // With from=to, constraint is suppressed.
482        opw_parameters.from[5] = 0.0; 
483        opw_parameters.to[5] = 0.0;       
484    }
485    
486
487    Ok(opw_parameters)
488}
489
490#[allow(dead_code)]
491// This function is not in use and exists for references only (old version)
492fn populate_opw_parameters_explicit(joint_map: HashMap<String, JointData>, joint_names: &Option<[&str; 6]>)
493                                    -> Result<URDFParameters, String> {
494    let mut opw_parameters = URDFParameters::default();
495
496    opw_parameters.b = 0.0; // We only support robots with b = 0 so far.
497
498    let names = joint_names.unwrap_or_else(
499        || ["joint1", "joint2", "joint3", "joint4", "joint5", "joint6"]);
500
501    for j in 0..6 {
502        let joint = joint_map
503            .get(names[j]).ok_or_else(|| format!("Joint {} not found: {}", j, names[j]))?;
504
505        opw_parameters.sign_corrections[j] = joint.sign_correction as i8;
506        opw_parameters.from[j] = joint.from;
507        opw_parameters.to[j] = joint.to;
508
509        match j + 1 { // Joint number 1 to 6 inclusive
510            1 => {
511                opw_parameters.c1 = joint.vector.z;
512            }
513            2 => {
514                opw_parameters.a1 = joint.vector.x;
515            }
516            3 => {
517                opw_parameters.c2 = joint.vector.z;
518                opw_parameters.b = joint.vector.y;
519                //opw_parameters.c2 = joint.vector.x; // Kuka                
520            }
521            4 => {
522                opw_parameters.a2 = -joint.vector.z;
523            }
524            5 => {
525                opw_parameters.c3 = joint.vector.x;
526                opw_parameters.c3 = joint.vector.z; // TX40                
527            }
528            6 => {
529                opw_parameters.c4 = joint.vector.x;
530                opw_parameters.c4 = joint.vector.z; // TX40                
531            }
532            _ => {
533                // Other joints not in use
534            }
535        }
536    }
537
538    Ok(opw_parameters)
539}
540
541
542#[cfg(test)]
543mod tests {
544    use super::*;
545
546    #[test]
547    fn test_process_joints() {
548        let xml = r#"
549            <robot>
550                <joint name="${prefix}JOint_2!">
551                    <origin xyz="4.0 5.0 6.0"></origin>
552                    <axis xyz="0 0 1"/>
553                    <limit lower="-3.15" upper="4.62" effort="0" velocity="3.67"/>
554                </joint>            
555                <joint name="joint1">
556                    <origin xyz="1.0 2.0 3.0"></origin>
557                    <axis xyz="0 -1 0"/>
558                    <limit lower="-3.14" upper="4.61" effort="0" velocity="3.67"/>
559                </joint>
560            </robot>
561        "#;
562
563        let joint_data = process_joints(xml, &None)
564            .expect("Failed to process XML joints");
565
566        assert_eq!(joint_data.len(), 2, "Should have extracted two joints");
567
568        let j1 = &joint_data["joint1"];
569        let j2 = &joint_data["joint2"];
570
571        // Tests for joint1
572        assert_eq!(j1.name, "joint1", "Joint1 name incorrect");
573        assert_eq!(j1.vector.x, 1.0, "Joint1 X incorrect");
574        assert_eq!(j1.vector.y, 2.0, "Joint1 Y incorrect");
575        assert_eq!(j1.vector.z, 3.0, "Joint1 Z incorrect");
576        assert_eq!(j1.sign_correction, -1, "Joint1 sign correction incorrect");
577        assert_eq!(j1.from, -3.14, "Joint1 lower limit incorrect");
578        assert_eq!(j1.to, 4.61, "Joint1 upper limit incorrect");
579
580        // Tests for joint2
581        assert_eq!(j2.name, "joint2", "Joint2 name incorrect");
582        assert_eq!(j2.vector.x, 4.0, "Joint2 X incorrect");
583        assert_eq!(j2.vector.y, 5.0, "Joint2 Y incorrect");
584        assert_eq!(j2.vector.z, 6.0, "Joint2 Z incorrect");
585        assert_eq!(j2.sign_correction, 1, "Joint2 sign correction incorrect");
586        assert_eq!(j2.from, -3.15, "Joint2 lower limit incorrect");
587        assert_eq!(j2.to, 4.62, "Joint2 upper limit incorrect");
588    }
589
590    #[test]
591    fn test_populate_named_joints() {
592        // Let's say they are zero-base numbered and have the side prefix in front.
593        // Also, there are joints for another robot with different prefix (multiple robots in URDF).
594        // This test shows that multiple robots are supported (if URDF defines a multi-robot cell)
595        let xml = r#"
596            <robot>
597                <joint name="right_joint_0" type="revolute"> <!-- must be ignored -->
598                  <origin xyz="0 0 0.950" rpy="0 0 0"/>
599                  <parent link="right_base_link"/>
600                  <child link="right_link_1"/>
601                  <axis xyz="0 0 1"/>
602                  <limit lower="-2.9670" upper="2.9670" effort="0" velocity="2.6179"/>
603                </joint>            
604                <joint name="left_joint_0" type="revolute"> <!-- numbered from 0 -->
605                  <origin xyz="0 0 0.450" rpy="0 0 0"/>
606                  <parent link="left_base_link"/>
607                  <child link="left_link_1"/>
608                  <axis xyz="0 0 1"/>
609                  <limit lower="-2.9670" upper="2.9670" effort="0" velocity="2.6179"/>
610                </joint>
611                <joint name="left_joint_1" type="revolute">
612                  <origin xyz="0.150 0 0" rpy="0 0 0"/>
613                  <parent link="left_link_1"/>
614                  <child link="left_link_2"/>
615                  <axis xyz="0 1 0"/>
616                  <limit lower="-1.5707" upper="2.7925" effort="0" velocity="2.7925"/>
617                </joint>
618                <joint name="left_joint_2" type="revolute">
619                  <origin xyz="0 0 0.600" rpy="0 0 0"/>
620                  <parent link="left_link_2"/>
621                  <child link="left_link_3"/>
622                  <axis xyz="0 -1 0"/>
623                  <limit lower="-2.9670" upper="2.9670" effort="0" velocity="2.9670"/>
624                </joint>
625                <joint name="left_joint_3" type="revolute">
626                  <origin xyz="0 0 0.100" rpy="0 0 0"/>
627                  <parent link="left_link_3"/>
628                  <child link="left_link_4"/>
629                  <axis xyz="-1 0 0"/>
630                  <limit lower="-3.3161" upper="3.3161" effort="0" velocity="6.9813"/>
631                </joint>
632                <joint name="left_joint_4" type="revolute">
633                  <origin xyz="0.615 0 0" rpy="0 0 0"/>
634                  <parent link="left_link_4"/>
635                  <child link="left_link_5"/>
636                  <axis xyz="0 -1 0"/>
637                  <limit lower="-2.4434" upper="2.4434" effort="0" velocity="6.9813"/>
638                </joint>
639                <joint name="left_joint_5" type="revolute">
640                  <origin xyz="0.100 0 0" rpy="0 0 0"/>
641                  <parent link="left_link_5"/>
642                  <child link="left_link_6"/>
643                  <axis xyz="-1 0 0"/>
644                  <limit lower="-6.2831" upper="6.2831" effort="0" velocity="9.0757"/>
645                </joint>                               
646            </robot>
647        "#;
648
649        let joints = ["left_joint_0", "left_joint_1", "left_joint_2",
650            "left_joint_3", "left_joint_4", "left_joint_5"];
651
652        let opw_parameters =
653            from_urdf(xml.to_string(), &Some(joints)).expect("Failed to parse parameters");
654
655        assert_eq!(opw_parameters.a1, 0.15, "a1 parameter mismatch");
656        assert_eq!(opw_parameters.a2, -0.10, "a2 parameter mismatch");
657        assert_eq!(opw_parameters.b, 0.0, "b parameter mismatch");
658        assert_eq!(opw_parameters.c1, 0.45, "c1 parameter mismatch");
659        assert_eq!(opw_parameters.c2, 0.6, "c2 parameter mismatch");
660        assert_eq!(opw_parameters.c3, 0.615, "c3 parameter mismatch");
661        assert_eq!(opw_parameters.c4, 0.10, "c4 parameter mismatch");
662    }
663}
664
665
rs_opw_kinematics/urdf.rs

rs_opw_kinematics/
urdf.rs
