17 #include <boost/test/unit_test.hpp>
18 #include <boost/utility/binary.hpp>
20 BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)
25 typedef SE3::HomogeneousMatrixType HomogeneousMatrixType;
26 typedef SE3::ActionMatrixType ActionMatrixType;
28 typedef Eigen::Matrix<double, 4, 1> Vector4;
36 SE3 m_from_quat(
quat, Vector3::Random());
42 HomogeneousMatrixType aMb = amb;
43 HomogeneousMatrixType bMc = bmc;
46 HomogeneousMatrixType aMc = amc;
49 HomogeneousMatrixType bMa = amb.
inverse();
61 Vector3 Mip = (aMb.inverse() * p4).head(3);
65 ActionMatrixType aXb = amb;
66 ActionMatrixType bXc = bmc;
67 ActionMatrixType aXc = amc;
70 ActionMatrixType bXa = amb.
inverse();
73 ActionMatrixType X_identity = identity.toActionMatrix();
76 ActionMatrixType X_identity_inverse = identity.toActionMatrixInverse();
80 BOOST_CHECK(aXb.inverse().transpose().isApprox(amb.toDualActionMatrix()));
90 SE3f::Matrix3 rot_float(amb.
rotation().cast<
float>());
91 SE3f amb_float = amb.
cast<
float>();
99 const SE3 Minv =
M.inverse();
106 const double prec = Eigen::NumTraits<double>::dummy_precision();
108 M.rotation() += prec * SE3::Matrix3::Random();
111 SE3 M_normalized =
M.normalized();
122 typedef SE3::ActionMatrixType ActionMatrixType;
123 typedef Motion::Vector6 Vector6;
135 Vector6 bv2_vec = bv2;
138 std::stringstream ss;
139 ss <<
bv << std::endl;
143 Vector6 bvPbv2_vec =
bv + bv2;
144 BOOST_CHECK(bvPbv2_vec.isApprox(bv_vec + bv2_vec));
149 Motion v_not_zero(Vector6::Ones());
152 Motion v_zero(Vector6::Zero());
160 Vector6 Mbv_vec = -
bv;
166 BOOST_CHECK(bv3.toVector().isApprox(bv_vec + bv2_vec));
181 Motion bvdividedbytwo(bvtwice / 2.);
189 ActionMatrixType aXb = amb;
193 ActionMatrixType bXc = bmc;
194 BOOST_CHECK(bmc.actInv(
bv).toVector().isApprox(bXc.inverse() * bv_vec));
199 BOOST_CHECK(amb.act(
bv).toVector().isApprox(amc.act(cv).toVector()));
204 vxv.toVector().tail(3).norm(),
209 Motion::ActionMatrixType actv2 = bv2.toActionMatrix();
211 BOOST_CHECK(v2xv.toVector().isApprox(actv2 *
bv.toVector()));
215 Force v2xf = bv2.cross(
f);
216 Motion::ActionMatrixType dualactv2 = bv2.toDualActionMatrix();
218 BOOST_CHECK(v2xf.toVector().isApprox(dualactv2 *
f.toVector()));
219 BOOST_CHECK(dualactv2.isApprox(-actv2.transpose()));
223 BOOST_CHECK(vxf.linear().isApprox(
bv.angular().cross(
f.linear())));
225 vxf.angular().norm(),
233 Force avxf = av.cross(af);
235 BOOST_CHECK(avxf.toVector().isApprox(amb.act(bvxf).toVector()));
241 Motion bw = amb.actInv(aw);
242 Motion avxw = av.cross(aw);
244 BOOST_CHECK(avxw.toVector().isApprox(amb.act(bvxw).toVector()));
247 bv.toVector().setOnes();
248 const double eps = 1e-6;
252 bv_approx.linear()[0] +=
eps;
268 Motionf a_float =
a.cast<
float>();
279 typedef Motion::Vector6 Vector6;
284 MotionV6
v(
v_ref.toVector());
288 MotionV6::MotionPlain v2(
v * 2.);
313 v2_ref =
v_ref.cross(v3);
323 Vector6 v6(Vector6::Random());
353 typedef SE3::ActionMatrixType ActionMatrixType;
354 typedef Force::Vector6 Vector6;
364 Vector6 bf2_vec = bf2;
367 std::stringstream ss;
368 ss <<
bf << std::endl;
372 Vector6 bfPbf2_vec =
bf + bf2;
373 BOOST_CHECK(bfPbf2_vec.isApprox(bf_vec + bf2_vec));
376 Vector6 Mbf_vec = -
bf;
382 BOOST_CHECK(bf3.toVector().isApprox(bf_vec + bf2_vec));
393 ActionMatrixType aXb = amb;
394 BOOST_CHECK(amb.act(
bf).toVector().isApprox(aXb.inverse().transpose() * bf_vec));
397 ActionMatrixType bXc = bmc;
398 BOOST_CHECK(bmc.actInv(
bf).toVector().isApprox(bXc.transpose() * bf_vec));
409 Force f_not_zero(Vector6::Ones());
412 Force f_zero(Vector6::Zero());
425 const double eps = 1e-6;
427 bf_approx.linear()[0] += 2. *
eps;
443 Forcef a_float =
a.cast<
float>();
451 const double alpha = 1.5;
462 typedef Force::Vector6 Vector6;
467 ForceV6
f(f_ref.toVector());
471 ForceV6::ForcePlain f2(
f * 2.);
472 Force f2_ref(f_ref * 2.);
496 f2_ref =
v.cross(f_ref);
506 Vector6 v6(Vector6::Random());
518 typedef Inertia::Matrix6 Matrix6;
522 BOOST_CHECK_EQUAL(matI(0, 0), aI.
mass());
523 BOOST_CHECK_EQUAL(matI(1, 1), aI.
mass());
524 BOOST_CHECK_EQUAL(matI(2, 2), aI.
mass());
527 (matI - matI.transpose()).norm(),
530 (matI.topRightCorner<3, 3>() * aI.
lever()).norm(),
535 BOOST_CHECK(I1.matrix().isApprox(Matrix6::Identity()));
548 BOOST_CHECK((bXa.transpose().inverse() * aI.matrix() * bXa.inverse()).isApprox(bI.matrix()));
551 BOOST_CHECK((bXa.transpose() * bI.matrix() * bXa).isApprox(bma.actInv(bI).matrix()));
558 BOOST_CHECK(vxf.toVector().isApprox(vxIv.toVector()));
563 BOOST_CHECK((I1.matrix() + I2.matrix()).isApprox((I1 + I2).matrix()));
571 BOOST_CHECK(I1_other.matrix().isApprox(I1.matrix()));
577 BOOST_CHECK((I1.matrix() + I2.matrix()).isApprox(I12.matrix()));
586 BOOST_CHECK((I1_other.matrix()).isApprox(I1.matrix()));
590 double kinetic_ref =
v.toVector().transpose() * aI.matrix() *
v.toVector();
591 double kinetic = aI.vtiv(
v);
592 BOOST_CHECK_SMALL(kinetic_ref - kinetic, 1e-12);
596 BOOST_CHECK(I1.matrix().isApprox(I1_bis.matrix()));
599 const double sphere_mass = 5.;
600 const double sphere_radius = 2.;
602 const double L_sphere = 2. / 5. * sphere_mass * sphere_radius * sphere_radius;
603 BOOST_CHECK_SMALL(I1.
mass() - sphere_mass, 1e-12);
610 BOOST_CHECK_SMALL(I1.
mass() - 2, 1e-12);
611 BOOST_CHECK_SMALL(I1.
lever().norm(), 1e-12);
616 BOOST_CHECK_SMALL(I1.
mass() - 2, 1e-12);
617 BOOST_CHECK_SMALL(I1.
lever().norm(), 1e-12);
622 BOOST_CHECK_SMALL(I1.
mass() - 2, 1e-12);
623 BOOST_CHECK_SMALL(I1.
lever().norm(), 1e-12);
628 BOOST_CHECK_SMALL(I1.
mass() - 1, 1e-12);
629 BOOST_CHECK_SMALL(I1.
lever().norm(), 1e-12);
631 Symmetric3(3.79705882, 0., 3.79705882, 0., 0., 1.81176471).matrix(),
646 const double eps = 1e-6;
654 Inertia::Matrix6 aIvariation = aI.
variation(
v);
656 Motion::ActionMatrixType vAction =
v.toActionMatrix();
657 Motion::ActionMatrixType vDualAction =
v.toDualActionMatrix();
659 Inertia::Matrix6 aImatrix = aI.matrix();
660 Inertia::Matrix6 aIvariation_ref = vDualAction * aImatrix - aImatrix * vAction;
662 BOOST_CHECK(aIvariation.isApprox(aIvariation_ref));
667 typedef Inertia::Matrix6 Matrix6;
671 const Matrix6 M_ref(
v.toDualActionMatrix() * I.matrix());
673 Inertia::vxi(
v, I,
M);
681 typedef Inertia::Matrix6 Matrix6;
685 const Matrix6 M_ref(I.matrix() *
v.toActionMatrix());
687 Inertia::ivx(
v, I,
M);
695 typedef Inertia::Matrix6 Matrix6;
702 Inertia::vxi(
v, I, M1);
704 Inertia::ivx(
v, I, M2);
713 Inertia::Matrix6 I_inv = I.inverse();
724 BOOST_CHECK_CLOSE(
v[0], I.
mass(), 1e-12);
729 Eigen::Matrix3d I_ov;
730 I_ov <<
v[4],
v[5],
v[7],
v[5],
v[6],
v[8],
v[7],
v[8],
v[9];
739 std::cout << aI << std::endl;
760 typedef Eigen::Matrix<double, 6, N> Matrix6N;
767 Matrix6N iF = Matrix6N::Random(), jF, jFinv, jF_ref, jFinv_ref;
771 for (
int k = 0; k <
N; ++k)
774 jF_ref = jMi.toDualActionMatrix() * iF;
781 Matrix6N iF2 = Matrix6N::Random();
782 jF_ref += jMi.toDualActionMatrix() * iF2;
784 forceSet::se3Action<ADDTO>(jMi, iF2, jF);
787 Matrix6N iF3 = Matrix6N::Random();
788 jF_ref -= jMi.toDualActionMatrix() * iF3;
790 forceSet::se3Action<RMTO>(jMi, iF3, jF);
795 jFinv_ref = jMi.
inverse().toDualActionMatrix() * iF;
798 jFinv_ref += jMi.
inverse().toDualActionMatrix() * iF2;
799 forceSet::se3ActionInverse<ADDTO>(jMi, iF2, jFinv);
802 jFinv_ref -= jMi.
inverse().toDualActionMatrix() * iF3;
803 forceSet::se3ActionInverse<RMTO>(jMi, iF3, jFinv);
808 for (
int k = 0; k <
N; ++k)
811 jF_ref =
v.toDualActionMatrix() * iF;
814 jF_ref +=
v.toDualActionMatrix() * iF2;
815 forceSet::motionAction<ADDTO>(
v, iF2, jF);
818 jF_ref -=
v.toDualActionMatrix() * iF3;
819 forceSet::motionAction<RMTO>(
v, iF3, jF);
825 Matrix6N iV = Matrix6N::Random(), jV, jV_ref, jVinv, jVinv_ref;
829 for (
int k = 0; k <
N; ++k)
832 jV_ref = jMi.toActionMatrix() * iV;
839 Matrix6N iV2 = Matrix6N::Random();
840 jV_ref += jMi.toActionMatrix() * iV2;
841 motionSet::se3Action<ADDTO>(jMi, iV2, jV);
844 Matrix6N iV3 = Matrix6N::Random();
845 jV_ref -= jMi.toActionMatrix() * iV3;
846 motionSet::se3Action<RMTO>(jMi, iV3, jV);
851 jVinv_ref = jMi.
inverse().toActionMatrix() * iV;
854 jVinv_ref += jMi.
inverse().toActionMatrix() * iV2;
855 motionSet::se3ActionInverse<ADDTO>(jMi, iV2, jVinv);
858 jVinv_ref -= jMi.
inverse().toActionMatrix() * iV3;
859 motionSet::se3ActionInverse<RMTO>(jMi, iV3, jVinv);
864 for (
int k = 0; k <
N; ++k)
867 jV_ref =
v.toActionMatrix() * iV;
870 jV_ref +=
v.toActionMatrix() * iV2;
871 motionSet::motionAction<ADDTO>(
v, iV2, jV);
874 jV_ref -=
v.toActionMatrix() * iV3;
875 motionSet::motionAction<RMTO>(
v, iV3, jV);
881 for (
int k = 0; k <
N; ++k)
884 jV_ref = I.matrix() * iV;
887 jV_ref += I.matrix() * iV2;
888 motionSet::inertiaAction<ADDTO>(I, iV2, jV);
891 jV_ref -= I.matrix() * iV3;
892 motionSet::inertiaAction<RMTO>(I, iV3, jV);
898 for (
int k = 0; k <
N; ++k)
901 for (
int k = 0; k <
N; ++k)
902 jF_ref.col(k) =
Force(
Motion(iV.col(k)).cross(
f)).toVector();
905 for (
int k = 0; k <
N; ++k)
906 jF_ref.col(k) +=
Force(
Motion(iV2.col(k)).cross(
f)).toVector();
907 motionSet::act<ADDTO>(iV2,
f, jF);
910 for (
int k = 0; k <
N; ++k)
911 jF_ref.col(k) -=
Force(
Motion(iV3.col(k)).cross(
f)).toVector();
912 motionSet::act<RMTO>(iV3,
f, jF);
920 typedef Motion::Vector6 Vector6;
923 Vector6 v6(Vector6::Random());
948 Matrix3
M(Matrix3::Random());
1013 using namespace Eigen;
1015 Vector3d
v(Vector3d::Random());
1016 const double alpha = 3;
1031 BOOST_CHECK(XAxis::vector() == Vector3d::UnitX());
1032 BOOST_CHECK(YAxis::vector() == Vector3d::UnitY());
1033 BOOST_CHECK(ZAxis::vector() == Vector3d::UnitZ());
1051 for (
int k = 0; k < Axis::dim; ++k)
1072 Force f(Force::Random());
1114 typedef Motion::ActionMatrixType ActionMatrixType;
1115 typedef ActionMatrixType::ColXpr ColType;
1118 ActionMatrixType Sxf, Sxf_ref;
1119 ActionMatrixType
S(ActionMatrixType::Identity());
1128 for (
int k = 0; k < 6; ++k)
1130 MotionRefOnColType Scol(
S.col(k));
1131 ForceRefOnColType(Sxf_ref.col(k)) = Scol.cross(
f);
1137 BOOST_AUTO_TEST_SUITE_END()