openrr_planner/
funcs.rs

Help
1/*
2Copyright 2017 Takashi Ogura
3
4Licensed under the Apache License, Version 2.0 (the "License");
5you may not use this file except in compliance with the License.
6You may obtain a copy of the License at
7
8    http://www.apache.org/licenses/LICENSE-2.0
9
10Unless required by applicable law or agreed to in writing, software
11distributed under the License is distributed on an "AS IS" BASIS,
12WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13See the License for the specific language governing permissions and
14limitations under the License.
15*/
16#![allow(clippy::trivially_copy_pass_by_ref, clippy::ptr_arg)]
17
18use std::f64::consts::PI;
19
20use k::{nalgebra as na, nalgebra::RealField};
21use num_traits::Float;
22use trajectory::{CubicSpline, Trajectory};
23
24use crate::errors::*;
25
26type Limits<T> = Vec<Option<k::joint::Range<T>>>;
27
28/// Clamp joint angles to set angles safely
29pub fn generate_clamped_joint_positions_from_limits<T>(
30    angles: &[T],
31    limits: &Limits<T>,
32) -> Result<Vec<T>>
33where
34    T: RealField + Copy,
35{
36    if angles.len() != limits.len() {
37        return Err(Error::DofMismatch(angles.len(), limits.len()));
38    }
39    Ok(limits
40        .iter()
41        .zip(angles.iter())
42        .map(|(range, angle)| match *range {
43            Some(ref range) => {
44                if *angle > range.max {
45                    range.max
46                } else if *angle < range.min {
47                    range.min
48                } else {
49                    *angle
50                }
51            }
52            None => *angle,
53        })
54        .collect())
55}
56
57#[deprecated(
58    since = "0.7.0",
59    note = "Please use k::Chain::set_joint_positions_clamped"
60)]
61/// Set joint positions safely
62///
63/// The input vec is clamped to the limits.
64pub fn set_clamped_joint_positions<T>(chain: &k::Chain<T>, vec: &[T]) -> Result<()>
65where
66    T: RealField + Copy + k::SubsetOf<f64>,
67{
68    let limits = chain.iter_joints().map(|j| j.limits).collect::<Vec<_>>();
69    let clamped = generate_clamped_joint_positions_from_limits(vec, &limits)?;
70    chain.set_joint_positions(&clamped)?;
71    Ok(())
72}
73
74/// Generate random joint angles from the optional limits
75///
76/// If the limit is None, -PI <-> PI is used.
77pub fn generate_random_joint_positions_from_limits<T>(limits: &Limits<T>) -> Vec<T>
78where
79    T: RealField + Copy,
80{
81    limits
82        .iter()
83        .map(|range| match range {
84            Some(range) => (range.max - range.min) * na::convert(rand::random()) + range.min,
85            None => na::convert::<f64, T>(rand::random::<f64>() - 0.5) * na::convert(2.0 * PI),
86        })
87        .collect()
88}
89
90/// If the joint has no limit, select the nearest value from (x + 2pi *).
91///
92/// ```
93/// let mut a = vec![0.1f64, 10.0];
94/// let limits = vec![Some(k::joint::Range::new(0.0, 0.2)), None];
95/// openrr_planner::modify_to_nearest_angle(&vec![1.0, 0.5], &mut a, &limits);
96/// assert_eq!(a[0], 0.1, "no change");
97/// assert!((a[1] - 3.716814).abs() < 0.000001);
98/// ```
99#[track_caller]
100pub fn modify_to_nearest_angle<T>(vec1: &[T], vec2: &mut [T], limits: &Limits<T>)
101where
102    T: RealField + Copy,
103{
104    assert_eq!(vec1.len(), vec2.len());
105    for i in 0..vec1.len() {
106        if limits[i].is_none() {
107            // TODO: deal not only no limit
108            let pi2 = T::pi() * na::convert(2.0);
109            let dist1 = (vec1[i] - vec2[i]).abs();
110            let dist2 = (vec1[i] - (vec2[i] - pi2)).abs();
111            if dist1 > dist2 {
112                vec2[i] -= pi2;
113            } else {
114                let dist3 = (vec1[i] - (vec2[i] + pi2)).abs();
115                if dist1 > dist3 {
116                    vec2[i] += pi2;
117                }
118            }
119        }
120    }
121}
122
123/// Struct for a point of a trajectory with multiple dimensions.
124#[derive(Debug, Clone)]
125pub struct TrajectoryPoint<T> {
126    pub position: Vec<T>,
127    pub velocity: Vec<T>,
128    pub acceleration: Vec<T>,
129}
130
131impl<T> TrajectoryPoint<T> {
132    /// Create trajectory point
133    pub fn new(position: Vec<T>, velocity: Vec<T>, acceleration: Vec<T>) -> Self {
134        Self {
135            position,
136            velocity,
137            acceleration,
138        }
139    }
140}
141
142/// Interpolate position vectors
143///
144/// returns vector of (position, velocity, acceleration)
145///
146/// # Example
147///
148/// ```
149/// let points = openrr_planner::interpolate(&[vec![0.0, 1.0], vec![2.0, 0.0]], 1.0, 0.1).unwrap();
150/// assert_eq!(points.len(), 12);
151/// assert_eq!(points[0].position[0], 0.0);
152/// assert_eq!(points[0].position[1], 1.0);
153/// assert_eq!(points[1].position[0], 0.2);
154/// assert_eq!(points[1].position[1], 0.9);
155/// ```
156pub fn interpolate<T>(
157    points: &[Vec<T>],
158    total_duration: T,
159    unit_duration: T,
160) -> Option<Vec<TrajectoryPoint<T>>>
161where
162    T: Float,
163{
164    let key_frame_unit_duration = total_duration / (T::from(points.len())? - T::one());
165    let times = (0_usize..points.len())
166        .map(|i| T::from(i).unwrap() * key_frame_unit_duration)
167        .collect::<Vec<T>>();
168    assert_eq!(times.len(), points.len());
169
170    let spline = CubicSpline::new(times, points.to_vec())?;
171    let mut t = T::zero();
172    let mut ret = Vec::with_capacity(points.len());
173    while t < total_duration {
174        ret.push(TrajectoryPoint {
175            position: spline.position(t)?,
176            velocity: spline.velocity(t)?,
177            acceleration: spline.acceleration(t)?,
178        });
179        t = t + unit_duration;
180    }
181    // Add final point
182    ret.push(TrajectoryPoint {
183        position: spline.position(total_duration)?,
184        velocity: spline.velocity(total_duration)?,
185        acceleration: spline.acceleration(total_duration)?,
186    });
187    Some(ret)
188}
189
190/// Set random joint angles
191pub fn set_random_joint_positions<T>(robot: &k::Chain<T>) -> ::std::result::Result<(), k::Error>
192where
193    T: RealField + Copy + k::SubsetOf<f64>,
194{
195    let limits = robot.iter_joints().map(|j| j.limits).collect();
196    robot.set_joint_positions(&generate_random_joint_positions_from_limits(&limits))
197}
198
199/// Create a sub-chain of the collision check model by a name list
200pub fn create_chain_from_joint_names<N>(
201    robot: &k::Chain<N>,
202    joint_names: &[String],
203) -> Result<k::Chain<N>>
204where
205    N: RealField + num_traits::Float + k::SubsetOf<f64>,
206{
207    let nodes = joint_names
208        .iter()
209        .map(|joint_name| {
210            robot
211                .find(joint_name)
212                .ok_or_else(|| Error::NotFound(joint_name.to_owned()))
213                .unwrap()
214                .clone()
215        })
216        .collect::<Vec<k::Node<N>>>();
217    Ok(k::Chain::from_nodes(nodes))
218}
openrr_planner/funcs.rs

openrr_planner/
funcs.rs
