rbdlsim/3rdparty/rbdl/tests/SpatialAlgebraTests.cc

587 lines
16 KiB
C++

#include <UnitTest++.h>
#include <iostream>
#include <iomanip>
#include "rbdl/Body.h"
#include "rbdl/rbdl_math.h"
#include "rbdl/rbdl_mathutils.h"
using namespace std;
using namespace RigidBodyDynamics;
using namespace RigidBodyDynamics::Math;
const double TEST_PREC = 1.0e-14;
SpatialMatrix spatial_adjoint(const SpatialMatrix &m) {
SpatialMatrix res (m);
res.block<3,3>(3,0) = m.block<3,3>(0,3);
res.block<3,3>(0,3) = m.block<3,3>(3,0);
return res;
}
SpatialMatrix spatial_inverse(const SpatialMatrix &m) {
SpatialMatrix res(m);
res.block<3,3>(0,0) = m.block<3,3>(0,0).transpose();
res.block<3,3>(3,0) = m.block<3,3>(3,0).transpose();
res.block<3,3>(0,3) = m.block<3,3>(0,3).transpose();
res.block<3,3>(3,3) = m.block<3,3>(3,3).transpose();
return res;
}
Matrix3d get_rotation (const SpatialMatrix &m) {
return m.block<3,3>(0,0);
}
Vector3d get_translation (const SpatialMatrix &m) {
return Vector3d (-m(4,2), m(3,2), -m(3,1));
}
/// \brief Checks the multiplication of a SpatialMatrix with a SpatialVector
TEST(TestSpatialMatrixTimesSpatialVector) {
SpatialMatrix s_matrix (
1., 0., 0., 0., 0., 7.,
0., 2., 0., 0., 8., 0.,
0., 0., 3., 9., 0., 0.,
0., 0., 6., 4., 0., 0.,
0., 5., 0., 0., 5., 0.,
4., 0., 0., 0., 0., 6.
);
SpatialVector s_vector (
1., 2., 3., 4., 5., 6.
);
SpatialVector result;
result = s_matrix * s_vector;
SpatialVector test_result (
43., 44., 45., 34., 35., 40.
);
CHECK_EQUAL (test_result, result);
}
/// \brief Checks the multiplication of a scalar with a SpatialVector
TEST(TestScalarTimesSpatialVector) {
SpatialVector s_vector (
1., 2., 3., 4., 5., 6.
);
SpatialVector result;
result = 3. * s_vector;
SpatialVector test_result(3., 6., 9., 12., 15., 18.);
CHECK_EQUAL (test_result, result);
}
/// \brief Checks the multiplication of a scalar with a SpatialMatrix
TEST(TestScalarTimesSpatialMatrix) {
SpatialMatrix s_matrix (
1., 0., 0., 0., 0., 7.,
0., 2., 0., 0., 8., 0.,
0., 0., 3., 9., 0., 0.,
0., 0., 6., 4., 0., 0.,
0., 5., 0., 0., 5., 0.,
4., 0., 0., 0., 0., 6.
);
SpatialMatrix result;
result = 3. * s_matrix;
SpatialMatrix test_result(
3., 0., 0., 0., 0., 21.,
0., 6., 0., 0., 24., 0.,
0., 0., 9., 27., 0., 0.,
0., 0., 18., 12., 0., 0.,
0., 15., 0., 0., 15., 0.,
12., 0., 0., 0., 0., 18.
);
CHECK_EQUAL (test_result, result);
}
/// \brief Checks the multiplication of a scalar with a SpatialMatrix
TEST(TestSpatialMatrixTimesSpatialMatrix) {
SpatialMatrix s_matrix (
1., 0., 0., 0., 0., 7.,
0., 2., 0., 0., 8., 0.,
0., 0., 3., 9., 0., 0.,
0., 0., 6., 4., 0., 0.,
0., 5., 0., 0., 5., 0.,
4., 0., 0., 0., 0., 6.
);
SpatialMatrix result;
result = s_matrix * s_matrix;
SpatialMatrix test_result(
29., 0., 0., 0., 0., 49.,
0., 44., 0., 0., 56., 0.,
0., 0., 63., 63., 0., 0.,
0., 0., 42., 70., 0., 0.,
0., 35., 0., 0., 65., 0.,
28., 0., 0., 0., 0., 64.
);
CHECK_EQUAL (test_result, result);
}
/// \brief Checks the adjoint method
//
// This method computes a spatial force transformation from a spatial
// motion transformation and vice versa
TEST(TestSpatialMatrixTransformAdjoint) {
SpatialMatrix s_matrix (
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.
);
SpatialMatrix result = spatial_adjoint(s_matrix);
SpatialMatrix test_result_matrix (
1., 2., 3., 19., 20., 21.,
7., 8., 9., 25., 26., 27.,
13., 14., 15., 31., 32., 33.,
4., 5., 6., 22., 23., 24.,
10., 11., 12., 28., 29., 30.,
16., 17., 18., 34., 35., 36.);
CHECK_EQUAL (test_result_matrix, result);
}
TEST(TestSpatialMatrixInverse) {
SpatialMatrix s_matrix (
0, 1, 2, 0, 1, 2,
3, 4, 5, 3, 4, 5,
6, 7, 8, 6, 7, 8,
0, 1, 2, 0, 1, 2,
3, 4, 5, 3, 4, 5,
6, 7, 8, 6, 7, 8
);
SpatialMatrix test_inv (
0, 3, 6, 0, 3, 6,
1, 4, 7, 1, 4, 7,
2, 5, 8, 2, 5, 8,
0, 3, 6, 0, 3, 6,
1, 4, 7, 1, 4, 7,
2, 5, 8, 2, 5, 8
);
CHECK_EQUAL (test_inv, spatial_inverse(s_matrix));
}
TEST(TestSpatialMatrixGetRotation) {
SpatialMatrix spatial_transform (
1., 2., 3., 0., 0., 0.,
4., 5., 6., 0., 0., 0.,
7., 8., 9., 0., 0., 0.,
0., 0., 0., 0., 0., 0.,
0., 0., 0., 0., 0., 0.,
0., 0., 0., 0., 0., 0.
);
// Matrix3d rotation = spatial_transform.block<3,3>(0,0);
Matrix3d rotation = get_rotation (spatial_transform);
Matrix3d test_result (
1., 2., 3.,
4., 5., 6.,
7., 8., 9.
);
CHECK_EQUAL(test_result, rotation);
}
TEST(TestSpatialMatrixGetTranslation) {
SpatialMatrix spatial_transform (
0., 0., 0., 0., 0., 0.,
0., 0., 0., 0., 0., 0.,
0., 0., 0., 0., 0., 0.,
0., -3., 2., 0., 0., 0.,
0., 0., -1., 0., 0., 0.,
0., 0., 0., 0., 0., 0.
);
Vector3d translation = get_translation(spatial_transform);
Vector3d test_result (
1., 2., 3.
);
CHECK_EQUAL( test_result, translation);
}
TEST(TestSpatialVectorCross) {
SpatialVector s_vec (1., 2., 3., 4., 5., 6.);
SpatialMatrix test_cross (
0., -3., 2., 0., 0., 0.,
3., 0., -1., 0., 0., 0.,
-2., 1., 0., 0., 0., 0.,
0., -6., 5., 0., -3., 2.,
6., 0., -4., 3., 0., -1.,
-5., 4., 0., -2., 1., 0.
);
SpatialMatrix s_vec_cross (crossm(s_vec));
CHECK_EQUAL (test_cross, s_vec_cross);
SpatialMatrix s_vec_crossf (crossf(s_vec));
SpatialMatrix test_crossf = -1. * crossm(s_vec).transpose();
CHECK_EQUAL (test_crossf, s_vec_crossf);
}
TEST(TestSpatialVectorCrossmCrossf) {
SpatialVector s_vec (1., 2., 3., 4., 5., 6.);
SpatialVector t_vec (9., 8., 7., 6., 5., 4.);
// by explicitly building the matrices (crossm/f with only one vector)
SpatialVector crossm_s_x_t = crossm(s_vec) * t_vec;
SpatialVector crossf_s_x_t = crossf(s_vec) * t_vec;
// by using direct computation that avoids building of the matrix
SpatialVector crossm_s_t = crossm(s_vec, t_vec);
SpatialVector crossf_s_t = crossf(s_vec, t_vec);
/*
cout << crossm_s_x_t << endl;
cout << "---" << endl;
cout << crossf_s_x_t << endl;
cout << "---" << endl;
cout << crossf_s_t << endl;
*/
CHECK_EQUAL (crossm_s_x_t, crossm_s_t);
CHECK_EQUAL (crossf_s_x_t, crossf_s_t);
}
TEST(TestSpatialTransformApply) {
Vector3d rot (1.1, 1.2, 1.3);
Vector3d trans (1.1, 1.2, 1.3);
SpatialTransform X_st;
X_st.r = trans;
SpatialMatrix X_66_matrix (SpatialMatrix::Zero(6,6));
X_66_matrix = Xrotz_mat (rot[2]) * Xroty_mat (rot[1]) * Xrotx_mat (rot[0]) * Xtrans_mat(trans);
X_st.E = X_66_matrix.block<3,3>(0,0);
// cout << X_66_matrix << endl;
// cout << X_st.E << endl;
// cout << X_st.r.transpose() << endl;
SpatialVector v (1.1, 2.1, 3.1, 4.1, 5.1, 6.1);
SpatialVector v_66_res = X_66_matrix * v;
SpatialVector v_st_res = X_st.apply(v);
// cout << (v_66_res - v_st_res).transpose() << endl;
CHECK_ARRAY_CLOSE (v_66_res.data(), v_st_res.data(), 6, TEST_PREC);
}
TEST(TestSpatialTransformApplyTranspose) {
Vector3d rot (1.1, 1.2, 1.3);
Vector3d trans (1.1, 1.2, 1.3);
SpatialTransform X_st;
X_st.r = trans;
SpatialMatrix X_66_matrix (SpatialMatrix::Zero(6,6));
X_66_matrix = Xrotz_mat (rot[2]) * Xroty_mat (rot[1]) * Xrotx_mat (rot[0]) * Xtrans_mat(trans);
X_st.E = X_66_matrix.block<3,3>(0,0);
// cout << X_66_matrix << endl;
// cout << X_st.E << endl;
// cout << X_st.r.transpose() << endl;
SpatialVector v (1.1, 2.1, 3.1, 4.1, 5.1, 6.1);
SpatialVector v_66_res = X_66_matrix.transpose() * v;
SpatialVector v_st_res = X_st.applyTranspose(v);
// cout << (v_66_res - v_st_res).transpose() << endl;
CHECK_ARRAY_CLOSE (v_66_res.data(), v_st_res.data(), 6, TEST_PREC);
}
TEST(TestSpatialTransformApplyAdjoint) {
SpatialTransform X (
Xrotz (0.5) *
Xroty (0.9) *
Xrotx (0.2) *
Xtrans (Vector3d (1.1, 1.2, 1.3))
);
SpatialMatrix X_adjoint = X.toMatrixAdjoint();
SpatialVector f (1.1, 2.1, 4.1, 9.2, 3.3, 0.8);
SpatialVector f_apply = X.applyAdjoint(f);
SpatialVector f_matrix = X_adjoint * f;
CHECK_ARRAY_CLOSE (f_matrix.data(), f_apply.data(), 6, TEST_PREC);
}
TEST(TestSpatialTransformToMatrix) {
Vector3d rot (1.1, 1.2, 1.3);
Vector3d trans (1.1, 1.2, 1.3);
SpatialMatrix X_matrix (SpatialMatrix::Zero(6,6));
X_matrix = Xrotz_mat (rot[2]) * Xroty_mat (rot[1]) * Xrotx_mat (rot[0]) * Xtrans_mat(trans);
SpatialTransform X_st;
X_st.E = X_matrix.block<3,3>(0,0);
X_st.r = trans;
// SpatialMatrix X_diff = X_st.toMatrix() - X_matrix;
// cout << "Error: " << endl << X_diff << endl;
CHECK_ARRAY_CLOSE (X_matrix.data(), X_st.toMatrix().data(), 36, TEST_PREC);
}
TEST(TestSpatialTransformToMatrixAdjoint) {
Vector3d rot (1.1, 1.2, 1.3);
Vector3d trans (1.1, 1.2, 1.3);
SpatialMatrix X_matrix (SpatialMatrix::Zero(6,6));
X_matrix = Xrotz_mat (rot[2]) * Xroty_mat (rot[1]) * Xrotx_mat (rot[0]) * Xtrans_mat(trans);
SpatialTransform X_st;
X_st.E = X_matrix.block<3,3>(0,0);
X_st.r = trans;
// SpatialMatrix X_diff = X_st.toMatrixAdjoint() - spatial_adjoint(X_matrix);
// cout << "Error: " << endl << X_diff << endl;
CHECK_ARRAY_CLOSE (spatial_adjoint(X_matrix).data(), X_st.toMatrixAdjoint().data(), 36, TEST_PREC);
}
TEST(TestSpatialTransformToMatrixTranspose) {
Vector3d rot (1.1, 1.2, 1.3);
Vector3d trans (1.1, 1.2, 1.3);
SpatialMatrix X_matrix (SpatialMatrix::Zero(6,6));
X_matrix = Xrotz_mat (rot[2]) * Xroty_mat (rot[1]) * Xrotx_mat (rot[0]) * Xtrans_mat(trans);
SpatialTransform X_st;
X_st.E = X_matrix.block<3,3>(0,0);
X_st.r = trans;
// we have to copy the matrix as it is only transposed via a flag and
// thus data() does not return the proper data.
SpatialMatrix X_matrix_transposed = X_matrix.transpose();
// SpatialMatrix X_diff = X_st.toMatrixTranspose() - X_matrix_transposed;
// cout << "Error: " << endl << X_diff << endl;
// cout << "X_st: " << endl << X_st.toMatrixTranspose() << endl;
// cout << "X: " << endl << X_matrix_transposed() << endl;
CHECK_ARRAY_CLOSE (X_matrix_transposed.data(), X_st.toMatrixTranspose().data(), 36, TEST_PREC);
}
TEST(TestSpatialTransformMultiply) {
Vector3d rot (1.1, 1.2, 1.3);
Vector3d trans (1.1, 1.2, 1.3);
SpatialMatrix X_matrix_1 (SpatialMatrix::Zero(6,6));
SpatialMatrix X_matrix_2 (SpatialMatrix::Zero(6,6));
X_matrix_1 = Xrotz_mat (rot[2]) * Xroty_mat (rot[1]) * Xrotx_mat (rot[0]) * Xtrans_mat(trans);
X_matrix_2 = Xrotz_mat (rot[2]) * Xroty_mat (rot[1]) * Xrotx_mat (rot[0]) * Xtrans_mat(trans);
SpatialTransform X_st_1;
SpatialTransform X_st_2;
X_st_1.E = X_matrix_1.block<3,3>(0,0);
X_st_1.r = trans;
X_st_2.E = X_matrix_2.block<3,3>(0,0);
X_st_2.r = trans;
SpatialTransform X_st_res = X_st_1 * X_st_2;
SpatialMatrix X_matrix_res = X_matrix_1 * X_matrix_2;
// SpatialMatrix X_diff = X_st_res.toMatrix() - X_matrix_res;
// cout << "Error: " << endl << X_diff << endl;
CHECK_ARRAY_CLOSE (X_matrix_res.data(), X_st_res.toMatrix().data(), 36, TEST_PREC);
}
TEST(TestSpatialTransformMultiplyEqual) {
Vector3d rot (1.1, 1.2, 1.3);
Vector3d trans (1.1, 1.2, 1.3);
SpatialMatrix X_matrix_1 (SpatialMatrix::Zero(6,6));
SpatialMatrix X_matrix_2 (SpatialMatrix::Zero(6,6));
X_matrix_1 = Xrotz_mat (rot[2]) * Xroty_mat (rot[1]) * Xrotx_mat (rot[0]) * Xtrans_mat(trans);
X_matrix_2 = Xrotz_mat (rot[2]) * Xroty_mat (rot[1]) * Xrotx_mat (rot[0]) * Xtrans_mat(trans);
SpatialTransform X_st_1;
SpatialTransform X_st_2;
X_st_1.E = X_matrix_1.block<3,3>(0,0);
X_st_1.r = trans;
X_st_2.E = X_matrix_2.block<3,3>(0,0);
X_st_2.r = trans;
SpatialTransform X_st_res = X_st_1;
X_st_res *= X_st_2;
SpatialMatrix X_matrix_res = X_matrix_1 * X_matrix_2;
// SpatialMatrix X_diff = X_st_res.toMatrix() - X_matrix_res;
// cout << "Error: " << endl << X_diff << endl;
CHECK_ARRAY_CLOSE (X_matrix_res.data(), X_st_res.toMatrix().data(), 36, TEST_PREC);
}
TEST(TestXrotAxis) {
SpatialTransform X_rotX = Xrotx (M_PI * 0.15);
SpatialTransform X_rotX_axis = Xrot (M_PI * 0.15, Vector3d (1., 0., 0.));
CHECK_ARRAY_CLOSE (X_rotX.toMatrix().data(), X_rotX_axis.toMatrix().data(), 36, TEST_PREC);
// all the other axes
SpatialTransform X_rotX_90 = Xrotx (M_PI * 0.5);
SpatialTransform X_rotX_90_axis = Xrot (M_PI * 0.5, Vector3d (1., 0., 0.));
CHECK_ARRAY_CLOSE (X_rotX_90.toMatrix().data(), X_rotX_90_axis.toMatrix().data(), 36, TEST_PREC);
SpatialTransform X_rotY_90 = Xroty (M_PI * 0.5);
SpatialTransform X_rotY_90_axis = Xrot (M_PI * 0.5, Vector3d (0., 1., 0.));
CHECK_ARRAY_CLOSE (X_rotY_90.toMatrix().data(), X_rotY_90_axis.toMatrix().data(), 36, TEST_PREC);
SpatialTransform X_rotZ_90 = Xrotz (M_PI * 0.5);
SpatialTransform X_rotZ_90_axis = Xrot (M_PI * 0.5, Vector3d (0., 0., 1.));
CHECK_ARRAY_CLOSE (X_rotZ_90.toMatrix().data(), X_rotZ_90_axis.toMatrix().data(), 36, TEST_PREC);
}
TEST(TestSpatialTransformApplySpatialRigidBodyInertiaAdd) {
SpatialRigidBodyInertia rbi (
1.1,
Vector3d (1.2, 1.3, 1.4),
Matrix3d (
1.1, 0.5, 0.3,
0.5, 1.2, 0.4,
0.3, 0.4, 1.3
));
SpatialMatrix rbi_matrix_added = rbi.toMatrix() + rbi.toMatrix();
SpatialRigidBodyInertia rbi_added = rbi + rbi;
// cout << "rbi = " << endl << rbi.toMatrix() << endl;
// cout << "rbi_added = " << endl << rbi_added.toMatrix() << endl;
// cout << "rbi_matrix_added = " << endl << rbi_matrix_added << endl;
// cout << "diff = " << endl <<
// rbi_added.toMatrix() - rbi_matrix_added << endl;
CHECK_ARRAY_CLOSE (
rbi_matrix_added.data(),
rbi_added.toMatrix().data(),
36,
TEST_PREC
);
}
TEST(TestSpatialTransformApplySpatialRigidBodyInertiaFull) {
SpatialRigidBodyInertia rbi (
1.1,
Vector3d (1.2, 1.3, 1.4),
Matrix3d (
1.1, 0.5, 0.3,
0.5, 1.2, 0.4,
0.3, 0.4, 1.3
));
SpatialTransform X (
Xrotz (0.5) *
Xroty (0.9) *
Xrotx (0.2) *
Xtrans (Vector3d (1.1, 1.2, 1.3))
);
SpatialRigidBodyInertia rbi_transformed = X.apply (rbi);
SpatialMatrix rbi_matrix_transformed = X.toMatrixAdjoint () * rbi.toMatrix() * X.inverse().toMatrix();
CHECK_ARRAY_CLOSE (
rbi_matrix_transformed.data(),
rbi_transformed.toMatrix().data(),
36,
TEST_PREC
);
}
TEST(TestSpatialTransformApplyTransposeSpatialRigidBodyInertiaFull) {
SpatialRigidBodyInertia rbi (
1.1,
Vector3d (1.2, 1.3, 1.4),
Matrix3d (
1.1, 0.5, 0.3,
0.5, 1.2, 0.4,
0.3, 0.4, 1.3
));
SpatialTransform X (
Xrotz (0.5) *
Xroty (0.9) *
Xrotx (0.2) *
Xtrans (Vector3d (1.1, 1.2, 1.3))
);
SpatialRigidBodyInertia rbi_transformed = X.applyTranspose (rbi);
SpatialMatrix rbi_matrix_transformed = X.toMatrixTranspose() * rbi.toMatrix() * X.toMatrix();
CHECK_ARRAY_CLOSE (
rbi_matrix_transformed.data(),
rbi_transformed.toMatrix().data(),
36,
TEST_PREC
);
}
TEST(TestSpatialRigidBodyInertiaCreateFromMatrix) {
double mass = 1.1;
Vector3d com (0., 0., 0.);
Matrix3d inertia (
1.1, 0.5, 0.3,
0.5, 1.2, 0.4,
0.3, 0.4, 1.3
);
SpatialRigidBodyInertia body_rbi(mass, com , inertia);
SpatialMatrix spatial_inertia = body_rbi.toMatrix();
SpatialRigidBodyInertia rbi;
rbi.createFromMatrix (spatial_inertia);
CHECK_EQUAL (mass, rbi.m);
CHECK_ARRAY_EQUAL (Vector3d(mass * com).data(), rbi.h.data(), 3);
Matrix3d rbi_I_matrix (
rbi.Ixx, rbi.Iyx, rbi.Izx,
rbi.Iyx, rbi.Iyy, rbi.Izy,
rbi.Izx, rbi.Izy, rbi.Izz
);
CHECK_ARRAY_EQUAL (inertia.data(), rbi_I_matrix.data(), 9);
}
#ifdef USE_SLOW_SPATIAL_ALGEBRA
TEST(TestSpatialLinSolve) {
SpatialVector b (1, 2, 0, 1, 1, 1);
SpatialMatrix A (
1., 2., 3., 0., 0., 0.,
3., 4., 5., 0., 0., 0.,
6., 7., 7., 0., 0., 0.,
0., 0., 0., 1., 0., 0.,
0., 0., 0., 0., 1., 0.,
0., 0., 0., 0., 0., 1.
);
SpatialVector x = SpatialLinSolve (A, b);
SpatialVector x_test (3.5, -6.5, 3.5, 1, 1, 1);
CHECK_ARRAY_CLOSE (x_test.data(), x.data(), 6, TEST_PREC);
}
#endif