multibody.rs - source
Description: Source of the Rust file `src/multibody.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
    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
    192
    193
    194
    195
    196
    197
    198
    199
    200
    201
    202
    203
    204
    205
    206
    207
    208
    209
    210
    211
    212
    213
    214
    215
    216
    217
    218
    219
    220
    221
    222
    223
    224
    225
    226
    227
    228
    229
    230
    231
    232
    233
    234
    235
    236
    237
    238
    239
    240
    241
    242
    243
    244
    245
    246
    247
    248
    249
    250
    251
    252
    253
    254
    255
    256
    257
    258
    259
    260
    261
    262
    263
    264
    265
    266
    267
    268
    269
    270
    271
    272
    273
    274
    275
    276
    277
    278
    279
    280
    281
    282
    283
    284
    285
    286
    287
    288
    289
    290
    291
    292
    293
    294
    295
    296
    297
    298
    299
    300
    301
    302
    303
    304
    305
    306
    307
    308
    309
    310
    311
    312
    313
    314
    315
    316
    317
    318
    319
    320
    321
    322
    323
    324
    325
    326
    327
    328
    329
    330
    331
    332
    333
    334
    335
    336
    337
    338
    339
    340
    341
    342
    343
    344
    345
    346
    347
    348
    349
    350
    351
    352
    353
    354
    355
    356
    357
    358
    359
    360
    361
    362
    363
    364
    365
    366
    367
    368
    369
    370
    371
    372
    373
    374
    375
    376
    377
    378
    379
    380
    381
    382
    383
    384
    385
    386
    387
    388
    389
    390
    391
    392
    393
    394
    395
    396
    397
    398
    399
    400
    401
    402
    403
    404
    405
    406
    407
    408
    409
    410
    411
    412
    413
    414
    415
    416
    417
    418
    419
    420
    421
    422
    423
    424
    425
    426
    427
    428
    429
    430
    431
    432
    433
    434
    435
    436
    437
    438
    439
    440
    441
    442
    443
    444
    445
    446
    447
    448
    449
    450
    451
    452
    453
    454
    455
    456
    457
    458
    459
    460
    461
    462
    463
    464
    465
    466
    467
    468
    469
    470
    471
    472
    473
    474
    475
    476
    477
    478
    479
    480
    481
    482
    483
    484
    485
    486
    487
    488
    489
    490
    491
    492
    493
    494
    495
    496
    497
    498
    499
    500
    501
    502
    503
    504
    505
    506
    507
    508
    509
    510
    511
    512
    513
    514
    515
    516
    517
    518
    519
    520
    521
    522
    523
    524
    525
    526
    527
    528
    529
    530
    531
    532
    533
    534
    535
    536
    537
    538
    539
    540
    541
    542
    543
    544
    545
    546
    547
    548
    549
    550
    551
    552
    553
    554
    555
    556
    557
    558
    559
    560
    561
    562
    563
    564
    565
    566
    567
    568
    569
    570
    571
    572
    573
    574
    575
    576
    577
    578
    579
    580
    581
    582
    583
    584
    585
    586
    587
    588
    589
    590
    591
    592
    593
    594
    595
    596
    597
    598
    599
    600
    601
    602
    603
    604
    605
    606
    607
    608
    609
    610
    611
    612
    613
    614
    615
    616
    617
    618
    619
    620
    621
    622
    623
    624
    625
    626
    627
    628
    629
    630
    631
    632
    633
    634
    635
    636
    637
    638
    639
    640
    641
    642
    643
    644
    645
    646
    647
    648
    649
    650
    651
    652
    653
    654
    655
    656
    657
    658
    659
    660
    661
    662
    663
    664
    665
    666
    667
    668
    669
    670
    671
    672
    673
    674
    675
    676
    677
    678
    679
    680
    681
    682
    683
    684
    685
    686
    687
    688
    689
    690
    691
    692
    693
    694
    695
    696
    697
    698
    699
    700
    701
    702
    703
    704
    705
    706
    707
    708
    709
    710
    711
    712
    713
    714
    715
    716
    717
    718
    719
    720
    721
    722
    723
    724
    725
    726
    727
    728
    729
    730
    731
    732
    733
    734
    735
    736
    737
    738
    739
    740
    741
    742
    743
    744
    745
    746
    747
    748
    749
    750
    751
    752
    753
    754
    755
    756
    757
    758
    759
    760
    761
    762
    763
    764
    765
    766
    767
    768
    769
    770
    771
    772
    773
    774
    775
    776
    777
    778
    779
    780
    781
    782
    783
    784
    785
    786
    787
    788
    789
    790
    791
    792
    793
    794
    795
    796
    797
    798
    799
    800
    801
    802
    803
    804
    805
    806
    807
    808
    809
    810
    811
    812
    813
    814
    815
    816
    817
    818
    819
    820
    821
    822
    823
    824
    825
    826
    827
    828
    829
    830
    831
    832
    833
    834
    835
    836
    837
    838
    839
    840
    841
    842
    843
    844
    845
    846
    847
    848
    849
    850
    851
    852
    853
    854
    855
    856
    857
    858
    859
    860
    861
    862
    863
    864
    865
    866
    867
    868
    869
    870
    871
    872
    873
    874
    875
    876
    877
    878
    879
    880
    881
    882
    883
    884
    885
    886
    887
    888
    889
    890
    891
    892
    893
    894
    895
    896
    897
    898
    899
    900
    901
    902
    903
    904
    905
    906
    907
    908
    909
    910
    911
    912
    913
    914
    915
    916
    917
    918
    919
    920
    921
    922
    923
    924
    925
    926
    927
    928
    929
    930
    931
    932
    933
    934
    935
    936
    937
    938
    939
    940
    941
    942
    943
    944
    945
    946
    947
    948
    949
    950
    951
    952
    953
    954
    955
    956
    957
    958
    959
    960
    961
    962
    963
    964
    965
    966
    967
    968
    969
    970
    971
    972
    973
    974
    975
    976
    977
    978
    979
    980
    981
    982
    983
    984
    985
    986
    987
    988
    989
    990
    991
    992
    993
    994
    995
    996
    997
    998
    999
    1000
    1001
    1002
    1003
    1004
    1005
    1006
    1007
    1008
    1009
    1010
    1011
    1012
    1013
    1014
    1015
    1016
    1017
    1018
    1019
    1020
    1021
    1022
    1023
    1024
    1025
    1026
    1027
    1028
    1029
    1030
    1031
    1032
    1033
    1034
    1035
    1036
    1037
    1038
    1039
    1040
    1041
    1042
    1043
    1044
    1045
    1046
    1047
    1048
    1049
    1050
    1051
    1052
    1053
    1054
    1055
    1056
    1057
    1058
    1059
    1060
    1061
    1062
    1063
    1064
    1065
    1066
    1067
    1068
    1069
    1070
    1071
    1072
    1073
    1074
    1075
    1076
    1077
    1078
    1079
    1080
    1081
    1082
    1083
    1084
    1085
    1086
    1087
    1088
    1089
    1090
    1091
    1092
    1093
    1094
    1095
    1096
    1097
    1098
    1099
    1100
    1101
    1102
    1103
    1104
    1105
    1106
    1107
    1108
    1109
    1110
    1111
    1112
    1113
    1114
    1115
    1116
    1117
    1118
    1119
    1120
    1121
    1122
    1123
    1124
    1125
    1126
    1127
    1128
    1129
    1130
    1131
    1132
    1133
    1134
    1135
    1136
    1137
    1138
    1139
    1140

    // #![no_std]
    #![cfg_attr(feature = "no_std", no_std)]

    #[cfg(feature = "no_std")]
    extern crate core as std;

    extern crate nalgebra as na;
    use crate::math_functions::*;
    use na::{
        DMatrix, DVector, Dyn, Isometry3, Matrix1, Matrix3, Matrix4, Matrix6, OMatrix, Quaternion,
        SMatrix, SVector, Translation3, UnitQuaternion, Vector1, Vector3, Vector6, U1, U6,
    };

    // use num::{One, Zero};

    #[derive(Clone, Debug)]
    pub enum JointType {
        Revolute,
        Prismatic,
        SixDOF,
    }

    /// Returns the number of DOFs for a given joint type.
    const fn dofs_joint(joint_type: &JointType) -> usize {
        match joint_type {
            JointType::Revolute => 1,
            JointType::Prismatic => 1,
            JointType::SixDOF => 6,
        }
    }

    /// Allows overloading of functions for both a single 6DOF configuration and for a vector of 6DOF configurations, which is required when there are more than one 6DOF joint in the multibody system.
    pub trait IntoHomogenousConfigurationVec {
        fn into(&self) -> Vec<Isometry3<f64>>;
    }

    impl IntoHomogenousConfigurationVec for Isometry3<f64> {
        fn into(&self) -> Vec<Isometry3<f64>> {
            vec![*self]
        }
    }

    impl IntoHomogenousConfigurationVec for Vec<Isometry3<f64>> {
        fn into(&self) -> Vec<Isometry3<f64>> {
            self.clone()
        }
    }

    pub struct MultiBody<const NUM_BODIES: usize, const NUM_DOFS: usize> {
        offset_matrices: Vec<Isometry3<f64>>,
        mass_matrices: Vec<Matrix6<f64>>,
        joint_types: Vec<JointType>,
        parent: Vec<u16>,
        g: Vec<Isometry3<f64>>,
        h: Vec<Isometry3<f64>>,
        nu: Vec<Vector6<f64>>,
        nu_prime: Vec<Vector6<f64>>,
        alpha: Vec<Vector6<f64>>,
        Phi: SMatrix<f64, 6, NUM_DOFS>,
        joint_dims: SVector<usize, NUM_BODIES>,
        joint_size_offsets: Vec<usize>,
        jacobians: Vec<SMatrix<f64, 6, NUM_DOFS>>,
        gravity: Vector3<f64>,
        r_com: Option<Vec<Vector3<f64>>>,
        mass: Option<Vec<f64>>,
        r_cob: Option<Vec<Vector3<f64>>>,
        volume: Option<Vec<f64>>,
        rho: Option<f64>,
    }

    // impl<T: na::RealField  + na::ClosedAdd + na::ClosedMul + na::ClosedDiv + Copy, const NUM_BODIES: usize, const NUM_DOFS: usize> MultiBody<T, NUM_BODIES, NUM_DOFS> {
    impl<const NUM_BODIES: usize, const NUM_DOFS: usize> MultiBody<NUM_BODIES, NUM_DOFS> {
        pub fn new(
            offset_matrices: Vec<Isometry3<f64>>,
            mass_matrices: Option<Vec<Matrix6<f64>>>,
            inertia_matrices: Option<Vec<Matrix3<f64>>>,
            joint_types: Vec<JointType>,
            parent: Vec<u16>,
            gravity: Vector3<f64>,
            r_com: Option<Vec<Vector3<f64>>>,
            r_cob: Option<Vec<Vector3<f64>>>,
            mass: Option<Vec<f64>>,
            volume: Option<Vec<f64>>,
            rho: Option<f64>,
        ) -> Result<MultiBody<NUM_BODIES, NUM_DOFS>, &'static str> {
            let mut joint_dims = SVector::<usize, NUM_BODIES>::zeros();
            let mut Phi = SMatrix::<f64, 6, NUM_DOFS>::zeros();
            let mut joint_size_offsets = 0;
            let mut joint_offset_vec = vec![0; NUM_BODIES];

            for i in 0..NUM_BODIES {
                joint_offset_vec[i] = joint_size_offsets;

                match joint_types[i] {
                    JointType::Revolute => {
                        let Phi_i = Vector6::new(0.0, 0.0, 0.0, 0.0, 0.0, 1.0);
                        Phi.fixed_view_mut::<6, 1>(0, i + joint_size_offsets)
                            .copy_from(&Phi_i);
                        joint_dims[i] = 1;
                    }
                    JointType::Prismatic => {
                        let Phi_i = Vector6::new(0.0, 0.0, 1.0, 0.0, 0.0, 0.0);
                        Phi.fixed_view_mut::<6, 1>(0, i + joint_size_offsets)
                            .copy_from(&Phi_i);
                        joint_dims[i] = 1;
                    }
                    JointType::SixDOF => {
                        Phi.fixed_view_mut::<6, 6>(0, i + joint_size_offsets)
                            .copy_from(&Matrix6::identity());
                        joint_dims[i] = 6;

                        joint_size_offsets += joint_dims[i] as usize - 1;
                    }
                }
            }

            let mass_matrices = match mass_matrices {
                Some(mass_matrices) => mass_matrices,
                None => {
                    let mut mass_mats = vec![Matrix6::zeros(); NUM_BODIES];
                    for i in 0..NUM_BODIES {
                        let m = mass.as_ref().expect("Scalar masses should be provided if the mass matrix is not given.")[i];
                        let r = r_com.as_ref().expect("The center of gravity must be given if the mass matrix is not given.")[i];
                        let inertia_mat = inertia_matrices.as_ref().expect("The 3x3 inertia matrices must be provided if the 6x6 mass matrix is not given.")[i];
                        mass_mats[i] = comp_rb_mass_matrix(m, &r, &inertia_mat);
                    }
                    mass_mats
                }
            };

            Ok(MultiBody {
                offset_matrices: offset_matrices,
                mass_matrices: mass_matrices,
                joint_types: joint_types,
                parent: parent,
                g: vec![Isometry3::identity(); NUM_BODIES],
                h: vec![Isometry3::identity(); NUM_BODIES],
                nu: vec![Vector6::zeros(); NUM_BODIES],
                nu_prime: vec![Vector6::zeros(); NUM_BODIES],
                alpha: vec![Vector6::zeros(); NUM_BODIES],
                Phi: Phi,
                joint_dims: joint_dims,
                joint_size_offsets: joint_offset_vec,
                jacobians: vec![SMatrix::<f64, 6, NUM_DOFS>::zeros(); NUM_BODIES],
                gravity: gravity,
                r_com: r_com,
                r_cob: r_cob,
                mass: mass,
                volume: volume,
                rho: rho,
            })
        }

        /// Converts a set of minimal coordinates to a set of homogenous coordinates.
        pub fn minimal_to_homogenous_configuration<Configuration, const D: usize>(
            &self,
            six_dof_vars: &Configuration,
            scalar_joint_vars: &SVector<f64, D>,
        ) -> Vec<Isometry3<f64>>
        where
            Configuration: IntoHomogenousConfigurationVec,
        {
            let six_dof_vars = six_dof_vars.into();
            let mut j = 0;
            let mut k = 0;

            let mut conf: Vec<Isometry3<f64>> = vec![Isometry3::identity(); NUM_BODIES];

            for i in 1..NUM_BODIES {
                match self.joint_types[i] {
                    JointType::Revolute => {
                        let mut temp = Isometry3::identity();
                        temp.rotation =
                            UnitQuaternion::from_axis_angle(&Vector3::z_axis(), scalar_joint_vars[j]);

                        conf[i] = temp;
                        j = j + 1;
                    }
                    JointType::Prismatic => {
                        let mut temp = Isometry3::identity();
                        temp.translation = Translation3::new(0.0, 0.0, scalar_joint_vars[j]);

                        conf[i] = temp;
                        j = j + 1;
                    }
                    JointType::SixDOF => {
                        conf[i] = six_dof_vars[k];
                        k = k + 1;
                    }
                }
            }
            conf
        }
     /// $$E=mc^2$$
        pub fn generalized_newton_euler(
            &mut self,
            conf: &Vec<Isometry3<f64>>,
            mu: &SVector<f64, NUM_DOFS>,
            mu_prime: &SVector<f64, NUM_DOFS>,
            sigma_prime: &SVector<f64, NUM_DOFS>,
            rigid_body_forces: &Vec<Vector6<f64>>,
            eta: &SVector<f64, NUM_DOFS>,
        ) -> SVector<f64, NUM_DOFS> {
            let mut w: Vec<Vector6<f64>> = vec![Vector6::zeros(); NUM_BODIES];
            let mut zeta = SVector::<f64, NUM_DOFS>::zeros();

            for i in 0..NUM_BODIES {
                match self.joint_types[i] {
                    JointType::Revolute | JointType::Prismatic => {
                        let idx = i + self.joint_size_offsets[i];
                        let mu_i = mu.fixed_rows::<1>(idx).try_into().unwrap();
                        let mu_prime_i = mu_prime.fixed_rows::<1>(idx).try_into().unwrap();
                        let sigma_prime_i = sigma_prime.fixed_rows::<1>(idx).try_into().unwrap();

                        w[i] = self.forward_step(
                            &conf[i],
                            &mu_i,
                            &mu_prime_i,
                            &sigma_prime_i,
                            &rigid_body_forces[i],
                            i,
                        );
                    }
                    JointType::SixDOF => {
                        let idx = i + self.joint_size_offsets[i];
                        let mu_i = mu.fixed_rows::<6>(idx).try_into().unwrap();
                        let mu_prime_i = mu_prime.fixed_rows::<6>(idx).try_into().unwrap();
                        let sigma_prime_i = sigma_prime.fixed_rows::<6>(idx).try_into().unwrap();

                        w[i] = self.forward_step(
                            &conf[i],
                            &mu_i,
                            &mu_prime_i,
                            &sigma_prime_i,
                            &rigid_body_forces[i],
                            i,
                        );
                    }
                }
            }

            // backward step
            for i in (0..NUM_BODIES).rev() {
                match self.joint_types[i] {
                    JointType::Revolute | JointType::Prismatic => {
                        let idx = i + self.joint_size_offsets[i];
                        let Phi_i = self.Phi.fixed_view::<6, 1>(0, idx);
                        // zeta[i] = (Phi_i.transpose() * w[i])[(0, 0)] + eta[idx];
                        zeta.fixed_view_mut::<1, 1>(idx, 0)
                            .copy_from(&((Phi_i.transpose() * w[i]) + Matrix1::new(eta[idx])));
                    }
                    JointType::SixDOF => {
                        let idx = i + self.joint_size_offsets[i];
                        let Phi_i = self.Phi.fixed_view::<6, 6>(0, idx);
                        zeta.fixed_view_mut::<6, 1>(idx, 0)
                            .copy_from(&(Phi_i.transpose() * w[i] + eta.fixed_rows::<6>(idx)));
                    }
                }

                let lambda_i = self.parent[i] as i32 - 1;
                if lambda_i >= 0 {
                    w[lambda_i as usize] =
                        w[lambda_i as usize] + Ad(&self.h[i].inverse()).transpose() * w[i];
                }
            }
            zeta
        }

        fn forward_step<const D: usize>(
            &mut self,
            conf: &Isometry3<f64>,
            mu: &SVector<f64, D>,
            mu_prime: &SVector<f64, D>,
            sigma_prime: &SVector<f64, D>,
            rigid_body_forces: &SVector<f64, 6>,
            node_idx: usize,
        ) -> Vector6<f64> {
            let idx_offset = self.joint_size_offsets[node_idx];

            let h = self.offset_matrices[node_idx] * conf;
            // println!("offset mat ind: {} : {}", node_idx, self.offset_matrices[node_idx].to_homogeneous());
            self.h[node_idx] = h;
            // println!("h[{}] {}", node_idx, self.h[node_idx].to_homogeneous());

            let Phi: SMatrix<f64, 6, D> = self
                .Phi
                .fixed_view::<6, D>(0, node_idx + idx_offset)
                .try_into()
                .unwrap();

            let lambda_i = self.parent[node_idx] as i32 - 1;

            if lambda_i < 0 {
                self.g[node_idx] = self.h[node_idx];
                self.nu[node_idx] = Phi * mu;
                self.nu_prime[node_idx] = Phi * mu_prime;

                match self.joint_types[node_idx] {
                    JointType::Revolute | JointType::Prismatic => {
                        self.alpha[node_idx] =
                            ad_se3(&self.nu_prime[node_idx]) * Phi * mu + Phi * sigma_prime;
                    }
                    JointType::SixDOF => {
                        let mu: SVector<f64, 6> = mu.fixed_rows::<6>(0).try_into().unwrap();
                        let mu_prime: SVector<f64, 6> = mu_prime.fixed_rows::<6>(0).try_into().unwrap();
                        let sigma_prime: SVector<f64, 6> =
                            sigma_prime.fixed_rows::<6>(0).try_into().unwrap();
                        let Phi: SMatrix<f64, 6, 6> = Phi.fixed_view::<6, 6>(0, 0).try_into().unwrap();

                        self.alpha[node_idx] = ad_se3(&self.nu_prime[node_idx]) * Phi * mu
                            + Phi * (sigma_prime + ad_se3(&mu) * mu_prime);
                    }
                }
            } else {
                self.g[node_idx] = self.g[lambda_i as usize] * self.h[node_idx];

                let Ad_h_inv = Ad(&self.h[node_idx].inverse());

                self.nu[node_idx] = Ad_h_inv * self.nu[lambda_i as usize] + Phi * mu;

                self.nu_prime[node_idx] = Ad_h_inv * self.nu_prime[lambda_i as usize] + Phi * mu_prime;

                match self.joint_types[node_idx] {
                    JointType::Revolute | JointType::Prismatic => {
                        self.alpha[node_idx] = Ad_h_inv * self.alpha[lambda_i as usize]
                            + ad_se3(&self.nu_prime[node_idx]) * Phi * mu
                            + Phi * sigma_prime;
                    }
                    JointType::SixDOF => {
                        let mu: SVector<f64, 6> = mu.fixed_rows::<6>(0).try_into().unwrap();
                        let mu_prime: SVector<f64, 6> = mu_prime.fixed_rows::<6>(0).try_into().unwrap();
                        let sigma_prime: SVector<f64, 6> =
                            sigma_prime.fixed_rows::<6>(0).try_into().unwrap();
                        let Phi: SMatrix<f64, 6, 6> = Phi.fixed_view::<6, 6>(0, 0).try_into().unwrap();

                        self.alpha[node_idx] = Ad_h_inv * self.alpha[lambda_i as usize]
                            + ad_se3(&self.nu_prime[node_idx]) * Phi * mu
                            + Phi * (sigma_prime + ad_se3(&mu) * mu_prime);
                    }
                }
            }

            // println!("alpha[{}] {}", node_idx, self.alpha[node_idx].to_homogeneous());

            self.mass_matrices[node_idx] * self.alpha[node_idx]
                - ad_se3(&self.nu_prime[node_idx]).transpose()
                    * self.mass_matrices[node_idx]
                    * self.nu[node_idx]
                + rigid_body_forces
        }

        /// Computes the mass matrix of the multibody system using the composite rigid body algorithm (CRB). Assumes that GNE/MNE/AB has been called.
        pub fn compute_mass_matrix(&self) -> SMatrix<f64, NUM_DOFS, NUM_DOFS> {
            let mut M_c = self.mass_matrices.clone();
            let mut M_o = SMatrix::<f64, NUM_DOFS, NUM_DOFS>::zeros();

            for i in (0..NUM_BODIES).rev() {
                let lambda_i = self.parent[i] as i32 - 1;
                let Ad_h_i_inv = Ad(&self.h[i].inverse());

                if lambda_i >= 0 {
                    M_c[lambda_i as usize] =
                        M_c[lambda_i as usize] + Ad_h_i_inv.transpose() * M_c[i] * Ad_h_i_inv;
                }
                let idx = i + self.joint_size_offsets[i];

                let Phi_i: OMatrix<f64, U6, Dyn>;

                match self.joint_types[i] {
                    JointType::Revolute | JointType::Prismatic => {
                        Phi_i = OMatrix::from_columns(&[self.Phi.fixed_view::<6, 1>(0, idx)]);
                    }
                    JointType::SixDOF => {
                        Phi_i = OMatrix::<f64, U6, Dyn>::from_iterator(
                            6,
                            self.Phi.fixed_view::<6, 6>(0, idx).iter().cloned(),
                        );
                    }
                }
                let mut X = M_c[i] * &Phi_i;
                M_o.view_mut((idx, idx), (self.joint_dims[i], self.joint_dims[i]))
                    .copy_from(&(Phi_i.transpose() * &X));

                let mut j = i;
                let lambda = |x: usize| -> i32 { self.parent[x] as i32 - 1 };

                while lambda(j) >= 0 {
                    X = Ad(&self.h[j].inverse()).transpose() * X;
                    j = lambda(j) as usize;
                    let Phi_j: OMatrix<f64, U6, Dyn>;

                    match self.joint_types[j] {
                        JointType::Revolute | JointType::Prismatic => {
                            Phi_j = OMatrix::from_columns(&[self
                                .Phi
                                .fixed_view::<6, 1>(0, j + self.joint_size_offsets[j])]);
                        }
                        JointType::SixDOF => {
                            Phi_j = OMatrix::<f64, U6, Dyn>::from_iterator(
                                6,
                                self.Phi
                                    .fixed_view::<6, 6>(0, j + self.joint_size_offsets[j])
                                    .iter()
                                    .cloned(),
                            );
                        }
                    }
                    let temp = X.transpose() * &Phi_j;

                    M_o.view_mut(
                        (idx, j + self.joint_size_offsets[j]),
                        (self.joint_dims[i], self.joint_dims[j]),
                    )
                    .copy_from(&temp);
                    M_o.view_mut(
                        (j + self.joint_size_offsets[j], idx),
                        (self.joint_dims[j], self.joint_dims[i]),
                    )
                    .copy_from(&temp.transpose());
                }
            }
            M_o
        }

        /// Computes the forward dynamics using the articulated body algorithm (AB).
        pub fn forward_dynamics_ab(
            &mut self,
            conf: &Vec<Isometry3<f64>>,
            mu: &SVector<f64, NUM_DOFS>,
            damping_forces: &Vec<Vector6<f64>>,
            thruster_forces: &Vec<Vector6<f64>>,
            eta: &SVector<f64, NUM_DOFS>,
            lin_vel_current: &Vector3<f64>,
            lin_accel_current: &Vector3<f64>,
        ) -> SVector<f64, NUM_DOFS> {
            let mut sigma = SVector::<f64, NUM_DOFS>::zeros();

            let mut nu_0 = Vector6::<f64>::zeros();
            nu_0.fixed_view_mut::<3, 1>(0, 0)
                .copy_from(&(-lin_vel_current));

            let mut a_e = vec![Vector3::<f64>::zeros(); NUM_BODIES];
            let mut b = vec![Vector6::<f64>::zeros(); NUM_BODIES];
            a_e[0] = self.gravity - lin_accel_current;

            let mut M_a = self.mass_matrices.clone();
            let mut V_inv : Vec<DMatrix::<f64>> = Vec::new();
            let mut U_vec : Vec<OMatrix<f64, U6, Dyn>> = Vec::new();
            let mut u: Vec<DVector::<f64>> = Vec::new();

            let lambda = |x: usize| -> i32 { self.parent[x] as i32 - 1 };

            for i in 0..NUM_BODIES {
                let idx = i + self.joint_size_offsets[i];
                self.h[i] = self.offset_matrices[i] * conf[i];

                let Phi_i = self.Phi.view((0, idx), (6, self.joint_dims[i]));
                let mu_i = mu.rows(idx, self.joint_dims[i]);

                if lambda(i) == -1 {
                    self.nu[i] = Ad(&self.h[i].inverse()) * nu_0 + Phi_i * mu_i;
                    a_e[i] = self.h[i].rotation.inverse() * a_e[0];
                } else {
                    self.nu[i] = Ad(&self.h[i].inverse()) * self.nu[lambda(i) as usize] + Phi_i * mu_i;
                    a_e[i] = self.h[i].rotation.inverse() * a_e[lambda(i) as usize];
                }

                let quat = UnitQuaternion::from_quaternion(*self.h[i].rotation.quaternion());
                b[i] = -ad_se3(&self.nu[i]).transpose() * M_a[i] * self.nu[i]
                    - damping_forces[i]
                    - self.compute_hydrostatic_force(&quat, &lin_accel_current, i)
                    - thruster_forces[i];
            }

            for i in (0..NUM_BODIES).rev() {
                let idx = i + self.joint_size_offsets[i];
                let Phi_i = self.Phi.view((0, idx), (6, self.joint_dims[i]));
                let U_i = M_a[i] * Phi_i;

                let V_i = Phi_i.transpose() * &U_i;

                let u_i = eta.rows(idx, self.joint_dims[i]) - Phi_i.transpose() * b[i];
                let V_i_inv = V_i.try_inverse().unwrap();

                if lambda(i) >= 0 {
                    let mu_i = mu.rows(idx, self.joint_dims[i]);

                    let M_bar = M_a[i] - &U_i * &V_i_inv * &U_i.transpose();
                    let b_bar = b[i] + &M_bar * ad_se3(&self.nu[i]) * &Phi_i * &mu_i + &U_i * &V_i_inv * &u_i;

                    let Ad_h_i_inv = Ad(&self.h[i].inverse());
                    M_a[lambda(i) as usize] = M_a[lambda(i) as usize] + Ad_h_i_inv.transpose() * M_bar * Ad_h_i_inv;
                    b[lambda(i) as usize] = b[lambda(i) as usize] + Ad_h_i_inv.transpose() * b_bar;
                }
                V_inv.insert(0, V_i_inv);
                U_vec.insert(0, U_i);
                u.insert(0, u_i);

            }

            let mut alpha_0 = Vector6::<f64>::zeros();
            alpha_0
                .fixed_view_mut::<3, 1>(0, 0)
                .copy_from(&(-lin_accel_current));

            for i in 0..NUM_BODIES {
                let idx = i + self.joint_size_offsets[i];
                let Phi_i = self.Phi.view((0, idx), (6, self.joint_dims[i]));
                let mu_i = mu.rows(idx, self.joint_dims[i]);

                let Ad_h_i_inv = Ad(&self.h[i].inverse());
                let alpha_bar : SVector<f64, 6>;

                if lambda(i) == -1 {
                    alpha_bar = Ad_h_i_inv * alpha_0 + ad_se3(&self.nu[i]) * Phi_i * mu_i;
                } else {
                    alpha_bar = Ad_h_i_inv * self.alpha[lambda(i) as usize] + ad_se3(&self.nu[i]) * Phi_i * mu_i;
                }
                let temp = &V_inv[i] * (&u[i] - &U_vec[i].transpose() * alpha_bar);
                sigma.rows_mut(idx, self.joint_dims[i]).copy_from(&temp);

                self.alpha[i] = alpha_bar + Phi_i * temp;
            }

            sigma
        }

        fn compute_hydrostatic_force(
            &self,
            quat: &UnitQuaternion<f64>,
            current_accel: &Vector3<f64>,
            body_id: usize,
        ) -> Vector6<f64> {
            let mut hydrostatic_force = Vector6::<f64>::zeros();

            let Rot = quat.to_rotation_matrix();
            let rho = match self.rho {
                Some(rho) => rho,
                None => 0.0,
            };
            let volume = match &self.volume {
                Some(volume) => volume[body_id],
                None => 0.0,
            };
            let r_cob = match &self.r_cob {
                Some(r_cob) => r_cob[body_id],
                None => Vector3::<f64>::zeros(),
            };

            let mass = self.mass.as_ref().unwrap()[body_id];
            let r_com = self.r_com.as_ref().unwrap()[body_id];

            let linear = (mass - rho * volume)
                * Rot.matrix().transpose()
                * (self.gravity - current_accel);
            let rotational = (mass * skew(&r_com) - rho * volume * skew(&r_cob))
                * Rot.matrix().transpose()
                * (self.gravity - current_accel);

            hydrostatic_force
                .fixed_view_mut::<3, 1>(0, 0)
                .copy_from(&linear);
            hydrostatic_force
                .fixed_view_mut::<3, 1>(3, 0)
                .copy_from(&rotational);

            hydrostatic_force
        }

        pub fn compute_jacobians(
            &mut self,
            config: &Vec<Isometry3<f64>>,
        ) -> Vec<SMatrix<f64, 6, NUM_DOFS>> {
            for i in 0..NUM_BODIES {
                let idx = i + self.joint_size_offsets[i];

                let Phi_i = self.Phi.view((0, idx), (6, self.joint_dims[i]));
                self.jacobians[i]
                    .view_mut((0, idx), (6, self.joint_dims[i]))
                    .copy_from(&Phi_i);

                let lambda = |x: usize| -> i32 { self.parent[x] as i32 - 1 };
                for j in i + 1..NUM_BODIES {
                    if lambda(j) >= 0 {
                        self.h[j] = self.offset_matrices[j] * config[j];
                        let temp = Ad(&self.h[j].inverse())
                            * self.jacobians[lambda(j) as usize]
                                .view((0, idx), (6, self.joint_dims[i]));

                        self.jacobians[j]
                            .view_mut((0, idx), (6, self.joint_dims[i]))
                            .copy_from(&temp);
                    }
                }
            }
            self.jacobians.clone()
        }

        pub fn compute_jacobian_derivatives(
            &mut self,
            mu: &SVector<f64, NUM_DOFS>,
        ) -> Vec<SMatrix<f64, 6, NUM_DOFS>> {
            let mut jacobian_derivs = vec![SMatrix::<f64, 6, NUM_DOFS>::zeros(); NUM_BODIES];

            for i in 0..NUM_BODIES {
                let idx = i + self.joint_size_offsets[i];
                for j in i + 1..NUM_BODIES {
                    let lambda = |x: usize| -> i32 { self.parent[x] as i32 - 1 };
                    if lambda(j) >= 0 {
                        let idx_j = j + self.joint_size_offsets[j];

                        let Phi_j = self.Phi.view((0, idx_j), (6, self.joint_dims[j]));
                        let mu_j = mu.rows(idx_j, self.joint_dims[j]);

                        let djac_ji = Ad(&self.h[j].inverse())
                            * jacobian_derivs[lambda(j) as usize]
                                .view((0, idx), (6, self.joint_dims[i]))
                            - ad_se3_dyn(&(Phi_j * mu_j))
                                * self.jacobians[j].view((0, idx), (6, self.joint_dims[i]));

                        jacobian_derivs[j]
                            .view_mut((0, idx), (6, self.joint_dims[i]))
                            .copy_from(&djac_ji);
                    }
                }
            }
            jacobian_derivs
        }

        pub fn compute_jacobian(
            &self,
            config: &Vec<Isometry3<f64>>,
            body_id: usize,
        ) -> SMatrix<f64, 6, NUM_DOFS> {
            let mut jacobian = SMatrix::<f64, 6, NUM_DOFS>::zeros();

            let idx = body_id + self.joint_size_offsets[body_id];

            let Phi_i = self.Phi.view((0, idx), (6, self.joint_dims[body_id]));
            jacobian
                .view_mut((0, idx), (6, self.joint_dims[body_id]))
                .copy_from(&Phi_i);

            let mut j = body_id;
            let lambda = |x: usize| -> i32 { self.parent[x] as i32 - 1 };

            let mut k = Isometry3::<f64>::identity();
            while lambda(j) >= 0 {
                let h_j = self.offset_matrices[j] * config[j];

                if j == body_id {
                    k = h_j;
                } else {
                    k = h_j * k;
                }
                j = lambda(j) as usize;
                let idx_j = j + self.joint_size_offsets[j];

                let Phi_j = self.Phi.view((0, idx_j), (6, self.joint_dims[j]));
                jacobian
                    .view_mut((0, idx_j), (6, self.joint_dims[j]))
                    .copy_from(&(Ad(&k.inverse()) * Phi_j));
            }
            jacobian
        }

        pub fn compute_jacobian_derivative(
            &self,
            config: &Vec<Isometry3<f64>>,
            mu: &SVector<f64, NUM_DOFS>,
            body_id: usize,
        ) -> SMatrix<f64, 6, NUM_DOFS> {
            let mut jacobian_deriv = SMatrix::<f64, 6, NUM_DOFS>::zeros();

            let mut j = body_id;
            let lambda = |x: usize| -> i32 { self.parent[x] as i32 - 1 };

            let mut Ad_h_inv = SMatrix::<f64, 6, 6>::identity();
            let mut nu = SVector::<f64, 6>::zeros();
            let mut h = Isometry3::<f64>::identity();

            while lambda(j) >= 0 {
                let idx_j = j + self.joint_size_offsets[j];
                let Phi_j = self.Phi.view((0, idx_j), (6, self.joint_dims[j]));
                let mu_j = mu.rows(idx_j, self.joint_dims[j]);
                let h_j = self.offset_matrices[j] * config[j];

                if j == body_id {
                    nu = SMatrix::<f64, 6, 6>::identity() * Phi_j * mu_j;
                    h = h_j;
                } else {
                    Ad_h_inv = Ad(&h.inverse());
                    nu = nu + Ad_h_inv * Phi_j * mu_j;
                    h = h_j * h;
                }
                j = lambda(j) as usize;
                let idx_j = j + self.joint_size_offsets[j];
                let Phi_j = self.Phi.view((0, idx_j), (6, self.joint_dims[j]));

                let jac_j = -ad_se3(&nu) * Ad(&h.inverse()) * Phi_j;
                jacobian_deriv
                    .view_mut((0, idx_j), (6, self.joint_dims[j]))
                    .copy_from(&jac_j);
            }
            jacobian_deriv
        }

        fn lambda(&self, x: usize) -> i32 {
            self.parent[x] as i32 - 1
        }
    }

    #[cfg(test)]
    mod tests {
        use core::f64::consts::PI;

        use approx::assert_relative_eq;
        use na::{Translation3, Vector2};

        use super::*;

        fn comp_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
        }

        #[test]
        fn gen_newton_euler_test() {
            let l_1 = 1.0;
            let l_2 = 1.0;

            let m_1 = 1.0;
            let m_2 = 1.0;

            let r_cg1 = Vector3::new(l_1 / 2.0, 0.0, 0.0);
            let r_cg2 = Vector3::new(l_2 / 2.0, 0.0, 0.0);

            // let p_01 = Translation3::new(l_1, 0.0, 0.0);
            let p_01 = Translation3::new(0.0, 0.0, 0.0);
            let q_01 = UnitQuaternion::identity();

            let p_12 = Translation3::new(l_1, 0.0, 0.0);
            // let q_12 = UnitQuaternion::from_euler_angles(0.0, 0.0, PI);
            let q_12 = UnitQuaternion::identity();

            let offset_matrix1 = Isometry3::from_parts(p_01, q_01);
            let offset_matrix2 = Isometry3::from_parts(p_12, q_12);
            let offset_matrices = vec![offset_matrix1, offset_matrix2];

            // let I_1 = Matrix3::identity();
            let I_1 = Matrix3::identity() - m_1 * skew(&r_cg1) * skew(&r_cg1);
            let I_2 = Matrix3::identity() - m_2 * skew(&r_cg2) * skew(&r_cg2);
            // let I_2 = Matrix3::identity();

            let M_1 = comp_mass_matrix(m_1, &r_cg1, &I_1);
            let M_2 = comp_mass_matrix(m_2, &r_cg2, &I_2);
            // let M_2 = Matrix6::zeros();

            let mass_matrices = vec![M_1, M_2];

            // let joint_types = vec![JointType::SixDOF, JointType::Revolute];
            let joint_types = vec![JointType::Revolute, JointType::Revolute];
            let parent = vec![0, 1];

            let mut multibody: MultiBody<2, 2> =
                MultiBody::new(offset_matrices, Some(mass_matrices), None, joint_types, parent, Vector3::new(0.0, 0.0, 9.81), None, None, None, None, None).unwrap();

            let mut conf: Vec<Isometry3<f64>> = Vec::new();
            let temp = Isometry3::identity();
            conf.push(temp);
            conf.push(Isometry3::from_parts(
                Translation3::identity(),
                UnitQuaternion::from_euler_angles(0.0, 0.0, PI / 2.0),
            ));

            let mu = Vector2::new(0.0, 1.0);
            let sigma_prime = Vector2::new(0.0, 0.0);
            let eta = SVector::<f64, 2>::zeros();
            let rigid_body_forces = vec![Vector6::<f64>::zeros(); 2];

            let zeta = multibody.generalized_newton_euler(
                &conf,
                &mu,
                &mu,
                &sigma_prime,
                &rigid_body_forces,
                &eta,
            );

            println!("zeta: {}", zeta);
            assert_relative_eq!(zeta, Vector2::new(-0.5, 0.0), epsilon = 0.00001);
        }

        #[test]
        fn snake_like_model_test() {
            let c1 = Isometry3::<f64>::identity();
            let q12 = UnitQuaternion::from_axis_angle(&Vector3::x_axis(), -PI / 2.0);
            let q21 = UnitQuaternion::from_axis_angle(&Vector3::x_axis(), PI / 2.0);

            let l1 = 0.62;
            let l2 = 0.10;

            let c2 = Isometry3::from_parts(Translation3::new(l1, 0.0, 0.0), UnitQuaternion::identity());
            let c3 = Isometry3::from_parts(Translation3::new(l2, 0.0, 0.0), q12);
            let c4 = Isometry3::from_parts(Translation3::new(l1, 0.0, 0.0), q21);

            // let offset_matrices = vec![c0, c1, c2, c1, c2, c1, c2, c1, c2];
            let offset_matrices = vec![c1, c2, c3, c4, c3, c4, c3, c4, c3];
            // let offset_matrices = vec![c1, c2, c4, c3, c4, c3, c4, c3, c4];

            let mut joint_types = vec![JointType::Revolute; 9];
            joint_types[0] = JointType::SixDOF;
            let parent: Vec<u16> = vec![0, 1, 2, 3, 4, 5, 6, 7, 8];

            let r_cg1 = Vector3::new(l1 / 2.0, 0.0, 0.0);
            let r_cg2 = Vector3::new(l2 / 2.0, 0.0, 0.0);

            let m1 = PI * 0.09 * 0.09 * l1 * 1000.0;
            let m2 = PI * 0.09 * 0.09 * l2 * 1000.0;

            let inertia_mat1 = Matrix3::new(
                1.0 / 2.0 * m1 * 0.09 * 0.09,
                0.0,
                0.0,
                0.0,
                1.0 / 12.0 * m1 * (3.0 * 0.09 * 0.09 + l1 * l1),
                0.0,
                0.0,
                0.0,
                1.0 / 12.0 * m1 * (3.0 * 0.09 * 0.09 + l1 * l1),
            ) - m1 * skew(&r_cg1) * skew(&r_cg1);

            let inertia_mat2 = Matrix3::new(
                1.0 / 2.0 * m2 * 0.09 * 0.09,
                0.0,
                0.0,
                0.0,
                1.0 / 12.0 * m2 * (3.0 * 0.09 * 0.09 + l2 * l2),
                0.0,
                0.0,
                0.0,
                1.0 / 12.0 * m2 * (3.0 * 0.09 * 0.09 + l2 * l2),
            ) - m2 * skew(&r_cg2) * skew(&r_cg2);

            let M_1 = comp_mass_matrix(m1, &r_cg1, &inertia_mat1);
            let M_2 = comp_mass_matrix(m2, &r_cg2, &inertia_mat2);

            println!("M_1: {}", M_1);

            let mass_matrices = vec![M_1, M_2, M_1, M_2, M_1, M_2, M_1, M_2, M_1];

            // let mut multibody =
                // MultiBody::<9, 14>::new(offset_matrices, mass_matrices, joint_types, parent);
            let mut multibody: MultiBody<9, 14> =
                MultiBody::new(offset_matrices, Some(mass_matrices), None, joint_types, parent, Vector3::new(0.0, 0.0, 9.81), None, None, None, None, None).unwrap();

            // let mut conf: Vec<Isometry3<f64>> = Vec::new();

            let configuration_base = Isometry3::identity();
            // let configuration_base = Isometry3::from_parts(Translation3::new(0.0, 0.0, 0.0), UnitQuaternion::from_axis_angle(&Vector3::x_axis(), PI / 2.0)));

            // let joint_angles = SVector::<f64, 8>::zeros();
            let joint_angles = SVector::<f64, 8>::from_vec(vec![1.0; 8]);

            let conf =
                multibody.minimal_to_homogenous_configuration(&vec![configuration_base], &joint_angles);

            // let mu = SVector::<f64, 14>::zeros();
            let mu = SVector::<f64, 14>::repeat(1.0);
            // let mu_prime = SVector::<f64, 14>::repeat(1.0);
            let mu_prime = SVector::<f64, 14>::from_vec(vec![
                2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0, 10.0, 11.0, 12.0, 13.0, 14.0, 15.0,
            ]);
            // let mu_prime = SVector::<f64, 14>::repeat(1.0);
            // let mu = SVector::<f64, 14>::new(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0);
            // println!("mu: {}", mu);
            let sigma_prime = SVector::<f64, 14>::zeros();
            // let sigma_prime = SVector::<f64, 14>::from_vec(vec![0.0, 0.0, 0.0, 0.0, 0.0, 1.0,0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]);
            let eta = SVector::<f64, 14>::zeros();
            let rigid_body_forces = vec![Vector6::<f64>::zeros(); 9];

            let zeta = multibody.generalized_newton_euler(
                &conf,
                &mu,
                &mu_prime,
                &sigma_prime,
                &rigid_body_forces,
                &eta,
            );

            let M_o = multibody.compute_mass_matrix();

            println!("zeta: {}", zeta);
            println!("mass matrix: {}", M_o);
            println!("mass matrix: {}", M_o.column(13));
        }

        #[test]
        fn jacobian_test() {
            let c1 = Isometry3::<f64>::identity();
            let q12 = UnitQuaternion::from_axis_angle(&Vector3::x_axis(), -PI / 2.0);
            let q21 = UnitQuaternion::from_axis_angle(&Vector3::x_axis(), PI / 2.0);

            let l1 = 0.62;
            let l2 = 0.10;

            let c2 = Isometry3::from_parts(Translation3::new(l1, 0.0, 0.0), UnitQuaternion::identity());
            let c3 = Isometry3::from_parts(Translation3::new(l2, 0.0, 0.0), q12);
            let c4 = Isometry3::from_parts(Translation3::new(l1, 0.0, 0.0), q21);

            // let offset_matrices = vec![c0, c1, c2, c1, c2, c1, c2, c1, c2];
            let offset_matrices = vec![c1, c2, c3, c4, c3, c4, c3, c4, c3];
            // let offset_matrices = vec![c1, c2, c4, c3, c4, c3, c4, c3, c4];

            let mut joint_types = vec![JointType::Revolute; 9];
            joint_types[0] = JointType::SixDOF;
            let parent: Vec<u16> = vec![0, 1, 2, 3, 4, 5, 6, 7, 8];

            let r_cg1 = Vector3::new(l1 / 2.0, 0.0, 0.0);
            let r_cg2 = Vector3::new(l2 / 2.0, 0.0, 0.0);

            let m1 = PI * 0.09 * 0.09 * l1 * 1000.0;
            let m2 = PI * 0.09 * 0.09 * l2 * 1000.0;

            let inertia_mat1 = Matrix3::new(
                1.0 / 2.0 * m1 * 0.09 * 0.09,
                0.0,
                0.0,
                0.0,
                1.0 / 12.0 * m1 * (3.0 * 0.09 * 0.09 + l1 * l1),
                0.0,
                0.0,
                0.0,
                1.0 / 12.0 * m1 * (3.0 * 0.09 * 0.09 + l1 * l1),
            ) - m1 * skew(&r_cg1) * skew(&r_cg1);

            let inertia_mat2 = Matrix3::new(
                1.0 / 2.0 * m2 * 0.09 * 0.09,
                0.0,
                0.0,
                0.0,
                1.0 / 12.0 * m2 * (3.0 * 0.09 * 0.09 + l2 * l2),
                0.0,
                0.0,
                0.0,
                1.0 / 12.0 * m2 * (3.0 * 0.09 * 0.09 + l2 * l2),
            ) - m2 * skew(&r_cg2) * skew(&r_cg2);

            let M_1 = comp_mass_matrix(m1, &r_cg1, &inertia_mat1);
            let M_2 = comp_mass_matrix(m2, &r_cg2, &inertia_mat2);

            let mass_matrices = vec![M_1, M_2, M_1, M_2, M_1, M_2, M_1, M_2, M_1];

            // let mut multibody =
            //     MultiBody::<9, 14>::new(offset_matrices, Some(mass_matrices), joint_types, parent);
            let mut multibody: MultiBody<9, 14> =
                MultiBody::new(offset_matrices, Some(mass_matrices), None, joint_types, parent, Vector3::new(0.0, 0.0, 9.81), None, None, None, None, None).unwrap();

            // let mut conf: Vec<Isometry3<f64>> = Vec::new();

            let configuration_base = Isometry3::identity();
            // let configuration_base = Isometry3::from_parts(Translation3::new(0.0, 0.0, 0.0), UnitQuaternion::from_axis_angle(&Vector3::x_axis(), PI / 2.0)));

            // let joint_angles = SVector::<f64, 8>::zeros();
            // let joint_angles = SVector::<f64, 8>::from_vec(vec![1.0; 8]);
            let joint_angles = SVector::<f64, 8>::from_vec(vec![
                PI / 4.0,
                PI / 3.0,
                PI / 5.0,
                PI / 7.0,
                PI / 15.0,
                PI / 10.0,
                PI / 4.0,
                PI / 2.5,
            ]);

            // let conf = multibody.minimal_to_homogenous_configuration(&vec![configuration_base], &joint_angles);
            let conf =
                multibody.minimal_to_homogenous_configuration(&configuration_base, &joint_angles);

            let jacs = multibody.compute_jacobians(&conf);
            let mu = SVector::<f64, 14>::repeat(1.0);
            let djacs = multibody.compute_jacobian_derivatives(&mu);
            let djac = multibody.compute_jacobian_derivative(&conf, &mu, 7);

            let jac = multibody.compute_jacobian(&conf, 7);
            // println!("size jacs: {}", jacs.size());

            println!("end-effector jac: {}", jacs[7].column(0));
            println!("end-effector jac_i: {}", jac.column(0));
            println!("end-effector djac: {}", djacs[7]);
            println!("end-effector djac_i: {}", djac);

            // assert_relative_eq!(djacs[7].column(6), djac.column(6), epsilon = 0.00000001);
            assert_relative_eq!(jacs[7].column(6), jac.column(6), epsilon = 0.00000001);

            for i in 0..6 {
                let i = 0;
                assert_relative_eq!(djacs[7].column(i), djac.column(i), epsilon = 0.00000001);
            }

            let djac_col: SVector<f64, 6> = djac.column(6).try_into().unwrap();
            let jac_col: SVector<f64, 6> = jac.column(6).try_into().unwrap();
            assert_relative_eq!(
                djac_col,
                Vector6::new(
                    -2.086813791,
                    -3.2459777,
                    -1.17134587,
                    3.35849774,
                    -0.0166829,
                    -1.869255169
                ),
                epsilon = 0.001
            );
            assert_relative_eq!(
                jac_col,
                Vector6::new(
                    0.559325,
                    -0.029257,
                    -0.62383325,
                    -0.04200122,
                    0.99555,
                    -0.08434
                ),
                epsilon = 0.001
            );
        }

        #[test]
        fn forward_dynamics_ab_test() {
            let c1 = Isometry3::<f64>::identity();
            let q12 = UnitQuaternion::from_axis_angle(&Vector3::x_axis(), -PI / 2.0);
            let q21 = UnitQuaternion::from_axis_angle(&Vector3::x_axis(), PI / 2.0);

            let l1 = 0.62;
            let l2 = 0.10;

            let c2 = Isometry3::from_parts(Translation3::new(l1, 0.0, 0.0), UnitQuaternion::identity());
            let c3 = Isometry3::from_parts(Translation3::new(l2, 0.0, 0.0), q12);
            let c4 = Isometry3::from_parts(Translation3::new(l1, 0.0, 0.0), q21);

            // let offset_matrices = vec![c0, c1, c2, c1, c2, c1, c2, c1, c2];
            let offset_matrices = vec![c1, c2, c3, c4, c3, c4, c3, c4, c3];
            // let offset_matrices = vec![c1, c2, c4, c3, c4, c3, c4, c3, c4];

            let mut joint_types = vec![JointType::Revolute; 9];
            joint_types[0] = JointType::SixDOF;
            let parent: Vec<u16> = vec![0, 1, 2, 3, 4, 5, 6, 7, 8];

            let r_cg1 = Vector3::new(l1 / 2.0, 0.0, 0.0);
            let r_cg2 = Vector3::new(l2 / 2.0, 0.0, 0.0);

            let m1 = PI * 0.09 * 0.09 * l1 * 1000.0;
            let m2 = PI * 0.09 * 0.09 * l2 * 1000.0;

            let inertia_mat1 = Matrix3::new(
                1.0 / 2.0 * m1 * 0.09 * 0.09,
                0.0,
                0.0,
                0.0,
                1.0 / 12.0 * m1 * (3.0 * 0.09 * 0.09 + l1 * l1),
                0.0,
                0.0,
                0.0,
                1.0 / 12.0 * m1 * (3.0 * 0.09 * 0.09 + l1 * l1),
            ) - m1 * skew(&r_cg1) * skew(&r_cg1);

            let inertia_mat2 = Matrix3::new(
                1.0 / 2.0 * m2 * 0.09 * 0.09,
                0.0,
                0.0,
                0.0,
                1.0 / 12.0 * m2 * (3.0 * 0.09 * 0.09 + l2 * l2),
                0.0,
                0.0,
                0.0,
                1.0 / 12.0 * m2 * (3.0 * 0.09 * 0.09 + l2 * l2),
            ) - m2 * skew(&r_cg2) * skew(&r_cg2);

            let M_1 = comp_mass_matrix(m1, &r_cg1, &inertia_mat1);
            let M_2 = comp_mass_matrix(m2, &r_cg2, &inertia_mat2);

            let mass_matrices = vec![M_1, M_2, M_1, M_2, M_1, M_2, M_1, M_2, M_1];

            // let mut multibody =
            //     MultiBody::<9, 14>::new(offset_matrices, Some(mass_matrices), joint_types, parent);
            let mut multibody: MultiBody<9, 14> =
                MultiBody::new(offset_matrices, Some(mass_matrices), None, joint_types, parent, Vector3::new(0.0, 0.0, 9.81), Some(vec![Vector3::zeros();9]), Some(vec![Vector3::zeros(); 9]), Some(vec![1.0;9]), Some(vec![1.0;9]), Some(1000.0)).unwrap();

            // let mut conf: Vec<Isometry3<f64>> = Vec::new();

            let configuration_base = Isometry3::identity();
            // let configuration_base = Isometry3::from_parts(Translation3::new(0.0, 0.0, 0.0), UnitQuaternion::from_axis_angle(&Vector3::x_axis(), PI / 2.0)));

            // let joint_angles = SVector::<f64, 8>::zeros();
            // let joint_angles = SVector::<f64, 8>::from_vec(vec![1.0; 8]);
            let joint_angles = SVector::<f64, 8>::from_vec(vec![
                PI / 4.0,
                PI / 3.0,
                PI / 5.0,
                PI / 7.0,
                PI / 15.0,
                PI / 10.0,
                PI / 4.0,
                PI / 2.5,
            ]);

            let conf =
                multibody.minimal_to_homogenous_configuration(&configuration_base, &joint_angles);

            let mu = SVector::<f64, 14>::repeat(1.0);

            let damping_forces = vec![Vector6::<f64>::zeros(); 9];
            let thruster_forces = vec![Vector6::<f64>::zeros(); 9];
            let eta = SVector::<f64, 14>::zeros();
            let lin_vel_current = SVector::<f64, 3>::zeros();
            let lin_accel_current = SVector::<f64, 3>::zeros();

            let accel = multibody.forward_dynamics_ab(&conf, &mu, &damping_forces, &thruster_forces, &eta, &lin_vel_current, &lin_accel_current);

            println!("accel: {}", accel);

        }
    }