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

[Joint] Start templatization of JointModelComposite

Introduce the notion of JointModelCollection
parent 6cc6eb5c
......@@ -192,6 +192,7 @@ SET(${PROJECT_NAME}_SPATIAL_HEADERS
SET(${PROJECT_NAME}_MULTIBODY_JOINT_HEADERS
multibody/joint/fwd.hpp
multibody/joint/joint-base.hpp
multibody/joint/joint-collection.hpp
multibody/joint/joint-revolute.hpp
multibody/joint/joint-revolute-unaligned.hpp
multibody/joint/joint-revolute-unbounded.hpp
......
......@@ -21,7 +21,7 @@
#include <eigenpy/exception.hpp>
#include <eigenpy/eigenpy.hpp>
#include "pinocchio/multibody/joint/joint-variant.hpp"
#include "pinocchio/multibody/joint/joint-collection.hpp"
namespace se3
{
......
......@@ -20,7 +20,7 @@
#include <eigenpy/exception.hpp>
#include <eigenpy/eigenpy.hpp>
#include "pinocchio/multibody/joint/joint-variant.hpp"
#include "pinocchio/multibody/joint/joint-collection.hpp"
namespace se3
{
......
......@@ -20,7 +20,7 @@
#include <eigenpy/exception.hpp>
#include <eigenpy/eigenpy.hpp>
#include "pinocchio/multibody/joint/joint-variant.hpp"
#include "pinocchio/multibody/joint/joint-collection.hpp"
#include "pinocchio/bindings/python/multibody/joint/joints-models.hpp"
namespace se3
......
......@@ -46,8 +46,8 @@ namespace se3
template<typename Scalar, int Options = 0> struct JointDataSphericalZYXTpl;
typedef JointDataSphericalZYXTpl<double> JointDataSphericalZYX;
template<typename Scalar, int Options, int axis> struct JointModelPrismatic;
template<typename Scalar, int Options, int axis> struct JointDataPrismatic;
template<typename Scalar, int Options, int axis> struct JointModelPrismaticTpl;
template<typename Scalar, int Options, int axis> struct JointDataPrismaticTpl;
template<typename Scalar, int Options = 0> struct JointModelPrismaticUnalignedTpl;
typedef JointModelPrismaticUnalignedTpl<double> JointModelPrismaticUnaligned;
......@@ -73,8 +73,14 @@ namespace se3
template<typename Scalar, int Options = 0> struct JointDataTranslationTpl;
typedef JointDataTranslationTpl<double> JointDataTranslation;
struct JointModelComposite;
struct JointDataComposite;
template<typename Scalar, int Options = 0> struct JointCollectionDefaultTpl;
typedef JointCollectionDefaultTpl<double> JointCollectionDefault;
template<typename JointCollection> struct JointModelCompositeTpl;
typedef JointModelCompositeTpl< JointCollectionDefaultTpl<double> > JointModelComposite;
template<typename JointCollection> struct JointDataCompositeTpl;
typedef JointDataCompositeTpl< JointCollectionDefaultTpl<double> > JointDataComposite;
struct JointModel;
struct JointData;
......
//
// Copyright (c) 2016 CNRS
// Copyright (c) 2016,2018 CNRS
//
// This file is part of Pinocchio
// Pinocchio is free software: you can redistribute it
......@@ -19,7 +19,7 @@
#define __se3_joint_basic_visitors_hpp__
#include <Eigen/StdVector>
#include "pinocchio/multibody/joint/joint-variant.hpp"
#include "pinocchio/multibody/joint/joint-collection.hpp"
namespace se3
......
//
// Copyright (c) 2018 CNRS
//
// This file is part of Pinocchio
// Pinocchio is free software: you can redistribute it
// and/or modify it under the terljMj of the GNU Lesser General Public
// License as published by the Free Software Foundation, either version
// 3 of the License, or (at your option) any later version.
//
// Pinocchio is distributed in the hope that it will be
// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
// General Lesser Public License for more details. You should have
// received a copy of the GNU Lesser General Public License along with
// Pinocchio If not, see
// <http://www.gnu.org/licenses/>.
#ifndef __se3_joint_collection_hpp__
#define __se3_joint_collection_hpp__
#include "pinocchio/multibody/joint/fwd.hpp"
#include "pinocchio/multibody/joint/joint-free-flyer.hpp"
#include "pinocchio/multibody/joint/joint-planar.hpp"
#include "pinocchio/multibody/joint/joint-prismatic.hpp"
#include "pinocchio/multibody/joint/joint-prismatic-unaligned.hpp"
#include "pinocchio/multibody/joint/joint-revolute-unaligned.hpp"
#include "pinocchio/multibody/joint/joint-revolute.hpp"
#include "pinocchio/multibody/joint/joint-revolute-unbounded.hpp"
#include "pinocchio/multibody/joint/joint-spherical-ZYX.hpp"
#include "pinocchio/multibody/joint/joint-spherical.hpp"
#include "pinocchio/multibody/joint/joint-translation.hpp"
#include <boost/variant.hpp>
#include <boost/variant/recursive_wrapper.hpp>
namespace se3
{
template<typename _Scalar, int _Options>
struct JointCollectionDefaultTpl
{
typedef _Scalar Scalar;
enum { Options = _Options };
// Joint Revolute
typedef JointModelRevoluteTpl<Scalar,Options,0> JointModelRX;
typedef JointModelRevoluteTpl<Scalar,Options,1> JointModelRY;
typedef JointModelRevoluteTpl<Scalar,Options,2> JointModelRZ;
// Joint Revolute Unaligned
typedef JointModelRevoluteUnalignedTpl<Scalar,Options> JointModelRevoluteUnaligned;
// Joint Revolute UBounded
typedef JointModelRevoluteUnboundedTpl<Scalar,Options,0> JointModelRUBX;
typedef JointModelRevoluteUnboundedTpl<Scalar,Options,1> JointModelRUBY;
typedef JointModelRevoluteUnboundedTpl<Scalar,Options,2> JointModelRUBZ;
// Joint Prismatic
typedef JointModelPrismaticTpl<Scalar,Options,0> JointModelPX;
typedef JointModelPrismaticTpl<Scalar,Options,1> JointModelPY;
typedef JointModelPrismaticTpl<Scalar,Options,2> JointModelPZ;
// Joint Prismatic Unaligned
typedef JointModelPrismaticUnalignedTpl<Scalar,Options> JointModelPrismaticUnaligned;
// Joint Spherical
typedef JointModelSphericalTpl<Scalar,Options> JointModelSpherical;
// Joint Spherical ZYX
typedef JointModelSphericalZYXTpl<Scalar,Options> JointModelSphericalZYX;
// Joint Translation
typedef JointModelTranslationTpl<Scalar,Options> JointModelTranslation;
// Joint FreeFlyer
typedef JointModelFreeFlyerTpl<Scalar,Options> JointModelFreeFlyer;
// Joint Planar
typedef JointModelPlanarTpl<Scalar,Options> JointModelPlanar;
// Joint Composite
typedef JointModelCompositeTpl<JointCollectionDefaultTpl> JointModelComposite;
typedef boost::variant<
JointModelRX, JointModelRY, JointModelRZ
,JointModelRevoluteUnaligned, JointModelSpherical,
JointModelSphericalZYX, JointModelPX, JointModelPY, JointModelPZ,
JointModelPrismaticUnaligned, JointModelFreeFlyer, JointModelPlanar, JointModelTranslation,
JointModelRUBX, JointModelRUBY, JointModelRUBZ,
boost::recursive_wrapper<JointModelComposite>
> JointModelVariant;
};
typedef JointCollectionDefault::JointModelVariant JointModelVariant;
typedef boost::variant< JointDataRX, JointDataRY, JointDataRZ, JointDataRevoluteUnaligned, JointDataSpherical,
JointDataSphericalZYX, JointDataPX, JointDataPY, JointDataPZ, JointDataPrismaticUnaligned,
JointDataFreeFlyer, JointDataPlanar, JointDataTranslation,
JointDataRUBX, JointDataRUBY, JointDataRUBZ,
boost::recursive_wrapper<JointDataComposite> > JointDataVariant;
} // namespace se3
#endif // ifndef __se3_joint_collection_hpp__
//
// Copyright (c) 2016 CNRS
// Copyright (c) 2016,2018 CNRS
//
// This file is part of Pinocchio
// Pinocchio is free software: you can redistribute it
......@@ -19,7 +19,7 @@
#define __se3_joint_composite_hpp__
#include "pinocchio/multibody/joint/fwd.hpp"
#include "pinocchio/multibody/joint/joint-variant.hpp"
#include "pinocchio/multibody/joint/joint-collection.hpp"
#include "pinocchio/multibody/joint/joint-basic-visitors.hpp"
#include "pinocchio/container/aligned-vector.hpp"
#include "pinocchio/spatial/act-on-set.hpp"
......@@ -28,58 +28,67 @@
namespace se3
{
struct JointComposite;
template<typename JointCollection> struct JointCompositeTpl;
template<>
struct traits<JointComposite>
template<typename _JointCollection>
struct traits< JointCompositeTpl<_JointCollection> >
{
typedef _JointCollection JointCollection;
enum {
Options = JointCollection::Options,
NQ = Eigen::Dynamic,
NV = Eigen::Dynamic
};
typedef double Scalar;
typedef JointDataComposite JointDataDerived;
typedef JointModelComposite JointModelDerived;
typedef ConstraintXd Constraint_t;
typedef SE3 Transformation_t;
typedef Motion Motion_t;
typedef Motion Bias_t;
typedef Eigen::Matrix<double,6,Eigen::Dynamic> F_t;
typedef typename JointCollection::Scalar Scalar;
typedef JointDataCompositeTpl<JointCollection> JointDataDerived;
typedef JointModelCompositeTpl<JointCollection> JointModelDerived;
typedef ConstraintTpl<Eigen::Dynamic,Scalar,Options> Constraint_t;
typedef SE3Tpl<Scalar,Options> Transformation_t;
typedef MotionTpl<Scalar,Options> Motion_t;
typedef MotionTpl<Scalar,Options> Bias_t;
typedef Eigen::Matrix<Scalar,6,Eigen::Dynamic,Options> F_t;
// [ABA]
typedef Eigen::Matrix<double,6,Eigen::Dynamic> U_t;
typedef Eigen::Matrix<double,Eigen::Dynamic,Eigen::Dynamic> D_t;
typedef Eigen::Matrix<double,6,Eigen::Dynamic> UD_t;
typedef Eigen::Matrix<Scalar,6,Eigen::Dynamic,Options> U_t;
typedef Eigen::Matrix<Scalar,Eigen::Dynamic,Eigen::Dynamic,Options> D_t;
typedef Eigen::Matrix<Scalar,6,Eigen::Dynamic,Options> UD_t;
typedef Eigen::Matrix<double,Eigen::Dynamic,1> ConfigVector_t;
typedef Eigen::Matrix<double,Eigen::Dynamic,1> TangentVector_t;
typedef Eigen::Matrix<Scalar,Eigen::Dynamic,1,Options> ConfigVector_t;
typedef Eigen::Matrix<Scalar,Eigen::Dynamic,1,Options> TangentVector_t;
};
template<> struct traits< JointDataComposite > { typedef JointComposite JointDerived; };
template<> struct traits< JointModelComposite > { typedef JointComposite JointDerived; };
struct JointDataComposite : public JointDataBase< JointDataComposite >
template<typename JointCollection>
struct traits< JointModelCompositeTpl<JointCollection> >
{ typedef JointCompositeTpl<JointCollection> JointDerived; };
template<typename JointCollection>
struct traits< JointDataCompositeTpl<JointCollection> >
{ typedef JointCompositeTpl<JointCollection> JointDerived; };
template<typename JointCollection>
struct JointDataCompositeTpl : public JointDataBase< JointDataCompositeTpl<JointCollection> >
{
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
typedef JointDataBase<JointDataComposite> Base;
typedef JointComposite Joint;
// typedef typename JointCollection::JointDataVariant JointDataVariant;
typedef JointDataBase< JointDataCompositeTpl<JointCollection> > Base;
typedef container::aligned_vector<JointDataVariant> JointDataVector;
// typedef boost::array<JointDataVariant,njoints> JointDataVector;
typedef Base::Transformation_t Transformation_t;
typedef Base::Motion_t Motion_t;
typedef Base::Bias_t Bias_t;
typedef Base::Constraint_t Constraint_t;
typedef Base::U_t U_t;
typedef Base::D_t D_t;
typedef Base::UD_t UD_t;
typedef typename Base::Transformation_t Transformation_t;
typedef typename Base::Motion_t Motion_t;
typedef typename Base::Bias_t Bias_t;
typedef typename Base::Constraint_t Constraint_t;
typedef typename Base::U_t U_t;
typedef typename Base::D_t D_t;
typedef typename Base::UD_t UD_t;
// JointDataComposite() {} // can become necessary if we want a vector of JointDataComposite ?
JointDataComposite(const JointDataVector & joint_data, const int /*nq*/, const int nv)
JointDataCompositeTpl(const JointDataVector & joint_data, const int /*nq*/, const int nv)
: joints(joint_data), iMlast(joint_data.size()), pjMi(joint_data.size())
, S(nv)
, M(), v(), c()
......@@ -107,25 +116,32 @@ namespace se3
};
struct JointModelComposite : public JointModelBase< JointModelComposite >
template<typename JointCollection>
struct JointModelCompositeTpl : public JointModelBase< JointModelCompositeTpl<JointCollection> >
{
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
typedef typename JointCollection::JointModelVariant JointModelVariant;
typedef typename traits<JointModelCompositeTpl>::JointDerived JointType;
typedef typename traits<JointType>::JointDataDerived JointDataDerived;
enum
{
NV = traits<JointComposite>::NV,
NQ = traits<JointComposite>::NQ
Options = traits<JointType>::Options,
NV = traits<JointType>::NV,
NQ = traits<JointType>::NQ
};
typedef traits<JointComposite>::Scalar Scalar;
typedef JointModelBase<JointModelComposite> Base;
typedef JointDataComposite JointData;
typedef typename traits<JointType>::Scalar Scalar;
typedef SE3Tpl<Scalar,Options> SE3;
typedef JointModelBase< JointModelCompositeTpl<JointCollection> > Base;
typedef JointDataCompositeTpl<JointCollection> JointData;
typedef container::aligned_vector<JointModelVariant> JointModelVector;
// typedef boost::array<JointModelVariant,njoints> JointModelVector;
typedef traits<JointComposite>::Transformation_t Transformation_t;
typedef traits<JointComposite>::Constraint_t Constraint_t;
typedef traits<JointComposite>::ConfigVector_t ConfigVector_t;
typedef traits<JointComposite>::TangentVector_t TangentVector_t;
typedef typename traits<JointType>::Transformation_t Transformation_t;
typedef typename traits<JointType>::Constraint_t Constraint_t;
typedef typename traits<JointType>::ConfigVector_t ConfigVector_t;
typedef typename traits<JointType>::TangentVector_t TangentVector_t;
using Base::id;
using Base::idx_q;
......@@ -135,7 +151,7 @@ namespace se3
using Base::nv;
/// \brief Empty contructor
JointModelComposite()
JointModelCompositeTpl()
: joints()
, jointPlacements()
, m_nq(0)
......@@ -146,10 +162,10 @@ namespace se3
/// \brief Default constructor with at least one joint
///
/// \param jmodel Model of the first joint.
/// \param placement Placement of the first joint wrt the joint origin
/// \param placement Placement of the first joint w.r.t. the joint origin.
///
template<typename JointModel>
JointModelComposite(const JointModelBase<JointModel> & jmodel, const SE3 & placement = SE3::Identity())
JointModelCompositeTpl(const JointModelBase<JointModel> & jmodel, const SE3 & placement = SE3::Identity())
: joints(1,jmodel.derived())
, jointPlacements(1,placement)
, m_nq(jmodel.nq())
......@@ -163,7 +179,7 @@ namespace se3
///
/// \param other Model to copy.
///
JointModelComposite(const JointModelComposite & other)
JointModelCompositeTpl(const JointModelCompositeTpl & other)
: Base(other)
, joints(other.joints)
, jointPlacements(other.jointPlacements)
......@@ -193,22 +209,24 @@ namespace se3
JointData createData() const
{
JointData::JointDataVector jdata(joints.size());
typename JointData::JointDataVector jdata(joints.size());
for (int i = 0; i < (int)joints.size(); ++i)
jdata[(size_t)i] = ::se3::createData(joints[(size_t)i]);
return JointDataDerived(jdata,nq(),nv());
}
template<typename _JointCollection>
friend struct JointCompositeCalcZeroOrderStep;
void calc(JointData & data, const Eigen::VectorXd & qs) const;
template<typename _JointCollection>
friend struct JointCompositeCalcFirstOrderStep;
void calc(JointData & data, const Eigen::VectorXd & qs, const Eigen::VectorXd & vs) const;
void calc_aba(JointData & data, Inertia::Matrix6 & I, const bool update_I) const
{
data.U.noalias() = I * data.S;
Eigen::MatrixXd tmp (data.S.matrix().transpose() * data.U);
Eigen::Matrix<Scalar,Eigen::Dynamic,Eigen::Dynamic,Options> tmp (data.S.matrix().transpose() * data.U);
data.Dinv = tmp.inverse();
data.UDinv.noalias() = data.U * data.Dinv;
......@@ -220,16 +238,15 @@ namespace se3
{
using std::max;
Scalar eps = 0;
for(JointModelVector::const_iterator it = joints.begin();
for(typename JointModelVector::const_iterator it = joints.begin();
it != joints.end(); ++it)
eps = max((Scalar)::se3::finiteDifferenceIncrement(*it),eps);
return eps;
}
int nv_impl() const { return m_nv; }
int nq_impl() const { return m_nq; }
int nv_impl() const { return m_nv; }
int nq_impl() const { return m_nq; }
/**
* @brief Update the indexes of subjoints in the stack
......@@ -240,7 +257,7 @@ namespace se3
updateJointIndexes();
}
static std::string classname() { return std::string("JointModelComposite"); }
static std::string classname() { return std::string("JointModelCompositeTpl"); }
std::string shortname() const { return classname(); }
JointModelComposite & operator=(const JointModelComposite & other)
......@@ -264,46 +281,44 @@ namespace se3
template<typename D>
typename SizeDepType<NQ>::template SegmentReturn<D>::ConstType
jointConfigSelector(const Eigen::MatrixBase<D>& a) const { return a.segment(i_q,nq()); }
jointConfigSelector(const Eigen::MatrixBase<D>& a) const { return a.segment(Base::i_q,nq()); }
template<typename D>
typename SizeDepType<NQ>::template SegmentReturn<D>::Type
jointConfigSelector( Eigen::MatrixBase<D>& a) const { return a.segment(i_q,nq()); }
jointConfigSelector( Eigen::MatrixBase<D>& a) const { return a.segment(Base::i_q,nq()); }
template<typename D>
typename SizeDepType<NV>::template SegmentReturn<D>::ConstType
jointVelocitySelector(const Eigen::MatrixBase<D>& a) const { return a.segment(i_v,nv()); }
jointVelocitySelector(const Eigen::MatrixBase<D>& a) const { return a.segment(Base::i_v,nv()); }
template<typename D>
typename SizeDepType<NV>::template SegmentReturn<D>::Type
jointVelocitySelector( Eigen::MatrixBase<D>& a) const { return a.segment(i_v,nv()); }
jointVelocitySelector( Eigen::MatrixBase<D>& a) const { return a.segment(Base::i_v,nv()); }
template<typename D>
typename SizeDepType<NV>::template ColsReturn<D>::ConstType
jointCols(const Eigen::MatrixBase<D>& A) const { return A.middleCols(i_v,nv()); }
jointCols(const Eigen::MatrixBase<D>& A) const { return A.middleCols(Base::i_v,nv()); }
template<typename D>
typename SizeDepType<NV>::template ColsReturn<D>::Type
jointCols(Eigen::MatrixBase<D>& A) const { return A.middleCols(i_v,nv()); }
jointCols(Eigen::MatrixBase<D>& A) const { return A.middleCols(Base::i_v,nv()); }
template<typename D>
typename SizeDepType<Eigen::Dynamic>::template SegmentReturn<D>::ConstType
jointConfigSelector_impl(const Eigen::MatrixBase<D>& a) const { return a.segment(i_q,nq()); }
jointConfigSelector_impl(const Eigen::MatrixBase<D>& a) const { return a.segment(Base::i_q,nq()); }
template<typename D>
typename SizeDepType<Eigen::Dynamic>::template SegmentReturn<D>::Type
jointConfigSelector_impl(Eigen::MatrixBase<D>& a) const { return a.segment(i_q,nq()); }
jointConfigSelector_impl(Eigen::MatrixBase<D>& a) const { return a.segment(Base::i_q,nq()); }
template<typename D>
typename SizeDepType<Eigen::Dynamic>::template SegmentReturn<D>::ConstType
jointVelocitySelector_impl(const Eigen::MatrixBase<D>& a) const { return a.segment(i_v,nv()); }
jointVelocitySelector_impl(const Eigen::MatrixBase<D>& a) const { return a.segment(Base::i_v,nv()); }
template<typename D>
typename SizeDepType<Eigen::Dynamic>::template SegmentReturn<D>::Type
jointVelocitySelector_impl(Eigen::MatrixBase<D>& a) const { return a.segment(i_v,nv()); }
jointVelocitySelector_impl(Eigen::MatrixBase<D>& a) const { return a.segment(Base::i_v,nv()); }
template<typename D>
typename SizeDepType<Eigen::Dynamic>::template ColsReturn<D>::ConstType
jointCols_impl(const Eigen::MatrixBase<D>& A) const { return A.middleCols(i_v,nv()); }
jointCols_impl(const Eigen::MatrixBase<D>& A) const { return A.middleCols(Base::i_v,nv()); }
template<typename D>
typename SizeDepType<Eigen::Dynamic>::template ColsReturn<D>::Type
jointCols_impl(Eigen::MatrixBase<D>& A) const { return A.middleCols(i_v,nv()); }
jointCols_impl(Eigen::MatrixBase<D>& A) const { return A.middleCols(Base::i_v,nv()); }
protected:
......
//
// Copyright (c) 2016 CNRS
// Copyright (c) 2016,2018 CNRS
//
// This file is part of Pinocchio
// Pinocchio is free software: you can redistribute it
......@@ -23,20 +23,24 @@
namespace se3
{
struct JointCompositeCalcZeroOrderStep : public fusion::JointVisitor<JointCompositeCalcZeroOrderStep>
template<typename JointCollection>
struct JointCompositeCalcZeroOrderStep : public fusion::JointVisitor< JointCompositeCalcZeroOrderStep<JointCollection> >
{
typedef boost::fusion::vector<const se3::JointModelComposite &,
se3::JointDataComposite &,
typedef JointModelCompositeTpl<JointCollection> JointModelComposite;
typedef JointDataCompositeTpl<JointCollection> JointDataComposite;
typedef boost::fusion::vector<const JointModelComposite &,
JointDataComposite &,
const Eigen::VectorXd &
> ArgsType;
JOINT_VISITOR_INIT (JointCompositeCalcZeroOrderStep);
JOINT_VISITOR_INIT (JointCompositeCalcZeroOrderStep<JointCollection>);
template<typename JointModel>
static void algo(const se3::JointModelBase<JointModel> & jmodel,
se3::JointDataBase<typename JointModel::JointDataDerived> & jdata,
const se3::JointModelComposite & model,
se3::JointDataComposite & data,
const JointModelComposite & model,
JointDataComposite & data,
const Eigen::VectorXd & q)
{
const JointIndex & i = jmodel.id();
......@@ -62,36 +66,41 @@ namespace se3
}
};
inline void JointModelComposite::calc(JointData & data, const Eigen::VectorXd & qs) const
template<typename JointCollection>
inline void JointModelCompositeTpl<JointCollection>::calc(JointData & data, const Eigen::VectorXd & qs) const
{
assert(joints.size() > 0);
assert(data.joints.size() == joints.size());
typedef JointCompositeCalcZeroOrderStep<JointCollection> Algo;
for (int i=(int)(joints.size()-1); i >= 0; --i)
{
JointCompositeCalcZeroOrderStep::run(joints[(size_t)i], data.joints[(size_t)i],
JointCompositeCalcZeroOrderStep::ArgsType (*this,data,qs)
);
Algo::run(joints[(size_t)i], data.joints[(size_t)i],typename Algo::ArgsType(*this,data,qs));
}
data.M = data.iMlast.front();
}
struct JointCompositeCalcFirstOrderStep : public fusion::JointVisitor<JointCompositeCalcFirstOrderStep>
template<typename JointCollection>
struct JointCompositeCalcFirstOrderStep : public fusion::JointVisitor< JointCompositeCalcFirstOrderStep<JointCollection> >
{
typedef boost::fusion::vector<const se3::JointModelComposite &,
se3::JointDataComposite &,
typedef JointModelCompositeTpl<JointCollection> JointModelComposite;
typedef JointDataCompositeTpl<JointCollection> JointDataComposite;
typedef boost::fusion::vector<const JointModelComposite &,
JointDataComposite &,
const Eigen::VectorXd &,
const Eigen::VectorXd &
> ArgsType;
JOINT_VISITOR_INIT (JointCompositeCalcFirstOrderStep);
JOINT_VISITOR_INIT (JointCompositeCalcFirstOrderStep<JointCollection>);
template<typename JointModel>
static void algo(const se3::JointModelBase<JointModel> & jmodel,
se3::JointDataBase<typename JointModel::JointDataDerived> & jdata,
const se3::JointModelComposite & model,
se3::JointDataComposite & data,
const JointModelComposite & model,
JointDataComposite & data,
const Eigen::VectorXd & q,
const Eigen::VectorXd & v)
{
......@@ -128,17 +137,19 @@ namespace se3
};
inline void JointModelComposite::calc(JointData & data, const Eigen::VectorXd & qs, const Eigen::VectorXd & vs) const
template<typename JointCollection>
inline void JointModelCompositeTpl<JointCollection>::calc(JointData & data, const Eigen::VectorXd & qs, const Eigen::VectorXd & vs) const
{