From 16f2d75ac42f56dbf619f431e1ed96527bc804ab Mon Sep 17 00:00:00 2001
From: jcarpent <jcarpent@laas.fr>
Date: Sat, 23 Jul 2016 13:17:58 +0200
Subject: [PATCH] [Unit test] Update unit tests according to the new Model API
---
unittest/geom.cpp | 33 +++---
unittest/joint-configurations.cpp | 163 ++++++++----------------------
unittest/joints.cpp | 152 ++++++++++++----------------
3 files changed, 121 insertions(+), 227 deletions(-)
diff --git a/unittest/geom.cpp b/unittest/geom.cpp
index 240d2ddfd..5f331f2ea 100644
--- a/unittest/geom.cpp
+++ b/unittest/geom.cpp
@@ -1,5 +1,5 @@
//
-// 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 )
diff --git a/unittest/joint-configurations.cpp b/unittest/joint-configurations.cpp
index 8d9b268c6..a52e8f377 100644
--- a/unittest/joint-configurations.cpp
+++ b/unittest/joint-configurations.cpp
@@ -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);
diff --git a/unittest/joints.cpp b/unittest/joints.cpp
index a8133c46d..ae030cf07 100644
--- a/unittest/joints.cpp
+++ b/unittest/joints.cpp
@@ -1,5 +1,5 @@
//
-// 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");
+ addJointAndBody(model,JointModelPX(),model.getJointId("universe"),SE3::Identity(),"root",inertia);
Data data (model);
@@ -608,7 +602,6 @@ BOOST_AUTO_TEST_CASE ( toJointModelDense )
{
using namespace se3;
-
JointModelRX jmodel;
jmodel.setIndexes (2, 0, 0);
@@ -660,21 +653,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.);
-
- modelPlanar.addJointAndBody (0, JointModelPlanar (), pos, inertia, "planar");
- modelFreeflyer.addJointAndBody(0, JointModelFreeFlyer(),pos, inertia, "ff");
+ addJointAndBody(modelPlanar,JointModelPlanar(),0,SE3::Identity(),"planar",inertia);
+ addJointAndBody(modelFreeflyer,JointModelFreeFlyer(),0,SE3::Identity(),"free-flyer",inertia);
Data dataPlanar(modelPlanar);
Data dataFreeFlyer(modelFreeflyer);
-
- Eigen::VectorXd q = Eigen::VectorXd::Ones (modelPlanar.nq);q[2] = PI /2; VectorFF qff; qff << 1, 1, 0, 0, 0, sqrt(2)/2, sqrt(2)/2 ;
- Eigen::VectorXd v = Eigen::VectorXd::Ones (modelPlanar.nv); Vector6 vff; vff << 1, 1, 0, 0, 0, 1;
- Eigen::VectorXd tauPlanar = Eigen::VectorXd::Ones (modelPlanar.nv); Eigen::VectorXd tauff = Eigen::VectorXd::Ones (modelFreeflyer.nv);
- Eigen::VectorXd aPlanar = Eigen::VectorXd::Ones (modelPlanar.nv); Eigen::VectorXd aff(vff);
+ Eigen::VectorXd q = Eigen::VectorXd::Ones (modelPlanar.nq);q[2] = PI /2;
+ VectorFF qff; qff << 1, 1, 0, 0, 0, sqrt(2)/2, sqrt(2)/2 ;
+ Eigen::VectorXd v = Eigen::VectorXd::Ones (modelPlanar.nv);
+ Vector6 vff; vff << 1, 1, 0, 0, 0, 1;
+ Eigen::VectorXd tauPlanar = Eigen::VectorXd::Ones (modelPlanar.nv);
+ Eigen::VectorXd tauff = Eigen::VectorXd::Ones (modelFreeflyer.nv);
+ Eigen::VectorXd aPlanar = Eigen::VectorXd::Ones (modelPlanar.nv);
+ Eigen::VectorXd aff(vff);
-
-
forwardKinematics(modelPlanar, dataPlanar, q, v);
forwardKinematics(modelFreeflyer, dataFreeFlyer, qff, vff);
@@ -693,8 +686,6 @@ BOOST_AUTO_TEST_CASE (vsFreeFlyer)
BOOST_CHECK(nle_expected_ff.isApprox(dataPlanar.nle));
BOOST_CHECK(dataFreeFlyer.com[0].isApprox(dataPlanar.com[0]));
-
-
// InverseDynamics == rnea
tauPlanar = rnea(modelPlanar, dataPlanar, q, v, aPlanar);
tauff = rnea(modelFreeflyer, dataFreeFlyer, qff, vff, aff);
@@ -731,15 +722,12 @@ BOOST_AUTO_TEST_CASE (vsFreeFlyer)
getJacobian<true>(modelPlanar, dataPlanar, 1, jacobian_planar);
getJacobian<true>(modelFreeflyer, dataFreeFlyer, 1, jacobian_ff);
-
Eigen::Matrix<double, 6, 3> jacobian_expected; jacobian_expected << jacobian_ff.col(0),
jacobian_ff.col(1),
jacobian_ff.col(5)
;
BOOST_CHECK(jacobian_planar.isApprox(jacobian_expected));
-
-
}
BOOST_AUTO_TEST_SUITE_END ()
@@ -758,21 +746,17 @@ 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.);
-
- modelTranslation.addJointAndBody (0, JointModelTranslation (), pos, inertia, "translation");
- modelFreeflyer.addJointAndBody(0, JointModelFreeFlyer(),pos, inertia, "ff");
+ addJointAndBody(modelTranslation,JointModelTranslation(),0,SE3::Identity(),"translation",inertia);
+ addJointAndBody(modelFreeflyer,JointModelFreeFlyer(),0,SE3::Identity(),"free-flyer",inertia);
Data dataTranslation(modelTranslation);
Data dataFreeFlyer(modelFreeflyer);
-
- Eigen::VectorXd q = Eigen::VectorXd::Ones (modelTranslation.nq); VectorFF qff; qff << 1, 1, 1, 0, 0, 0, 1 ;
+ Eigen::VectorXd q = Eigen::VectorXd::Ones (modelTranslation.nq); VectorFF qff; qff << 1, 1, 1, 0, 0, 0, 1 ;
Eigen::VectorXd v = Eigen::VectorXd::Ones (modelTranslation.nv); Vector6 vff; vff << 1, 1, 1, 0, 0, 0;
Eigen::VectorXd tauTranslation = Eigen::VectorXd::Ones (modelTranslation.nv); Eigen::VectorXd tauff(6); tauff << 1, 1, 1, 0, 0, 0;
Eigen::VectorXd aTranslation = Eigen::VectorXd::Ones (modelTranslation.nv); Eigen::VectorXd aff(vff);
-
-
forwardKinematics(modelTranslation, dataTranslation, q, v);
forwardKinematics(modelFreeflyer, dataFreeFlyer, qff, vff);
@@ -791,8 +775,6 @@ BOOST_AUTO_TEST_CASE (vsFreeFlyer)
BOOST_CHECK(nle_expected_ff.isApprox(dataTranslation.nle));
BOOST_CHECK(dataFreeFlyer.com[0].isApprox(dataTranslation.com[0]));
-
-
// InverseDynamics == rnea
tauTranslation = rnea(modelTranslation, dataTranslation, q, v, aTranslation);
tauff = rnea(modelFreeflyer, dataFreeFlyer, qff, vff, aff);
@@ -832,8 +814,6 @@ BOOST_AUTO_TEST_CASE (vsFreeFlyer)
;
BOOST_CHECK(jacobian_planar.isApprox(jacobian_expected));
-
-
}
BOOST_AUTO_TEST_SUITE_END ()
@@ -841,9 +821,7 @@ BOOST_AUTO_TEST_SUITE (JointRevoluteUnbounded)
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;
@@ -853,8 +831,8 @@ BOOST_AUTO_TEST_CASE (vsRX)
SE3 pos(1); pos.translation() = SE3::Linear_t(1.,0.,0.);
JointModelRUBX joint_model_RUX;
- modelRX.addJointAndBody (0, JointModelRX (), pos, inertia, "px");
- modelRevoluteUnbounded.addJointAndBody(0, joint_model_RUX ,pos, inertia, "revolute unbounded x");
+ addJointAndBody(modelRX,JointModelRX(),0,SE3::Identity(),"rx",inertia);
+ addJointAndBody(modelRevoluteUnbounded,joint_model_RUX,0,SE3::Identity(),"revolute unbounded x",inertia);
Data dataRX(modelRX);
Data dataRevoluteUnbounded(modelRevoluteUnbounded);
--
GitLab