Commit 1e69fd6d authored by jcarpent's avatar jcarpent
Browse files

[Test] Adjust code to removing of JointModel Lie groups method

parent fb14d535
//
// Copyright (c) 2016 CNRS
// Copyright (c) 2016-2018 CNRS
//
// This file is part of Pinocchio
// Pinocchio is free software: you can redistribute it
......@@ -81,10 +81,11 @@ struct FiniteDiffJoint
{
typedef typename JointModel::ConfigVector_t CV;
typedef typename JointModel::TangentVector_t TV;
typedef typename LieGroup<JointModel>::type LieGroupType;
init(jmodel); jmodel.setIndexes(0,0,0);
typename JointModel::JointDataDerived jdata = jmodel.createData();
CV q = jmodel.random();
CV q; LieGroupType().random(q);
jmodel.calc(jdata,q);
SE3 M_ref(jdata.M);
......@@ -98,7 +99,7 @@ struct FiniteDiffJoint
for(int k=0;k<jmodel.nv();++k)
{
v[k] = eps;
q_int = jmodel.integrate(q,v);
q_int = LieGroupType().integrate(q,v);
jmodel.calc(jdata,q_int);
SE3 M_int = jdata.M;
......@@ -134,12 +135,13 @@ void FiniteDiffJoint::init<JointModelComposite>(JointModelBase<JointModelComposi
jmodel.derived().addJoint(JointModelRZ());
}
//template<>
//void FiniteDiffJoint::operator()< JointModelComposite > (JointModelBase<JointModelComposite> & ) const
//{
template<>
void FiniteDiffJoint::operator()< JointModelComposite > (JointModelBase<JointModelComposite> & ) const
{
// DO NOT CHECK BECAUSE IT IS NOT WORKINK YET - TODO
// typedef typename JointModel::ConfigVector_t CV;
// typedef typename JointModel::TangentVector_t TV;
//
//
// se3::JointModelComposite jmodel((se3::JointModelRX())/*, (se3::JointModelRY())*/);
// jmodel.setIndexes(0,0,0);
//
......@@ -148,11 +150,11 @@ void FiniteDiffJoint::init<JointModelComposite>(JointModelBase<JointModelComposi
// CV q = jmodel.random();
// jmodel.calc(jdata,q);
// SE3 M_ref(jdata.M);
//
//
// CV q_int;
// TV v(Eigen::VectorXd::Random(jmodel.nv())); v.setZero();
// double eps = 1e-4;
//
//
// assert(q.size() == jmodel.nq()&& "nq false");
// assert(v.size() == jmodel.nv()&& "nv false");
// Eigen::MatrixXd S(6,jmodel.nv()), S_ref(ConstraintXd(jdata.S).matrix());
......@@ -164,17 +166,17 @@ void FiniteDiffJoint::init<JointModelComposite>(JointModelBase<JointModelComposi
// q_int = jmodel.integrate(q,v);
// jmodel.calc(jdata,q_int);
// SE3 M_int = jdata.M;
//
//
// S.col(k) = log6(M_ref.inverse()*M_int).toVector();
// S.col(k) /= eps;
//
//
// v[k] = 0.;
// }
//
//
// std::cout << "S\n" << S << std::endl;
// std::cout << "S_ref\n" << S_ref << std::endl;
// // BOOST_CHECK(S.isApprox(S_ref,eps*1e1)); //@TODO Uncomment to test once JointComposite maths are ok
//}
// BOOST_CHECK(S.isApprox(S_ref,eps*1e1)); //@TODO Uncomment to test once JointComposite maths are ok
}
BOOST_AUTO_TEST_SUITE ( BOOST_TEST_MODULE )
......
......@@ -46,10 +46,11 @@ void test_joint_methods(const JointModelBase<JointModel> & jmodel, JointModelCom
typedef typename JointModel::ConfigVector_t ConfigVector_t;
typedef typename JointModel::TangentVector_t TangentVector_t;
typedef typename LieGroup<JointModel>::type LieGroupType;
ConfigVector_t ql(ConfigVector_t::Constant(jmodel.nq(),-M_PI));
ConfigVector_t qu(ConfigVector_t::Constant(jmodel.nq(),M_PI));
ConfigVector_t q = jmodel.randomConfiguration(ql,qu);
ConfigVector_t q = LieGroupType().randomConfiguration(ql,qu);
jmodel.calc(jdata,q);
......@@ -59,7 +60,7 @@ void test_joint_methods(const JointModelBase<JointModel> & jmodel, JointModelCom
jmodel_composite.calc(jdata_composite,q);
BOOST_CHECK(jdata_composite.M.isApprox((SE3)jdata.M));
q = jmodel.randomConfiguration(ql,qu);
q = LieGroupType().randomConfiguration(ql,qu);
TangentVector_t v = TangentVector_t::Random(jmodel.nv());
jmodel.calc(jdata,q,v);
jmodel_composite.calc(jdata_composite,q,v);
......@@ -68,21 +69,6 @@ void test_joint_methods(const JointModelBase<JointModel> & jmodel, JointModelCom
BOOST_CHECK(jdata_composite.v.isApprox((Motion)jdata.v));
BOOST_CHECK(jdata_composite.c.isApprox((Motion)jdata.c));
{
VectorXd q1(jmodel.random());
jmodel.normalize(q1);
VectorXd q2(jmodel.random());
jmodel.normalize(q2);
VectorXd v(VectorXd::Random(jmodel.nv()));
BOOST_CHECK(jmodel_composite.integrate(q1,v).isApprox(jmodel.integrate(q1,v)));
BOOST_CHECK(jmodel_composite.difference(q1,q2).isApprox(jmodel.difference(q1,q2)));
const double alpha = 0.2;
BOOST_CHECK(jmodel_composite.interpolate(q1,q2,alpha).isApprox(jmodel.interpolate(q1,q2,alpha)));
BOOST_CHECK(std::fabs(jmodel_composite.distance(q1,q2)-jmodel.distance(q1,q2))<= NumTraits<double>::dummy_precision());
}
Inertia::Matrix6 I(Inertia::Random().matrix());
jmodel.calc_aba(jdata,I,false);
jmodel_composite.calc_aba(jdata_composite,I,false);
......@@ -110,16 +96,15 @@ struct TestJointComposite{
test_joint_methods(jmodel);
}
void operator()(const JointModelBase<JointModelComposite> &) const
{
JointModelComposite jmodel_composite;
jmodel_composite.addJoint(se3::JointModelRX());
jmodel_composite.addJoint(se3::JointModelRY());
jmodel_composite.setIndexes(0,0,0);
test_joint_methods(jmodel_composite);
}
// void operator()(const JointModelBase<JointModelComposite> &) const
// {
// JointModelComposite jmodel_composite;
// jmodel_composite.addJoint(se3::JointModelRX());
// jmodel_composite.addJoint(se3::JointModelRY());
// jmodel_composite.setIndexes(0,0,0);
//
// test_joint_methods(jmodel_composite);
// }
void operator()(const JointModelBase<JointModelRevoluteUnaligned> &) const
{
......@@ -143,7 +128,17 @@ BOOST_AUTO_TEST_SUITE ( BOOST_TEST_MODULE )
BOOST_AUTO_TEST_CASE(test_basic)
{
boost::mpl::for_each<JointModelVariant::types>(TestJointComposite());
typedef boost::variant< JointModelRX, JointModelRY, JointModelRZ, JointModelRevoluteUnaligned
, JointModelSpherical, JointModelSphericalZYX
, JointModelPX, JointModelPY, JointModelPZ
, JointModelPrismaticUnaligned
, JointModelFreeFlyer
, JointModelPlanar
, JointModelTranslation
, JointModelRUBX, JointModelRUBY, JointModelRUBZ
> Variant;
boost::mpl::for_each<Variant::types>(TestJointComposite());
}
BOOST_AUTO_TEST_CASE(test_equivalent)
......
......@@ -71,102 +71,6 @@ void buildModel(Model & model)
BOOST_AUTO_TEST_SUITE ( BOOST_TEST_MODULE )
struct TestIntegrationJoint
{
template<typename JointModel>
static void init (JointModelBase<JointModel> & /*jmodel*/) {}
template<typename JointModel>
void operator()(JointModelBase<JointModel> & jmodel)
{
init(jmodel);
typedef typename JointModel::ConfigVector_t CV;
typedef typename JointModel::TangentVector_t TV;
typedef typename JointModel::Transformation_t SE3;
jmodel.setIndexes(0,0,0);
typename JointModel::JointDataDerived jdata = jmodel.createData();
CV q0 = jmodel.random();
TV qdot(TV::Random());
jmodel.calc(jdata,q0,qdot);
SE3 M0 = jdata.M;
Motion v0 = jdata.v;
CV q1 = jmodel.integrate(q0,qdot);
jmodel.calc(jdata,q1);
SE3 M1 = jdata.M;
SE3 M1_exp = M0*exp6(v0);
BOOST_CHECK_MESSAGE(M1.isApprox(M1_exp), std::string("Error when integrating1 " + jmodel.shortname()));
qdot *= -1;
jmodel.calc(jdata,q0,qdot);
M0 = jdata.M;
v0 = jdata.v;
q1 = jmodel.integrate(q0,qdot);
jmodel.calc(jdata,q1);
M1 = jdata.M;
M1_exp = M0*exp6(v0);
BOOST_CHECK_MESSAGE(M1.isApprox(M1_exp), std::string("Error when integrating2 " + jmodel.shortname()));
}
};
template<>
void TestIntegrationJoint::operator()< JointModelSphericalZYX >(JointModelBase< JointModelSphericalZYX > & /*jmodel*/) {}
template<>
void TestIntegrationJoint::operator()< JointModelComposite >(JointModelBase< JointModelComposite > & /*jmodel*/)
{
se3::JointModelComposite jmodel((se3::JointModelRX()));
jmodel.addJoint(se3::JointModelRY());
jmodel.setIndexes(1,0,0);
se3::JointModelComposite::JointDataDerived jdata = jmodel.createData();
typedef JointModel::ConfigVector_t CV;
typedef JointModel::TangentVector_t TV;
typedef JointModel::Transformation_t SE3;
CV q0 = jmodel.random();
TV qdot(Eigen::VectorXd::Random(jmodel.nv()));
jmodel.calc(jdata,q0,qdot);
SE3 M0 = jdata.M;
Motion v0 = jdata.v;
CV q1 = jmodel.integrate(q0,qdot);
jmodel.calc(jdata,q1);
SE3 M1 = jdata.M;
// SE3 M1_exp = M0*exp6(v0);
// The computations in JointModelComposite::calc() may be wrong, this results cannot be tested yet.
// BOOST_CHECK_MESSAGE(M1.isApprox(M1_exp), std::string("Error when integrating " + jmodel.shortname()));
}
template<>
void TestIntegrationJoint::init<JointModelRevoluteUnaligned>(JointModelBase<JointModelRevoluteUnaligned> & jmodel)
{
jmodel.derived().axis.setRandom(); jmodel.derived().axis.normalize();
}
template<>
void TestIntegrationJoint::init<JointModelPrismaticUnaligned>(JointModelBase<JointModelPrismaticUnaligned> & jmodel)
{
jmodel.derived().axis.setRandom(); jmodel.derived().axis.normalize();
}
BOOST_AUTO_TEST_CASE (intergration_test_joint)
{
boost::mpl::for_each<JointModelVariant::types>(TestIntegrationJoint());
}
BOOST_AUTO_TEST_CASE ( integration_test )
{
Model model; buildModel(model);
......@@ -189,55 +93,6 @@ BOOST_AUTO_TEST_CASE ( integration_test )
BOOST_CHECK_MESSAGE(results[0].isApprox(qs[0], 1e-12), "integration of full body with zero velocity - wrong results");
}
struct TestDifferentiationJoint
{
template<typename JointModel>
static void init (JointModelBase<JointModel> & /*jmodel*/) {}
template<typename JointModel>
void operator()(JointModelBase<JointModel> & jmodel)
{
init(jmodel);
typedef typename JointModel::ConfigVector_t CV;
typedef typename JointModel::TangentVector_t TV;
jmodel.setIndexes(0,0,0);
CV q0 = jmodel.random();
CV q1 = jmodel.random();
TV qdot = jmodel.difference(q0,q1);
BOOST_CHECK_MESSAGE( jmodel.isSameConfiguration(jmodel.integrate(q0, qdot), q1), std::string("Error in difference for joint " + jmodel.shortname()));
BOOST_CHECK_MESSAGE( jmodel.isSameConfiguration(jmodel.integrate(q1, -qdot), q0), std::string("Error in difference for joint " + jmodel.shortname()));
}
};
template<>
void TestDifferentiationJoint::operator()< JointModelSphericalZYX >(JointModelBase< JointModelSphericalZYX > & /*jmodel*/) {}
template<>
void TestDifferentiationJoint::init<JointModelRevoluteUnaligned>(JointModelBase<JointModelRevoluteUnaligned> & jmodel)
{
jmodel.derived().axis.setRandom(); jmodel.derived().axis.normalize();
}
template<>
void TestDifferentiationJoint::init<JointModelPrismaticUnaligned>(JointModelBase<JointModelPrismaticUnaligned> & jmodel)
{
jmodel.derived().axis.setRandom(); jmodel.derived().axis.normalize();
}
BOOST_AUTO_TEST_CASE (differentiation_test_joint)
{
boost::mpl::for_each<JointModelVariant::types>(TestDifferentiationJoint());
}
BOOST_AUTO_TEST_CASE ( integrate_difference_test )
{
Model model; buildModel(model);
......@@ -252,64 +107,6 @@ BOOST_AUTO_TEST_CASE ( integrate_difference_test )
}
struct TestInterpolationJoint
{
template<typename JointModel>
static void init (JointModelBase<JointModel> & /*jmodel*/) {}
template<typename JointModel>
void operator()(JointModelBase<JointModel> & jmodel)
{
init(jmodel);
typedef typename JointModel::ConfigVector_t CV;
jmodel.setIndexes(0,0,0);
CV q0 = jmodel.random();
CV q1 = jmodel.random();
double u = 0;
BOOST_CHECK_MESSAGE(jmodel.isSameConfiguration(jmodel.interpolate(q0, q1,u),q0)
, std::string("Error in interpolation with u = 0 for joint " + jmodel.shortname()));
u = 0.3;
BOOST_CHECK_MESSAGE(jmodel.isSameConfiguration(jmodel.interpolate(jmodel.interpolate(q0, q1,u), q1, 1),q1)
, std::string("Error in double interpolation for joint " + jmodel.shortname()));
u = 1;
BOOST_CHECK_MESSAGE(jmodel.isSameConfiguration(jmodel.interpolate(q0, q1,u),q1)
, std::string("Error in interpolation with u = 1 for joint " + jmodel.shortname()));
}
};
template<>
void TestInterpolationJoint::operator()< JointModelSphericalZYX >(JointModelBase< JointModelSphericalZYX > & /*jmodel*/) {}
template<>
void TestInterpolationJoint::init<JointModelRevoluteUnaligned>(JointModelBase<JointModelRevoluteUnaligned> & jmodel)
{
jmodel.derived().axis.setRandom(); jmodel.derived().axis.normalize();
}
template<>
void TestInterpolationJoint::init<JointModelPrismaticUnaligned>(JointModelBase<JointModelPrismaticUnaligned> & jmodel)
{
jmodel.derived().axis.setRandom(); jmodel.derived().axis.normalize();
}
BOOST_AUTO_TEST_CASE (interpolation_test_joint)
{
boost::mpl::for_each<JointModelVariant::types>(TestInterpolationJoint());
}
BOOST_AUTO_TEST_CASE ( neutral_configuration_test )
{
Model model; buildModel(model);
......@@ -356,7 +153,6 @@ BOOST_AUTO_TEST_CASE ( uniform_sampling_test )
}
}
BOOST_AUTO_TEST_CASE ( normalize_test )
{
Model model; buildModel(model);
......
//
// Copyright (c) 2016 CNRS
// Copyright (c) 2016-2018 CNRS
//
// This file is part of Pinocchio
// Pinocchio is free software: you can redistribute it
......@@ -18,6 +18,8 @@
#include "pinocchio/multibody/joint/joint-composite.hpp"
#include "pinocchio/multibody/joint/joint.hpp"
#include "pinocchio/multibody/liegroup/liegroup.hpp"
#include <boost/test/unit_test.hpp>
#include <boost/utility/binary.hpp>
......@@ -26,55 +28,44 @@ using namespace se3;
template <typename JointModel>
void test_joint_methods (JointModel & jmodel, typename JointModel::JointDataDerived & jdata)
{
std::cout << "Testing Joint over " << jmodel.shortname() << std::endl;
Eigen::VectorXd q1(Eigen::VectorXd::Random (jmodel.nq()));
Eigen::VectorXd q1_dot(Eigen::VectorXd::Random (jmodel.nv()));
Eigen::VectorXd q2(Eigen::VectorXd::Random (jmodel.nq()));
double u = 0.3;
se3::Inertia::Matrix6 Ia(se3::Inertia::Random().matrix());
bool update_I = false;
q1 = jmodel.random();
q2 = jmodel.random();
jmodel.calc(jdata, q1, q1_dot);
jmodel.calc_aba(jdata, Ia, update_I);
se3::JointModel jma(jmodel);
se3::JointData jda(jdata);
jma.calc(jda, q1, q1_dot);
jma.calc_aba(jda, Ia, update_I);
std::string error_prefix("JointModel on " + jma.shortname());
BOOST_CHECK_MESSAGE(jmodel.nq() == jma.nq() ,std::string(error_prefix + " - nq "));
BOOST_CHECK_MESSAGE(jmodel.nv() == jma.nv() ,std::string(error_prefix + " - nv "));
BOOST_CHECK_MESSAGE(jmodel.idx_q() == jma.idx_q() ,std::string(error_prefix + " - Idx_q "));
BOOST_CHECK_MESSAGE(jmodel.idx_v() == jma.idx_v() ,std::string(error_prefix + " - Idx_v "));
BOOST_CHECK_MESSAGE(jmodel.id() == jma.id() ,std::string(error_prefix + " - JointId "));
BOOST_CHECK_MESSAGE(jmodel.integrate(q1,q1_dot).isApprox(jma.integrate(q1,q1_dot)) ,std::string(error_prefix + " - integrate "));
BOOST_CHECK_MESSAGE(jmodel.interpolate(q1,q2,u).isApprox(jma.interpolate(q1,q2,u)) ,std::string(error_prefix + " - interpolate "));
BOOST_CHECK_MESSAGE(jmodel.randomConfiguration( -1 * Eigen::VectorXd::Ones(jmodel.nq()),
Eigen::VectorXd::Ones(jmodel.nq())).size()
== jma.randomConfiguration(-1 * Eigen::VectorXd::Ones(jma.nq()),
Eigen::VectorXd::Ones(jma.nq())).size()
,std::string(error_prefix + " - RandomConfiguration dimensions "));
BOOST_CHECK_MESSAGE(jmodel.difference(q1,q2).isApprox(jma.difference(q1,q2)) ,std::string(error_prefix + " - difference "));
BOOST_CHECK_MESSAGE(fabs(jmodel.distance(q1,q2) - jma.distance(q1,q2)) < 1e-12 ,std::string(error_prefix + " - distance "));
BOOST_CHECK_MESSAGE(jda.S().matrix().isApprox(jdata.S.matrix()),std::string(error_prefix + " - ConstraintXd "));
BOOST_CHECK_MESSAGE( (jda.M()).isApprox((jdata.M)),std::string(error_prefix + " - Joint transforms ")); // == or isApprox ?
BOOST_CHECK_MESSAGE( (jda.v()).isApprox( (se3::Motion(jdata.v))),std::string(error_prefix + " - Joint motions "));
BOOST_CHECK_MESSAGE((jda.c()) == (jdata.c),std::string(error_prefix + " - Joint bias "));
BOOST_CHECK_MESSAGE((jda.U()).isApprox(jdata.U),std::string(error_prefix + " - Joint U inertia matrix decomposition "));
BOOST_CHECK_MESSAGE((jda.Dinv()).isApprox(jdata.Dinv),std::string(error_prefix + " - Joint DInv inertia matrix decomposition "));
BOOST_CHECK_MESSAGE((jda.UDinv()).isApprox(jdata.UDinv),std::string(error_prefix + " - Joint UDInv inertia matrix decomposition "));
typedef typename LieGroup<JointModel>::type LieGroupType;
std::cout << "Testing Joint over " << jmodel.shortname() << std::endl;
Eigen::VectorXd q1(Eigen::VectorXd::Random (jmodel.nq()));
Eigen::VectorXd q1_dot(Eigen::VectorXd::Random (jmodel.nv()));
Eigen::VectorXd q2(Eigen::VectorXd::Random (jmodel.nq()));
double u = 0.3;
se3::Inertia::Matrix6 Ia(se3::Inertia::Random().matrix());
bool update_I = false;
q1 = LieGroupType().random();
q2 = LieGroupType().random();
jmodel.calc(jdata, q1, q1_dot);
jmodel.calc_aba(jdata, Ia, update_I);
se3::JointModel jma(jmodel);
se3::JointData jda(jdata);
jma.calc(jda, q1, q1_dot);
jma.calc_aba(jda, Ia, update_I);
std::string error_prefix("JointModel on " + jma.shortname());
BOOST_CHECK_MESSAGE(jmodel.nq() == jma.nq() ,std::string(error_prefix + " - nq "));
BOOST_CHECK_MESSAGE(jmodel.nv() == jma.nv() ,std::string(error_prefix + " - nv "));
BOOST_CHECK_MESSAGE(jmodel.idx_q() == jma.idx_q() ,std::string(error_prefix + " - Idx_q "));
BOOST_CHECK_MESSAGE(jmodel.idx_v() == jma.idx_v() ,std::string(error_prefix + " - Idx_v "));
BOOST_CHECK_MESSAGE(jmodel.id() == jma.id() ,std::string(error_prefix + " - JointId "));
BOOST_CHECK_MESSAGE(jda.S().matrix().isApprox(jdata.S.matrix()),std::string(error_prefix + " - ConstraintXd "));
BOOST_CHECK_MESSAGE( (jda.M()).isApprox((jdata.M)),std::string(error_prefix + " - Joint transforms ")); // == or isApprox ?
BOOST_CHECK_MESSAGE( (jda.v()).isApprox( (se3::Motion(jdata.v))),std::string(error_prefix + " - Joint motions "));
BOOST_CHECK_MESSAGE((jda.c()) == (jdata.c),std::string(error_prefix + " - Joint bias "));
BOOST_CHECK_MESSAGE((jda.U()).isApprox(jdata.U),std::string(error_prefix + " - Joint U inertia matrix decomposition "));
BOOST_CHECK_MESSAGE((jda.Dinv()).isApprox(jdata.Dinv),std::string(error_prefix + " - Joint DInv inertia matrix decomposition "));
BOOST_CHECK_MESSAGE((jda.UDinv()).isApprox(jdata.UDinv),std::string(error_prefix + " - Joint UDInv inertia matrix decomposition "));
// Test vxS
typedef typename JointModel::Constraint_t Constraint_t;
......@@ -121,7 +112,8 @@ struct TestJoint{
se3::JointModelComposite::JointDataDerived jdata = jmodel.createData();
test_joint_methods(jmodel, jdata);
// TODO: fixme when LieGroups will be implemented for JointModelComposite
// test_joint_methods(jmodel, jdata);
}
void operator()(const se3::JointModelRevoluteUnaligned & ) const
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment