Verified Commit 29f48d44 authored by Justin Carpentier's avatar Justin Carpentier Committed by Justin Carpentier
Browse files

[Joint/JointComposite] Improve templatization of JointComposite

It now takes Scalar and Options as input template parameters
parent 61d40814
......@@ -78,11 +78,13 @@ namespace se3
template<typename Scalar, int Options = 0> struct JointCollectionDefaultTpl;
typedef JointCollectionDefaultTpl<double> JointCollectionDefault;
template<typename JointCollection> struct JointModelCompositeTpl;
typedef JointModelCompositeTpl< JointCollectionDefaultTpl<double> > JointModelComposite;
template<typename Scalar, int Options = 0, template<typename S, int O> class JointCollectionTpl = JointCollectionDefaultTpl>
struct JointModelCompositeTpl;
typedef JointModelCompositeTpl<double> JointModelComposite;
template<typename JointCollection> struct JointDataCompositeTpl;
typedef JointDataCompositeTpl< JointCollectionDefaultTpl<double> > JointDataComposite;
template<typename Scalar, int Options = 0, template<typename S, int O> class JointCollectionTpl = JointCollectionDefaultTpl>
struct JointDataCompositeTpl;
typedef JointDataCompositeTpl<double> JointDataComposite;
template<typename JointCollection> struct JointModelTpl;
typedef JointModelTpl<JointCollectionDefault> JointModel;
......
......@@ -79,7 +79,7 @@ namespace se3
typedef JointModelPlanarTpl<Scalar,Options> JointModelPlanar;
// Joint Composite
typedef JointModelCompositeTpl<JointCollectionDefaultTpl> JointModelComposite;
typedef JointModelCompositeTpl<Scalar,Options,JointCollectionDefaultTpl> JointModelComposite;
typedef boost::variant<
JointModelRX, JointModelRY, JointModelRZ
......@@ -131,7 +131,7 @@ namespace se3
typedef JointDataPlanarTpl<Scalar,Options> JointDataPlanar;
// Joint Composite
typedef JointDataCompositeTpl<JointCollectionDefaultTpl> JointDataComposite;
typedef JointDataCompositeTpl<Scalar,Options,JointCollectionDefaultTpl> JointDataComposite;
typedef boost::variant<
JointDataRX, JointDataRY, JointDataRZ
......
......@@ -27,22 +27,23 @@
namespace se3
{
template<typename JointCollection> struct JointCompositeTpl;
template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
struct JointCompositeTpl;
template<typename _JointCollection>
struct traits< JointCompositeTpl<_JointCollection> >
template<typename _Scalar, int _Options, template<typename S, int O> class JointCollectionTpl>
struct traits< JointCompositeTpl<_Scalar,_Options,JointCollectionTpl> >
{
typedef _JointCollection JointCollection;
typedef _Scalar Scalar;
enum {
Options = JointCollection::Options,
Options = _Options,
NQ = Eigen::Dynamic,
NV = Eigen::Dynamic
};
typedef typename JointCollection::Scalar Scalar;
typedef JointDataCompositeTpl<JointCollection> JointDataDerived;
typedef JointModelCompositeTpl<JointCollection> JointModelDerived;
typedef JointCollectionTpl<Scalar,Options> JointCollection;
typedef JointDataCompositeTpl<Scalar,Options,JointCollectionTpl> JointDataDerived;
typedef JointModelCompositeTpl<Scalar,Options,JointCollectionTpl> JointModelDerived;
typedef ConstraintTpl<Eigen::Dynamic,Scalar,Options> Constraint_t;
typedef SE3Tpl<Scalar,Options> Transformation_t;
typedef MotionTpl<Scalar,Options> Motion_t;
......@@ -58,23 +59,26 @@ namespace se3
typedef Eigen::Matrix<Scalar,Eigen::Dynamic,1,Options> TangentVector_t;
};
template<typename JointCollection>
struct traits< JointModelCompositeTpl<JointCollection> >
{ typedef JointCompositeTpl<JointCollection> JointDerived; };
template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
struct traits< JointModelCompositeTpl<Scalar,Options,JointCollectionTpl> >
{ typedef JointCompositeTpl<Scalar,Options,JointCollectionTpl> JointDerived; };
template<typename JointCollection>
struct traits< JointDataCompositeTpl<JointCollection> >
{ typedef JointCompositeTpl<JointCollection> JointDerived; };
template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
struct traits< JointDataCompositeTpl<Scalar,Options,JointCollectionTpl> >
{ typedef JointCompositeTpl<Scalar,Options,JointCollectionTpl> JointDerived; };
template<typename JointCollection>
struct JointDataCompositeTpl : public JointDataBase< JointDataCompositeTpl<JointCollection> >
template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
struct JointDataCompositeTpl
: public JointDataBase< JointDataCompositeTpl<Scalar,Options,JointCollectionTpl> >
{
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
typedef JointDataBase< JointDataCompositeTpl<Scalar,Options,JointCollectionTpl> > Base;
typedef JointCollectionTpl<Scalar,Options> JointCollection;
typedef JointDataTpl<JointCollection> JointDataVariant;
// typedef typename JointCollection::JointDataVariant JointDataVariant;
typedef JointDataBase< JointDataCompositeTpl<JointCollection> > Base;
typedef container::aligned_vector<JointDataVariant> JointDataVector;
// typedef boost::array<JointDataVariant,njoints> JointDataVector;
......@@ -116,17 +120,16 @@ namespace se3
};
template<typename JointCollection>
template<typename _Scalar, int _Options, template<typename S, int O> class JointCollectionTpl>
struct JointModelCompositeTpl
: public JointModelBase< JointModelCompositeTpl<JointCollection> >
: public JointModelBase< JointModelCompositeTpl<_Scalar,_Options,JointCollectionTpl> >
{
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
typedef JointModelTpl<JointCollection> JointModelVariant;
// typedef typename JointCollection::JointModelVariant JointModelVariant;
typedef typename traits<JointModelCompositeTpl>::JointDerived JointType;
typedef typename traits<JointType>::JointDataDerived JointDataDerived;
typedef typename traits<JointType>::JointDataDerived JointData;
typedef typename traits<JointType>::Scalar Scalar;
typedef JointModelBase<JointModelCompositeTpl> Base;
enum
{
......@@ -135,10 +138,12 @@ namespace se3
NQ = traits<JointType>::NQ
};
typedef typename traits<JointType>::Scalar Scalar;
typedef JointCollectionTpl<Scalar,Options> JointCollection;
typedef JointModelTpl<JointCollection> JointModelVariant;
// typedef typename JointCollection::JointModelVariant JointModelVariant;
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 typename traits<JointType>::Transformation_t Transformation_t;
......@@ -216,16 +221,16 @@ namespace se3
typename JointData::JointDataVector jdata(joints.size());
for (int i = 0; i < (int)joints.size(); ++i)
jdata[(size_t)i] = ::se3::createData<JointCollection>(joints[(size_t)i]);
return JointDataDerived(jdata,nq(),nv());
return JointData(jdata,nq(),nv());
}
template<typename _JointCollection, typename ConfigVectorType>
template<typename, int, template<typename S, int O> class, typename>
friend struct JointCompositeCalcZeroOrderStep;
template<typename ConfigVectorType>
void calc(JointData & data, const Eigen::MatrixBase<ConfigVectorType> & qs) const;
template<typename _JointCollection,typename ConfigVectorType, typename TangentVectorType>
template<typename, int, template<typename S, int O> class, typename, typename>
friend struct JointCompositeCalcFirstOrderStep;
template<typename ConfigVectorType, typename TangentVectorType>
......@@ -292,8 +297,6 @@ namespace se3
JointModelVector joints;
/// \brief Vector of joint placements. Those placements correspond to the origin of the joint relatively to their parent.
container::aligned_vector<SE3> jointPlacements;
/// \brief Dimensions of the config and tangent space of the composite joint.
int m_nq,m_nv;
template<typename D>
typename SizeDepType<NQ>::template SegmentReturn<D>::ConstType
......@@ -338,6 +341,9 @@ namespace se3
protected:
template<typename, int, template<typename,int> class>
friend struct JointModelCompositeTpl;
/// \brief Update the indexes of the joints contained in the composition according
/// to the position of the joint composite.
void updateJointIndexes()
......@@ -398,20 +404,20 @@ namespace se3
namespace boost
{
template<typename JointCollection>
struct has_nothrow_constructor< ::se3::JointModelCompositeTpl<JointCollection> >
template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
struct has_nothrow_constructor< ::se3::JointModelCompositeTpl<Scalar,Options,JointCollectionTpl> >
: public integral_constant<bool,true> {};
template<typename JointCollection>
struct has_nothrow_copy< ::se3::JointModelCompositeTpl<JointCollection> >
template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
struct has_nothrow_copy< ::se3::JointModelCompositeTpl<Scalar,Options,JointCollectionTpl> >
: public integral_constant<bool,true> {};
template<typename JointCollection>
struct has_nothrow_constructor< ::se3::JointDataCompositeTpl<JointCollection> >
template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
struct has_nothrow_constructor< ::se3::JointDataCompositeTpl<Scalar,Options,JointCollectionTpl> >
: public integral_constant<bool,true> {};
template<typename JointCollection>
struct has_nothrow_copy< ::se3::JointDataCompositeTpl<JointCollection> >
template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
struct has_nothrow_copy< ::se3::JointDataCompositeTpl<Scalar,Options,JointCollectionTpl> >
: public integral_constant<bool,true> {};
}
......
......@@ -23,12 +23,12 @@
namespace se3
{
template<typename JointCollection, typename ConfigVectorType>
template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl, typename ConfigVectorType>
struct JointCompositeCalcZeroOrderStep
: fusion::JointVisitorBase< JointCompositeCalcZeroOrderStep<JointCollection,ConfigVectorType> >
: fusion::JointVisitorBase< JointCompositeCalcZeroOrderStep<Scalar,Options,JointCollectionTpl,ConfigVectorType> >
{
typedef JointModelCompositeTpl<JointCollection> JointModelComposite;
typedef JointDataCompositeTpl<JointCollection> JointDataComposite;
typedef JointModelCompositeTpl<Scalar,Options,JointCollectionTpl> JointModelComposite;
typedef JointDataCompositeTpl<Scalar,Options,JointCollectionTpl> JointDataComposite;
typedef boost::fusion::vector<const JointModelComposite &,
JointDataComposite &,
......@@ -66,15 +66,15 @@ namespace se3
};
template<typename JointCollection>
template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
template<typename ConfigVectorType>
inline void JointModelCompositeTpl<JointCollection>::calc(JointData & data,
const Eigen::MatrixBase<ConfigVectorType> & qs) const
inline void JointModelCompositeTpl<Scalar,Options,JointCollectionTpl>::
calc(JointData & data, const Eigen::MatrixBase<ConfigVectorType> & qs) const
{
assert(joints.size() > 0);
assert(data.joints.size() == joints.size());
typedef JointCompositeCalcZeroOrderStep<JointCollection,ConfigVectorType> Algo;
typedef JointCompositeCalcZeroOrderStep<Scalar,Options,JointCollectionTpl,ConfigVectorType> Algo;
for (int i=(int)(joints.size()-1); i >= 0; --i)
{
......@@ -85,12 +85,12 @@ namespace se3
data.M = data.iMlast.front();
}
template<typename JointCollection, typename ConfigVectorType, typename TangentVectorType>
template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType>
struct JointCompositeCalcFirstOrderStep
: public fusion::JointVisitorBase< JointCompositeCalcFirstOrderStep<JointCollection,ConfigVectorType,TangentVectorType> >
: public fusion::JointVisitorBase< JointCompositeCalcFirstOrderStep<Scalar,Options,JointCollectionTpl,ConfigVectorType,TangentVectorType> >
{
typedef JointModelCompositeTpl<JointCollection> JointModelComposite;
typedef JointDataCompositeTpl<JointCollection> JointDataComposite;
typedef JointModelCompositeTpl<Scalar,Options,JointCollectionTpl> JointModelComposite;
typedef JointDataCompositeTpl<Scalar,Options,JointCollectionTpl> JointDataComposite;
typedef boost::fusion::vector<const JointModelComposite &,
JointDataComposite &,
......@@ -139,16 +139,17 @@ namespace se3
};
template<typename JointCollection>
template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
template<typename ConfigVectorType, typename TangentVectorType>
inline void JointModelCompositeTpl<JointCollection>::calc(JointData & jdata,
const Eigen::MatrixBase<ConfigVectorType> & qs,
const Eigen::MatrixBase<TangentVectorType> & vs) const
inline void JointModelCompositeTpl<Scalar,Options,JointCollectionTpl>
::calc(JointData & jdata,
const Eigen::MatrixBase<ConfigVectorType> & qs,
const Eigen::MatrixBase<TangentVectorType> & vs) const
{
assert(joints.size() > 0);
assert(jdata.joints.size() == joints.size());
typedef JointCompositeCalcFirstOrderStep<JointCollection,ConfigVectorType,TangentVectorType> Algo;
typedef JointCompositeCalcFirstOrderStep<Scalar,Options,JointCollectionTpl,ConfigVectorType,TangentVectorType> Algo;
for (int i=(int)(joints.size()-1); i >= 0; --i)
{
......
......@@ -43,8 +43,8 @@ namespace se3
typedef typename LieGroupMap::operation<JointModel>::type type;
};
template<typename JointCollection>
struct LieGroupMap::operation< JointModelCompositeTpl<JointCollection> >
template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
struct LieGroupMap::operation< JointModelCompositeTpl<Scalar,Options,JointCollectionTpl> >
{};
template<typename Scalar, int Options>
......
Markdown is supported
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