math_functions.rs - source
Description: Source of the Rust file `src/math_functions.rs`.
Keywords: rust, rustlang, rust-lang

    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
    }