Commit a0f10edc authored by Valenza Florian's avatar Valenza Florian
Browse files

[Major][Unittest] Unifying unittest framework with boost_test_suite

parent a71c0449
......@@ -54,6 +54,7 @@ SET(${PROJECT_NAME}_MATH_HEADERS
SET(${PROJECT_NAME}_TOOLS_HEADERS
tools/timer.hpp
tools/matrix-comparison.hpp
)
SET(${PROJECT_NAME}_SPATIAL_HEADERS
......
#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);
}
}
}
......@@ -13,11 +13,74 @@
#include "pinocchio/tools/timer.hpp"
#include <iostream>
#include <boost/utility/binary.hpp>
#ifdef NDEBUG
# include <Eigen/Cholesky>
#endif
#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>
BOOST_AUTO_TEST_SUITE ( CholeskyTest)
BOOST_AUTO_TEST_CASE ( test_cholesky )
{
using namespace Eigen;
using namespace se3;
se3::Model model;
se3::buildModels::humanoidSimple(model,true);
se3::Data data(model);
VectorXd q = VectorXd::Zero(model.nq);
data.M.fill(0); // Only nonzero coeff of M are initialized by CRBA.
crba(model,data,q);
se3::cholesky::decompose(model,data);
data.M.triangularView<Eigen::StrictlyLower>() =
data.M.triangularView<Eigen::StrictlyUpper>().transpose();
const Eigen::MatrixXd & U = data.U;
const Eigen::VectorXd & D = data.D;
const Eigen::MatrixXd & M = data.M;
// #ifndef NDEBUG
std::cout << "M = [\n" << M << "];" << std::endl;
std::cout << "U = [\n" << U << "];" << std::endl;
std::cout << "D = [\n" << D.transpose() << "];" << std::endl;
// #endif
is_matrix_absolutely_closed(M, 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);
Eigen::VectorXd Utv = v; se3::cholesky::Utv(model,data,Utv);
is_matrix_absolutely_closed(Utv, 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);
Eigen::VectorXd Utiv = v; se3::cholesky::Utiv(model,data,Utiv);
is_matrix_absolutely_closed(Utiv, 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);
Eigen::VectorXd Mv = v; se3::cholesky::Mv(model,data,Mv,true);
is_matrix_absolutely_closed(Mv, M*v, 1e-12);
Mv = v; se3::cholesky::Mv(model,data,Mv,false);
is_matrix_absolutely_closed(Mv, M*v, 1e-12);
}
/* The flag triger the following timers:
* 000001: sparse UDUt cholesky
* 000010: dense Eigen LDLt cholesky (with pivot)
......@@ -26,21 +89,35 @@
* 010000: sparse U\v substitution
* 100000: sparse M*v multiplication without Cholesky
*/
void timings(const se3::Model & model, se3::Data& data, long flag)
BOOST_AUTO_TEST_CASE ( test_timings )
{
using namespace Eigen;
using namespace se3;
se3::Model model;
se3::buildModels::humanoidSimple(model,true);
se3::Data data(model);
VectorXd q = VectorXd::Zero(model.nq);
data.M.fill(0); // Only nonzero coeff of M are initialized by CRBA.
crba(model,data,q);
long flag = BOOST_BINARY(1000101);
StackTicToc timer(StackTicToc::US);
#ifdef NDEBUG
#ifdef NDEBUG
#ifdef _INTENSE_TESTING_
const int NBT = 1000*1000;
const int NBT = 1000*1000;
#else
const int NBT = 10;
#endif
#else
const int NBT = 1;
std::cout << "(the time score in debug mode is not relevant) " ;
#endif
bool verbose = flag & (flag-1) ; // True is two or more binaries of the flag are 1.
bool verbose = flag & (flag-1) ; // True is two or more binaries of the flag are 1.
if(verbose) std::cout <<"--" << std::endl;
if( flag >> 0 & 1 )
......@@ -130,65 +207,7 @@ void timings(const se3::Model & model, se3::Data& data, long flag)
timer.toc(std::cout,NBT);
}
}
}
void assertValues(const se3::Model & model, se3::Data& data)
{
se3::cholesky::decompose(model,data);
data.M.triangularView<Eigen::StrictlyLower>() =
data.M.triangularView<Eigen::StrictlyUpper>().transpose();
const Eigen::MatrixXd & U = data.U;
const Eigen::VectorXd & D = data.D;
const Eigen::MatrixXd & M = data.M;
// #ifndef NDEBUG
std::cout << "M = [\n" << M << "];" << std::endl;
std::cout << "U = [\n" << U << "];" << std::endl;
std::cout << "D = [\n" << D.transpose() << "];" << std::endl;
// #endif
assert( M.isApprox(U*D.asDiagonal()*U.transpose()) );
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);
assert( Uv.isApprox(U*v));
Eigen::VectorXd Utv = v; se3::cholesky::Utv(model,data,Utv);
assert( Utv.isApprox(U.transpose()*v));
Eigen::VectorXd Uiv = v; se3::cholesky::Uiv(model,data,Uiv);
assert( Uiv.isApprox(U.inverse()*v));
Eigen::VectorXd Utiv = v; se3::cholesky::Utiv(model,data,Utiv);
assert( Utiv.isApprox(U.transpose().inverse()*v));
Eigen::VectorXd Miv = v; se3::cholesky::solve(model,data,Miv);
assert( Miv.isApprox(M.inverse()*v));
Eigen::VectorXd Mv = v; se3::cholesky::Mv(model,data,Mv,true);
assert( Mv.isApprox(M*v));
Mv = v; se3::cholesky::Mv(model,data,Mv,false);
assert( Mv.isApprox(M*v));
}
int main()
{
using namespace Eigen;
using namespace se3;
se3::Model model;
se3::buildModels::humanoidSimple(model,true);
se3::Data data(model);
VectorXd q = VectorXd::Zero(model.nq);
data.M.fill(0); // Only nonzero coeff of M are initialized by CRBA.
crba(model,data,q);
assertValues(model,data);
timings(model,data,BOOST_BINARY(1000101));
return 0;
}
BOOST_AUTO_TEST_SUITE_END ()
......@@ -10,15 +10,64 @@
#include "pinocchio/multibody/parser/sample-models.hpp"
#include <iostream>
#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE ComTest
#include <boost/test/unit_test.hpp>
#include <boost/utility/binary.hpp>
#include "pinocchio/tools/matrix-comparison.hpp"
void timings(const se3::Model & model, se3::Data& data, long flag)
BOOST_AUTO_TEST_SUITE ( ComTest)
BOOST_AUTO_TEST_CASE ( test_com )
{
using namespace Eigen;
using namespace se3;
se3::Model model;
se3::buildModels::humanoidSimple(model);
se3::Data data(model);
VectorXd q = VectorXd::Zero(model.nq);
data.M.fill(0); crba(model,data,q);
/* Test COM against CRBA*/
Vector3d com = centerOfMass(model,data,q);
is_matrix_absolutely_closed(data.com[0], 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);
/* Test Jcom against CRBA */
Eigen::MatrixXd Jcom = jacobianCenterOfMass(model,data,q);
is_matrix_absolutely_closed(Jcom, getJacobianComFromCrba(model,data), 1e-12);
std::cout << "com = [ " << data.com[0].transpose() << " ];" << std::endl;
std::cout << "mass = [ " << data.mass[0] << " ];" << std::endl;
std::cout << "Jcom = [ " << data.Jcom << " ];" << std::endl;
std::cout << "M3 = [ " << data.M.topRows<3>() << " ];" << std::endl;
}
BOOST_AUTO_TEST_CASE ( test_timings )
{
using namespace Eigen;
using namespace se3;
se3::Model model;
se3::buildModels::humanoidSimple(model);
se3::Data data(model);
long flag = BOOST_BINARY(1111);
StackTicToc timer(StackTicToc::US);
#ifdef NDEBUG
#ifdef NDEBUG
#ifdef _INTENSE_TESTING_
const int NBT = 1000*1000;
const int NBT = 1000*1000;
#else
const int NBT = 10;
#endif
......@@ -28,7 +77,7 @@ void timings(const se3::Model & model, se3::Data& data, long flag)
std::cout << "(the time score in debug mode is not relevant) " ;
#endif
bool verbose = flag & (flag-1) ; // True is two or more binaries of the flag are 1.
bool verbose = flag & (flag-1) ; // True is two or more binaries of the flag are 1.
if(verbose) std::cout <<"--" << std::endl;
Eigen::VectorXd q = Eigen::VectorXd::Zero(model.nq);
......@@ -62,49 +111,7 @@ void timings(const se3::Model & model, se3::Data& data, long flag)
if(verbose) std::cout << "Jcom =\t";
timer.toc(std::cout,NBT);
}
}
void assertValues(const se3::Model & model, se3::Data& data)
{
using namespace Eigen;
using namespace se3;
VectorXd q = VectorXd::Zero(model.nq);
data.M.fill(0); crba(model,data,q);
{ /* Test COM against CRBA*/
Vector3d com = centerOfMass(model,data,q);
assert( data.com[0].isApprox( getComFromCrba(model,data) ));
}
{ /* Test COM against Jcom (both use different way of compute the COM. */
Vector3d com = centerOfMass(model,data,q);
jacobianCenterOfMass(model,data,q);
assert(com.isApprox(data.com[0]));
}
{ /* Test Jcom against CRBA */
Eigen::MatrixXd Jcom = jacobianCenterOfMass(model,data,q);
assert( Jcom.isApprox( getJacobianComFromCrba(model,data) ));
}
std::cout << "com = [ " << data.com[0].transpose() << " ];" << std::endl;
std::cout << "mass = [ " << data.mass[0] << " ];" << std::endl;
std::cout << "Jcom = [ " << data.Jcom << " ];" << std::endl;
std::cout << "M3 = [ " << data.M.topRows<3>() << " ];" << std::endl;
}
int main()
{
using namespace Eigen;
using namespace se3;
se3::Model model;
se3::buildModels::humanoidSimple(model);
se3::Data data(model);
assertValues(model,data);
timings(model,data,BOOST_BINARY(111));
return 0;
}
BOOST_AUTO_TEST_SUITE_END ()
......@@ -18,9 +18,9 @@
#include "pinocchio/tools/timer.hpp"
#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE NLETests
#define BOOST_TEST_MODULE CATTests
#include <boost/test/unit_test.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include "pinocchio/tools/matrix-comparison.hpp"
#include <iostream>
......@@ -30,23 +30,6 @@
#include <pmmintrin.h>
#endif
inline void is_matrix_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_CLOSE (M1 (i,j), M2 (i,j), tolerance);
}
}
}
BOOST_AUTO_TEST_SUITE ( ComputeAllTerms )
......@@ -72,9 +55,9 @@ BOOST_AUTO_TEST_CASE ( test_against_algo )
crba(model,data_other,q);
computeJacobians(model,data_other,q);
is_matrix_closed (data.nle, data_other.nle);
is_matrix_closed (data.M.triangularView<Eigen::Upper>(), data_other.M.triangularView<Eigen::Upper>());
is_matrix_closed (data.J, data_other.J);
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);
// -------
q.setZero ();
......@@ -86,9 +69,9 @@ BOOST_AUTO_TEST_CASE ( test_against_algo )
crba(model,data_other,q);
computeJacobians(model,data_other,q);
is_matrix_closed (data.nle, data_other.nle);
is_matrix_closed (data.M.triangularView<Eigen::Upper>(), data_other.M.triangularView<Eigen::Upper>());
is_matrix_closed (data.J, data_other.J);
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);
// -------
q.setOnes ();
......@@ -100,9 +83,9 @@ BOOST_AUTO_TEST_CASE ( test_against_algo )
crba(model,data_other,q);
computeJacobians(model,data_other,q);
is_matrix_closed (data.nle, data_other.nle);
is_matrix_closed (data.M.triangularView<Eigen::Upper>(), data_other.M.triangularView<Eigen::Upper>());
is_matrix_closed (data.J, data_other.J);
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);
// -------
q.setRandom ();
......@@ -114,9 +97,9 @@ BOOST_AUTO_TEST_CASE ( test_against_algo )
crba(model,data_other,q);
computeJacobians(model,data_other,q);
is_matrix_closed (data.nle, data_other.nle);
is_matrix_closed (data.M.triangularView<Eigen::Upper>(), data_other.M.triangularView<Eigen::Upper>());
is_matrix_closed (data.J, data_other.J);
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_AUTO_TEST_SUITE_END ()
......@@ -7,9 +7,17 @@
#include "pinocchio/multibody/force-set.hpp"
#include "pinocchio/multibody/joint/joint-revolute.hpp"
bool testForceSet()
#define BOOST_TEST_DYN_LINK
#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 )
BOOST_AUTO_TEST_CASE ( test_ForceSet )
{
using namespace se3;
using namespace se3;
typedef Eigen::Matrix<double,4,4> Matrix4;
typedef SE3::Matrix6 Matrix6;
typedef SE3::Vector3 Vector3;
......@@ -22,7 +30,7 @@ bool testForceSet()
ForceSet F(12);
ForceSet F2(Eigen::Matrix<double,3,2>::Zero(),Eigen::Matrix<double,3,2>::Zero());
F.block(10,2) = F2;
assert( F.matrix().col(10).norm() == 0.0 );
BOOST_CHECK_EQUAL(F.matrix().col(10).norm() , 0.0 );
assert( isnan(F.matrix()(0,9)) );
std::cout << "F10 = " << F2.matrix() << std::endl;
......@@ -31,24 +39,25 @@ bool testForceSet()
ForceSet F3(Eigen::Matrix<double,3,12>::Random(),Eigen::Matrix<double,3,12>::Random());
ForceSet F4 = amb.act(F3);
SE3::Matrix6 aXb= amb;
assert( (aXb.transpose().inverse()*F3.matrix()).isApprox(F4.matrix()) );
is_matrix_absolutely_closed((aXb.transpose().inverse()*F3.matrix()), F4.matrix(), 1e-12);
ForceSet bF = bmc.act(F3);
ForceSet aF = amb.act(bF);
ForceSet aF2 = amc.act(F3);
assert( aF.matrix().isApprox( aF2.matrix() ) );
is_matrix_absolutely_closed(aF.matrix(), aF2.matrix(), 1e-12);
ForceSet F36 = amb.act(F3.block(3,6));
assert( (aXb.transpose().inverse()*F3.matrix().block(0,3,6,6)).isApprox(F36.matrix()) );
is_matrix_absolutely_closed((aXb.transpose().inverse()*F3.matrix().block(0,3,6,6)), F36.matrix(), 1e-12);
ForceSet F36full(12); F36full.block(3,6) = amb.act(F3.block(3,6));
assert( (aXb.transpose().inverse()*F3.matrix().block(0,3,6,6))
.isApprox(F36full.matrix().block(0,3,6,6)) );
return true;
is_matrix_absolutely_closed((aXb.transpose().inverse()*F3.matrix().block(0,3,6,6)),
F36full.matrix().block(0,3,6,6),
1e-12);
}
bool testConstraintRX()
BOOST_AUTO_TEST_CASE ( test_ConstraintRX )
{
using namespace se3;
......@@ -61,19 +70,15 @@ bool testConstraintRX()
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;
assert( F.matrix().isApprox( Y.toMatrix().col(3) ) );
is_matrix_absolutely_closed(F.matrix(), 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);
assert( StF2.isApprox( ConstraintXd(S).matrix().transpose()*F2.matrix().block(0,5,6,3) ) );
return true;
is_matrix_absolutely_closed(StF2,
ConstraintXd(S).matrix().transpose()*F2.matrix().block(0,5,6,3),
1e-12);
}
int main()
{
testForceSet();
testConstraintRX();
return 1;
}
BOOST_AUTO_TEST_SUITE_END ()
......@@ -14,55 +14,73 @@
#include <iostream>
void timings(const se3::Model & model, se3::Data& data, int NBT = 100000)
{
Eigen::VectorXd q = Eigen::VectorXd::Zero(model.nq);
StackTicToc timer(StackTicToc::US); timer.tic();
SMOOTH(NBT)
{
crba(model,data,q);
}
timer.toc(std::cout,NBT);
}
void assertValues(const se3::Model & model, se3::Data& data)
{
using namespace Eigen;
using namespace se3;
Eigen::MatrixXd M(model.nv,model.nv);
Eigen::VectorXd q = Eigen::VectorXd::Zero(model.nq);
Eigen::VectorXd v = Eigen::VectorXd::Zero(model.nv);
Eigen::VectorXd a = Eigen::VectorXd::Zero(model.nv);
data.M.fill(0); crba(model,data,q);
data.M.triangularView<Eigen::StrictlyLower>() = data.M.transpose().triangularView<Eigen::StrictlyLower>();
/* Joint inertia from iterative crba. */
const Eigen::VectorXd bias = rnea(model,data,q,v,a);
for(int i=0;i<model.nv;++i)
{
M.col(i) = rnea(model,data,q,v,Eigen::VectorXd::Unit(model.nv,i)) - bias;
}
#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE CrbaTest
#include <boost/test/unit_test.hpp>
#include <boost/utility/binary.hpp>
#include "pinocchio/tools/matrix-comparison.hpp"
// std::cout << "Mcrb = [ " << data.M << " ];" << std::endl;
// std::cout << "Mrne = [ " << M << " ]; " << std::endl;
assert( M.isApprox(data.M) );
}
BOOST_AUTO_TEST_SUITE ( CrbaTest)
int main()
BOOST_AUTO_TEST_CASE ( test_crba )
{
se3::Model model;
se3::buildModels::humanoidSimple(model);
se3::Data data(model);
#ifdef NDEBUG
#ifdef _INTENSE_TESTING_
const int NBT = 1000*1000;
#else
const int NBT = 10;
#endif
Eigen::VectorXd q = Eigen::VectorXd::Zero(model.nq);
StackTicToc timer(StackTicToc::US); timer.tic();
SMOOTH(NBT)
{
crba(model,data,q);
}
timer.toc(std::cout,NBT);
#else
int NBT = 1;
using namespace Eigen;
using namespace se3;
Eigen::MatrixXd M(model.nv,model.nv);
Eigen::VectorXd q = Eigen::VectorXd::Zero(model.nq);
Eigen::VectorXd v = Eigen::VectorXd::Zero(model.nv);
Eigen::VectorXd a = Eigen::VectorXd::Zero(model.nv);
data.M.fill(0); crba(model,data,q);
data.M.triangularView<Eigen::StrictlyLower>() = data.M.transpose().triangularView<Eigen::StrictlyLower>();
#ifdef NDEBUG
timings(model,data);