diff --git a/CMakeLists.txt b/CMakeLists.txt
index b2815889e4bd67f242cb1a057f722ba5c17ad341..a3e22bec3b9499f936dcf2e9eac829fad13006ce 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -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
diff --git a/src/tools/matrix-comparison.hpp b/src/tools/matrix-comparison.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..6607b934fa6f3a04e5e3f1d7315a3f0e0e5defd4
--- /dev/null
+++ b/src/tools/matrix-comparison.hpp
@@ -0,0 +1,22 @@
+#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 c21266c77540172305fa816d0c33de36e1a47abb..8a5ae53e9ec36e340f78eb09d1cbde8b9392ba1f 100644
--- a/unittest/cholesky.cpp
+++ b/unittest/cholesky.cpp
@@ -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,19 +89,32 @@
* 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 _INTENSE_TESTING_
- 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
+ #ifdef NDEBUG
+ #ifdef _INTENSE_TESTING_
+ 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.
if(verbose) std::cout <<"--" << std::endl;
@@ -130,65 +206,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 ()
diff --git a/unittest/com.cpp b/unittest/com.cpp
index b1ac8ff7df3b6fe235939e153af0f642bf71d0ce..00811c366c2f3c370c2df37d795596578dae4eab 100644
--- a/unittest/com.cpp
+++ b/unittest/com.cpp
@@ -10,91 +10,51 @@
#include "pinocchio/multibody/parser/sample-models.hpp"
#include <iostream>
-#include <boost/utility/binary.hpp>
-void timings(const se3::Model & model, se3::Data& data, long flag)
-{
- using namespace se3;
- StackTicToc timer(StackTicToc::US);
-#ifdef NDEBUG
-#ifdef _INTENSE_TESTING_
- const int NBT = 1000*1000;
-#else
- const int NBT = 10;
-#endif
+#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"
-#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.
- if(verbose) std::cout <<"--" << std::endl;
- Eigen::VectorXd q = Eigen::VectorXd::Zero(model.nq);
+BOOST_AUTO_TEST_SUITE ( ComTest)
- if( flag >> 0 & 1 )
- {
- timer.tic();
- SMOOTH(NBT)
- {
- centerOfMass(model,data,q);
- }
- if(verbose) std::cout << "COM =\t";
- timer.toc(std::cout,NBT);
- }
- if( flag >> 1 & 1 )
- {
- timer.tic();
- SMOOTH(NBT)
- {
- centerOfMass(model,data,q,false);
- }
- if(verbose) std::cout << "Without sub-tree =\t";
- timer.toc(std::cout,NBT);
- }
- if( flag >> 2 & 1 )
- {
- timer.tic();
- SMOOTH(NBT)
- {
- jacobianCenterOfMass(model,data,q);
- }
- if(verbose) std::cout << "Jcom =\t";
- timer.toc(std::cout,NBT);
- }
-}
-
-void assertValues(const se3::Model & model, se3::Data& data)
+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);
- assert( data.com[0].isApprox( getComFromCrba(model,data) ));
- }
+ /* 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. */
- 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) ));
- }
+ /* 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;
}
-
-int main()
+
+
+BOOST_AUTO_TEST_CASE ( test_timings )
{
using namespace Eigen;
using namespace se3;
@@ -103,8 +63,55 @@ int main()
se3::buildModels::humanoidSimple(model);
se3::Data data(model);
- assertValues(model,data);
- timings(model,data,BOOST_BINARY(111));
+ long flag = BOOST_BINARY(1111);
+ StackTicToc timer(StackTicToc::US);
+ #ifdef NDEBUG
+ #ifdef _INTENSE_TESTING_
+ 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
- return 0;
+ 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);
+
+ if( flag >> 0 & 1 )
+ {
+ timer.tic();
+ SMOOTH(NBT)
+ {
+ centerOfMass(model,data,q);
+ }
+ if(verbose) std::cout << "COM =\t";
+ timer.toc(std::cout,NBT);
+ }
+
+ if( flag >> 1 & 1 )
+ {
+ timer.tic();
+ SMOOTH(NBT)
+ {
+ centerOfMass(model,data,q,false);
+ }
+ if(verbose) std::cout << "Without sub-tree =\t";
+ timer.toc(std::cout,NBT);
+ }
+
+ if( flag >> 2 & 1 )
+ {
+ timer.tic();
+ SMOOTH(NBT)
+ {
+ jacobianCenterOfMass(model,data,q);
+ }
+ if(verbose) std::cout << "Jcom =\t";
+ timer.toc(std::cout,NBT);
+ }
}
+
+BOOST_AUTO_TEST_SUITE_END ()
diff --git a/unittest/compute-all-terms.cpp b/unittest/compute-all-terms.cpp
index 547610dbaabbec7f9f16b0f7a77bd4584888b4c4..71c952ad563382beb5aa8a6a82c33edb5c4010b9 100644
--- a/unittest/compute-all-terms.cpp
+++ b/unittest/compute-all-terms.cpp
@@ -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 ()
diff --git a/unittest/constraint.cpp b/unittest/constraint.cpp
index 08170d73b75634f1b9c795368a6a215aff0f8d5b..f31478db97fd5e1e67b04c798548c004398e047f 100644
--- a/unittest/constraint.cpp
+++ b/unittest/constraint.cpp
@@ -7,7 +7,15 @@
#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;
typedef Eigen::Matrix<double,4,4> Matrix4;
@@ -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 ()
+
diff --git a/unittest/crba.cpp b/unittest/crba.cpp
index f5de9d2abaad7ba1bc6a4246b7d44f0c95d9c944..6c7418475007838e360e196679e720d72f5b93fd 100644
--- a/unittest/crba.cpp
+++ b/unittest/crba.cpp
@@ -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);
-#else
- assertValues(model,data);
- std::cout << "(the time score in debug mode is not relevant) " ;
- timings(model,data,1);
-#endif // ifndef NDEBUG
+ /* 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;
+ }
+
+ // std::cout << "Mcrb = [ " << data.M << " ];" << std::endl;
+ // std::cout << "Mrne = [ " << M << " ]; " << std::endl;
+ is_matrix_absolutely_closed(M,data.M,1e-12);
+
+ std::cout << "(the time score in debug mode is not relevant) " ;
+
+ q = Eigen::VectorXd::Zero(model.nq);
+
+ StackTicToc timer(StackTicToc::US); timer.tic();
+ SMOOTH(NBT)
+ {
+ crba(model,data,q);
+ }
+ timer.toc(std::cout,NBT);
+
+ #endif // ifndef NDEBUG
- return 0;
}
+
+BOOST_AUTO_TEST_SUITE_END ()
+
diff --git a/unittest/fusion.cpp b/unittest/fusion.cpp
index 6848d9a6e75d9cd9ad508f8016467c3625b96732..cd2dc6e690825f50736f196d232d1af0d81e8b60 100644
--- a/unittest/fusion.cpp
+++ b/unittest/fusion.cpp
@@ -12,6 +12,11 @@
#include "pinocchio/tools/timer.hpp"
#include <Eigen/Core>
+#define BOOST_TEST_DYN_LINK
+#define BOOST_TEST_MODULE FusionTest
+#include <boost/test/unit_test.hpp>
+#include <boost/utility/binary.hpp>
+
struct TestObj
{
int i;
@@ -60,7 +65,7 @@ struct CRTPDerived2 : public CRTPBase<CRTPDerived2>
// ReturnType operator()( const CRTP<D> & crtp ) const
// {
// return static_cast<D*>(this)->algo(crtp,
-// static_cast<D*>(this)->args);
+// static_cast<D*>(this)->args);
// }
// static
@@ -85,7 +90,7 @@ struct Launcher : public boost::static_visitor<int>
{
typedef bf::vector<const double &,const int &, const Eigen::MatrixXd &,
- const Eigen::MatrixXd &,const Eigen::MatrixXd &,const TestObj &> Args;
+ const Eigen::MatrixXd &,const Eigen::MatrixXd &,const TestObj &> Args;
Args args;
Launcher(Args args) : args(args) {}
@@ -103,7 +108,7 @@ struct Launcher : public boost::static_visitor<int>
template<typename D>
static int algo(CRTPBase<D> & crtp, const double & x,const int & y, const Eigen::MatrixXd & z,
- const Eigen::MatrixXd & ,const Eigen::MatrixXd & ,const TestObj & a)
+ const Eigen::MatrixXd & ,const Eigen::MatrixXd & ,const TestObj & a)
{
return crtp.hh(x,y,z,a);
}
@@ -127,9 +132,12 @@ namespace boost {
// res append2(t1 a1,t2 a2,v a3) { return push_front(push_front(a3,a2),a1); }
}}
-
-int main()
+
+
+BOOST_AUTO_TEST_SUITE ( FusionTest)
+
+BOOST_AUTO_TEST_CASE ( test_fusion )
{
CRTPDerived d;
//CRTPBase<CRTPDerived> & dref = d;
@@ -140,7 +148,7 @@ int main()
//Args args(1.0,1,Eigen::MatrixXd::Zero(3,3),TestObj(1));
Launcher::run(v, Launcher::Args(1.0,1,Eigen::MatrixXd::Zero(3,3),Eigen::MatrixXd::Zero(3,3),
- Eigen::MatrixXd::Zero(3,3),TestObj(1)) );
+ Eigen::MatrixXd::Zero(3,3),TestObj(1)) );
int i,j; double k;
bf::vector<int&> arg = bf::make_vector(boost::ref(j));
@@ -150,6 +158,7 @@ int main()
bf::vector<int &,double &,int &> arg2 = bf::append2(boost::ref(i),boost::ref(k),arg);
//bf::push_front(bf::push_front(arg1,boost::ref(k)),boost::ref(j));
-
- return 0;
}
+
+BOOST_AUTO_TEST_SUITE_END ()
+
diff --git a/unittest/jacobian.cpp b/unittest/jacobian.cpp
index 83c7b19bad252c555e61c70143e9fd85388501a4..def40f7591c5c8be39367dbd1332714dfd19e36e 100644
--- a/unittest/jacobian.cpp
+++ b/unittest/jacobian.cpp
@@ -6,87 +6,24 @@
#include "pinocchio/tools/timer.hpp"
#include <iostream>
-#include <boost/utility/binary.hpp>
-
-void timings(const se3::Model & model, se3::Data& data, long flag)
-{
- using namespace se3;
- StackTicToc timer(StackTicToc::US);
-#ifdef NDEBUG
-#ifdef _INTENSE_TESTING_
- 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.
- if(verbose) std::cout <<"--" << std::endl;
- Eigen::VectorXd q = Eigen::VectorXd::Zero(model.nq);
+#define BOOST_TEST_DYN_LINK
+#define BOOST_TEST_MODULE JacobianTest
+#include <boost/test/unit_test.hpp>
+#include <boost/utility/binary.hpp>
+#include "pinocchio/tools/matrix-comparison.hpp"
- if( flag >> 0 & 1 )
- {
- timer.tic();
- SMOOTH(NBT)
- {
- computeJacobians(model,data,q);
- }
- if(verbose) std::cout << "Compute =\t";
- timer.toc(std::cout,NBT);
- }
- if( flag >> 1 & 1 )
- {
- computeJacobians(model,data,q);
- Model::Index idx = model.existBodyName("rarm6")?model.getBodyId("rarm6"):model.nbody-1;
- Eigen::MatrixXd Jrh(6,model.nv); Jrh.fill(0);
-
- timer.tic();
- SMOOTH(NBT)
- {
- getJacobian<false>(model,data,idx,Jrh);
- }
- if(verbose) std::cout << "Copy =\t";
- timer.toc(std::cout,NBT);
- }
- if( flag >> 2 & 1 )
- {
- computeJacobians(model,data,q);
- Model::Index idx = model.existBodyName("rarm6")?model.getBodyId("rarm6"):model.nbody-1;
- Eigen::MatrixXd Jrh(6,model.nv); Jrh.fill(0);
-
- timer.tic();
- SMOOTH(NBT)
- {
- getJacobian<true>(model,data,idx,Jrh);
- }
- if(verbose) std::cout << "Change frame =\t";
- timer.toc(std::cout,NBT);
- }
- if( flag >> 3 & 1 )
- {
- computeJacobians(model,data,q);
- Model::Index idx = model.existBodyName("rarm6")?model.getBodyId("rarm6"):model.nbody-1;
- Eigen::MatrixXd Jrh(6,model.nv); Jrh.fill(0);
-
- timer.tic();
- SMOOTH(NBT)
- {
- jacobian(model,data,q,idx);
- }
- if(verbose) std::cout << "Single jacobian =\t";
- timer.toc(std::cout,NBT);
- }
-}
+BOOST_AUTO_TEST_SUITE ( JacobianTest)
-void assertValues(const se3::Model & model, se3::Data& data)
+BOOST_AUTO_TEST_CASE ( test_jacobian )
{
using namespace Eigen;
using namespace se3;
+ se3::Model model;
+ se3::buildModels::humanoidSimple(model);
+ se3::Data data(model);
+
VectorXd q = VectorXd::Zero(model.nq);
computeJacobians(model,data,q);
@@ -94,29 +31,30 @@ void assertValues(const se3::Model & model, se3::Data& data)
MatrixXd Jrh(6,model.nv); Jrh.fill(0);
getJacobian<false>(model,data,idx,Jrh);
- { /* Test J*q == v */
- VectorXd qdot = VectorXd::Random(model.nv);
- VectorXd qddot = VectorXd::Zero(model.nv);
- rnea( model,data,q,qdot,qddot );
- Motion v = data.oMi[idx].act( data.v[idx] );
- assert( v.toVector().isApprox( Jrh*qdot ));
- v.toVector().isApprox(Jrh*qdot);
- }
+ /* Test J*q == v */
+ VectorXd qdot = VectorXd::Random(model.nv);
+ 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);
- { /* Test local jacobian: rhJrh == rhXo oJrh */
- MatrixXd rhJrh(6,model.nv); rhJrh.fill(0);
- getJacobian<true>(model,data,idx,rhJrh);
- MatrixXd XJrh(6,model.nv);
- motionSet::se3Action( data.oMi[idx].inverse(), Jrh,XJrh );
- assert( XJrh.isApprox(rhJrh) );
- data.J.fill(0);
- XJrh = jacobian(model,data,q,idx);
- assert( XJrh.isApprox(rhJrh) );
- }
+ /* Test local jacobian: rhJrh == rhXo oJrh */
+ MatrixXd rhJrh(6,model.nv); rhJrh.fill(0);
+ 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);
+
+
+ data.J.fill(0);
+ XJrh = jacobian(model,data,q,idx);
+ is_matrix_absolutely_closed(XJrh,rhJrh,1e-12);
+
}
-
-int main()
+
+
+BOOST_AUTO_TEST_CASE ( test_timings )
{
using namespace Eigen;
using namespace se3;
@@ -125,8 +63,79 @@ int main()
se3::buildModels::humanoidSimple(model);
se3::Data data(model);
- assertValues(model,data);
- timings(model,data,BOOST_BINARY(1111));
+ long flag = BOOST_BINARY(1111);
+ StackTicToc timer(StackTicToc::US);
+ #ifdef NDEBUG
+ #ifdef _INTENSE_TESTING_
+ 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
- return 0;
+ 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);
+
+ if( flag >> 0 & 1 )
+ {
+ timer.tic();
+ SMOOTH(NBT)
+ {
+ computeJacobians(model,data,q);
+ }
+ if(verbose) std::cout << "Compute =\t";
+ timer.toc(std::cout,NBT);
+ }
+
+ if( flag >> 1 & 1 )
+ {
+ computeJacobians(model,data,q);
+ Model::Index idx = model.existBodyName("rarm6")?model.getBodyId("rarm6"):model.nbody-1;
+ Eigen::MatrixXd Jrh(6,model.nv); Jrh.fill(0);
+
+ timer.tic();
+ SMOOTH(NBT)
+ {
+ getJacobian<false>(model,data,idx,Jrh);
+ }
+ if(verbose) std::cout << "Copy =\t";
+ timer.toc(std::cout,NBT);
+ }
+
+ if( flag >> 2 & 1 )
+ {
+ computeJacobians(model,data,q);
+ Model::Index idx = model.existBodyName("rarm6")?model.getBodyId("rarm6"):model.nbody-1;
+ Eigen::MatrixXd Jrh(6,model.nv); Jrh.fill(0);
+
+ timer.tic();
+ SMOOTH(NBT)
+ {
+ getJacobian<true>(model,data,idx,Jrh);
+ }
+ if(verbose) std::cout << "Change frame =\t";
+ timer.toc(std::cout,NBT);
+ }
+
+ if( flag >> 3 & 1 )
+ {
+ computeJacobians(model,data,q);
+ Model::Index idx = model.existBodyName("rarm6")?model.getBodyId("rarm6"):model.nbody-1;
+ Eigen::MatrixXd Jrh(6,model.nv); Jrh.fill(0);
+
+ timer.tic();
+ SMOOTH(NBT)
+ {
+ jacobian(model,data,q,idx);
+ }
+ if(verbose) std::cout << "Single jacobian =\t";
+ timer.toc(std::cout,NBT);
+ }
}
+
+BOOST_AUTO_TEST_SUITE_END ()
+
diff --git a/unittest/joints.cpp b/unittest/joints.cpp
index 929942ad0a45061b04107abbdd1851acf299c91f..25f84f77cba54322c49f8e52c587ac0aba9503b9 100644
--- a/unittest/joints.cpp
+++ b/unittest/joints.cpp
@@ -16,20 +16,20 @@
#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE JointsTest
#include <boost/test/unit_test.hpp>
-#include <boost/test/floating_point_comparison.hpp>
+#include "pinocchio/tools/matrix-comparison.hpp"
//#define VERBOSE
template <typename JoinData_t>
void printOutJointData (
#ifdef VERBOSE
- const Eigen::VectorXd & q,
- const Eigen::VectorXd & q_dot,
- const JoinData_t & joint_data
+ const Eigen::VectorXd & q,
+ const Eigen::VectorXd & q_dot,
+ const JoinData_t & joint_data
#else
- const Eigen::VectorXd & ,
- const Eigen::VectorXd & ,
- const JoinData_t &
+ const Eigen::VectorXd & ,
+ const Eigen::VectorXd & ,
+ const JoinData_t &
#endif
)
{
@@ -45,23 +45,6 @@ void printOutJointData (
#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 ( JointSphericalZYX )
@@ -70,7 +53,6 @@ BOOST_AUTO_TEST_CASE ( test_kinematics )
using namespace se3;
typedef Motion::Vector3 Vector3;
- typedef Motion::Vector6 Vector6;
Motion expected_v_J (Motion::Zero ());
Motion expected_c_J (Motion::Zero ());
@@ -93,10 +75,10 @@ BOOST_AUTO_TEST_CASE ( test_kinematics )
printOutJointData <JointDataSphericalZYX> (q, q_dot, joint_data);
- is_matrix_closed (expected_configuration.rotation (), joint_data.M.rotation());
- is_matrix_closed (expected_configuration.translation (), joint_data.M.translation ());
- is_matrix_closed (expected_v_J.toVector (), ((Motion) joint_data.v).toVector());
- is_matrix_closed (expected_c_J.toVector (), ((Motion) joint_data.c).toVector());
+ 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());
// -------
q = Vector3 (1., 0., 0.);
@@ -113,10 +95,10 @@ BOOST_AUTO_TEST_CASE ( test_kinematics )
expected_v_J.angular () << 0., 0., 1.;
- is_matrix_closed (expected_configuration.rotation (), joint_data.M.rotation(), 1e-12);
- is_matrix_closed (expected_configuration.translation (), joint_data.M.translation (), 1e-12);
- is_matrix_closed (expected_v_J.toVector (), ((Motion) joint_data.v).toVector(), 1e-12);
- is_matrix_closed (expected_c_J.toVector (), ((Motion) joint_data.c).toVector(), 1e-12);
+ 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);
// -------
q = Vector3 (0., 1., 0.);
@@ -133,10 +115,10 @@ BOOST_AUTO_TEST_CASE ( test_kinematics )
expected_v_J.angular () << 0., 1., 0.;
- is_matrix_closed (expected_configuration.rotation (), joint_data.M.rotation(), 1e-12);
- is_matrix_closed (expected_configuration.translation (), joint_data.M.translation (), 1e-12);
- is_matrix_closed (expected_v_J.toVector (), ((Motion) joint_data.v).toVector(), 1e-12);
- is_matrix_closed (expected_c_J.toVector (), ((Motion) joint_data.c).toVector(), 1e-12);
+ 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);
// -------
q = Vector3 (0., 0., 1.);
@@ -153,10 +135,10 @@ BOOST_AUTO_TEST_CASE ( test_kinematics )
expected_v_J.angular () << 1., 0., 0.;
- is_matrix_closed (expected_configuration.rotation (), joint_data.M.rotation(), 1e-12);
- is_matrix_closed (expected_configuration.translation (), joint_data.M.translation (), 1e-12);
- is_matrix_closed (expected_v_J.toVector (), ((Motion) joint_data.v).toVector(), 1e-12);
- is_matrix_closed (expected_c_J.toVector (), ((Motion) joint_data.c).toVector(), 1e-12);
+ 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);
// -------
q = Vector3 (1., 1., 1.);
@@ -174,10 +156,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_closed (expected_configuration.rotation (), joint_data.M.rotation(), 1e-10);
- is_matrix_closed (expected_configuration.translation (), joint_data.M.translation (), 1e-10);
- is_matrix_closed (expected_v_J.toVector (), ((Motion) joint_data.v).toVector(), 1e-10);
- is_matrix_closed (expected_c_J.toVector (), ((Motion) joint_data.c).toVector(), 1e-10);
+ 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);
// -------
q = Vector3 (1., 1.5, 1.9);
@@ -195,10 +177,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_closed (expected_configuration.rotation (), joint_data.M.rotation(), 1e-10);
- is_matrix_closed (expected_configuration.translation (), joint_data.M.translation (), 1e-10);
- is_matrix_closed (expected_v_J.toVector (), ((Motion) joint_data.v).toVector(), 1e-10);
- is_matrix_closed (expected_c_J.toVector (), ((Motion) joint_data.c).toVector(), 1e-10);
+ 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_AUTO_TEST_CASE ( test_rnea )
@@ -221,7 +203,7 @@ BOOST_AUTO_TEST_CASE ( test_rnea )
rnea (model, data, q, v, a);
Vector3 tau_expected (0., -4.905, 0.);
- is_matrix_closed (tau_expected, data.tau, 1e-14);
+ is_matrix_absolutely_closed (tau_expected, data.tau, 1e-14);
q = Eigen::VectorXd::Ones (model.nq);
v = Eigen::VectorXd::Ones (model.nv);
@@ -230,7 +212,7 @@ BOOST_AUTO_TEST_CASE ( test_rnea )
rnea (model, data, q, v, a);
tau_expected << -0.53611600195085, -0.74621832606188, -0.38177329067604;
- is_matrix_closed (tau_expected, data.tau, 1e-12);
+ is_matrix_absolutely_closed (tau_expected, data.tau, 1e-12);
q << 3, 2, 1;
v = Eigen::VectorXd::Ones (model.nv);
@@ -239,7 +221,7 @@ BOOST_AUTO_TEST_CASE ( test_rnea )
rnea (model, data, q, v, a);
tau_expected << 0.73934458094049, 2.7804530848031, 0.50684940972146;
- is_matrix_closed (tau_expected, data.tau, 1e-12);
+ is_matrix_absolutely_closed (tau_expected, data.tau, 1e-12);
}
BOOST_AUTO_TEST_CASE ( test_crba )
@@ -265,7 +247,7 @@ BOOST_AUTO_TEST_CASE ( test_crba )
0, 1.25, 0,
0, 0, 1;
- is_matrix_closed (M_expected, data.M, 1e-14);
+ is_matrix_absolutely_closed (M_expected, data.M, 1e-14);
q = Eigen::VectorXd::Ones (model.nq);
@@ -275,7 +257,7 @@ BOOST_AUTO_TEST_CASE ( test_crba )
-5.5511151231258e-17, 1.25, 0,
-0.8414709848079, 0, 1;
- is_matrix_closed (M_expected, data.M, 1e-12);
+ is_matrix_absolutely_closed (M_expected, data.M, 1e-12);
q << 3, 2, 1;
@@ -285,7 +267,7 @@ BOOST_AUTO_TEST_CASE ( test_crba )
0, 1.25, 0,
-0.90929742682568, 0, 1;
- is_matrix_closed (M_expected, data.M, 1e-10);
+ is_matrix_absolutely_closed (M_expected, data.M, 1e-10);
}
BOOST_AUTO_TEST_SUITE_END ()
@@ -297,8 +279,6 @@ BOOST_AUTO_TEST_CASE ( test_kinematics )
{
using namespace se3;
- typedef Motion::Vector3 Vector3;
- typedef Motion::Vector6 Vector6;
Motion expected_v_J (Motion::Zero ());
Motion expected_c_J (Motion::Zero ());
@@ -321,10 +301,10 @@ BOOST_AUTO_TEST_CASE ( test_kinematics )
printOutJointData <JointDataPX> (q, q_dot, joint_data);
- is_matrix_closed (expected_configuration.rotation (), joint_data.M.rotation());
- is_matrix_closed (expected_configuration.translation (), joint_data.M.translation ());
- is_matrix_closed (expected_v_J.toVector (), ((Motion) joint_data.v).toVector());
- is_matrix_closed (expected_c_J.toVector (), ((Motion) joint_data.c).toVector());
+ 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());
// -------
q << 1.;
@@ -339,16 +319,15 @@ BOOST_AUTO_TEST_CASE ( test_kinematics )
expected_v_J.linear () << 1., 0., 0.;
- is_matrix_closed (expected_configuration.rotation (), joint_data.M.rotation(), 1e-12);
- is_matrix_closed (expected_configuration.translation (), joint_data.M.translation (), 1e-12);
- is_matrix_closed (expected_v_J.toVector (), ((Motion) joint_data.v).toVector(), 1e-12);
- is_matrix_closed (expected_c_J.toVector (), ((Motion) joint_data.c).toVector(), 1e-12);
+ 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_AUTO_TEST_CASE ( test_rnea )
{
using namespace se3;
- typedef Eigen::VectorXd VectorXd;
typedef Eigen::Matrix <double, 3, 1> Vector3;
typedef Eigen::Matrix <double, 3, 3> Matrix3;
@@ -368,7 +347,7 @@ BOOST_AUTO_TEST_CASE ( test_rnea )
Eigen::VectorXd tau_expected (Eigen::VectorXd::Zero (model.nq));
tau_expected << 0;
- is_matrix_closed (tau_expected, data.tau, 1e-14);
+ is_matrix_absolutely_closed (tau_expected, data.tau, 1e-14);
// -----
q = Eigen::VectorXd::Ones (model.nq);
@@ -378,7 +357,7 @@ BOOST_AUTO_TEST_CASE ( test_rnea )
rnea (model, data, q, v, a);
tau_expected << 1;
- is_matrix_closed (tau_expected, data.tau, 1e-12);
+ is_matrix_absolutely_closed (tau_expected, data.tau, 1e-12);
q << 3;
v = Eigen::VectorXd::Ones (model.nv);
@@ -387,7 +366,7 @@ BOOST_AUTO_TEST_CASE ( test_rnea )
rnea (model, data, q, v, a);
tau_expected << 1;
- is_matrix_closed (tau_expected, data.tau, 1e-12);
+ is_matrix_absolutely_closed (tau_expected, data.tau, 1e-12);
}
BOOST_AUTO_TEST_CASE ( test_crba )
@@ -410,19 +389,19 @@ BOOST_AUTO_TEST_CASE ( test_crba )
crba (model, data, q);
M_expected << 1.0;
- is_matrix_closed (M_expected, data.M, 1e-14);
+ is_matrix_absolutely_closed (M_expected, data.M, 1e-14);
q = Eigen::VectorXd::Ones (model.nq);
crba (model, data, q);
- is_matrix_closed (M_expected, data.M, 1e-12);
+ is_matrix_absolutely_closed (M_expected, data.M, 1e-12);
q << 3;
crba (model, data, q);
- is_matrix_closed (M_expected, data.M, 1e-10);
+ is_matrix_absolutely_closed (M_expected, data.M, 1e-10);
}
BOOST_AUTO_TEST_SUITE_END ()
@@ -436,7 +415,6 @@ BOOST_AUTO_TEST_CASE ( test_kinematics )
typedef Motion::Vector3 Vector3;
typedef Eigen::Matrix <double, 4, 1> Vector4;
- typedef Motion::Vector6 Vector6;
Motion expected_v_J (Motion::Zero ());
Motion expected_c_J (Motion::Zero ());
@@ -459,10 +437,10 @@ BOOST_AUTO_TEST_CASE ( test_kinematics )
printOutJointData <JointDataSpherical> (q, q_dot, joint_data);
- is_matrix_closed (expected_configuration.rotation (), joint_data.M.rotation());
- is_matrix_closed (expected_configuration.translation (), joint_data.M.translation ());
- is_matrix_closed (expected_v_J.toVector (), ((Motion) joint_data.v).toVector());
- is_matrix_closed (expected_c_J.toVector (), ((Motion) joint_data.c).toVector());
+ 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());
// -------
q = Vector4 (1., 0, 0., 1.); q.normalize();
@@ -479,10 +457,10 @@ BOOST_AUTO_TEST_CASE ( test_kinematics )
expected_v_J.angular () << 1., 0., 0.;
- is_matrix_closed (expected_configuration.rotation (), joint_data.M.rotation(), 1e-12);
- is_matrix_closed (expected_configuration.translation (), joint_data.M.translation (), 1e-12);
- is_matrix_closed (expected_v_J.toVector (), ((Motion) joint_data.v).toVector(), 1e-12);
- is_matrix_closed (expected_c_J.toVector (), ((Motion) joint_data.c).toVector(), 1e-12);
+ 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);
// -------
q = Vector4 (0., 1., 0., 1.); q.normalize();
@@ -499,10 +477,10 @@ BOOST_AUTO_TEST_CASE ( test_kinematics )
expected_v_J.angular () << 0., 1., 0.;
- is_matrix_closed (expected_configuration.rotation (), joint_data.M.rotation(), 1e-12);
- is_matrix_closed (expected_configuration.translation (), joint_data.M.translation (), 1e-12);
- is_matrix_closed (expected_v_J.toVector (), ((Motion) joint_data.v).toVector(), 1e-12);
- is_matrix_closed (expected_c_J.toVector (), ((Motion) joint_data.c).toVector(), 1e-12);
+ 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);
// -------
q = Vector4 (0., 0, 1., 1.); q.normalize();
@@ -519,10 +497,10 @@ BOOST_AUTO_TEST_CASE ( test_kinematics )
expected_v_J.angular () << 0., 0., 1.;
- is_matrix_closed (expected_configuration.rotation (), joint_data.M.rotation(), 1e-12);
- is_matrix_closed (expected_configuration.translation (), joint_data.M.translation (), 1e-12);
- is_matrix_closed (expected_v_J.toVector (), ((Motion) joint_data.v).toVector(), 1e-12);
- is_matrix_closed (expected_c_J.toVector (), ((Motion) joint_data.c).toVector(), 1e-12);
+ 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);
// -------
q = Vector4 (1., 1., 1., 1.); q.normalize();
@@ -539,10 +517,10 @@ BOOST_AUTO_TEST_CASE ( test_kinematics )
expected_v_J.angular () << 1., 1., 1.;
- is_matrix_closed (expected_configuration.rotation (), joint_data.M.rotation(), 1e-10);
- is_matrix_closed (expected_configuration.translation (), joint_data.M.translation (), 1e-10);
- is_matrix_closed (expected_v_J.toVector (), ((Motion) joint_data.v).toVector(), 1e-10);
- is_matrix_closed (expected_c_J.toVector (), ((Motion) joint_data.c).toVector(), 1e-10);
+ 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);
// -------
q = Vector4 (1., 1.5, 1.9, 1.); q.normalize();
@@ -559,10 +537,10 @@ BOOST_AUTO_TEST_CASE ( test_kinematics )
expected_v_J.angular () = q_dot;
- is_matrix_closed (expected_configuration.rotation (), joint_data.M.rotation(), 1e-10);
- is_matrix_closed (expected_configuration.translation (), joint_data.M.translation (), 1e-10);
- is_matrix_closed (expected_v_J.toVector (), ((Motion) joint_data.v).toVector(), 1e-10);
- is_matrix_closed (expected_c_J.toVector (), ((Motion) joint_data.c).toVector(), 1e-10);
+ 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_AUTO_TEST_CASE ( test_rnea )
@@ -587,7 +565,7 @@ BOOST_AUTO_TEST_CASE ( test_rnea )
tau_expected << 0, -4.905, 0;
- is_matrix_closed (tau_expected, data.tau, 1e-14);
+ is_matrix_absolutely_closed (tau_expected, data.tau, 1e-14);
q = Eigen::VectorXd::Ones (model.nq); q.normalize ();
v = Eigen::VectorXd::Ones (model.nv);
@@ -596,7 +574,7 @@ BOOST_AUTO_TEST_CASE ( test_rnea )
rnea (model, data, q, v, a);
tau_expected << 1, 1, 6.405;
- is_matrix_closed (tau_expected, data.tau, 1e-12);
+ is_matrix_absolutely_closed (tau_expected, data.tau, 1e-12);
q << 3, 2, 1, 1; q.normalize ();
v = Eigen::VectorXd::Ones (model.nv);
@@ -605,7 +583,7 @@ BOOST_AUTO_TEST_CASE ( test_rnea )
rnea (model, data, q, v, a);
tau_expected << 1, 4.597, 4.77;
- is_matrix_closed (tau_expected, data.tau, 1e-12);
+ is_matrix_absolutely_closed (tau_expected, data.tau, 1e-12);
}
BOOST_AUTO_TEST_CASE ( test_crba )
@@ -629,18 +607,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_closed (M_expected, data.M, 1e-14);
+ is_matrix_absolutely_closed (M_expected, data.M, 1e-14);
q = Eigen::VectorXd::Ones (model.nq); q.normalize();
crba (model, data, q);
- is_matrix_closed (M_expected, data.M, 1e-12);
+ is_matrix_absolutely_closed (M_expected, data.M, 1e-12);
q << 3, 2, 1, 1; q.normalize();
crba (model, data, q);
- is_matrix_closed (M_expected, data.M, 1e-10);
+ is_matrix_absolutely_closed (M_expected, data.M, 1e-10);
}
BOOST_AUTO_TEST_SUITE_END ()
@@ -652,9 +630,6 @@ BOOST_AUTO_TEST_CASE ( test_kinematics )
{
using namespace se3;
- typedef Motion::Vector3 Vector3;
- typedef Eigen::Matrix <double, 4, 1> Vector4;
- typedef Motion::Vector6 Vector6;
Motion expected_v_J (Motion::Zero ());
Motion expected_c_J (Motion::Zero ());
@@ -685,10 +660,10 @@ BOOST_AUTO_TEST_CASE ( test_kinematics )
printOutJointData <JointDataRevoluteUnaligned> (q, q_dot, joint_data_RU);
- is_matrix_closed (joint_data_RU.M.rotation(), joint_data_RX.M.rotation());
- is_matrix_closed (joint_data_RU.M.translation (), joint_data_RX.M.translation ());
- is_matrix_closed (((Motion) joint_data_RU.v).toVector(), ((Motion) joint_data_RX.v).toVector());
- is_matrix_closed (((Motion) joint_data_RU.c).toVector(), ((Motion) joint_data_RX.c).toVector());
+ 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());
}
@@ -711,7 +686,9 @@ BOOST_AUTO_TEST_CASE ( test_rnea )
Data dataRX (modelRX);
Data dataRU (modelRU);
- assert(modelRU.nq == modelRX.nq && modelRU.nv == modelRX.nv && "models don't have same dof");
+ BOOST_CHECK_EQUAL(modelRU.nq,modelRX.nq);
+ BOOST_CHECK_EQUAL(modelRU.nv,modelRX.nv);
+
Eigen::VectorXd q (Eigen::VectorXd::Zero (modelRU.nq));
Eigen::VectorXd v (Eigen::VectorXd::Zero (modelRU.nv));
@@ -720,7 +697,7 @@ BOOST_AUTO_TEST_CASE ( test_rnea )
rnea (modelRX, dataRX, q, v, a);
rnea (modelRU, dataRU, q, v, a);
- is_matrix_closed (dataRX.tau, dataRU.tau, 1e-14);
+ is_matrix_absolutely_closed (dataRX.tau, dataRU.tau, 1e-14);
q = Eigen::VectorXd::Ones (modelRU.nq); //q.normalize ();
v = Eigen::VectorXd::Ones (modelRU.nv);
@@ -729,7 +706,7 @@ BOOST_AUTO_TEST_CASE ( test_rnea )
rnea (modelRX, dataRX, q, v, a);
rnea (modelRU, dataRU, q, v, a);
- is_matrix_closed (dataRX.tau, dataRU.tau, 1e-12);
+ is_matrix_absolutely_closed (dataRX.tau, dataRU.tau, 1e-12);
q << 3.;
v = Eigen::VectorXd::Ones (modelRU.nv);
@@ -738,7 +715,7 @@ BOOST_AUTO_TEST_CASE ( test_rnea )
rnea (modelRX, dataRX, q, v, a);
rnea (modelRU, dataRU, q, v, a);
- is_matrix_closed (dataRX.tau, dataRU.tau, 1e-12);
+ is_matrix_absolutely_closed (dataRX.tau, dataRU.tau, 1e-12);
}
BOOST_AUTO_TEST_CASE ( test_crba )
@@ -761,14 +738,16 @@ BOOST_AUTO_TEST_CASE ( test_crba )
Data dataRX (modelRX);
Data dataRU (modelRU);
- assert(modelRU.nq == modelRX.nq && modelRU.nv == modelRX.nv && "models don't have same dof");
+ BOOST_CHECK_EQUAL(modelRU.nq,modelRX.nq);
+ BOOST_CHECK_EQUAL(modelRU.nv,modelRX.nv);
+
Eigen::VectorXd q (Eigen::VectorXd::Zero (modelRU.nq));
crba (modelRX, dataRX, q);
crba (modelRU, dataRU, q);
- is_matrix_closed (dataRX.M, dataRU.M, 1e-14);
+ is_matrix_absolutely_closed (dataRX.M, dataRU.M, 1e-14);
// ----
q = Eigen::VectorXd::Ones (modelRU.nq);
@@ -776,7 +755,7 @@ BOOST_AUTO_TEST_CASE ( test_crba )
crba (modelRX, dataRX, q);
crba (modelRU, dataRU, q);
- is_matrix_closed (dataRX.M, dataRU.M, 1e-14);
+ is_matrix_absolutely_closed (dataRX.M, dataRU.M, 1e-14);
// ----
q << 3;
@@ -784,7 +763,7 @@ BOOST_AUTO_TEST_CASE ( test_crba )
crba (modelRX, dataRX, q);
crba (modelRU, dataRU, q);
- is_matrix_closed (dataRX.M, dataRU.M, 1e-14);
+ is_matrix_absolutely_closed (dataRX.M, dataRU.M, 1e-14);
}
BOOST_AUTO_TEST_SUITE_END ()
@@ -817,11 +796,9 @@ BOOST_AUTO_TEST_CASE ( test_merge_body )
-0.625, 2.78125, 0.,
0., 0., 3.28125;
- assert (mergedInertia.mass()== expected_mass);
- if (mergedInertia.mass()!= expected_mass)
- exit(-1);
- is_matrix_closed (mergedInertia.lever(), expected_com);
- is_matrix_closed (mergedInertia.inertia().matrix(), expectedBodyInertia);
+ BOOST_CHECK_EQUAL(mergedInertia.mass(), expected_mass);
+ is_matrix_absolutely_closed (mergedInertia.lever(), expected_com);
+ is_matrix_absolutely_closed (mergedInertia.inertia().matrix(), expectedBodyInertia);
exit(0);
}
diff --git a/unittest/non-linear-effects.cpp b/unittest/non-linear-effects.cpp
index f58c402d6ff3b53d52cdf5548bb9db7311503e19..5dd5d85651ad3d99245b3cc671eb457a8c834479 100644
--- a/unittest/non-linear-effects.cpp
+++ b/unittest/non-linear-effects.cpp
@@ -18,7 +18,7 @@
#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE NLETests
#include <boost/test/unit_test.hpp>
-#include <boost/test/floating_point_comparison.hpp>
+#include "pinocchio/tools/matrix-comparison.hpp"
#include <iostream>
@@ -28,22 +28,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 ( NLE )
@@ -70,7 +54,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_closed (tau_nle, tau_rnea);
+ is_matrix_absolutely_closed (tau_nle, tau_rnea);
// -------
q.setZero ();
@@ -79,7 +63,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_closed (tau_nle, tau_rnea);
+ is_matrix_absolutely_closed (tau_nle, tau_rnea);
// -------
q.setOnes ();
@@ -88,7 +72,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_closed (tau_nle, tau_rnea);
+ is_matrix_absolutely_closed (tau_nle, tau_rnea);
// -------
q.setRandom ();
@@ -97,7 +81,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_closed (tau_nle, tau_rnea);
+ is_matrix_absolutely_closed (tau_nle, tau_rnea);
}
BOOST_AUTO_TEST_SUITE_END ()
diff --git a/unittest/rnea.cpp b/unittest/rnea.cpp
index 40b88cfffb665b94c8c79a29adbebc4e71a13f86..0426f11dc5ce5f5559d71a4b0dbc88c8f04fcd10 100644
--- a/unittest/rnea.cpp
+++ b/unittest/rnea.cpp
@@ -18,16 +18,24 @@
//#define __SSE3__
#include <fenv.h>
+
#ifdef __SSE3__
#include <pmmintrin.h>
#endif
-int main()
-{
-#ifdef __SSE3__
- _MM_SET_DENORMALS_ZERO_MODE(_MM_DENORMALS_ZERO_ON);
-#endif
+#define BOOST_TEST_DYN_LINK
+#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 )
+
+BOOST_AUTO_TEST_CASE ( test_rnea )
+{
+ #ifdef __SSE3__
+ _MM_SET_DENORMALS_ZERO_MODE(_MM_DENORMALS_ZERO_ON);
+ #endif
using namespace Eigen;
using namespace se3;
@@ -40,13 +48,13 @@ int main()
VectorXd q = VectorXd::Random(model.nq);
VectorXd v = VectorXd::Random(model.nv);
VectorXd a = VectorXd::Random(model.nv);
-
-#ifdef NDEBUG
- int NBT = 10000;
-#else
- int NBT = 1;
- std::cout << "(the time score in debug mode is not relevant) " ;
-#endif
+
+ #ifdef NDEBUG
+ int NBT = 10000;
+ #else
+ int NBT = 1;
+ std::cout << "(the time score in debug mode is not relevant) " ;
+ #endif
StackTicToc timer(StackTicToc::US); timer.tic();
SMOOTH(NBT)
@@ -55,5 +63,5 @@ int main()
}
timer.toc(std::cout,NBT);
- return 0;
}
+BOOST_AUTO_TEST_SUITE_END ()
diff --git a/unittest/symmetric.cpp b/unittest/symmetric.cpp
index 3f7904747e1b0fe0d585eececb19af8db0dfe8d2..f725841143829d5d7f3bd8b4843fda987e3f46a2 100644
--- a/unittest/symmetric.cpp
+++ b/unittest/symmetric.cpp
@@ -19,25 +19,62 @@
#include "pinocchio/tools/timer.hpp"
#include <boost/random.hpp>
-#include <assert.h>
+
+
#include "pinocchio/spatial/symmetric3.hpp"
+#define BOOST_TEST_DYN_LINK
+#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>
-EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Matrix3d);
-EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(se3::Symmetric3);
+EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Matrix3d)
+EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(se3::Symmetric3)
-/* --- PINOCCHIO ------------------------------------------------------------ */
-/* --- PINOCCHIO ------------------------------------------------------------ */
-/* --- PINOCCHIO ------------------------------------------------------------ */
void timeSym3(const se3::Symmetric3 & S,
- const se3::Symmetric3::Matrix3 & R,
- se3::Symmetric3 & res)
+ const se3::Symmetric3::Matrix3 & R,
+ se3::Symmetric3 & res)
{
res = S.rotate(R);
}
-void testSym3()
+#ifdef WITH_METAPOD
+
+#include <metapod/tools/spatial/lti.hh>
+#include <metapod/tools/spatial/rm-general.hh>
+
+EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(metapod::Spatial::ltI<double>)
+EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(metapod::Spatial::RotationMatrixTpl<double>)
+
+void timeLTI(const metapod::Spatial::ltI<double>& S,
+ const metapod::Spatial::RotationMatrixTpl<double>& R,
+ metapod::Spatial::ltI<double> & res)
+{
+ res = R.rotTSymmetricMatrix(S);
+}
+
+#endif
+
+void timeSelfAdj( const Eigen::Matrix3d & A,
+ const Eigen::Matrix3d & Sdense,
+ Eigen::Matrix3d & ASA )
+{
+ typedef Eigen::SelfAdjointView<const Eigen::Matrix3d,Eigen::Upper> Sym3;
+ Sym3 S(Sdense);
+ ASA.triangularView<Eigen::Upper>()
+ = A * S * A.transpose();
+}
+
+BOOST_AUTO_TEST_SUITE ( symmetricTest)
+
+/* --- PINOCCHIO ------------------------------------------------------------ */
+/* --- PINOCCHIO ------------------------------------------------------------ */
+/* --- PINOCCHIO ------------------------------------------------------------ */
+BOOST_AUTO_TEST_CASE ( test_pinocchio_Sym3 )
{
using namespace se3;
typedef Symmetric3::Matrix3 Matrix3;
@@ -48,17 +85,17 @@ void testSym3()
{
Matrix3 M = Matrix3::Random(); M = M*M.transpose();
Symmetric3 S(M);
- assert( S.matrix().isApprox(M) );
+ is_matrix_absolutely_closed(S.matrix(), M, 1e-12);
}
-
+
// S += S
{
Symmetric3
- S = Symmetric3::Random(),
- S2 = Symmetric3::Random();
+ S = Symmetric3::Random(),
+ S2 = Symmetric3::Random();
Symmetric3 Scopy = S;
S+=S2;
- assert( S.matrix().isApprox( S2.matrix()+Scopy.matrix()) );
+ is_matrix_absolutely_closed(S.matrix(), S2.matrix()+Scopy.matrix(), 1e-12);
}
// S + M
@@ -67,10 +104,10 @@ void testSym3()
Matrix3 M = Matrix3::Random(); M = M*M.transpose();
Symmetric3 S2 = S + M;
- assert( S2.matrix().isApprox( S.matrix()+M ));
+ is_matrix_absolutely_closed(S2.matrix(), S.matrix()+M, 1e-12);
S2 = S - M;
- assert( S2.matrix().isApprox( S.matrix()-M ));
+ is_matrix_absolutely_closed(S2.matrix(), S.matrix()-M, 1e-12);
}
// S*v
@@ -78,21 +115,21 @@ void testSym3()
Symmetric3 S = Symmetric3::Random();
Vector3 v = Vector3::Random();
Vector3 Sv = S*v;
- assert( Sv.isApprox( S.matrix()*v ));
+ is_matrix_absolutely_closed(Sv, S.matrix()*v, 1e-12);
}
// Random
for(int i=0;i<100;++i )
- {
- Matrix3 M = Matrix3::Random(); M = M*M.transpose();
- Symmetric3 S = Symmetric3::RandomPositive();
- Vector3 v = Vector3::Random();
- assert( (v.transpose()*(S*v))[0] > 0);
- }
+ {
+ Matrix3 M = Matrix3::Random(); M = M*M.transpose();
+ Symmetric3 S = Symmetric3::RandomPositive();
+ Vector3 v = Vector3::Random();
+ BOOST_CHECK_GT( (v.transpose()*(S*v))[0] , 0);
+ }
// Identity
{
- assert( Symmetric3::Identity().matrix().isApprox( Matrix3::Identity() ) );
+ is_matrix_absolutely_closed(Symmetric3::Identity().matrix(), Matrix3::Identity(), 1e-12);
}
// Skew2
@@ -101,94 +138,85 @@ void testSym3()
Symmetric3 vxvx = Symmetric3::SkewSquare(v);
Vector3 p = Vector3::UnitX();
- assert( (vxvx*p).isApprox( v.cross(v.cross(p)) ));
+ is_matrix_absolutely_closed(vxvx*p, v.cross(v.cross(p)), 1e-12);
+
p = Vector3::UnitY();
- assert( (vxvx*p).isApprox( v.cross(v.cross(p)) ));
+ is_matrix_absolutely_closed(vxvx*p, v.cross(v.cross(p)), 1e-12);
+
p = Vector3::UnitZ();
- assert( (vxvx*p).isApprox( v.cross(v.cross(p)) ));
+ is_matrix_absolutely_closed(vxvx*p, v.cross(v.cross(p)), 1e-12);
Matrix3 vx = skew(v);
Matrix3 vxvx2 = (vx*vx).eval();
- assert( vxvx.matrix().isApprox(vxvx2) );
+ is_matrix_absolutely_closed(vxvx.matrix(), vxvx2, 1e-12);
Symmetric3 S = Symmetric3::RandomPositive();
- assert( (S-Symmetric3::SkewSquare(v)).matrix()
- .isApprox( S.matrix()-vxvx2 ) );
+ is_matrix_absolutely_closed((S-Symmetric3::SkewSquare(v)).matrix(),
+ S.matrix()-vxvx2, 1e-12);
+
double m = Eigen::internal::random<double>()+1;
- assert( (S-m*Symmetric3::SkewSquare(v)).matrix()
- .isApprox( S.matrix()-m*vxvx2 ) );
+ is_matrix_absolutely_closed((S-m*Symmetric3::SkewSquare(v)).matrix(),
+ S.matrix()-m*vxvx2, 1e-12);
+
Symmetric3 S2 = S;
S -= Symmetric3::SkewSquare(v);
- assert(S.matrix().isApprox( S2.matrix()-vxvx2 ) );
+ is_matrix_absolutely_closed(S.matrix(), S2.matrix()-vxvx2, 1e-12);
+
S = S2; S -= m*Symmetric3::SkewSquare(v);
- assert(S.matrix().isApprox( S2.matrix()-m*vxvx2 ) );
+ is_matrix_absolutely_closed(S.matrix(), S2.matrix()-m*vxvx2, 1e-12);
}
// (i,j)
{
- Matrix3 M = Matrix3::Random(); M = M*M.transpose();
- Symmetric3 S(M);
- for(int i=0;i<3;++i)
- for(int j=0;j<3;++j)
- assert( S(i,j) == M(i,j) );
+ Matrix3 M = Matrix3::Random(); M = M*M.transpose();
+ Symmetric3 S(M);
+ for(int i=0;i<3;++i)
+ for(int j=0;j<3;++j)
+ BOOST_CHECK_EQUAL(S(i,j), M(i,j) );
+ }
}
- }
- // SRS
- {
- Symmetric3 S = Symmetric3::RandomPositive();
- Matrix3 R = (Eigen::Quaterniond(Eigen::Matrix<double,4,1>::Random())).normalized().matrix();
-
- Symmetric3 RSRt = S.rotate(R);
- assert( RSRt.matrix().isApprox( R*S.matrix()*R.transpose() ));
+ // SRS
+ {
+ Symmetric3 S = Symmetric3::RandomPositive();
+ 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);
- Symmetric3 RtSR = S.rotate(R.transpose());
- assert( RtSR.matrix().isApprox( R.transpose()*S.matrix()*R ));
- }
+ Symmetric3 RtSR = S.rotate(R.transpose());
+ is_matrix_absolutely_closed(RtSR.matrix(), R.transpose()*S.matrix()*R, 1e-12);
+ }
- // Time test
- {
- const int NBT = 100000;
- Symmetric3 S = Symmetric3::RandomPositive();
+ // Time test
+ {
+ const int NBT = 100000;
+ Symmetric3 S = Symmetric3::RandomPositive();
- std::vector<Symmetric3> Sres (NBT);
- std::vector<Matrix3> Rs (NBT);
- for(int i=0;i<NBT;++i)
- Rs[i] = (Eigen::Quaterniond(Eigen::Matrix<double,4,1>::Random())).normalized().matrix();
+ std::vector<Symmetric3> Sres (NBT);
+ std::vector<Matrix3> Rs (NBT);
+ for(int i=0;i<NBT;++i)
+ Rs[i] = (Eigen::Quaterniond(Eigen::Matrix<double,4,1>::Random())).normalized().matrix();
- std::cout << "Pinocchio: ";
- StackTicToc timer(StackTicToc::US); timer.tic();
- SMOOTH(NBT)
+ std::cout << "Pinocchio: ";
+ StackTicToc timer(StackTicToc::US); timer.tic();
+ SMOOTH(NBT)
{
- timeSym3(S,Rs[_smooth],Sres[_smooth]);
+ timeSym3(S,Rs[_smooth],Sres[_smooth]);
}
- timer.toc(std::cout,NBT);
- }
+ timer.toc(std::cout,NBT);
+ }
}
/* --- METAPOD -------------------------------------------------------------- */
/* --- METAPOD -------------------------------------------------------------- */
/* --- METAPOD -------------------------------------------------------------- */
-#ifdef WITH_METAPOD
-
-#include <metapod/tools/spatial/lti.hh>
-#include <metapod/tools/spatial/rm-general.hh>
-
-EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(metapod::Spatial::ltI<double>);
-EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(metapod::Spatial::RotationMatrixTpl<double>);
-
-void timeLTI(const metapod::Spatial::ltI<double>& S,
- const metapod::Spatial::RotationMatrixTpl<double>& R,
- metapod::Spatial::ltI<double> & res)
-{
- res = R.rotTSymmetricMatrix(S);
-}
-
-void testLTI()
+BOOST_AUTO_TEST_CASE ( test_metapod_LTI )
{
+#ifdef WITH_METAPOD
using namespace metapod::Spatial;
typedef ltI<double> Sym3;
@@ -202,7 +230,7 @@ void testLTI()
R.rotTSymmetricMatrix(S);
timeLTI(S,R,S2);
- assert( S2.toMatrix().isApprox( R.toMatrix().transpose()*S.toMatrix()*R.toMatrix()) );
+ is_matrix_absolutely_closed(S2.toMatrix(), R.toMatrix().transpose()*S.toMatrix()*R.toMatrix(), 1e-12);
const int NBT = 100000;
std::vector<Sym3> Sres (NBT);
@@ -213,35 +241,20 @@ void testLTI()
std::cout << "Metapod: ";
StackTicToc timer(StackTicToc::US); timer.tic();
SMOOTH(NBT)
- {
- timeLTI(S,Rs[_smooth],Sres[_smooth]);
- }
+ {
+ timeLTI(S, Rs[_smooth], Sres[_smooth]);
+ }
timer.toc(std::cout,NBT);
-}
-
-#else // #ifdef WITH_METAPOD
-
-void testLTI()
-{
+#else
std::cout << "Metapod is not installed ... skipping this test. " << std::endl;
+#endif
}
-#endif // #ifdef WITH_METAPOD
-
/* --- EIGEN SYMMETRIC ------------------------------------------------------ */
/* --- EIGEN SYMMETRIC ------------------------------------------------------ */
/* --- EIGEN SYMMETRIC ------------------------------------------------------ */
-void timeSelfAdj( const Eigen::Matrix3d & A,
- const Eigen::Matrix3d & Sdense,
- Eigen::Matrix3d & ASA )
-{
- typedef Eigen::SelfAdjointView<const Eigen::Matrix3d,Eigen::Upper> Sym3;
- Sym3 S(Sdense);
- ASA.triangularView<Eigen::Upper>()
- = A * S * A.transpose();
-}
-void testSelfAdj()
+BOOST_AUTO_TEST_CASE ( test_eigen_SelfAdj )
{
using namespace se3;
typedef Eigen::Matrix3d Matrix3;
@@ -251,7 +264,7 @@ void testSelfAdj()
Sym3 S(M);
{
Matrix3 Scp = S;
- assert( Scp-Scp.transpose()==Matrix3::Zero());
+ is_matrix_absolutely_closed(Scp-Scp.transpose(), Matrix3::Zero(), 1e-16);
}
Matrix3 M2 = Matrix3::Random();
@@ -264,7 +277,7 @@ void testSelfAdj()
{
Matrix3 Masa1 = ASA1.selfadjointView<Eigen::Upper>();
Matrix3 Masa2 = ASA2.selfadjointView<Eigen::Upper>();
- assert(Masa1.isApprox(Masa2));
+ is_matrix_absolutely_closed(Masa1, Masa2, 1e-16);
}
const int NBT = 100000;
@@ -276,18 +289,10 @@ void testSelfAdj()
std::cout << "Eigen: ";
StackTicToc timer(StackTicToc::US); timer.tic();
SMOOTH(NBT)
- {
- timeSelfAdj(Rs[_smooth],M,Sres[_smooth]);
- }
+ {
+ timeSelfAdj(Rs[_smooth],M,Sres[_smooth]);
+ }
timer.toc(std::cout,NBT);
}
+BOOST_AUTO_TEST_SUITE_END ()
-
-int main()
-{
- testSelfAdj();
- testLTI();
- testSym3();
-
- return 0;
-}
diff --git a/unittest/tspatial.cpp b/unittest/tspatial.cpp
index 84e935f436890dd72f9637c27afd79306665f109..4d8da25847ffebde7aba09e0d4164453bb1fc264 100644
--- a/unittest/tspatial.cpp
+++ b/unittest/tspatial.cpp
@@ -6,16 +6,18 @@
#include "pinocchio/spatial/inertia.hpp"
#include "pinocchio/spatial/act-on-set.hpp"
-#define TEST_SE3
-#define TEST_MOTION
-#define TEST_FORCE
-#define TEST_INERTIA
-#define TEST_SYM3
+#define BOOST_TEST_DYN_LINK
+#define BOOST_TEST_MODULE tspatialTest
+#include <boost/test/unit_test.hpp>
+#include <boost/utility/binary.hpp>
+#include "pinocchio/tools/matrix-comparison.hpp"
-bool testSE3()
+BOOST_AUTO_TEST_SUITE ( tspatialTest)
+
+BOOST_AUTO_TEST_CASE ( test_SE3 )
{
- using namespace se3;
+using namespace se3;
typedef Eigen::Matrix<double,4,4> Matrix4;
typedef SE3::Matrix6 Matrix6;
typedef SE3::Vector3 Vector3;
@@ -29,41 +31,36 @@ bool testSE3()
// Test internal product
Matrix4 aMc = amc;
- assert(aMc.isApprox(aMb*bMc));
+ is_matrix_absolutely_closed(aMc, aMb*bMc, 1e-12);
Matrix4 bMa = amb.inverse();
- assert(bMa.isApprox(aMb.inverse()));
+ is_matrix_absolutely_closed(bMa, aMb.inverse(), 1e-12);
- { // Test point action
- Vector3 p = Vector3::Random();
- Eigen::Matrix<double,4,1> p4; p4.head(3) = p; p4[3] = 1;
+ // 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);
- assert(amb.act(p).isApprox(Mp));
- Vector3 Mip = (aMb.inverse()*p4).head(3);
- assert(amb.actInv(p).isApprox(Mip));
- }
+ Vector3 Mp = (aMb*p4).head(3);
+ is_matrix_absolutely_closed(amb.act(p), Mp, 1e-12);
- { // Test action matrix
- Matrix6 aXb = amb;
- Matrix6 bXc = bmc;
- Matrix6 aXc = amc;
- assert(aXc.isApprox(aXb*bXc));
+ Vector3 Mip = (aMb.inverse()*p4).head(3);
+ is_matrix_absolutely_closed(amb.actInv(p), Mip, 1e-12);
- Matrix6 bXa = amb.inverse();
- assert(bXa.isApprox(aXb.inverse()));
- }
+ // Test action matrix
+ Matrix6 aXb = amb;
+ Matrix6 bXc = bmc;
+ Matrix6 aXc = amc;
+ is_matrix_absolutely_closed(aXc, aXb*bXc, 1e-12);
- return true;
+ Matrix6 bXa = amb.inverse();
+ is_matrix_absolutely_closed(bXa, aXb.inverse(), 1e-12);
}
-bool testMotion()
+BOOST_AUTO_TEST_CASE ( test_Motion )
{
using namespace se3;
- typedef Eigen::Matrix<double,4,4> Matrix4;
typedef SE3::Matrix6 Matrix6;
- typedef SE3::Vector3 Vector3;
typedef Motion::Vector6 Vector6;
SE3 amb = SE3::Random();
@@ -78,41 +75,46 @@ bool testMotion()
// Test .+.
Vector6 bvPbv2_vec = bv+bv2;
- assert( bvPbv2_vec.isApprox(bv_vec+bv2_vec) );
+ is_matrix_absolutely_closed(bvPbv2_vec, bv_vec+bv2_vec, 1e-12);
+
// Test -.
Vector6 Mbv_vec = -bv;
- assert( Mbv_vec.isApprox(-bv_vec) );
+ is_matrix_absolutely_closed( Mbv_vec, -bv_vec, 1e-12);
+
// Test .+=.
Motion bv3 = bv; bv3 += bv2;
- assert(bv3.toVector().isApprox(bv_vec+bv2_vec));
+ is_matrix_absolutely_closed( bv3.toVector(), bv_vec+bv2_vec, 1e-12);
+
// Test .=V6
bv3 = bv2_vec;
- assert(bv3.toVector().isApprox(bv2_vec));
+ is_matrix_absolutely_closed( bv3.toVector(), bv2_vec, 1e-12);
+
// Test constructor from V6
Motion bv4(bv2_vec);
- assert(bv4.toVector().isApprox(bv2_vec));
+ is_matrix_absolutely_closed( bv4.toVector(), bv2_vec, 1e-12);
// Test action
Matrix6 aXb = amb;
- assert( amb.act(bv).toVector().isApprox(aXb*bv_vec));
+ is_matrix_absolutely_closed(amb.act(bv).toVector(), aXb*bv_vec, 1e-12);
+
// Test action inverse
Matrix6 bXc = bmc;
- assert( bmc.actInv(bv).toVector().isApprox(bXc.inverse()*bv_vec));
+ is_matrix_absolutely_closed(bmc.actInv(bv).toVector(), bXc.inverse()*bv_vec, 1e-12);
// Test double action
Motion cv = Motion::Random();
bv = bmc.act(cv);
- assert( amb.act(bv).toVector().isApprox(amc.act(cv).toVector()) );
+ is_matrix_absolutely_closed(amb.act(bv).toVector(), amc.act(cv).toVector(), 1e-12);
// Simple test for cross product vxv
Motion vxv = bv.cross(bv);
- assert(vxv.toVector().tail(3).isMuchSmallerThan(1e-3));
+ BOOST_CHECK_SMALL(vxv.toVector().tail(3).norm(), 1e-3); //previously ensure that (vxv.toVector().tail(3).isMuchSmallerThan(1e-3));
// Simple test for cross product vxf
Force f = Force(bv.toVector());
Force vxf = bv.cross(f);
- assert( vxf.linear().isApprox( bv.angular().cross(f.linear())));
- assert( vxf.angular().isMuchSmallerThan(1e-3));
+ is_matrix_absolutely_closed(vxf.linear(), 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
Motion av = Motion::Random();
@@ -121,7 +123,7 @@ bool testMotion()
Force bf = amb.actInv(af);
Force avxf = av.cross(af);
Force bvxf = bv.cross(bf);
- assert( avxf.toVector().isApprox( amb.act(bvxf).toVector()) );
+ is_matrix_absolutely_closed(avxf.toVector(), amb.act(bvxf).toVector(), 1e-12);
// Test frame change for vxv
av = Motion::Random();
@@ -130,18 +132,13 @@ bool testMotion()
Motion bw = amb.actInv(aw);
Motion avxw = av.cross(aw);
Motion bvxw = bv.cross(bw);
- assert( avxw.toVector().isApprox( amb.act(bvxw).toVector()) );
-
- return true;
+ is_matrix_absolutely_closed(avxw.toVector(), amb.act(bvxw).toVector(), 1e-12);
}
-
-bool testForce()
+BOOST_AUTO_TEST_CASE ( test_Force )
{
using namespace se3;
- typedef Eigen::Matrix<double,4,4> Matrix4;
typedef SE3::Matrix6 Matrix6;
- typedef SE3::Vector3 Vector3;
typedef Force::Vector6 Vector6;
SE3 amb = SE3::Random();
@@ -156,93 +153,99 @@ bool testForce()
// Test .+.
Vector6 bfPbf2_vec = bf+bf2;
- assert( bfPbf2_vec.isApprox(bf_vec+bf2_vec) );
+ is_matrix_absolutely_closed(bfPbf2_vec, bf_vec+bf2_vec, 1e-12);
+
// Test -.
Vector6 Mbf_vec = -bf;
- assert( Mbf_vec.isApprox(-bf_vec) );
+ is_matrix_absolutely_closed(Mbf_vec, -bf_vec, 1e-12);
+
// Test .+=.
Force bf3 = bf; bf3 += bf2;
- assert(bf3.toVector().isApprox(bf_vec+bf2_vec));
+ is_matrix_absolutely_closed(bf3.toVector(), bf_vec+bf2_vec, 1e-12);
+
// Test .= V6
bf3 = bf2_vec;
- assert(bf3.toVector().isApprox(bf2_vec));
+ is_matrix_absolutely_closed(bf3.toVector(), bf2_vec, 1e-12);
+
// Test constructor from V6
Force bf4(bf2_vec);
- assert(bf4.toVector().isApprox(bf2_vec));
+ is_matrix_absolutely_closed(bf4.toVector(), bf2_vec, 1e-12);
+
// Test action
Matrix6 aXb = amb;
- assert( amb.act(bf).toVector().isApprox(aXb.inverse().transpose()*bf_vec));
+ is_matrix_absolutely_closed(amb.act(bf).toVector(), aXb.inverse().transpose()*bf_vec, 1e-12);
+
// Test action inverse
Matrix6 bXc = bmc;
- assert( bmc.actInv(bf).toVector().isApprox(bXc.transpose()*bf_vec));
+ is_matrix_absolutely_closed(bmc.actInv(bf).toVector(), bXc.transpose()*bf_vec, 1e-12);
+
// Test double action
Force cf = Force::Random();
bf = bmc.act(cf);
- assert( amb.act(bf).toVector().isApprox(amc.act(cf).toVector()) );
+ is_matrix_absolutely_closed(amb.act(bf).toVector(), amc.act(cf).toVector(), 1e-12);
// Simple test for cross product
// Force vxv = bf.cross(bf);
- // assert(vxv.toVector().isMuchSmallerThan(bf.toVector()));
-
- return true;
+ // ensure that (vxv.toVector().isMuchSmallerThan(bf.toVector()));
}
-bool testInertia()
+BOOST_AUTO_TEST_CASE ( test_Inertia )
{
using namespace se3;
typedef Inertia::Matrix6 Matrix6;
typedef Inertia::Matrix3 Matrix3;
- typedef Inertia::Vector3 Vector3;
- typedef Eigen::Matrix<double,4,4> Matrix4;
Inertia aI = Inertia::Random();
Matrix6 matI = aI;
- assert( (matI(0,0) == aI.mass())
- && (matI(1,1) == aI.mass())
- && (matI(1,1) == aI.mass()) );
- assert( (matI-matI.transpose()).isMuchSmallerThan(matI) );
- assert( (matI.topRightCorner<3,3>()*aI.lever()).isMuchSmallerThan(aI.lever()) );
+ BOOST_CHECK_EQUAL(matI(0,0), aI.mass());
+ BOOST_CHECK_EQUAL(matI(1,1), aI.mass());
+ BOOST_CHECK_EQUAL(matI(2,2), aI.mass()); // 1,1 before unifying
+
+ BOOST_CHECK_SMALL((matI-matI.transpose()).norm(),matI.norm()); //previously ensure that( (matI-matI.transpose()).isMuchSmallerThan(matI) );
+ BOOST_CHECK_SMALL((matI.topRightCorner<3,3>()*aI.lever()).norm(),
+ aI.lever().norm()); //previously ensure that( (matI.topRightCorner<3,3>()*aI.lever()).isMuchSmallerThan(aI.lever()) );
Inertia I1 = Inertia::Identity();
- assert( I1.matrix() == Matrix6::Identity() );
+ is_matrix_absolutely_closed(I1.matrix(), Matrix6::Identity(), 1e-12);
// Test motion-to-force map
Motion v = Motion::Random();
Force f = I1 * v;
- assert( f.toVector() == v.toVector() );
+ is_matrix_absolutely_closed(f.toVector(), v.toVector(), 1e-12);
// Test Inertia group application
SE3 bma = SE3::Random();
Inertia bI = bma.act(aI);
Matrix6 bXa = bma;
- assert( (bma.rotation()*aI.inertia().matrix()*bma.rotation().transpose())
- .isApprox((Matrix3)bI.inertia()) );
- assert( (bXa.transpose().inverse() * aI.matrix() * bXa.inverse()).isApprox( bI.matrix()) );
+ 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);
// Test inverse action
- assert( (bXa.transpose() * bI.matrix() * bXa).isApprox( bma.actInv(bI).matrix()) );
+ is_matrix_absolutely_closed((bXa.transpose() * bI.matrix() * bXa),
+ 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);
- assert( vxf.toVector().isApprox(vxIv.toVector()) );
+ is_matrix_absolutely_closed(vxf.toVector(), vxIv.toVector(), 1e-12);
// Test operator+
I1 = Inertia::Random();
Inertia I2 = Inertia::Random();
- assert( (I1.matrix()+I2.matrix()).isApprox((I1+I2).matrix()) );
+ is_matrix_absolutely_closed(I1.matrix()+I2.matrix(), (I1+I2).matrix(), 1e-12);
+
// operator +=
Inertia I12 = I1;
I12 += I2;
- assert( (I1.matrix()+I2.matrix()).isApprox(I12.matrix()) );
-
- return true;
+ is_matrix_absolutely_closed(I1.matrix()+I2.matrix(), I12.matrix(), 1e-12);
}
-bool testActOnSet()
+BOOST_AUTO_TEST_CASE ( test_ActOnSet )
{
const int N = 20;
typedef Eigen::Matrix<double,6,N> Matrix6N;
@@ -251,24 +254,14 @@ bool testActOnSet()
Matrix6N iF = Matrix6N::Random(),jF;
se3::forceSet::se3Action(jMi,iF,jF);
for( int k=0;k<N;++k )
- assert( jMi.act(se3::Force(iF.col(k))).toVector().isApprox( jF.col(k) ));
+ is_matrix_absolutely_closed(jMi.act(se3::Force(iF.col(k))).toVector(), jF.col(k), 1e-12);
+
Matrix6N iV = Matrix6N::Random(),jV;
se3::motionSet::se3Action(jMi,iV,jV);
for( int k=0;k<N;++k )
- assert( jMi.act(se3::Motion(iV.col(k))).toVector().isApprox( jV.col(k) ));
+ is_matrix_absolutely_closed(jMi.act(se3::Motion(iV.col(k))).toVector(), jV.col(k), 1e-12);
- return true;
-}
-
-
-int main()
-{
- testSE3();
- testMotion();
- testForce();
- testInertia();
- testActOnSet();
- return 0;
}
+BOOST_AUTO_TEST_SUITE_END ()
diff --git a/unittest/udut.cpp b/unittest/udut.cpp
index 9432c3ac7398a75af6e956a2aa28fa0c7936071b..fe0df4f7827ddb8c184da9e87502af38197f84cc 100644
--- a/unittest/udut.cpp
+++ b/unittest/udut.cpp
@@ -2,6 +2,10 @@
#include <Eigen/Core>
#include <pinocchio/spatial/skew.hpp>
+#define BOOST_TEST_DYN_LINK
+#define BOOST_TEST_MODULE UdutTest
+#include <boost/test/unit_test.hpp>
+
template<int N>
void udut( Eigen::Matrix<double,N,N> & M )
{
@@ -26,9 +30,9 @@ void udut( Eigen::Matrix<double,N,N> & M )
}
}
+BOOST_AUTO_TEST_SUITE ( Udut )
-#include <Eigen/Core>
-int main(int argc, const char ** argv)
+BOOST_AUTO_TEST_CASE ( udut )
{
using namespace Eigen;
@@ -45,7 +49,6 @@ int main(int argc, const char ** argv)
std::cout << "A = [\n" << A << "];" << std::endl;
udut(A);
std::cout << "U = [\n" << A << "];" << std::endl;
-
-
- return 0;
}
+
+BOOST_AUTO_TEST_SUITE_END()
diff --git a/unittest/urdf.cpp b/unittest/urdf.cpp
index 0aabfddd20c7e3de45b0d3e7c1cea3252ad0f426..a0bd72f751eecf345f87deed653b6b14a01e6f91 100644
--- a/unittest/urdf.cpp
+++ b/unittest/urdf.cpp
@@ -3,15 +3,21 @@
#include "pinocchio/multibody/model.hpp"
#include "pinocchio/multibody/parser/urdf.hpp"
-int main(int argc, const char**argv)
+#define BOOST_TEST_DYN_LINK
+#define BOOST_TEST_MODULE UrdfTest
+#include <boost/test/unit_test.hpp>
+
+
+BOOST_AUTO_TEST_SUITE ( ParsingUrdfFile )
+
+BOOST_AUTO_TEST_CASE ( buildModel )
{
std::string filename = PINOCCHIO_SOURCE_DIR"/models/simple_humanoid.urdf";
- if(argc>1) filename = argv[1];
-#ifndef NDEBUG
- std::cout << "Parse filename \"" << filename << "\"" << std::endl;
-#endif
- se3::Model model = se3::urdf::buildModel(filename);
-
- return 0;
+ #ifndef NDEBUG
+ std::cout << "Parse filename \"" << filename << "\"" << std::endl;
+ #endif
+ se3::Model model = se3::urdf::buildModel(filename);
}
+
+BOOST_AUTO_TEST_SUITE_END()
diff --git a/unittest/value.cpp b/unittest/value.cpp
index 41a92f1156592adde9ac7c064f672ee289cf6618..f534106de01627c8ab41634dc369e04651ea44e6 100644
--- a/unittest/value.cpp
+++ b/unittest/value.cpp
@@ -19,10 +19,17 @@
#include "pinocchio/algorithm/rnea.hpp"
#include "pinocchio/algorithm/kinematics.hpp"
-int main(int argc, const char**argv)
+#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 )
+
+BOOST_AUTO_TEST_CASE ( test_000 )
{
std::string filename = PINOCCHIO_SOURCE_DIR"/models/simple_humanoid.urdf";
- if(argc>1) filename = argv[1];
se3::Model model = se3::urdf::buildModel(filename,true);
model.gravity.linear( Eigen::Vector3d(0,0,-9.8));
@@ -35,105 +42,116 @@ int main(int argc, const char**argv)
std::cout << std::setprecision(10);
- /* --- Test # 000 --- */
- {
- Eigen::VectorXd expected(model.nv);
- expected << 0, 0, 1281.84, 0, -40.5132, 0, 4.4492, -1.5386, 0, -1.5386, -1.5386, 0, -4.4492, -1.5386, 0, -1.5386, -1.5386, 0, -37.436, 0, 0, -2.548, 0, 0, 0.392, 0, 0.392, 0, -2.548, 0, 0, 0.392, 0, 0.392, 0 ;
- q = Eigen::VectorXd::Zero(model.nq);
- v = Eigen::VectorXd::Zero(model.nv);
- a = Eigen::VectorXd::Zero(model.nv);
- rnea(model,data,q,v,a);
- assert( expected.isApprox(data.tau) && "Test # 000 failed" );
- }
-
- /* --- Test # 0V0--- */
- {
- Eigen::VectorXd expected(model.nv);
- expected << -48.10636, -73.218816, 1384.901025, -7.2292939, -186.342371, -20.6685852, -0.78887946, -1.869651075, 1.8889752, -2.036768175, -2.105948175, -1.023232, -18.3738505, -4.133954895, 9.0456456, -4.276615035, -4.143427035, -2.534896, -180.338765, 15.71570676, -29.8639164, -59.917862, -2.3307916, -2.7648728, -45.76782776, 3.4151272, -18.4320456, -4.5768072, -76.60945104, -0.5897908, -2.640844, -63.93417064, 5.359156, -25.8309196, -6.976116;
- q = Eigen::VectorXd::Zero(model.nq);
- for(int i=6;i<model.nv;++i) v[i] = i/10.;
- a = Eigen::VectorXd::Zero(model.nv);
- rnea(model,data,q,v,a);
- //std::cout << (expected-data.tau).norm() << std::endl;
- assert( expected.isApprox(data.tau,1e-6) && "Test # 0V0 failed" );
- }
-
- /* --- Test # 0VA--- */
- {
- Eigen::VectorXd expected(model.nv);
- expected << -15.3331636, -0.67891816, 1273.80521, 102.9435113, 110.3509945, 81.52296995, 13.31476408, 14.26606068, 3.682505252, 9.048274318, 4.663303518, 2.05308568, 12.54347834, 25.92680911, 6.327105656, 16.71123385, 8.96650473, 3.54200704, 70.15812475, 77.02410963, 73.81994844, 41.73185754, 28.75786872, 28.94251127, 31.65847724, 20.40431127, 18.18579154, 6.838471928, 50.44193173, 34.07362801, 34.53507156, 38.33983417, 24.61507156, 22.2842788, 8.23435884;
- q = Eigen::VectorXd::Zero(model.nq);
- 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);
- assert( expected.isApprox(data.tau,1e-6) && "Test # 0VA failed");
- }
-
- /* --- Test # Q00 --- */
- {
- Eigen::VectorXd expected(model.nv);
- expected << 5.367234435e-15, -2.587860481e-14, 1281.84, -133.3062501, 198.975587, -7.120345979e-16, 15.06850407, 58.39287139, -22.14971864, 17.14327289, 1.291543104, 0.7402017048, -4.386231387, 22.73949408, -19.01681794, 0.8839600793, -0.3197599308, -0.466827706, 65.47086697, 32.71449398, -4.250622066, -0.7937685568, -0.15349648, -1.070480752, -3.066302263, -0.3557903212, -0.2183951073, 0.182684221, -0.6648425468, -2.902772493, 0.1250340934, 0.4402877138, -0.3158584741, -0.0865162794, 0.3918733239;
- for(int i=7;i<model.nq;++i) q[i] = (i-1)/10.;
- v = Eigen::VectorXd::Zero(model.nv);
- a = Eigen::VectorXd::Zero(model.nv);
- rnea(model,data,q,v,a);
- assert( expected.isApprox(data.tau,1e-6) && "Test # Q00 failed");
- }
-
- /* --- Test # QVA --- */
- {
- Eigen::VectorXd expected(model.nv);
- expected << -1.911650826, 1.250211406, 1284.82058, -139.0188156, 201.744449, 1.554847332, 15.1910084, 59.27339983, -21.70753738, 17.84339797, 1.754639468, 0.670280632, -2.968778954, 23.0776205, -17.56870284, 1.765761886, 0.2889992363, -0.392159764, 68.83598707, 34.59002827, -4.604435817, -0.3832225891, 1.085231916, -0.348635267, -2.831371037, -1.047616506, -0.228384161, 0.5656880079, 1.302869049, 0.8481280783, 0.7042182131, 1.554751317, -0.3908790552, -0.1294643218, 1.421077555;
- for(int i=7;i<model.nq;++i) q[i] = (i-1)/10.;
- 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);
- assert( expected.isApprox(data.tau,1e-6) && "Test # QVA failed");
- }
-
+ Eigen::VectorXd expected(model.nv);
+ expected << 0, 0, 1281.84, 0, -40.5132, 0, 4.4492, -1.5386, 0, -1.5386, -1.5386, 0, -4.4492, -1.5386, 0, -1.5386, -1.5386, 0, -37.436, 0, 0, -2.548, 0, 0, 0.392, 0, 0.392, 0, -2.548, 0, 0, 0.392, 0, 0.392, 0 ;
q = Eigen::VectorXd::Zero(model.nq);
v = Eigen::VectorXd::Zero(model.nv);
a = Eigen::VectorXd::Zero(model.nv);
- for(int i=7;i<model.nq;++i) q[i] = (i-1)/10.;
-
- Eigen::Vector3d rpy(1,2,3);
- Eigen::Matrix3d R = ( Eigen::AngleAxisd(rpy[0],Eigen::Vector3d::UnitX())
- * Eigen::AngleAxisd(rpy[1],Eigen::Vector3d::UnitY())
- * Eigen::AngleAxisd(rpy[2],Eigen::Vector3d::UnitZ())).matrix();
- q.segment<4>(3) = Eigen::Quaterniond(R).coeffs();
+ rnea(model,data,q,v,a);
+ is_matrix_absolutely_closed (expected,data.tau,1e-12);
+}
- // std::cout << "R1 = " << ( Eigen::AngleAxisd(rpy[0],Eigen::Vector3d::UnitX())).matrix() << std::endl;
- // std::cout << "R12 = " << ( Eigen::AngleAxisd(rpy[1],Eigen::Vector3d::UnitY())
- // * Eigen::AngleAxisd(rpy[0],Eigen::Vector3d::UnitX())).matrix() << std::endl;
+BOOST_AUTO_TEST_CASE( test_0V0 )
+{
+ std::string filename = PINOCCHIO_SOURCE_DIR"/models/simple_humanoid.urdf";
+ se3::Model model = se3::urdf::buildModel(filename,true);
+ model.gravity.linear( Eigen::Vector3d(0,0,-9.8));
+ se3::Data data(model);
- // std::cout << "R123 = " << R << std::endl;
+
+ Eigen::VectorXd q = Eigen::VectorXd::Zero(model.nq);
+ Eigen::VectorXd v = Eigen::VectorXd::Zero(model.nv);
+ Eigen::VectorXd a = Eigen::VectorXd::Zero(model.nv);
+
+ std::cout << std::setprecision(10);
+ Eigen::VectorXd expected(model.nv);
+ expected << -48.10636, -73.218816, 1384.901025, -7.2292939, -186.342371, -20.6685852, -0.78887946, -1.869651075, 1.8889752, -2.036768175, -2.105948175, -1.023232, -18.3738505, -4.133954895, 9.0456456, -4.276615035, -4.143427035, -2.534896, -180.338765, 15.71570676, -29.8639164, -59.917862, -2.3307916, -2.7648728, -45.76782776, 3.4151272, -18.4320456, -4.5768072, -76.60945104, -0.5897908, -2.640844, -63.93417064, 5.359156, -25.8309196, -6.976116;
+ q = Eigen::VectorXd::Zero(model.nq);
+ for(int i=6;i<model.nv;++i) v[i] = i/10.;
+ 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_AUTO_TEST_CASE( test_0VA )
+{
+ std::string filename = PINOCCHIO_SOURCE_DIR"/models/simple_humanoid.urdf";
+
+ se3::Model model = se3::urdf::buildModel(filename,true);
+ model.gravity.linear( Eigen::Vector3d(0,0,-9.8));
+ se3::Data data(model);
- //kinematics(model,data,q,v);
+
+ Eigen::VectorXd q = Eigen::VectorXd::Zero(model.nq);
+ Eigen::VectorXd v = Eigen::VectorXd::Zero(model.nv);
+ Eigen::VectorXd a = Eigen::VectorXd::Zero(model.nv);
+
+ std::cout << std::setprecision(10);
+ Eigen::VectorXd expected(model.nv);
+ expected << -15.3331636, -0.67891816, 1273.80521, 102.9435113, 110.3509945, 81.52296995, 13.31476408, 14.26606068, 3.682505252, 9.048274318, 4.663303518, 2.05308568, 12.54347834, 25.92680911, 6.327105656, 16.71123385, 8.96650473, 3.54200704, 70.15812475, 77.02410963, 73.81994844, 41.73185754, 28.75786872, 28.94251127, 31.65847724, 20.40431127, 18.18579154, 6.838471928, 50.44193173, 34.07362801, 34.53507156, 38.33983417, 24.61507156, 22.2842788, 8.23435884;
+ q = Eigen::VectorXd::Zero(model.nq);
+ 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);
+}
- using namespace Eigen;
- using namespace se3;
+BOOST_AUTO_TEST_CASE( test_Q00 )
+{
+ std::string filename = PINOCCHIO_SOURCE_DIR"/models/simple_humanoid.urdf";
- // std::cout << std::setprecision(10);
+ se3::Model model = se3::urdf::buildModel(filename,true);
+ model.gravity.linear( Eigen::Vector3d(0,0,-9.8));
+ se3::Data data(model);
- // std::cout << "Number of dof : " << model.nv << std::endl;
- // std::cout << "rnea(0,0,0) = g(0) = " << data.tau.transpose() << std::endl;
+
+ Eigen::VectorXd q = Eigen::VectorXd::Zero(model.nq);
+ Eigen::VectorXd v = Eigen::VectorXd::Zero(model.nv);
+ Eigen::VectorXd a = Eigen::VectorXd::Zero(model.nv);
+
+ std::cout << std::setprecision(10);
- // for( int i=0;i<3/*model.nbody*/;++i )
- // {
- // if(model.parents[i]!=i-1)
- // std::cout << "************** END EFFECTOR" << std::endl;
+ Eigen::VectorXd expected(model.nv);
+ expected << 5.367234435e-15, -2.587860481e-14, 1281.84, -133.3062501, 198.975587, -7.120345979e-16, 15.06850407, 58.39287139, -22.14971864, 17.14327289, 1.291543104, 0.7402017048, -4.386231387, 22.73949408, -19.01681794, 0.8839600793, -0.3197599308, -0.466827706, 65.47086697, 32.71449398, -4.250622066, -0.7937685568, -0.15349648, -1.070480752, -3.066302263, -0.3557903212, -0.2183951073, 0.182684221, -0.6648425468, -2.902772493, 0.1250340934, 0.4402877138, -0.3158584741, -0.0865162794, 0.3918733239;
+ for(int i=7;i<model.nq;++i) q[i] = (i-1)/10.;
+ v = Eigen::VectorXd::Zero(model.nv);
+ a = Eigen::VectorXd::Zero(model.nv);
+ rnea(model,data,q,v,a);
+ std::cout << expected << "\n ---------------- \n"
+ << data.tau << std::endl;
- // std::cout << "\n\n === " << i << " ========================" << std::endl;
- // std::cout << "Joint "<<i<<" = " << model.names[i] << std::endl;
- // std::cout << "m"<<i<<" = \n" << (SE3::Matrix4)data.oMi[i] << std::endl;
- // std::cout << "v"<<i<<" = \n" << SE3::Vector6(data.v[i]).transpose()<< std::endl;
- // std::cout << "a"<<i<<" = \n" << SE3::Vector6(data.a[i]).transpose() << std::endl;
- // std::cout << "f"<<i<<" = \n" << SE3::Vector6(data.f[i]).transpose() << std::endl;
- // }
+ is_matrix_absolutely_closed (expected,data.tau,1e-6);
+
+}
- return 0;
+BOOST_AUTO_TEST_CASE( test_QVA )
+{
+ std::string filename = PINOCCHIO_SOURCE_DIR"/models/simple_humanoid.urdf";
+
+ se3::Model model = se3::urdf::buildModel(filename,true);
+ model.gravity.linear( Eigen::Vector3d(0,0,-9.8));
+ se3::Data data(model);
+
+
+ Eigen::VectorXd q = Eigen::VectorXd::Zero(model.nq);
+ Eigen::VectorXd v = Eigen::VectorXd::Zero(model.nv);
+ Eigen::VectorXd a = Eigen::VectorXd::Zero(model.nv);
+
+ std::cout << std::setprecision(10);
+ Eigen::VectorXd expected(model.nv);
+ expected << -1.911650826, 1.250211406, 1284.82058, -139.0188156, 201.744449, 1.554847332, 15.1910084, 59.27339983, -21.70753738, 17.84339797, 1.754639468, 0.670280632, -2.968778954, 23.0776205, -17.56870284, 1.765761886, 0.2889992363, -0.392159764, 68.83598707, 34.59002827, -4.604435817, -0.3832225891, 1.085231916, -0.348635267, -2.831371037, -1.047616506, -0.228384161, 0.5656880079, 1.302869049, 0.8481280783, 0.7042182131, 1.554751317, -0.3908790552, -0.1294643218, 1.421077555;
+ for(int i=7;i<model.nq;++i) q[i] = (i-1)/10.;
+ 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_AUTO_TEST_SUITE_END ()
+
+
+
+
diff --git a/unittest/variant.cpp b/unittest/variant.cpp
index d75faff06e8bd548b48a54c23f349d841c33d25e..0cc0bbd29661008f0a4cd68a51bbbbe5fba2342e 100644
--- a/unittest/variant.cpp
+++ b/unittest/variant.cpp
@@ -1,6 +1,4 @@
-#include "pinocchio/spatial/fwd.hpp"
-#include "pinocchio/spatial/se3.hpp"
-#include "pinocchio/multibody/joint.hpp"
+
#include "pinocchio/multibody/model.hpp"
#include "pinocchio/multibody/joint/joint-generic.hpp"
@@ -8,10 +6,17 @@
#include "pinocchio/tools/timer.hpp"
-int main()
+#define BOOST_TEST_DYN_LINK
+#define BOOST_TEST_MODULE VariantTest
+#include <boost/test/unit_test.hpp>
+#include <boost/utility/binary.hpp>
+
+BOOST_AUTO_TEST_SUITE ( VariantTest)
+
+BOOST_AUTO_TEST_CASE ( test_variant )
{
- using namespace Eigen;
- using namespace se3;
+ using namespace Eigen;
+ using namespace se3;;
JointModelVariant jmodel = JointModelRX();
@@ -22,3 +27,5 @@ int main()
se3::Model model;
}
+
+BOOST_AUTO_TEST_SUITE_END ()