spyder-ik 0.1.0

Motion planner for hexapod robots
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
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90

use nalgebra::{Rotation3, Vector3};

#[derive(Clone, Debug)]
pub enum LegId {
    Group1 { id: u64 },
    Group2 { id: u64 },
}

impl LegId {
    pub fn id(&self) -> usize {
        match self {
            LegId::Group1 { id } => *id as usize,
            LegId::Group2 { id } => *id as usize,
        }
    }
}

#[derive(Clone, Debug)]
pub struct Leg {
    pub id: LegId,
    pub heading: Option<f64>,
    pub home: Vector3<f64>,
}

impl Leg {
    // set heading per leg
    pub fn set_heading(&mut self, heading: f64) {
        self.heading = Some(heading)
    }

    pub fn transform(
        // apply a rotation + heading if applicable
        &self,
        mut path: Vec<Vector3<f64>>,
        cw_dir_ang: f64,
    ) -> Vec<Vector3<f64>> {
        let rot = cw_dir_ang.to_radians() + self.heading.unwrap_or_default();

        let rot_matrix = Rotation3::from_axis_angle(&Vector3::z_axis(), rot);

        for pt in &mut path {
            // apply rotation
            *pt = rot_matrix * *pt;
            // centre on target
            pt.x += self.home.x;
            pt.y += self.home.y;
        }
        path
    }
}

#[cfg(test)]
pub mod tests {
    use std::f64::consts::PI;

    use super::*;

    pub fn mock_leg() -> Leg {
        Leg {
            id: LegId::Group2 { id: 1 },
            heading: None,
            home: Vector3::new(160.0, 0.0, 100.0),
        }
    }

    #[test]
    fn test_transform() {
        let mut leg = mock_leg();
        leg.set_heading(-PI / 3.0);

        let mut path: Vec<Vector3<f64>> = vec![];
        path.push(Vector3::new(0.0, -9.411764705882348, 100.0));
        path.push(Vector3::new(0.0, -14.117647058823536, 100.0));
        path.push(Vector3::new(0.0, -18.82352941176471, 100.0));

        path = leg.transform(path, 0.0);
        assert_eq!(
            path[0],
            Vector3::new(151.84917267026412, -4.705882352941175, 100.0)
        );
        assert_eq!(
            path[1],
            Vector3::new(147.77375900539616, -7.05882352941177, 100.0)
        );
        assert_eq!(
            path[2],
            Vector3::new(143.69834534052822, -9.411764705882357, 100.0)
        )
    }
}