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

extern crate nalgebra as na;

use na::{SMatrix, OMatrix, OVector, Vector3, Vector6, Matrix3, Matrix6, Isometry3, U6, Dyn};

use core::ops::Mul;
fn test_scalar_mult<T>(a: T, b: Vector3<T>) -> Vector3<T> 
where 
T: na::Scalar + num_traits::Zero + num_traits::One + na::ClosedAdd + na::ClosedMul , {
    b.mul(a)
}


pub fn Ad_inv(h: &Isometry3<f64>) -> Matrix6<f64> {
    let mut Ad_h_inv = Matrix6::zeros();
    // let R_inv = h.rotation.to_rotation_matrix().matrix().transpose();
    // let R_inv = h.rotation.to_rotation_matrix()
    let h_inv = h.inverse().to_homogeneous();
    let R_inv = h_inv.fixed_view::<3, 3>(0, 0);
    let p: Vector3<f64> = h_inv.fixed_view::<3, 1>(0, 3).try_into().unwrap();

    Ad_h_inv.fixed_view_mut::<3, 3>(0, 0).copy_from(&R_inv);

    Ad_h_inv
        .fixed_view_mut::<3, 3>(0, 3)
        .copy_from(&(skew(&p) * &R_inv));
    Ad_h_inv.fixed_view_mut::<3, 3>(3, 3).copy_from(&R_inv);
    Ad_h_inv
}

pub fn Ad(h: &Isometry3<f64>) -> Matrix6<f64> {
    let mut Ad_h = Matrix6::zeros();
    let h = h.to_homogeneous();
    let R = h.fixed_view::<3, 3>(0, 0);
    let p: Vector3<f64> = h.fixed_view::<3, 1>(0, 3).try_into().unwrap();

    Ad_h.fixed_view_mut::<3, 3>(0, 0).copy_from(&R);

    Ad_h
        .fixed_view_mut::<3, 3>(0, 3)
        .copy_from(&(skew(&p) * &R));
    Ad_h.fixed_view_mut::<3, 3>(3, 3).copy_from(&R);
    Ad_h
}

pub fn skew<T: na::RealField + Copy>(v: &Vector3<T>) -> Matrix3<T> {
    let mut skew = Matrix3::<T>::zeros();
    skew[(0, 1)] = -v[2];
    skew[(0, 2)] = v[1];
    skew[(1, 0)] = v[2];
    skew[(1, 2)] = -v[0];
    skew[(2, 0)] = -v[1];
    skew[(2, 1)] = v[0];
    skew
}

pub fn ad_se3(v: &Vector6<f64>) -> SMatrix<f64, 6, 6> {
    let mut ad = SMatrix::<f64, 6, 6>::zeros();
    let lin_vel = v.fixed_view::<3, 1>(0, 0).try_into().unwrap();
    let ang_vel = v.fixed_view::<3, 1>(3, 0).try_into().unwrap();

    ad.fixed_view_mut::<3, 3>(0, 0).copy_from(&skew(&ang_vel));
    ad.fixed_view_mut::<3, 3>(0, 3).copy_from(&skew(&lin_vel));
    ad.fixed_view_mut::<3, 3>(3, 3).copy_from(&skew(&ang_vel));

    ad
}

pub fn ad_se3_dyn(v: &OVector<f64, Dyn>) -> OMatrix<f64, Dyn, Dyn> {
    let mut ad = OMatrix::<f64, Dyn, Dyn>::zeros(6, 6);
    // let mut ad = OMatrix::<f64>::zeros(6, 6);
    // let lin_vel = v.fixed_view::<3, 1>(0, 0).try_into().unwrap();
    // let ang_vel = v.fixed_view::<3, 1>(3, 0).try_into().unwrap();
    let lin_vel = v.rows(0, 3).try_into().unwrap();
    let ang_vel = v.rows(3, 3).try_into().unwrap();

    // ad.fixed_view_mut::<3, 3>(0, 0).copy_from(&skew(&ang_vel));
    // ad.fixed_view_mut::<3, 3>(0, 3).copy_from(&skew(&lin_vel));
    // ad.fixed_view_mut::<3, 3>(3, 3).copy_from(&skew(&ang_vel));
    ad.view_mut((0, 0), (3, 3)).copy_from(&skew_dyn(&ang_vel));
    ad.view_mut((0, 3), (3, 3)).copy_from(&skew_dyn(&lin_vel));
    ad.view_mut((3, 3), (3, 3)).copy_from(&skew_dyn(&ang_vel));

    ad
}

fn skew_dyn(v: &OVector<f64, Dyn>) -> Matrix3<f64> {
    let mut skew = Matrix3::zeros();
    skew[(0, 1)] = -v[2];
    skew[(0, 2)] = v[1];
    skew[(1, 0)] = v[2];
    skew[(1, 2)] = -v[0];
    skew[(2, 0)] = -v[1];
    skew[(2, 1)] = v[0];
    skew
}

pub fn comp_rb_mass_matrix(m: f64, r: &Vector3<f64>, inertia_mat: &Matrix3<f64>) -> Matrix6<f64> {
    let mut mass_matrix = Matrix6::zeros();
    mass_matrix
        .fixed_view_mut::<3, 3>(0, 0)
        .copy_from(&(m * Matrix3::identity()));
    mass_matrix
        .fixed_view_mut::<3, 3>(0, 3)
        .copy_from(&(-m * skew(&r)));
    mass_matrix
        .fixed_view_mut::<3, 3>(3, 0)
        .copy_from(&(m * skew(&r)));
    mass_matrix
        .fixed_view_mut::<3, 3>(3, 3)
        .copy_from(&inertia_mat);
    mass_matrix
}