posture 0.1.0

positional posture of spatial robot
Documentation 
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73

use core::f64::consts::PI;
use core::ops::{Deref, DerefMut};
use serde::{Deserialize, Serialize};

#[derive(Debug, Clone, Serialize, Deserialize)]
pub struct JointPose(pub Vec<f64>);
impl JointPose {
    pub fn new(num: usize) -> Self {
        Self(vec![0.0; num])
    }
    pub fn into_inner(self) -> Vec<f64> {
        self.0
    }
    /// 根据normal位置增减圈数，并返回增减的圈数
    pub fn near_to(&mut self, normal: &Self) -> Option<Vec<i32>> {
        let mut changed = false;
        let len = self.len().min(normal.len());
        let mut ret = Vec::with_capacity(len);
        for i in 0..len {
            ret.push(0);
            while (self[i] - normal[i]).abs() > 2.0 * PI {
                changed = true;
                if self[i] > normal[i] {
                    self[i] -= 2.0 * PI;
                    ret[i] -= 1;
                } else {
                    self[i] += 2.0 * PI;
                    ret[i] += 1;
                }
            }
        }
        if changed {
            Some(ret)
        } else {
            None
        }
    }
}

impl From<Vec<f64>> for JointPose {
    fn from(item: Vec<f64>) -> Self {
        Self(item)
    }
}
impl From<JointPose> for Vec<f64> {
    fn from(item: JointPose) -> Self {
        item.0
    }
}
impl PartialEq for JointPose {
    fn eq(&self, other: &Self) -> bool {
        let neq = self
            .iter()
            .zip(other.iter())
            .find(|(a, b)| (**a - **b).abs() >= 1e-3);
        if neq.is_none() {
            true
        } else {
            false
        }
    }
}
impl Deref for JointPose {
    type Target = Vec<f64>;
    fn deref(&self) -> &Self::Target {
        &self.0
    }
}
impl DerefMut for JointPose {
    fn deref_mut(&mut self) -> &mut Self::Target {
        &mut self.0
    }
}