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

[All] Continue templatization of ModelTpl and DataTpl

parent 084b56b1
......@@ -426,9 +426,13 @@ namespace se3
// Check whether all masses are nonzero and diagonal of inertia is nonzero
// The second test is overconstraining.
inline bool ABAChecker::checkModel_impl( const Model& model ) const
template<typename JointCollection>
inline bool ABAChecker::checkModel_impl(const ModelTpl<JointCollection> & model) const
{
for(JointIndex j=1;int(j)<model.njoints;j++)
typedef ModelTpl<JointCollection> Model;
typedef typename Model::JointIndex JointIndex;
for(JointIndex j=1;j<(JointIndex)model.njoints;j++)
if( (model.inertias[j].mass () < 1e-5)
|| (model.inertias[j].inertia().data()[0] < 1e-5)
|| (model.inertias[j].inertia().data()[3] < 1e-5)
......
......@@ -40,13 +40,16 @@ namespace se3
inline const AlgorithmCheckerDerived& derived() const
{ return *static_cast<const AlgorithmCheckerDerived*>(this); }
inline bool checkModel(const Model & model) const { return derived().checkModel_impl(model); }
template<typename JointCollection>
inline bool checkModel(const ModelTpl<JointCollection> & model) const
{ return derived().checkModel_impl(model); }
};
#define DEFINE_ALGO_CHECKER(NAME) \
struct NAME##Checker : public AlgorithmCheckerBase<NAME##Checker> \
{ \
inline bool checkModel_impl( const Model& ) const; \
#define DEFINE_ALGO_CHECKER(NAME) \
struct NAME##Checker : public AlgorithmCheckerBase<NAME##Checker> \
{ \
template<typename JointCollection> \
inline bool checkModel_impl(const ModelTpl<JointCollection> &) const; \
}
/// Simple model checker, that assert that model.parents is indeed a tree.
......@@ -64,7 +67,8 @@ namespace se3
// Calls model.check for each checker in the fusion::list.
// Each list element is supposed to implement the AlgorithmCheckerBase API.
bool checkModel_impl(const Model & model) const;
template<typename JointCollection>
bool checkModel_impl(const ModelTpl<JointCollection> & model) const;
const ArgType & checkerList;
};
......@@ -87,7 +91,8 @@ namespace se3
// Calls model.check for each checker in the fusion::list.
// Each list element is supposed to implement the AlgorithmCheckerBase API.
bool checkModel_impl(const Model & model) const;
template<typename JointCollection>
bool checkModel_impl(const ModelTpl<JointCollection> & model) const;
const ArgType & checkerList;
};
......@@ -106,7 +111,9 @@ namespace se3
/// \param[in] data corresponding data
///
/// \returns True if data is valid wrt model.
inline bool checkData(const Model & model, const Data & data);
template<typename JointCollection>
inline bool checkData(const ModelTpl<JointCollection> & model,
const DataTpl<JointCollection> & data);
} // namespace se3
......
......@@ -27,46 +27,62 @@ namespace se3
{
// Dedicated structure for the fusion::accumulate algorithm: validate the check-algorithm
// for all elements in a fusion list of AlgoCheckers.
template<typename JointCollection>
struct AlgoFusionChecker
{
typedef bool result_type;
const Model& model;
typedef ModelTpl<JointCollection> Model;
const Model & model;
AlgoFusionChecker(const Model&model) : model(model) {}
AlgoFusionChecker(const Model & model) : model(model) {}
inline bool operator()(const bool& accumul, const boost::fusion::void_ &) const
inline bool operator()(const bool & accumul, const boost::fusion::void_ &) const
{ return accumul; }
template<typename T>
inline bool operator()(const bool& accumul, const AlgorithmCheckerBase<T> & t) const
inline bool operator()(const bool & accumul, const AlgorithmCheckerBase<T> & t) const
{ return accumul && t.checkModel(model); }
};
} // namespace internal
// Check the validity of the kinematic tree defined by parents.
inline bool ParentChecker::checkModel_impl( const Model& model ) const
template<typename JointCollection>
inline bool ParentChecker::checkModel_impl(const ModelTpl<JointCollection> & model) const
{
for( JointIndex j=1;(int)j<model.njoints;++j )
if( model.parents[j]>=j ) return false;
typedef ModelTpl<JointCollection> Model;
typedef typename Model::JointIndex JointIndex;
for(JointIndex j=1;j<(JointIndex)model.njoints;++j)
if(model.parents[j]>=j)
return false;
return true;
}
#if !defined(BOOST_FUSION_HAS_VARIADIC_LIST)
template<BOOST_PP_ENUM_PARAMS(PINOCCHIO_ALGO_CHECKER_LIST_MAX_LIST_SIZE,class T)>
bool AlgorithmCheckerList<BOOST_PP_ENUM_PARAMS(PINOCCHIO_ALGO_CHECKER_LIST_MAX_LIST_SIZE,T)>::checkModel_impl(const Model& model) const
template<typename JointCollection>
bool AlgorithmCheckerList<BOOST_PP_ENUM_PARAMS(PINOCCHIO_ALGO_CHECKER_LIST_MAX_LIST_SIZE,T)>
::checkModel_impl(const ModelTpl<JointCollection> & model) const
{
return boost::fusion::accumulate(checkerList,true,internal::AlgoFusionChecker(model));
return boost::fusion::accumulate(checkerList,
true,
internal::AlgoFusionChecker<JointCollection>(model));
}
#else
template<class ...T>
bool AlgorithmCheckerList<T...>::checkModel_impl(const Model& model) const
template<typename JointCollection>
bool AlgorithmCheckerList<T...>::checkModel_impl(const ModelTpl<JointCollection> & model) const
{
return boost::fusion::accumulate(checkerList,true,internal::AlgoFusionChecker(model));
return boost::fusion::accumulate(checkerList,
true,
internal::AlgoFusionChecker<JointCollection>(model));
}
#endif
inline bool checkData(const Model & model, const Data & data)
template<typename JointCollection>
inline bool checkData(const ModelTpl<JointCollection> & model,
const DataTpl<JointCollection> & data)
{
#define CHECK_DATA(a) if(!(a)) return false;
......@@ -84,7 +100,10 @@ namespace se3
CHECK_DATA( (int)data.Ycrb.size() == model.njoints );
CHECK_DATA( (int)data.Yaba.size() == model.njoints );
CHECK_DATA( (int)data.Fcrb.size() == model.njoints );
BOOST_FOREACH(const Data::Matrix6x & F,data.Fcrb) CHECK_DATA( F.cols() == model.nv );
BOOST_FOREACH(const Data::Matrix6x & F,data.Fcrb)
{
CHECK_DATA( F.cols() == model.nv );
}
CHECK_DATA( (int)data.iMf.size() == model.njoints );
CHECK_DATA( (int)data.iMf.size() == model.njoints );
CHECK_DATA( (int)data.com.size() == model.njoints );
......@@ -143,7 +162,9 @@ namespace se3
return true;
}
inline bool Model::check(const Data & data) const { return checkData(*this,data); }
template<typename JointCollection>
inline bool ModelTpl<JointCollection>::check(const DataTpl<JointCollection> & data) const
{ return checkData(*this,data); }
} // namespace se3
......
......@@ -222,21 +222,31 @@ namespace se3
namespace internal
{
inline bool isDescendant(const Model& model, const JointIndex j, const JointIndex root)
template<typename JointCollection>
inline bool isDescendant(const ModelTpl<JointCollection> & model,
const typename ModelTpl<JointCollection>::JointIndex j,
const typename ModelTpl<JointCollection>::JointIndex root)
{
if(int(j)>=model.njoints) return false;
if(j==0) return root==0;
typedef ModelTpl<JointCollection> Model;
typedef typename Model::JointIndex JointIndex;
if(j>=(JointIndex)model.njoints) return false;
if(j==0) return root==0;
return (j==root) || isDescendant(model,model.parents[j],root);
}
}
inline bool CRBAChecker::checkModel_impl( const Model& model ) const
template<typename JointCollection>
inline bool CRBAChecker::checkModel_impl(const ModelTpl<JointCollection> & model) const
{
typedef ModelTpl<JointCollection> Model;
typedef typename Model::JointIndex JointIndex;
// For CRBA, the tree must be "compact", i.e. all descendants of a node i are stored
// immediately after i in the "parents" map, i.e. forall joint i, the interval i+1..n-1
// can be separated in two intervals [i+1..k] and [k+1..n-1], where any [i+1..k] is a descendant
// of i and none of [k+1..n-1] is a descendant of i.
for( JointIndex i=1; int(i)<model.njoints-1; ++i ) // no need to check joints 0 and N-1
for(JointIndex i=1; i < (JointIndex)(model.njoints-1); ++i) // no need to check joints 0 and N-1
{
JointIndex k=i+1;
while(internal::isDescendant(model,k,i)) ++k;
......
......@@ -30,10 +30,10 @@ namespace se3
#define DEFAULT_CHECKERS makeDefaultCheckerList()
inline bool Model::check() const { return this->check(DEFAULT_CHECKERS); }
template<typename JointCollection>
inline bool ModelTpl<JointCollection>::check() const
{ return this->check(DEFAULT_CHECKERS); }
} // namespace se3
#endif // ifndef __se3_default_check_hpp__
......@@ -34,19 +34,46 @@
namespace se3
{
struct Data
template<typename JointCollection>
struct DataTpl
{
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
enum { Options = JointCollection::Options };
typedef ModelTpl<JointCollection> Model;
typedef typename JointCollection::Scalar Scalar;
typedef SE3Tpl<Scalar,Options> SE3;
typedef MotionTpl<Scalar,Options> Motion;
typedef ForceTpl<Scalar,Options> Force;
typedef InertiaTpl<Scalar,Options> Inertia;
typedef FrameTpl<Scalar,Options> Frame;
typedef se3::Index Index;
typedef se3::JointIndex JointIndex;
typedef se3::GeomIndex GeomIndex;
typedef se3::FrameIndex FrameIndex;
typedef std::vector<Index> IndexVector;
typedef JointModelTpl<JointCollection> JointModel;
typedef JointDataTpl<JointCollection> JointData;
typedef container::aligned_vector<JointModel> JointModelVector;
typedef container::aligned_vector<JointData> JointDataVector;
typedef Eigen::Matrix<Scalar,Eigen::Dynamic,Eigen::Dynamic,Options> MatrixXs;
typedef Eigen::Matrix<Scalar,Eigen::Dynamic,1,Options> VectorXs;
typedef Eigen::Matrix<Scalar,3,1,Options> Vector3;
/// \brief The 6d jacobian type (temporary)
typedef Eigen::Matrix<double,6,Eigen::Dynamic> Matrix6x;
typedef Eigen::Matrix<Scalar,6,Eigen::Dynamic,Options> Matrix6x;
/// \brief The 3d jacobian type (temporary)
typedef Eigen::Matrix<double,3,Eigen::Dynamic> Matrix3x;
typedef SE3::Vector3 Vector3;
typedef Eigen::Matrix<Scalar,3,Eigen::Dynamic,Options> Matrix3x;
typedef Eigen::Matrix<double,6,6,Eigen::RowMajor> RowMatrix6;
typedef Eigen::Matrix<double,Eigen::Dynamic,Eigen::Dynamic,Eigen::RowMajor> RowMatrixXd;
typedef Eigen::Matrix<Scalar,6,6,Eigen::RowMajor | Options> RowMatrix6;
typedef Eigen::Matrix<Scalar,Eigen::Dynamic,Eigen::Dynamic,Eigen::RowMajor | Options> RowMatrixXd;
/// \brief Vector of se3::JointData associated to the se3::JointModel stored in model,
/// encapsulated in JointDataAccessor.
......@@ -88,17 +115,17 @@ namespace se3
container::aligned_vector<SE3> liMi;
/// \brief Vector of joint torques (dim model.nv).
Eigen::VectorXd tau;
VectorXs tau;
/// \brief Vector of Non Linear Effects (dim model.nv). It corresponds to concatenation of the Coriolis, centrifugal and gravitational effects.
/// \note In the multibody dynamics equation \f$ M\ddot{q} + b(q, \dot{q}) = \tau \f$,
/// the non linear effects are associated to the term \f$b\f$.
Eigen::VectorXd nle;
VectorXs nle;
/// \brief Vector of generalized gravity (dim model.nv).
/// \note In the multibody dynamics equation \f$ M\ddot{q} + c(q, \dot{q}) + g(q) = \tau \f$,
/// the gravity effect is associated to the \f$g\f$ term.
Eigen::VectorXd g;
VectorXs g;
/// \brief Vector of absolute operationnel frame placements (wrt the world).
container::aligned_vector<SE3> oMf;
......@@ -108,16 +135,16 @@ namespace se3
container::aligned_vector<Inertia> Ycrb;
/// \brief Vector of sub-tree composite rigid body inertia time derivatives \f$ \dot{Y}_{crb}\f$. See Data::Ycrb for more details.
container::aligned_vector<Inertia::Matrix6> dYcrb; // TODO: change with dense symmetric matrix6
container::aligned_vector<typename Inertia::Matrix6> dYcrb; // TODO: change with dense symmetric matrix6
/// \brief The joint space inertia matrix (a square matrix of dim model.nv).
Eigen::MatrixXd M;
MatrixXs M;
/// \brief The inverse of the joint space inertia matrix (a square matrix of dim model.nv).
RowMatrixXd Minv;
/// \brief The Coriolis matrix (a square matrix of dim model.nv).
Eigen::MatrixXd C;
MatrixXs C;
/// \brief Variation of the forceset with respect to the joint configuration.
Matrix6x dFdq;
......@@ -138,32 +165,32 @@ namespace se3
Matrix6x IS;
/// \brief Right variation of the inertia matrix
container::aligned_vector<Inertia::Matrix6> vxI;
container::aligned_vector<typename Inertia::Matrix6> vxI;
/// \brief Left variation of the inertia matrix
container::aligned_vector<Inertia::Matrix6> Ivx;
container::aligned_vector<typename Inertia::Matrix6> Ivx;
/// \brief Inertia quantities expressed in the world frame
container::aligned_vector<Inertia> oYcrb;
/// \brief Time variation of the inertia quantities expressed in the world frame
container::aligned_vector<Inertia::Matrix6> doYcrb;
container::aligned_vector<typename Inertia::Matrix6> doYcrb;
/// \brief Temporary for derivative algorithms
Inertia::Matrix6 Itmp;
typename Inertia::Matrix6 Itmp;
/// \brief Temporary for derivative algorithms
RowMatrix6 M6tmpR;
/// \brief The joint accelerations computed from ABA
Eigen::VectorXd ddq;
VectorXs ddq;
// ABA internal data
/// \brief Inertia matrix of the subtree expressed as dense matrix [ABA]
container::aligned_vector<Inertia::Matrix6> Yaba; // TODO: change with dense symmetric matrix6
container::aligned_vector<typename Inertia::Matrix6> Yaba; // TODO: change with dense symmetric matrix6
/// \brief Intermediate quantity corresponding to apparent torque [ABA]
Eigen::VectorXd u; // Joint Inertia
VectorXs u; // Joint Inertia
// CCRBA return quantities
/// \brief Centroidal Momentum Matrix
......@@ -177,9 +204,16 @@ namespace se3
/// \brief Centroidal momentum quantity.
/// \note The centroidal momentum is expressed in the frame centered at the CoM and aligned with the inertial frame (i.e. the world frame).
/// \note \f$ h_g = \left( m\dot{c}, L_{g} \right); \f$. \f$ h_g \f$ is the stack of the linear momentum and the angular momemtum vectors.
///
Force hg;
/// \brief Centroidal momentum time derivative.
/// \note The centroidal momentum time derivative is expressed in the frame centered at the CoM and aligned with the inertial frame (i.e. the world frame).
/// \note \f$ \dot{h_g} = \left( m\ddot{c}, \dot{L}_{g} \right); \f$. \f$ \dot{h_g} \f$ is the stack of the linear momentum variation and the angular momemtum variation.
///
Force dhg;
/// \brief Centroidal Composite Rigid Body Inertia.
/// \note \f$ hg = Ig v_{\text{mean}}\f$ map a mean velocity to the current centroil momentum quantity.
Inertia Ig;
......@@ -193,16 +227,16 @@ namespace se3
std::vector<int> nvSubtree;
/// \brief Joint space intertia matrix square root (upper trianglular part) computed with a Cholesky Decomposition.
Eigen::MatrixXd U;
MatrixXs U;
/// \brief Diagonal of the joint space intertia matrix obtained by a Cholesky Decomposition.
Eigen::VectorXd D;
VectorXs D;
/// \brief Diagonal inverse of the joint space intertia matrix obtained by a Cholesky Decomposition.
Eigen::VectorXd Dinv;
VectorXs Dinv;
/// \brief Temporary of size NV used in Cholesky Decomposition.
Eigen::VectorXd tmp;
VectorXs tmp;
/// \brief First previous non-zero row in M (used in Cholesky Decomposition).
std::vector<int> parents_fromRow;
......@@ -227,31 +261,31 @@ namespace se3
Matrix6x dAdv;
/// \brief Partial derivative of the joint torque vector with respect to the joint configuration.
Eigen::MatrixXd dtau_dq;
MatrixXs dtau_dq;
/// \brief Partial derivative of the joint torque vector with respect to the joint velocity.
Eigen::MatrixXd dtau_dv;
MatrixXs dtau_dv;
/// \brief Partial derivative of the joint acceleration vector with respect to the joint configuration.
Eigen::MatrixXd ddq_dq;
MatrixXs ddq_dq;
/// \brief Partial derivative of the joint acceleration vector with respect to the joint velocity.
Eigen::MatrixXd ddq_dv;
MatrixXs ddq_dv;
/// \brief Vector of joint placements wrt to algorithm end effector.
container::aligned_vector<SE3> iMf;
/// \brief Vector of subtree center of mass positions expressed in the root joint of the subtree. In other words, com[j] is the CoM position of the subtree supported by joint \f$ j \f$ and expressed in the joint frame \f$ j \f$. The element com[0] corresponds to the center of mass position of the whole model and expressed in the global frame.
container::aligned_vector<Eigen::Vector3d> com;
container::aligned_vector<Vector3> com;
/// \brief Vector of subtree center of mass linear velocities expressed in the root joint of the subtree. In other words, vcom[j] is the CoM linear velocity of the subtree supported by joint \f$ j \f$ and expressed in the joint frame \f$ j \f$. The element vcom[0] corresponds to the velocity of the CoM of the whole model expressed in the global frame.
container::aligned_vector<Eigen::Vector3d> vcom;
container::aligned_vector<Vector3> vcom;
/// \brief Vector of subtree center of mass linear accelerations expressed in the root joint of the subtree. In other words, acom[j] is the CoM linear acceleration of the subtree supported by joint \f$ j \f$ and expressed in the joint frame \f$ j \f$. The element acom[0] corresponds to the acceleration of the CoM of the whole model expressed in the global frame.
container::aligned_vector<Eigen::Vector3d> acom;
container::aligned_vector<Vector3> acom;
/// \brief Vector of subtree mass. In other words, mass[j] is the mass of the subtree supported by joint \f$ j \f$. The element mass[0] corrresponds to the total mass of the model.
std::vector<double> mass;
std::vector<Scalar> mass;
/// \brief Jacobien of center of mass.
/// \note This Jacobian maps the joint velocity vector to the velocity of the center of mass, expressed in the inertial frame. In other words, \f$ v_{\text{CoM}} = J_{\text{CoM}} \dot{q}\f$.
......@@ -259,33 +293,33 @@ namespace se3
/// \brief Kinetic energy of the model.
double kinetic_energy;
Scalar kinetic_energy;
/// \brief Potential energy of the model.
double potential_energy;
Scalar potential_energy;
// Temporary variables used in forward dynamics
/// \brief Inverse of the operational-space inertia matrix
Eigen::MatrixXd JMinvJt;
MatrixXs JMinvJt;
/// \brief Cholesky decompostion of \f$\JMinvJt\f$.
Eigen::LLT<Eigen::MatrixXd> llt_JMinvJt;
Eigen::LLT<MatrixXs> llt_JMinvJt;
/// \brief Lagrange Multipliers corresponding to the contact forces in se3::forwardDynamics.
Eigen::VectorXd lambda_c;
VectorXs lambda_c;
/// \brief Temporary corresponding to \f$ \sqrt{D} U^{-1} J^{\top} \f$.
Eigen::MatrixXd sDUiJt;
MatrixXs sDUiJt;
/// \brief Temporary corresponding to the residual torque \f$ \tau - b(q,\dot{q}) \f$.
Eigen::VectorXd torque_residual;
VectorXs torque_residual;
/// \brief Generalized velocity after impact.
Eigen::VectorXd dq_after;
VectorXs dq_after;
/// \brief Lagrange Multipliers corresponding to the contact impulses in se3::impulseDynamics.
Eigen::VectorXd impulse_c;
VectorXs impulse_c;
// data related to regressor
Matrix3x staticRegressor;
......@@ -295,11 +329,11 @@ namespace se3
///
/// \param[in] model The model structure of the rigid body system.
///
explicit Data(const Model & model);
explicit DataTpl(const Model & model);
private:
void computeLastChild(const Model& model);
void computeParents_fromRow(const Model& model);
void computeLastChild(const Model & model);
void computeParents_fromRow(const Model & model);
};
......
......@@ -32,7 +32,8 @@
namespace se3
{
inline Data::Data(const Model & model)
template<typename JointCollection>
inline DataTpl<JointCollection>::DataTpl(const ModelTpl<JointCollection> & model)
: joints(0)
, a((std::size_t)model.njoints)
, oa((std::size_t)model.njoints)
......@@ -102,17 +103,21 @@ namespace se3
, impulse_c()
, staticRegressor(3,4*(model.njoints-1))
{
typedef typename Model::JointIndex JointIndex;
/* Create data strcture associated to the joints */
for(Model::Index i=0;i<(Model::JointIndex)(model.njoints);++i)
joints.push_back(CreateJointData::run(model.joints[i]));
for(JointIndex i=0;i<(JointIndex)(model.njoints);++i)
joints.push_back(CreateJointData<JointCollection>::run(model.joints[i]));
/* Init for CRBA */
M.fill(0); Minv.setZero();
for(Model::Index i=0;i<(Model::Index)(model.njoints);++i ) { Fcrb[i].resize(6,model.nv); }
M.setZero(); Minv.setZero();
for(JointIndex i=0;i<(JointIndex)(model.njoints);++i)
{ Fcrb[i].resize(6,model.nv); }
computeLastChild(model);
/* Init for Coriolis */
C.fill(0.);
C.setZero();
/* Init for Cholesky */
U.setIdentity();
......@@ -136,9 +141,11 @@ namespace se3
oMf[0].setIdentity();
}
inline void Data::computeLastChild(const Model & model)
template<typename JointCollection>
inline void DataTpl<JointCollection>::computeLastChild(const ModelTpl<JointCollection> & model)
{
typedef Model::Index Index;
typedef typename Model::Index Index;
std::fill(lastChild.begin(),lastChild.end(),-1);
for( int i=model.njoints-1;i>=0;--i )
{
......@@ -152,22 +159,25 @@ namespace se3
}
}
inline void Data::computeParents_fromRow(const Model & model)
template<typename JointCollection>
inline void DataTpl<JointCollection>::computeParents_fromRow(const ModelTpl<JointCollection> & model)
{
for( Model::Index joint=1;joint<(Model::Index)(model.njoints);joint++)
typedef typename Model::Index Index;
for(Index joint=1;joint<(Index)(model.njoints);joint++)
{
const Model::Index & parent = model.parents[joint];
const Index & parent = model.parents[joint];
const int nvj = nv (model.joints[joint]);
const int idx_vj = idx_v(model.joints[joint]);
if(parent>0) parents_fromRow[(Model::Index)idx_vj] = idx_v(model.joints[parent])+nv(model.joints[parent])-1;
else parents_fromRow[(Model::Index)idx_vj] = -1;
nvSubtree_fromRow[(Model::Index)idx_vj] = nvSubtree[joint];
if(parent>0) parents_fromRow[(Index)idx_vj] = idx_v(model.joints[parent])+nv(model.joints[parent])-1;
else parents_fromRow[(Index)idx_vj] = -1;
nvSubtree_fromRow[(Index)idx_vj] = nvSubtree[joint];
for(int row=1;row<nvj;++row)
{
parents_fromRow[(Model::Index)(idx_vj+row)] = idx_vj+row-1;
nvSubtree_fromRow[(Model::Index)(idx_vj+row)] = nvSubtree[joint]-row;
parents_fromRow[(Index)(idx_vj+row)] = idx_vj+row-1;
nvSubtree_fromRow[(Index)(idx_vj+row)] = nvSubtree[joint]-row;
}
}
}
......
//
// Copyright (c) 2017 CNRS
// Copyright (c) 2017-2018 CNRS
//
// This file is part of Pinocchio
// Pinocchio is free software: you can redistribute it
......@@ -19,6 +19,7 @@
#define __se3_multibody_fwd_hpp__
# include <cstddef> // std::size_t
#include "pinocchio/multibody/joint/fwd.hpp"
namespace se3
{
......@@ -28,14 +29,18 @@ namespace se3
typedef Index FrameIndex;
typedef Index PairIndex;
template<typename _Scalar, int _Options=0> struct FrameTpl;
struct Model;
struct Data;
template<typename Scalar, int Options=0> struct FrameTpl;
typedef FrameTpl<double> Frame;
template<typename JointCollection> struct ModelTpl;
typedef ModelTpl<JointCollectionDefault> Model;
template<typename JointCollection> struct DataTpl;
typedef DataTpl<JointCollectionDefault> Data;
struct GeometryModel;
struct GeometryData;
typedef FrameTpl<double> Frame;