rspp 0.1.7

rust probolistic programming.
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
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191

extern crate nalgebra as na;
use na::*;
use rand::Rng;
use rayon::vec;
use statrs::distribution;
use std::f64::consts::PI;
///原文链接：https://blog.csdn.net/weixin_62891964/article/details/126839894
///标准D-H参数（SDH）  z是关节旋转轴,下一个x_i垂直相交上一个z_i-1
///
///关节角theta_i：绕轴z_i-1，x_i-1旋转到x_i 的角度；[-pi,pi]
///关节距离d_i     从轴z_i-1，x_i-1移动到xi 的距离； zi-1正方向

///连杆长度a_i：    从轴𝑥𝑖，z_i-1移动到zi的距离； xi正方向

///连杆扭角αlpha_𝑖：绕轴𝑥𝑖，z_i-1旋转到zi的角度 [-pi,pi]

#[derive(Debug)]
pub struct DH {
    pub theta: f64,
    pub alpha: f64,
    pub a: f64,
    pub d: f64,
}

/*

*/
impl DH {
    pub fn trans_mat(&self) -> Matrix4<f64> {
        let st = self.theta.sin();
        let ct = self.theta.cos();
        let sa = self.alpha.sin();
        let ca = self.alpha.cos();
        let tr = Matrix4::new(
            ct,
            -st * ca,
            st * sa,
            self.a * ct,
            st,
            ct * ca,
            -ct * sa,
            self.a * st,
            0.,
            sa,
            ca,
            self.d,
            0.,
            0.,
            0.,
            1.,
        )
        .transpose();
        tr
    }
    pub fn basic_jacobian(&self, trans_prev: Matrix4<f64>, xyz: Matrix3x1<f64>) -> Vec<f64> {
        let pos_prev = Matrix3x1::new(trans_prev[(3, 0)], trans_prev[(3, 1)], trans_prev[(3, 2)]);
        let z_prev = Vector3::new(trans_prev[(2, 0)], trans_prev[(2, 1)], trans_prev[(2, 2)]);
        let dif = xyz - pos_prev;
        let mut c = z_prev.cross(&dif).as_slice().to_owned();
        c.push(z_prev.x);
        c.push(z_prev.y);
        c.push(z_prev.z);
        c
    }
}
/*
    def basic_jacobian(trans_prev, ee_pos):
        print ("arg==",trans_prev,ee_pos)
        pos_prev = np.array(
            [trans_prev[0, 3], trans_prev[1, 3], trans_prev[2, 3]])
        z_axis_prev = np.array(
            [trans_prev[0, 2], trans_prev[1, 2], trans_prev[2, 2]])

        basic_jacobian = np.hstack(
            (np.cross(z_axis_prev, ee_pos - pos_prev), z_axis_prev))

        print (111,pos_prev,z_axis_prev,basic_jacobian)
        print (222,np.cross(z_axis_prev, ee_pos - pos_prev))
        return basic_jacobian
*/
fn xyz(m: &Matrix4<f64>) -> (f64, f64, f64) {
    // !!!!!!!!!!! rs 的行列索引和numpy反着来的
    let x = m[(3, 0)];
    let y = m[(3, 1)];
    let z = m[(3, 2)];
    (x, y, z)
}
fn euler_angle(m: &Matrix4<f64>) -> (f64, f64, f64) {
    let mut alpha = m[(2, 1)].atan2(m[(2, 0)]);
    if !(alpha >= -PI / 2.0 && alpha <= PI / 2.0) {
        alpha = m[(2, 1)].atan2(m[(2, 0)]) + PI;
    }
    if !(alpha >= -PI / 2.0 && alpha <= PI / 2.0) {
        alpha = m[(2, 1)].atan2(m[(2, 0)]) - PI;
    }
    let beta_y = m[(2, 0)] * alpha.cos() + m[(2, 1)] * alpha.sin();
    let beta_x = m[(2, 2)];
    let beta = beta_y.atan2(beta_x);
    let gama_y = -m[(0, 0)] * alpha.sin() + m[(0, 1)] * alpha.cos();
    let gama_x = -m[(1, 0)] * alpha.sin() + m[(1, 1)] * alpha.cos();
    let gama = gama_y.atan2(gama_x);
    (alpha, beta, gama)
}
pub struct DHS(Vec<DH>);
impl DHS {
    pub fn from_root_to_end(&self) -> Matrix4<f64> {
        let mut m = Matrix4::<f64>::identity();
        for i in &self.0 {
            let tr = i.trans_mat();
            // !!!!!!!!!! trans is before
            m = tr * m;
        }
        m
    }
    pub fn forward(&self) -> Vec<f64> {
        let m = self.from_root_to_end();
        let p = xyz(&m);
        let euler = euler_angle(&m);
        vec![p.0, p.1, p.2, euler.0, euler.1, euler.2]
    }
    pub fn jacobian(&self) -> Vec<Vec<f64>> {
        let cur = self.forward();
        let xyz = Matrix3x1::new(cur[0], cur[1], cur[2]);
        let mut jac_mat_list = Vec::new();
        let mut trans = Matrix4::<f64>::identity();
        for link in &self.0 {
            let jac = link.basic_jacobian(trans, xyz);
            jac_mat_list.push(jac);
            let tr = link.trans_mat();
            trans = tr * trans;
        }
        jac_mat_list
    }
    /// one round
    pub fn inverse(&mut self, tar_pos: &Vec<f64>) -> Vec<f64> {
        let cur = self.forward();
        let dif: Vec<f64> = tar_pos.into_iter().zip(cur).map(|v| v.0 - v.1).collect();
        let jac = self.jacobian();
        let trans = self.from_root_to_end();
        let eu = euler_angle(&trans);
        dbg!(jac, eu);
        let l_zyz = vec![
            0.,
            eu.0.sin(),
            eu.0.cos() * eu.1.sin(),
            0.,
            eu.0.cos(),
            eu.0.sin() * eu.1.sin(),
            1.,
            0.,
            eu.1.cos(),
        ];
        let K_zyz = Matrix3::from_vec(l_zyz);
        dbg!(K_zyz);
        vec![0.]
    }
}

#[test]
fn test_f_kina() {
    let dh1 = DH {
        theta: 0.,
        alpha: -PI / 2.,
        a: 0.1,
        d: 0.,
    };
    let dh2 = DH {
        theta: 0.,
        alpha: PI / 2.,
        a: 0.,
        d: 0.,
    };
    let dh3 = DH {
        theta: 0.,
        alpha: 0.,
        a: 0.,
        d: 0.,
    };
    let target = vec![
        0.4615526833399779,
        -0.10358889419961226,
        -0.4060298761019099,
        -0.44900959684297026,
        -0.04790287716219266,
        0.17072276427146582,
    ];
    let l = vec![dh1, dh2, dh3];
    let mut dhs = DHS(l);
    let res = dhs.inverse(&target);
    dbg!(111111, res);
}