multibody_dynamics/
math_functions.rs

1#![allow(non_snake_case)]
2#![allow(dead_code)]
3extern crate nalgebra as na;
4
5use na::{Dyn, Isometry3, Matrix3, Matrix6, OMatrix, OVector, SMatrix, Vector3, Vector6};
6
7pub fn Ad_inv(h: &Isometry3<f64>) -> Matrix6<f64> {
8    let mut Ad_h_inv = Matrix6::zeros();
9    // let R_inv = h.rotation.to_rotation_matrix().matrix().transpose();
10    // let R_inv = h.rotation.to_rotation_matrix()
11    let h_inv = h.inverse().to_homogeneous();
12    let R_inv = h_inv.fixed_view::<3, 3>(0, 0);
13    let p: Vector3<f64> = h_inv.fixed_view::<3, 1>(0, 3).into();
14
15    Ad_h_inv.fixed_view_mut::<3, 3>(0, 0).copy_from(&R_inv);
16
17    Ad_h_inv
18        .fixed_view_mut::<3, 3>(0, 3)
19        .copy_from(&(skew(&p) * R_inv));
20    Ad_h_inv.fixed_view_mut::<3, 3>(3, 3).copy_from(&R_inv);
21    Ad_h_inv
22}
23
24pub fn Ad(h: &Isometry3<f64>) -> Matrix6<f64> {
25    let mut Ad_h = Matrix6::zeros();
26    let h = h.to_homogeneous();
27    let R = h.fixed_view::<3, 3>(0, 0);
28    let p: Vector3<f64> = h.fixed_view::<3, 1>(0, 3).into();
29
30    Ad_h.fixed_view_mut::<3, 3>(0, 0).copy_from(&R);
31
32    Ad_h.fixed_view_mut::<3, 3>(0, 3).copy_from(&(skew(&p) * R));
33    Ad_h.fixed_view_mut::<3, 3>(3, 3).copy_from(&R);
34    Ad_h
35}
36
37pub fn skew<T: na::RealField + Copy>(v: &Vector3<T>) -> Matrix3<T> {
38    let mut skew = Matrix3::<T>::zeros();
39    skew[(0, 1)] = -v[2];
40    skew[(0, 2)] = v[1];
41    skew[(1, 0)] = v[2];
42    skew[(1, 2)] = -v[0];
43    skew[(2, 0)] = -v[1];
44    skew[(2, 1)] = v[0];
45    skew
46}
47
48pub fn ad_se3(v: &Vector6<f64>) -> SMatrix<f64, 6, 6> {
49    let mut ad = SMatrix::<f64, 6, 6>::zeros();
50    let lin_vel = v.fixed_view::<3, 1>(0, 0).into();
51    let ang_vel = v.fixed_view::<3, 1>(3, 0).into();
52
53    ad.fixed_view_mut::<3, 3>(0, 0).copy_from(&skew(&ang_vel));
54    ad.fixed_view_mut::<3, 3>(0, 3).copy_from(&skew(&lin_vel));
55    ad.fixed_view_mut::<3, 3>(3, 3).copy_from(&skew(&ang_vel));
56
57    ad
58}
59
60pub fn ad_se3_dyn(v: &OVector<f64, Dyn>) -> OMatrix<f64, Dyn, Dyn> {
61    let mut ad = OMatrix::<f64, Dyn, Dyn>::zeros(6, 6);
62    // let mut ad = OMatrix::<f64>::zeros(6, 6);
63    // let lin_vel = v.fixed_view::<3, 1>(0, 0).into();
64    // let ang_vel = v.fixed_view::<3, 1>(3, 0).into();
65    let lin_vel = v.rows(0, 3).into();
66    let ang_vel = v.rows(3, 3).into();
67
68    // ad.fixed_view_mut::<3, 3>(0, 0).copy_from(&skew(&ang_vel));
69    // ad.fixed_view_mut::<3, 3>(0, 3).copy_from(&skew(&lin_vel));
70    // ad.fixed_view_mut::<3, 3>(3, 3).copy_from(&skew(&ang_vel));
71    ad.view_mut((0, 0), (3, 3)).copy_from(&skew_dyn(&ang_vel));
72    ad.view_mut((0, 3), (3, 3)).copy_from(&skew_dyn(&lin_vel));
73    ad.view_mut((3, 3), (3, 3)).copy_from(&skew_dyn(&ang_vel));
74
75    ad
76}
77
78fn skew_dyn(v: &OVector<f64, Dyn>) -> Matrix3<f64> {
79    let mut skew = Matrix3::zeros();
80    skew[(0, 1)] = -v[2];
81    skew[(0, 2)] = v[1];
82    skew[(1, 0)] = v[2];
83    skew[(1, 2)] = -v[0];
84    skew[(2, 0)] = -v[1];
85    skew[(2, 1)] = v[0];
86    skew
87}
88
89pub fn comp_rb_mass_matrix(m: f64, r: &Vector3<f64>, inertia_mat: &Matrix3<f64>) -> Matrix6<f64> {
90    let mut mass_matrix = Matrix6::zeros();
91    let skew_r = skew(r);
92
93    mass_matrix
94        .fixed_view_mut::<3, 3>(0, 0)
95        .copy_from(&(m * Matrix3::identity()));
96    mass_matrix
97        .fixed_view_mut::<3, 3>(0, 3)
98        .copy_from(&(-m * skew_r));
99    mass_matrix
100        .fixed_view_mut::<3, 3>(3, 0)
101        .copy_from(&(m * skew_r));
102    mass_matrix
103        .fixed_view_mut::<3, 3>(3, 3)
104        .copy_from(inertia_mat);
105    mass_matrix
106}
multibody_dynamics/math_functions.rs

multibody_dynamics/
math_functions.rs
