Commit 16f2d75a authored by jcarpent's avatar jcarpent
Browse files

[Unit test] Update unit tests according to the new Model API

parent 6015f5e3
//
// Copyright (c) 2015 CNRS
// Copyright (c) 2015-2016 CNRS
//
// This file is part of Pinocchio
// Pinocchio is free software: you can redistribute it
......@@ -55,6 +55,8 @@
#define BOOST_TEST_MODULE GeomTest
#include <boost/test/unit_test.hpp>
using namespace se3;
typedef std::map <std::string, se3::SE3> PositionsMap_t;
typedef std::map <std::string, se3::SE3> JointPositionsMap_t;
typedef std::map <std::string, se3::SE3> GeometryPositionsMap_t;
......@@ -143,15 +145,16 @@ BOOST_AUTO_TEST_SUITE ( GeomTest )
BOOST_AUTO_TEST_CASE ( simple_boxes )
{
se3::Model model;
se3::GeometryModel model_geom;
using namespace se3;
Model model;
GeometryModel model_geom;
model.addJointAndBody(model.getJointId("universe"),JointModelPlanar(),SE3::Identity(),Inertia::Random(),
"planar1_joint", "planar1_body");
model.addJointAndBody(model.getJointId("universe"),JointModelPlanar(),SE3::Identity(),Inertia::Random(),
"planar2_joint", "planar2_body");
Model::JointIndex idx;
idx = model.addJoint(model.getJointId("universe"),JointModelPlanar(),SE3::Identity(),"planar1_joint");
model.appendBodyToJoint(idx,Inertia::Random(),SE3::Identity(),"planar1_body");
idx = model.addJoint(model.getJointId("universe"),JointModelPlanar(),SE3::Identity(),"planar2_joint");
model.appendBodyToJoint(idx,Inertia::Random(),SE3::Identity(),"planar2_body");
boost::shared_ptr<fcl::Box> Sample(new fcl::Box(1));
fcl::CollisionObject box1(Sample, fcl::Transform3f());
......@@ -224,16 +227,9 @@ BOOST_AUTO_TEST_CASE ( loading_model )
}
#ifdef WITH_URDFDOM
#ifdef WITH_HPP_FCL
#if defined(WITH_URDFDOM) && defined(WITH_HPP_FCL)
BOOST_AUTO_TEST_CASE (radius)
{
typedef se3::Model Model;
typedef se3::GeometryModel GeometryModel;
typedef se3::Data Data;
typedef se3::GeometryData GeometryData;
typedef std::vector<double> vector_t;
// Building the model in pinocchio and compute kinematics/geometry for configuration q_pino
std::string filename = PINOCCHIO_SOURCE_DIR"/models/romeo.urdf";
std::vector < std::string > package_dirs;
......@@ -246,10 +242,9 @@ BOOST_AUTO_TEST_CASE (radius)
GeometryData geomData(geom);
se3::computeBodyRadius(model, geom, geomData);
BOOST_FOREACH( double radius, geomData.radius) radius;
BOOST_FOREACH( double radius, geomData.radius) BOOST_CHECK(radius>=0.);
}
#endif // #ifdef WITH_URDFDOM
#endif // #ifdef WITH_HPP_FCL
#endif // if defined(WITH_URDFDOM) && defined(WITH_HPP_FCL)
#ifdef WITH_HPP_MODEL_URDF
BOOST_AUTO_TEST_CASE ( romeo_joints_meshes_positions )
......
......@@ -33,109 +33,58 @@
#include <boost/test/unit_test.hpp>
#include <boost/utility/binary.hpp>
using namespace se3;
bool configurations_are_equals(const Eigen::VectorXd & conf1, const Eigen::VectorXd & conf2)
{
long size = conf1.size();
if ( ! conf1.segment<3>(0).isApprox(conf2.segment<3>(0)) )
return false;
if( ! se3::defineSameRotation(Eigen::Quaterniond(conf1.segment<4>(3)), Eigen::Quaterniond(conf2.segment<4>(3))))
if( ! defineSameRotation(Eigen::Quaterniond(conf1.segment<4>(3)), Eigen::Quaterniond(conf2.segment<4>(3))))
return false;
if( ! se3::defineSameRotation(Eigen::Quaterniond(conf1.segment<4>(7)), Eigen::Quaterniond(conf2.segment<4>(7))))
if( ! defineSameRotation(Eigen::Quaterniond(conf1.segment<4>(7)), Eigen::Quaterniond(conf2.segment<4>(7))))
return false;
if ( ! conf1.segment(11, size-11).isApprox(conf2.segment(11, size-11)) )
return false;
return true;
}
se3::Model createModelWithAllJoints()
template<typename D>
void addJointAndBody(Model & model, const JointModelBase<D> & jmodel, const Model::JointIndex parent_id, const SE3 & joint_placement, const std::string & name, const Inertia & Y)
{
using namespace se3;
Model model;
model.addJointAndBody(model.getJointId("universe"),JointModelFreeFlyer(),SE3::Identity(),Inertia::Random(),
"freeflyer_joint", "freeflyer_body");
model.addJointAndBody(model.getJointId("freeflyer_joint"),JointModelSpherical(),SE3::Identity(),Inertia::Random(),
"spherical_joint", "spherical_body");
model.addJointAndBody(model.getJointId("spherical_joint"),JointModelRX(),SE3::Identity(),Inertia::Random(),
"revolute_joint", "revolute_body");
model.addJointAndBody(model.getJointId("revolute_joint"),JointModelPX(),SE3::Identity(),Inertia::Random(),
"px_joint", "px_body");
model.addJointAndBody(model.getJointId("px_joint"),JointModelPrismaticUnaligned(Eigen::Vector3d(1,0,0)),SE3::Identity(),Inertia::Random(),
"pu_joint", "pu_body");
model.addJointAndBody(model.getJointId("pu_joint"),JointModelRevoluteUnaligned(Eigen::Vector3d(0,0,1)),SE3::Identity(),Inertia::Random(),
"ru_joint", "ru_body");
model.addJointAndBody(model.getJointId("ru_joint"),JointModelSphericalZYX(),SE3::Identity(),Inertia::Random(),
"sphericalZYX_joint", "sphericalZYX_body");
model.addJointAndBody(model.getJointId("sphericalZYX_joint"),JointModelTranslation(),SE3::Identity(),Inertia::Random(),
"translation_joint", "translation_body");
model.addJointAndBody(model.getJointId("translation_joint"),JointModelPlanar(),SE3::Identity(),Inertia::Random(),
"planar_joint", "planar_body");
return model;
Model::JointIndex idx;
typedef typename D::TangentVector_t TV;
typedef typename D::ConfigVector_t CV;
idx = model.addJoint(parent_id,jmodel,joint_placement,name + "_joint",
TV::Zero(),
1e3 * (TV::Random() + TV::Constant(1)),
1e3 * (CV::Random() - CV::Constant(1)),
1e3 * (CV::Random() + CV::Constant(1))
);
model.appendBodyToJoint(idx,Y,SE3::Identity(),name + "_body");
}
se3::Model createBoundedModelWithAllJoints()
void buildModel(Model & model)
{
using namespace se3;
Model model;
model.addJointAndBody(model.getJointId("universe"),JointModelFreeFlyer(),SE3::Identity(),Inertia::Random(),
Eigen::VectorXd::Zero(6), 1e3 * (Eigen::VectorXd::Random(6).array() + 1),
1e3 * (Eigen::VectorXd::Random(7).array() - 1),
1e3 * (Eigen::VectorXd::Random(7).array() + 1),
"freeflyer_joint", "freeflyer_body");
model.addJointAndBody(model.getJointId("freeflyer_joint"),JointModelSpherical(),SE3::Identity(),Inertia::Random(),
Eigen::VectorXd::Zero(3), 1e3 * (Eigen::VectorXd::Random(3).array() + 1),
1e3 * (Eigen::VectorXd::Random(4).array() - 1),
1e3 * (Eigen::VectorXd::Random(4).array() + 1),
"spherical_joint", "spherical_body");
model.addJointAndBody(model.getJointId("spherical_joint"),JointModelRX(),SE3::Identity(),Inertia::Random(),
Eigen::VectorXd::Random(1).array() + 1, Eigen::VectorXd::Random(1).array() + 1,
Eigen::VectorXd::Random(1).array() - 1, Eigen::VectorXd::Random(1).array() + 1,
"revolute_joint", "revolute_body");
model.addJointAndBody(model.getJointId("revolute_joint"),JointModelPX(),SE3::Identity(),Inertia::Random(),
Eigen::VectorXd::Random(1).array() + 1, Eigen::VectorXd::Random(1).array() + 1,
Eigen::VectorXd::Random(1).array() - 1, Eigen::VectorXd::Random(1).array() + 1,
"px_joint", "px_body");
model.addJointAndBody(model.getJointId("px_joint"),JointModelPrismaticUnaligned(Eigen::Vector3d(1,0,0)),SE3::Identity(),Inertia::Random(),
Eigen::VectorXd::Random(1).array() + 1, Eigen::VectorXd::Random(1).array() + 1,
Eigen::VectorXd::Random(1).array() - 1, Eigen::VectorXd::Random(1).array() + 1,
"pu_joint", "pu_body");
model.addJointAndBody(model.getJointId("pu_joint"),JointModelRevoluteUnaligned(Eigen::Vector3d(0,0,1)),SE3::Identity(),Inertia::Random(),
Eigen::VectorXd::Random(1).array() + 1, Eigen::VectorXd::Random(1).array() + 1,
Eigen::VectorXd::Random(1).array() - 1, Eigen::VectorXd::Random(1).array() + 1,
"ru_joint", "ru_body");
model.addJointAndBody(model.getJointId("ru_joint"),JointModelSphericalZYX(),SE3::Identity(),Inertia::Random(),
Eigen::VectorXd::Random(3).array() + 1, Eigen::VectorXd::Random(3).array() + 1,
Eigen::VectorXd::Random(3).array() - 1, Eigen::VectorXd::Random(3).array() + 1,
"sphericalZYX_joint", "sphericalZYX_body");
model.addJointAndBody(model.getJointId("sphericalZYX_joint"),JointModelTranslation(),SE3::Identity(),Inertia::Random(),
Eigen::VectorXd::Random(3).array() + 1, Eigen::VectorXd::Random(3).array() + 1,
Eigen::VectorXd::Random(3).array() - 1, Eigen::VectorXd::Random(3).array() + 1,
"translation_joint", "translation_body");
model.addJointAndBody(model.getJointId("translation_joint"),JointModelPlanar(),SE3::Identity(),Inertia::Random(),
Eigen::VectorXd::Random(3).array() + 1, Eigen::VectorXd::Random(3).array() + 1,
Eigen::VectorXd::Random(3).array() - 1, Eigen::VectorXd::Random(3).array() + 1,
"planar_joint", "planar_body");
return model;
addJointAndBody(model,JointModelFreeFlyer(),model.getJointId("universe"),SE3::Identity(),"freeflyer",Inertia::Random());
addJointAndBody(model,JointModelSpherical(),model.getJointId("freeflyer_joint"),SE3::Identity(),"spherical",Inertia::Random());
addJointAndBody(model,JointModelRX(),model.getJointId("spherical_joint"),SE3::Identity(),"rx",Inertia::Random());
addJointAndBody(model,JointModelPX(),model.getJointId("rx_joint"),SE3::Identity(),"px",Inertia::Random());
addJointAndBody(model,JointModelPrismaticUnaligned(SE3::Vector3(1,0,0)),model.getJointId("px_joint"),SE3::Identity(),"pu",Inertia::Random());
addJointAndBody(model,JointModelRevoluteUnaligned(SE3::Vector3(0,0,1)),model.getJointId("pu_joint"),SE3::Identity(),"ru",Inertia::Random());
addJointAndBody(model,JointModelSphericalZYX(),model.getJointId("ru_joint"),SE3::Identity(),"sphericalZYX",Inertia::Random());
addJointAndBody(model,JointModelTranslation(),model.getJointId("sphericalZYX_joint"),SE3::Identity(),"translation",Inertia::Random());
addJointAndBody(model,JointModelPlanar(),model.getJointId("translation_joint"),SE3::Identity(),"planar",Inertia::Random());
}
BOOST_AUTO_TEST_SUITE ( JointConfigurationsTest )
BOOST_AUTO_TEST_CASE ( integration_test )
{
using namespace se3;
// Creating the Model and Data
Model model = createModelWithAllJoints();
se3::Data data(model);
Model model; buildModel(model);
Data data(model);
std::vector<Eigen::VectorXd> qs(2);
std::vector<Eigen::VectorXd> qdots(2);
......@@ -181,11 +130,8 @@ BOOST_AUTO_TEST_CASE ( integration_test )
BOOST_AUTO_TEST_CASE ( interpolation_test )
{
using namespace se3;
// Creating the Model and Data
Model model = createModelWithAllJoints();
se3::Data data(model);
Model model; buildModel(model);
Data data(model);
std::vector<Eigen::VectorXd> qs1(3);
std::vector<Eigen::VectorXd> qs2(3);
......@@ -300,12 +246,8 @@ BOOST_AUTO_TEST_CASE ( interpolation_test )
BOOST_AUTO_TEST_CASE ( differentiation_test )
{
using namespace se3;
// Creating the Model and Data
Model model = createModelWithAllJoints();
se3::Data data(model);
Model model; buildModel(model);
Data data(model);
//
// Test Case 0 : Difference between two configs
......@@ -378,16 +320,13 @@ BOOST_AUTO_TEST_CASE ( differentiation_test )
BOOST_AUTO_TEST_CASE ( distance_computation_test )
{
using namespace se3;
Model model; buildModel(model);
Data data(model);
// Creating the Model and Data
Model model = createModelWithAllJoints();
se3::Data data(model);
//
// Test Case 0 : distance between two confis
//
Eigen::VectorXd q1(Eigen::VectorXd::Zero(model.nq));
Eigen::VectorXd q2(Eigen::VectorXd::Ones(model.nq));
......@@ -465,11 +404,7 @@ BOOST_AUTO_TEST_CASE ( distance_computation_test )
BOOST_AUTO_TEST_CASE ( neutral_configuration_test )
{
using namespace se3;
// Creating the Model and Data
Model model = createModelWithAllJoints();
Model model; buildModel(model);
Eigen::VectorXd expected(model.nq);
expected << 0,0,0,0,0,0,1,
......@@ -488,43 +423,29 @@ BOOST_AUTO_TEST_CASE ( neutral_configuration_test )
BOOST_AUTO_TEST_CASE ( uniform_sampling_test )
{
using namespace se3;
// Creating the Model and Data
Model model = createBoundedModelWithAllJoints();
se3::Data data(model);
Model model; buildModel(model);
Eigen::VectorXd q1(randomConfiguration(model));
for (int i = 0; i < q1.size(); ++i)
for (int i = 0; i < model.nq; ++i)
{
BOOST_CHECK_MESSAGE(q1[i] >= model.lowerPositionLimit[i] && q1[i] <= model.upperPositionLimit[i], " UniformlySample : Generated config not in bounds");
}
}
BOOST_AUTO_TEST_CASE ( integrate_difference_test )
{
using namespace se3;
// Creating the Model and Data
Model model = createBoundedModelWithAllJoints();
se3::Data data(model);
Model model; buildModel(model);
Eigen::VectorXd q1(randomConfiguration(model, -1 * Eigen::VectorXd::Ones(model.nq), Eigen::VectorXd::Ones(model.nq) ));
Eigen::VectorXd q2(randomConfiguration(model, -1 * Eigen::VectorXd::Ones(model.nq), Eigen::VectorXd::Ones(model.nq) ));
BOOST_CHECK_MESSAGE(configurations_are_equals(integrate(model, q1, differentiate(model, q1,q2)), q2), "relation between integrate and differentiate");
}
BOOST_AUTO_TEST_CASE ( normalize_test )
{
using namespace se3;
// Creating the Model and Data
Model model = createModelWithAllJoints();
se3::Data data(model);
Model model; buildModel(model);
Eigen::VectorXd q (Eigen::VectorXd::Ones(model.nq));
se3::normalize(model, q);
......
//
// Copyright (c) 2015 CNRS
// Copyright (c) 2015-2016 CNRS
// Copyright (c) 2015 Wandercraft, 86 rue de Paris 91400 Orsay, France.
//
// This file is part of Pinocchio
......@@ -44,6 +44,8 @@
//#define VERBOSE
using namespace se3;
template <typename JoinData_t>
void printOutJointData (
#ifdef VERBOSE
......@@ -58,7 +60,6 @@ void printOutJointData (
)
{
using namespace std;
using namespace se3;
#ifdef VERBOSE
cout << "q: " << q.transpose () << endl;
......@@ -69,6 +70,14 @@ void printOutJointData (
#endif
}
template<typename D>
void addJointAndBody(Model & model, const JointModelBase<D> & jmodel, const Model::JointIndex parent_id, const SE3 & joint_placement, const std::string & joint_name, const Inertia & Y)
{
Model::JointIndex idx;
idx = model.addJoint(parent_id,jmodel,joint_placement,joint_name);
model.appendBodyToJoint(idx,Y);
}
BOOST_AUTO_TEST_SUITE (JointRevoluteUnaligned)
......@@ -76,7 +85,6 @@ BOOST_AUTO_TEST_CASE (vsRX)
{
using namespace se3;
typedef Eigen::Matrix <double, 3, 1> Vector3;
typedef Eigen::Matrix <double, 6, 1> Vector6;
typedef Eigen::Matrix <double, 3, 3> Matrix3;
Eigen::Vector3d axis;
......@@ -88,19 +96,19 @@ BOOST_AUTO_TEST_CASE (vsRX)
SE3 pos(1); pos.translation() = SE3::Linear_t(1.,0.,0.);
JointModelRevoluteUnaligned joint_model_RU(axis);
modelRX.addJointAndBody (0, JointModelRX (), pos, inertia, "rx");
modelRevoluteUnaligned.addJointAndBody(0, joint_model_RU ,pos, inertia, "revolute-unaligne");
addJointAndBody(modelRX,JointModelRX(),0,pos,"rx",inertia);
addJointAndBody(modelRevoluteUnaligned,joint_model_RU,0,pos,"revolute-unaligned",inertia);
Data dataRX(modelRX);
Data dataRevoluteUnaligned(modelRevoluteUnaligned);
Eigen::VectorXd q = Eigen::VectorXd::Ones (modelRX.nq);
Eigen::VectorXd v = Eigen::VectorXd::Ones (modelRX.nv);
Eigen::VectorXd tauRX = Eigen::VectorXd::Ones (modelRX.nv); Eigen::VectorXd tauRevoluteUnaligned = Eigen::VectorXd::Ones (modelRevoluteUnaligned.nv);
Eigen::VectorXd aRX = Eigen::VectorXd::Ones (modelRX.nv); Eigen::VectorXd aRevoluteUnaligned(aRX);
Eigen::VectorXd tauRX = Eigen::VectorXd::Ones (modelRX.nv);
Eigen::VectorXd tauRevoluteUnaligned = Eigen::VectorXd::Ones (modelRevoluteUnaligned.nv);
Eigen::VectorXd aRX = Eigen::VectorXd::Ones (modelRX.nv);
Eigen::VectorXd aRevoluteUnaligned(aRX);
forwardKinematics(modelRX, dataRX, q, v);
forwardKinematics(modelRevoluteUnaligned, dataRevoluteUnaligned, q, v);
......@@ -116,8 +124,6 @@ BOOST_AUTO_TEST_CASE (vsRX)
BOOST_CHECK(dataRevoluteUnaligned.nle.isApprox(dataRX.nle));
BOOST_CHECK(dataRevoluteUnaligned.com[0].isApprox(dataRX.com[0]));
// InverseDynamics == rnea
tauRX = rnea(modelRX, dataRX, q, v, aRX);
tauRevoluteUnaligned = rnea(modelRevoluteUnaligned, dataRevoluteUnaligned, q, v, aRevoluteUnaligned);
......@@ -128,10 +134,9 @@ BOOST_AUTO_TEST_CASE (vsRX)
Eigen::VectorXd aAbaRX = aba(modelRX,dataRX, q, v, tauRX);
Eigen::VectorXd aAbaRevoluteUnaligned = aba(modelRevoluteUnaligned,dataRevoluteUnaligned, q, v, tauRevoluteUnaligned);
BOOST_CHECK(aAbaRX.isApprox(aAbaRevoluteUnaligned));
// crba
// CRBA
crba(modelRX, dataRX,q);
crba(modelRevoluteUnaligned, dataRevoluteUnaligned, q);
......@@ -147,8 +152,6 @@ BOOST_AUTO_TEST_CASE (vsRX)
BOOST_CHECK(jacobianRX.isApprox(jacobianRevoluteUnaligned));
}
BOOST_AUTO_TEST_SUITE_END ()
......@@ -158,7 +161,6 @@ BOOST_AUTO_TEST_CASE (vsPX)
{
using namespace se3;
typedef Eigen::Matrix <double, 3, 1> Vector3;
typedef Eigen::Matrix <double, 6, 1> Vector6;
typedef Eigen::Matrix <double, 3, 3> Matrix3;
Eigen::Vector3d axis;
......@@ -170,20 +172,20 @@ BOOST_AUTO_TEST_CASE (vsPX)
SE3 pos(1); pos.translation() = SE3::Linear_t(1.,0.,0.);
JointModelPrismaticUnaligned joint_model_PU(axis);
modelPX.addJointAndBody (0, JointModelPX (), pos, inertia, "px");
modelPrismaticUnaligned.addJointAndBody(0, joint_model_PU ,pos, inertia, "prismatic-unaligne");
addJointAndBody(modelPX,JointModelPX(),0,pos,"px",inertia);
addJointAndBody(modelPrismaticUnaligned,joint_model_PU,0,pos,"prismatic-unaligned",inertia);
Data dataPX(modelPX);
Data dataPrismaticUnaligned(modelPrismaticUnaligned);
Eigen::VectorXd q = Eigen::VectorXd::Ones (modelPX.nq);
Eigen::VectorXd v = Eigen::VectorXd::Ones (modelPX.nv);
Eigen::VectorXd tauPX = Eigen::VectorXd::Ones (modelPX.nv); Eigen::VectorXd tauPrismaticUnaligned = Eigen::VectorXd::Ones (modelPrismaticUnaligned.nv);
Eigen::VectorXd aPX = Eigen::VectorXd::Ones (modelPX.nv); Eigen::VectorXd aPrismaticUnaligned(aPX);
Eigen::VectorXd tauPX = Eigen::VectorXd::Ones (modelPX.nv);
Eigen::VectorXd tauPrismaticUnaligned = Eigen::VectorXd::Ones (modelPrismaticUnaligned.nv);
Eigen::VectorXd aPX = Eigen::VectorXd::Ones (modelPX.nv);
Eigen::VectorXd aPrismaticUnaligned(aPX);
forwardKinematics(modelPX, dataPX, q, v);
forwardKinematics(modelPrismaticUnaligned, dataPrismaticUnaligned, q, v);
......@@ -198,8 +200,6 @@ BOOST_AUTO_TEST_CASE (vsPX)
BOOST_CHECK(dataPrismaticUnaligned.nle.isApprox(dataPX.nle));
BOOST_CHECK(dataPrismaticUnaligned.com[0].isApprox(dataPX.com[0]));
// InverseDynamics == rnea
tauPX = rnea(modelPX, dataPX, q, v, aPX);
tauPrismaticUnaligned = rnea(modelPrismaticUnaligned, dataPrismaticUnaligned, q, v, aPrismaticUnaligned);
......@@ -210,7 +210,6 @@ BOOST_AUTO_TEST_CASE (vsPX)
Eigen::VectorXd aAbaPX = aba(modelPX,dataPX, q, v, tauPX);
Eigen::VectorXd aAbaPrismaticUnaligned = aba(modelPrismaticUnaligned,dataPrismaticUnaligned, q, v, tauPrismaticUnaligned);
BOOST_CHECK(aAbaPX.isApprox(aAbaPrismaticUnaligned));
// crba
......@@ -227,10 +226,7 @@ BOOST_AUTO_TEST_CASE (vsPX)
getJacobian<true>(modelPX, dataPX, 1, jacobianPX);
getJacobian<true>(modelPrismaticUnaligned, dataPrismaticUnaligned, 1, jacobianPrismaticUnaligned);
BOOST_CHECK(jacobianPX.isApprox(jacobianPrismaticUnaligned));
}
BOOST_AUTO_TEST_SUITE_END ()
......@@ -249,21 +245,21 @@ BOOST_AUTO_TEST_CASE (vsFreeFlyer)
Inertia inertia (1., Vector3 (0.5, 0., 0.0), Matrix3::Identity ());
SE3 pos(1); pos.translation() = SE3::Linear_t(1.,0.,0.);
modelSpherical.addJointAndBody (0, JointModelSpherical (), pos, inertia, "spherical");
modelFreeflyer.addJointAndBody(0, JointModelFreeFlyer(),pos, inertia, "ff");
addJointAndBody(modelSpherical,JointModelSpherical(),0,pos,"spherical",inertia);
addJointAndBody(modelFreeflyer,JointModelFreeFlyer(),0,pos,"free-flyer",inertia);
Data dataSpherical(modelSpherical);
Data dataFreeFlyer(modelFreeflyer);
Eigen::VectorXd q = Eigen::VectorXd::Ones (modelSpherical.nq);q.normalize(); VectorFF qff; qff << 0, 0, 0, q[0], q[1], q[2], q[3];
Eigen::VectorXd v = Eigen::VectorXd::Ones (modelSpherical.nv); Vector6 vff; vff << 0, 0, 0, 1, 1, 1;
Eigen::VectorXd tauSpherical = Eigen::VectorXd::Ones (modelSpherical.nv); Eigen::VectorXd tauff; tauff.resize(7); tauff << 0,0,0,1,1,1,1;
Eigen::VectorXd aSpherical = Eigen::VectorXd::Ones (modelSpherical.nv); Eigen::VectorXd aff(vff);
Eigen::VectorXd q = Eigen::VectorXd::Ones (modelSpherical.nq);q.normalize();
VectorFF qff; qff << 0, 0, 0, q[0], q[1], q[2], q[3];
Eigen::VectorXd v = Eigen::VectorXd::Ones (modelSpherical.nv);
Vector6 vff; vff << 0, 0, 0, 1, 1, 1;
Eigen::VectorXd tauSpherical = Eigen::VectorXd::Ones (modelSpherical.nv);
Eigen::VectorXd tauff; tauff.resize(7); tauff << 0,0,0,1,1,1,1;
Eigen::VectorXd aSpherical = Eigen::VectorXd::Ones (modelSpherical.nv);
Eigen::VectorXd aff(vff);
forwardKinematics(modelSpherical, dataSpherical, q, v);
forwardKinematics(modelFreeflyer, dataFreeFlyer, qff, vff);
......@@ -282,8 +278,6 @@ BOOST_AUTO_TEST_CASE (vsFreeFlyer)
BOOST_CHECK(nle_expected_ff.isApprox(dataSpherical.nle));
BOOST_CHECK(dataFreeFlyer.com[0].isApprox(dataSpherical.com[0]));
// InverseDynamics == rnea
tauSpherical = rnea(modelSpherical, dataSpherical, q, v, aSpherical);
tauff = rnea(modelFreeflyer, dataFreeFlyer, qff, vff, aff);
......@@ -345,9 +339,8 @@ BOOST_AUTO_TEST_CASE (vsFreeFlyer)
Inertia inertia (1., Vector3 (0.5, 0., 0.0), Matrix3::Identity ());
SE3 pos(1); pos.translation() = SE3::Linear_t(1.,0.,0.);
modelSphericalZYX.addJointAndBody (0, JointModelSphericalZYX (), pos, inertia, "spherical");
modelFreeflyer.addJointAndBody(0, JointModelFreeFlyer(),pos, inertia, "ff");
addJointAndBody(modelSphericalZYX,JointModelSphericalZYX(),0,pos,"spherical-zyx",inertia);
addJointAndBody(modelFreeflyer,JointModelFreeFlyer(),0,pos,"free-flyer",inertia);
Data dataSphericalZYX(modelSphericalZYX);
Data dataFreeFlyer(modelFreeflyer);
......@@ -357,13 +350,15 @@ BOOST_AUTO_TEST_CASE (vsFreeFlyer)
Eigen::AngleAxisd pitchAngle(1, Eigen::Vector3d::UnitX());
Eigen::Quaterniond q_sph = rollAngle * yawAngle * pitchAngle;
Eigen::VectorXd q = Eigen::VectorXd::Ones (modelSphericalZYX.nq); VectorFF qff; qff << 0, 0, 0, q_sph.x(), q_sph.y(), q_sph.z(), q_sph.w();
Eigen::VectorXd v = Eigen::VectorXd::Ones (modelSphericalZYX.nv); Vector6 vff; vff << 0, 0, 0, 1, 1, 1;
Eigen::VectorXd tauSpherical = Eigen::VectorXd::Ones (modelSphericalZYX.nv); Eigen::VectorXd tauff; tauff.resize(6); tauff << 0,0,0,1,1,1;
Eigen::VectorXd aSpherical = Eigen::VectorXd::Ones (modelSphericalZYX.nv); Eigen::VectorXd aff(vff);
Eigen::VectorXd q = Eigen::VectorXd::Ones (modelSphericalZYX.nq);
VectorFF qff; qff << 0, 0, 0, q_sph.x(), q_sph.y(), q_sph.z(), q_sph.w();
Eigen::VectorXd v = Eigen::VectorXd::Ones (modelSphericalZYX.nv);
Vector6 vff; vff << 0, 0, 0, 1, 1, 1;
Eigen::VectorXd tauSpherical = Eigen::VectorXd::Ones (modelSphericalZYX.nv);
Eigen::VectorXd tauff; tauff.resize(6); tauff << 0,0,0,1,1,1;
Eigen::VectorXd aSpherical = Eigen::VectorXd::Ones (modelSphericalZYX.nv);
Eigen::VectorXd aff(vff);
forwardKinematics(modelSphericalZYX, dataSphericalZYX, q, v);
forwardKinematics(modelFreeflyer, dataFreeFlyer, qff, vff);
......@@ -375,7 +370,6 @@ BOOST_AUTO_TEST_CASE (vsFreeFlyer)
BOOST_CHECK(dataFreeFlyer.Ycrb[1].matrix().isApprox(dataSphericalZYX.Ycrb[1].matrix()));
BOOST_CHECK(dataFreeFlyer.com[0].isApprox(dataSphericalZYX.com[0]));
}
BOOST_AUTO_TEST_CASE ( test_rnea )
......@@ -387,7 +381,7 @@ BOOST_AUTO_TEST_CASE ( test_rnea )
Model model;
Inertia inertia (1., Vector3 (0.5, 0., 0.0), Matrix3::Identity ());
model.addJointAndBody (model.getJointId("universe"), JointModelSphericalZYX (), SE3::Identity (), inertia, "root");
addJointAndBody(model,JointModelSphericalZYX(),model.getJointId("universe"),SE3::Identity(),"root",inertia);
Data data (model);
......@@ -429,7 +423,7 @@ BOOST_AUTO_TEST_CASE ( test_crba )
Model model;
Inertia inertia (1., Vector3 (0.5, 0., 0.0), Matrix3::Identity ());
model.addJointAndBody (model.getJointId("universe"), JointModelSphericalZYX (), SE3::Identity (), inertia, "root");
addJointAndBody(model,JointModelSphericalZYX(),model.getJointId("universe"),SE3::Identity(),"root",inertia);
Data data (model);
......@@ -528,7 +522,7 @@ BOOST_AUTO_TEST_CASE ( test_rnea )
Model model;
Inertia inertia (1., Vector3 (0.5, 0., 0.0), Matrix3::Identity ());
model.addJointAndBody (model.getJointId("universe"), JointModelPX(), SE3::Identity (), inertia, "root");
addJointAndBody(model,JointModelPX(),model.getJointId("universe"),SE3::Identity(),"root",inertia);
Data data (model);
......@@ -573,7 +567,7 @@ BOOST_AUTO_TEST_CASE ( test_crba )
Model model;
Inertia inertia (1., Vector3 (0.5, 0., 0.0), Matrix3::Identity ());
model.addJointAndBody (model.getJointId("universe"), JointModelPX (), SE3::Identity (), inertia, "root");