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

[Marginal] Cosmetic changes - fix indentation problems

parent a0f10edc
......@@ -28,8 +28,8 @@ BOOST_AUTO_TEST_SUITE ( CholeskyTest)
BOOST_AUTO_TEST_CASE ( test_cholesky )
{
using namespace Eigen;
using namespace se3;
using namespace Eigen;
using namespace se3;
se3::Model model;
se3::buildModels::humanoidSimple(model,true);
......@@ -47,11 +47,11 @@ BOOST_AUTO_TEST_CASE ( test_cholesky )
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
is_matrix_absolutely_closed(M, U*D.asDiagonal()*U.transpose() , 1e-12);
......@@ -91,7 +91,7 @@ BOOST_AUTO_TEST_CASE ( test_cholesky )
*/
BOOST_AUTO_TEST_CASE ( test_timings )
{
using namespace Eigen;
using namespace Eigen;
using namespace se3;
se3::Model model;
......@@ -106,18 +106,17 @@ BOOST_AUTO_TEST_CASE ( test_timings )
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
bool verbose = flag & (flag-1) ; // True is two or more binaries of the flag are 1.
#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;
if( flag >> 0 & 1 )
......
......@@ -29,22 +29,22 @@ BOOST_AUTO_TEST_CASE ( test_com )
se3::buildModels::humanoidSimple(model);
se3::Data data(model);
VectorXd q = VectorXd::Zero(model.nq);
VectorXd q = VectorXd::Zero(model.nq);
data.M.fill(0); crba(model,data,q);
/* Test COM against CRBA*/
Vector3d com = centerOfMass(model,data,q);
is_matrix_absolutely_closed(data.com[0], getComFromCrba(model,data), 1e-12);
Vector3d com = centerOfMass(model,data,q);
is_matrix_absolutely_closed(data.com[0], getComFromCrba(model,data), 1e-12);
/* Test COM against Jcom (both use different way of compute the COM. */
com = centerOfMass(model,data,q);
jacobianCenterOfMass(model,data,q);
is_matrix_absolutely_closed(com, data.com[0], 1e-12);
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);
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;
......@@ -66,52 +66,52 @@ BOOST_AUTO_TEST_CASE ( test_timings )
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
bool verbose = flag & (flag-1) ; // True is two or more binaries of the flag are 1.
#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);
if( flag >> 0 & 1 )
{
timer.tic();
SMOOTH(NBT)
{
timer.tic();
SMOOTH(NBT)
{
centerOfMass(model,data,q);
}
if(verbose) std::cout << "COM =\t";
timer.toc(std::cout,NBT);
centerOfMass(model,data,q);
}
if(verbose) std::cout << "COM =\t";
timer.toc(std::cout,NBT);
}
if( flag >> 1 & 1 )
{
timer.tic();
SMOOTH(NBT)
{
timer.tic();
SMOOTH(NBT)
{
centerOfMass(model,data,q,false);
}
if(verbose) std::cout << "Without sub-tree =\t";
timer.toc(std::cout,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)
{
timer.tic();
SMOOTH(NBT)
{
jacobianCenterOfMass(model,data,q);
}
if(verbose) std::cout << "Jcom =\t";
timer.toc(std::cout,NBT);
jacobianCenterOfMass(model,data,q);
}
if(verbose) std::cout << "Jcom =\t";
timer.toc(std::cout,NBT);
}
}
BOOST_AUTO_TEST_SUITE_END ()
......@@ -17,7 +17,7 @@ BOOST_AUTO_TEST_SUITE ( Constraint )
BOOST_AUTO_TEST_CASE ( test_ForceSet )
{
using namespace se3;
using namespace se3;
typedef Eigen::Matrix<double,4,4> Matrix4;
typedef SE3::Matrix6 Matrix6;
typedef SE3::Vector3 Vector3;
......@@ -75,8 +75,8 @@ BOOST_AUTO_TEST_CASE ( test_ConstraintRX )
ForceSet F2( Eigen::Matrix<double,3,9>::Random(),Eigen::Matrix<double,3,9>::Random() );
Eigen::MatrixXd StF2 = S.transpose()*F2.block(5,3);
is_matrix_absolutely_closed(StF2,
ConstraintXd(S).matrix().transpose()*F2.matrix().block(0,5,6,3),
1e-12);
ConstraintXd(S).matrix().transpose()*F2.matrix().block(0,5,6,3),
1e-12);
}
BOOST_AUTO_TEST_SUITE_END ()
......
......@@ -29,56 +29,56 @@ BOOST_AUTO_TEST_CASE ( test_crba )
se3::Data data(model);
#ifdef NDEBUG
#ifdef _INTENSE_TESTING_
const int NBT = 1000*1000;
#else
const int NBT = 10;
#endif
#ifdef _INTENSE_TESTING_
const int NBT = 1000*1000;
#else
const int NBT = 10;
#endif
Eigen::VectorXd q = Eigen::VectorXd::Zero(model.nq);
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);
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;
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>();
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;
}
/* 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 << "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);
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
#endif // ifndef NDEBUG
}
......
......@@ -65,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
......@@ -90,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) {}
......@@ -108,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);
}
......@@ -139,7 +139,7 @@ BOOST_AUTO_TEST_SUITE ( FusionTest)
BOOST_AUTO_TEST_CASE ( test_fusion )
{
CRTPDerived d;
CRTPDerived d;
//CRTPBase<CRTPDerived> & dref = d;
CRTPVariant v = d;
......@@ -148,7 +148,7 @@ BOOST_AUTO_TEST_CASE ( test_fusion )
//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));
......
......@@ -66,73 +66,75 @@ BOOST_AUTO_TEST_CASE ( test_timings )
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
#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);
if( flag >> 0 & 1 )
{
timer.tic();
SMOOTH(NBT)
{
timer.tic();
SMOOTH(NBT)
{
computeJacobians(model,data,q);
}
if(verbose) std::cout << "Compute =\t";
timer.toc(std::cout,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)
{
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);
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)
{
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);
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)
{
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);
jacobian(model,data,q,idx);
}
if(verbose) std::cout << "Single jacobian =\t";
timer.toc(std::cout,NBT);
}
}
BOOST_AUTO_TEST_SUITE_END ()
......
......@@ -23,13 +23,13 @@
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
)
{
......
......@@ -34,7 +34,7 @@ BOOST_AUTO_TEST_SUITE ( Rnea )
BOOST_AUTO_TEST_CASE ( test_rnea )
{
#ifdef __SSE3__
_MM_SET_DENORMALS_ZERO_MODE(_MM_DENORMALS_ZERO_ON);
_MM_SET_DENORMALS_ZERO_MODE(_MM_DENORMALS_ZERO_ON);
#endif
using namespace Eigen;
using namespace se3;
......
......@@ -36,8 +36,8 @@ EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Matrix3d)
EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(se3::Symmetric3)
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);
}
......@@ -51,8 +51,8 @@ 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)
const metapod::Spatial::RotationMatrixTpl<double>& R,
metapod::Spatial::ltI<double> & res)
{
res = R.rotTSymmetricMatrix(S);
}
......@@ -60,8 +60,8 @@ void timeLTI(const metapod::Spatial::ltI<double>& S,
#endif
void timeSelfAdj( const Eigen::Matrix3d & A,
const Eigen::Matrix3d & Sdense,
Eigen::Matrix3d & ASA )
const Eigen::Matrix3d & Sdense,
Eigen::Matrix3d & ASA )
{
typedef Eigen::SelfAdjointView<const Eigen::Matrix3d,Eigen::Upper> Sym3;
Sym3 S(Sdense);
......@@ -76,7 +76,7 @@ BOOST_AUTO_TEST_SUITE ( symmetricTest)
/* --- PINOCCHIO ------------------------------------------------------------ */
BOOST_AUTO_TEST_CASE ( test_pinocchio_Sym3 )
{
using namespace se3;
using namespace se3;
typedef Symmetric3::Matrix3 Matrix3;
typedef Symmetric3::Vector3 Vector3;
......@@ -86,13 +86,13 @@ BOOST_AUTO_TEST_CASE ( test_pinocchio_Sym3 )
Matrix3 M = Matrix3::Random(); M = M*M.transpose();
Symmetric3 S(M);
is_matrix_absolutely_closed(S.matrix(), M, 1e-12);
}
}
// S += S
{
Symmetric3
S = Symmetric3::Random(),
S2 = Symmetric3::Random();
S = Symmetric3::Random(),
S2 = Symmetric3::Random();
Symmetric3 Scopy = S;
S+=S2;
is_matrix_absolutely_closed(S.matrix(), S2.matrix()+Scopy.matrix(), 1e-12);
......@@ -120,12 +120,12 @@ BOOST_AUTO_TEST_CASE ( test_pinocchio_Sym3 )
// Random
for(int i=0;i<100;++i )
{
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);
}
{
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
{
......@@ -152,11 +152,11 @@ BOOST_AUTO_TEST_CASE ( test_pinocchio_Sym3 )
Symmetric3 S = Symmetric3::RandomPositive();
is_matrix_absolutely_closed((S-Symmetric3::SkewSquare(v)).matrix(),
S.matrix()-vxvx2, 1e-12);
S.matrix()-vxvx2, 1e-12);
double m = Eigen::internal::random<double>()+1;
is_matrix_absolutely_closed((S-m*Symmetric3::SkewSquare(v)).matrix(),
S.matrix()-m*vxvx2, 1e-12);
S.matrix()-m*vxvx2, 1e-12);
Symmetric3 S2 = S;
......@@ -170,44 +170,44 @@ BOOST_AUTO_TEST_CASE ( test_pinocchio_Sym3 )
// (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) );
Matrix3 M = Matrix3::Random(); M = M*M.transpose();
Symmetric3 S(M);
for(int i=0;i<3;++i)