rs_opw_kinematics/
constraints.rs

1//! Joint limit support
2
3use std::f64::consts::PI;
4use std::f64::INFINITY;
5use std::ops::RangeInclusive;
6use rand::Rng;
7use crate::kinematic_traits::{Joints, JOINTS_AT_ZERO};
8use crate::utils::deg;
9
10#[derive(Clone, Debug, Copy)]
11pub struct Constraints {
12    /// Normalized lower limit. If more than upper limit, the range wraps-around through 0
13    pub from: [f64; 6],
14
15    /// Normalized upper limit. If less than lower limit, the range wraps-around through 0
16    pub to: [f64; 6],
17
18    // Constraint centers. Used in distance from constraints based sorting.
19    pub centers: [f64; 6],
20
21    // How far from the center the value could be
22    pub tolerances: [f64; 6],
23
24    /// Used when sorting the solutions by both middle values of the constraints and the previous
25    /// values of the joints. 1.0 gives the absolute priority to constraints, 0.0. gives
26    /// absolute priority for joints.
27    pub sorting_weight: f64,
28}
29
30/// When sorting solutions, give absolute priority to constraints (the closer to the midrange
31/// of constraints, the better)
32pub const BY_CONSTRAINS: f64 = 1.0;
33/// When sorting solutions, give absolute priority to previous joints values (the closer to the 
34/// previous value, the better). 
35pub const BY_PREV: f64 = 0.0;
36
37const TWO_PI: f64 = 2.0 * PI;
38
39impl Constraints {
40    /// Create constraints that restrict the joint rotations between 'from' to 'to' values.
41    /// Wrapping arround is supported so order is important. For instance,
42    /// from = 0.1 and to = 0.2 (radians) means the joint
43    /// is allowed to rotate to 0.11, 0.12 ... 1.99, 0.2. 
44    /// from = 0.2 ant to = 0.1 is also valid but means the joint is allowed to rotate
45    /// to 0.21, 0.22, 0.99, 2 * PI or 0.0 (wrapping around), then to 0.09 and finally 0.1,
46    /// so the other side of the circle. The sorting_weight parameter influences sorting of the
47    /// results: 0.0 (or BY_PREV) gives absolute priority to the previous values of the joints,
48    /// 1.0 (or BY_CONSTRAINTS) gives absolute priority to the middle values of constraints.
49    /// Intermediate values like 0.5 provide the weighted compromise.
50    pub fn new(from: Joints, to: Joints, sorting_weight: f64) -> Self {
51        let (centers, tolerances) = Self::compute_centers(from, to);
52
53        Constraints {
54            from: from,
55            to: to,
56            centers: centers,
57            tolerances: tolerances,
58            sorting_weight: sorting_weight,
59        }
60    }
61
62    /// Initializes `Constraints` from an array of 6 ranges (`RangeInclusive<f64>`),
63    /// where each range specifies the `from` (start) and `to` (end) values for each joint.
64    /// This is convenience method, where VALUES MUST BE GIVEN IN DEGREES.
65    ///
66    /// # Parameters
67    /// - `ranges`: An array of 6 `RangeInclusive<f64>` values, each representing a range for one joint.
68    ///   - The start of each range is taken as the `from` bound.
69    ///   - The end of each range is taken as the `to` bound.
70    /// - `sorting_weight`: A `f64` value representing the sorting weight for the constraint.
71    ///
72    /// # Returns
73    /// A new instance of `Constraints` with `from`, `to`, `centers`, and `tolerances` calculated
74    /// based on the specified ranges.
75    ///
76    /// # Example
77    /// ```
78    /// use rs_opw_kinematics::constraints::{Constraints, BY_PREV};
79    /// let constraints = Constraints::from_degrees(
80    ///   [0.0..=90.0, 45.0..=135.0, -90.0..=90.0, 0.0..=180.0, -45.0..=45.0, -180.0..=180.0], 
81    ///   BY_PREV);
82    /// ```
83    pub fn from_degrees(ranges: [RangeInclusive<f64>; 6], sorting_weight: f64) -> Self {
84        let from: Joints = [
85            ranges[0].start().to_radians(),
86            ranges[1].start().to_radians(),
87            ranges[2].start().to_radians(),
88            ranges[3].start().to_radians(),
89            ranges[4].start().to_radians(),
90            ranges[5].start().to_radians(),
91        ];
92
93        let to: Joints = [
94            ranges[0].end().to_radians(),
95            ranges[1].end().to_radians(),
96            ranges[2].end().to_radians(),
97            ranges[3].end().to_radians(),
98            ranges[4].end().to_radians(),
99            ranges[5].end().to_radians(),
100        ];
101
102        let (centers, tolerances) = Self::compute_centers(from, to);
103
104        Constraints {
105            from,
106            to,
107            centers,
108            tolerances,
109            sorting_weight,
110        }
111    }
112
113    fn compute_centers(from: Joints, to: Joints) -> (Joints, Joints) {
114        let mut centers: Joints = JOINTS_AT_ZERO;
115        let mut tolerances: Joints = JOINTS_AT_ZERO;
116
117        for j_idx in 0..6 {
118            let a = from[j_idx];
119            let mut b = to[j_idx];
120            if a == b {
121                tolerances[j_idx] = INFINITY; // No constraint, not checked
122            } else if a < b {
123                // Values do not wrap arround
124                centers[j_idx] = (a + b) / 2.0;
125                tolerances[j_idx] = (b - a) / 2.0;
126            } else {
127                // Values wrap arround. Move b forward by period till it gets ahead.
128                while b < a {
129                    b = b + TWO_PI;
130                }
131                centers[j_idx] = (a + b) / 2.0;
132                tolerances[j_idx] = (b - a) / 2.0;
133            }
134        }
135        (centers, tolerances)
136    }
137
138    pub fn update_range(&mut self, from: Joints, to: Joints) {
139        let (centers, tolerances) = Self::compute_centers(from, to);
140
141        self.from = from;
142        self.to = to;
143        self.centers = centers;
144        self.tolerances = tolerances;
145        // This method does not change the sorting weight.
146    }
147
148    fn inside_bounds(angle1: f64, angle2: f64, tolerance: f64) -> bool {
149        if tolerance.is_infinite() {
150            return true;
151        }
152        let mut difference = (angle1 - angle2).abs();
153        difference = difference % TWO_PI;
154        if difference > PI {
155            difference = TWO_PI - difference;
156        }
157        difference <= tolerance
158    }
159
160    /// Checks if all values in the given vector or angles satisfy these constraints.
161    #[must_use = "Ignoring compliance check result may cause constraint violations."]
162    pub fn compliant(&self, angles: &[f64; 6]) -> bool {
163        let ok = angles.iter().enumerate().all(|(i, &angle)| {
164            // '!' is used to negate the condition from 'out_of_bounds' directly in the 'all' call.
165            Self::inside_bounds(angle, self.centers[i], self.tolerances[i])
166        });
167        ok
168    }
169
170    /// Return new vector of angle arrays, removing all that have members not satisfying these
171    /// constraints.
172    #[must_use = "Ignoring filtered results means no constraints are actually applied."]
173    pub fn filter(&self, angles: &Vec<[f64; 6]>) -> Vec<[f64; 6]> {
174        angles.into_iter()
175            .filter(|angle_array| self.compliant(&angle_array))
176            .cloned()
177            .collect()
178    }
179
180
181    pub fn to_yaml(&self) -> String {
182        format!(
183            "constraints:\n  \
184               from: [{}]\n  \
185               to: [{}]\n",
186            self.from.iter().map(|x| deg(x))
187                .collect::<Vec<_>>().join(", "),
188            self.to.iter().map(|x| deg(x))
189                .collect::<Vec<_>>().join(", ")
190        )
191    }
192
193
194    /// Generate a random valid angle within the defined constraints for each joint.
195    pub fn random_angles(&self) -> Joints {
196        let mut rng = rand::rng();
197
198        fn random_angle(rng: &mut impl Rng, from: f64, to: f64) -> f64 {
199            if from < to {
200                rng.random_range(from..to)
201            } else {
202                let range_length = TWO_PI - (from - to);
203                let segment = rng.random_range(0.0..range_length);
204                if segment < (TWO_PI - from) {
205                    from + segment
206                } else {
207                    segment - (TWO_PI - from)
208                }
209            }
210        }
211
212        [
213            random_angle(&mut rng, self.from[0], self.to[0]),
214            random_angle(&mut rng, self.from[1], self.to[1]),
215            random_angle(&mut rng, self.from[2], self.to[2]),
216            random_angle(&mut rng, self.from[3], self.to[3]),
217            random_angle(&mut rng, self.from[4], self.to[4]),
218            random_angle(&mut rng, self.from[5], self.to[5]),
219        ]
220    }
221
222}
223
224impl Default for Constraints {
225    fn default() -> Self {
226        Constraints {
227            from: [0.0; 6],
228            to: [TWO_PI; 6],
229            centers: [PI; 6],
230            tolerances: [PI; 6],
231            sorting_weight: BY_PREV,
232        }
233    }
234}
235
236#[cfg(test)]
237mod tests {
238    use crate::kinematic_traits::Solutions;
239    use crate::utils::{as_radians};
240    use super::*;
241
242    #[test]
243    fn test_historical_failure_1() {
244        let from = as_radians([9, 18, 28, 38, -5, 55]);
245        let angles = as_radians([10, 20, 30, 40, 0, 60]);
246        let to = as_radians([11, 22, 33, 44, 5, 65]);
247
248        let limits = Constraints::new(from, to, BY_CONSTRAINS);
249
250        let sols: Solutions = vec![angles];
251        assert_eq!(limits.filter(&sols).len(), 1);
252
253        assert!(limits.compliant(&angles));
254    }
255
256    #[test]
257    fn test_no_wrap_around() {
258        let angles = [0.1 * PI, 0.2 * PI, 0.3 * PI, 0.4 * PI, 0.5 * PI, 0.6 * PI];
259        let from = [0.0, 0.15 * PI, 0.25 * PI, 0.35 * PI, 0.45 * PI, 0.55 * PI];
260        let to = [0.2 * PI, 0.3 * PI, 0.4 * PI, 0.5 * PI, 0.6 * PI, 0.7 * PI];
261        let limits = Constraints::new(from, to, BY_CONSTRAINS);
262        assert!(limits.compliant(&angles));
263    }
264
265    #[test]
266    fn test_with_wrap_around() {
267        let angles = [0.9 * PI, 1.9 * PI, 0.05 * PI, 1.05 * PI, 1.95 * PI, 0.95 * PI];
268        let from = [0.8 * PI, 1.8 * PI, 0.0, 1.0 * PI, 1.9 * PI, 0.9 * PI];
269        let to = [0.1 * PI, 1.1 * PI, 0.2 * PI, 1.2 * PI, 0.0, 1.0 * PI];
270        let limits = Constraints::new(from, to, BY_CONSTRAINS);
271        assert!(limits.compliant(&angles));
272    }
273
274    #[test]
275    fn test_full_circle() {
276        let angles = [0.0, 1.0 * PI, 0.5 * PI, 1.5 * PI, 0.25 * PI, 0.75 * PI];
277        let from = [0.0; 6];
278        let to = [2.0 * PI; 6];
279        let limits = Constraints::new(from, to, BY_CONSTRAINS);
280        assert!(limits.compliant(&angles));
281    }
282
283    #[test]
284    fn test_invalid_angles_no_wrap_around() {
285        let angles = [0.15 * PI, 0.25 * PI, 0.55 * PI, 0.65 * PI, 0.75 * PI, 0.85 * PI];
286        let from = [0.2 * PI, 0.3 * PI, 0.6 * PI, 0.7 * PI, 0.8 * PI, 0.9 * PI];
287        let to = [0.1 * PI, 0.2 * PI, 0.5 * PI, 0.6 * PI, 0.7 * PI, 0.8 * PI];
288        let limits = Constraints::new(from, to, BY_CONSTRAINS);
289        assert!(!limits.compliant(&angles));
290    }
291
292    #[test]
293    fn test_invalid_angles_with_wrap_around() {
294        let angles = [0.8 * PI, 1.8 * PI, 1.0 * PI, 0.0, 2.1 * PI, 1.1 * PI];
295        let from = [0.9 * PI, 2.0 * PI, 0.1 * PI, 0.2 * PI, 2.2 * PI, 1.2 * PI];
296        let to = [0.0, 1.0 * PI, 0.05 * PI, 0.1 * PI, 2.0 * PI, 1.0 * PI];
297        let limits = Constraints::new(from, to, BY_CONSTRAINS);
298        assert!(!limits.compliant(&angles));
299    }
300
301    #[test]
302    fn test_filter_angles() {
303        let from = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0];
304        let to = [PI / 2.0, PI / 2.0, PI / 2.0, PI / 2.0, PI / 2.0, PI / 2.0];
305        let angles = vec![
306            [PI / 3.0, PI / 4.0, PI / 6.0, PI / 3.0, PI / 4.0, PI / 6.0], // Should be retained
307            [PI, 2.0 * PI, PI, PI, PI, PI], // Should be removed
308        ];
309
310        let limits = Constraints::new(from, to, BY_CONSTRAINS);
311        let filtered_angles = limits.filter(&angles);
312        assert_eq!(filtered_angles.len(), 1);
313        assert_eq!(filtered_angles[0], [PI / 3.0, PI / 4.0, PI / 6.0, PI / 3.0, PI / 4.0, PI / 6.0]);
314    }
315
316    #[test]
317    fn test_random_angles_compliance_non_wrapping() {
318        // Define non-wrapping constraints
319        let from = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0];
320        let to = [PI / 2.0, PI / 2.0, PI / 2.0, PI / 2.0, PI / 2.0, PI / 2.0];
321        let constraints = Constraints::new(from, to, BY_CONSTRAINS);
322
323        let total_samples = 360;
324        for _ in 0..total_samples {
325            let random_angles = constraints.random_angles();
326            assert!(constraints.compliant(&random_angles));
327        }
328    }
329
330    #[test]
331    fn test_random_angles_compliance_wrapping() {
332        // Define wrapping constraints
333        let from = [PI / 2.0, 0.0, -PI / 2.0, 0.0, -PI, -PI];
334        let to = [0.0, PI / 2.0, PI / 2.0, PI, PI / 2.0, 0.0];
335        let constraints = Constraints::new(from, to, BY_CONSTRAINS);
336
337        let total_samples = 360;
338        for _ in 0..total_samples {
339            let random_angles = constraints.random_angles();
340            assert!(constraints.compliant(&random_angles));
341        }
342    }
343}
344
rs_opw_kinematics/constraints.rs

rs_opw_kinematics/
constraints.rs
