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
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135

use std::f64::consts::PI;

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

use crate::{ik::InverseKinematicsError::UnreachableTarget, spyder::LegParams};

#[derive(Error, Debug)]
pub enum InverseKinematicsError {
    #[error("Invalid target x: {0}, y: {1}, z: {2}")]
    InvalidTarget(f64, f64, f64),

    #[error("Unreachable target x: {0}, y: {1}, z: {2}, max: {3}, min: {4},  r1: {5}")]
    UnreachableTarget(f64, f64, f64, f64, f64, f64),

    #[error("Invalid link lengths l1: {0}, l2: {1}, l3: {2}")]
    InvalidGeometry(f64, f64, f64),

    #[error("Target lies on coxa axis xp: {0}, yp: {0}")]
    CoxaSingularity(f64, f64),

    #[error("Target lies on femur joint r2: {0} zp: {1}")]
    FemurSingularity(f64, f64),

    #[error("Link lengths differ by too many orders of magnitude l2: {0}, l3: {1}")]
    LinkLength(f64, f64),

    #[error("Joint angles out of range: coxa: {0}, femur: {1}, tibia: {2}")]
    OutOfRange(f64, f64, f64),
}

pub fn inv(
    params: &LegParams,
    xp: f64,
    yp: f64,
    zp: f64,
) -> Result<Vector3<f64>, InverseKinematicsError> {
    // validate parameters
    if !(xp.is_finite() && yp.is_finite() && zp.is_finite()) {
        return Err(InverseKinematicsError::InvalidTarget(xp, yp, zp));
    }

    if !(params.l1.is_normal() && params.l2.is_normal() && params.l3.is_normal()) {
        return Err(InverseKinematicsError::InvalidGeometry(
            params.l1, params.l2, params.l3,
        ));
    }

    // Reach limits
    let reach_max = params.l2 + params.l3;
    let reach_min = f64::abs(params.l2 - params.l3);

    // horizontal displacement from origin to target
    let delta_x = f64::hypot(xp, yp);
    // account for coxa singularity
    if !delta_x.is_normal() {
        return Err(InverseKinematicsError::CoxaSingularity(xp, yp));
    }

    // horizontal reach from femur joint to foot
    let r2 = delta_x - params.l1;

    // coxa yaw
    let theta1 = yp.atan2(xp);

    // femur joint to foot
    let r1 = f64::hypot(r2, zp);
    // femur singularity
    if !r1.is_normal() {
        return Err(InverseKinematicsError::FemurSingularity(r2, zp));
    }

    // validate if target is reachable
    if reach_max < r1 || reach_min > r1 {
        return Err(UnreachableTarget(xp, yp, zp, reach_max, reach_min, r1));
    }

    // femur joint angle
    let alpha = zp.atan2(r2);
    let beta = ((params.l2.powf(2.0) + r1.powf(2.0) - params.l3.powf(2.0))
        / (2.0 * params.l2 * r1))
        .clamp(-1.0, 1.0)
        .acos();

    let theta2 = alpha + beta;

    // tibia joint angle
    let phi = ((params.l2.powf(2.0) + params.l3.powf(2.0) - r1.powf(2.0))
        / (2.0 * params.l2 * params.l3))
        .clamp(-1.0, 1.0)
        .acos();
    let theta3 = phi - PI;

    // If beta or phi is out of range the link lengths differ by too many
    // orders of magnitude
    if !(beta.is_finite() && phi.is_finite()) {
        return Err(InverseKinematicsError::LinkLength(params.l2, params.l3));
    }

    // guard against NaN and infinite for joint angles
    if !(theta1.is_finite() && theta2.is_finite() && theta3.is_finite()) {
        return Err(InverseKinematicsError::OutOfRange(theta1, theta2, theta3));
    }

    Ok(Vector3::new(theta1, theta2, theta3))
}

#[cfg(test)]
mod tests {
    use super::*;

    #[test]
    fn test_valid_target() {
        let leg_params = LegParams {
            l1: 42.25,
            l2: 120.0,
            l3: 180.0,
        };
        let res = inv(&leg_params, 0.0, 42.35294117647058, 186.36678200692043).unwrap();
        assert_eq!(format!("{:.2}", res.x), "1.57".to_string());
        assert_eq!(format!("{:.2}", res.y), "2.76".to_string());
        assert_eq!(format!("{:.2}", res.z), "-1.85".to_string());
    }

    #[test]
    #[should_panic]
    fn test_invalid_target() {
        let leg_params = LegParams {
            l1: 42.25,
            l2: 120.0,
            l3: 180.0,
        };
        inv(&leg_params, 0.0, 200.0, 300.0).unwrap();
    }
}