Verified Commit 8eace9aa authored by Justin Carpentier's avatar Justin Carpentier Committed by Justin Carpentier
Browse files

[Joint/JointTpl] Improve templatization of Joint{Model,Data}Tpl

parent 29f48d44
...@@ -41,10 +41,10 @@ namespace se3 ...@@ -41,10 +41,10 @@ namespace se3
EIGEN_MAKE_ALIGNED_OPERATOR_NEW EIGEN_MAKE_ALIGNED_OPERATOR_NEW
enum { Options = JointCollection::Options }; enum { Options = JointCollection::Options };
typedef typename JointCollection::Scalar Scalar;
typedef ModelTpl<JointCollection> Model; typedef ModelTpl<JointCollection> Model;
typedef typename JointCollection::Scalar Scalar;
typedef SE3Tpl<Scalar,Options> SE3; typedef SE3Tpl<Scalar,Options> SE3;
typedef MotionTpl<Scalar,Options> Motion; typedef MotionTpl<Scalar,Options> Motion;
typedef ForceTpl<Scalar,Options> Force; typedef ForceTpl<Scalar,Options> Force;
...@@ -57,8 +57,8 @@ namespace se3 ...@@ -57,8 +57,8 @@ namespace se3
typedef se3::FrameIndex FrameIndex; typedef se3::FrameIndex FrameIndex;
typedef std::vector<Index> IndexVector; typedef std::vector<Index> IndexVector;
typedef JointModelTpl<JointCollection> JointModel; typedef JointModelTpl<Scalar,Options,JointCollectionDefaultTpl> JointModel;
typedef JointDataTpl<JointCollection> JointData; typedef JointDataTpl<Scalar,Options,JointCollectionDefaultTpl> JointData;
typedef container::aligned_vector<JointModel> JointModelVector; typedef container::aligned_vector<JointModel> JointModelVector;
typedef container::aligned_vector<JointData> JointDataVector; typedef container::aligned_vector<JointData> JointDataVector;
......
...@@ -107,7 +107,7 @@ namespace se3 ...@@ -107,7 +107,7 @@ namespace se3
/* Create data strcture associated to the joints */ /* Create data strcture associated to the joints */
for(JointIndex i=0;i<(JointIndex)(model.njoints);++i) for(JointIndex i=0;i<(JointIndex)(model.njoints);++i)
joints.push_back(CreateJointData<JointCollection>::run(model.joints[i])); joints.push_back(CreateJointData<Scalar,Options,JointCollectionDefaultTpl>::run(model.joints[i]));
/* Init for CRBA */ /* Init for CRBA */
M.setZero(); Minv.setZero(); M.setZero(); Minv.setZero();
......
...@@ -86,11 +86,13 @@ namespace se3 ...@@ -86,11 +86,13 @@ namespace se3
struct JointDataCompositeTpl; struct JointDataCompositeTpl;
typedef JointDataCompositeTpl<double> JointDataComposite; typedef JointDataCompositeTpl<double> JointDataComposite;
template<typename JointCollection> struct JointModelTpl; template<typename Scalar, int Options = 0, template<typename S, int O> class JointCollectionTpl = JointCollectionDefaultTpl>
typedef JointModelTpl<JointCollectionDefault> JointModel; struct JointModelTpl;
typedef JointModelTpl<double> JointModel;
template<typename JointCollection> struct JointDataTpl; template<typename Scalar, int Options = 0, template<typename S, int O> class JointCollectionTpl = JointCollectionDefaultTpl>
typedef JointDataTpl<JointCollectionDefault> JointData; struct JointDataTpl;
typedef JointDataTpl<double> JointData;
} }
......
...@@ -32,9 +32,9 @@ namespace se3 ...@@ -32,9 +32,9 @@ namespace se3
* *
* @return The created JointDataTpl * @return The created JointDataTpl
*/ */
template<typename JointCollection> template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
inline JointDataTpl<JointCollection> inline JointDataTpl<Scalar,Options,JointCollectionTpl>
createData(const JointModelTpl<JointCollection> & jmodel); createData(const JointModelTpl<Scalar,Options,JointCollectionTpl> & jmodel);
/** /**
...@@ -48,9 +48,9 @@ namespace se3 ...@@ -48,9 +48,9 @@ namespace se3
* @param jdata The JointDataVariant we want to update * @param jdata The JointDataVariant we want to update
* @param[in] q The full model's (in which the joint belongs to) configuration vector * @param[in] q The full model's (in which the joint belongs to) configuration vector
*/ */
template<typename JointCollection, typename ConfigVectorType> template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl, typename ConfigVectorType>
inline void calc_zero_order(const JointModelTpl<JointCollection> & jmodel, inline void calc_zero_order(const JointModelTpl<Scalar,Options,JointCollectionTpl> & jmodel,
JointDataTpl<JointCollection> & jdata, JointDataTpl<Scalar,Options,JointCollectionTpl> & jdata,
const Eigen::MatrixBase<ConfigVectorType> & q); const Eigen::MatrixBase<ConfigVectorType> & q);
/** /**
...@@ -65,9 +65,9 @@ namespace se3 ...@@ -65,9 +65,9 @@ namespace se3
* @param jdata The JointDataVariant we want to update * @param jdata The JointDataVariant we want to update
* @param[in] q The full model's (in which the joint belongs to) configuration vector * @param[in] q The full model's (in which the joint belongs to) configuration vector
*/ */
template<typename JointCollection, typename ConfigVectorType, typename TangentVectorType> template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType>
inline void calc_first_order(const JointModelTpl<JointCollection> & jmodel, inline void calc_first_order(const JointModelTpl<Scalar,Options,JointCollectionTpl> & jmodel,
JointDataTpl<JointCollection> & jdata, JointDataTpl<Scalar,Options,JointCollectionTpl> & jdata,
const Eigen::MatrixBase<ConfigVectorType> & q, const Eigen::MatrixBase<ConfigVectorType> & q,
const Eigen::MatrixBase<TangentVectorType> & v); const Eigen::MatrixBase<TangentVectorType> & v);
...@@ -83,9 +83,9 @@ namespace se3 ...@@ -83,9 +83,9 @@ namespace se3
* @param[inout] I Inertia matrix of the subtree following the jmodel in the kinematic chain as dense matrix * @param[inout] I Inertia matrix of the subtree following the jmodel in the kinematic chain as dense matrix
* @param[in] update_I If I should be updated or not * @param[in] update_I If I should be updated or not
*/ */
template<typename JointCollection, typename Matrix6Type> template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl, typename Matrix6Type>
inline void calc_aba(const JointModelTpl<JointCollection> & jmodel, inline void calc_aba(const JointModelTpl<Scalar,Options,JointCollectionTpl> & jmodel,
JointDataTpl<JointCollection> & jdata, JointDataTpl<Scalar,Options,JointCollectionTpl> & jdata,
const Eigen::MatrixBase<Matrix6Type> & I, const Eigen::MatrixBase<Matrix6Type> & I,
const bool update_I); const bool update_I);
...@@ -96,9 +96,9 @@ namespace se3 ...@@ -96,9 +96,9 @@ namespace se3
/// ///
/// \returns The finite diffrence increment. /// \returns The finite diffrence increment.
/// ///
template<typename JointCollection> template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
inline typename JointCollection::Scalar inline Scalar
finiteDifferenceIncrement(const JointModelTpl<JointCollection> & jmodel); finiteDifferenceIncrement(const JointModelTpl<Scalar,Options,JointCollectionTpl> & jmodel);
/** /**
* @brief Visit a JointModelVariant through JointNvVisitor to get the dimension of * @brief Visit a JointModelVariant through JointNvVisitor to get the dimension of
...@@ -108,8 +108,8 @@ namespace se3 ...@@ -108,8 +108,8 @@ namespace se3
* *
* @return The dimension of joint tangent space * @return The dimension of joint tangent space
*/ */
template<typename JointCollection> template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
inline int nv(const JointModelTpl<JointCollection> & jmodel); inline int nv(const JointModelTpl<Scalar,Options,JointCollectionTpl> & jmodel);
...@@ -121,8 +121,8 @@ namespace se3 ...@@ -121,8 +121,8 @@ namespace se3
* *
* @return The dimension of joint configuration space * @return The dimension of joint configuration space
*/ */
template<typename JointCollection> template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
inline int nq(const JointModelTpl<JointCollection> & jmodel); inline int nq(const JointModelTpl<Scalar,Options,JointCollectionTpl> & jmodel);
/** /**
...@@ -134,8 +134,8 @@ namespace se3 ...@@ -134,8 +134,8 @@ namespace se3
* @return The index in the full model configuration space corresponding to the first * @return The index in the full model configuration space corresponding to the first
* degree of freedom of jmodel * degree of freedom of jmodel
*/ */
template<typename JointCollection> template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
inline int idx_q(const JointModelTpl<JointCollection> & jmodel); inline int idx_q(const JointModelTpl<Scalar,Options,JointCollectionTpl> & jmodel);
/** /**
...@@ -147,8 +147,8 @@ namespace se3 ...@@ -147,8 +147,8 @@ namespace se3
* @return The index in the full model tangent space corresponding to the first * @return The index in the full model tangent space corresponding to the first
* joint tangent space degree * joint tangent space degree
*/ */
template<typename JointCollection> template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
inline int idx_v(const JointModelTpl<JointCollection> & jmodel); inline int idx_v(const JointModelTpl<Scalar,Options,JointCollectionTpl> & jmodel);
/** /**
...@@ -158,8 +158,8 @@ namespace se3 ...@@ -158,8 +158,8 @@ namespace se3
* *
* @return The index of the joint in the kinematic chain * @return The index of the joint in the kinematic chain
*/ */
template<typename JointCollection> template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
inline JointIndex id(const JointModelTpl<JointCollection> & jmodel); inline JointIndex id(const JointModelTpl<Scalar,Options,JointCollectionTpl> & jmodel);
/** /**
* @brief Visit a JointModelVariant through JointSetIndexesVisitor to set * @brief Visit a JointModelVariant through JointSetIndexesVisitor to set
...@@ -172,8 +172,8 @@ namespace se3 ...@@ -172,8 +172,8 @@ namespace se3
* *
* @return The index of the joint in the kinematic chain * @return The index of the joint in the kinematic chain
*/ */
template<typename JointCollection> template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
inline void setIndexes(JointModelTpl<JointCollection> & jmodel, JointIndex id, int q,int v); inline void setIndexes(JointModelTpl<Scalar,Options,JointCollectionTpl> & jmodel, JointIndex id, int q,int v);
/** /**
...@@ -181,8 +181,8 @@ namespace se3 ...@@ -181,8 +181,8 @@ namespace se3
* *
* @param jmodel The JointModelVariant we want the shortname of the type held in * @param jmodel The JointModelVariant we want the shortname of the type held in
*/ */
template<typename JointCollection> template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
inline std::string shortname(const JointModelTpl<JointCollection> & jmodel); inline std::string shortname(const JointModelTpl<Scalar,Options,JointCollectionTpl> & jmodel);
// //
...@@ -198,9 +198,9 @@ namespace se3 ...@@ -198,9 +198,9 @@ namespace se3
* *
* @return The constraint dense corresponding to the joint derived constraint * @return The constraint dense corresponding to the joint derived constraint
*/ */
template<typename JointCollection> template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
inline ConstraintTpl<Eigen::Dynamic,typename JointCollection::Scalar, JointCollection::Options> inline ConstraintTpl<Eigen::Dynamic,Scalar,Options>
constraint_xd(const JointDataTpl<JointCollection> & jdata); constraint_xd(const JointDataTpl<Scalar,Options,JointCollectionTpl> & jdata);
/** /**
* @brief Visit a JointDataVariant through JointTransformVisitor to get the joint internal transform (transform * @brief Visit a JointDataVariant through JointTransformVisitor to get the joint internal transform (transform
...@@ -210,9 +210,9 @@ namespace se3 ...@@ -210,9 +210,9 @@ namespace se3
* *
* @return The joint transform corresponding to the joint derived transform (sXp) * @return The joint transform corresponding to the joint derived transform (sXp)
*/ */
template<typename JointCollection> template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
inline SE3Tpl<typename JointCollection::Scalar,JointCollection::Options> inline SE3Tpl<Scalar,Options>
joint_transform(const JointDataTpl<JointCollection> & jdata); joint_transform(const JointDataTpl<Scalar,Options,JointCollectionTpl> & jdata);
/** /**
* @brief Visit a JointDataVariant through JointMotionVisitor to get the joint internal motion * @brief Visit a JointDataVariant through JointMotionVisitor to get the joint internal motion
...@@ -222,9 +222,9 @@ namespace se3 ...@@ -222,9 +222,9 @@ namespace se3
* *
* @return The motion dense corresponding to the joint derived motion * @return The motion dense corresponding to the joint derived motion
*/ */
template<typename JointCollection> template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
inline MotionTpl<typename JointCollection::Scalar,JointCollection::Options> inline MotionTpl<Scalar,Options>
motion(const JointDataTpl<JointCollection> & jdata); motion(const JointDataTpl<Scalar,Options,JointCollectionTpl> & jdata);
/** /**
* @brief Visit a JointDataVariant through JointBiasVisitor to get the joint bias * @brief Visit a JointDataVariant through JointBiasVisitor to get the joint bias
...@@ -234,9 +234,9 @@ namespace se3 ...@@ -234,9 +234,9 @@ namespace se3
* *
* @return The motion dense corresponding to the joint derived bias * @return The motion dense corresponding to the joint derived bias
*/ */
template<typename JointCollection> template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
inline MotionTpl<typename JointCollection::Scalar,JointCollection::Options> inline MotionTpl<Scalar,Options>
bias(const JointDataTpl<JointCollection> & jdata); bias(const JointDataTpl<Scalar,Options,JointCollectionTpl> & jdata);
/** /**
* @brief Visit a JointDataVariant through JointUInertiaVisitor to get the U matrix of the inertia matrix * @brief Visit a JointDataVariant through JointUInertiaVisitor to get the U matrix of the inertia matrix
...@@ -246,9 +246,9 @@ namespace se3 ...@@ -246,9 +246,9 @@ namespace se3
* *
* @return The U matrix of the inertia matrix decomposition * @return The U matrix of the inertia matrix decomposition
*/ */
template<typename JointCollection> template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
inline Eigen::Matrix<typename JointCollection::Scalar,6,Eigen::Dynamic,JointCollection::Options> inline Eigen::Matrix<Scalar,6,Eigen::Dynamic,Options>
u_inertia(const JointDataTpl<JointCollection> & jdata); u_inertia(const JointDataTpl<Scalar,Options,JointCollectionTpl> & jdata);
/** /**
* @brief Visit a JointDataVariant through JointDInvInertiaVisitor to get the D^{-1} matrix of the inertia matrix * @brief Visit a JointDataVariant through JointDInvInertiaVisitor to get the D^{-1} matrix of the inertia matrix
...@@ -258,9 +258,9 @@ namespace se3 ...@@ -258,9 +258,9 @@ namespace se3
* *
* @return The D^{-1} matrix of the inertia matrix decomposition * @return The D^{-1} matrix of the inertia matrix decomposition
*/ */
template<typename JointCollection> template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
inline Eigen::Matrix<typename JointCollection::Scalar,Eigen::Dynamic,Eigen::Dynamic,JointCollection::Options> inline Eigen::Matrix<Scalar,Eigen::Dynamic,Eigen::Dynamic,Options>
dinv_inertia(const JointDataTpl<JointCollection> & jdata); dinv_inertia(const JointDataTpl<Scalar,Options,JointCollectionTpl> & jdata);
/** /**
* @brief Visit a JointDataVariant through JointUDInvInertiaVisitor to get U*D^{-1} matrix of the inertia matrix * @brief Visit a JointDataVariant through JointUDInvInertiaVisitor to get U*D^{-1} matrix of the inertia matrix
...@@ -270,9 +270,9 @@ namespace se3 ...@@ -270,9 +270,9 @@ namespace se3
* *
* @return The U*D^{-1} matrix of the inertia matrix decomposition * @return The U*D^{-1} matrix of the inertia matrix decomposition
*/ */
template<typename JointCollection> template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
inline Eigen::Matrix<typename JointCollection::Scalar,6,Eigen::Dynamic,JointCollection::Options> inline Eigen::Matrix<Scalar,6,Eigen::Dynamic,Options>
udinv_inertia(const JointDataTpl<JointCollection> & jdata); udinv_inertia(const JointDataTpl<Scalar,Options,JointCollectionTpl> & jdata);
} // namespace se3 } // namespace se3
......
...@@ -67,29 +67,21 @@ namespace se3 ...@@ -67,29 +67,21 @@ namespace se3
struct traits< JointDataCompositeTpl<Scalar,Options,JointCollectionTpl> > struct traits< JointDataCompositeTpl<Scalar,Options,JointCollectionTpl> >
{ typedef JointCompositeTpl<Scalar,Options,JointCollectionTpl> JointDerived; }; { typedef JointCompositeTpl<Scalar,Options,JointCollectionTpl> JointDerived; };
template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl> template<typename _Scalar, int _Options, template<typename S, int O> class JointCollectionTpl>
struct JointDataCompositeTpl struct JointDataCompositeTpl
: public JointDataBase< JointDataCompositeTpl<Scalar,Options,JointCollectionTpl> > : public JointDataBase< JointDataCompositeTpl<_Scalar,_Options,JointCollectionTpl> >
{ {
EIGEN_MAKE_ALIGNED_OPERATOR_NEW EIGEN_MAKE_ALIGNED_OPERATOR_NEW
typedef JointDataBase< JointDataCompositeTpl<Scalar,Options,JointCollectionTpl> > Base; typedef JointDataBase<JointDataCompositeTpl> Base;
typedef JointCollectionTpl<Scalar,Options> JointCollection; typedef JointCompositeTpl<_Scalar,_Options,JointCollectionTpl> JointDerived;
SE3_JOINT_TYPEDEF_TEMPLATE;
typedef JointDataTpl<JointCollection> JointDataVariant;
// typedef typename JointCollection::JointDataVariant JointDataVariant;
typedef JointCollectionTpl<Scalar,Options> JointCollection;
typedef JointDataTpl<Scalar,Options,JointCollectionTpl> JointDataVariant;
typedef container::aligned_vector<JointDataVariant> JointDataVector; typedef container::aligned_vector<JointDataVariant> JointDataVector;
// typedef boost::array<JointDataVariant,njoints> JointDataVector;
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() {} // can become necessary if we want a vector of JointDataComposite ?
JointDataCompositeTpl(const JointDataVector & joint_data, const int /*nq*/, const int nv) JointDataCompositeTpl(const JointDataVector & joint_data, const int /*nq*/, const int nv)
...@@ -126,30 +118,16 @@ namespace se3 ...@@ -126,30 +118,16 @@ namespace se3
{ {
EIGEN_MAKE_ALIGNED_OPERATOR_NEW EIGEN_MAKE_ALIGNED_OPERATOR_NEW
typedef typename traits<JointModelCompositeTpl>::JointDerived JointType;
typedef typename traits<JointType>::JointDataDerived JointData;
typedef typename traits<JointType>::Scalar Scalar;
typedef JointModelBase<JointModelCompositeTpl> Base; typedef JointModelBase<JointModelCompositeTpl> Base;
typedef JointCompositeTpl<_Scalar,_Options,JointCollectionTpl> JointDerived;
enum SE3_JOINT_TYPEDEF_TEMPLATE;
{
Options = traits<JointType>::Options,
NV = traits<JointType>::NV,
NQ = traits<JointType>::NQ
};
typedef JointCollectionTpl<Scalar,Options> JointCollection; typedef JointCollectionTpl<Scalar,Options> JointCollection;
typedef JointModelTpl<JointCollection> JointModelVariant; typedef JointModelTpl<Scalar,Options,JointCollectionTpl> JointModelVariant;
// typedef typename JointCollection::JointModelVariant JointModelVariant;
typedef SE3Tpl<Scalar,Options> SE3; typedef SE3Tpl<Scalar,Options> SE3;
typedef container::aligned_vector<JointModelVariant> JointModelVector; typedef container::aligned_vector<JointModelVariant> JointModelVector;
// typedef boost::array<JointModelVariant,njoints> JointModelVector;
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::id;
using Base::idx_q; using Base::idx_q;
...@@ -216,29 +194,29 @@ namespace se3 ...@@ -216,29 +194,29 @@ namespace se3
updateJointIndexes(); updateJointIndexes();
} }
JointData createData() const JointDataDerived createData() const
{ {
typename JointData::JointDataVector jdata(joints.size()); typename JointDataDerived::JointDataVector jdata(joints.size());
for (int i = 0; i < (int)joints.size(); ++i) for (int i = 0; i < (int)joints.size(); ++i)
jdata[(size_t)i] = ::se3::createData<JointCollection>(joints[(size_t)i]); jdata[(size_t)i] = ::se3::createData<Scalar,Options,JointCollectionTpl>(joints[(size_t)i]);
return JointData(jdata,nq(),nv()); return JointDataDerived(jdata,nq(),nv());
} }
template<typename, int, template<typename S, int O> class, typename> template<typename, int, template<typename S, int O> class, typename>
friend struct JointCompositeCalcZeroOrderStep; friend struct JointCompositeCalcZeroOrderStep;
template<typename ConfigVectorType> template<typename ConfigVectorType>
void calc(JointData & data, const Eigen::MatrixBase<ConfigVectorType> & qs) const; void calc(JointDataDerived & data, const Eigen::MatrixBase<ConfigVectorType> & qs) const;
template<typename, int, template<typename S, int O> class, typename, typename> template<typename, int, template<typename S, int O> class, typename, typename>
friend struct JointCompositeCalcFirstOrderStep; friend struct JointCompositeCalcFirstOrderStep;
template<typename ConfigVectorType, typename TangentVectorType> template<typename ConfigVectorType, typename TangentVectorType>
void calc(JointData & data, void calc(JointDataDerived & data,
const Eigen::MatrixBase<ConfigVectorType> & qs, const Eigen::MatrixBase<ConfigVectorType> & qs,
const Eigen::MatrixBase<TangentVectorType> & vs) const; const Eigen::MatrixBase<TangentVectorType> & vs) const;
void calc_aba(JointData & data, Inertia::Matrix6 & I, const bool update_I) const void calc_aba(JointDataDerived & data, Inertia::Matrix6 & I, const bool update_I) const
{ {
data.U.noalias() = I * data.S; data.U.noalias() = I * data.S;
Eigen::Matrix<Scalar,Eigen::Dynamic,Eigen::Dynamic,Options> tmp (data.S.matrix().transpose() * data.U); Eigen::Matrix<Scalar,Eigen::Dynamic,Eigen::Dynamic,Options> tmp (data.S.matrix().transpose() * data.U);
......
...@@ -69,7 +69,7 @@ namespace se3 ...@@ -69,7 +69,7 @@ namespace se3
template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl> template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
template<typename ConfigVectorType> template<typename ConfigVectorType>
inline void JointModelCompositeTpl<Scalar,Options,JointCollectionTpl>:: inline void JointModelCompositeTpl<Scalar,Options,JointCollectionTpl>::
calc(JointData & data, const Eigen::MatrixBase<ConfigVectorType> & qs) const calc(JointDataDerived & data, const Eigen::MatrixBase<ConfigVectorType> & qs) const
{ {
assert(joints.size() > 0); assert(joints.size() > 0);
assert(data.joints.size() == joints.size()); assert(data.joints.size() == joints.size());
...@@ -142,7 +142,7 @@ namespace se3 ...@@ -142,7 +142,7 @@ namespace se3
template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl> template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
template<typename ConfigVectorType, typename TangentVectorType> template<typename ConfigVectorType, typename TangentVectorType>
inline void JointModelCompositeTpl<Scalar,Options,JointCollectionTpl> inline void JointModelCompositeTpl<Scalar,Options,JointCollectionTpl>
::calc(JointData & jdata, ::calc(JointDataDerived & jdata,
const Eigen::MatrixBase<ConfigVectorType> & qs, const Eigen::MatrixBase<ConfigVectorType> & qs,
const Eigen::MatrixBase<TangentVectorType> & vs) const const Eigen::MatrixBase<TangentVectorType> & vs) const
{ {
......
...@@ -29,20 +29,24 @@ ...@@ -29,20 +29,24 @@
namespace se3 namespace se3
{ {
template<typename JointCollection> struct JointTpl; template<typename Scalar, int Options = 0, template<typename S, int O> class JointCollectionTpl = JointCollectionDefaultTpl>
typedef JointTpl<JointCollectionDefault> Joint; struct JointTpl;
typedef JointTpl<double> Joint;
template<typename JointCollection> template<typename _Scalar, int _Options, template<typename S, int O> class JointCollectionTpl>
struct traits< JointTpl<JointCollection> > struct traits< JointTpl<_Scalar,_Options,JointCollectionTpl> >
{