diff --git a/CMakeLists.txt b/CMakeLists.txt
index 93988c81df2ea454b1fd753585881cd5e1de95f4..43816ecd3fcfc32dadad57da11cd0963d5f4fe54 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -83,7 +83,6 @@ SET(${PROJECT_NAME}_MATH_HEADERS
SET(${PROJECT_NAME}_TOOLS_HEADERS
tools/timer.hpp
- tools/matrix-comparison.hpp
)
SET(${PROJECT_NAME}_SPATIAL_HEADERS
diff --git a/src/tools/matrix-comparison.hpp b/src/tools/matrix-comparison.hpp
deleted file mode 100644
index 2e597d235f139cad0a8019243517fcc3723019e0..0000000000000000000000000000000000000000
--- a/src/tools/matrix-comparison.hpp
+++ /dev/null
@@ -1,39 +0,0 @@
-//
-// Copyright (c) 2015 CNRS
-//
-// This file is part of Pinocchio
-// Pinocchio is free software: you can redistribute it
-// and/or modify it under the terms of the GNU Lesser General Public
-// License as published by the Free Software Foundation, either version
-// 3 of the License, or (at your option) any later version.
-//
-// Pinocchio is distributed in the hope that it will be
-// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
-// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
-// General Lesser Public License for more details. You should have
-// received a copy of the GNU Lesser General Public License along with
-// Pinocchio If not, see
-// <http://www.gnu.org/licenses/>.
-
-#include <Eigen/Core>
-#include <limits>
-
-#include <boost/test/unit_test.hpp>
-#include <boost/test/floating_point_comparison.hpp>
-
-inline void is_matrix_absolutely_closed (const Eigen::MatrixXd & M1,
- const Eigen::MatrixXd & M2,
- double tolerance = std::numeric_limits <Eigen::MatrixXd::Scalar>::epsilon ()
- )
-{
- BOOST_REQUIRE_EQUAL (M1.rows (), M2.rows ());
- BOOST_REQUIRE_EQUAL (M1.cols (), M2.cols ());
-
- for (Eigen::MatrixXd::Index i = 0; i < M1.rows (); i++)
- {
- for (Eigen::MatrixXd::Index j = 0; j < M1.cols (); j++)
- {
- BOOST_CHECK_SMALL (M1 (i,j) - M2 (i,j), tolerance);
- }
- }
-}
diff --git a/unittest/cholesky.cpp b/unittest/cholesky.cpp
index b41493b5f06af45de59b54ff84494b333b1cf158..2e57d01d42596eccab4b4a4e0aef64275682c704 100644
--- a/unittest/cholesky.cpp
+++ b/unittest/cholesky.cpp
@@ -37,7 +37,6 @@
#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE CholeskyTest
#include <boost/test/unit_test.hpp>
-#include "pinocchio/tools/matrix-comparison.hpp"
#include <boost/utility/binary.hpp>
@@ -70,31 +69,31 @@ BOOST_AUTO_TEST_CASE ( test_cholesky )
std::cout << "D = [\n" << D.transpose() << "];" << std::endl;
#endif
- is_matrix_absolutely_closed(M, U*D.asDiagonal()*U.transpose() , 1e-12);
+ BOOST_CHECK(M.isApprox(U*D.asDiagonal()*U.transpose() , 1e-12));
Eigen::VectorXd v = Eigen::VectorXd::Random(model.nv);
// std::cout << "v = [" << v.transpose() << "]';" << std::endl;
Eigen::VectorXd Uv = v; se3::cholesky::Uv(model,data,Uv);
- is_matrix_absolutely_closed(Uv, U*v, 1e-12);
+ BOOST_CHECK(Uv.isApprox(U*v, 1e-12));
Eigen::VectorXd Utv = v; se3::cholesky::Utv(model,data,Utv);
- is_matrix_absolutely_closed(Utv, U.transpose()*v, 1e-12);
+ BOOST_CHECK(Utv.isApprox(U.transpose()*v, 1e-12));
Eigen::VectorXd Uiv = v; se3::cholesky::Uiv(model,data,Uiv);
- is_matrix_absolutely_closed(Uiv, U.inverse()*v, 1e-12);
+ BOOST_CHECK(Uiv.isApprox(U.inverse()*v, 1e-12));
Eigen::VectorXd Utiv = v; se3::cholesky::Utiv(model,data,Utiv);
- is_matrix_absolutely_closed(Utiv, U.transpose().inverse()*v, 1e-12);
+ BOOST_CHECK(Utiv.isApprox(U.transpose().inverse()*v, 1e-12));
Eigen::VectorXd Miv = v; se3::cholesky::solve(model,data,Miv);
- is_matrix_absolutely_closed(Miv, M.inverse()*v, 1e-12);
+ BOOST_CHECK(Miv.isApprox(M.inverse()*v, 1e-12));
Eigen::VectorXd Mv = v; se3::cholesky::Mv(model,data,Mv,true);
- is_matrix_absolutely_closed(Mv, M*v, 1e-12);
+ BOOST_CHECK(Mv.isApprox(M*v, 1e-12));
Mv = v; se3::cholesky::Mv(model,data,Mv,false);
- is_matrix_absolutely_closed(Mv, M*v, 1e-12);
+ BOOST_CHECK(Mv.isApprox(M*v, 1e-12));
}
diff --git a/unittest/com.cpp b/unittest/com.cpp
index f96bfe346300aebeb3b6c57095306ef0e1402e36..b560be65bb33b947cb0f6119a10b7fc899b74de8 100644
--- a/unittest/com.cpp
+++ b/unittest/com.cpp
@@ -33,7 +33,6 @@
#define BOOST_TEST_MODULE ComTest
#include <boost/test/unit_test.hpp>
#include <boost/utility/binary.hpp>
-#include "pinocchio/tools/matrix-comparison.hpp"
BOOST_AUTO_TEST_SUITE ( ComTest)
@@ -57,16 +56,16 @@ BOOST_AUTO_TEST_CASE ( test_com )
/* Test COM against CRBA*/
Vector3d com = centerOfMass(model,data,q);
- is_matrix_absolutely_closed(data.com[0], getComFromCrba(model,data), 1e-12);
+ BOOST_CHECK(data.com[0].isApprox(getComFromCrba(model,data), 1e-12));
/* Test COM against Jcom (both use different way of compute the COM. */
com = centerOfMass(model,data,q);
jacobianCenterOfMass(model,data,q);
- is_matrix_absolutely_closed(com, data.com[0], 1e-12);
+ BOOST_CHECK(com.isApprox(data.com[0], 1e-12));
/* Test COM against Jcom (both use different way of compute the COM. */
centerOfMassAcceleration(model,data,q,v,a);
- is_matrix_absolutely_closed(com, data.com[0], 1e-12);
+ BOOST_CHECK(com.isApprox(data.com[0], 1e-12));
/* Test vCoM againt nle algorithm without gravity field */
a.setZero();
@@ -75,14 +74,14 @@ BOOST_AUTO_TEST_CASE ( test_com )
nonLinearEffects(model, data, q, v);
se3::SE3::Vector3 acom_from_nle (data.nle.head <3> ()/data.mass[0]);
- is_matrix_absolutely_closed(data.liMi[1].rotation() * acom_from_nle, data.acom[0], 1e-12);
+ BOOST_CHECK((data.liMi[1].rotation() * acom_from_nle).isApprox(data.acom[0], 1e-12));
/* Test Jcom against CRBA */
Eigen::MatrixXd Jcom = jacobianCenterOfMass(model,data,q);
- is_matrix_absolutely_closed(data.Jcom, getJacobianComFromCrba(model,data), 1e-12);
+ BOOST_CHECK(data.Jcom.isApprox(getJacobianComFromCrba(model,data), 1e-12));
/* Test CoM vecolity againt jacobianCenterOfMass */
- is_matrix_absolutely_closed(Jcom * v, data.vcom[0], 1e-12);
+ BOOST_CHECK((Jcom * v).isApprox(data.vcom[0], 1e-12));
// std::cout << "com = [ " << data.com[0].transpose() << " ];" << std::endl;
diff --git a/unittest/compute-all-terms.cpp b/unittest/compute-all-terms.cpp
index f288b617f483a847909d95dce1516eaeb4afb4a5..81cb5cb29e9872bf34daf18fee7a29c74cd50865 100644
--- a/unittest/compute-all-terms.cpp
+++ b/unittest/compute-all-terms.cpp
@@ -37,7 +37,6 @@
#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE CATTests
#include <boost/test/unit_test.hpp>
-#include "pinocchio/tools/matrix-comparison.hpp"
#include <iostream>
@@ -72,9 +71,10 @@ BOOST_AUTO_TEST_CASE ( test_against_algo )
crba(model,data_other,q);
computeJacobians(model,data_other,q);
- is_matrix_absolutely_closed (data.nle, data_other.nle);
- is_matrix_absolutely_closed (data.M.triangularView<Eigen::Upper>(), data_other.M.triangularView<Eigen::Upper>());
- is_matrix_absolutely_closed (data.J, data_other.J);
+ BOOST_CHECK (data.nle.isApprox(data_other.nle, 1e-12));
+ BOOST_CHECK (Eigen::MatrixXd(data.M.triangularView<Eigen::Upper>())
+ .isApprox(Eigen::MatrixXd(data_other.M.triangularView<Eigen::Upper>()), 1e-12));
+ BOOST_CHECK (data.J.isApprox(data_other.J, 1e-12));
// -------
q.setZero ();
@@ -86,9 +86,10 @@ BOOST_AUTO_TEST_CASE ( test_against_algo )
crba(model,data_other,q);
computeJacobians(model,data_other,q);
- is_matrix_absolutely_closed (data.nle, data_other.nle);
- is_matrix_absolutely_closed (data.M.triangularView<Eigen::Upper>(), data_other.M.triangularView<Eigen::Upper>());
- is_matrix_absolutely_closed (data.J, data_other.J);
+ BOOST_CHECK (data.nle.isApprox(data_other.nle, 1e-12));
+ BOOST_CHECK (Eigen::MatrixXd(data.M.triangularView<Eigen::Upper>())
+ .isApprox(Eigen::MatrixXd(data_other.M.triangularView<Eigen::Upper>()), 1e-12));
+ BOOST_CHECK (data.J.isApprox(data_other.J, 1e-12));
// -------
q.setOnes ();
@@ -100,9 +101,10 @@ BOOST_AUTO_TEST_CASE ( test_against_algo )
crba(model,data_other,q);
computeJacobians(model,data_other,q);
- is_matrix_absolutely_closed (data.nle, data_other.nle);
- is_matrix_absolutely_closed (data.M.triangularView<Eigen::Upper>(), data_other.M.triangularView<Eigen::Upper>());
- is_matrix_absolutely_closed (data.J, data_other.J);
+ BOOST_CHECK (data.nle.isApprox(data_other.nle, 1e-12));
+ BOOST_CHECK (Eigen::MatrixXd(data.M.triangularView<Eigen::Upper>())
+ .isApprox(Eigen::MatrixXd(data_other.M.triangularView<Eigen::Upper>()), 1e-12));
+ BOOST_CHECK (data.J.isApprox(data_other.J, 1e-12));
// -------
q.setRandom ();
@@ -114,9 +116,10 @@ BOOST_AUTO_TEST_CASE ( test_against_algo )
crba(model,data_other,q);
computeJacobians(model,data_other,q);
- is_matrix_absolutely_closed (data.nle, data_other.nle);
- is_matrix_absolutely_closed (data.M.triangularView<Eigen::Upper>(), data_other.M.triangularView<Eigen::Upper>());
- is_matrix_absolutely_closed (data.J, data_other.J);
+ BOOST_CHECK (data.nle.isApprox(data_other.nle, 1e-12));
+ BOOST_CHECK (Eigen::MatrixXd(data.M.triangularView<Eigen::Upper>())
+ .isApprox(Eigen::MatrixXd(data_other.M.triangularView<Eigen::Upper>()), 1e-12));
+ BOOST_CHECK (data.J.isApprox(data_other.J, 1e-12));
}
BOOST_AUTO_TEST_SUITE_END ()
diff --git a/unittest/constraint.cpp b/unittest/constraint.cpp
index 3aa1c4c4beaf2f77b79aa044af741b9e5e828a9c..cb074d34e2a1ac934af1950b3e205b7f482e826c 100644
--- a/unittest/constraint.cpp
+++ b/unittest/constraint.cpp
@@ -28,7 +28,6 @@
#define BOOST_TEST_MODULE ConstraintTest
#include <boost/test/unit_test.hpp>
#include <boost/utility/binary.hpp>
-#include "pinocchio/tools/matrix-comparison.hpp"
BOOST_AUTO_TEST_SUITE ( Constraint )
@@ -56,22 +55,21 @@ BOOST_AUTO_TEST_CASE ( test_ForceSet )
ForceSet F3(Eigen::Matrix<double,3,12>::Random(),Eigen::Matrix<double,3,12>::Random());
ForceSet F4 = amb.act(F3);
SE3::Matrix6 aXb= amb;
- is_matrix_absolutely_closed((aXb.transpose().inverse()*F3.matrix()), F4.matrix(), 1e-12);
+ BOOST_CHECK((aXb.transpose().inverse()*F3.matrix()).isApprox(F4.matrix(), 1e-12));
ForceSet bF = bmc.act(F3);
ForceSet aF = amb.act(bF);
ForceSet aF2 = amc.act(F3);
- is_matrix_absolutely_closed(aF.matrix(), aF2.matrix(), 1e-12);
+ BOOST_CHECK(aF.matrix().isApprox(aF2.matrix(), 1e-12));
ForceSet F36 = amb.act(F3.block(3,6));
- is_matrix_absolutely_closed((aXb.transpose().inverse()*F3.matrix().block(0,3,6,6)), F36.matrix(), 1e-12);
+ BOOST_CHECK((aXb.transpose().inverse()*F3.matrix().block(0,3,6,6)).isApprox(F36.matrix(), 1e-12));
ForceSet F36full(12); F36full.block(3,6) = amb.act(F3.block(3,6));
- is_matrix_absolutely_closed((aXb.transpose().inverse()*F3.matrix().block(0,3,6,6)),
- F36full.matrix().block(0,3,6,6),
- 1e-12);
+ BOOST_CHECK((aXb.transpose().inverse()*F3.matrix().block(0,3,6,6)).isApprox(F36full.matrix().block(0,3,6,6),
+ 1e-12));
}
BOOST_AUTO_TEST_CASE ( test_ConstraintRX )
@@ -87,13 +85,12 @@ BOOST_AUTO_TEST_CASE ( test_ConstraintRX )
ForceSet F(1); F.block(0,1) = Y*S;
std::cout << "Y*S = \n" << (Y*S).matrix() << std::endl;
std::cout << "F=Y*S = \n" << F.matrix() << std::endl;
- is_matrix_absolutely_closed(F.matrix(), Y.toMatrix().col(3), 1e-12);
+ BOOST_CHECK(F.matrix().isApprox(Y.toMatrix().col(3), 1e-12));
ForceSet F2( Eigen::Matrix<double,3,9>::Random(),Eigen::Matrix<double,3,9>::Random() );
Eigen::MatrixXd StF2 = S.transpose()*F2.block(5,3);
- is_matrix_absolutely_closed(StF2,
- ConstraintXd(S).matrix().transpose()*F2.matrix().block(0,5,6,3),
- 1e-12);
+ BOOST_CHECK(StF2.isApprox(ConstraintXd(S).matrix().transpose()*F2.matrix().block(0,5,6,3)
+ , 1e-12));
}
BOOST_AUTO_TEST_SUITE_END ()
diff --git a/unittest/crba.cpp b/unittest/crba.cpp
index 79aba6a0ef3ab903073a7474b4427d4156a3705f..d2ac90311afb1dc8b6b58c5b971bd4c9aba6dae7 100644
--- a/unittest/crba.cpp
+++ b/unittest/crba.cpp
@@ -35,7 +35,6 @@
#define BOOST_TEST_MODULE CrbaTest
#include <boost/test/unit_test.hpp>
#include <boost/utility/binary.hpp>
-#include "pinocchio/tools/matrix-comparison.hpp"
BOOST_AUTO_TEST_SUITE ( CrbaTest)
@@ -82,7 +81,7 @@ BOOST_AUTO_TEST_CASE ( test_crba )
// std::cout << "Mcrb = [ " << data.M << " ];" << std::endl;
// std::cout << "Mrne = [ " << M << " ]; " << std::endl;
- is_matrix_absolutely_closed(M,data.M,1e-12);
+ BOOST_CHECK(M.isApprox(data.M,1e-12));
std::cout << "(the time score in debug mode is not relevant) " ;
diff --git a/unittest/energy.cpp b/unittest/energy.cpp
index 7308d27d7794d72bbd3c1fc7bcb765f01649d8a6..7584c4401c9aa15e33d6d157fb97b54ba1b0aa7c 100644
--- a/unittest/energy.cpp
+++ b/unittest/energy.cpp
@@ -24,7 +24,6 @@
#define BOOST_TEST_MODULE ComTest
#include <boost/test/unit_test.hpp>
#include <boost/utility/binary.hpp>
-#include "pinocchio/tools/matrix-comparison.hpp"
#include <boost/test/floating_point_comparison.hpp>
BOOST_AUTO_TEST_SUITE(EnergyTest)
diff --git a/unittest/geom.cpp b/unittest/geom.cpp
index 4d211e9b8a470a991a34b7c845e26916c5254956..8487396e230378c353a0a6cfbf5287ee1691ffbe 100644
--- a/unittest/geom.cpp
+++ b/unittest/geom.cpp
@@ -55,7 +55,6 @@
#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE GeomTest
#include <boost/test/unit_test.hpp>
-#include "pinocchio/tools/matrix-comparison.hpp"
typedef std::map <std::string, se3::SE3> PositionsMap_t;
typedef std::map <std::string, se3::SE3> JointPositionsMap_t;
diff --git a/unittest/jacobian.cpp b/unittest/jacobian.cpp
index df8790a64963c26960d6d52e7479ca272301ec37..7afe932ff3d7dbf0ec9139ee38c39fa273ee2c0f 100644
--- a/unittest/jacobian.cpp
+++ b/unittest/jacobian.cpp
@@ -28,7 +28,6 @@
#define BOOST_TEST_MODULE JacobianTest
#include <boost/test/unit_test.hpp>
#include <boost/utility/binary.hpp>
-#include "pinocchio/tools/matrix-comparison.hpp"
BOOST_AUTO_TEST_SUITE ( JacobianTest)
@@ -53,7 +52,7 @@ BOOST_AUTO_TEST_CASE ( test_jacobian )
VectorXd qddot = VectorXd::Zero(model.nv);
rnea( model,data,q,qdot,qddot );
Motion v = data.oMi[idx].act( data.v[idx] );
- is_matrix_absolutely_closed(v.toVector(),Jrh*qdot,1e-12);
+ BOOST_CHECK(v.toVector().isApprox(Jrh*qdot,1e-12));
/* Test local jacobian: rhJrh == rhXo oJrh */
@@ -61,12 +60,12 @@ BOOST_AUTO_TEST_CASE ( test_jacobian )
getJacobian<true>(model,data,idx,rhJrh);
MatrixXd XJrh(6,model.nv);
motionSet::se3Action( data.oMi[idx].inverse(), Jrh,XJrh );
- is_matrix_absolutely_closed(XJrh,rhJrh,1e-12);
+ BOOST_CHECK(XJrh.isApprox(rhJrh,1e-12));
data.J.fill(0);
XJrh = jacobian(model,data,q,idx);
- is_matrix_absolutely_closed(XJrh,rhJrh,1e-12);
+ BOOST_CHECK(XJrh.isApprox(rhJrh,1e-12));
}
diff --git a/unittest/joints.cpp b/unittest/joints.cpp
index f84d551bf32bd4178f30c2d5ee46bb525fc352b1..d7c2338610c9627e560388101421a34bbac366f8 100644
--- a/unittest/joints.cpp
+++ b/unittest/joints.cpp
@@ -36,7 +36,6 @@
#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE JointsTest
#include <boost/test/unit_test.hpp>
-#include "pinocchio/tools/matrix-comparison.hpp"
//#define VERBOSE
@@ -95,10 +94,10 @@ BOOST_AUTO_TEST_CASE ( test_kinematics )
printOutJointData <JointDataSphericalZYX> (q, q_dot, joint_data);
- is_matrix_absolutely_closed (expected_configuration.rotation (), joint_data.M.rotation());
- is_matrix_absolutely_closed (expected_configuration.translation (), joint_data.M.translation ());
- is_matrix_absolutely_closed (expected_v_J.toVector (), ((Motion) joint_data.v).toVector());
- is_matrix_absolutely_closed (expected_c_J.toVector (), ((Motion) joint_data.c).toVector());
+ BOOST_CHECK (expected_configuration.rotation ().isApprox(joint_data.M.rotation(), 1e-12));
+ BOOST_CHECK (expected_configuration.translation ().isApprox(joint_data.M.translation (), 1e-12));
+ BOOST_CHECK (expected_v_J.toVector ().isApprox(((Motion) joint_data.v).toVector(), 1e-12));
+ BOOST_CHECK (expected_c_J.toVector ().isApprox(((Motion) joint_data.c).toVector(), 1e-12));
// -------
q = Vector3 (1., 0., 0.);
@@ -115,10 +114,10 @@ BOOST_AUTO_TEST_CASE ( test_kinematics )
expected_v_J.angular () << 0., 0., 1.;
- is_matrix_absolutely_closed (expected_configuration.rotation (), joint_data.M.rotation(), 1e-12);
- is_matrix_absolutely_closed (expected_configuration.translation (), joint_data.M.translation (), 1e-12);
- is_matrix_absolutely_closed (expected_v_J.toVector (), ((Motion) joint_data.v).toVector(), 1e-12);
- is_matrix_absolutely_closed (expected_c_J.toVector (), ((Motion) joint_data.c).toVector(), 1e-12);
+ BOOST_CHECK (expected_configuration.rotation ().isApprox(joint_data.M.rotation(), 1e-12));
+ BOOST_CHECK (expected_configuration.translation ().isApprox(joint_data.M.translation (), 1e-12));
+ BOOST_CHECK (expected_v_J.toVector ().isApprox(((Motion) joint_data.v).toVector(), 1e-12));
+ BOOST_CHECK (expected_c_J.toVector ().isApprox(((Motion) joint_data.c).toVector(), 1e-12));
// -------
q = Vector3 (0., 1., 0.);
@@ -135,10 +134,10 @@ BOOST_AUTO_TEST_CASE ( test_kinematics )
expected_v_J.angular () << 0., 1., 0.;
- is_matrix_absolutely_closed (expected_configuration.rotation (), joint_data.M.rotation(), 1e-12);
- is_matrix_absolutely_closed (expected_configuration.translation (), joint_data.M.translation (), 1e-12);
- is_matrix_absolutely_closed (expected_v_J.toVector (), ((Motion) joint_data.v).toVector(), 1e-12);
- is_matrix_absolutely_closed (expected_c_J.toVector (), ((Motion) joint_data.c).toVector(), 1e-12);
+ BOOST_CHECK (expected_configuration.rotation ().isApprox(joint_data.M.rotation(), 1e-12));
+ BOOST_CHECK (expected_configuration.translation ().isApprox(joint_data.M.translation (), 1e-12));
+ BOOST_CHECK (expected_v_J.toVector ().isApprox(((Motion) joint_data.v).toVector(), 1e-12));
+ BOOST_CHECK (expected_c_J.toVector ().isApprox(((Motion) joint_data.c).toVector(), 1e-12));
// -------
q = Vector3 (0., 0., 1.);
@@ -155,10 +154,10 @@ BOOST_AUTO_TEST_CASE ( test_kinematics )
expected_v_J.angular () << 1., 0., 0.;
- is_matrix_absolutely_closed (expected_configuration.rotation (), joint_data.M.rotation(), 1e-12);
- is_matrix_absolutely_closed (expected_configuration.translation (), joint_data.M.translation (), 1e-12);
- is_matrix_absolutely_closed (expected_v_J.toVector (), ((Motion) joint_data.v).toVector(), 1e-12);
- is_matrix_absolutely_closed (expected_c_J.toVector (), ((Motion) joint_data.c).toVector(), 1e-12);
+ BOOST_CHECK (expected_configuration.rotation ().isApprox(joint_data.M.rotation(), 1e-12));
+ BOOST_CHECK (expected_configuration.translation ().isApprox(joint_data.M.translation (), 1e-12));
+ BOOST_CHECK (expected_v_J.toVector ().isApprox(((Motion) joint_data.v).toVector(), 1e-12));
+ BOOST_CHECK (expected_c_J.toVector ().isApprox(((Motion) joint_data.c).toVector(), 1e-12));
// -------
q = Vector3 (1., 1., 1.);
@@ -176,10 +175,10 @@ BOOST_AUTO_TEST_CASE ( test_kinematics )
expected_v_J.angular () << 0.1585290151921, 0.99495101928098, -0.54954440308147;
expected_c_J.angular () << -0.54030230586814, -1.257617821355, -1.4495997326938;
- is_matrix_absolutely_closed (expected_configuration.rotation (), joint_data.M.rotation(), 1e-10);
- is_matrix_absolutely_closed (expected_configuration.translation (), joint_data.M.translation (), 1e-10);
- is_matrix_absolutely_closed (expected_v_J.toVector (), ((Motion) joint_data.v).toVector(), 1e-10);
- is_matrix_absolutely_closed (expected_c_J.toVector (), ((Motion) joint_data.c).toVector(), 1e-10);
+ BOOST_CHECK (expected_configuration.rotation ().isApprox(joint_data.M.rotation(), 1e-12));
+ BOOST_CHECK (expected_configuration.translation ().isApprox(joint_data.M.translation (), 1e-12));
+ BOOST_CHECK (expected_v_J.toVector ().isApprox(((Motion) joint_data.v).toVector(), 1e-12));
+ BOOST_CHECK (expected_c_J.toVector ().isApprox(((Motion) joint_data.c).toVector(), 1e-12));
// -------
q = Vector3 (1., 1.5, 1.9);
@@ -197,10 +196,10 @@ BOOST_AUTO_TEST_CASE ( test_kinematics )
expected_v_J.angular () << -0.99498997320811, -0.83599146030869, -2.8846374616388;
expected_c_J.angular () << -0.42442321000622, -8.5482150213859, 2.7708697933151;
- is_matrix_absolutely_closed (expected_configuration.rotation (), joint_data.M.rotation(), 1e-10);
- is_matrix_absolutely_closed (expected_configuration.translation (), joint_data.M.translation (), 1e-10);
- is_matrix_absolutely_closed (expected_v_J.toVector (), ((Motion) joint_data.v).toVector(), 1e-10);
- is_matrix_absolutely_closed (expected_c_J.toVector (), ((Motion) joint_data.c).toVector(), 1e-10);
+ BOOST_CHECK (expected_configuration.rotation ().isApprox(joint_data.M.rotation(), 1e-12));
+ BOOST_CHECK (expected_configuration.translation ().isApprox(joint_data.M.translation (), 1e-12));
+ BOOST_CHECK (expected_v_J.toVector ().isApprox(((Motion) joint_data.v).toVector(), 1e-12));
+ BOOST_CHECK (expected_c_J.toVector ().isApprox(((Motion) joint_data.c).toVector(), 1e-12));
}
BOOST_AUTO_TEST_CASE ( test_rnea )
@@ -223,7 +222,7 @@ BOOST_AUTO_TEST_CASE ( test_rnea )
rnea (model, data, q, v, a);
Vector3 tau_expected (0., -4.905, 0.);
- is_matrix_absolutely_closed (tau_expected, data.tau, 1e-14);
+ BOOST_CHECK (tau_expected.isApprox(data.tau, 1e-14));
q = Eigen::VectorXd::Ones (model.nq);
v = Eigen::VectorXd::Ones (model.nv);
@@ -232,7 +231,7 @@ BOOST_AUTO_TEST_CASE ( test_rnea )
rnea (model, data, q, v, a);
tau_expected << -0.53611600195085, -0.74621832606188, -0.38177329067604;
- is_matrix_absolutely_closed (tau_expected, data.tau, 1e-12);
+ BOOST_CHECK (tau_expected.isApprox(data.tau, 1e-12));
q << 3, 2, 1;
v = Eigen::VectorXd::Ones (model.nv);
@@ -241,7 +240,7 @@ BOOST_AUTO_TEST_CASE ( test_rnea )
rnea (model, data, q, v, a);
tau_expected << 0.73934458094049, 2.7804530848031, 0.50684940972146;
- is_matrix_absolutely_closed (tau_expected, data.tau, 1e-12);
+ BOOST_CHECK (tau_expected.isApprox(data.tau, 1e-12));
}
BOOST_AUTO_TEST_CASE ( test_crba )
@@ -267,7 +266,7 @@ BOOST_AUTO_TEST_CASE ( test_crba )
0, 1.25, 0,
0, 0, 1;
- is_matrix_absolutely_closed (M_expected, data.M, 1e-14);
+ BOOST_CHECK (M_expected.isApprox(data.M, 1e-14));
q = Eigen::VectorXd::Ones (model.nq);
@@ -277,7 +276,7 @@ BOOST_AUTO_TEST_CASE ( test_crba )
-5.5511151231258e-17, 1.25, 0,
-0.8414709848079, 0, 1;
- is_matrix_absolutely_closed (M_expected, data.M, 1e-12);
+ BOOST_CHECK (M_expected.isApprox(data.M, 1e-12));
q << 3, 2, 1;
@@ -287,7 +286,7 @@ BOOST_AUTO_TEST_CASE ( test_crba )
0, 1.25, 0,
-0.90929742682568, 0, 1;
- is_matrix_absolutely_closed (M_expected, data.M, 1e-10);
+ BOOST_CHECK (M_expected.isApprox(data.M, 1e-10));
}
BOOST_AUTO_TEST_SUITE_END ()
@@ -321,10 +320,10 @@ BOOST_AUTO_TEST_CASE ( test_kinematics )
printOutJointData <JointDataPX> (q, q_dot, joint_data);
- is_matrix_absolutely_closed (expected_configuration.rotation (), joint_data.M.rotation());
- is_matrix_absolutely_closed (expected_configuration.translation (), joint_data.M.translation ());
- is_matrix_absolutely_closed (expected_v_J.toVector (), ((Motion) joint_data.v).toVector());
- is_matrix_absolutely_closed (expected_c_J.toVector (), ((Motion) joint_data.c).toVector());
+ BOOST_CHECK (expected_configuration.rotation ().isApprox(joint_data.M.rotation(), 1e-12));
+ BOOST_CHECK (expected_configuration.translation ().isApprox(joint_data.M.translation (), 1e-12));
+ BOOST_CHECK (expected_v_J.toVector ().isApprox(((Motion) joint_data.v).toVector(), 1e-12));
+ BOOST_CHECK (expected_c_J.toVector ().isApprox(((Motion) joint_data.c).toVector(), 1e-12));
// -------
q << 1.;
@@ -339,10 +338,10 @@ BOOST_AUTO_TEST_CASE ( test_kinematics )
expected_v_J.linear () << 1., 0., 0.;
- is_matrix_absolutely_closed (expected_configuration.rotation (), joint_data.M.rotation(), 1e-12);
- is_matrix_absolutely_closed (expected_configuration.translation (), joint_data.M.translation (), 1e-12);
- is_matrix_absolutely_closed (expected_v_J.toVector (), ((Motion) joint_data.v).toVector(), 1e-12);
- is_matrix_absolutely_closed (expected_c_J.toVector (), ((Motion) joint_data.c).toVector(), 1e-12);
+ BOOST_CHECK (expected_configuration.rotation ().isApprox(joint_data.M.rotation(), 1e-12));
+ BOOST_CHECK (expected_configuration.translation ().isApprox(joint_data.M.translation (), 1e-12));
+ BOOST_CHECK (expected_v_J.toVector ().isApprox(((Motion) joint_data.v).toVector(), 1e-12));
+ BOOST_CHECK (expected_c_J.toVector ().isApprox(((Motion) joint_data.c).toVector(), 1e-12));
}
BOOST_AUTO_TEST_CASE ( test_rnea )
@@ -367,7 +366,7 @@ BOOST_AUTO_TEST_CASE ( test_rnea )
Eigen::VectorXd tau_expected (Eigen::VectorXd::Zero (model.nq));
tau_expected << 0;
- is_matrix_absolutely_closed (tau_expected, data.tau, 1e-14);
+ BOOST_CHECK (tau_expected.isApprox(data.tau, 1e-14));
// -----
q = Eigen::VectorXd::Ones (model.nq);
@@ -377,7 +376,7 @@ BOOST_AUTO_TEST_CASE ( test_rnea )
rnea (model, data, q, v, a);
tau_expected << 1;
- is_matrix_absolutely_closed (tau_expected, data.tau, 1e-12);
+ BOOST_CHECK (tau_expected.isApprox(data.tau, 1e-12));
q << 3;
v = Eigen::VectorXd::Ones (model.nv);
@@ -386,7 +385,7 @@ BOOST_AUTO_TEST_CASE ( test_rnea )
rnea (model, data, q, v, a);
tau_expected << 1;
- is_matrix_absolutely_closed (tau_expected, data.tau, 1e-12);
+ BOOST_CHECK (tau_expected.isApprox(data.tau, 1e-12));
}
BOOST_AUTO_TEST_CASE ( test_crba )
@@ -409,19 +408,19 @@ BOOST_AUTO_TEST_CASE ( test_crba )
crba (model, data, q);
M_expected << 1.0;
- is_matrix_absolutely_closed (M_expected, data.M, 1e-14);
+ BOOST_CHECK (M_expected.isApprox(data.M, 1e-14));
q = Eigen::VectorXd::Ones (model.nq);
crba (model, data, q);
- is_matrix_absolutely_closed (M_expected, data.M, 1e-12);
+ BOOST_CHECK (M_expected.isApprox(data.M, 1e-12));
q << 3;
crba (model, data, q);
- is_matrix_absolutely_closed (M_expected, data.M, 1e-10);
+ BOOST_CHECK (M_expected.isApprox(data.M, 1e-10));
}
BOOST_AUTO_TEST_SUITE_END ()
@@ -456,10 +455,10 @@ BOOST_AUTO_TEST_CASE ( test_kinematics )
printOutJointData <JointDataTranslation> (q, q_dot, joint_data);
- is_matrix_absolutely_closed (expected_configuration.rotation (), joint_data.M.rotation());
- is_matrix_absolutely_closed (expected_configuration.translation (), joint_data.M.translation ());
- is_matrix_absolutely_closed (expected_v_J.toVector (), ((Motion) joint_data.v).toVector());
- is_matrix_absolutely_closed (expected_c_J.toVector (), ((Motion) joint_data.c).toVector());
+ BOOST_CHECK (expected_configuration.rotation ().isApprox(joint_data.M.rotation(), 1e-12));
+ BOOST_CHECK (expected_configuration.translation ().isApprox(joint_data.M.translation (), 1e-12));
+ BOOST_CHECK (expected_v_J.toVector ().isApprox(((Motion) joint_data.v).toVector(), 1e-12));
+ BOOST_CHECK (expected_c_J.toVector ().isApprox(((Motion) joint_data.c).toVector(), 1e-12));
// -------
q = Vector3 (1., 0., 0.);
@@ -473,10 +472,10 @@ BOOST_AUTO_TEST_CASE ( test_kinematics )
expected_v_J.linear () << 1., 0., 0.;
- is_matrix_absolutely_closed (expected_configuration.rotation (), joint_data.M.rotation(), 1e-12);
- is_matrix_absolutely_closed (expected_configuration.translation (), joint_data.M.translation (), 1e-12);
- is_matrix_absolutely_closed (expected_v_J.toVector (), ((Motion) joint_data.v).toVector(), 1e-12);
- is_matrix_absolutely_closed (expected_c_J.toVector (), ((Motion) joint_data.c).toVector(), 1e-12);
+ BOOST_CHECK (expected_configuration.rotation ().isApprox(joint_data.M.rotation(), 1e-12));
+ BOOST_CHECK (expected_configuration.translation ().isApprox(joint_data.M.translation (), 1e-12));
+ BOOST_CHECK (expected_v_J.toVector ().isApprox(((Motion) joint_data.v).toVector(), 1e-12));
+ BOOST_CHECK (expected_c_J.toVector ().isApprox(((Motion) joint_data.c).toVector(), 1e-12));
// -------
q = Vector3 (0., 1., 0.);
@@ -490,10 +489,10 @@ BOOST_AUTO_TEST_CASE ( test_kinematics )
expected_v_J.linear () << 0., 1., 0.;
- is_matrix_absolutely_closed (expected_configuration.rotation (), joint_data.M.rotation(), 1e-12);
- is_matrix_absolutely_closed (expected_configuration.translation (), joint_data.M.translation (), 1e-12);
- is_matrix_absolutely_closed (expected_v_J.toVector (), ((Motion) joint_data.v).toVector(), 1e-12);
- is_matrix_absolutely_closed (expected_c_J.toVector (), ((Motion) joint_data.c).toVector(), 1e-12);
+ BOOST_CHECK (expected_configuration.rotation ().isApprox(joint_data.M.rotation(), 1e-12));
+ BOOST_CHECK (expected_configuration.translation ().isApprox(joint_data.M.translation (), 1e-12));
+ BOOST_CHECK (expected_v_J.toVector ().isApprox(((Motion) joint_data.v).toVector(), 1e-12));
+ BOOST_CHECK (expected_c_J.toVector ().isApprox(((Motion) joint_data.c).toVector(), 1e-12));
// -------
q = Vector3 (0., 0., 1.);
@@ -507,10 +506,10 @@ BOOST_AUTO_TEST_CASE ( test_kinematics )
expected_v_J.linear () << 0., 0., 1.;
- is_matrix_absolutely_closed (expected_configuration.rotation (), joint_data.M.rotation(), 1e-12);
- is_matrix_absolutely_closed (expected_configuration.translation (), joint_data.M.translation (), 1e-12);
- is_matrix_absolutely_closed (expected_v_J.toVector (), ((Motion) joint_data.v).toVector(), 1e-12);
- is_matrix_absolutely_closed (expected_c_J.toVector (), ((Motion) joint_data.c).toVector(), 1e-12);
+ BOOST_CHECK (expected_configuration.rotation ().isApprox(joint_data.M.rotation(), 1e-12));
+ BOOST_CHECK (expected_configuration.translation ().isApprox(joint_data.M.translation (), 1e-12));
+ BOOST_CHECK (expected_v_J.toVector ().isApprox(((Motion) joint_data.v).toVector(), 1e-12));
+ BOOST_CHECK (expected_c_J.toVector ().isApprox(((Motion) joint_data.c).toVector(), 1e-12));
// -------
q = Vector3 (1., 1., 1.);
@@ -523,10 +522,10 @@ BOOST_AUTO_TEST_CASE ( test_kinematics )
expected_configuration.translation () << 1., 1., 1.;
expected_v_J.linear () << 1., 1., 1.;
- is_matrix_absolutely_closed (expected_configuration.rotation (), joint_data.M.rotation(), 1e-10);
- is_matrix_absolutely_closed (expected_configuration.translation (), joint_data.M.translation (), 1e-10);
- is_matrix_absolutely_closed (expected_v_J.toVector (), ((Motion) joint_data.v).toVector(), 1e-10);
- is_matrix_absolutely_closed (expected_c_J.toVector (), ((Motion) joint_data.c).toVector(), 1e-10);
+ BOOST_CHECK (expected_configuration.rotation ().isApprox(joint_data.M.rotation(), 1e-12));
+ BOOST_CHECK (expected_configuration.translation ().isApprox(joint_data.M.translation (), 1e-12));
+ BOOST_CHECK (expected_v_J.toVector ().isApprox(((Motion) joint_data.v).toVector(), 1e-12));
+ BOOST_CHECK (expected_c_J.toVector ().isApprox(((Motion) joint_data.c).toVector(), 1e-12));
// -------
q = Vector3 (1., 1.5, 1.9);
@@ -539,10 +538,10 @@ BOOST_AUTO_TEST_CASE ( test_kinematics )
expected_configuration.translation () = q;
expected_v_J.linear () = q_dot;
- is_matrix_absolutely_closed (expected_configuration.rotation (), joint_data.M.rotation(), 1e-10);
- is_matrix_absolutely_closed (expected_configuration.translation (), joint_data.M.translation (), 1e-10);
- is_matrix_absolutely_closed (expected_v_J.toVector (), ((Motion) joint_data.v).toVector(), 1e-10);
- is_matrix_absolutely_closed (expected_c_J.toVector (), ((Motion) joint_data.c).toVector(), 1e-10);
+ BOOST_CHECK (expected_configuration.rotation ().isApprox(joint_data.M.rotation(), 1e-12));
+ BOOST_CHECK (expected_configuration.translation ().isApprox(joint_data.M.translation (), 1e-12));
+ BOOST_CHECK (expected_v_J.toVector ().isApprox(((Motion) joint_data.v).toVector(), 1e-12));
+ BOOST_CHECK (expected_c_J.toVector ().isApprox(((Motion) joint_data.c).toVector(), 1e-12));
}
BOOST_AUTO_TEST_CASE ( test_rnea )
@@ -567,7 +566,7 @@ BOOST_AUTO_TEST_CASE ( test_rnea )
tau_expected << 0, 0, 9.81;
- is_matrix_absolutely_closed (tau_expected, data.tau, 1e-14);
+ BOOST_CHECK (tau_expected.isApprox(data.tau, 1e-14));
q = Eigen::VectorXd::Ones (model.nq);
v = Eigen::VectorXd::Ones (model.nv);
@@ -576,7 +575,7 @@ BOOST_AUTO_TEST_CASE ( test_rnea )
rnea (model, data, q, v, a);
tau_expected << 1, 1, 10.81;
- is_matrix_absolutely_closed (tau_expected, data.tau, 1e-12);
+ BOOST_CHECK (tau_expected.isApprox(data.tau, 1e-12));
q << 3, 2, 1;
v = Eigen::VectorXd::Ones (model.nv);
@@ -585,7 +584,7 @@ BOOST_AUTO_TEST_CASE ( test_rnea )
rnea (model, data, q, v, a);
tau_expected << 1, 1, 10.81;
- is_matrix_absolutely_closed (tau_expected, data.tau, 1e-12);
+ BOOST_CHECK (tau_expected.isApprox(data.tau, 1e-12));
}
BOOST_AUTO_TEST_CASE ( test_crba )
@@ -608,19 +607,19 @@ BOOST_AUTO_TEST_CASE ( test_crba )
crba (model, data, q);
M_expected = Matrix3::Identity ();
- is_matrix_absolutely_closed (M_expected, data.M, 1e-14);
+ BOOST_CHECK (M_expected.isApprox(data.M, 1e-14));
q = Eigen::VectorXd::Ones (model.nq);
crba (model, data, q);
- is_matrix_absolutely_closed (M_expected, data.M, 1e-12);
+ BOOST_CHECK (M_expected.isApprox(data.M, 1e-12));
q << 3, 2, 1;
crba (model, data, q);
- is_matrix_absolutely_closed (M_expected, data.M, 1e-10);
+ BOOST_CHECK (M_expected.isApprox(data.M, 1e-10));
}
BOOST_AUTO_TEST_SUITE_END ()
@@ -656,10 +655,10 @@ BOOST_AUTO_TEST_CASE ( test_kinematics )
printOutJointData <JointDataSpherical> (q, q_dot, joint_data);
- is_matrix_absolutely_closed (expected_configuration.rotation (), joint_data.M.rotation());
- is_matrix_absolutely_closed (expected_configuration.translation (), joint_data.M.translation ());
- is_matrix_absolutely_closed (expected_v_J.toVector (), ((Motion) joint_data.v).toVector());
- is_matrix_absolutely_closed (expected_c_J.toVector (), ((Motion) joint_data.c).toVector());
+ BOOST_CHECK (expected_configuration.rotation ().isApprox(joint_data.M.rotation(), 1e-12));
+ BOOST_CHECK (expected_configuration.translation ().isApprox(joint_data.M.translation (), 1e-12));
+ BOOST_CHECK (expected_v_J.toVector ().isApprox(((Motion) joint_data.v).toVector(), 1e-12));
+ BOOST_CHECK (expected_c_J.toVector ().isApprox(((Motion) joint_data.c).toVector(), 1e-12));
// -------
q = Vector4 (1., 0, 0., 1.); q.normalize();
@@ -676,10 +675,10 @@ BOOST_AUTO_TEST_CASE ( test_kinematics )
expected_v_J.angular () << 1., 0., 0.;
- is_matrix_absolutely_closed (expected_configuration.rotation (), joint_data.M.rotation(), 1e-12);
- is_matrix_absolutely_closed (expected_configuration.translation (), joint_data.M.translation (), 1e-12);
- is_matrix_absolutely_closed (expected_v_J.toVector (), ((Motion) joint_data.v).toVector(), 1e-12);
- is_matrix_absolutely_closed (expected_c_J.toVector (), ((Motion) joint_data.c).toVector(), 1e-12);
+ BOOST_CHECK (expected_configuration.rotation ().isApprox(joint_data.M.rotation(), 1e-12));
+ BOOST_CHECK (expected_configuration.translation ().isApprox(joint_data.M.translation (), 1e-12));
+ BOOST_CHECK (expected_v_J.toVector ().isApprox(((Motion) joint_data.v).toVector(), 1e-12));
+ BOOST_CHECK (expected_c_J.toVector ().isApprox(((Motion) joint_data.c).toVector(), 1e-12));
// -------
q = Vector4 (0., 1., 0., 1.); q.normalize();
@@ -696,10 +695,10 @@ BOOST_AUTO_TEST_CASE ( test_kinematics )
expected_v_J.angular () << 0., 1., 0.;
- is_matrix_absolutely_closed (expected_configuration.rotation (), joint_data.M.rotation(), 1e-12);
- is_matrix_absolutely_closed (expected_configuration.translation (), joint_data.M.translation (), 1e-12);
- is_matrix_absolutely_closed (expected_v_J.toVector (), ((Motion) joint_data.v).toVector(), 1e-12);
- is_matrix_absolutely_closed (expected_c_J.toVector (), ((Motion) joint_data.c).toVector(), 1e-12);
+ BOOST_CHECK (expected_configuration.rotation ().isApprox(joint_data.M.rotation(), 1e-12));
+ BOOST_CHECK (expected_configuration.translation ().isApprox(joint_data.M.translation (), 1e-12));
+ BOOST_CHECK (expected_v_J.toVector ().isApprox(((Motion) joint_data.v).toVector(), 1e-12));
+ BOOST_CHECK (expected_c_J.toVector ().isApprox(((Motion) joint_data.c).toVector(), 1e-12));
// -------
q = Vector4 (0., 0, 1., 1.); q.normalize();
@@ -716,10 +715,10 @@ BOOST_AUTO_TEST_CASE ( test_kinematics )
expected_v_J.angular () << 0., 0., 1.;
- is_matrix_absolutely_closed (expected_configuration.rotation (), joint_data.M.rotation(), 1e-12);
- is_matrix_absolutely_closed (expected_configuration.translation (), joint_data.M.translation (), 1e-12);
- is_matrix_absolutely_closed (expected_v_J.toVector (), ((Motion) joint_data.v).toVector(), 1e-12);
- is_matrix_absolutely_closed (expected_c_J.toVector (), ((Motion) joint_data.c).toVector(), 1e-12);
+ BOOST_CHECK (expected_configuration.rotation ().isApprox(joint_data.M.rotation(), 1e-12));
+ BOOST_CHECK (expected_configuration.translation ().isApprox(joint_data.M.translation (), 1e-12));
+ BOOST_CHECK (expected_v_J.toVector ().isApprox(((Motion) joint_data.v).toVector(), 1e-12));
+ BOOST_CHECK (expected_c_J.toVector ().isApprox(((Motion) joint_data.c).toVector(), 1e-12));
// -------
q = Vector4 (1., 1., 1., 1.); q.normalize();
@@ -736,10 +735,10 @@ BOOST_AUTO_TEST_CASE ( test_kinematics )
expected_v_J.angular () << 1., 1., 1.;
- is_matrix_absolutely_closed (expected_configuration.rotation (), joint_data.M.rotation(), 1e-10);
- is_matrix_absolutely_closed (expected_configuration.translation (), joint_data.M.translation (), 1e-10);
- is_matrix_absolutely_closed (expected_v_J.toVector (), ((Motion) joint_data.v).toVector(), 1e-10);
- is_matrix_absolutely_closed (expected_c_J.toVector (), ((Motion) joint_data.c).toVector(), 1e-10);
+ BOOST_CHECK (expected_configuration.rotation ().isApprox(joint_data.M.rotation(), 1e-12));
+ BOOST_CHECK (expected_configuration.translation ().isApprox(joint_data.M.translation (), 1e-12));
+ BOOST_CHECK (expected_v_J.toVector ().isApprox(((Motion) joint_data.v).toVector(), 1e-12));
+ BOOST_CHECK (expected_c_J.toVector ().isApprox(((Motion) joint_data.c).toVector(), 1e-12));
// -------
q = Vector4 (1., 1.5, 1.9, 1.); q.normalize();
@@ -756,10 +755,10 @@ BOOST_AUTO_TEST_CASE ( test_kinematics )
expected_v_J.angular () = q_dot;
- is_matrix_absolutely_closed (expected_configuration.rotation (), joint_data.M.rotation(), 1e-10);
- is_matrix_absolutely_closed (expected_configuration.translation (), joint_data.M.translation (), 1e-10);
- is_matrix_absolutely_closed (expected_v_J.toVector (), ((Motion) joint_data.v).toVector(), 1e-10);
- is_matrix_absolutely_closed (expected_c_J.toVector (), ((Motion) joint_data.c).toVector(), 1e-10);
+ BOOST_CHECK (expected_configuration.rotation ().isApprox(joint_data.M.rotation(), 1e-12));
+ BOOST_CHECK (expected_configuration.translation ().isApprox(joint_data.M.translation (), 1e-12));
+ BOOST_CHECK (expected_v_J.toVector ().isApprox(((Motion) joint_data.v).toVector(), 1e-12));
+ BOOST_CHECK (expected_c_J.toVector ().isApprox(((Motion) joint_data.c).toVector(), 1e-12));
}
BOOST_AUTO_TEST_CASE ( test_rnea )
@@ -784,7 +783,7 @@ BOOST_AUTO_TEST_CASE ( test_rnea )
tau_expected << 0, -4.905, 0;
- is_matrix_absolutely_closed (tau_expected, data.tau, 1e-14);
+ BOOST_CHECK (tau_expected.isApprox(data.tau, 1e-14));
q = Eigen::VectorXd::Ones (model.nq); q.normalize ();
v = Eigen::VectorXd::Ones (model.nv);
@@ -793,7 +792,7 @@ BOOST_AUTO_TEST_CASE ( test_rnea )
rnea (model, data, q, v, a);
tau_expected << 1, 1, 6.405;
- is_matrix_absolutely_closed (tau_expected, data.tau, 1e-12);
+ BOOST_CHECK (tau_expected.isApprox(data.tau, 1e-12));
q << 3, 2, 1, 1; q.normalize ();
v = Eigen::VectorXd::Ones (model.nv);
@@ -802,7 +801,7 @@ BOOST_AUTO_TEST_CASE ( test_rnea )
rnea (model, data, q, v, a);
tau_expected << 1, 4.597, 4.77;
- is_matrix_absolutely_closed (tau_expected, data.tau, 1e-12);
+ BOOST_CHECK (tau_expected.isApprox(data.tau, 1e-12));
}
BOOST_AUTO_TEST_CASE ( test_crba )
@@ -826,18 +825,18 @@ BOOST_AUTO_TEST_CASE ( test_crba )
M_expected = Matrix3::Identity ();
M_expected(1,1) = 1.25; M_expected(2,2) = 1.25;
- is_matrix_absolutely_closed (M_expected, data.M, 1e-14);
+ BOOST_CHECK (M_expected.isApprox(data.M, 1e-14));
q = Eigen::VectorXd::Ones (model.nq); q.normalize();
crba (model, data, q);
- is_matrix_absolutely_closed (M_expected, data.M, 1e-12);
+ BOOST_CHECK (M_expected.isApprox(data.M, 1e-12));
q << 3, 2, 1, 1; q.normalize();
crba (model, data, q);
- is_matrix_absolutely_closed (M_expected, data.M, 1e-10);
+ BOOST_CHECK (M_expected.isApprox(data.M, 1e-10));
}
BOOST_AUTO_TEST_SUITE_END ()
@@ -874,10 +873,14 @@ BOOST_AUTO_TEST_CASE ( test_kinematics )
printOutJointData <JointDataRevoluteUnaligned> (q, q_dot, joint_data_RU);
- is_matrix_absolutely_closed (joint_data_RU.M.rotation(), joint_data_RX.M.rotation());
- is_matrix_absolutely_closed (joint_data_RU.M.translation (), joint_data_RX.M.translation ());
- is_matrix_absolutely_closed (((Motion) joint_data_RU.v).toVector(), ((Motion) joint_data_RX.v).toVector());
- is_matrix_absolutely_closed (((Motion) joint_data_RU.c).toVector(), ((Motion) joint_data_RX.c).toVector());
+ BOOST_CHECK (joint_data_RU.M.rotation()
+ .isApprox(joint_data_RX.M.rotation(), 1e-12));
+ BOOST_CHECK (joint_data_RU.M.translation ()
+ .isApprox(joint_data_RX.M.translation (), 1e-12));
+ BOOST_CHECK (((Motion) joint_data_RU.v).toVector()
+ .isApprox(((Motion) joint_data_RX.v).toVector(), 1e-12));
+ BOOST_CHECK (((Motion) joint_data_RU.c).toVector()
+ .isApprox(((Motion) joint_data_RX.c).toVector(), 1e-12));
}
@@ -911,7 +914,7 @@ BOOST_AUTO_TEST_CASE ( test_rnea )
rnea (modelRX, dataRX, q, v, a);
rnea (modelRU, dataRU, q, v, a);
- is_matrix_absolutely_closed (dataRX.tau, dataRU.tau, 1e-14);
+ BOOST_CHECK (dataRX.tau.isApprox(dataRU.tau, 1e-14));
q = Eigen::VectorXd::Ones (modelRU.nq); //q.normalize ();
v = Eigen::VectorXd::Ones (modelRU.nv);
@@ -920,7 +923,7 @@ BOOST_AUTO_TEST_CASE ( test_rnea )
rnea (modelRX, dataRX, q, v, a);
rnea (modelRU, dataRU, q, v, a);
- is_matrix_absolutely_closed (dataRX.tau, dataRU.tau, 1e-12);
+ BOOST_CHECK (dataRX.tau.isApprox(dataRU.tau, 1e-12));
q << 3.;
v = Eigen::VectorXd::Ones (modelRU.nv);
@@ -929,7 +932,7 @@ BOOST_AUTO_TEST_CASE ( test_rnea )
rnea (modelRX, dataRX, q, v, a);
rnea (modelRU, dataRU, q, v, a);
- is_matrix_absolutely_closed (dataRX.tau, dataRU.tau, 1e-12);
+ BOOST_CHECK (dataRX.tau.isApprox(dataRU.tau, 1e-12));
}
BOOST_AUTO_TEST_CASE ( test_crba )
@@ -961,7 +964,7 @@ BOOST_AUTO_TEST_CASE ( test_crba )
crba (modelRX, dataRX, q);
crba (modelRU, dataRU, q);
- is_matrix_absolutely_closed (dataRX.M, dataRU.M, 1e-14);
+ BOOST_CHECK (dataRX.M.isApprox(dataRU.M, 1e-14));
// ----
q = Eigen::VectorXd::Ones (modelRU.nq);
@@ -969,7 +972,7 @@ BOOST_AUTO_TEST_CASE ( test_crba )
crba (modelRX, dataRX, q);
crba (modelRU, dataRU, q);
- is_matrix_absolutely_closed (dataRX.M, dataRU.M, 1e-14);
+ BOOST_CHECK (dataRX.M.isApprox(dataRU.M, 1e-14));
// ----
q << 3;
@@ -977,7 +980,7 @@ BOOST_AUTO_TEST_CASE ( test_crba )
crba (modelRX, dataRX, q);
crba (modelRU, dataRU, q);
- is_matrix_absolutely_closed (dataRX.M, dataRU.M, 1e-14);
+ BOOST_CHECK (dataRX.M.isApprox(dataRU.M, 1e-14));
}
BOOST_AUTO_TEST_SUITE_END ()
@@ -1013,10 +1016,14 @@ BOOST_AUTO_TEST_CASE ( test_kinematics )
printOutJointData <JointDataPrismaticUnaligned> (q, q_dot, joint_data_PU);
- is_matrix_absolutely_closed (joint_data_PU.M.rotation(), joint_data_PX.M.rotation());
- is_matrix_absolutely_closed (joint_data_PU.M.translation (), joint_data_PX.M.translation ());
- is_matrix_absolutely_closed (((Motion) joint_data_PU.v).toVector(), ((Motion) joint_data_PX.v).toVector());
- is_matrix_absolutely_closed (((Motion) joint_data_PU.c).toVector(), ((Motion) joint_data_PX.c).toVector());
+ BOOST_CHECK (joint_data_PU.M.rotation()
+ .isApprox(joint_data_PX.M.rotation(), 1e-12));
+ BOOST_CHECK (joint_data_PU.M.translation ()
+ .isApprox(joint_data_PX.M.translation (), 1e-12));
+ BOOST_CHECK (((Motion) joint_data_PU.v).toVector()
+ .isApprox(((Motion) joint_data_PX.v).toVector(), 1e-12));
+ BOOST_CHECK (((Motion) joint_data_PU.c).toVector()
+ .isApprox(((Motion) joint_data_PX.c).toVector(), 1e-12));
}
@@ -1050,7 +1057,7 @@ BOOST_AUTO_TEST_CASE ( test_rnea )
rnea (modelPX, dataRX, q, v, a);
rnea (modelPU, dataRU, q, v, a);
- is_matrix_absolutely_closed (dataRX.tau, dataRU.tau, 1e-14);
+ BOOST_CHECK (dataRX.tau.isApprox(dataRU.tau, 1e-14));
q = Eigen::VectorXd::Ones (modelPU.nq); //q.normalize ();
v = Eigen::VectorXd::Ones (modelPU.nv);
@@ -1059,7 +1066,7 @@ BOOST_AUTO_TEST_CASE ( test_rnea )
rnea (modelPX, dataRX, q, v, a);
rnea (modelPU, dataRU, q, v, a);
- is_matrix_absolutely_closed (dataRX.tau, dataRU.tau, 1e-12);
+ BOOST_CHECK (dataRX.tau.isApprox(dataRU.tau, 1e-12));
q << 3.;
v = Eigen::VectorXd::Ones (modelPU.nv);
@@ -1068,7 +1075,7 @@ BOOST_AUTO_TEST_CASE ( test_rnea )
rnea (modelPX, dataRX, q, v, a);
rnea (modelPU, dataRU, q, v, a);
- is_matrix_absolutely_closed (dataRX.tau, dataRU.tau, 1e-12);
+ BOOST_CHECK (dataRX.tau.isApprox(dataRU.tau, 1e-12));
}
BOOST_AUTO_TEST_CASE ( test_crba )
@@ -1100,7 +1107,7 @@ BOOST_AUTO_TEST_CASE ( test_crba )
crba (modelPX, dataRX, q);
crba (modelPU, dataRU, q);
- is_matrix_absolutely_closed (dataRX.M, dataRU.M, 1e-14);
+ BOOST_CHECK (dataRX.M.isApprox(dataRU.M, 1e-14));
// ----
q = Eigen::VectorXd::Ones (modelPU.nq);
@@ -1108,7 +1115,7 @@ BOOST_AUTO_TEST_CASE ( test_crba )
crba (modelPX, dataRX, q);
crba (modelPU, dataRU, q);
- is_matrix_absolutely_closed (dataRX.M, dataRU.M, 1e-14);
+ BOOST_CHECK (dataRX.M.isApprox(dataRU.M, 1e-14));
// ----
q << 3;
@@ -1116,7 +1123,7 @@ BOOST_AUTO_TEST_CASE ( test_crba )
crba (modelPX, dataRX, q);
crba (modelPU, dataRU, q);
- is_matrix_absolutely_closed (dataRX.M, dataRU.M, 1e-14);
+ BOOST_CHECK (dataRX.M.isApprox(dataRU.M, 1e-14));
}
BOOST_AUTO_TEST_SUITE_END ()
@@ -1150,8 +1157,8 @@ BOOST_AUTO_TEST_CASE ( test_merge_body )
0., 0., 3.28125;
BOOST_CHECK_EQUAL(mergedInertia.mass(), expected_mass);
- is_matrix_absolutely_closed (mergedInertia.lever(), expected_com);
- is_matrix_absolutely_closed (mergedInertia.inertia().matrix(), expectedBodyInertia);
+ BOOST_CHECK (mergedInertia.lever().isApprox(expected_com, 1e-12));
+ BOOST_CHECK (mergedInertia.inertia().matrix().isApprox(expectedBodyInertia, 1e-12));
exit(0);
}
diff --git a/unittest/non-linear-effects.cpp b/unittest/non-linear-effects.cpp
index a170385d47998c1022b6343bb649d6a8644c3e49..ed44d8e73db8bac55ec4207b45fae3f40f516a2f 100644
--- a/unittest/non-linear-effects.cpp
+++ b/unittest/non-linear-effects.cpp
@@ -35,7 +35,6 @@
#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE NLETests
#include <boost/test/unit_test.hpp>
-#include "pinocchio/tools/matrix-comparison.hpp"
#include <iostream>
@@ -71,7 +70,7 @@ BOOST_AUTO_TEST_CASE ( test_against_rnea )
tau_nle = nonLinearEffects(model,data_nle,q,v);
tau_rnea = rnea(model,data_rnea,q,v,VectorXd::Zero (model.nv));
- is_matrix_absolutely_closed (tau_nle, tau_rnea);
+ BOOST_CHECK (tau_nle.isApprox(tau_rnea, 1e-12));
// -------
q.setZero ();
@@ -80,7 +79,7 @@ BOOST_AUTO_TEST_CASE ( test_against_rnea )
tau_nle = nonLinearEffects(model,data_nle,q,v);
tau_rnea = rnea(model,data_rnea,q,v,VectorXd::Zero (model.nv));
- is_matrix_absolutely_closed (tau_nle, tau_rnea);
+ BOOST_CHECK (tau_nle.isApprox(tau_rnea, 1e-12));
// -------
q.setOnes ();
@@ -89,7 +88,7 @@ BOOST_AUTO_TEST_CASE ( test_against_rnea )
tau_nle = nonLinearEffects(model,data_nle,q,v);
tau_rnea = rnea(model,data_rnea,q,v,VectorXd::Zero (model.nv));
- is_matrix_absolutely_closed (tau_nle, tau_rnea);
+ BOOST_CHECK (tau_nle.isApprox(tau_rnea, 1e-12));
// -------
q.setRandom ();
@@ -98,7 +97,7 @@ BOOST_AUTO_TEST_CASE ( test_against_rnea )
tau_nle = nonLinearEffects(model,data_nle,q,v);
tau_rnea = rnea(model,data_rnea,q,v,VectorXd::Zero (model.nv));
- is_matrix_absolutely_closed (tau_nle, tau_rnea);
+ BOOST_CHECK (tau_nle.isApprox(tau_rnea, 1e-12));
}
BOOST_AUTO_TEST_SUITE_END ()
diff --git a/unittest/rnea.cpp b/unittest/rnea.cpp
index 8399b51ffd843fcde3bb7df21979db2269c039e6..7d3cbe1cd56c297d5e63e41bc85760f4a4fd97d1 100644
--- a/unittest/rnea.cpp
+++ b/unittest/rnea.cpp
@@ -44,7 +44,6 @@
#define BOOST_TEST_MODULE RneaTest
#include <boost/test/unit_test.hpp>
#include <boost/utility/binary.hpp>
-#include "pinocchio/tools/matrix-comparison.hpp"
BOOST_AUTO_TEST_SUITE ( Rnea )
diff --git a/unittest/symmetric.cpp b/unittest/symmetric.cpp
index 045bb014016cf5970114de031370250826a86b55..b636eb348c4f1a1d8ade32b85ab902bb0b2036b8 100644
--- a/unittest/symmetric.cpp
+++ b/unittest/symmetric.cpp
@@ -45,7 +45,6 @@
#define BOOST_TEST_MODULE symmetricTest
#include <boost/test/unit_test.hpp>
#include <boost/utility/binary.hpp>
-#include "pinocchio/tools/matrix-comparison.hpp"
#include <Eigen/StdVector>
@@ -102,7 +101,7 @@ BOOST_AUTO_TEST_CASE ( test_pinocchio_Sym3 )
{
Matrix3 M = Matrix3::Random(); M = M*M.transpose();
Symmetric3 S(M);
- is_matrix_absolutely_closed(S.matrix(), M, 1e-12);
+ BOOST_CHECK(S.matrix().isApprox(M, 1e-12));
}
// S += S
@@ -112,7 +111,7 @@ BOOST_AUTO_TEST_CASE ( test_pinocchio_Sym3 )
S2 = Symmetric3::Random();
Symmetric3 Scopy = S;
S+=S2;
- is_matrix_absolutely_closed(S.matrix(), S2.matrix()+Scopy.matrix(), 1e-12);
+ BOOST_CHECK(S.matrix().isApprox(S2.matrix()+Scopy.matrix(), 1e-12));
}
// S + M
@@ -121,10 +120,10 @@ BOOST_AUTO_TEST_CASE ( test_pinocchio_Sym3 )
Matrix3 M = Matrix3::Random(); M = M*M.transpose();
Symmetric3 S2 = S + M;
- is_matrix_absolutely_closed(S2.matrix(), S.matrix()+M, 1e-12);
+ BOOST_CHECK(S2.matrix().isApprox(S.matrix()+M, 1e-12));
S2 = S - M;
- is_matrix_absolutely_closed(S2.matrix(), S.matrix()-M, 1e-12);
+ BOOST_CHECK(S2.matrix().isApprox(S.matrix()-M, 1e-12));
}
// S*v
@@ -132,7 +131,7 @@ BOOST_AUTO_TEST_CASE ( test_pinocchio_Sym3 )
Symmetric3 S = Symmetric3::Random();
Vector3 v = Vector3::Random();
Vector3 Sv = S*v;
- is_matrix_absolutely_closed(Sv, S.matrix()*v, 1e-12);
+ BOOST_CHECK(Sv.isApprox(S.matrix()*v, 1e-12));
}
// Random
@@ -146,7 +145,7 @@ BOOST_AUTO_TEST_CASE ( test_pinocchio_Sym3 )
// Identity
{
- is_matrix_absolutely_closed(Symmetric3::Identity().matrix(), Matrix3::Identity(), 1e-12);
+ BOOST_CHECK(Symmetric3::Identity().matrix().isApprox(Matrix3::Identity(), 1e-12));
}
// Skew2
@@ -155,33 +154,33 @@ BOOST_AUTO_TEST_CASE ( test_pinocchio_Sym3 )
Symmetric3 vxvx = Symmetric3::SkewSquare(v);
Vector3 p = Vector3::UnitX();
- is_matrix_absolutely_closed(vxvx*p, v.cross(v.cross(p)), 1e-12);
+ BOOST_CHECK((vxvx*p).isApprox(v.cross(v.cross(p)), 1e-12));
p = Vector3::UnitY();
- is_matrix_absolutely_closed(vxvx*p, v.cross(v.cross(p)), 1e-12);
+ BOOST_CHECK((vxvx*p).isApprox(v.cross(v.cross(p)), 1e-12));
p = Vector3::UnitZ();
- is_matrix_absolutely_closed(vxvx*p, v.cross(v.cross(p)), 1e-12);
+ BOOST_CHECK((vxvx*p).isApprox(v.cross(v.cross(p)), 1e-12));
Matrix3 vx = skew(v);
Matrix3 vxvx2 = (vx*vx).eval();
- is_matrix_absolutely_closed(vxvx.matrix(), vxvx2, 1e-12);
+ BOOST_CHECK(vxvx.matrix().isApprox(vxvx2, 1e-12));
Symmetric3 S = Symmetric3::RandomPositive();
- is_matrix_absolutely_closed((S-Symmetric3::SkewSquare(v)).matrix(),
- S.matrix()-vxvx2, 1e-12);
+ BOOST_CHECK((S-Symmetric3::SkewSquare(v)).matrix()
+ .isApprox(S.matrix()-vxvx2, 1e-12));
double m = Eigen::internal::random<double>()+1;
- is_matrix_absolutely_closed((S-m*Symmetric3::SkewSquare(v)).matrix(),
- S.matrix()-m*vxvx2, 1e-12);
+ BOOST_CHECK((S-m*Symmetric3::SkewSquare(v)).matrix()
+ .isApprox(S.matrix()-m*vxvx2, 1e-12));
Symmetric3 S2 = S;
S -= Symmetric3::SkewSquare(v);
- is_matrix_absolutely_closed(S.matrix(), S2.matrix()-vxvx2, 1e-12);
+ BOOST_CHECK(S.matrix().isApprox(S2.matrix()-vxvx2, 1e-12));
S = S2; S -= m*Symmetric3::SkewSquare(v);
- is_matrix_absolutely_closed(S.matrix(), S2.matrix()-m*vxvx2, 1e-12);
+ BOOST_CHECK(S.matrix().isApprox(S2.matrix()-m*vxvx2, 1e-12));
}
@@ -201,10 +200,10 @@ BOOST_AUTO_TEST_CASE ( test_pinocchio_Sym3 )
Matrix3 R = (Eigen::Quaterniond(Eigen::Matrix<double,4,1>::Random())).normalized().matrix();
Symmetric3 RSRt = S.rotate(R);
- is_matrix_absolutely_closed(RSRt.matrix(), R*S.matrix()*R.transpose(), 1e-12);
+ BOOST_CHECK(RSRt.matrix().isApprox(R*S.matrix()*R.transpose(), 1e-12));
Symmetric3 RtSR = S.rotate(R.transpose());
- is_matrix_absolutely_closed(RtSR.matrix(), R.transpose()*S.matrix()*R, 1e-12);
+ BOOST_CHECK(RtSR.matrix().isApprox(R.transpose()*S.matrix()*R, 1e-12));
}
// Test operator vtiv
@@ -256,7 +255,7 @@ BOOST_AUTO_TEST_CASE ( test_metapod_LTI )
R.rotTSymmetricMatrix(S);
timeLTI(S,R,S2);
- is_matrix_absolutely_closed(S2.toMatrix(), R.toMatrix().transpose()*S.toMatrix()*R.toMatrix(), 1e-12);
+ BOOST_CHECK(S2.toMatrix().isApprox(R.toMatrix().transpose()*S.toMatrix()*R.toMatrix(), 1e-12));
const size_t NBT = 100000;
std::vector<Sym3> Sres (NBT);
@@ -290,7 +289,7 @@ BOOST_AUTO_TEST_CASE ( test_eigen_SelfAdj )
Sym3 S(M);
{
Matrix3 Scp = S;
- is_matrix_absolutely_closed(Scp-Scp.transpose(), Matrix3::Zero(), 1e-16);
+ BOOST_CHECK((Scp-Scp.transpose()).isApprox(Matrix3::Zero(), 1e-16));
}
Matrix3 M2 = Matrix3::Random();
@@ -303,7 +302,7 @@ BOOST_AUTO_TEST_CASE ( test_eigen_SelfAdj )
{
Matrix3 Masa1 = ASA1.selfadjointView<Eigen::Upper>();
Matrix3 Masa2 = ASA2.selfadjointView<Eigen::Upper>();
- is_matrix_absolutely_closed(Masa1, Masa2, 1e-16);
+ BOOST_CHECK(Masa1.isApprox(Masa2, 1e-16));
}
const size_t NBT = 100000;
diff --git a/unittest/tspatial.cpp b/unittest/tspatial.cpp
index 9e790debf4bfaf38917e0dcc921342fdd9528134..1c5823f3c2421d27de3fa860a96e60118455061d 100644
--- a/unittest/tspatial.cpp
+++ b/unittest/tspatial.cpp
@@ -29,7 +29,6 @@
#define BOOST_TEST_MODULE tspatialTest
#include <boost/test/unit_test.hpp>
#include <boost/utility/binary.hpp>
-#include "pinocchio/tools/matrix-comparison.hpp"
BOOST_AUTO_TEST_SUITE ( tspatialTest)
@@ -50,30 +49,30 @@ using namespace se3;
// Test internal product
Matrix4 aMc = amc;
- is_matrix_absolutely_closed(aMc, aMb*bMc, 1e-12);
+ BOOST_CHECK(aMc.isApprox(aMb*bMc, 1e-12));
Matrix4 bMa = amb.inverse();
- is_matrix_absolutely_closed(bMa, aMb.inverse(), 1e-12);
+ BOOST_CHECK(bMa.isApprox(aMb.inverse(), 1e-12));
// Test point action
Vector3 p = Vector3::Random();
Eigen::Matrix<double,4,1> p4; p4.head(3) = p; p4[3] = 1;
Vector3 Mp = (aMb*p4).head(3);
- is_matrix_absolutely_closed(amb.act(p), Mp, 1e-12);
+ BOOST_CHECK(amb.act(p).isApprox(Mp, 1e-12));
Vector3 Mip = (aMb.inverse()*p4).head(3);
- is_matrix_absolutely_closed(amb.actInv(p), Mip, 1e-12);
+ BOOST_CHECK(amb.actInv(p).isApprox(Mip, 1e-12));
// Test action matrix
Matrix6 aXb = amb;
Matrix6 bXc = bmc;
Matrix6 aXc = amc;
- is_matrix_absolutely_closed(aXc, aXb*bXc, 1e-12);
+ BOOST_CHECK(aXc.isApprox(aXb*bXc, 1e-12));
Matrix6 bXa = amb.inverse();
- is_matrix_absolutely_closed(bXa, aXb.inverse(), 1e-12);
+ BOOST_CHECK(bXa.isApprox(aXb.inverse(), 1e-12));
}
BOOST_AUTO_TEST_CASE ( test_Motion )
@@ -94,36 +93,36 @@ BOOST_AUTO_TEST_CASE ( test_Motion )
// Test .+.
Vector6 bvPbv2_vec = bv+bv2;
- is_matrix_absolutely_closed(bvPbv2_vec, bv_vec+bv2_vec, 1e-12);
+ BOOST_CHECK(bvPbv2_vec.isApprox(bv_vec+bv2_vec, 1e-12));
// Test -.
Vector6 Mbv_vec = -bv;
- is_matrix_absolutely_closed( Mbv_vec, -bv_vec, 1e-12);
+ BOOST_CHECK( Mbv_vec.isApprox(-bv_vec, 1e-12));
// Test .+=.
Motion bv3 = bv; bv3 += bv2;
- is_matrix_absolutely_closed( bv3.toVector(), bv_vec+bv2_vec, 1e-12);
+ BOOST_CHECK( bv3.toVector().isApprox(bv_vec+bv2_vec, 1e-12));
// Test .=V6
bv3 = bv2_vec;
- is_matrix_absolutely_closed( bv3.toVector(), bv2_vec, 1e-12);
+ BOOST_CHECK( bv3.toVector().isApprox(bv2_vec, 1e-12));
// Test constructor from V6
Motion bv4(bv2_vec);
- is_matrix_absolutely_closed( bv4.toVector(), bv2_vec, 1e-12);
+ BOOST_CHECK( bv4.toVector().isApprox(bv2_vec, 1e-12));
// Test action
Matrix6 aXb = amb;
- is_matrix_absolutely_closed(amb.act(bv).toVector(), aXb*bv_vec, 1e-12);
+ BOOST_CHECK(amb.act(bv).toVector().isApprox(aXb*bv_vec, 1e-12));
// Test action inverse
Matrix6 bXc = bmc;
- is_matrix_absolutely_closed(bmc.actInv(bv).toVector(), bXc.inverse()*bv_vec, 1e-12);
+ BOOST_CHECK(bmc.actInv(bv).toVector().isApprox(bXc.inverse()*bv_vec, 1e-12));
// Test double action
Motion cv = Motion::Random();
bv = bmc.act(cv);
- is_matrix_absolutely_closed(amb.act(bv).toVector(), amc.act(cv).toVector(), 1e-12);
+ BOOST_CHECK(amb.act(bv).toVector().isApprox(amc.act(cv).toVector(), 1e-12));
// Simple test for cross product vxv
Motion vxv = bv.cross(bv);
@@ -132,7 +131,7 @@ BOOST_AUTO_TEST_CASE ( test_Motion )
// Simple test for cross product vxf
Force f = Force(bv.toVector());
Force vxf = bv.cross(f);
- is_matrix_absolutely_closed(vxf.linear(), bv.angular().cross(f.linear()), 1e-12);
+ BOOST_CHECK(vxf.linear().isApprox(bv.angular().cross(f.linear()), 1e-12));
BOOST_CHECK_SMALL(vxf.angular().norm(), 1e-3);//previously ensure that ( vxf.angular().isMuchSmallerThan(1e-3));
// Test frame change for vxf
@@ -142,7 +141,7 @@ BOOST_AUTO_TEST_CASE ( test_Motion )
Force bf = amb.actInv(af);
Force avxf = av.cross(af);
Force bvxf = bv.cross(bf);
- is_matrix_absolutely_closed(avxf.toVector(), amb.act(bvxf).toVector(), 1e-12);
+ BOOST_CHECK(avxf.toVector().isApprox(amb.act(bvxf).toVector(), 1e-12));
// Test frame change for vxv
av = Motion::Random();
@@ -151,7 +150,7 @@ BOOST_AUTO_TEST_CASE ( test_Motion )
Motion bw = amb.actInv(aw);
Motion avxw = av.cross(aw);
Motion bvxw = bv.cross(bw);
- is_matrix_absolutely_closed(avxw.toVector(), amb.act(bvxw).toVector(), 1e-12);
+ BOOST_CHECK(avxw.toVector().isApprox(amb.act(bvxw).toVector(), 1e-12));
}
BOOST_AUTO_TEST_CASE ( test_Force )
@@ -172,37 +171,37 @@ BOOST_AUTO_TEST_CASE ( test_Force )
// Test .+.
Vector6 bfPbf2_vec = bf+bf2;
- is_matrix_absolutely_closed(bfPbf2_vec, bf_vec+bf2_vec, 1e-12);
+ BOOST_CHECK(bfPbf2_vec.isApprox(bf_vec+bf2_vec, 1e-12));
// Test -.
Vector6 Mbf_vec = -bf;
- is_matrix_absolutely_closed(Mbf_vec, -bf_vec, 1e-12);
+ BOOST_CHECK(Mbf_vec.isApprox(-bf_vec, 1e-12));
// Test .+=.
Force bf3 = bf; bf3 += bf2;
- is_matrix_absolutely_closed(bf3.toVector(), bf_vec+bf2_vec, 1e-12);
+ BOOST_CHECK(bf3.toVector().isApprox(bf_vec+bf2_vec, 1e-12));
// Test .= V6
bf3 = bf2_vec;
- is_matrix_absolutely_closed(bf3.toVector(), bf2_vec, 1e-12);
+ BOOST_CHECK(bf3.toVector().isApprox(bf2_vec, 1e-12));
// Test constructor from V6
Force bf4(bf2_vec);
- is_matrix_absolutely_closed(bf4.toVector(), bf2_vec, 1e-12);
+ BOOST_CHECK(bf4.toVector().isApprox(bf2_vec, 1e-12));
// Test action
Matrix6 aXb = amb;
- is_matrix_absolutely_closed(amb.act(bf).toVector(), aXb.inverse().transpose()*bf_vec, 1e-12);
+ BOOST_CHECK(amb.act(bf).toVector().isApprox(aXb.inverse().transpose()*bf_vec, 1e-12));
// Test action inverse
Matrix6 bXc = bmc;
- is_matrix_absolutely_closed(bmc.actInv(bf).toVector(), bXc.transpose()*bf_vec, 1e-12);
+ BOOST_CHECK(bmc.actInv(bf).toVector().isApprox(bXc.transpose()*bf_vec, 1e-12));
// Test double action
Force cf = Force::Random();
bf = bmc.act(cf);
- is_matrix_absolutely_closed(amb.act(bf).toVector(), amc.act(cf).toVector(), 1e-12);
+ BOOST_CHECK(amb.act(bf).toVector().isApprox(amc.act(cf).toVector(), 1e-12));
// Simple test for cross product
// Force vxv = bf.cross(bf);
@@ -226,42 +225,42 @@ BOOST_AUTO_TEST_CASE ( test_Inertia )
aI.lever().norm()); //previously ensure that( (matI.topRightCorner<3,3>()*aI.lever()).isMuchSmallerThan(aI.lever()) );
Inertia I1 = Inertia::Identity();
- is_matrix_absolutely_closed(I1.matrix(), Matrix6::Identity(), 1e-12);
+ BOOST_CHECK(I1.matrix().isApprox(Matrix6::Identity(), 1e-12));
// Test motion-to-force map
Motion v = Motion::Random();
Force f = I1 * v;
- is_matrix_absolutely_closed(f.toVector(), v.toVector(), 1e-12);
+ BOOST_CHECK(f.toVector().isApprox(v.toVector(), 1e-12));
// Test Inertia group application
SE3 bma = SE3::Random();
Inertia bI = bma.act(aI);
Matrix6 bXa = bma;
- is_matrix_absolutely_closed((bma.rotation()*aI.inertia().matrix()*bma.rotation().transpose()),
- (Matrix3)bI.inertia(), 1e-12);
- is_matrix_absolutely_closed((bXa.transpose().inverse() * aI.matrix() * bXa.inverse()),
- bI.matrix(), 1e-12);
+ BOOST_CHECK((bma.rotation()*aI.inertia().matrix()*bma.rotation().transpose())
+ .isApprox((Matrix3)bI.inertia(), 1e-12));
+ BOOST_CHECK((bXa.transpose().inverse() * aI.matrix() * bXa.inverse())
+ .isApprox(bI.matrix(), 1e-12));
// Test inverse action
- is_matrix_absolutely_closed((bXa.transpose() * bI.matrix() * bXa),
- bma.actInv(bI).matrix(), 1e-12);
+ BOOST_CHECK((bXa.transpose() * bI.matrix() * bXa)
+ .isApprox(bma.actInv(bI).matrix(), 1e-12));
// Test vxIv cross product
v = Motion::Random();
f = aI*v;
Force vxf = v.cross(f);
Force vxIv = aI.vxiv(v);
- is_matrix_absolutely_closed(vxf.toVector(), vxIv.toVector(), 1e-12);
+ BOOST_CHECK(vxf.toVector().isApprox(vxIv.toVector(), 1e-12));
// Test operator+
I1 = Inertia::Random();
Inertia I2 = Inertia::Random();
- is_matrix_absolutely_closed(I1.matrix()+I2.matrix(), (I1+I2).matrix(), 1e-12);
+ BOOST_CHECK((I1.matrix()+I2.matrix()).isApprox((I1+I2).matrix(), 1e-12));
// operator +=
Inertia I12 = I1;
I12 += I2;
- is_matrix_absolutely_closed(I1.matrix()+I2.matrix(), I12.matrix(), 1e-12);
+ BOOST_CHECK((I1.matrix()+I2.matrix()).isApprox(I12.matrix(), 1e-12));
// Test operator vtiv
double kinetic_ref = v.toVector().transpose() * aI.matrix() * v.toVector();
@@ -270,7 +269,7 @@ BOOST_AUTO_TEST_CASE ( test_Inertia )
// Test constructor (Matrix6)
Inertia I1_bis(I1.matrix());
- is_matrix_absolutely_closed(I1.matrix(), I1_bis.matrix(), 1e-12);
+ BOOST_CHECK(I1.matrix().isApprox(I1_bis.matrix(), 1e-12));
}
BOOST_AUTO_TEST_CASE ( test_ActOnSet )
@@ -282,13 +281,13 @@ BOOST_AUTO_TEST_CASE ( test_ActOnSet )
Matrix6N iF = Matrix6N::Random(),jF;
se3::forceSet::se3Action(jMi,iF,jF);
for( int k=0;k<N;++k )
- is_matrix_absolutely_closed(jMi.act(se3::Force(iF.col(k))).toVector(), jF.col(k), 1e-12);
+ BOOST_CHECK(jMi.act(se3::Force(iF.col(k))).toVector().isApprox(jF.col(k), 1e-12));
Matrix6N iV = Matrix6N::Random(),jV;
se3::motionSet::se3Action(jMi,iV,jV);
for( int k=0;k<N;++k )
- is_matrix_absolutely_closed(jMi.act(se3::Motion(iV.col(k))).toVector(), jV.col(k), 1e-12);
+ BOOST_CHECK(jMi.act(se3::Motion(iV.col(k))).toVector().isApprox(jV.col(k), 1e-12));
}
@@ -304,30 +303,30 @@ BOOST_AUTO_TEST_CASE ( test_Explog )
// exp3 and log3.
Vector3 v3(Vector3::Random());
Matrix3 R(se3::exp3(v3));
- is_matrix_absolutely_closed(R.transpose(), R.inverse(), EPSILON);
+ BOOST_CHECK(R.transpose().isApprox(R.inverse(), EPSILON));
BOOST_CHECK_SMALL(R.determinant() - 1.0, EPSILON);
Vector3 v3FromLog(se3::log3(R));
- is_matrix_absolutely_closed(v3, v3FromLog, EPSILON);
+ BOOST_CHECK(v3.isApprox(v3FromLog, EPSILON));
// exp6 and log6.
se3::Motion nu = se3::Motion::Random();
se3::SE3 m = se3::exp6(nu);
- is_matrix_absolutely_closed(m.rotation().transpose(), m.rotation().inverse(),
- EPSILON);
+ BOOST_CHECK(m.rotation().transpose().isApprox(m.rotation().inverse(),
+ EPSILON));
BOOST_CHECK_SMALL(m.rotation().determinant() - 1.0, EPSILON);
se3::Motion nuFromLog(se3::log6(m));
- is_matrix_absolutely_closed(nu.linear(), nuFromLog.linear(), EPSILON);
- is_matrix_absolutely_closed(nu.angular(), nuFromLog.angular(), EPSILON);
+ BOOST_CHECK(nu.linear().isApprox(nuFromLog.linear(), EPSILON));
+ BOOST_CHECK(nu.angular().isApprox(nuFromLog.angular(), EPSILON));
Vector6 v6(Vector6::Random());
se3::SE3 m2(se3::exp6(v6));
- is_matrix_absolutely_closed(m2.rotation().transpose(), m2.rotation().inverse(),
- EPSILON);
+ BOOST_CHECK(m2.rotation().transpose().isApprox(m2.rotation().inverse(),
+ EPSILON));
BOOST_CHECK_SMALL(m2.rotation().determinant() - 1.0, EPSILON);
Matrix4 M = m2.toHomogeneousMatrix();
se3::Motion nu2FromLog(se3::log6(M));
Vector6 v6FromLog(nu2FromLog.toVector());
- is_matrix_absolutely_closed(v6, v6FromLog, EPSILON);
+ BOOST_CHECK(v6.isApprox(v6FromLog, EPSILON));
}
BOOST_AUTO_TEST_SUITE_END ()
diff --git a/unittest/value.cpp b/unittest/value.cpp
index dfa746199e0e94f526f711628526ceb7a1859b3e..85bf88cee35e39d4bb900a0658cf2429b9af5b67 100644
--- a/unittest/value.cpp
+++ b/unittest/value.cpp
@@ -39,7 +39,6 @@
#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE ValueTest
#include <boost/test/unit_test.hpp>
-#include "pinocchio/tools/matrix-comparison.hpp"
BOOST_AUTO_TEST_SUITE ( RneaRevoluteJoints )
@@ -65,7 +64,7 @@ BOOST_AUTO_TEST_CASE ( test_000 )
v = Eigen::VectorXd::Zero(model.nv);
a = Eigen::VectorXd::Zero(model.nv);
rnea(model,data,q,v,a);
- is_matrix_absolutely_closed (expected,data.tau,1e-12);
+ BOOST_CHECK (expected.isApprox(data.tau,1e-12));
}
@@ -91,7 +90,7 @@ BOOST_AUTO_TEST_CASE( test_0V0 )
a = Eigen::VectorXd::Zero(model.nv);
rnea(model,data,q,v,a);
//std::cout << (expected-data.tau).norm() << std::endl;
- is_matrix_absolutely_closed (expected,data.tau,1e-7);
+ BOOST_CHECK (expected.isApprox(data.tau,1e-7));
}
BOOST_AUTO_TEST_CASE( test_0VA )
@@ -114,7 +113,7 @@ BOOST_AUTO_TEST_CASE( test_0VA )
for(int i=6;i<model.nv;++i) v[i] = i/100.;
for(int i=6;i<model.nv;++i) a[i] = i/10.;
rnea(model,data,q,v,a);
- is_matrix_absolutely_closed (expected,data.tau,1e-6);
+ BOOST_CHECK (expected.isApprox(data.tau,1e-6));
}
BOOST_AUTO_TEST_CASE( test_Q00 )
@@ -141,7 +140,7 @@ BOOST_AUTO_TEST_CASE( test_Q00 )
std::cout << expected << "\n ---------------- \n"
<< data.tau << std::endl;
- is_matrix_absolutely_closed (expected,data.tau,1e-6);
+ BOOST_CHECK (expected.isApprox(data.tau,1e-6));
}
@@ -165,7 +164,7 @@ BOOST_AUTO_TEST_CASE( test_QVA )
for(int i=6;i<model.nv;++i) v[i] = i/100.;
for(int i=6;i<model.nv;++i) a[i] = i/100.;
rnea(model,data,q,v,a);
- is_matrix_absolutely_closed (expected,data.tau,1e-7);
+ BOOST_CHECK (expected.isApprox(data.tau,1e-7));
}
BOOST_AUTO_TEST_SUITE_END ()