rs_opw_kinematics/utils/
utils.rs

1//! Helper functions
2
3use crate::kinematic_traits::{Joints, Solutions};
4use nalgebra::{Isometry3, UnitQuaternion, Vector6};
5
6/// Checks the solution for validity. This is only internally needed as all returned
7/// solutions are already checked.
8pub(crate) mod opw_kinematics {
9    use crate::kinematic_traits::Joints;
10
11    /// Checks if all elements in the array are finite
12    pub fn is_valid(qs: &Joints) -> bool {
13        qs.iter().all(|&q| q.is_finite())
14    }
15}
16
17/// Convert array of f32's in degrees to Joints
18/// that are array of f64's in radians
19pub fn joints(angles: &[f32; 6]) -> Joints {
20    [
21        (angles[0] as f64).to_radians(),
22        (angles[1] as f64).to_radians(),
23        (angles[2] as f64).to_radians(),
24        (angles[3] as f64).to_radians(),
25        (angles[4] as f64).to_radians(),
26        (angles[5] as f64).to_radians(),
27    ]
28}
29
30/// Convert joints that are array of f64's in radians to
31/// array of f32's in degrees
32pub fn to_degrees(angles: &Joints) -> [f32; 6] {
33    [
34        angles[0].to_degrees() as f32,
35        angles[1].to_degrees() as f32,
36        angles[2].to_degrees() as f32,
37        angles[3].to_degrees() as f32,
38        angles[4].to_degrees() as f32,
39        angles[5].to_degrees() as f32,
40    ]
41}
42
43/// Print joint values for all solutions, converting radianst to degrees.
44pub fn dump_solutions(solutions: &Solutions) {
45    if solutions.is_empty() {
46        println!("No solutions");
47    }
48    for sol_idx in 0..solutions.len() {
49        let mut row_str = String::new();
50        for joint_idx in 0..6 {
51            let computed = solutions[sol_idx][joint_idx];
52            row_str.push_str(&format!("{:5.2} ", computed.to_degrees()));
53        }
54        println!("[{}]", row_str.trim_end());
55    }
56}
57
58pub fn dump_solutions_degrees(solutions: &Solutions) {
59    if solutions.is_empty() {
60        println!("No solutions");
61    }
62    for sol_idx in 0..solutions.len() {
63        let mut row_str = String::new();
64        for joint_idx in 0..6 {
65            let computed = solutions[sol_idx][joint_idx];
66            row_str.push_str(&format!("{:5.2} ", computed));
67        }
68        println!("[{}]", row_str.trim_end());
69    }
70}
71
72/// Print joint values, converting radianst to degrees.
73pub fn dump_joints(joints: &Joints) {
74    let mut row_str = String::new();
75    for joint_idx in 0..6 {
76        let computed = joints[joint_idx];
77        row_str.push_str(&format!("{:5.2} ", computed.to_degrees()));
78    }
79    println!("[{}]", row_str.trim_end());
80}
81
82pub fn dump_pose(isometry: &Isometry3<f64>) {
83    // Extract translation components
84    let translation = isometry.translation.vector;
85
86    // Extract rotation components and convert to Euler angles in radians
87    let rotation: UnitQuaternion<f64> = isometry.rotation;
88
89    // Print translation and rotation
90    println!(
91        "x: {:.5}, y: {:.5}, z: {:.5},  quat: {:.5},{:.5},{:.5},{:.5}",
92        translation.x, translation.y, translation.z, rotation.i, rotation.j, rotation.k, rotation.w
93    );
94}
95
96/// Allows to specify joint values in degrees (converts to radians)
97pub fn as_radians(degrees: [i32; 6]) -> Joints {
98    std::array::from_fn(|i| (degrees[i] as f64).to_radians())
99}
100
101/// formatting for YAML output
102pub(crate) fn deg(x: &f64) -> String {
103    if *x == 0.0 {
104        return "0".to_string();
105    }
106    format!("deg({:.4})", x.to_degrees())
107}
108
109/// Converts ```nalgebra::Vector6<f64>``` to Joints ([f64; 6])
110pub fn vector6_to_joints(v: Vector6<f64>) -> Joints {
111    [v[0], v[1], v[2], v[3], v[4], v[5]]
112}
113
114/// Converts ```Joints ([f64; 6])``` to a ```Vector6<f64>```
115pub fn joints_to_vector6(j: Joints) -> nalgebra::Vector6<f64> {
116    Vector6::new(j[0], j[1], j[2], j[3], j[4], j[5])
117}
118
119/// Calculates the transition cost between two sets of joint positions,
120/// weighted by given coefficients (rotating heavy base joints is
121/// more expensive). This function returns maximum rotation
122pub fn transition_costs(from: &Joints, to: &Joints, coefficients: &Joints) -> f64 {
123    [(from[0] - to[0]).abs() * coefficients[0]
124        + (from[1] - to[1]).abs() * coefficients[1]
125        + (from[2] - to[2]).abs() * coefficients[2]
126        + (from[3] - to[3]).abs() * coefficients[3]
127        + (from[4] - to[4]).abs() * coefficients[4]
128        + (from[5] - to[5]).abs() * coefficients[5]]
129    .iter()
130    .fold(f64::NEG_INFINITY, |a, &b| a.max(b))
131}
132
133pub fn assert_pose_eq(ta: &Isometry3<f64>, tb: &Isometry3<f64>,
134                 distance_tolerance: f64, angular_tolerance: f64) -> bool {
135    fn bad(ta: &Isometry3<f64>, tb: &Isometry3<f64>) {
136        dump_pose(ta);
137        dump_pose(tb);
138    }
139    
140    let translation_distance = (ta.translation.vector - tb.translation.vector).norm();
141    let angular_distance = ta.rotation.angle_to(&tb.rotation);
142
143    if translation_distance.abs() > distance_tolerance {
144        bad(ta, tb);        
145        panic!("Poses have too different translations");        
146    }
147
148    if angular_distance.abs() > angular_tolerance {
149        bad(ta, tb);
150        panic!("Poses have too different angles");
151    }
152    true
153}
154
155#[cfg(test)]
156mod tests {
157    use super::opw_kinematics::*;
158    use std::f64::consts::PI;
159
160    #[test]
161    fn test_is_valid_with_all_finite() {
162        let qs = [0.0, 1.0, -1.0, 0.5, -0.5, PI];
163        assert!(is_valid(&qs));
164    }
165
166    #[test]
167    fn test_is_valid_with_nan() {
168        let qs = [0.0, f64::NAN, 1.0, -1.0, 0.5, -0.5];
169        assert!(!is_valid(&qs));
170    }
171
172    #[test]
173    fn test_is_valid_with_infinity() {
174        let qs = [0.0, f64::INFINITY, 1.0, -1.0, 0.5, -0.5];
175        assert!(!is_valid(&qs));
176    }
177}
rs_opw_kinematics/utils/utils.rs

rs_opw_kinematics/utils/
utils.rs
