diff --git a/benchmark/CMakeLists.txt b/benchmark/CMakeLists.txt
index 94a4c035f01c77612714ce80ef0a14fc62e3d144..f11f2a0cd58931377e992e57995412ca37ca7ba5 100644
--- a/benchmark/CMakeLists.txt
+++ b/benchmark/CMakeLists.txt
@@ -30,9 +30,13 @@ ELSE(BUILD_BENCHMARK)
ADD_EXECUTABLE(timings EXCLUDE_FROM_ALL timings.cpp)
ENDIF(BUILD_BENCHMARK)
TARGET_LINK_LIBRARIES (timings ${PROJECT_NAME})
+PKG_CONFIG_USE_DEPENDENCY(timings eigen3)
IF(BUILD_PYTHON_INTERFACE)
TARGET_LINK_LIBRARIES(timings ${PYTHON_LIBRARIES})
ENDIF(BUILD_PYTHON_INTERFACE)
+IF(${URDFDOM_FOUND})
+ PKG_CONFIG_USE_DEPENDENCY(timings urdfdom)
+ENDIF(${URDFDOM_FOUND})
SET_TARGET_PROPERTIES (timings PROPERTIES COMPILE_DEFINITIONS PINOCCHIO_SOURCE_DIR="${${PROJECT_NAME}_SOURCE_DIR}")
# timings
diff --git a/src/multibody/joint/joint-base.hpp b/src/multibody/joint/joint-base.hpp
index 8f79fb2165162c93e57921a81bcc9b487deceb55..ec9f3a3b5366f83082e8aef10a6336d8b88004a4 100644
--- a/src/multibody/joint/joint-base.hpp
+++ b/src/multibody/joint/joint-base.hpp
@@ -353,20 +353,50 @@ namespace se3
int i_q; // Index of the joint configuration in the joint configuration vector.
int i_v; // Index of the joint velocity in the joint velocity vector.
-
-
int nv() const { return derived().nv_impl(); }
int nq() const { return derived().nq_impl(); }
// Both _impl methods are reimplemented by dynamic-size joints.
int nv_impl() const { return NV; }
int nq_impl() const { return NQ; }
- const int & idx_q() const { return i_q; }
- const int & idx_v() const { return i_v; }
- const JointIndex & id() const { return i_id; }
+ int idx_q() const { return i_q; }
+ int idx_v() const { return i_v; }
+ JointIndex id() const { return i_id; }
void setIndexes(JointIndex id,int q,int v) { i_id = id, i_q = q; i_v = v; }
-
+
+ void disp(std::ostream & os) const
+ {
+ using namespace std;
+ os
+ << shortname() << endl
+ << " index: " << i_id << endl
+ << " index q: " << i_q << endl
+ << " index v: " << i_v << endl
+ << " nq: " << nq() << endl
+ << " nv: " << nv() << endl
+ ;
+ }
+
+ friend std::ostream & operator << (std::ostream & os, const JointModelBase<Derived> & joint)
+ {
+ joint.disp(os);
+ return os;
+ }
+
+ std::string shortname() const { return derived().shortname(); }
+ static std::string classname() { return Derived::classname(); }
+
+ template <class OtherDerived>
+ bool operator==(const JointModelBase<OtherDerived> & other) const { return derived().isEqual(other); }
+
+ template <class OtherDerived>
+ bool isEqual(const JointModelBase<OtherDerived> &) const { return false; }
+
+ bool isEqual(const JointModelBase<Derived> & other) const
+ {
+ return other.id() == id() && other.idx_q() == idx_q() && other.idx_v() == idx_v();
+ }
/* Acces to dedicated segment in robot config space. */
// Const access
@@ -400,7 +430,6 @@ namespace se3
typename SizeDepType<NV>::template SegmentReturn<D>::Type
jointVelocitySelector_impl( Eigen::MatrixBase<D>& a) const { return a.template segment<NV>(i_v); }
-
template<typename D>
typename SizeDepType<NV>::template ColsReturn<D>::ConstType
jointCols(const Eigen::MatrixBase<D>& A) const { return derived().jointCols_impl(A); }
@@ -415,6 +444,7 @@ namespace se3
jointCols_impl(Eigen::MatrixBase<D>& A) const { return A.template middleCols<NV>(i_v); }
JointModelDense<NQ, NV> toDense() const { return derived().toDense_impl(); }
+
}; // struct JointModelBase
} // namespace se3
diff --git a/src/multibody/joint/joint-basic-visitors.hxx b/src/multibody/joint/joint-basic-visitors.hxx
index c0faf57c8d2d649d229709052e55a86b35137a95..23d253610a002f3ed42104ac5d1feb88d8dc17f7 100644
--- a/src/multibody/joint/joint-basic-visitors.hxx
+++ b/src/multibody/joint/joint-basic-visitors.hxx
@@ -412,8 +412,8 @@ namespace se3
public:
template<typename D>
- std::string operator()(const JointModelBase<D> &) const
- { return D::shortname(); }
+ std::string operator()(const JointModelBase<D> & jmodel) const
+ { return jmodel.shortname(); }
static std::string run(const JointModelVariant & jmodel)
{ return boost::apply_visitor( JointShortnameVisitor(), jmodel ); }
diff --git a/src/multibody/joint/joint-dense.hpp b/src/multibody/joint/joint-dense.hpp
index 5d970c15bf300fd5814db392cbdc7e07941fd03d..324ef6251a535670cf7a253c23dba2d3b9e661a7 100644
--- a/src/multibody/joint/joint-dense.hpp
+++ b/src/multibody/joint/joint-dense.hpp
@@ -238,23 +238,8 @@ namespace se3
return *this;
}
- static const std::string shortname()
- {
- return std::string("JointModelDense");
- }
-
- template <class D>
- bool operator == (const JointModelBase<D> &) const
- {
- return false;
- }
-
- bool operator == (const JointModelBase<JointModelDense> & jmodel) const
- {
- return jmodel.id() == id()
- && jmodel.idx_q() == idx_q()
- && jmodel.idx_v() == idx_v();
- }
+ static std::string classname() { return std::string("JointModelDense"); }
+ std::string shortname() const { return classname(); }
}; // struct JointModelDense
diff --git a/src/multibody/joint/joint-free-flyer.hpp b/src/multibody/joint/joint-free-flyer.hpp
index f6786bd90e79dd0b14decb703ce50288bf05e3c3..2939525911036bd0774ebdd837401e79d1060491 100644
--- a/src/multibody/joint/joint-free-flyer.hpp
+++ b/src/multibody/joint/joint-free-flyer.hpp
@@ -379,31 +379,14 @@ namespace se3
q.segment<4>(idx_q()+3).normalize();
}
- JointModelDense<NQ, NV> toDense_impl() const
+ JointModelDense<NQ,NV> toDense_impl() const
{
- return JointModelDense<NQ, NV>( id(),
- idx_q(),
- idx_v()
- );
+ return JointModelDense<NQ,NV>(id(),idx_q(),idx_v());
}
- static const std::string shortname()
- {
- return std::string("JointModelFreeFlyer");
- }
+ static std::string classname() { return std::string("JointModelFreeFlyer"); }
+ std::string shortname() const { return classname(); }
- template <class D>
- bool operator == (const JointModelBase<D> &) const
- {
- return false;
- }
-
- bool operator == (const JointModelBase<JointModelFreeFlyer> & jmodel) const
- {
- return jmodel.id() == id()
- && jmodel.idx_q() == idx_q()
- && jmodel.idx_v() == idx_v();
- }
}; // struct JointModelFreeFlyer
} // namespace se3
diff --git a/src/multibody/joint/joint-planar.hpp b/src/multibody/joint/joint-planar.hpp
index 4a8e7f35b7bf435daf98187248bb476f7ac1d415..215b455b02d8ddfd33a0e73c4ccc8a2bcaada408 100644
--- a/src/multibody/joint/joint-planar.hpp
+++ b/src/multibody/joint/joint-planar.hpp
@@ -434,29 +434,12 @@ namespace se3
JointModelDense<NQ, NV> toDense_impl() const
{
- return JointModelDense<NQ, NV>( id(),
- idx_q(),
- idx_v()
- );
+ return JointModelDense<NQ, NV>(id(),idx_q(),idx_v());
}
- static const std::string shortname()
- {
- return std::string("JointModelPlanar");
- }
+ static std::string classname() { return std::string("JointModelPlanar");}
+ std::string shortname() const { return classname(); }
- template <class D>
- bool operator == (const JointModelBase<D> &) const
- {
- return false;
- }
-
- bool operator == (const JointModelBase<JointModelPlanar> & jmodel) const
- {
- return jmodel.id() == id()
- && jmodel.idx_q() == idx_q()
- && jmodel.idx_v() == idx_v();
- }
}; // struct JointModelPlanar
} // namespace se3
diff --git a/src/multibody/joint/joint-prismatic-unaligned.hpp b/src/multibody/joint/joint-prismatic-unaligned.hpp
index 00d92e14f8c6639fe5315ad4addd91337600f8a2..2d6796e10f125a142a29730782c3a422df88b064 100644
--- a/src/multibody/joint/joint-prismatic-unaligned.hpp
+++ b/src/multibody/joint/joint-prismatic-unaligned.hpp
@@ -438,33 +438,14 @@ namespace se3
return q;
}
- JointModelDense<NQ, NV> toDense_impl() const
+ JointModelDense<NQ,NV> toDense_impl() const
{
- return JointModelDense<NQ, NV>( id(),
- idx_q(),
- idx_v()
- );
+ return JointModelDense<NQ,NV>(id(),idx_q(),idx_v());
}
- static const std::string shortname()
- {
- return std::string("JointModelPrismaticUnaligned");
- }
+ static std::string classname() { return std::string("JointModelPrismaticUnaligned"); }
+ std::string shortname() const { return classname(); }
- template <class D>
- bool operator== (const JointModelBase<D> &) const
- {
- return false;
- }
-
- bool operator== (const JointModelBase<JointModelPrismaticUnaligned> & jmodel) const
- {
- return jmodel.id() == id()
- && jmodel.idx_q() == idx_q()
- && jmodel.idx_v() == idx_v();
- }
-
- public:
Vector3 axis;
}; // struct JointModelPrismaticUnaligned
diff --git a/src/multibody/joint/joint-prismatic.hpp b/src/multibody/joint/joint-prismatic.hpp
index 85f167934bf84029e6c087fa587b185897c272b0..9eae87b126f27f10259ae700ebf75a1b086942a7 100644
--- a/src/multibody/joint/joint-prismatic.hpp
+++ b/src/multibody/joint/joint-prismatic.hpp
@@ -493,28 +493,14 @@ namespace se3
return q;
}
- JointModelDense<NQ, NV> toDense_impl() const
+ JointModelDense<NQ,NV> toDense_impl() const
{
- return JointModelDense<NQ, NV>( id(),
- idx_q(),
- idx_v()
- );
+ return JointModelDense<NQ,NV>(id(),idx_q(),idx_v());
}
- static const std::string shortname();
+ static std::string classname();
+ std::string shortname() const { return classname(); }
- template <class D>
- bool operator == (const JointModelBase<D> &) const
- {
- return false;
- }
-
- bool operator == (const JointModelBase<JointModelPrismatic> & jmodel) const
- {
- return jmodel.id() == id()
- && jmodel.idx_q() == idx_q()
- && jmodel.idx_v() == idx_v();
- }
}; // struct JointModelPrismatic
typedef JointPrismatic<0> JointPX;
@@ -522,7 +508,7 @@ namespace se3
typedef JointModelPrismatic<0> JointModelPX;
template<> inline
- const std::string JointModelPrismatic<0>::shortname()
+ std::string JointModelPrismatic<0>::classname()
{
return std::string("JointModelPX");
}
@@ -532,7 +518,7 @@ namespace se3
typedef JointModelPrismatic<1> JointModelPY;
template<> inline
- const std::string JointModelPrismatic<1>::shortname()
+ std::string JointModelPrismatic<1>::classname()
{
return std::string("JointModelPY");
}
@@ -542,7 +528,7 @@ namespace se3
typedef JointModelPrismatic<2> JointModelPZ;
template<> inline
- const std::string JointModelPrismatic<2>::shortname()
+ std::string JointModelPrismatic<2>::classname()
{
return std::string("JointModelPZ");
}
diff --git a/src/multibody/joint/joint-revolute-unaligned.hpp b/src/multibody/joint/joint-revolute-unaligned.hpp
index 448f6522fcfb14f86978025c0e5e97d8da544095..66bf8a916d42ad49ac7662a4bb6b9813b4631ea5 100644
--- a/src/multibody/joint/joint-revolute-unaligned.hpp
+++ b/src/multibody/joint/joint-revolute-unaligned.hpp
@@ -442,33 +442,14 @@ namespace se3
return q;
}
- JointModelDense<NQ, NV> toDense_impl() const
+ JointModelDense<NQ,NV> toDense_impl() const
{
- return JointModelDense<NQ, NV>( id(),
- idx_q(),
- idx_v()
- );
+ return JointModelDense<NQ,NV>(id(),idx_q(),idx_v());
}
- static const std::string shortname()
- {
- return std::string("JointModelRevoluteUnaligned");
- }
+ static std::string classname() { return std::string("JointModelRevoluteUnaligned"); }
+ std::string shortname() const { return classname(); }
- template <class D>
- bool operator == (const JointModelBase<D> &) const
- {
- return false;
- }
-
- bool operator == (const JointModelBase<JointModelRevoluteUnaligned> & jmodel) const
- {
- return jmodel.id() == id()
- && jmodel.idx_q() == idx_q()
- && jmodel.idx_v() == idx_v();
- }
-
- public:
Motion::Vector3 axis;
}; // struct JointModelRevoluteUnaligned
diff --git a/src/multibody/joint/joint-revolute-unbounded.hpp b/src/multibody/joint/joint-revolute-unbounded.hpp
index 344392f105fecf4127bf5639d31bba4271e98240..a227674ab0eadf8115450ce4a7dc8c99f5fd7bb6 100644
--- a/src/multibody/joint/joint-revolute-unbounded.hpp
+++ b/src/multibody/joint/joint-revolute-unbounded.hpp
@@ -243,28 +243,13 @@ namespace se3
q.segment<NQ>(idx_q()).normalize();
}
- JointModelDense<NQ, NV> toDense_impl() const
+ JointModelDense<NQ,NV> toDense_impl() const
{
- return JointModelDense<NQ, NV>( id(),
- idx_q(),
- idx_v()
- );
+ return JointModelDense<NQ,NV>(id(),idx_q(),idx_v());
}
- static const std::string shortname();
-
- template <class D>
- bool operator == (const JointModelBase<D> &) const
- {
- return false;
- }
-
- bool operator == (const JointModelBase<JointModelRevoluteUnbounded <axis> > & jmodel) const
- {
- return jmodel.id() == id()
- && jmodel.idx_q() == idx_q()
- && jmodel.idx_v() == idx_v();
- }
+ static std::string classname();
+ std::string shortname() const { return classname(); }
}; // struct JointModelRevoluteUnbounded
@@ -273,7 +258,7 @@ namespace se3
typedef JointModelRevoluteUnbounded<0> JointModelRUBX;
template<> inline
- const std::string JointModelRevoluteUnbounded<0>::shortname()
+ std::string JointModelRevoluteUnbounded<0>::classname()
{
return std::string("JointModelRUBX") ;
}
@@ -283,7 +268,7 @@ namespace se3
typedef JointModelRevoluteUnbounded<1> JointModelRUBY;
template<> inline
- const std::string JointModelRevoluteUnbounded<1>::shortname()
+ std::string JointModelRevoluteUnbounded<1>::classname()
{
return std::string("JointModelRUBY") ;
}
@@ -293,7 +278,7 @@ namespace se3
typedef JointModelRevoluteUnbounded<2> JointModelRUBZ;
template<> inline
- const std::string JointModelRevoluteUnbounded<2>::shortname()
+ std::string JointModelRevoluteUnbounded<2>::classname()
{
return std::string("JointModelRUBZ") ;
}
diff --git a/src/multibody/joint/joint-revolute.hpp b/src/multibody/joint/joint-revolute.hpp
index 1df83af33bf16eae78268fbd7082db5a357485a4..0f42a70d13ff2e95cfdc1c12a6ace98a9d021109 100644
--- a/src/multibody/joint/joint-revolute.hpp
+++ b/src/multibody/joint/joint-revolute.hpp
@@ -527,28 +527,13 @@ namespace se3
return q;
}
- JointModelDense<NQ, NV> toDense_impl() const
+ JointModelDense<NQ,NV> toDense_impl() const
{
- return JointModelDense<NQ, NV>( id(),
- idx_q(),
- idx_v()
- );
+ return JointModelDense<NQ,NV>(id(),idx_q(),idx_v());
}
- static const std::string shortname();
-
- template <class D>
- bool operator == (const JointModelBase<D> &) const
- {
- return false;
- }
-
- bool operator == (const JointModelBase<JointModelRevolute <axis> > & jmodel) const
- {
- return jmodel.id() == id()
- && jmodel.idx_q() == idx_q()
- && jmodel.idx_v() == idx_v();
- }
+ static std::string classname();
+ std::string shortname() const { return classname(); }
}; // struct JointModelRevolute
@@ -557,7 +542,7 @@ namespace se3
typedef JointModelRevolute<0> JointModelRX;
template<> inline
- const std::string JointModelRevolute<0>::shortname()
+ std::string JointModelRevolute<0>::classname()
{
return std::string("JointModelRX") ;
}
@@ -567,7 +552,7 @@ namespace se3
typedef JointModelRevolute<1> JointModelRY;
template<> inline
- const std::string JointModelRevolute<1>::shortname()
+ std::string JointModelRevolute<1>::classname()
{
return std::string("JointModelRY") ;
}
@@ -577,7 +562,7 @@ namespace se3
typedef JointModelRevolute<2> JointModelRZ;
template<> inline
- const std::string JointModelRevolute<2>::shortname()
+ std::string JointModelRevolute<2>::classname()
{
return std::string("JointModelRZ") ;
}
diff --git a/src/multibody/joint/joint-spherical-ZYX.hpp b/src/multibody/joint/joint-spherical-ZYX.hpp
index 07eed8ee316aab5d22fa711b42ee43fe70838c79..e8c18fce34a674701f17690f7fe20f748a38a9d6 100644
--- a/src/multibody/joint/joint-spherical-ZYX.hpp
+++ b/src/multibody/joint/joint-spherical-ZYX.hpp
@@ -459,31 +459,13 @@ namespace se3
return q;
}
- JointModelDense<NQ, NV> toDense_impl() const
+ JointModelDense<NQ,NV> toDense_impl() const
{
- return JointModelDense<NQ, NV>( id(),
- idx_q(),
- idx_v()
- );
+ return JointModelDense<NQ,NV>(id(),idx_q(),idx_v());
}
- static const std::string shortname()
- {
- return std::string("JointModelSphericalZYX");
- }
-
- template <class D>
- bool operator == (const JointModelBase<D> &) const
- {
- return false;
- }
-
- bool operator == (const JointModelBase<JointModelSphericalZYX> & jmodel) const
- {
- return jmodel.id() == id()
- && jmodel.idx_q() == idx_q()
- && jmodel.idx_v() == idx_v();
- }
+ static std::string classname() { return std::string("JointModelSphericalZYX"); }
+ std::string shortname() const { return classname(); }
}; // struct JointModelSphericalZYX
diff --git a/src/multibody/joint/joint-spherical.hpp b/src/multibody/joint/joint-spherical.hpp
index df7eaac16bf8a7c16952c484b3881a39749a3d7e..a143221080bf57172aebe6b3ac26209862b8f544 100644
--- a/src/multibody/joint/joint-spherical.hpp
+++ b/src/multibody/joint/joint-spherical.hpp
@@ -404,31 +404,14 @@ namespace se3
q.segment<NQ>(idx_q()).normalize();
}
- JointModelDense<NQ, NV> toDense_impl() const
+ JointModelDense<NQ,NV> toDense_impl() const
{
- return JointModelDense<NQ, NV>( id(),
- idx_q(),
- idx_v()
- );
+ return JointModelDense<NQ,NV>(id(),idx_q(),idx_v());
}
- static const std::string shortname()
- {
- return std::string("JointModelSpherical");
- }
+ static std::string classname() { return std::string("JointModelSpherical"); }
+ std::string shortname() const { return classname(); }
- template <class D>
- bool operator == (const JointModelBase<D> &) const
- {
- return false;
- }
-
- bool operator == (const JointModelBase<JointModelSpherical> & jmodel) const
- {
- return jmodel.id() == id()
- && jmodel.idx_q() == idx_q()
- && jmodel.idx_v() == idx_v();
- }
}; // struct JointModelSpherical
} // namespace se3
diff --git a/src/multibody/joint/joint-translation.hpp b/src/multibody/joint/joint-translation.hpp
index 7bfc7e3afa2652fa5c5b5fa0ee269457ce56cb1d..25ae5b84f4366209f1a9420d7a1b1794144dc831 100644
--- a/src/multibody/joint/joint-translation.hpp
+++ b/src/multibody/joint/joint-translation.hpp
@@ -356,12 +356,12 @@ namespace se3
Eigen::VectorXd::ConstFixedSegmentReturnType<NQ>::Type & q_0 = q0.segment<NQ> (idx_q ());
Eigen::VectorXd::ConstFixedSegmentReturnType<NQ>::Type & q_1 = q1.segment<NQ> (idx_q ());
- return ( q_1 - q_0);
+ return q_1-q_0;
}
double distance_impl(const Eigen::VectorXd & q0,const Eigen::VectorXd & q1) const
{
- return difference_impl(q0, q1).norm();
+ return difference_impl(q0,q1).norm();
}
ConfigVector_t neutralConfiguration_impl() const
@@ -371,31 +371,14 @@ namespace se3
return q;
}
- JointModelDense<NQ, NV> toDense_impl() const
+ JointModelDense<NQ,NV> toDense_impl() const
{
- return JointModelDense<NQ, NV>( id(),
- idx_q(),
- idx_v()
- );
+ return JointModelDense<NQ,NV>(id(),idx_q(),idx_v());
}
- static const std::string shortname()
- {
- return std::string("JointModelTranslation");
- }
+ static std::string classname() { return std::string("JointModelTranslation"); }
+ std::string shortname() const { return classname(); }
- template <class D>
- bool operator == (const JointModelBase<D> &) const
- {
- return false;
- }
-
- bool operator == (const JointModelBase<JointModelTranslation> & jmodel) const
- {
- return jmodel.id() == id()
- && jmodel.idx_q() == idx_q()
- && jmodel.idx_v() == idx_v();
- }
}; // struct JointModelTranslation
} // namespace se3
diff --git a/src/multibody/joint/joint.hpp b/src/multibody/joint/joint.hpp
index 39497f12366a0736185104cafac683d577feb2e5..551436b4fde3f66f2af3ed5c2e84a76070b03f93 100644
--- a/src/multibody/joint/joint.hpp
+++ b/src/multibody/joint/joint.hpp
@@ -32,7 +32,6 @@ namespace se3
template<>
struct traits<Joint>
{
-
enum {
NQ = -1, // Dynamic because unknown at compilation
NV = -1
@@ -77,9 +76,8 @@ namespace se3
const D_t Dinv() const { return dinv_inertia(*this); }
const UD_t UDinv() const { return udinv_inertia(*this); }
-
JointData() : JointDataBoostVariant() {}
- JointData(const JointDataVariant & jdata ) : JointDataBoostVariant(jdata){}
+ JointData(const JointDataVariant & jdata) : JointDataBoostVariant(jdata) {}
};
@@ -90,65 +88,59 @@ namespace se3
typedef Joint JointDerived;
SE3_JOINT_TYPEDEF;
SE3_JOINT_USE_INDEXES;
- using JointModelBase<JointModel>::id;
- using JointModelBase<JointModel>::setIndexes;
+ using Base::id;
+ using Base::setIndexes;
+ using Base::operator==;
+ JointModel() : JointModelBoostVariant() {}
+ JointModel(const JointModelVariant & model_variant) : JointModelBoostVariant(model_variant)
+ {}
+
JointModelVariant& toVariant() { return *static_cast<JointModelVariant*>(this); }
const JointModelVariant& toVariant() const { return *static_cast<const JointModelVariant*>(this); }
- JointDataVariant createData()
- {
- return ::se3::createData(*this);
- }
+ JointDataVariant createData() { return ::se3::createData(*this); }
+ void calc(JointData & data,const Eigen::VectorXd & q) const { calc_zero_order(*this,data,q); }
- void calc (JointData & data,
- const Eigen::VectorXd & qs) const
+ void calc (JointData & data, const Eigen::VectorXd & q, const Eigen::VectorXd & v) const
{
- calc_zero_order(*this, data, qs);
-
- }
-
- void calc (JointData & data,
- const Eigen::VectorXd & qs,
- const Eigen::VectorXd & vs ) const
- {
- calc_first_order(*this, data, qs, vs);
+ calc_first_order(*this,data,q,v);
}
void calc_aba(JointData & data, Inertia::Matrix6 & I, const bool update_I) const
{
- ::se3::calc_aba(*this, data, I, update_I);
+ ::se3::calc_aba(*this,data,I,update_I);
}
- ConfigVector_t integrate_impl(const Eigen::VectorXd & qs,const Eigen::VectorXd & vs) const
+ ConfigVector_t integrate_impl(const Eigen::VectorXd & q,const Eigen::VectorXd & v) const
{
- return ::se3::integrate(*this, qs, vs);
+ return ::se3::integrate(*this,q,v);
}
ConfigVector_t interpolate_impl(const Eigen::VectorXd & q0,const Eigen::VectorXd & q1, const double u) const
{
- return ::se3::interpolate(*this, q0, q1, u);
+ return ::se3::interpolate(*this,q0,q1,u);
}
- ConfigVector_t randomConfiguration_impl(const ConfigVector_t & lower_pos_limit, const ConfigVector_t & upper_pos_limit ) const throw (std::runtime_error)
+ ConfigVector_t randomConfiguration_impl(const ConfigVector_t & lower_pos_limit, const ConfigVector_t & upper_pos_limit) const throw (std::runtime_error)
{
- return ::se3::randomConfiguration(*this, lower_pos_limit, upper_pos_limit);
+ return ::se3::randomConfiguration(*this,lower_pos_limit,upper_pos_limit);
}
TangentVector_t difference_impl(const Eigen::VectorXd & q0,const Eigen::VectorXd & q1) const
{
- return ::se3::difference(*this, q0, q1);
+ return ::se3::difference(*this,q0,q1);
}
double distance_impl(const Eigen::VectorXd & q0,const Eigen::VectorXd & q1) const
{
- return ::se3::distance(*this, q0, q1);
+ return ::se3::distance(*this,q0,q1);
}
void normalize_impl(Eigen::VectorXd & q) const
{
- return ::se3::normalize(*this, q);
+ return ::se3::normalize(*this,q);
}
ConfigVector_t neutralConfiguration_impl() const
@@ -156,29 +148,8 @@ namespace se3
return ::se3::neutralConfiguration(*this);
}
- JointModel() : JointModelBoostVariant() {}
- JointModel( const JointModelVariant & model_variant ) : JointModelBoostVariant(model_variant)
- {}
-
-
- const std::string shortname()
- {
- return ::se3::shortname(*this);
- }
-
- template <class D>
- bool operator == (const JointModelBase<D> &) const
- {
- return false;
- }
-
- bool operator == (const JointModelBase<JointModel> & jmodel) const
- {
- return jmodel.id() == id()
- && jmodel.idx_q() == idx_q()
- && jmodel.idx_v() == idx_v();
- }
-
+ std::string shortname() const { return ::se3::shortname(*this); }
+ static std::string classname() { return "JointModel"; }
int nq_impl() const { return ::se3::nq(*this); }
int nv_impl() const { return ::se3::nv(*this); }
@@ -188,11 +159,7 @@ namespace se3
JointIndex id() const { return ::se3::id(*this); }
- // void setIndexes(JointIndex ,int ,int ) { SE3_STATIC_ASSERT(false, THIS_METHOD_SHOULD_NOT_BE_CALLED_ON_DERIVED_CLASS); }
- void setIndexes(JointIndex id,int nq,int nv)
- {
- ::se3::setIndexes(*this,id, nq, nv);
- }
+ void setIndexes(JointIndex id,int nq,int nv) { ::se3::setIndexes(*this,id, nq, nv); }
};
typedef std::vector<JointData> JointDataVector;
diff --git a/src/python/joints-variant.hpp b/src/python/joints-variant.hpp
index 21f9a54b4b7f482a90ed27525015ed42a403241d..b60ea778e4c494cd7dd82a8760bffabeea1faf63 100644
--- a/src/python/joints-variant.hpp
+++ b/src/python/joints-variant.hpp
@@ -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
@@ -23,8 +23,6 @@
#include "pinocchio/multibody/joint/joint-variant.hpp"
#include "pinocchio/python/joints-models.hpp"
-
-
namespace se3
{
namespace python
@@ -33,46 +31,37 @@ namespace se3
struct jointModelVariantVisitor : boost::static_visitor<PyObject *>
{
- static result_type convert(JointModelVariant const &jv)
+ static result_type convert(JointModelVariant const & jv)
{
return apply_visitor(jointModelVariantVisitor(), jv);
}
template<typename T>
- result_type operator()(T const &t) const
+ result_type operator()(T const & t) const
{
return boost::python::incref(boost::python::object(t).ptr());
}
};
-
- struct exposer {
- template<class T> inline void operator()(T)
+ struct exposer
+ {
+ template<class T>
+ void operator()(T)
{
- expose_constructors<T>(bp::class_<T>(T::shortname().c_str(),bp::init<>()).def(JointPythonVisitor<T >())
- );
- bp::implicitly_convertible<T, se3::JointModelVariant>();
+ expose_constructors<T>(bp::class_<T>(T::classname().c_str(),bp::init<>()).def(JointPythonVisitor<T>()));
+ bp::implicitly_convertible<T,se3::JointModelVariant>();
}
-
-
};
// For the moment, only expose models of joint. Not data ( to do it, split exposer into exposerModels & exposer_Datas and do another for_each)
static void exposeVariants()
{
-
boost::mpl::for_each<JointModelVariant::types>(exposer());
-
bp::to_python_converter<se3::JointModelVariant, jointModelVariantVisitor>();
-
// bp::def("make_variant", se3::make_variant);
-
}
-
-
-
- }} // namespace se3::python
+ } // namespace python
+} // namespace se3
#endif // ifndef __se3_python_joints_variant_hpp__
-
diff --git a/unittest/joint.cpp b/unittest/joint.cpp
index 0dd6a2734331a325a467b51f2513f0662fc10aec..af387b59f91796cf20c8272376f832d43337c193 100644
--- a/unittest/joint.cpp
+++ b/unittest/joint.cpp
@@ -44,9 +44,9 @@ void test_joint_methods (T & jmodel, typename T::JointDataDerived & jdata)
se3::Inertia::Matrix6 Ia(se3::Inertia::Random().matrix());
bool update_I = false;
- if(T::shortname() == "JointModelRUBX" ||
- T::shortname() == "JointModelRUBY" ||
- T::shortname() == "JointModelRUBZ")
+ if(jmodel.shortname() == "JointModelRUBX" ||
+ jmodel.shortname() == "JointModelRUBY" ||
+ jmodel.shortname() == "JointModelRUBZ")
{
// normalize cos/sin
q1.normalize();
@@ -65,7 +65,7 @@ void test_joint_methods (T & jmodel, typename T::JointDataDerived & jdata)
jma.calc(jda, q1, q1_dot);
jma.calc_aba(jda, Ia, update_I);
- std::string error_prefix("JointModel on " + T::shortname());
+ std::string error_prefix("JointModel on " + jma.shortname());
BOOST_CHECK_MESSAGE(jmodel.nq() == jma.nq() ,std::string(error_prefix + " - nq "));
BOOST_CHECK_MESSAGE(jmodel.nv() == jma.nv() ,std::string(error_prefix + " - nv "));