Commit 24409b42 authored by Valenza Florian's avatar Valenza Florian
Browse files

[C++][unittest] Fix computations in jointcomposite::calc() + reworked unittest...

[C++][unittest] Fix computations in jointcomposite::calc() + reworked unittest (comparison of R3xSO3 (either two joints with no inertia between them or Jointcomposite))
parent 2b24ae72
......@@ -3,7 +3,7 @@
//
// This file is part of Pinocchio
// Pinocchio is free software: you can redistribute it
// and/or modify it under the terms of the GNU Lesser General Public
// and/or modify it under the terljMj of the GNU Lesser General Public
// License as published by the Free Software Foundation, either version
// 3 of the License, or (at your option) any later version.
//
......@@ -22,6 +22,9 @@
// #include "pinocchio/multibody/joint/joint-basic-visitors.hpp"
#include "pinocchio/multibody/joint/joint.hpp"
EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(se3::SE3)
EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(se3::Motion)
namespace se3
{
......@@ -66,6 +69,7 @@ namespace se3
int nq_composite,nv_composite;
Constraint_t S;
std::vector<Transformation_t> ljMj;
Transformation_t M;
Motion_t v;
Bias_t c;
......@@ -83,6 +87,7 @@ namespace se3
, nq_composite(nq)
, nv_composite(nv)
, S(Eigen::MatrixXd::Zero(6, nv_composite))
, ljMj(joints.size())
, M(Transformation_t::Identity())
, v(Motion_t::Zero())
, c(Bias_t::Zero())
......@@ -195,7 +200,8 @@ namespace se3
{
JointDataVector::iterator::difference_type index = i - data.joints.begin();
calc_zero_order(joints[(std::size_t)index], *i, qs);
data.M = data.M * joint_transform(*i);
data.ljMj[(std::size_t)index] = joint_transform(*i);
data.M = data.M * data.ljMj[(std::size_t)index];
}
}
......@@ -207,20 +213,22 @@ namespace se3
data.v.setZero();
data.c.setZero();
data.S.matrix().setZero();
std::cout << "Size of joint stack: " << data.joints.size() << std::endl;
for (JointDataVector::iterator i = data.joints.begin(); i != data.joints.end(); ++i)
{
JointDataVector::iterator::difference_type index = i - data.joints.begin();
std::cout << "i = " << index << std::endl;
std::cout << "Index in joint composite : \t" << index << std::endl;
std::cout << "nq of contained joint : \t" << ::se3::nq(joints[(std::size_t)index]) << std::endl;
calc_first_order(joints[(std::size_t)index], *i, qs, vs);
data.M = data.M * joint_transform(*i);
data.ljMj[(std::size_t)index] = joint_transform(*i);
data.M = data.M * data.ljMj[(std::size_t)index];
data.v = motion(*i);
if (i == data.joints.begin())
{}
else
{
data.v += data.ljMj[(std::size_t)index].actInv(motion(*(i-1)));
}
}
data.v = motion(data.joints[joints.size()-1]);
data.c = bias(data.joints[joints.size()-1]);
// data.S = constraint_xd(data.joints[joints.size()-1])
int start_col = nv_composite;
int sub_constraint_dimension = (int)constraint_xd(data.joints[joints.size()-1]).matrix().cols();
start_col -= sub_constraint_dimension;
......@@ -232,9 +240,8 @@ namespace se3
sub_constraint_dimension = (int)constraint_xd(*i).matrix().cols();
start_col -= sub_constraint_dimension;
iMcomposite = joint_transform(*(i-1)) * iMcomposite;
data.v += iMcomposite.actInv(motion(*i));
data.S.matrix().middleCols(start_col,sub_constraint_dimension) = iMcomposite.actInv(constraint_xd(*i));
iMcomposite = joint_transform(*(i+0)) * iMcomposite;
data.S.matrix().middleCols(start_col,sub_constraint_dimension) = iMcomposite.actInv(constraint_xd(*(i+0)));
Motion acceleration_d_entrainement_coriolis = Motion::Zero(); // TODO: compute
data.c += iMcomposite.actInv(bias(*i)) + acceleration_d_entrainement_coriolis;
......@@ -251,7 +258,7 @@ namespace se3
for (JointDataVector::iterator i = data.joints.begin(); i != data.joints.end(); ++i)
{
JointDataVector::iterator::difference_type index = i - data.joints.begin();
::se3::calc_aba(joints[(std::size_t)index], *i, I, false); // HERE MEMORY ACCES VIOLATION WHEN VISITING A JOINT(JOINTMODELCOMPOSITE). SEE UNITTEST JOINT
::se3::calc_aba(joints[(std::size_t)index], *i, I, false);
}
data.U = I * data.S;
Eigen::MatrixXd tmp = data.S.matrix().transpose() * I * data.S.matrix();
......@@ -262,6 +269,12 @@ namespace se3
I -= data.UDinv * data.U.transpose();
}
Scalar finiteDifferenceIncrement() const
{
using std::sqrt;
return 2.*sqrt(sqrt(Eigen::NumTraits<Scalar>::epsilon()));
}
ConfigVector_t integrate_impl(const Eigen::VectorXd & qs,const Eigen::VectorXd & vs) const
{
ConfigVector_t result(Eigen::VectorXd::Zero(nq_composite));
......
......@@ -34,10 +34,8 @@
#include <iostream>
#include <Eigen/Cholesky>
EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(se3::SE3)
EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(se3::Inertia)
EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(se3::Force)
EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(se3::Motion)
EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Matrix<double,6,Eigen::Dynamic>)
EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Matrix<double,6,6>)
EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(se3::SE3::Vector3)
......
......@@ -129,6 +129,49 @@ void FiniteDiffJoint::init<JointModelPrismaticUnaligned>(JointModelBase<JointMod
template<>
void FiniteDiffJoint::operator()< JointModelDense<-1,-1> > (JointModelBase< JointModelDense<-1,-1> > & /*jmodel*/) const {}
template<>
void FiniteDiffJoint::operator()< JointModelComposite > (JointModelBase<JointModelComposite> & ) const
{
typedef typename JointModel::ConfigVector_t CV;
typedef typename JointModel::TangentVector_t TV;
se3::JointModelComposite jmodel((se3::JointModelRX())/*, (se3::JointModelRY())*/);
jmodel.setIndexes(0,0,0);
jmodel.updateComponentsIndexes();
se3::JointModelComposite::JointDataDerived jdata = jmodel.createData();
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());
eps = jmodel.finiteDifferenceIncrement();
for(int k=0;k<jmodel.nv();++k)
{
v[k] = eps;
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));
}
BOOST_AUTO_TEST_SUITE(FiniteDifferences)
BOOST_AUTO_TEST_CASE(increment)
......
......@@ -23,8 +23,10 @@
#include "pinocchio/algorithm/aba.hpp"
#include "pinocchio/algorithm/joint-configuration.hpp"
#include "pinocchio/algorithm/compute-all-terms.hpp"
#include "pinocchio/algorithm/jacobian.hpp"
#include <iostream>
#include <cmath>
#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE JointCompositeTest
......@@ -182,7 +184,6 @@ BOOST_AUTO_TEST_CASE ( test_recursive_variant)
BOOST_AUTO_TEST_CASE (TestCopyComposite)
{
std::cout << "\n\n --- Test Copy composite" << std::endl;
using namespace Eigen;
using namespace se3;
......@@ -201,7 +202,6 @@ BOOST_AUTO_TEST_CASE (TestCopyComposite)
JointModelComposite model_copy = jmodel_composite_planar;
JointDataComposite data_copy = model_copy.createData();
std::cout << model_copy << std::endl;
BOOST_CHECK_MESSAGE( model_copy.max_joints == jmodel_composite_planar.max_joints, "Test Copy Composite, max_joints are differents");
BOOST_CHECK_MESSAGE( model_copy.nq() == jmodel_composite_planar.nq(), "Test Copy Composite, nq are differents");
......@@ -215,143 +215,112 @@ BOOST_AUTO_TEST_CASE (TestCopyComposite)
}
BOOST_AUTO_TEST_CASE (TestVariantOverComposite)
BOOST_AUTO_TEST_CASE ( test_R3xSO3)
{
std::cout << "\n\n --- Test Variant Over composite" << std::endl;
std::cout << " Testing R3xSO3 vs jointcomposite<R3 - SO3>" << std::endl;
using namespace Eigen;
using namespace se3;
JointModelComposite jmodel_composite_planar(3);
jmodel_composite_planar.addJointModel(JointModelPX());
jmodel_composite_planar.addJointModel(JointModelPY());
jmodel_composite_planar.addJointModel(JointModelRZ());
jmodel_composite_planar.setIndexes(1,0,0);
jmodel_composite_planar.updateComponentsIndexes();
JointDataComposite jdata_composite_planar = jmodel_composite_planar.createData();
Eigen::VectorXd q1(Eigen::VectorXd::Random(3));
Eigen::VectorXd q1_dot(Eigen::VectorXd::Random(3));
JointModelVariant jmvariant_comp(jmodel_composite_planar);
JointDataVariant jdvariant_comp(jdata_composite_planar);
std::cout << " Extract the composite joint from the variant, and visit each joint from the stack" << std::endl;
JointModelComposite extracted_model = boost::get<JointModelComposite>(jmvariant_comp);
for (std::size_t i = 0; i < 3; ++i)
{
calc_first_order(extracted_model.joints[i], jdata_composite_planar.joints[i], q1, q1_dot);
std::cout << se3::nq(extracted_model.joints[i]) << std::endl;
}
std::cout << " Testing visiting a variant over the composite joint" << std::endl;
std::cout << nv(jmvariant_comp) << std::endl;
calc_first_order(jmvariant_comp, jdvariant_comp, q1, q1_dot); // here assertion 'false' failed has_fallback_type_
}
Model model_composite;
Model model_zero_mass;
Model model_ff;
// Compare a stack of joint ( PX, PY, RZ) to a planar joint
BOOST_AUTO_TEST_CASE ( KinematicModelCompositePlanar)
{
std::cout << " Testing Planar Model vs composite planar model" << std::endl;
using namespace Eigen;
using namespace se3;
Model model_composite_planar;
Model model_planar;
Inertia body_inertia(Inertia::Random());
SE3 placement(SE3::Identity());
model_planar.addJoint(model_planar.getBodyId("universe"),JointModelPlanar(), placement, "planar_joint");
model_planar.appendBodyToJoint(model_planar.getJointId("planar_joint"), body_inertia, SE3::Identity(), "planar_body");
model_zero_mass.addJoint(model_zero_mass.getBodyId("universe"),JointModelTranslation(), placement, "R3_joint");
model_zero_mass.addJoint(model_zero_mass.getJointId("R3_joint"), JointModelSpherical(), SE3::Identity(), "SO3_joint");
model_zero_mass.appendBodyToJoint(model_zero_mass.getJointId("SO3_joint"), body_inertia, SE3::Identity(), "SO3_body");
JointModelComposite jmodel_composite_planar(3);
jmodel_composite_planar.addJointModel(JointModelPX());
jmodel_composite_planar.addJointModel(JointModelPY());
jmodel_composite_planar.addJointModel(JointModelRZ());
JointModelComposite jmodel_composite(2);
jmodel_composite.addJointModel(JointModelTranslation());
jmodel_composite.addJointModel(JointModelSpherical());
model_composite_planar.addJoint(model_composite_planar.getBodyId("universe"),jmodel_composite_planar, placement, "composite_planar_joint");
model_composite_planar.appendBodyToJoint(model_composite_planar.getJointId("composite_planar_joint"), body_inertia, SE3::Identity(), "composite_planar_body");
model_composite.addJoint(model_composite.getBodyId("universe"),jmodel_composite, placement, "composite_R3xSO3_joint");
model_composite.appendBodyToJoint(model_composite.getJointId("composite_R3xSO3_joint"), body_inertia, SE3::Identity(), "composite_R3xSO3_body");
// When Model will be cleaned in coming pull request, this will be done in addBody(addJoint)
boost::get<JointModelComposite>(model_composite_planar.joints[model_composite_planar.getJointId("composite_planar_joint")]).updateComponentsIndexes();
boost::get<JointModelComposite>(model_composite.joints[model_composite.getJointId("composite_R3xSO3_joint")]).updateComponentsIndexes();
model_ff.addJoint(model_ff.getBodyId("universe"),JointModelFreeFlyer(), placement, "ff_joint");
model_ff.appendBodyToJoint(model_ff.getJointId("ff_joint"), body_inertia, SE3::Identity(), "ff_body");
BOOST_CHECK_MESSAGE(model_composite.nq == model_zero_mass.nq ,"Model with R3 - SO3 vs composite <R3xSO3> - dimensions nq are not equal");
BOOST_CHECK_MESSAGE(model_composite.nq == model_zero_mass.nq ,"Model with R3 - SO3 vs composite <R3xSO3> - dimensions nv are not equal");
BOOST_CHECK_MESSAGE(model_composite_planar.nq == model_planar.nq ,"Model with planar joint vs composite PxPyRz - dimensions nq are not equal");
BOOST_CHECK_MESSAGE(model_composite_planar.nq == model_planar.nq ,"Model with planar joint vs composite PxPyRz - dimensions nv are not equal");
Data data_zero_mass(model_zero_mass);
Data data_composite(model_composite);
Data data_ff(model_ff);
// Data data_planar(model_planar);
// Data data_composite_planar(model_composite_planar);
Eigen::VectorXd q(Eigen::VectorXd::Random(model_zero_mass.nq));normalize(model_zero_mass,q);
Eigen::VectorXd q_dot(Eigen::VectorXd::Random(model_zero_mass.nv));
Eigen::VectorXd q_ddot(Eigen::VectorXd::Random(model_zero_mass.nv));
Eigen::VectorXd q1(Eigen::VectorXd::Random(model_zero_mass.nq));normalize(model_zero_mass,q1);
Eigen::VectorXd q2(Eigen::VectorXd::Random(model_zero_mass.nq));normalize(model_zero_mass,q2);
Eigen::VectorXd tau(Eigen::VectorXd::Random(model_zero_mass.nq));
double u = 0.3;
// Test that algorithms do not crash
integrate(model_composite,q,q_dot);
interpolate(model_composite,q1,q2,u);
differentiate(model_composite,q1,q2);
distance(model_composite,q1,q2);
// Eigen::VectorXd q(Eigen::VectorXd::Random(model_planar.nq));
// Eigen::VectorXd q_dot(Eigen::VectorXd::Random(model_planar.nv));
// Eigen::VectorXd q_ddot(Eigen::VectorXd::Random(model_planar.nv));
// Eigen::VectorXd q1(Eigen::VectorXd::Random(model_planar.nq));
// Eigen::VectorXd q2(Eigen::VectorXd::Random(model_planar.nq));
// Eigen::VectorXd tau(Eigen::VectorXd::Random(model_planar.nq));
// double u = 0.3;
aba(model_composite,data_composite, q,q_dot, tau);
centerOfMass(model_composite, data_composite,q,q_dot,q_ddot,true,false);
forwardKinematics(model_composite,data_composite, q, q_dot, q_ddot);
computeAllTerms(model_zero_mass,data_zero_mass,q,q_dot);
// // Test that algorithms do not crash
// integrate(model_composite_planar,q,q_dot);
// interpolate(model_composite_planar,q1,q2,u);
// differentiate(model_composite_planar,q1,q2);
// distance(model_composite_planar,q1,q2);
// randomConfiguration(model_composite_planar);
forwardKinematics(model_zero_mass, data_zero_mass, q, q_dot, q_ddot);
computeAllTerms(model_composite,data_composite,q,q_dot);
// // aba(model_composite_planar,data_composite_planar, q,q_dot, tau);
// centerOfMass(model_composite_planar, data_composite_planar,q,q_dot,q_ddot,true,false);
// emptyForwardPass(model_composite_planar, data_composite_planar);
// forwardKinematics(model_composite_planar,data_composite_planar, q );
// forwardKinematics(model_composite_planar,data_composite_planar, q, q_dot);
// forwardKinematics(model_composite_planar,data_composite_planar, q, q_dot, q_ddot);
// computeAllTerms(model_planar,data_planar,q,q_dot);
// computeAllTerms(model_composite_planar,data_composite_planar,q,q_dot);
// Model::Index last_joint_pxpyrz = (Model::Index) model_planar.nbody-1;
// Model::Index last_joint_composite = (Model::Index) model_composite_planar.nbody-1;
Model::Index index_joint_R3xSO3 = (Model::Index) model_zero_mass.njoint-1;
Model::Index index_joint_composite = (Model::Index) model_composite.njoint-1;
// BOOST_CHECK_MESSAGE(data_composite_planar.oMi[last_joint_composite]
// .isApprox(data_planar.oMi[last_joint_pxpyrz]) , "composite planar joint vs PxPyRz - oMi last joint not equal");
// BOOST_CHECK_MESSAGE(data_composite_planar.v[last_joint_composite]
// == data_planar.v[last_joint_pxpyrz] , "composite planar joint vs PxPyRz - v last joint not equal");
BOOST_CHECK_MESSAGE(data_composite.oMi[index_joint_composite]
.isApprox(data_zero_mass.oMi[index_joint_R3xSO3]) , "composite<R3xSO3> vs R3-SO3 - oMi last joint not equal");
// BOOST_CHECK_MESSAGE(data_composite_planar.a[last_joint_composite]
// == data_planar.a[last_joint_pxpyrz] , "composite planar joint vs PxPyRz - a last joint not equal");
// BOOST_CHECK_MESSAGE(data_composite_planar.f[last_joint_composite]
// == data_planar.f[last_joint_pxpyrz] , "composite planar joint vs PxPyRz - f last joint not equal");
BOOST_CHECK_MESSAGE(data_composite.v[index_joint_composite]
== data_zero_mass.v[index_joint_R3xSO3] , "composite<R3xSO3> vs R3-SO3 - v last joint not equal");
// BOOST_CHECK_MESSAGE(data_composite_planar.com[last_joint_composite]
// .isApprox(data_planar.com[last_joint_pxpyrz]) , "composite planar joint vs PxPyRz - com last joint not equal");
BOOST_CHECK_MESSAGE(data_composite.a[index_joint_composite] //@TODO
== data_zero_mass.a[index_joint_R3xSO3] , "composite planar joint vs PxPyRz - a last joint not equal");
// BOOST_CHECK_MESSAGE(data_composite_planar.vcom[last_joint_composite]
// .isApprox(data_planar.vcom[last_joint_pxpyrz]) , "composite planar joint vs PxPyRz - vcom last joint not equal");
BOOST_CHECK_MESSAGE(data_composite.f[index_joint_composite] //@TODO
== data_zero_mass.f[index_joint_R3xSO3] , "composite planar joint vs PxPyRz - f last joint not equal");
// BOOST_CHECK_MESSAGE(data_composite_planar.mass[last_joint_composite]
// == data_planar.mass[last_joint_pxpyrz] , "composite planar joint vs PxPyRz - mass last joint not equal");
BOOST_CHECK_MESSAGE(data_composite.com[index_joint_composite]
.isApprox(data_zero_mass.com[index_joint_R3xSO3]) , "composite<R3xSO3> vs R3-SO3 - com last joint not equal");
// BOOST_CHECK_MESSAGE(data_composite_planar.kinetic_energy
// == data_planar.kinetic_energy , "composite planar joint vs PxPyRz - kinetic energy not equal");
BOOST_CHECK_MESSAGE(data_composite.vcom[index_joint_composite]
.isApprox(data_zero_mass.vcom[index_joint_R3xSO3]) , "composite<R3xSO3> vs R3-SO3 - vcom last joint not equal");
// BOOST_CHECK_MESSAGE(data_composite_planar.potential_energy
// == data_planar.potential_energy , "composite planar joint vs PxPyRz - potential energy not equal");
BOOST_CHECK_MESSAGE(data_composite.mass[index_joint_composite]
== data_zero_mass.mass[index_joint_R3xSO3] , "composite<R3xSO3> vs R3-SO3 - mass last joint not equal");
// BOOST_CHECK_MESSAGE(data_composite_planar.nle[last_joint_composite]
// .isApprox(data_planar.nle[last_joint_pxpyrz]) , "composite planar joint vs PxPyRz - nle not equal");
BOOST_CHECK_MESSAGE(data_composite.kinetic_energy
== data_zero_mass.kinetic_energy , "composite<R3xSO3> vs R3-SO3 - kinetic energy not equal");
// BOOST_CHECK_MESSAGE(data_composite_planar.M[last_joint_composite]
// .isApprox(data_planar.M[last_joint_pxpyrz]) , "composite planar joint vs PxPyRz - Mass Matrix not equal");
BOOST_CHECK_MESSAGE(data_composite.potential_energy
== data_zero_mass.potential_energy , "composite<R3xSO3> vs R3-SO3 - potential energy not equal");
// BOOST_CHECK_MESSAGE(data_composite_planar.J[last_joint_composite]
// .isApprox(data_planar.J[last_joint_pxpyrz]) , "composite planar joint vs PxPyRz - Jacobian not equal");
BOOST_CHECK_MESSAGE(data_composite.nle //@TODO
.isApprox(data_zero_mass.nle) , "composite planar joint vs PxPyRz - nle not equal");
BOOST_CHECK_MESSAGE(data_composite.M //@TODO
.isApprox(data_zero_mass.M) , "composite planar joint vs PxPyRz - Mass Matrix not equal");
// BOOST_CHECK_MESSAGE(data_composite_planar.Jcom
// .isApprox(data_planar.Jcom) , "composite planar joint vs PxPyRz - Jacobian com not equal");
BOOST_CHECK_MESSAGE(integrate(model_composite, q,q_dot).isApprox(integrate(model_zero_mass ,q,q_dot)) ,std::string(" composite<R3xSO3> vs R3-SO3 - integrate model error "));
BOOST_CHECK_MESSAGE(interpolate(model_composite, q1,q2,u).isApprox(interpolate(model_zero_mass ,q1,q2,u)) ,std::string(" composite<R3xSO3> vs R3-SO3 - interpolate model error "));
BOOST_CHECK_MESSAGE(differentiate(model_composite, q1,q2).isApprox(differentiate(model_zero_mass ,q1,q2)) ,std::string(" composite<R3xSO3> vs R3-SO3 - differentiate model error "));
// BOOST_CHECK_MESSAGE(fabs(distance(model_composite, q1,q2) - distance(model_zero_mass ,q1,q2)) < 1e-12 ,std::string(" composite<R3xSO3> vs R3-SO3 - distance model error "));
}
BOOST_AUTO_TEST_SUITE_END ()
......
......@@ -47,11 +47,12 @@ void addJointAndBody(Model & model, const JointModelBase<D> & jmodel, const Mode
typedef typename D::TangentVector_t TV;
typedef typename D::ConfigVector_t CV;
idx = model.addJoint(parent_id,jmodel,joint_placement,name + "_joint",
idx = model.addJoint(parent_id,jmodel,joint_placement,
TV::Zero(),
1e3 * (TV::Random() + TV::Constant(1)),
1e3 * (CV::Random() - CV::Constant(1)),
1e3 * (CV::Random() + CV::Constant(1))
1e3 * (CV::Random() + CV::Constant(1)),
name + "_joint"
);
model.appendBodyToJoint(idx,Y,SE3::Identity());
......@@ -101,7 +102,7 @@ struct TestIntegrationJoint
SE3 M1 = jdata.M;
SE3 M1_exp = M0*exp6(v0);
BOOST_CHECK(M1.isApprox(M1_exp));
BOOST_CHECK_MESSAGE(M1.isApprox(M1_exp), std::string("Error when integrating " + jmodel.shortname()));
qdot *= -1;
......@@ -114,7 +115,7 @@ struct TestIntegrationJoint
M1 = jdata.M;
M1_exp = M0*exp6(v0);
BOOST_CHECK(M1.isApprox(M1_exp));
BOOST_CHECK_MESSAGE(M1.isApprox(M1_exp), std::string("Error when integrating " + jmodel.shortname()));
}
};
......@@ -125,6 +126,35 @@ void TestIntegrationJoint::operator()< JointModelDense<-1,-1> >(JointModelBase<
template<>
void TestIntegrationJoint::operator()< JointModelSphericalZYX >(JointModelBase< JointModelSphericalZYX > & /*jmodel*/) {}
template<>
void TestIntegrationJoint::operator()< JointModelComposite >(JointModelBase< JointModelComposite > & /*jmodel*/)
{
se3::JointModelComposite jmodel((se3::JointModelRX()), (se3::JointModelRY()));
jmodel.setIndexes(1,0,0);
jmodel.updateComponentsIndexes();
se3::JointModelComposite::JointDataDerived jdata = jmodel.createData();
typedef typename JointModel::ConfigVector_t CV;
typedef typename JointModel::TangentVector_t TV;
typedef typename 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);
BOOST_CHECK_MESSAGE(M1.isApprox(M1_exp), std::string("Error when integrating " + jmodel.shortname()));
}
template<>
void TestIntegrationJoint::init<JointModelRevoluteUnaligned>(JointModelBase<JointModelRevoluteUnaligned> & jmodel)
{
......@@ -186,48 +216,9 @@ struct TestDifferentiationJoint
TV qdot = jmodel.difference(q0,q1);
BOOST_CHECK_MESSAGE( jmodel.integrate(q0, qdot).isApprox(q1), std::string("Error in difference for joint " + jmodel.shortname()));
BOOST_CHECK_MESSAGE( jmodel.isSameConfiguration(jmodel.integrate(q0, qdot), q1), std::string("Error in difference for joint " + jmodel.shortname()));
BOOST_CHECK_MESSAGE( jmodel.integrate(q1, -qdot).isApprox(q0), std::string("Error in difference for joint " + jmodel.shortname()));
}
void operator()(JointModelBase<JointModelFreeFlyer> & jmodel)
{
init(jmodel);
typedef JointModelFreeFlyer::ConfigVector_t CV;
typedef JointModelFreeFlyer::TangentVector_t TV;
typedef JointModelFreeFlyer::Transformation_t SE3;
jmodel.setIndexes(0,0,0);
CV q0 = jmodel.random();
CV q1 = jmodel.random();
TV qdot = jmodel.difference(q0,q1);
BOOST_CHECK_MESSAGE( jmodel.integrate(q0, qdot).head<3>().isApprox(q1.head<3>()), std::string("Error in difference for joint " + jmodel.shortname()));
BOOST_CHECK_MESSAGE( defineSameRotation(Eigen::Quaterniond(jmodel.integrate(q0, qdot).tail<4>()), Eigen::Quaterniond(q1.tail<4>()))
, std::string("Error in difference for joint " + jmodel.shortname()));
}
void operator()(JointModelBase<JointModelSpherical> & jmodel)
{
init(jmodel);
typedef JointModelSpherical::ConfigVector_t CV;
typedef JointModelSpherical::TangentVector_t TV;
typedef JointModelSpherical::Transformation_t SE3;
jmodel.setIndexes(0,0,0);
CV q0 = jmodel.random();
CV q1 = jmodel.random();
TV qdot = jmodel.difference(q0,q1);
BOOST_CHECK_MESSAGE( defineSameRotation(Eigen::Quaterniond(jmodel.integrate(q0, qdot)), Eigen::Quaterniond(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()));
}
......@@ -291,85 +282,21 @@ struct TestInterpolationJoint
double u = 0;
BOOST_CHECK_MESSAGE( jmodel.interpolate(q0, q1,u).isApprox(q0)
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.interpolate(jmodel.interpolate(q0, q1,u), q1, 1).isApprox(q1)
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.interpolate(q0, q1,u).isApprox(q1)
BOOST_CHECK_MESSAGE(jmodel.isSameConfiguration(jmodel.interpolate(q0, q1,u),q1)
, std::string("Error in interpolation with u = 1 for joint " + jmodel.shortname()));
}
void operator()(JointModelBase<JointModelFreeFlyer> & jmodel)
{
init(jmodel);
typedef JointModelFreeFlyer::ConfigVector_t CV;
typedef JointModelFreeFlyer::TangentVector_t TV;
typedef JointModelFreeFlyer::Transformation_t SE3;
jmodel.setIndexes(0,0,0);
CV q0 = jmodel.random();
CV q1 = jmodel.random();
double u = 0;
BOOST_CHECK_MESSAGE( jmodel.interpolate(q0, q1,u).head<3>().isApprox(q0.head<3>())
, std::string("Error in interpolation with u = 0 for joint " + jmodel.shortname()));
BOOST_CHECK_MESSAGE( defineSameRotation(Eigen::Quaterniond(jmodel.interpolate(q0, q1,u).tail<4>()), Eigen::Quaterniond(q0.tail<4>()) )
, std::string("Error in interpolation with u = 0 for joint " + jmodel.shortname()));
u = 0.3;
BOOST_CHECK_MESSAGE( jmodel.interpolate(jmodel.interpolate(q0, q1,u), q1, 1).head<3>().isApprox(q1.head<3>())
, std::string("Error in double interpolation for joint " + jmodel.shortname()));
BOOST_CHECK_MESSAGE( defineSameRotation(Eigen::Quaterniond(jmodel.interpolate(jmodel.interpolate(q0, q1,u), q1, 1).tail<4>()), Eigen::Quaterniond(q1.tail<4>()) )
, std::string("Error in double interpolation for joint " + jmodel.shortname()));
u = 1;
BOOST_CHECK_MESSAGE( jmodel.interpolate(q0, q1,u).head<3>().isApprox(q1.head<3>())
, std::string("Error in interpolation with u = 1 for joint " + jmodel.shortname()));
BOOST_CHECK_MESSAGE( defineSameRotation(Eigen::Quaterniond(jmodel.interpolate(q0, q1,u).tail<4>()), Eigen::Quaterniond(q1.tail<4>()) )
, std::string("Error in interpolation with u = 1 for joint " + jmodel.shortname()));