Skip to content
Snippets Groups Projects
Commit 2f34210b authored by Nicolas Mansard's avatar Nicolas Mansard Committed by Valenza Florian
Browse files

Clean unittest.

parent 9c59ddc7
No related branches found
No related tags found
No related merge requests found
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)
#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;
}
......@@ -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;
}
......@@ -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;
......
#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;
}
#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;
}
......@@ -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;
}
#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;
}
......@@ -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));
......
......@@ -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);
......
......@@ -8,9 +8,6 @@
#include "pinocchio/tools/timer.hpp"
int main()
{
using namespace Eigen;
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment