[Joint/JointComposite] Improve templatization of JointComposite

It now takes Scalar and Options as input template parameters

src/multibody/joint/fwd.hpp

@@ -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;

src/multibody/joint/joint-collection.hpp

@@ -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

src/multibody/joint/joint-composite.hpp

@@ -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> {};
 }
 

src/multibody/joint/joint-composite.hxx

@@ -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)
     {

src/multibody/liegroup/liegroup.hpp

@@ -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>