diff --git a/src/multibody/joint/joint-free-flyer.hpp b/src/multibody/joint/joint-free-flyer.hpp
index 9edc40e1b8f9ab551e7b1c8f699b3d898db22da4..2939525911036bd0774ebdd837401e79d1060491 100644
--- a/src/multibody/joint/joint-free-flyer.hpp
+++ b/src/multibody/joint/joint-free-flyer.hpp
@@ -379,12 +379,9 @@ 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 std::string classname() { return std::string("JointModelFreeFlyer"); }
diff --git a/src/multibody/joint/joint-planar.hpp b/src/multibody/joint/joint-planar.hpp
index 0f601fa30263362faffe785554d73bcf16402fcc..215b455b02d8ddfd33a0e73c4ccc8a2bcaada408 100644
--- a/src/multibody/joint/joint-planar.hpp
+++ b/src/multibody/joint/joint-planar.hpp
@@ -434,10 +434,7 @@ 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 std::string classname() { return std::string("JointModelPlanar");}
diff --git a/src/multibody/joint/joint-prismatic-unaligned.hpp b/src/multibody/joint/joint-prismatic-unaligned.hpp
index e5393a2a8817cf2c1d6be4d46b9266f2fd3d4242..2d6796e10f125a142a29730782c3a422df88b064 100644
--- a/src/multibody/joint/joint-prismatic-unaligned.hpp
+++ b/src/multibody/joint/joint-prismatic-unaligned.hpp
@@ -438,12 +438,9 @@ 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 std::string classname() { return std::string("JointModelPrismaticUnaligned"); }
diff --git a/src/multibody/joint/joint-prismatic.hpp b/src/multibody/joint/joint-prismatic.hpp
index b278c1136a37d0a566aacbb68d979967db89c461..9eae87b126f27f10259ae700ebf75a1b086942a7 100644
--- a/src/multibody/joint/joint-prismatic.hpp
+++ b/src/multibody/joint/joint-prismatic.hpp
@@ -493,12 +493,9 @@ 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 std::string classname();
diff --git a/src/multibody/joint/joint-revolute-unaligned.hpp b/src/multibody/joint/joint-revolute-unaligned.hpp
index 690fb3bc5eec994ebeaca6a3bd68161e6711cf02..66bf8a916d42ad49ac7662a4bb6b9813b4631ea5 100644
--- a/src/multibody/joint/joint-revolute-unaligned.hpp
+++ b/src/multibody/joint/joint-revolute-unaligned.hpp
@@ -442,12 +442,9 @@ 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 std::string classname() { return std::string("JointModelRevoluteUnaligned"); }
diff --git a/src/multibody/joint/joint-revolute-unbounded.hpp b/src/multibody/joint/joint-revolute-unbounded.hpp
index 4e06b542a03926d236398c3f0465d854024ac11a..a227674ab0eadf8115450ce4a7dc8c99f5fd7bb6 100644
--- a/src/multibody/joint/joint-revolute-unbounded.hpp
+++ b/src/multibody/joint/joint-revolute-unbounded.hpp
@@ -243,12 +243,9 @@ 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 std::string classname();
diff --git a/src/multibody/joint/joint-revolute.hpp b/src/multibody/joint/joint-revolute.hpp
index 70877c02e203dad4c640398a374e320ec0f27f44..0f42a70d13ff2e95cfdc1c12a6ace98a9d021109 100644
--- a/src/multibody/joint/joint-revolute.hpp
+++ b/src/multibody/joint/joint-revolute.hpp
@@ -527,12 +527,9 @@ 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 std::string classname();
diff --git a/src/multibody/joint/joint-spherical-ZYX.hpp b/src/multibody/joint/joint-spherical-ZYX.hpp
index 5c58d9b572cd34f42071d2256bd9280109953e36..e8c18fce34a674701f17690f7fe20f748a38a9d6 100644
--- a/src/multibody/joint/joint-spherical-ZYX.hpp
+++ b/src/multibody/joint/joint-spherical-ZYX.hpp
@@ -459,12 +459,9 @@ 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 std::string classname() { return std::string("JointModelSphericalZYX"); }
diff --git a/src/multibody/joint/joint-spherical.hpp b/src/multibody/joint/joint-spherical.hpp
index 05b8563708943994b372f50221026419eea00c09..a143221080bf57172aebe6b3ac26209862b8f544 100644
--- a/src/multibody/joint/joint-spherical.hpp
+++ b/src/multibody/joint/joint-spherical.hpp
@@ -404,12 +404,9 @@ 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 std::string classname() { return std::string("JointModelSpherical"); }
diff --git a/src/multibody/joint/joint-translation.hpp b/src/multibody/joint/joint-translation.hpp
index c78e63b425d79eeb7936976e52072201193003f6..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,12 +371,9 @@ 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 std::string classname() { return std::string("JointModelTranslation"); }
diff --git a/src/multibody/joint/joint.hpp b/src/multibody/joint/joint.hpp
index 6017881eb006d8779833ba3672927f2e6be6fe4a..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) {}
};
@@ -94,62 +92,55 @@ namespace se3
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
@@ -168,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 c949cb51517a7ade61f1f73527e6ec8207530723..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,45 +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::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__
-