diff --git a/unittest/CMakeLists.txt b/unittest/CMakeLists.txt index a5069a4fa6fa07a63776b0748fff52dd2ae2e777..2c34be711c2a132954b16d9623114ddc3f9298d7 100644 --- a/unittest/CMakeLists.txt +++ b/unittest/CMakeLists.txt @@ -1,40 +1,53 @@ -ADD_EXECUTABLE(tspatial EXCLUDE_FROM_ALL ./tspatial.cpp) + + +ADD_EXECUTABLE(tspatial EXCLUDE_FROM_ALL tspatial.cpp) PKG_CONFIG_USE_DEPENDENCY(tspatial eigen3) -ADD_EXECUTABLE(symmetric EXCLUDE_FROM_ALL ./symmetric.cpp) +ADD_EXECUTABLE(symmetric EXCLUDE_FROM_ALL symmetric.cpp) PKG_CONFIG_USE_DEPENDENCY(symmetric eigen3) +IF(METAPOD_FOUND) + GET_TARGET_PROPERTY(SYMMETRIC_CFLAGS symmetric COMPILE_FLAGS) + SET_PROPERTY(TARGET symmetric + PROPERTY COMPILE_FLAGS "${SYMMETRIC_CFLAGS} -DWITH_METAPOD") +ENDIF(METAPOD_FOUND) -ADD_EXECUTABLE(constraint EXCLUDE_FROM_ALL ./constraint.cpp) -PKG_CONFIG_USE_DEPENDENCY(constraint eigen3) -ADD_EXECUTABLE(rnea EXCLUDE_FROM_ALL ./rnea.cpp) +ADD_EXECUTABLE(rnea EXCLUDE_FROM_ALL rnea.cpp) PKG_CONFIG_USE_DEPENDENCY(rnea eigen3) -ADD_EXECUTABLE(crba EXCLUDE_FROM_ALL ./crba.cpp) -PKG_CONFIG_USE_DEPENDENCY(crba eigenpy) # Should be eigen3 as well. Problem of link with ROS. -PKG_CONFIG_USE_DEPENDENCY(crba urdfdom) +ADD_EXECUTABLE(crba EXCLUDE_FROM_ALL crba.cpp) +PKG_CONFIG_USE_DEPENDENCY(crba eigen3) -ADD_EXECUTABLE(jacobian EXCLUDE_FROM_ALL ./jacobian.cpp) +ADD_EXECUTABLE(jacobian EXCLUDE_FROM_ALL jacobian.cpp) PKG_CONFIG_USE_DEPENDENCY(jacobian eigen3) -ADD_EXECUTABLE(com EXCLUDE_FROM_ALL ./com.cpp) +ADD_EXECUTABLE(com EXCLUDE_FROM_ALL com.cpp) PKG_CONFIG_USE_DEPENDENCY(com eigen3) -ADD_EXECUTABLE(cholesky EXCLUDE_FROM_ALL ./cholesky.cpp) +ADD_EXECUTABLE(cholesky EXCLUDE_FROM_ALL cholesky.cpp) PKG_CONFIG_USE_DEPENDENCY(cholesky eigen3) -PKG_CONFIG_USE_DEPENDENCY(cholesky urdfdom) -ADD_EXECUTABLE(urdf EXCLUDE_FROM_ALL ./urdf.cpp) -PKG_CONFIG_USE_DEPENDENCY(urdf eigenpy) -PKG_CONFIG_USE_DEPENDENCY(urdf urdfdom) -# The following two lines should be simpler with APPEND, but bug on CMake 2.8.7 (U12.04). -GET_TARGET_PROPERTY(URDF_CFLAGS urdf COMPILE_FLAGS) -SET_PROPERTY(TARGET urdf PROPERTY COMPILE_FLAGS "${URDF_CFLAGS} -DPINOCCHIO_SOURCE_DIR=\\\"${${PROJECT_NAME}_SOURCE_DIR}\\\"") +IF(URDFDOM_FOUND) + ADD_EXECUTABLE(urdf EXCLUDE_FROM_ALL urdf.cpp) + PKG_CONFIG_USE_DEPENDENCY(urdf eigen3) + PKG_CONFIG_USE_DEPENDENCY(urdf urdfdom) + + # The following two lines should be simpler with APPEND, but bug on CMake 2.8.7 (U12.04). + GET_TARGET_PROPERTY(URDF_CFLAGS urdf COMPILE_FLAGS) + SET_PROPERTY(TARGET urdf PROPERTY COMPILE_FLAGS "${URDF_CFLAGS} -DPINOCCHIO_SOURCE_DIR=\\\"${${PROJECT_NAME}_SOURCE_DIR}\\\"") + + ADD_EXECUTABLE(value EXCLUDE_FROM_ALL value.cpp) + PKG_CONFIG_USE_DEPENDENCY(value eigenpy) + PKG_CONFIG_USE_DEPENDENCY(value urdfdom) + GET_TARGET_PROPERTY(VALUE_CFLAGS value COMPILE_FLAGS) + SET_PROPERTY(TARGET value PROPERTY COMPILE_FLAGS "${VALUE_CFLAGS} -DPINOCCHIO_SOURCE_DIR=\\\"${${PROJECT_NAME}_SOURCE_DIR}\\\"") + +ENDIF(URDFDOM_FOUND) +# Work in progress -ADD_EXECUTABLE(value EXCLUDE_FROM_ALL ./value.cpp) -PKG_CONFIG_USE_DEPENDENCY(value eigen3) -PKG_CONFIG_USE_DEPENDENCY(value urdfdom) +#ADD_EXECUTABLE(constraint EXCLUDE_FROM_ALL constraint.cpp) +#PKG_CONFIG_USE_DEPENDENCY(constraint eigen3) -ADD_EXECUTABLE(variant EXCLUDE_FROM_ALL ./variant.cpp) -PKG_CONFIG_USE_DEPENDENCY(variant eigen3) +#ADD_EXECUTABLE(variant EXCLUDE_FROM_ALL variant.cpp) +#PKG_CONFIG_USE_DEPENDENCY(variant eigen3) diff --git a/unittest/cholesky.cpp b/unittest/cholesky.cpp index d9dd781bc1fe7c71032417e0a2a05f07e1631dd4..06bdc14b8083020f9ca685be31ff440d03c6262a 100644 --- a/unittest/cholesky.cpp +++ b/unittest/cholesky.cpp @@ -1,26 +1,15 @@ -#ifdef NDEBUG -#pragma GCC diagnostic ignored "-Wunused-but-set-variable" -#pragma GCC diagnostic ignored "-Wunused-variable" -#endif - -#include "pinocchio/spatial/fwd.hpp" #include "pinocchio/spatial/se3.hpp" -#include "pinocchio/multibody/joint.hpp" -#include "pinocchio/multibody/visitor.hpp" #include "pinocchio/multibody/model.hpp" -#include "pinocchio/algorithm/crba.hpp" #include "pinocchio/algorithm/cholesky.hpp" +#include "pinocchio/multibody/parser/sample-models.hpp" +#include "pinocchio/tools/timer.hpp" #include <iostream> +#include <boost/utility/binary.hpp> #ifdef NDEBUG # include <Eigen/Cholesky> #endif -#include "pinocchio/tools/timer.hpp" -#include "pinocchio/multibody/parser/sample-models.hpp" - -#include <boost/utility/binary.hpp> - /* The flag triger the following timers: * 000001: sparse UDUt cholesky * 000010: dense Eigen LDLt cholesky (with pivot) @@ -36,6 +25,7 @@ void timings(const se3::Model & model, se3::Data& data, long flag) const int NBT = 1000*1000; #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. @@ -66,7 +56,6 @@ void timings(const se3::Model & model, se3::Data& data, long flag) timer.toc(std::cout,NBT); } - if( flag >> 2 & 31 ) { std::vector<Eigen::VectorXd> randvec(NBT); @@ -141,16 +130,16 @@ void assertValues(const se3::Model & model, se3::Data& data) 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 + // #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; + // std::cout << "v = [" << v.transpose() << "]';" << std::endl; Eigen::VectorXd Uv = v; se3::cholesky::Uv(model,data,Uv); assert( Uv.isApprox(U*v)); @@ -180,17 +169,10 @@ int main() 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); -#ifndef NDEBUG assertValues(model,data); -#else timings(model,data,BOOST_BINARY(1000101)); -#endif return 0; } diff --git a/unittest/com.cpp b/unittest/com.cpp index 3864f655b4328a224853e2ea3d74b6f170802586..196bea346bd471ffc86f17a88859b9b636a00560 100644 --- a/unittest/com.cpp +++ b/unittest/com.cpp @@ -20,6 +20,7 @@ void timings(const se3::Model & model, se3::Data& data, long flag) const int NBT = 1000*1000; #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. @@ -43,7 +44,7 @@ void timings(const se3::Model & model, se3::Data& data, long flag) { centerOfMass(model,data,q,false); } - if(verbose) std::cout << "Wo stree =\t"; + if(verbose) std::cout << "Without sub-tree =\t"; timer.toc(std::cout,NBT); } if( flag >> 2 & 1 ) @@ -69,7 +70,6 @@ void assertValues(const se3::Model & model, se3::Data& data) { /* Test COM against CRBA*/ Vector3d com = centerOfMass(model,data,q); assert( data.com[0].isApprox( getComFromCrba(model,data) )); - } { /* Test COM against Jcom (both use different way of compute the COM. */ @@ -98,11 +98,8 @@ int main() se3::buildModels::humanoidSimple(model); se3::Data data(model); -#ifndef NDEBUG assertValues(model,data); -#else - timings(model,data,BOOST_BINARY(101)); -#endif + timings(model,data,BOOST_BINARY(111)); return 0; } diff --git a/unittest/constraint.cpp b/unittest/constraint.cpp index 7b053e8a2ea447a8eb7ef31da9bc64dd55f66836..08170d73b75634f1b9c795368a6a215aff0f8d5b 100644 --- a/unittest/constraint.cpp +++ b/unittest/constraint.cpp @@ -55,7 +55,7 @@ bool testConstraintRX() Inertia Y = Inertia::Random(); JointRX::ConstraintRevolute S; - std::cout << "Y = \n" << Y.toMatrix() << std::endl; + std::cout << "Y = \n" << Y.matrix() << std::endl; std::cout << "S = \n" << ((ConstraintXd)S).matrix() << std::endl; ForceSet F(1); F.block(0,1) = Y*S; diff --git a/unittest/crba.cpp b/unittest/crba.cpp index d20729323fc37520c6f1237a4885a7989216c1f8..c6e080003ecba0f7a7246bca4f2ee9b2787a9744 100644 --- a/unittest/crba.cpp +++ b/unittest/crba.cpp @@ -1,62 +1,58 @@ -#include "pinocchio/spatial/fwd.hpp" -#include "pinocchio/spatial/se3.hpp" -#include "pinocchio/multibody/joint.hpp" -#include "pinocchio/multibody/visitor.hpp" #include "pinocchio/multibody/model.hpp" #include "pinocchio/algorithm/crba.hpp" #include "pinocchio/algorithm/rnea.hpp" -#include "pinocchio/multibody/parser/urdf.hpp" #include "pinocchio/multibody/parser/sample-models.hpp" +#include "pinocchio/tools/timer.hpp" #include <iostream> -#include "pinocchio/tools/timer.hpp" - -int main(int argc, const char ** argv) +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; - se3::Model model; + 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); - std::string filename = "/home/nmansard/src/metapod/data/simple_arm.urdf"; - if(argc>1) filename = argv[1]; - se3::buildModels::humanoidSimple(model); - //model = se3::urdf::buildModel(filename,argc>1); - - se3::Data data(model); - VectorXd q = VectorXd::Zero(model.nq); - - StackTicToc timer(StackTicToc::US); timer.tic(); -#ifdef NDEBUG - SMOOTH(1000*100) -#endif - { - crba(model,data,q); + /* 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; } - timer.toc(std::cout,1000*100); -#ifndef NDEBUG std::cout << "Mcrb = [ " << data.M << " ];" << std::endl; + std::cout << "Mrne = [ " << M << " ]; " << std::endl; + assert( M.isApprox(data.M) ); +} -#ifdef __se3_rnea_hpp__ - /* Joint inertia from iterative crba. */ - { - Eigen::MatrixXd M(model.nv,model.nv); - Eigen::VectorXd v = Eigen::VectorXd::Zero(model.nv); - Eigen::VectorXd a = Eigen::VectorXd::Zero(model.nv); - 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 << "Mrne = [ " << M << " ]; " << std::endl; - } -#endif // ifdef __se3_rnea_hpp__ -#endif // ifndef NDEBUG +int main() +{ + se3::Model model; + se3::buildModels::humanoidSimple(model); + se3::Data data(model); +#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 return 0; } diff --git a/unittest/jacobian.cpp b/unittest/jacobian.cpp index d5678d0af46bed501db1af4d0bb5c90122964cd1..4866ba3e929db37187d231063a5b1b1f72f3b6f1 100644 --- a/unittest/jacobian.cpp +++ b/unittest/jacobian.cpp @@ -1,10 +1,9 @@ #include "pinocchio/multibody/model.hpp" #include "pinocchio/algorithm/jacobian.hpp" #include "pinocchio/algorithm/rnea.hpp" -#include "pinocchio/algorithm/crba.hpp" #include "pinocchio/spatial/act-on-set.hpp" -#include "pinocchio/tools/timer.hpp" #include "pinocchio/multibody/parser/sample-models.hpp" +#include "pinocchio/tools/timer.hpp" #include <iostream> #include <boost/utility/binary.hpp> @@ -17,6 +16,7 @@ void timings(const se3::Model & model, se3::Data& data, long flag) const int NBT = 1000*1000; #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. @@ -61,7 +61,7 @@ void timings(const se3::Model & model, se3::Data& data, long flag) if(verbose) std::cout << "Change frame =\t"; timer.toc(std::cout,NBT); } - if( flag >> 2 & 1 ) + if( flag >> 3 & 1 ) { computeJacobians(model,data,q); Model::Index idx = model.existBodyName("rarm6")?model.getBodyId("rarm6"):model.nbody-1; @@ -108,9 +108,6 @@ void assertValues(const se3::Model & model, se3::Data& data) XJrh = jacobian(model,data,q,idx); assert( XJrh.isApprox(rhJrh) ); } - - std::cout << "Jrh = [ " << Jrh << " ];" << std::endl; - std::cout << "J = [ " << data.J << " ];" << std::endl; } int main() @@ -122,11 +119,8 @@ int main() se3::buildModels::humanoidSimple(model); se3::Data data(model); -#ifndef NDEBUG assertValues(model,data); -#else - timings(model,data,BOOST_BINARY(111)); -#endif + timings(model,data,BOOST_BINARY(1111)); return 0; } diff --git a/unittest/rnea.cpp b/unittest/rnea.cpp index efcfebc351097e6418dc293f23dc0ff8fc459f39..9e0c5d772994cd077a8fc12785b9e8ead0c9fe9c 100644 --- a/unittest/rnea.cpp +++ b/unittest/rnea.cpp @@ -5,11 +5,9 @@ #include "pinocchio/multibody/model.hpp" #include "pinocchio/algorithm/rnea.hpp" #include "pinocchio/multibody/parser/sample-models.hpp" - -#include <iostream> - #include "pinocchio/tools/timer.hpp" +#include <iostream> //#define __SSE3__ #include <fenv.h> @@ -36,12 +34,19 @@ int main() 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 + StackTicToc timer(StackTicToc::US); timer.tic(); - SMOOTH(100000) + SMOOTH(NBT) { rnea(model,data,q,v,a); } - timer.toc(std::cout,100000); + timer.toc(std::cout,NBT); return 0; } diff --git a/unittest/symmetric.cpp b/unittest/symmetric.cpp index e7dd1a5a4aa2b6ac4fabc2a8cf446f0db6e3273c..dbb75921d966064af74606bbdcb61bcfa15ddf00 100644 --- a/unittest/symmetric.cpp +++ b/unittest/symmetric.cpp @@ -1,4 +1,18 @@ -#pragma GCC diagnostic ignored "-Wunused-but-set-variable" +/* --- Unitary test symmetric.cpp This code tests and compares three ways of + * expressing symmetric matrices. In addition to the unitary validation (test + * of the basic operations), the code is validating the computation + * performances of each methods. + * + * The three methods are: + * - Eigen SelfAdjoint (a mask atop of a classical dense matrix) ==> the least efficient. + * - Metapod Symmetric with LTI factorization. + * - Pinocchio rewritting of Metapod code with LTI factor as well and minor improvement. + * + * Expected time scores on a I7 2.1GHz: + * - Eigen: 2.5us + * - Metapod: 4us + * - Pinocchio: 6us + */ #include "pinocchio/spatial/fwd.hpp" #include "pinocchio/spatial/se3.hpp" @@ -10,9 +24,12 @@ #include "pinocchio/spatial/symmetric3.hpp" #include <Eigen/StdVector> -EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION( Eigen::Matrix3d ); +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) @@ -94,12 +111,9 @@ void testSym3() Matrix3 vxvx2 = (vx*vx).eval(); assert( vxvx.matrix().isApprox(vxvx2) ); - std::cout << "Sksq ... " << std::endl; Symmetric3 S = Symmetric3::RandomPositive(); assert( (S-Symmetric3::SkewSquare(v)).matrix() .isApprox( S.matrix()-vxvx2 ) ); - std::cout << "SmSq = " << (S-Symmetric3::SkewSquare(v)).matrix() << std::endl; - std::cout << "SmSq2 = " << S.matrix()-vxvx2 << std::endl; double m = Eigen::internal::random<double>()+1; assert( (S-m*Symmetric3::SkewSquare(v)).matrix() .isApprox( S.matrix()-m*vxvx2 ) ); @@ -144,6 +158,7 @@ void testSym3() 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) { @@ -152,9 +167,16 @@ void testSym3() timer.toc(std::cout,NBT); } } -/* --- METAPOD ---------------------------------------------- */ + +/* --- 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>); @@ -188,15 +210,27 @@ void testLTI() for(int i=0;i<NBT;++i) Rs[i].randomInit(); + std::cout << "Metapod: "; StackTicToc timer(StackTicToc::US); timer.tic(); SMOOTH(NBT) { timeLTI(S,Rs[_smooth],Sres[_smooth]); } timer.toc(std::cout,NBT); - } +#else // #ifdef WITH_METAPOD + +void testLTI() +{ + std::cout << "Metapod is not installed ... skipping this test. " << std::endl; +} + +#endif // #ifdef WITH_METAPOD + +/* --- EIGEN SYMMETRIC ------------------------------------------------------ */ +/* --- EIGEN SYMMETRIC ------------------------------------------------------ */ +/* --- EIGEN SYMMETRIC ------------------------------------------------------ */ void timeSelfAdj( const Eigen::Matrix3d & A, const Eigen::Matrix3d & Sdense, Eigen::Matrix3d & ASA ) @@ -233,12 +267,19 @@ void testSelfAdj() assert(Masa1.isApprox(Masa2)); } + const int NBT = 100000; + std::vector<Eigen::Matrix3d> Sres (NBT); + std::vector<Eigen::Matrix3d> Rs (NBT); + for(int i=0;i<NBT;++i) + Rs[i] = (Eigen::Quaterniond(Eigen::Matrix<double,4,1>::Random())).normalized().matrix(); + + std::cout << "Eigen: "; StackTicToc timer(StackTicToc::US); timer.tic(); - SMOOTH(1000000) + SMOOTH(NBT) { - timeSelfAdj(A,M,ASA2); + timeSelfAdj(Rs[_smooth],M,Sres[_smooth]); } - timer.toc(std::cout,1000000); + timer.toc(std::cout,NBT); } @@ -248,6 +289,5 @@ int main() testLTI(); testSym3(); - std::cout << std::endl; return 0; } diff --git a/unittest/tspatial.cpp b/unittest/tspatial.cpp index 1a76f19e69c449decdb73e43ff26de6b08833a7e..d868988f77fb2fa21c84061afa3db3ff80baca7f 100644 --- a/unittest/tspatial.cpp +++ b/unittest/tspatial.cpp @@ -109,7 +109,7 @@ bool testMotion() assert(vxv.toVector().tail(3).isMuchSmallerThan(1e-3)); // Simple test for cross product vxf - Force f = bv.toVector(); + 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)); diff --git a/unittest/value.cpp b/unittest/value.cpp index 6856ca18b036e077dc237278f598cacaacfdb6b7..12d9717ff9ea591d37248ac23b95dc189476bd3c 100644 --- a/unittest/value.cpp +++ b/unittest/value.cpp @@ -8,7 +8,7 @@ int main(int argc, const char**argv) { - std::string filename = "/home/nmansard/src/rbdl/rbdl_evaluate_performances/models/simple_humanoid.urdf"; + std::string filename = PINOCCHIO_SOURCE_DIR"/models/simple_humanoid.urdf"; if(argc>1) filename = argv[1]; se3::Model model = se3::urdf::buildModel(filename); diff --git a/unittest/variant.cpp b/unittest/variant.cpp index ee65a314645edafef7a0eb7c5c5d898158291fb3..d75faff06e8bd548b48a54c23f349d841c33d25e 100644 --- a/unittest/variant.cpp +++ b/unittest/variant.cpp @@ -8,9 +8,6 @@ #include "pinocchio/tools/timer.hpp" - - - int main() { using namespace Eigen;