Verified Commit e2854e01 authored by jcarpent's avatar jcarpent Committed by Justin Carpentier
Browse files

[Joint] Update Joint{Model,Data}Tpl to correctly work with JointModelCompositeTpl

parent 84cf38f2
......@@ -20,9 +20,12 @@
#include "pinocchio/assert.hpp"
#include "pinocchio/multibody/joint/joint-collection.hpp"
#include "pinocchio/multibody/joint/joint-composite.hpp"
#include "pinocchio/multibody/joint/joint-basic-visitors.hxx"
#include "pinocchio/container/aligned-vector.hpp"
#include <boost/mpl/contains.hpp>
namespace se3
{
......@@ -87,12 +90,15 @@ namespace se3
const UD_t UDinv() const { return udinv_inertia(*this); }
JointDataTpl() : JointDataVariant() {}
JointDataTpl(const JointDataVariant & jdata) : JointDataVariant(jdata) {}
JointDataTpl(const JointDataVariant & jdata_variant)
: JointDataVariant(jdata_variant)
{}
};
template<typename JointCollection>
struct JointModelTpl : public JointModelBase< JointModelTpl<JointCollection> >, JointCollection::JointModelVariant
struct JointModelTpl : JointModelBase< JointModelTpl<JointCollection> >, JointCollection::JointModelVariant
{
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
......@@ -109,22 +115,36 @@ namespace se3
using Base::operator==;
JointModelTpl() : JointModelVariant() {}
JointModelTpl(const JointModelVariant & model_variant) : JointModelVariant(model_variant)
JointModelTpl(const JointModelVariant & jmodel_variant)
: JointCollection::JointModelVariant(jmodel_variant)
{}
template<typename JointModelDerived>
JointModelTpl(const JointModelBase<JointModelDerived> & jmodel)
: JointCollection::JointModelVariant((JointModelVariant)jmodel.derived())
{
BOOST_MPL_ASSERT((boost::mpl::contains<typename JointModelVariant::types,JointModelDerived>));
}
JointModelVariant & toVariant()
{ return *static_cast<JointModelVariant*>(this); }
const JointModelVariant & toVariant() const
{ return *static_cast<const JointModelVariant*>(this); }
JointDataVariant createData()
JointDataDerived createData()
{ return ::se3::createData<JointCollection>(*this); }
void calc(JointDataDerived & data,const Eigen::VectorXd & q) const
template<typename ConfigVector>
void calc(JointDataDerived & data,
const Eigen::MatrixBase<ConfigVector> & q) const
{ calc_zero_order(*this,data,q); }
void calc(JointDataDerived & data, const Eigen::VectorXd & q, const Eigen::VectorXd & v) const
template<typename ConfigVector, typename TangentVector>
void calc(JointDataDerived & data,
const Eigen::MatrixBase<ConfigVector> & q,
const Eigen::MatrixBase<TangentVector> & v) const
{ calc_first_order(*this,data,q,v); }
void calc_aba(JointDataDerived & data, Inertia::Matrix6 & I, const bool update_I) const
......@@ -141,7 +161,8 @@ namespace se3
JointIndex id() const { return ::se3::id(*this); }
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 container::aligned_vector<JointData> JointDataVector;
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment