kidy/dynamics/
mod.rs

1use crate::KidyChain;
2use liealg::{prelude::*, se3};
3use nalgebra::{Matrix6, Vector3, Vector6};
4
5impl<const N: usize> KidyChain<N> {
6    // tau = M(q) * q_dot_dot + C(q, q_dot) * q_dot + G(q)
7    pub fn id(
8        &self,
9        theta: [f64; N],
10        dtheta: [f64; N],
11        ddtheta: [f64; N],
12        gravity: [f64; 3],
13        f_tip: se3<f64>,
14    ) -> [f64; N] {
15        let mut twists = vec![se3::new([0.0, 0.0, 0.0], [0.0, 0.0, 0.0]); N + 1];
16        let mut dtwists = vec![se3::new([0.0, 0.0, 0.0], [0.0, 0.0, 0.0]); N + 1];
17
18        // add gravity
19        let g = se3::new([0.0, 0.0, 0.0], gravity);
20        dtwists[0] = g;
21
22        // i^T_i-1
23        for i in 1..N + 1 {
24            // i^T_i-1: transformation from i to i-1
25            let t = (self.local_screw[i - 1].clone() * -theta[i - 1]).exp()
26                * self.local_zero_pose[i].clone().inv();
27
28            // V_i: twist of link i
29            let v =
30                t.adjoint().act(&twists[i - 1]) + self.local_screw[i - 1].clone() * dtheta[i - 1];
31            twists[i] = v.clone();
32
33            // dV_i: acceleration of link i
34            let dv = t.adjoint().act(&dtwists[i - 1])
35                + ad_mul_se(ad(&v), &self.local_screw[i - 1]) * dtheta[i - 1]
36                + self.local_screw[i - 1].clone() * ddtheta[i - 1];
37            dtwists[i] = dv;
38        }
39
40        let mut wrenchs = vec![se3::new([0.0, 0.0, 0.0], [0.0, 0.0, 0.0]); N + 1];
41        wrenchs[N] = f_tip;
42        let mut tau = [0.0; N];
43        for i in (1..N + 1).rev() {
44            let t = (self.local_screw[i].clone() * -theta[i - 1]).exp()
45                * self.local_zero_pose[i + 1].clone().inv();
46            wrenchs[i] = t.adjoint().transpose().act(&wrenchs[i + 1])
47                + inertial_mul_se3(self.local_spatial_inertial[i], &dtwists[i])
48                - ad_mul_se(ad(&twists[i]).transpose(), &twists[i]);
49            tau[i - 1] = cross_se3(&wrenchs[i], &self.local_screw[i - 1]);
50        }
51        tau
52    }
53
54    // M(q)^-1 * (tau - C(q, q_dot) * q_dot - G(q)) = q_dot_dot
55    pub fn fd(&self) {
56        println!("KidyChain fd");
57    }
58}
59
60fn ad(v: &se3<f64>) -> Matrix6<f64> {
61    let v = v.vee().as_array();
62    let p = Vector3::new(v[0], v[1], v[2]);
63    let w = Vector3::new(v[3], v[4], v[5]);
64    let a = liealg::hat(&p);
65    let b = liealg::hat(&w);
66    let mut res = Matrix6::zeros();
67    res.view_mut((0, 0), (3, 3)).copy_from(&a);
68    res.view_mut((3, 3), (3, 3)).copy_from(&a);
69    res.view_mut((3, 0), (3, 3)).copy_from(&b);
70    res
71}
72
73fn ad_mul_se(m: Matrix6<f64>, w: &se3<f64>) -> se3<f64> {
74    let vec6 = Vector6::from_column_slice(w.vee().as_slice());
75    let res = m * vec6;
76    se3::new([res[0], res[1], res[2]], [res[3], res[4], res[5]])
77}
78
79fn inertial_mul_se3(g: Matrix6<f64>, v: &se3<f64>) -> se3<f64> {
80    let vec6 = Vector6::from_column_slice(v.vee().as_slice());
81    let res = g * vec6;
82    se3::new([res[0], res[1], res[2]], [res[3], res[4], res[5]])
83}
84
85fn cross_se3(v: &se3<f64>, w: &se3<f64>) -> f64 {
86    let vec_v = Vector6::from_column_slice(v.vee().as_slice());
87    let vec_w = Vector6::from_column_slice(w.vee().as_slice());
88    vec_v.dot(&vec_w)
89}
90
91#[cfg(test)]
92mod test {
93    // use super::*;
94
95    #[test]
96    fn test_ad() {
97        // let v = se3::new([1.0, 2.0, 3.0], [4.0, 5.0, 6.0]);
98        // let res = ad(v);
99        // println!("{}", res);
100    }
101}
kidy/dynamics/mod.rs

kidy/dynamics/
mod.rs
