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

use nalgebra::Vector3;
use thiserror::Error;

#[derive(Error, Debug)]
pub enum BezierError {
    #[error("Data points must not be zero - got: {0}")]
    InvalidNPoints(u64),

    #[error("Unreachable data point - y_pos: {0}, z_pos: {1}")]
    Unreachable(f64, f64),
}

#[derive(Clone, Debug)]
pub struct Bezier {
    pub pos_vec: Vec<Vector3<f64>>,
}

#[derive(Clone, Default, Debug)]
pub struct Vec2D {
    pub x: f64,
    pub y: f64,
}

impl std::fmt::Display for Vec2D {
    fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
        write!(f, "x: {0}, y: {1}", self.x, self.y)
    }
}

#[derive(Clone, Default, Debug)]
pub struct BezierBuilder {
    pub p0: Vec2D,
    pub p1: Vec2D,
    pub p2: Vec2D,
    pub npoints: Option<u64>,
}

impl BezierBuilder {
    pub fn ctrl_pts(mut self, p0: Vec2D, p1: Vec2D, p2: Vec2D) -> Self {
        self.p0 = p0;
        self.p1 = p1;
        self.p2 = p2;
        self
    }

    pub fn npoints(mut self, npoints: u64) -> Self {
        self.npoints = Some(npoints);
        self
    }

    pub fn build(self) -> Result<Bezier, BezierError> {
        let Some(npoints) = self.npoints else {
            return Err(BezierError::InvalidNPoints(self.npoints.unwrap()));
        };

        let t_step: f64 = 1.0 / (npoints - 1) as f64;

        let mut t: Vec<f64> = Vec::new();
        for i in 0..npoints {
            t.push(i as f64 * t_step)
        }

        let pos_vec = t
            .iter()
            .map(|t| {
                let y_pos: f64 = ((1.0 - t).powf(2.0) * self.p0.x)
                    + ((2.0 * (1.0 - t) * t) * self.p1.x)
                    + (t.powf(2.0) * self.p2.x);

                let z_pos: f64 = ((1.0 - t).powf(2.0) * self.p0.y)
                    + ((2.0 * (1.0 - t) * t) * self.p1.y)
                    + (t.powf(2.0) * self.p2.y);

                if !(y_pos.is_finite() && z_pos.is_finite()) {
                    return Err(BezierError::Unreachable(y_pos, z_pos));
                }

                Ok(Vector3::new(0.0, y_pos, z_pos))
            })
            .collect::<Result<Vec<Vector3<f64>>, BezierError>>()?;

        Ok(Bezier { pos_vec })
    }
}