Verified Commit dbd090f8 authored by Justin Carpentier's avatar Justin Carpentier
Browse files

test/serialization: add test of Joint Transform

parent ec15d720
......@@ -2,6 +2,8 @@
// Copyright (c) 2019 INRIA
//
#include "pinocchio/algorithm/joint-configuration.hpp"
#include "pinocchio/serialization/archive.hpp"
#include "pinocchio/serialization/eigen.hpp"
......@@ -307,6 +309,80 @@ BOOST_AUTO_TEST_CASE(test_multibody_joints_model_serialization)
boost::mpl::for_each<JointModelVariant::types>(TestJointModel());
}
struct TestJointTransform
{
template <typename JointModel>
void operator()(const pinocchio::JointModelBase<JointModel> &) const
{
typedef typename JointModel::JointDerived JointDerived;
typedef typename pinocchio::traits<JointDerived>::Transformation_t Transform;
typedef typename pinocchio::traits<JointDerived>::JointDataDerived JointData;
typedef pinocchio::JointDataBase<JointData> JointDataBase;
JointModel jmodel = init<JointModel>::run();
JointData jdata = jmodel.createData();
JointDataBase & jdata_base = static_cast<JointDataBase &>(jdata);
typedef typename pinocchio::LieGroup<JointModel>::type LieGroupType;
LieGroupType lg;
Eigen::VectorXd lb(Eigen::VectorXd::Constant(jmodel.nq(),-1.));
Eigen::VectorXd ub(Eigen::VectorXd::Constant(jmodel.nq(), 1.));
Eigen::VectorXd q_random = lg.randomConfiguration(lb,ub);
jmodel.calc(jdata,q_random);
Transform & m = jdata_base.M();
test(m);
}
template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
void operator()(const pinocchio::JointModelCompositeTpl<Scalar,Options,JointCollectionTpl> &)
{
// Do nothing
}
template<typename JointModel>
void operator()(const pinocchio::JointModelMimic<JointModel> &)
{
typedef pinocchio::JointModelMimic<JointModel> JointModelMimic;
typedef typename JointModelMimic::JointDerived JointDerived;
typedef typename pinocchio::traits<JointDerived>::Transformation_t Transform;
typedef typename pinocchio::traits<JointDerived>::JointDataDerived JointDataMimic;
typedef pinocchio::JointDataBase<JointDataMimic> JointDataBase;
JointModelMimic jmodel_mimic = init<JointModelMimic>::run();
JointModel jmodel = init<JointModel>::run();
JointDataMimic jdata_mimic = jmodel_mimic.createData();
JointDataBase & jdata_mimic_base = static_cast<JointDataBase &>(jdata_mimic);
typedef typename pinocchio::LieGroup<JointModel>::type LieGroupType;
LieGroupType lg;
Eigen::VectorXd lb(Eigen::VectorXd::Constant(jmodel.nq(),-1.));
Eigen::VectorXd ub(Eigen::VectorXd::Constant(jmodel.nq(), 1.));
Eigen::VectorXd q_random = lg.randomConfiguration(lb,ub);
jmodel_mimic.calc(jdata_mimic,q_random);
Transform & m = jdata_mimic_base.M();
test(m);
}
template<typename Transform>
static void test(Transform & m)
{
generic_test(m,TEST_SERIALIZATION_FOLDER"/JointTransform","transform");
}
};
BOOST_AUTO_TEST_CASE(test_multibody_joints_transform_serialization)
{
using namespace pinocchio;
boost::mpl::for_each<JointModelVariant::types>(TestJointTransform());
}
BOOST_AUTO_TEST_CASE(test_model_serialization)
{
using namespace pinocchio;
......
Markdown is supported
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