Unverified Commit b29438da authored by Justin Carpentier's avatar Justin Carpentier Committed by GitHub
Browse files

Merge pull request #1141 from jcarpent/devel

Split test files and update submodule CMake
parents 819f8c12 a968b712
......@@ -33,7 +33,7 @@ INCLUDE(cmake/python.cmake)
INCLUDE(cmake/ide.cmake)
INCLUDE(cmake/apple.cmake)
# If needed, fix CMake policy for APPLE systems
# If needed, set CMake policy for APPLE systems
APPLY_DEFAULT_APPLE_CONFIGURATION()
IF(WIN32)
......@@ -43,10 +43,10 @@ ELSE(WIN32)
ENDIF(WIN32)
# --- OPTIONS ----------------------------------------
OPTION (BUILD_BENCHMARK "Build the benchmarks" OFF)
OPTION (BUILD_UTILS "Build the utils" OFF)
OPTION (BUILD_PYTHON_INTERFACE "Build the python binding" ON)
OPTION (BUILD_WITH_COMMIT_VERSION "Build libraries by setting specific commit version" OFF)
OPTION(BUILD_BENCHMARK "Build the benchmarks" OFF)
OPTION(BUILD_UTILS "Build the utils" OFF)
OPTION(BUILD_PYTHON_INTERFACE "Build the python binding" ON)
OPTION(BUILD_WITH_COMMIT_VERSION "Build libraries by setting specific commit version" OFF)
IF(DEFINED BUILD_UNIT_TESTS)
MESSAGE(AUTHOR_WARNING "BUILD_UNIT_TESTS is deprecated. Use BUILD_TESTING instead.\
......@@ -104,7 +104,7 @@ ENDIF(BUILD_WITH_URDF_SUPPORT)
IF(${URDFDOM_VERSION} VERSION_GREATER "0.4.2")
SET(CMAKE_CXX_STANDARD 11)
SET(CMAKE_CXX_STANDARD_REQUIRED ON)
MESSAGE("-- Since urdfdom >= 1.0.0, the default C++ standard is C++11. The project is then compiled with C++11 standard.")
MESSAGE(STATUS "Since urdfdom >= 1.0.0, the default C++ standard is C++11. The project is then compiled with C++11 standard.")
ENDIF(${URDFDOM_VERSION} VERSION_GREATER "0.4.2")
IF(BUILD_WITH_AUTODIFF_SUPPORT)
......@@ -149,7 +149,7 @@ ENDIF(BUILD_PYTHON_INTERFACE)
IF(BUILD_WITH_HPP_FCL_SUPPORT)
ADD_DEFINITIONS(-DPINOCCHIO_WITH_HPP_FCL)
LIST(APPEND CFLAGS_DEPENDENCIES "-DPINOCCHIO_WITH_HPP_FCL")
ADD_REQUIRED_DEPENDENCY("hpp-fcl >= 1.3.0")
ADD_REQUIRED_DEPENDENCY("hpp-fcl >= 1.4.0")
# Check whether hpp-fcl python bindings are available.
SET(BUILD_WITH_HPP_FCL_PYTHON_BINDINGS FALSE)
IF(BUILD_PYTHON_INTERFACE)
......
......@@ -44,7 +44,7 @@ IF(BUILD_PYTHON_INTERFACE)
if (EIGEN3_FOUND)
include_directories(${EIGEN3_INCLUDE_DIR})
endif()
SET(PKG_CONFIG_PYWRAP_REQUIRES "eigenpy >= 2.0.0")
SET(PKG_CONFIG_PYWRAP_REQUIRES "eigenpy >= 2.2.0")
FOREACH(dep ${PKG_CONFIG_PYWRAP_REQUIRES})
ADD_COMPILE_DEPENDENCY(${dep})
ENDFOREACH(dep ${PKG_CONFIG_PYWRAP_REQUIRES})
......@@ -81,7 +81,6 @@ IF(NOT BUILD_WITH_HPP_FCL_PYTHON_BINDINGS)
LIST(REMOVE_ITEM ${PROJECT_NAME}_PYTHON_HEADERS
multibody/fcl/transform.hpp
)
LIST(REMOVE_ITEM ${PROJECT_NAME}_PYTHON_SOURCES
multibody/fcl/expose-fcl.cpp
)
......
......@@ -5,8 +5,6 @@
#include "pinocchio/bindings/python/fwd.hpp"
#include "pinocchio/bindings/python/multibody/fcl/transform.hpp"
#include "pinocchio/bindings/python/utils/std-vector.hpp"
namespace pinocchio
{
namespace python
......@@ -14,7 +12,7 @@ namespace pinocchio
void exposeFCL()
{
namespace bp = boost::python;
bp::import ("hppfcl");
bp::import("hppfcl");
typedef ::hpp::fcl::Transform3f Transform3f;
......
Subproject commit a5c65a0ee51191be3f2e2667b95830706c211bb2
Subproject commit 078c4aee2d9d6c50007d587480b3e13dd1cca0bd
......@@ -89,21 +89,29 @@ IF(BUILD_PYTHON_INTERFACE)
ADD_SUBDIRECTORY(python)
ENDIF(BUILD_PYTHON_INTERFACE)
# Test over the joints
ADD_PINOCCHIO_UNIT_TEST(all-joints)
ADD_PINOCCHIO_UNIT_TEST(joint-revolute)
ADD_PINOCCHIO_UNIT_TEST(joint-prismatic)
ADD_PINOCCHIO_UNIT_TEST(joint-planar)
ADD_PINOCCHIO_UNIT_TEST(joint-free-flyer)
ADD_PINOCCHIO_UNIT_TEST(joint-spherical)
ADD_PINOCCHIO_UNIT_TEST(joint-translation)
ADD_PINOCCHIO_UNIT_TEST(joint-generic)
ADD_PINOCCHIO_UNIT_TEST(joint-composite)
ADD_PINOCCHIO_UNIT_TEST(joint-mimic)
ADD_PINOCCHIO_UNIT_TEST(model)
ADD_PINOCCHIO_UNIT_TEST(data)
ADD_PINOCCHIO_UNIT_TEST(constraint)
ADD_PINOCCHIO_UNIT_TEST(joints)
ADD_PINOCCHIO_UNIT_TEST(compute-all-terms)
ADD_PINOCCHIO_UNIT_TEST(energy)
ADD_PINOCCHIO_UNIT_TEST(frames)
ADD_PINOCCHIO_UNIT_TEST(joint-configurations)
ADD_PINOCCHIO_UNIT_TEST(joint-generic)
ADD_PINOCCHIO_UNIT_TEST(explog)
ADD_PINOCCHIO_UNIT_TEST(finite-differences)
ADD_PINOCCHIO_UNIT_TEST(visitor)
ADD_PINOCCHIO_UNIT_TEST(algo-check)
ADD_PINOCCHIO_UNIT_TEST(joint-composite)
ADD_PINOCCHIO_UNIT_TEST(joint-mimic)
ADD_PINOCCHIO_UNIT_TEST(liegroups)
ADD_PINOCCHIO_UNIT_TEST(regressor)
......
//
// Copyright(c) 2015-2020 CNRS INRIA
// Copyright(c) 2015 Wandercraft, 86 rue de Paris 91400 Orsay, France.
//
#include "pinocchio/math/fwd.hpp"
#include "pinocchio/multibody/joint/joints.hpp"
#include "pinocchio/algorithm/rnea.hpp"
#include "pinocchio/algorithm/aba.hpp"
#include "pinocchio/algorithm/crba.hpp"
#include "pinocchio/algorithm/jacobian.hpp"
#include "pinocchio/algorithm/compute-all-terms.hpp"
#include <boost/test/unit_test.hpp>
#include <iostream>
using namespace pinocchio;
template<typename JointModel_> struct init;
template<typename JointModel_>
struct init
{
static JointModel_ run()
{
JointModel_ jmodel;
jmodel.setIndexes(0,0,0);
return jmodel;
}
};
template<typename Scalar, int Options>
struct init<pinocchio::JointModelRevoluteUnalignedTpl<Scalar,Options> >
{
typedef pinocchio::JointModelRevoluteUnalignedTpl<Scalar,Options> JointModel;
static JointModel run()
{
typedef typename JointModel::Vector3 Vector3;
JointModel jmodel(Vector3::Random().normalized());
jmodel.setIndexes(0,0,0);
return jmodel;
}
};
template<typename Scalar, int Options>
struct init<pinocchio::JointModelRevoluteUnboundedUnalignedTpl<Scalar,Options> >
{
typedef pinocchio::JointModelRevoluteUnboundedUnalignedTpl<Scalar,Options> JointModel;
static JointModel run()
{
typedef typename JointModel::Vector3 Vector3;
JointModel jmodel(Vector3::Random().normalized());
jmodel.setIndexes(0,0,0);
return jmodel;
}
};
template<typename Scalar, int Options>
struct init<pinocchio::JointModelPrismaticUnalignedTpl<Scalar,Options> >
{
typedef pinocchio::JointModelPrismaticUnalignedTpl<Scalar,Options> JointModel;
static JointModel run()
{
typedef typename JointModel::Vector3 Vector3;
JointModel jmodel(Vector3::Random().normalized());
jmodel.setIndexes(0,0,0);
return jmodel;
}
};
template<typename Scalar, int Options, template<typename,int> class JointCollection>
struct init<pinocchio::JointModelTpl<Scalar,Options,JointCollection> >
{
typedef pinocchio::JointModelTpl<Scalar,Options,JointCollection> JointModel;
static JointModel run()
{
typedef pinocchio::JointModelRevoluteTpl<Scalar,Options,0> JointModelRX;
JointModel jmodel((JointModelRX()));
jmodel.setIndexes(0,0,0);
return jmodel;
}
};
template<typename Scalar, int Options, template<typename,int> class JointCollection>
struct init<pinocchio::JointModelCompositeTpl<Scalar,Options,JointCollection> >
{
typedef pinocchio::JointModelCompositeTpl<Scalar,Options,JointCollection> JointModel;
static JointModel run()
{
typedef pinocchio::JointModelRevoluteTpl<Scalar,Options,0> JointModelRX;
typedef pinocchio::JointModelRevoluteTpl<Scalar,Options,1> JointModelRY;
JointModel jmodel((JointModelRX()));
jmodel.addJoint(JointModelRY());
jmodel.setIndexes(0,0,0);
return jmodel;
}
};
template<typename JointModel_>
struct init<pinocchio::JointModelMimic<JointModel_> >
{
typedef pinocchio::JointModelMimic<JointModel_> JointModel;
static JointModel run()
{
JointModel_ jmodel_ref = init<JointModel_>::run();
JointModel jmodel(jmodel_ref,1.,0.);
jmodel.setIndexes(0,0,0);
return jmodel;
}
};
BOOST_AUTO_TEST_SUITE(joint_model_base_test)
template<typename TestDerived>
struct TestJointModel
{
template<typename JointModel>
void operator()(const pinocchio::JointModelBase<JointModel> &) const
{
JointModel jmodel = init<JointModel>::run();
return TestDerived::test(jmodel);
}
};
struct TestJointModelIsEqual : TestJointModel<TestJointModelIsEqual>
{
template<typename JointModel>
static void test(const JointModelBase<JointModel> & jmodel)
{
JointModel jmodel_copy = jmodel.derived();
BOOST_CHECK(jmodel_copy == jmodel.derived());
JointModel jmodel_any;
BOOST_CHECK(jmodel_any != jmodel.derived());
BOOST_CHECK(!jmodel_any.isEqual(jmodel.derived()));
}
};
BOOST_AUTO_TEST_CASE(isEqual)
{
typedef JointCollectionDefault::JointModelVariant JointModelVariant;
boost::mpl::for_each<JointModelVariant::types>(TestJointModelIsEqual());
JointModelRX joint_revolutex;
JointModelRY joint_revolutey;
BOOST_CHECK(joint_revolutex != joint_revolutey);
JointModel jmodelx(joint_revolutex);
jmodelx.setIndexes(0,0,0);
TestJointModelIsEqual()(JointModel());
JointModel jmodel_any;
BOOST_CHECK(jmodel_any != jmodelx);
}
struct TestJointModelCast : TestJointModel<TestJointModelCast>
{
template<typename JointModel>
static void test(const JointModelBase<JointModel> & jmodel)
{
typedef typename JointModel::Scalar Scalar;
BOOST_CHECK(jmodel.template cast<Scalar>() == jmodel);
BOOST_CHECK(jmodel.template cast<long double>().template cast<double>() == jmodel);
}
};
BOOST_AUTO_TEST_CASE(cast)
{
typedef JointCollectionDefault::JointModelVariant JointModelVariant;
boost::mpl::for_each<JointModelVariant::types>(TestJointModelCast());
TestJointModelCast()(JointModel());
}
struct TestJointModelDisp : TestJointModel<TestJointModelDisp>
{
template<typename JointModel>
static void test(const JointModelBase<JointModel> & jmodel)
{
typedef typename JointModel::JointDataDerived JointData;
std::cout << "shortname: " << jmodel.shortname() << std::endl;
std::cout << "classname: " << jmodel.classname() << std::endl;
std::cout << "disp:\n" << jmodel << std::endl;
JointData jdata = jmodel.createData();
std::cout << "shortname: " << jdata.shortname() << std::endl;
std::cout << "classname: " << jdata.classname() << std::endl;
std::cout << "disp:\n" << jdata << std::endl;
}
};
BOOST_AUTO_TEST_CASE(test_disp)
{
typedef JointCollectionDefault::JointModelVariant JointModelVariant;
boost::mpl::for_each<JointModelVariant::types>(TestJointModelDisp());
TestJointModelDisp()(JointModel());
}
BOOST_AUTO_TEST_SUITE_END()
//
// Copyright (c) 2015-2020 CNRS INRIA
// Copyright (c) 2015 Wandercraft, 86 rue de Paris 91400 Orsay, France.
//
#include "pinocchio/math/fwd.hpp"
#include "pinocchio/multibody/joint/joints.hpp"
#include "pinocchio/algorithm/rnea.hpp"
#include "pinocchio/algorithm/aba.hpp"
#include "pinocchio/algorithm/crba.hpp"
#include "pinocchio/algorithm/jacobian.hpp"
#include "pinocchio/algorithm/compute-all-terms.hpp"
#include <boost/test/unit_test.hpp>
#include <iostream>
using namespace pinocchio;
BOOST_AUTO_TEST_SUITE(JointFreeFlyer)
BOOST_AUTO_TEST_CASE(spatial)
{
Motion v(Motion::Random());
ConstraintIdentityTpl<double,0> constraint;
Motion Sv = constraint * v.toVector();
BOOST_CHECK(Sv == v);
}
BOOST_AUTO_TEST_SUITE_END()
//
// Copyright (c) 2015-2020 CNRS INRIA
// Copyright (c) 2015 Wandercraft, 86 rue de Paris 91400 Orsay, France.
//
#include "pinocchio/math/fwd.hpp"
#include "pinocchio/multibody/joint/joints.hpp"
#include "pinocchio/algorithm/rnea.hpp"
#include "pinocchio/algorithm/aba.hpp"
#include "pinocchio/algorithm/crba.hpp"
#include "pinocchio/algorithm/jacobian.hpp"
#include "pinocchio/algorithm/compute-all-terms.hpp"
#include <boost/test/unit_test.hpp>
#include <iostream>
using namespace pinocchio;
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(JointPlanar)
BOOST_AUTO_TEST_CASE(spatial)
{
SE3 M(SE3::Random());
Motion v(Motion::Random());
MotionPlanar mp(1.,2.,3.);
Motion mp_dense(mp);
BOOST_CHECK(M.act(mp).isApprox(M.act(mp_dense)));
BOOST_CHECK(M.actInv(mp).isApprox(M.actInv(mp_dense)));
BOOST_CHECK(v.cross(mp).isApprox(v.cross(mp_dense)));
}
BOOST_AUTO_TEST_CASE(vsFreeFlyer)
{
using namespace pinocchio;
typedef SE3::Vector3 Vector3;
typedef Eigen::Matrix <double, 6, 1> Vector6;
typedef Eigen::Matrix <double, 4, 1> VectorPl;
typedef Eigen::Matrix <double, 7, 1> VectorFF;
typedef SE3::Matrix3 Matrix3;
Model modelPlanar, modelFreeflyer;
Inertia inertia(1., Vector3(0.5, 0., 0.0), Matrix3::Identity());
SE3 pos(1); pos.translation() = SE3::LinearType(1.,0.,0.);
addJointAndBody(modelPlanar,JointModelPlanar(),0,SE3::Identity(),"planar",inertia);
addJointAndBody(modelFreeflyer,JointModelFreeFlyer(),0,SE3::Identity(),"free-flyer",inertia);
Data dataPlanar(modelPlanar);
Data dataFreeFlyer(modelFreeflyer);
VectorPl q; q << 1, 1, 0, 1; // Angle is 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);
computeAllTerms(modelPlanar, dataPlanar, q, v);
computeAllTerms(modelFreeflyer, dataFreeFlyer, qff, vff);
BOOST_CHECK(dataFreeFlyer.oMi[1].isApprox(dataPlanar.oMi[1]));
BOOST_CHECK(dataFreeFlyer.liMi[1].isApprox(dataPlanar.liMi[1]));
BOOST_CHECK(dataFreeFlyer.Ycrb[1].matrix().isApprox(dataPlanar.Ycrb[1].matrix()));
BOOST_CHECK(dataFreeFlyer.f[1].toVector().isApprox(dataPlanar.f[1].toVector()));
Eigen::VectorXd nle_expected_ff(3); nle_expected_ff << dataFreeFlyer.nle[0],
dataFreeFlyer.nle[1],
dataFreeFlyer.nle[5]
;
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);
Vector3 tau_expected; tau_expected << tauff(0), tauff(1), tauff(5);
BOOST_CHECK(tauPlanar.isApprox(tau_expected));
// ForwardDynamics == aba
Eigen::VectorXd aAbaPlanar = aba(modelPlanar,dataPlanar, q, v, tauPlanar);
Eigen::VectorXd aAbaFreeFlyer = aba(modelFreeflyer,dataFreeFlyer, qff, vff, tauff);
Vector3 a_expected; a_expected << aAbaFreeFlyer[0],
aAbaFreeFlyer[1],
aAbaFreeFlyer[5]
;
BOOST_CHECK(aAbaPlanar.isApprox(a_expected));
// crba
crba(modelPlanar, dataPlanar,q);
crba(modelFreeflyer, dataFreeFlyer, qff);
Eigen::Matrix<double, 3, 3> M_expected;
M_expected.block<2,2>(0,0) = dataFreeFlyer.M.block<2,2>(0,0);
M_expected.block<1,2>(2,0) = dataFreeFlyer.M.block<1,2>(5,0);
M_expected.block<2,1>(0,2) = dataFreeFlyer.M.col(5).head<2>();
M_expected.block<1,1>(2,2) = dataFreeFlyer.M.col(5).tail<1>();
BOOST_CHECK(dataPlanar.M.isApprox(M_expected));
// Jacobian
Eigen::Matrix<double, 6, Eigen::Dynamic> jacobian_planar;jacobian_planar.resize(6,3); jacobian_planar.setZero();
Eigen::Matrix<double, 6, Eigen::Dynamic> jacobian_ff;jacobian_ff.resize(6,6);jacobian_ff.setZero();
computeJointJacobians(modelPlanar, dataPlanar, q);
computeJointJacobians(modelFreeflyer, dataFreeFlyer, qff);
getJointJacobian(modelPlanar, dataPlanar, 1, LOCAL, jacobian_planar);
getJointJacobian(modelFreeflyer, dataFreeFlyer, 1, LOCAL, 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()
//
// Copyright (c) 2015-2020 CNRS INRIA
// Copyright (c) 2015 Wandercraft, 86 rue de Paris 91400 Orsay, France.
//
#include "pinocchio/math/fwd.hpp"
#include "pinocchio/multibody/joint/joints.hpp"
#include "pinocchio/algorithm/rnea.hpp"
#include "pinocchio/algorithm/aba.hpp"
#include "pinocchio/algorithm/crba.hpp"
#include "pinocchio/algorithm/jacobian.hpp"
#include "pinocchio/algorithm/compute-all-terms.hpp"
#include <boost/test/unit_test.hpp>
#include <iostream>
using namespace pinocchio;
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( JointPrismatic )
BOOST_AUTO_TEST_CASE(spatial)
{
typedef TransformPrismaticTpl<double,0,0> TransformX;
typedef TransformPrismaticTpl<double,0,1> TransformY;
typedef TransformPrismaticTpl<double,0,2> TransformZ;
typedef SE3::Vector3 Vector3;
const double displacement = 0.2;
SE3 Mplain, Mrand(SE3::Random());
TransformX Mx(displacement);
Mplain = Mx;
BOOST_CHECK(Mplain.translation().isApprox(Vector3(displacement,0,0)));
BOOST_CHECK(Mplain.rotation().isIdentity());
BOOST_CHECK((Mrand*Mplain).isApprox(Mrand*Mx));
TransformY My(displacement);
Mplain = My;
BOOST_CHECK(Mplain.translation().isApprox(Vector3(0,displacement,0)));
BOOST_CHECK(Mplain.rotation().isIdentity());
BOOST_CHECK((Mrand*Mplain).isApprox(Mrand*My));
TransformZ Mz(displacement);
Mplain = Mz;
BOOST_CHECK(Mplain.translation().isApprox(Vector3(0,0,displacement)));
BOOST_CHECK(Mplain.rotation().isIdentity());
BOOST_CHECK((Mrand*Mplain).isApprox(Mrand*Mz));
SE3 M(SE3::Random());
Motion v(Motion::Random());
MotionPrismaticTpl<double,0,0> mp_x(2.);
Motion mp_dense_x(mp_x);
BOOST_CHECK(M.act(mp_x).isApprox(M.act(mp_dense_x)));
BOOST_CHECK(M.actInv(mp_x).isApprox(M.actInv(mp_dense_x)));
BOOST_CHECK(v.cross(mp_x).isApprox(v.cross(mp_dense_x)));
MotionPrismaticTpl<double,0,1> mp_y(2.);
Motion mp_dense_y(mp_y);
BOOST_CHECK(M.act(mp_y).isApprox(M.act(mp_dense_y)));
BOOST_CHECK(M.actInv(mp_y).isApprox(M.actInv(mp_dense_y)));
BOOST_CHECK(v.cross(mp_y).isApprox(v.cross(mp_dense_y)));
MotionPrismaticTpl<double,0,2> mp_z(2.);
Motion mp_dense_z(mp_z);
BOOST_CHECK(M.act(mp_z).isApprox(M.act(mp_dense_z)));
BOOST_CHECK(M.actInv(mp_z).isApprox(M.actInv(mp_dense_z)));
BOOST_CHECK(v.cross(mp_z).isApprox(v.cross(mp_dense_z)));
}
BOOST_AUTO_TEST_CASE( test_kinematics )
{
using namespace pinocchio;
Motion expected_v_J(Motion::Zero());
Motion expected_c_J(Motion::Zero());
SE3 expected_configuration(SE3::Identity());
JointDataPX joint_data;