diff --git a/CMakeLists.txt b/CMakeLists.txt
index c0db751474..b6b6904522 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -87,6 +87,7 @@ SET(${PROJECT_NAME}_SPATIAL_HEADERS
 
 SET(${PROJECT_NAME}_MULTIBODY_JOINT_HEADERS
   multibody/joint/joint-base.hpp
+  multibody/joint/joint-dense.hpp
   multibody/joint/joint-revolute.hpp
   multibody/joint/joint-revolute-unaligned.hpp
   multibody/joint/joint-spherical.hpp
diff --git a/src/algorithm/center-of-mass.hpp b/src/algorithm/center-of-mass.hpp
index 6709e608ed..2d5b994f00 100644
--- a/src/algorithm/center-of-mass.hpp
+++ b/src/algorithm/center-of-mass.hpp
@@ -263,7 +263,8 @@ namespace se3
       	  = data.mass[i]*oSk.template topLeftCorner<3,1>() 
       	  - data.com[i].cross(oSk.template bottomLeftCorner<3,1>()) ;
       else
-      	data.Jcom.template block<3,JointModel::NV>(0,jmodel.idx_v())
+      	jmodel.jointCols(data.Jcom)
+          //data.Jcom.template block<3,JointModel::NV>(0,jmodel.idx_v())
       	  = data.mass[i]*oSk.template topRows<3>() 
       	  - skew(data.com[i]) * oSk.template bottomRows<3>() ;
 
diff --git a/src/algorithm/crba.hpp b/src/algorithm/crba.hpp
index d14530161e..2ddf922dd2 100644
--- a/src/algorithm/crba.hpp
+++ b/src/algorithm/crba.hpp
@@ -86,7 +86,8 @@ namespace se3
       const Model::Index & i = (Model::Index) jmodel.id();
 
       /* F[1:6,i] = Y*S */
-      data.Fcrb[i].block<6,JointModel::NV>(0,jmodel.idx_v()) = data.Ycrb[i] * jdata.S();
+      //data.Fcrb[i].block<6,JointModel::NV>(0,jmodel.idx_v()) = data.Ycrb[i] * jdata.S();
+      jmodel.jointCols(data.Fcrb[i]) = data.Ycrb[i] * jdata.S();
 
       /* M[i,SUBTREE] = S'*F[1:6,SUBTREE] */
       data.M.block(jmodel.idx_v(),jmodel.idx_v(),jmodel.nv(),data.nvSubtree[i]) 
diff --git a/src/algorithm/jacobian.hpp b/src/algorithm/jacobian.hpp
index 0533da7515..0f2ebab3e4 100644
--- a/src/algorithm/jacobian.hpp
+++ b/src/algorithm/jacobian.hpp
@@ -62,7 +62,7 @@ namespace se3
       if(parent>0) data.oMi[i] = data.oMi[parent]*data.liMi[i];
       else         data.oMi[i] = data.liMi[i];
 
-      data.J.block(0,jmodel.idx_v(),6,jmodel.nv()) = data.oMi[i].act(jdata.S());
+      jmodel.jointCols(data.J) = data.oMi[i].act(jdata.S());
     }
 
   };
@@ -128,7 +128,7 @@ namespace se3
       data.liMi[i] = model.jointPlacements[i]*jdata.M();
       data.iMf[parent] = data.liMi[i]*data.iMf[i];
 
-      data.J.block(0,jmodel.idx_v(),6,jmodel.nv()) = data.iMf[i].inverse().act(jdata.S());
+      jmodel.jointCols(data.J) = data.iMf[i].inverse().act(jdata.S());
     }
 
   };
diff --git a/src/algorithm/joint-limits.hpp b/src/algorithm/joint-limits.hpp
index 46e0b32a4e..47975425db 100644
--- a/src/algorithm/joint-limits.hpp
+++ b/src/algorithm/joint-limits.hpp
@@ -52,11 +52,11 @@ namespace se3
       using namespace se3;
       
       // TODO: as limits are static, no need of q, nor computations
-      jmodel.jointLimit(data.effortLimit) = jmodel.maxEffortLimit();
-      jmodel.jointTangentLimit(data.velocityLimit) = jmodel.maxVelocityLimit();
+      jmodel.jointVelocitySelector(data.effortLimit) = jmodel.maxEffortLimit();
+      jmodel.jointVelocitySelector(data.velocityLimit) = jmodel.maxVelocityLimit();
 
-      jmodel.jointLimit(data.lowerPositionLimit) = jmodel.lowerPosLimit(); // if computation needed, do it here, or may be in lowerPosLimit
-      jmodel.jointLimit(data.upperPositionLimit) = jmodel.upperPosLimit();
+      jmodel.jointConfigSelector(data.lowerPositionLimit) = jmodel.lowerPosLimit(); // if computation needed, do it here, or may be in lowerPosLimit
+      jmodel.jointConfigSelector(data.upperPositionLimit) = jmodel.upperPosLimit();
     }
 
   };
diff --git a/src/algorithm/kinematics.hpp b/src/algorithm/kinematics.hpp
index 1e4d61b9aa..77a59ceec3 100644
--- a/src/algorithm/kinematics.hpp
+++ b/src/algorithm/kinematics.hpp
@@ -180,7 +180,7 @@ namespace se3
       else
         data.oMi[i] = data.liMi[i];
       
-      data.a[i]  = jdata.S() * jmodel.jointMotion(a) + jdata.c() + (data.v[i] ^ jdata.v()) ;
+      data.a[i]  = jdata.S() * jmodel.jointVelocitySelector(a) + jdata.c() + (data.v[i] ^ jdata.v()) ;
       data.a[i] += data.liMi[i].actInv(data.a[parent]);
     }
   };
diff --git a/src/algorithm/non-linear-effects.hpp b/src/algorithm/non-linear-effects.hpp
index 643e3b2a4b..c9f861fde5 100644
--- a/src/algorithm/non-linear-effects.hpp
+++ b/src/algorithm/non-linear-effects.hpp
@@ -88,7 +88,7 @@ namespace se3
 		     const size_t i)
     {
       const Model::Index & parent  = model.parents[i];      
-      jmodel.jointForce(data.nle)  = jdata.S().transpose()*data.f[i];
+      jmodel.jointVelocitySelector(data.nle)  = jdata.S().transpose()*data.f[i];
       if(parent>0) data.f[(size_t) parent] += data.liMi[i].act(data.f[i]);
     }
   };
diff --git a/src/algorithm/rnea.hpp b/src/algorithm/rnea.hpp
index f59aec1f22..bcb0a74e0e 100644
--- a/src/algorithm/rnea.hpp
+++ b/src/algorithm/rnea.hpp
@@ -67,7 +67,7 @@ namespace se3
       data.v[(Model::Index)i] = jdata.v();
       if(parent>0) data.v[(Model::Index)i] += data.liMi[(Model::Index)i].actInv(data.v[parent]);
       
-      data.a[(Model::Index)i]  = jdata.S()*jmodel.jointMotion(a) + jdata.c() + (data.v[(Model::Index)i] ^ jdata.v()) ; 
+      data.a[(Model::Index)i]  = jdata.S()*jmodel.jointVelocitySelector(a) + jdata.c() + (data.v[(Model::Index)i] ^ jdata.v()) ; 
       data.a[(Model::Index)i] += data.liMi[(Model::Index)i].actInv(data.a[parent]);
       
       data.f[(Model::Index)i] = model.inertias[(Model::Index)i]*data.a[(Model::Index)i] + model.inertias[(Model::Index)i].vxiv(data.v[(Model::Index)i]); // -f_ext
@@ -92,7 +92,7 @@ namespace se3
 		     int i)
     {
       const Model::Index & parent  = model.parents[(Model::Index)i];      
-      jmodel.jointForce(data.tau)  = jdata.S().transpose()*data.f[(Model::Index)i];
+      jmodel.jointVelocitySelector(data.tau)  = jdata.S().transpose()*data.f[(Model::Index)i];
       if(parent>0) data.f[(Model::Index)parent] += data.liMi[(Model::Index)i].act(data.f[(Model::Index)i]);
     }
   };
diff --git a/src/multibody/constraint.hpp b/src/multibody/constraint.hpp
index d8950d1c2e..7819f90caa 100644
--- a/src/multibody/constraint.hpp
+++ b/src/multibody/constraint.hpp
@@ -81,8 +81,15 @@ namespace se3
 
   }; // traits ConstraintTpl
 
+  namespace internal
+  {  
+    template<int Dim, typename Scalar, int Options>
+    struct ActionReturn<ConstraintTpl<Dim,Scalar,Options> >
+    { typedef Eigen::Matrix<Scalar,6,Dim> Type; };
+  }
+
   template<int _Dim, typename _Scalar, int _Options>
-  class ConstraintTpl : ConstraintBase<ConstraintTpl < _Dim, _Scalar, _Options > >
+  class ConstraintTpl : public ConstraintBase<ConstraintTpl < _Dim, _Scalar, _Options > >
   { 
   public:
 
@@ -101,13 +108,16 @@ namespace se3
 
     ConstraintTpl() : S() 
     {
-      EIGEN_STATIC_ASSERT_FIXED_SIZE(DenseBase);
+#ifndef NDEBUG
       S.fill( NAN ); 
+#endif
     } 
 
     ConstraintTpl(const int dim) : S(6,dim)
     {
+#ifndef NDEBUG
       S.fill( NAN );
+#endif
     }
 
     Motion __mult__(const JointMotion& vj) const 
@@ -123,6 +133,14 @@ namespace se3
 
       JointForce operator* (const Force& f) const
       { return ref.S.transpose()*f.toVector(); }
+
+      template<typename D>
+      typename Eigen::Matrix<_Scalar,NV,Eigen::Dynamic>
+      operator*( const Eigen::MatrixBase<D> & F )
+      {
+        return ref.S.transpose()*F;
+      }
+
     };
     Transpose transpose() const { return Transpose(*this); }
 
@@ -131,10 +149,24 @@ namespace se3
 
     int nv_impl() const { return NV; }
 
+    //template<int Dim,typename Scalar,int Options>
+    friend Eigen::Matrix<_Scalar,6,_Dim>
+    operator*( const InertiaTpl<_Scalar,_Options> & Y,const ConstraintTpl<_Dim,_Scalar,_Options> & S)
+    { return Y.matrix()*S.S; }
+
+    Eigen::Matrix<_Scalar,6,NV> se3Action(const SE3 & m) const
+    {
+      return m.toActionMatrix()*S;
+    }
+
+
   private:
     DenseBase S;
   }; // class ConstraintTpl
 
+
+
+
   typedef ConstraintTpl<1,double,0> Constraint1d;
   typedef ConstraintTpl<3,double,0> Constraint3d;
   typedef ConstraintTpl<6,double,0> Constraint6d;
diff --git a/src/multibody/joint.hpp b/src/multibody/joint.hpp
index ea04b580f9..e4d20d71e2 100644
--- a/src/multibody/joint.hpp
+++ b/src/multibody/joint.hpp
@@ -19,6 +19,7 @@
 #define __se3_joint_hpp__
 
 #include "pinocchio/multibody/joint/joint-base.hpp"
+#include "pinocchio/multibody/joint/joint-dense.hpp"
 #include "pinocchio/multibody/joint/joint-revolute.hpp"
 #include "pinocchio/multibody/joint/joint-revolute-unaligned.hpp"
 #include "pinocchio/multibody/joint/joint-spherical.hpp"
diff --git a/src/multibody/joint/joint-base.hpp b/src/multibody/joint/joint-base.hpp
index db32f6de5d..b5ef901af1 100644
--- a/src/multibody/joint/joint-base.hpp
+++ b/src/multibody/joint/joint-base.hpp
@@ -18,8 +18,8 @@
 #ifndef __se3_joint_base_hpp__
 #define __se3_joint_base_hpp__
 
-#include "pinocchio/spatial/fwd.hpp"
-#include "pinocchio/spatial/motion.hpp"
+#include "pinocchio/spatial/se3.hpp"
+#include "pinocchio/spatial/inertia.hpp"
 #include "pinocchio/multibody/constraint.hpp"
 #include <Eigen/Geometry>
 #include <Eigen/StdVector>
@@ -65,18 +65,18 @@ namespace se3
    */
 #ifdef __clang__
 
-#define SE3_JOINT_TYPEDEF_ARG(prefix)					     \
-  typedef int Index;						     \
-  typedef prefix traits<Joint>::JointData JointData;		     \
-  typedef prefix traits<Joint>::JointModel JointModel;	     \
-  typedef prefix traits<Joint>::Constraint_t Constraint_t;	     \
-  typedef prefix traits<Joint>::Transformation_t Transformation_t; \
-  typedef prefix traits<Joint>::Motion_t Motion_t;		     \
-  typedef prefix traits<Joint>::Bias_t Bias_t;		     \
-  typedef prefix traits<Joint>::F_t F_t;			     \
-  enum {							     \
-    NQ = traits<Joint>::NQ,					     \
-    NV = traits<Joint>::NV					     \
+#define SE3_JOINT_TYPEDEF_ARG(prefix)              \
+   typedef int Index;                \
+   typedef prefix traits<Joint>::JointData JointData;        \
+   typedef prefix traits<Joint>::JointModel JointModel;      \
+   typedef prefix traits<Joint>::Constraint_t Constraint_t;      \
+   typedef prefix traits<Joint>::Transformation_t Transformation_t; \
+   typedef prefix traits<Joint>::Motion_t Motion_t;        \
+   typedef prefix traits<Joint>::Bias_t Bias_t;        \
+   typedef prefix traits<Joint>::F_t F_t;          \
+   enum {                  \
+    NQ = traits<Joint>::NQ,              \
+    NV = traits<Joint>::NV               \
   }
 
 #define SE3_JOINT_TYPEDEF SE3_JOINT_TYPEDEF_ARG()
@@ -84,32 +84,32 @@ namespace se3
 
 #elif (__GNUC__ == 4) && (__GNUC_MINOR__ == 4) && (__GNUC_PATCHLEVEL__ == 2)
 
-#define SE3_JOINT_TYPEDEF_NOARG()				\
-  typedef int Index;						\
-  typedef traits<Joint>::JointData JointData;			\
-  typedef traits<Joint>::JointModel JointModel;			\
-  typedef traits<Joint>::Constraint_t Constraint_t;		\
-  typedef traits<Joint>::Transformation_t Transformation_t;	\
-  typedef traits<Joint>::Motion_t Motion_t;			\
-  typedef traits<Joint>::Bias_t Bias_t;				\
-  typedef traits<Joint>::F_t F_t;				\
-  enum {							\
-    NQ = traits<Joint>::NQ,					\
-    NV = traits<Joint>::NV					\
+#define SE3_JOINT_TYPEDEF_NOARG()       \
+  typedef int Index;            \
+  typedef traits<Joint>::JointData JointData;     \
+  typedef traits<Joint>::JointModel JointModel;     \
+  typedef traits<Joint>::Constraint_t Constraint_t;   \
+  typedef traits<Joint>::Transformation_t Transformation_t; \
+  typedef traits<Joint>::Motion_t Motion_t;     \
+  typedef traits<Joint>::Bias_t Bias_t;       \
+  typedef traits<Joint>::F_t F_t;       \
+  enum {              \
+    NQ = traits<Joint>::NQ,         \
+    NV = traits<Joint>::NV          \
   }
 
-#define SE3_JOINT_TYPEDEF_ARG(prefix)					\
-  typedef int Index;							\
-  typedef prefix traits<Joint>::JointData JointData;			\
-  typedef prefix traits<Joint>::JointModel JointModel;			\
-  typedef prefix traits<Joint>::Constraint_t Constraint_t;		\
-  typedef prefix traits<Joint>::Transformation_t Transformation_t;	\
-  typedef prefix traits<Joint>::Motion_t Motion_t;			\
-  typedef prefix traits<Joint>::Bias_t Bias_t;				\
-  typedef prefix traits<Joint>::F_t F_t;				\
-  enum {								\
-    NQ = traits<Joint>::NQ,						\
-    NV = traits<Joint>::NV						\
+#define SE3_JOINT_TYPEDEF_ARG(prefix)         \
+  typedef int Index;              \
+  typedef prefix traits<Joint>::JointData JointData;      \
+  typedef prefix traits<Joint>::JointModel JointModel;      \
+  typedef prefix traits<Joint>::Constraint_t Constraint_t;    \
+  typedef prefix traits<Joint>::Transformation_t Transformation_t;  \
+  typedef prefix traits<Joint>::Motion_t Motion_t;      \
+  typedef prefix traits<Joint>::Bias_t Bias_t;        \
+  typedef prefix traits<Joint>::F_t F_t;        \
+  enum {                \
+    NQ = traits<Joint>::NQ,           \
+    NV = traits<Joint>::NV            \
   }
 
 #define SE3_JOINT_TYPEDEF SE3_JOINT_TYPEDEF_NOARG()
@@ -137,9 +137,14 @@ namespace se3
 #endif
 
 #define SE3_JOINT_USE_INDEXES \
-    typedef JointModelBase<JointModel> Base; \
-    using Base::idx_q; \
-    using Base::idx_v
+  typedef JointModelBase<JointModel> Base; \
+  using Base::idx_q; \
+  using Base::idx_v
+
+
+  template <int _NQ, int _NV> struct JointDataDense;
+  template <int _NQ, int _NV> struct JointModelDense;
+  template <int _NQ, int _NV> struct JointDense;
 
   template<typename _JointData>
   struct JointDataBase
@@ -155,6 +160,42 @@ namespace se3
     const Motion_t         & v() const  { return static_cast<const JointData*>(this)->v;   }
     const Bias_t           & c() const  { return static_cast<const JointData*>(this)->c;   }
     F_t& F()        { return static_cast<      JointData*>(this)->F; }
+
+    JointDataDense<NQ, NV> toDense() const  { return static_cast<const JointData*>(this)->toDense_impl();   }
+
+  }; // struct JointDataBase
+
+  template<int NV>
+  struct SizeDepType
+  {
+    template<class Mat>
+    struct SegmentReturn 
+    {
+      typedef typename Mat::template FixedSegmentReturnType<NV>::Type Type;
+      typedef typename Mat::template ConstFixedSegmentReturnType<NV>::Type ConstType;
+    };
+    template<class Mat>
+    struct ColsReturn
+    {
+      typedef typename Mat::template NColsBlockXpr<NV>::Type Type;
+      typedef typename Mat::template ConstNColsBlockXpr<NV>::Type ConstType;
+    };
+  };
+  template<>
+  struct SizeDepType<Eigen::Dynamic>
+  {
+    template<class Mat>
+    struct SegmentReturn 
+    {
+      typedef typename Mat::SegmentReturnType Type;
+      typedef typename Mat::ConstSegmentReturnType ConstType;
+    };
+    template<class Mat>
+    struct ColsReturn
+    {
+      typedef typename Mat::ColsBlockXpr Type;
+      typedef typename Mat::ConstColsBlockXpr ConstType;
+    };
   };
 
   template<typename _JointModel>
@@ -168,14 +209,13 @@ namespace se3
 
     JointData createData() const { return static_cast<const JointModel*>(this)->createData(); }
     void calc( JointData& data, 
-	       const Eigen::VectorXd & qs ) const
+      const Eigen::VectorXd & qs ) const
     { return static_cast<const JointModel*>(this)->calc(data,qs); }
     void calc( JointData& data, 
-	       const Eigen::VectorXd & qs, 
-	       const Eigen::VectorXd & vs ) const
+      const Eigen::VectorXd & qs, 
+      const Eigen::VectorXd & vs ) const
     { return static_cast<const JointModel*>(this)->calc(data,qs,vs); }
 
-  private:
     Index i_id; // ID of the joint in the multibody list.
     int i_q;    // Index of the joint configuration in the joint configuration vector.
     int i_v;    // Index of the joint velocity in the joint velocity vector.
@@ -183,12 +223,16 @@ namespace se3
     Eigen::Matrix<double,NQ,1> position_lower;
     Eigen::Matrix<double,NQ,1> position_upper;
 
-    Eigen::Matrix<double,NQ,1> effortMax;
+    Eigen::Matrix<double,NV,1> effortMax;
     Eigen::Matrix<double,NV,1> velocityMax;
 
-  public:
-          int     nv()    const { return NV; }
-          int     nq()    const { return NQ; }
+    
+    int     nv()    const { return derived().nv_impl(); }
+    int     nq()    const { return derived().nq_impl(); }
+    // Both _impl methods are reimplemented by dynamic-size joints.
+    int     nv_impl() const { return NV; }
+    int     nq_impl() const { return NQ; }
+
     const int &   idx_q() const { return i_q; }
     const int &   idx_v() const { return i_v; }
     const Index & id()    const { return i_id; }
@@ -196,72 +240,79 @@ namespace se3
     const Eigen::Matrix<double,NQ,1> & lowerPosLimit() const { return position_lower;}
     const Eigen::Matrix<double,NQ,1> & upperPosLimit() const { return position_upper;}
 
-    const Eigen::Matrix<double,NQ,1> & maxEffortLimit() const { return effortMax;}
+    const Eigen::Matrix<double,NV,1> & maxEffortLimit() const { return effortMax;}
     const Eigen::Matrix<double,NV,1> & maxVelocityLimit() const { return velocityMax;}
 
-
     void setIndexes(Index id,int q,int v) { i_id = id, i_q = q; i_v = v; }
 
     void setLowerPositionLimit(const Eigen::VectorXd & lowerPos)
     {
-      if (lowerPos.rows() == NQ)
-        position_lower = lowerPos;
-      else
-        position_lower.fill(-std::numeric_limits<double>::infinity());
+      position_lower = lowerPos;
     }
 
     void setUpperPositionLimit(const Eigen::VectorXd & upperPos)
     {
-      if (upperPos.rows() == NQ)
-        position_upper = upperPos;
-      else
-        position_upper.fill(std::numeric_limits<double>::infinity());
+      position_upper = upperPos;
     }
 
     void setMaxEffortLimit(const Eigen::VectorXd & effort)
     {
-      if (effort.rows() == NQ)
-        effortMax = effort;
-      else
-        effortMax.fill(std::numeric_limits<double>::infinity());
+      effortMax = effort;
     }
 
     void setMaxVelocityLimit(const Eigen::VectorXd & v)
     {
-      if (v.rows() == NV)
-        velocityMax = v;
-      else
-        velocityMax.fill(std::numeric_limits<double>::infinity());
+      velocityMax = v;
     }
 
-
+    /* Acces to dedicated segment in robot config space.  */
+    // Const access
     template<typename D>
-    typename D::template ConstFixedSegmentReturnType<NV>::Type jointMotion(const Eigen::MatrixBase<D>& a) const     { return a.template segment<NV>(i_v); }
+    typename SizeDepType<NQ>::template SegmentReturn<D>::ConstType 
+    jointConfigSelector(const Eigen::MatrixBase<D>& a) const       { return derived().jointConfigSelector_impl(a); }
     template<typename D>
-    typename D::template FixedSegmentReturnType<NV>::Type jointMotion(Eigen::MatrixBase<D>& a) const 
-    { return a.template segment<NV>(i_v); }
+    typename SizeDepType<NQ>::template SegmentReturn<D>::ConstType 
+    jointConfigSelector_impl(const Eigen::MatrixBase<D>& a) const   { return a.template segment<NQ>(i_q); }
+    // Non-const access
     template<typename D>
-    typename D::template ConstFixedSegmentReturnType<NV>::Type jointForce(const Eigen::MatrixBase<D>& tau) const 
-    { return tau.template segment<NV>(i_v); }
+    typename SizeDepType<NQ>::template SegmentReturn<D>::Type 
+    jointConfigSelector( Eigen::MatrixBase<D>& a) const { return derived().jointConfigSelector_impl(a); }
     template<typename D>
-    typename D::template FixedSegmentReturnType<NV>::Type jointForce(Eigen::MatrixBase<D>& tau) const 
-    { return tau.template segment<NV>(i_v); }
+    typename SizeDepType<NQ>::template SegmentReturn<D>::Type 
+    jointConfigSelector_impl( Eigen::MatrixBase<D>& a) const { return a.template segment<NQ>(i_q); }
 
+    /* Acces to dedicated segment in robot config velocity space.  */
+    // Const access
+    template<typename D>
+    typename SizeDepType<NV>::template SegmentReturn<D>::ConstType 
+    jointVelocitySelector(const Eigen::MatrixBase<D>& a) const       { return derived().jointVelocitySelector_impl(a); }
     template<typename D>
-    typename D::template ConstFixedSegmentReturnType<NQ>::Type jointLimit(const Eigen::MatrixBase<D>& limit) const 
-    { return limit.template segment<NQ>(i_q); }
+    typename SizeDepType<NV>::template SegmentReturn<D>::ConstType 
+    jointVelocitySelector_impl(const Eigen::MatrixBase<D>& a) const   { return a.template segment<NV>(i_v); }
+    // Non-const access
     template<typename D>
-    typename D::template FixedSegmentReturnType<NQ>::Type jointLimit(Eigen::MatrixBase<D>& limit) const 
-    { return limit.template segment<NQ>(i_q); }
+    typename SizeDepType<NV>::template SegmentReturn<D>::Type 
+    jointVelocitySelector( Eigen::MatrixBase<D>& a) const { return derived().jointVelocitySelector_impl(a); }
+    template<typename D>
+    typename SizeDepType<NV>::template SegmentReturn<D>::Type 
+    jointVelocitySelector_impl( Eigen::MatrixBase<D>& a) const { return a.template segment<NV>(i_v); }
+
 
     template<typename D>
-    typename D::template ConstFixedSegmentReturnType<NV>::Type jointTangentLimit(const Eigen::MatrixBase<D>& limit) const 
-    { return limit.template segment<NV>(i_v); }
+    typename SizeDepType<NV>::template ColsReturn<D>::ConstType 
+    jointCols(const Eigen::MatrixBase<D>& A) const       { return derived().jointCols_impl(A); }
+    template<typename D>
+    typename SizeDepType<NV>::template ColsReturn<D>::ConstType 
+    jointCols_impl(const Eigen::MatrixBase<D>& A) const       { return A.template middleCols<NV>(i_v); }
     template<typename D>
-    typename D::template FixedSegmentReturnType<NV>::Type jointTangentLimit(Eigen::MatrixBase<D>& limit) const 
-    { return limit.template segment<NV>(i_v); }
+    typename SizeDepType<NV>::template ColsReturn<D>::Type 
+    jointCols(Eigen::MatrixBase<D>& A) const       { return derived().jointCols_impl(A); }
+    template<typename D>
+    typename SizeDepType<NV>::template ColsReturn<D>::Type 
+    jointCols_impl(Eigen::MatrixBase<D>& A) const       { return A.template middleCols<NV>(i_v); }
 
-  };
+    JointModelDense<NQ, NV> toDense() const  { return static_cast<const JointModel*>(this)->toDense_impl();   }
+  }; // struct JointModelBase
 
 } // namespace se3
 
diff --git a/src/multibody/joint/joint-dense.hpp b/src/multibody/joint/joint-dense.hpp
new file mode 100644
index 0000000000..b9cae0db5a
--- /dev/null
+++ b/src/multibody/joint/joint-dense.hpp
@@ -0,0 +1,214 @@
+//
+// Copyright (c) 2015 CNRS
+//
+// This file is part of Pinocchio
+// Pinocchio is free software: you can redistribute it
+// and/or modify it under the terms 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_dense_hpp__
+#define __se3_joint_dense_hpp__
+
+#include "pinocchio/multibody/joint/joint-base.hpp"
+
+namespace se3
+{
+
+  template<int _NQ, int _NV>
+  struct traits< JointDense<_NQ, _NV > >
+  {
+    typedef JointDataDense<_NQ, _NV> JointData;
+    typedef JointModelDense<_NQ, _NV> JointModel;
+    typedef ConstraintXd Constraint_t;
+    typedef SE3 Transformation_t;
+    typedef Motion Motion_t;
+    typedef BiasZero Bias_t;
+    typedef Eigen::Matrix<double,6,Eigen::Dynamic> F_t;
+    enum {
+      NQ = _NQ, // pb 
+      NV = _NV
+    };
+  };
+
+  template<int _NQ, int _NV> struct traits< JointDataDense<_NQ, _NV > > { typedef JointDense<_NQ,_NV > Joint; };
+  template<int _NQ, int _NV> struct traits< JointModelDense<_NQ, _NV > > { typedef JointDense<_NQ,_NV > Joint; };
+
+  template <int _NQ, int _NV>
+  struct JointDataDense : public JointDataBase< JointDataDense<_NQ, _NV > >
+  {
+    typedef JointDense<_NQ, _NV > Joint;
+    SE3_JOINT_TYPEDEF_TEMPLATE;
+
+    Constraint_t S;
+    Transformation_t M;
+    Motion_t v;
+    Bias_t c;
+
+    F_t F;
+
+    /// Removed Default constructor of JointDataDense because it was calling default constructor of
+    /// ConstraintXd -> eigen_static_assert
+    /// JointDataDense should always be instanciated from a JointDataXX.toDense()
+    // JointDataDense() : M(1)
+    // {
+    //   M.translation(SE3::Vector3::Zero());
+    // }
+
+    JointDataDense() {};
+
+    JointDataDense( Constraint_t S,
+                    Transformation_t M,
+                    Motion_t v,
+                    Bias_t c,
+                    F_t F
+                    )
+    : S(S)
+    , M(M)
+    , v(v)
+    , c(c)
+    , F(F)
+    {}
+
+    JointDataDense<_NQ, _NV> toDense_impl() const
+    {
+      assert(false && "Trying to convert a jointDataDense to JointDataDense : useless"); // disapear with release optimizations
+      return *this;
+    }
+
+  }; // struct JointDataDense
+
+  template <int _NQ, int _NV>
+  struct JointModelDense : public JointModelBase< JointModelDense<_NQ, _NV > >
+  {
+    typedef JointDense<_NQ, _NV > Joint;
+    SE3_JOINT_TYPEDEF_TEMPLATE;
+
+    using JointModelBase<JointModelDense<_NQ, _NV > >::idx_q;
+    using JointModelBase<JointModelDense<_NQ, _NV > >::idx_v;
+    using JointModelBase<JointModelDense<_NQ, _NV > >::setLowerPositionLimit;
+    using JointModelBase<JointModelDense<_NQ, _NV > >::setUpperPositionLimit;
+    using JointModelBase<JointModelDense<_NQ, _NV > >::setMaxEffortLimit;
+    using JointModelBase<JointModelDense<_NQ, _NV > >::setMaxVelocityLimit;
+    using JointModelBase<JointModelDense<_NQ, _NV > >::setIndexes;
+    using JointModelBase<JointModelDense<_NQ, _NV > >::i_v;
+    using JointModelBase<JointModelDense<_NQ, _NV > >::i_q;
+
+    int nv_dyn,nq_dyn;
+    
+    JointData createData() const
+    {
+      //assert(false && "JointModelDense is read-only, should not createData");
+      return JointData();
+    }
+    void calc( JointData& , 
+     const Eigen::VectorXd &  ) const
+    {
+      assert(false && "JointModelDense is read-only, should not perform any calc");
+    }
+
+    void calc( JointData&  ,
+     const Eigen::VectorXd & , 
+     const Eigen::VectorXd &  ) const
+    {
+      assert(false && "JointModelDense is read-only, should not perform any calc");
+    }
+
+    JointModelDense()
+    {
+      setLowerPositionLimit(Eigen::Matrix<double,NQ,1>(1));
+      setUpperPositionLimit(Eigen::Matrix<double,NQ,1>(1));
+      setMaxEffortLimit(Eigen::Matrix<double,NV,1>(1));
+      setMaxVelocityLimit(Eigen::Matrix<double,NV,1>(1));
+    }
+    JointModelDense(  Index idx,
+                      int idx_q,
+                      int idx_v,
+                      Eigen::Matrix<double,NQ,1> lowPos,
+                      Eigen::Matrix<double,NQ,1> upPos,
+                      Eigen::Matrix<double,NV,1> maxEff,
+                      Eigen::Matrix<double,NV,1> maxVel
+                    )
+    {
+      setIndexes(idx, idx_q, idx_v);
+      setLowerPositionLimit(lowPos);
+      setUpperPositionLimit(upPos);
+      setMaxEffortLimit(maxEff);
+      setMaxVelocityLimit(maxVel);
+    }
+
+    int     nv_impl() const { return nv_dyn; }
+    int     nq_impl() const { return nq_dyn; }
+
+    template<typename D>
+    typename SizeDepType<NQ>::template SegmentReturn<D>::ConstType
+    jointConfigSelector_impl(const Eigen::MatrixBase<D>& a) const { return a.template segment(i_q,nq_dyn); }
+    template<typename D>
+    typename SizeDepType<NQ>::template SegmentReturn<D>::Type
+    jointConfigSelector_impl( Eigen::MatrixBase<D>& a) const { return a.template segment(i_q,nq_dyn); }
+
+    template<typename D>
+    typename SizeDepType<NV>::template SegmentReturn<D>::ConstType
+    jointVelocitySelector_impl(const Eigen::MatrixBase<D>& a) const { return a.template segment(i_v,nv_dyn); }
+    template<typename D>
+    typename SizeDepType<NV>::template SegmentReturn<D>::Type
+    jointVelocitySelector_impl( Eigen::MatrixBase<D>& a) const { return a.template segment(i_v,nv_dyn); }
+
+    template<typename D>
+    typename SizeDepType<NV>::template ColsReturn<D>::ConstType 
+    jointCols_impl(const Eigen::MatrixBase<D>& A) const { return A.template middleCols<NV>(i_v); }
+    template<typename D>
+    typename SizeDepType<NV>::template ColsReturn<D>::Type 
+    jointCols_impl(Eigen::MatrixBase<D>& A) const { return A.template middleCols<NV>(i_v); }
+
+    JointModelDense<_NQ, _NV> toDense_impl() const
+    {
+      assert(false && "Trying to convert a jointModelDense to JointModelDense : useless");
+      return *this;
+    }
+
+  }; // struct JointModelDense
+
+  template<>
+  template<typename D>
+  typename SizeDepType<Eigen::Dynamic>::template SegmentReturn<D>::ConstType
+  JointModelDense<Eigen::Dynamic,Eigen::Dynamic>::
+  jointConfigSelector_impl(const Eigen::MatrixBase<D>& a) const { return a.segment(i_q,nq_dyn); }
+  template<>
+  template<typename D>
+  typename SizeDepType<Eigen::Dynamic>::template SegmentReturn<D>::Type
+  JointModelDense<Eigen::Dynamic,Eigen::Dynamic>::
+  jointConfigSelector_impl(Eigen::MatrixBase<D>& a) const { return a.segment(i_q,nq_dyn); }
+  template<>
+  template<typename D>
+  typename SizeDepType<Eigen::Dynamic>::template SegmentReturn<D>::ConstType
+  JointModelDense<Eigen::Dynamic,Eigen::Dynamic>::
+  jointVelocitySelector_impl(const Eigen::MatrixBase<D>& a) const { return a.segment(i_v,nv_dyn); }
+  template<>
+  template<typename D>
+  typename SizeDepType<Eigen::Dynamic>::template SegmentReturn<D>::Type
+  JointModelDense<Eigen::Dynamic,Eigen::Dynamic>::
+  jointVelocitySelector_impl(Eigen::MatrixBase<D>& a) const { return a.segment(i_v,nv_dyn); }
+
+  template<>
+  template<typename D>
+  typename SizeDepType<Eigen::Dynamic>::template ColsReturn<D>::ConstType 
+  JointModelDense<Eigen::Dynamic,Eigen::Dynamic>::
+  jointCols_impl(const Eigen::MatrixBase<D>& A) const { return A.middleCols(i_v,nv_dyn); }
+  template<>
+  template<typename D>
+  typename SizeDepType<Eigen::Dynamic>::template ColsReturn<D>::Type 
+  JointModelDense<Eigen::Dynamic,Eigen::Dynamic>::
+  jointCols_impl(Eigen::MatrixBase<D>& A) const { return A.middleCols(i_v,nv_dyn); }
+
+} // namespace se3
+
+#endif // ifndef __se3_joint_dense_hpp__
diff --git a/src/multibody/joint/joint-free-flyer.hpp b/src/multibody/joint/joint-free-flyer.hpp
index 05b10bb099..62e436b6aa 100644
--- a/src/multibody/joint/joint-free-flyer.hpp
+++ b/src/multibody/joint/joint-free-flyer.hpp
@@ -18,6 +18,7 @@
 #ifndef __se3_joint_free_flyer_hpp__
 #define __se3_joint_free_flyer_hpp__
 
+#include "pinocchio/spatial/inertia.hpp"
 #include "pinocchio/multibody/joint/joint-base.hpp"
 #include "pinocchio/multibody/constraint.hpp"
 
@@ -57,32 +58,34 @@ namespace se3
   }; // traits ConstraintRevolute
 
 
-  struct ConstraintIdentity : ConstraintBase < ConstraintIdentity >
-  {
-    SPATIAL_TYPEDEF_NO_TEMPLATE(ConstraintIdentity);
-    enum { NV = 6, Options = 0 };
-    typedef traits<ConstraintIdentity>::JointMotion JointMotion;
-    typedef traits<ConstraintIdentity>::JointForce JointForce;
-    typedef traits<ConstraintIdentity>::DenseBase DenseBase;
-
-    SE3::Matrix6 se3Action(const SE3 & m) const { return m.toActionMatrix(); }
-
-    struct TransposeConst 
+    struct ConstraintIdentity : ConstraintBase < ConstraintIdentity >
     {
-      Force::Vector6 operator* (const Force & phi)
-      {  return phi.toVector();  }
-    };
-    
-    TransposeConst transpose() const { return TransposeConst(); }
-    operator ConstraintXd () const { return ConstraintXd(SE3::Matrix6::Identity()); }
-  }; // struct ConstraintIdentity
-
-  template<typename D>
-  Motion operator* (const ConstraintIdentity&, const Eigen::MatrixBase<D>& v)
-  {
-    EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(D,6);
-    return Motion(v);
-  }
+      SPATIAL_TYPEDEF_NO_TEMPLATE(ConstraintIdentity);
+      enum { NV = 6, Options = 0 };
+      typedef traits<ConstraintIdentity>::JointMotion JointMotion;
+      typedef traits<ConstraintIdentity>::JointForce JointForce;
+      typedef traits<ConstraintIdentity>::DenseBase DenseBase;
+
+      SE3::Matrix6 se3Action(const SE3 & m) const { return m.toActionMatrix(); }
+
+      int nv_impl() const { return NV; }
+
+      struct TransposeConst 
+      {
+        Force::Vector6 operator* (const Force & phi)
+        {  return phi.toVector();  }
+      };
+      
+      TransposeConst transpose() const { return TransposeConst(); }
+      operator ConstraintXd () const { return ConstraintXd(SE3::Matrix6::Identity()); }
+    }; // struct ConstraintIdentity
+
+    template<typename D>
+    Motion operator* (const ConstraintIdentity&, const Eigen::MatrixBase<D>& v)
+    {
+      EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(D,6);
+      return Motion(v);
+    }
 
 
   /* [CRBA] ForceSet operator* (Inertia Y,Constraint S) */
@@ -107,6 +110,7 @@ namespace se3
   }
 
   struct JointFreeFlyer;
+
   template<>
   struct traits<JointFreeFlyer>
   {
@@ -143,17 +147,30 @@ namespace se3
     JointDataFreeFlyer() : M(1)
     {
     }
-  }; // Struct JointDataFreeFlyer
+
+    JointDataDense<NQ, NV> toDense_impl() const
+    {
+      return JointDataDense<NQ, NV>(S, M, v, c, F);
+    }
+  }; // struct JointDataFreeFlyer
 
   struct JointModelFreeFlyer : public JointModelBase<JointModelFreeFlyer>
   {
     typedef JointFreeFlyer Joint;
     SE3_JOINT_TYPEDEF;
 
-    JointData createData() const { return JointData(); }
+    using JointModelBase<JointModelFreeFlyer>::id;
+    using JointModelBase<JointModelFreeFlyer>::idx_q;
+    using JointModelBase<JointModelFreeFlyer>::idx_v;
+    using JointModelBase<JointModelFreeFlyer>::lowerPosLimit;
+    using JointModelBase<JointModelFreeFlyer>::upperPosLimit;
+    using JointModelBase<JointModelFreeFlyer>::maxEffortLimit;
+    using JointModelBase<JointModelFreeFlyer>::maxVelocityLimit;
+    using JointModelBase<JointModelFreeFlyer>::setIndexes;
 
+    JointData createData() const { return JointData(); }
     void calc( JointData& data,
-	       const Eigen::VectorXd & qs) const
+         const Eigen::VectorXd & qs) const
     {
       Eigen::VectorXd::ConstFixedSegmentReturnType<NQ>::Type q = qs.segment<NQ>(idx_q());
       const JointData::Quaternion quat(Eigen::Matrix<double,4,1> (q.tail <4> ())); // TODO
@@ -161,10 +178,9 @@ namespace se3
       data.M.rotation (quat.matrix());
       data.M.translation (q.head<3>());
     }
-
     void calc( JointData& data, 
-	       const Eigen::VectorXd & qs, 
-	       const Eigen::VectorXd & vs ) const
+         const Eigen::VectorXd & qs, 
+         const Eigen::VectorXd & vs ) const
     {
       Eigen::VectorXd::ConstFixedSegmentReturnType<NQ>::Type q = qs.segment<NQ>(idx_q());
       data.v = vs.segment<NV>(idx_v());
@@ -174,7 +190,24 @@ namespace se3
       data.M.rotation (quat.matrix());
       data.M.translation (q.head<3>());
     }
-  }; // Struct JointModelFreeFlyer
+
+    JointModelDense<NQ, NV> toDense_impl() const
+    {
+      return JointModelDense<NQ, NV>( id(),
+                                      idx_q(),
+                                      idx_v(),
+                                      lowerPosLimit(),
+                                      upperPosLimit(),
+                                      maxEffortLimit(),
+                                      maxVelocityLimit()
+                                    );
+    }
+
+    bool operator == (const JointModelFreeFlyer& /*Ohter*/) const
+    {
+      return true; // TODO ?? used to bind variant in python
+    }
+  }; // struct JointModelFreeFlyer
 
 } // namespace se3
 
diff --git a/src/multibody/joint/joint-generic.hpp b/src/multibody/joint/joint-generic.hpp
index b7a88b6efe..ea5d647e58 100644
--- a/src/multibody/joint/joint-generic.hpp
+++ b/src/multibody/joint/joint-generic.hpp
@@ -20,6 +20,7 @@
 
 #include "pinocchio/exception.hpp"
 #include "pinocchio/multibody/joint/joint-base.hpp"
+#include "pinocchio/multibody/joint/joint-dense.hpp"
 #include "pinocchio/multibody/joint/joint-variant.hpp"
 
 namespace se3
diff --git a/src/multibody/joint/joint-planar.hpp b/src/multibody/joint/joint-planar.hpp
index 52d74bff5d..244223df7e 100644
--- a/src/multibody/joint/joint-planar.hpp
+++ b/src/multibody/joint/joint-planar.hpp
@@ -19,6 +19,7 @@
 #define __se3_joint_planar_hpp__
 
 #include "pinocchio/multibody/joint/joint-base.hpp"
+#include "pinocchio/multibody/joint/joint-dense.hpp"
 #include "pinocchio/multibody/constraint.hpp"
 #include "pinocchio/math/sincos.hpp"
 #include "pinocchio/spatial/motion.hpp"
@@ -121,6 +122,7 @@ namespace se3
   {
 
     SPATIAL_TYPEDEF_NO_TEMPLATE(ConstraintPlanar);
+    enum { NV = 3, Options = 0 }; // to check
     typedef traits<ConstraintPlanar>::JointMotion JointMotion;
     typedef traits<ConstraintPlanar>::JointForce JointForce;
     typedef traits<ConstraintPlanar>::DenseBase DenseBase;
@@ -129,6 +131,8 @@ namespace se3
     Motion operator* (const MotionPlanar & vj) const
     { return vj; }
 
+    int nv_impl() const { return NV; }
+
 
     struct ConstraintTranspose
     {
@@ -264,7 +268,14 @@ namespace se3
     Motion_t v;
     Bias_t c;
 
+    F_t F; // TODO if not used anymore, clean F_t
+
     JointDataPlanar () : M(1) {}
+
+    JointDataDense<NQ, NV> toDense_impl() const
+    {
+      return JointDataDense<NQ, NV>(S, M, v, c, F);
+    }
   }; // struct JointDataPlanar
 
   struct JointModelPlanar : public JointModelBase<JointModelPlanar>
@@ -272,6 +283,15 @@ namespace se3
     typedef JointPlanar Joint;
     SE3_JOINT_TYPEDEF;
 
+    using JointModelBase<JointModelPlanar>::id;
+    using JointModelBase<JointModelPlanar>::idx_q;
+    using JointModelBase<JointModelPlanar>::idx_v;
+    using JointModelBase<JointModelPlanar>::lowerPosLimit;
+    using JointModelBase<JointModelPlanar>::upperPosLimit;
+    using JointModelBase<JointModelPlanar>::maxEffortLimit;
+    using JointModelBase<JointModelPlanar>::maxVelocityLimit;
+    using JointModelBase<JointModelPlanar>::setIndexes;
+
     JointData createData() const { return JointData(); }
 
     void calc (JointData & data,
@@ -302,6 +322,23 @@ namespace se3
       data.v.y_dot_ = q_dot(1);
       data.v.theta_dot_ = q_dot(2);
     }
+
+    JointModelDense<NQ, NV> toDense_impl() const
+    {
+      return JointModelDense<NQ, NV>( id(),
+                                      idx_q(),
+                                      idx_v(),
+                                      lowerPosLimit(),
+                                      upperPosLimit(),
+                                      maxEffortLimit(),
+                                      maxVelocityLimit()
+                                    );
+    }
+
+    bool operator == (const JointModelPlanar& /*Ohter*/) const
+    {
+      return true; // TODO ?? used to bind variant in python
+    }
   }; // struct JointModelPlanar
 
 } // namespace se3
diff --git a/src/multibody/joint/joint-prismatic.hpp b/src/multibody/joint/joint-prismatic.hpp
index abba769689..40e23fb7f2 100644
--- a/src/multibody/joint/joint-prismatic.hpp
+++ b/src/multibody/joint/joint-prismatic.hpp
@@ -19,6 +19,7 @@
 #define __se3_joint_prismatic_hpp__
 
 #include "pinocchio/multibody/joint/joint-base.hpp"
+#include "pinocchio/multibody/joint/joint-dense.hpp"
 #include "pinocchio/multibody/constraint.hpp"
 #include "pinocchio/spatial/inertia.hpp"
 
@@ -154,6 +155,8 @@ namespace se3
      return res;
     }
 
+    int nv_impl() const { return NV; }
+
     struct TransposeConst
     {
       const ConstraintPrismatic & ref; 
@@ -346,6 +349,12 @@ namespace se3
     {
       M.rotation(SE3::Matrix3::Identity());
     }
+
+    JointDataDense<NQ, NV> toDense_impl() const
+    {
+      return JointDataDense<NQ, NV>(S, M, v, c, F);
+    }
+
   }; // struct JointDataPrismatic
 
   template<int axis>
@@ -354,8 +363,13 @@ namespace se3
     typedef JointPrismatic<axis> Joint;
     SE3_JOINT_TYPEDEF_TEMPLATE;
 
+    using JointModelBase<JointModelPrismatic>::id;
     using JointModelBase<JointModelPrismatic>::idx_q;
     using JointModelBase<JointModelPrismatic>::idx_v;
+    using JointModelBase<JointModelPrismatic>::lowerPosLimit;
+    using JointModelBase<JointModelPrismatic>::upperPosLimit;
+    using JointModelBase<JointModelPrismatic>::maxEffortLimit;
+    using JointModelBase<JointModelPrismatic>::maxVelocityLimit;
     using JointModelBase<JointModelPrismatic>::setIndexes;
     
     JointData createData() const { return JointData(); }
@@ -376,6 +390,23 @@ namespace se3
       data.M.translation(JointPrismatic<axis>::cartesianTranslation(q));
       data.v.v = v;
     }
+
+    JointModelDense<NQ, NV> toDense_impl() const
+    {
+      return JointModelDense<NQ, NV>( id(),
+                                      idx_q(),
+                                      idx_v(),
+                                      lowerPosLimit(),
+                                      upperPosLimit(),
+                                      maxEffortLimit(),
+                                      maxVelocityLimit()
+                                    );
+    }
+
+    bool operator == (const JointModelPrismatic& /*Ohter*/) const
+    {
+      return true; // TODO ?? used to bind variant in python
+    }
   }; // struct JointModelPrismatic
 
   typedef JointPrismatic<0> JointPX;
diff --git a/src/multibody/joint/joint-revolute-unaligned.hpp b/src/multibody/joint/joint-revolute-unaligned.hpp
index 770cd2b3b1..3b18f608bb 100644
--- a/src/multibody/joint/joint-revolute-unaligned.hpp
+++ b/src/multibody/joint/joint-revolute-unaligned.hpp
@@ -19,6 +19,7 @@
 #define __se3_joint_revolute_unaligned_hpp__
 
 #include "pinocchio/multibody/joint/joint-base.hpp"
+#include "pinocchio/multibody/joint/joint-dense.hpp"
 #include "pinocchio/multibody/constraint.hpp"
 #include "pinocchio/spatial/inertia.hpp"
 #include "pinocchio/math/sincos.hpp"
@@ -113,6 +114,10 @@ namespace se3
     struct ConstraintRevoluteUnaligned : ConstraintBase < ConstraintRevoluteUnaligned >
     {
       SPATIAL_TYPEDEF_NO_TEMPLATE(ConstraintRevoluteUnaligned);
+      enum { NV = 1, Options = 0 };
+      typedef traits<ConstraintRevoluteUnaligned>::JointMotion JointMotion;
+      typedef traits<ConstraintRevoluteUnaligned>::JointForce JointForce;
+      typedef traits<ConstraintRevoluteUnaligned>::DenseBase DenseBase;
 
       ConstraintRevoluteUnaligned() : axis(Eigen::Vector3d::Constant(NAN)) {}
       ConstraintRevoluteUnaligned(const Motion::Vector3 & _axis) : axis(_axis) {}
@@ -134,6 +139,8 @@ namespace se3
         return res;
       }
 
+      int nv_impl() const { return NV; }
+
       struct TransposeConst
       {
       	const ConstraintRevoluteUnaligned & ref; 
@@ -250,15 +257,25 @@ namespace se3
       : M(1),S(axis),v(axis,NAN)
       ,angleaxis(NAN,axis)
     {}
-  };
+
+    JointDataDense<NQ, NV> toDense_impl() const
+    {
+      return JointDataDense<NQ, NV>(S, M, v, c, F);
+    }
+  }; // struct JointDataRevoluteUnaligned
 
   struct JointModelRevoluteUnaligned : public JointModelBase< JointModelRevoluteUnaligned >
   {
     typedef JointRevoluteUnaligned Joint;
     SE3_JOINT_TYPEDEF;
 
+    using JointModelBase<JointModelRevoluteUnaligned>::id;
     using JointModelBase<JointModelRevoluteUnaligned>::idx_q;
     using JointModelBase<JointModelRevoluteUnaligned>::idx_v;
+    using JointModelBase<JointModelRevoluteUnaligned>::lowerPosLimit;
+    using JointModelBase<JointModelRevoluteUnaligned>::upperPosLimit;
+    using JointModelBase<JointModelRevoluteUnaligned>::maxEffortLimit;
+    using JointModelBase<JointModelRevoluteUnaligned>::maxVelocityLimit;
     using JointModelBase<JointModelRevoluteUnaligned>::setIndexes;
     
     JointModelRevoluteUnaligned() : axis(Eigen::Vector3d::Constant(NAN))   {}
@@ -296,7 +313,24 @@ namespace se3
     }
 
     Eigen::Vector3d axis;
-  };
+
+    JointModelDense<NQ, NV> toDense_impl() const
+    {
+      return JointModelDense<NQ, NV>( id(),
+                                      idx_q(),
+                                      idx_v(),
+                                      lowerPosLimit(),
+                                      upperPosLimit(),
+                                      maxEffortLimit(),
+                                      maxVelocityLimit()
+                                    );
+    }
+
+    bool operator == (const JointModelRevoluteUnaligned& /*Ohter*/) const
+    {
+      return true; // TODO ?? used to bind variant in python
+    }
+  }; // struct JointModelRevoluteUnaligned
 
 } //namespace se3
 
diff --git a/src/multibody/joint/joint-revolute.hpp b/src/multibody/joint/joint-revolute.hpp
index b35e5617ff..b71ef48c14 100644
--- a/src/multibody/joint/joint-revolute.hpp
+++ b/src/multibody/joint/joint-revolute.hpp
@@ -18,10 +18,10 @@
 #ifndef __se3_joint_revolute_hpp__
 #define __se3_joint_revolute_hpp__
 
-#include "pinocchio/multibody/joint/joint-base.hpp"
-#include "pinocchio/multibody/constraint.hpp"
-#include "pinocchio/spatial/inertia.hpp"
 #include "pinocchio/math/sincos.hpp"
+#include "pinocchio/spatial/inertia.hpp"
+#include "pinocchio/multibody/joint/joint-base.hpp"
+#include "pinocchio/multibody/joint/joint-dense.hpp"
 
 namespace se3
 {
@@ -37,12 +37,11 @@ namespace se3
       double w; 
       CartesianVector3(const double & w) : w(w) {}
       CartesianVector3() : w(1) {}
-      operator Eigen::Vector3d ();
-    }; // struct CartesianVector3
+      operator Eigen::Vector3d (); // { return Eigen::Vector3d(w,0,0); }
+    };
     template<>CartesianVector3<0>::operator Eigen::Vector3d () { return Eigen::Vector3d(w,0,0); }
     template<>CartesianVector3<1>::operator Eigen::Vector3d () { return Eigen::Vector3d(0,w,0); }
     template<>CartesianVector3<2>::operator Eigen::Vector3d () { return Eigen::Vector3d(0,0,w); }
-
     Eigen::Vector3d operator+ (const Eigen::Vector3d & w1,const CartesianVector3<0> & wx)
     { return Eigen::Vector3d(w1[0]+wx.w,w1[1],w1[2]); }
     Eigen::Vector3d operator+ (const Eigen::Vector3d & w1,const CartesianVector3<1> & wy)
@@ -52,6 +51,7 @@ namespace se3
   } // namespace revolute
 
   template<int axis> struct MotionRevolute;
+
   template<int axis>
   struct traits< MotionRevolute < axis > >
   {
@@ -106,6 +106,7 @@ namespace se3
   }
 
   template<int axis> struct ConstraintRevolute;
+
   template<int axis>
   struct traits< ConstraintRevolute<axis> >
   {
@@ -154,6 +155,8 @@ namespace se3
       return res;
     }
 
+    int nv_impl() const { return NV; }
+    
     struct TransposeConst
     {
       const ConstraintRevolute<axis> & ref; 
@@ -182,8 +185,8 @@ namespace se3
      *   - MatrixBase operator* (Constraint::Transpose S, ForceSet::Block)
      *   - SE3::act(ForceSet::Block)
      */
-    operator ConstraintXd () const
-    {
+     operator ConstraintXd () const
+     {
       Eigen::Matrix<double,6,1> S;
       S << Eigen::Vector3d::Zero(), (Eigen::Vector3d)revolute::CartesianVector3<axis>();
       return ConstraintXd(S);
@@ -373,6 +376,11 @@ namespace se3
     {
       M.translation(SE3::Vector3::Zero());
     }
+
+    JointDataDense<NQ, NV> toDense_impl() const
+    {
+      return JointDataDense<NQ, NV>(S, M, v, c, F);
+    }
   }; // struct JointDataRevolute
 
   template<int axis>
@@ -381,8 +389,13 @@ namespace se3
     typedef JointRevolute<axis> Joint;
     SE3_JOINT_TYPEDEF_TEMPLATE;
 
+    using JointModelBase<JointModelRevolute>::id;
     using JointModelBase<JointModelRevolute>::idx_q;
     using JointModelBase<JointModelRevolute>::idx_v;
+    using JointModelBase<JointModelRevolute>::lowerPosLimit;
+    using JointModelBase<JointModelRevolute>::upperPosLimit;
+    using JointModelBase<JointModelRevolute>::maxEffortLimit;
+    using JointModelBase<JointModelRevolute>::maxVelocityLimit;
     using JointModelBase<JointModelRevolute>::setIndexes;
     
     JointData createData() const { return JointData(); }
@@ -404,6 +417,22 @@ namespace se3
       data.v.w = v;
     }
 
+    JointModelDense<NQ, NV> toDense_impl() const
+    {
+      return JointModelDense<NQ, NV>( id(),
+                                      idx_q(),
+                                      idx_v(),
+                                      lowerPosLimit(),
+                                      upperPosLimit(),
+                                      maxEffortLimit(),
+                                      maxVelocityLimit()
+                                    );
+    }
+
+    bool operator == (const JointModelRevolute<axis>& /*Other*/) const
+    {
+      return true; // TODO ??
+    }
 
   }; // struct JointModelRevolute
 
diff --git a/src/multibody/joint/joint-spherical-ZYX.hpp b/src/multibody/joint/joint-spherical-ZYX.hpp
index 82bd3ace36..bb8a50b083 100644
--- a/src/multibody/joint/joint-spherical-ZYX.hpp
+++ b/src/multibody/joint/joint-spherical-ZYX.hpp
@@ -19,6 +19,7 @@
 #define __se3_joint_spherical_ZYX_hpp__
 #include <iostream>
 #include "pinocchio/multibody/joint/joint-base.hpp"
+#include "pinocchio/multibody/joint/joint-dense.hpp"
 #include "pinocchio/multibody/constraint.hpp"
 #include "pinocchio/math/sincos.hpp"
 #include "pinocchio/spatial/inertia.hpp"
@@ -51,6 +52,8 @@ namespace se3
 
       operator Motion () const { return Motion (Motion::Vector3::Zero (), c_J); }
 
+      operator BiasZero() const { return BiasZero();}
+
       typename MotionTpl<Scalar,Options>::Vector3 & operator() () { return c_J; }
       const typename MotionTpl<Scalar,Options>::Vector3 & operator() () const { return c_J; }
 
@@ -87,12 +90,11 @@ namespace se3
 
     struct ConstraintRotationalSubspace
     {
-    public:
+      enum { NV = 3, Options = 0 };
       typedef _Scalar Scalar;
       typedef Eigen::Matrix <_Scalar,3,3,_Options> Matrix3;
       typedef Eigen::Matrix <_Scalar,3,1,_Options> Vector3;
 
-    public:
       Matrix3 S_minimal;
 
       Motion operator* (const MotionSpherical & vj) const
@@ -107,6 +109,8 @@ namespace se3
       Matrix3 &  matrix () { return S_minimal; }
       const Matrix3 & matrix () const { return S_minimal; }
 
+      int nv_impl() const { return NV; }
+
       struct ConstraintTranspose
       {
         const ConstraintRotationalSubspace & ref;
@@ -242,17 +246,33 @@ namespace se3
     Motion_t v;
     Bias_t c;
 
+    F_t F;
+
     JointDataSphericalZYX () : M(1)
     {
       M.translation (Transformation_t::Vector3::Zero ());
     }
-  };
+
+    JointDataDense<NQ, NV> toDense_impl() const
+    {
+      return JointDataDense<NQ, NV>(S, M, v, c, F);
+    }
+  }; // strcut JointDataSphericalZYX
 
   struct JointModelSphericalZYX : public JointModelBase<JointModelSphericalZYX>
   {
     typedef JointSphericalZYX Joint;
     SE3_JOINT_TYPEDEF;
 
+    using JointModelBase<JointModelSphericalZYX>::id;
+    using JointModelBase<JointModelSphericalZYX>::idx_q;
+    using JointModelBase<JointModelSphericalZYX>::idx_v;
+    using JointModelBase<JointModelSphericalZYX>::lowerPosLimit;
+    using JointModelBase<JointModelSphericalZYX>::upperPosLimit;
+    using JointModelBase<JointModelSphericalZYX>::maxEffortLimit;
+    using JointModelBase<JointModelSphericalZYX>::maxVelocityLimit;
+    using JointModelBase<JointModelSphericalZYX>::setIndexes;
+
     JointData createData() const { return JointData(); }
 
     void calc (JointData & data,
@@ -307,7 +327,25 @@ namespace se3
       data.c ()(1) = -s1 * s2 * q_dot (0) * q_dot (1) + c1 * c2 * q_dot (0) * q_dot (2) - s2 * q_dot (1) * q_dot (2);
       data.c ()(2) = -s1 * c2 * q_dot (0) * q_dot (1) - c1 * s2 * q_dot (0) * q_dot (2) - c2 * q_dot (1) * q_dot (2);
     }
-  };
+
+    JointModelDense<NQ, NV> toDense_impl() const
+    {
+      return JointModelDense<NQ, NV>( id(),
+                                      idx_q(),
+                                      idx_v(),
+                                      lowerPosLimit(),
+                                      upperPosLimit(),
+                                      maxEffortLimit(),
+                                      maxVelocityLimit()
+                                    );
+    }
+
+    bool operator == (const JointModelSphericalZYX& /*Ohter*/) const
+    {
+      return true; // TODO ?? used to bind variant in python
+    }
+
+  }; // struct JointModelSphericalZYX
 
 } // namespace se3
 
diff --git a/src/multibody/joint/joint-spherical.hpp b/src/multibody/joint/joint-spherical.hpp
index f74dcf0e34..c079a8253a 100644
--- a/src/multibody/joint/joint-spherical.hpp
+++ b/src/multibody/joint/joint-spherical.hpp
@@ -19,6 +19,7 @@
 #define __se3_joint_spherical_hpp__
 
 #include "pinocchio/multibody/joint/joint-base.hpp"
+#include "pinocchio/multibody/joint/joint-dense.hpp"
 #include "pinocchio/multibody/constraint.hpp"
 #include "pinocchio/math/sincos.hpp"
 #include "pinocchio/spatial/inertia.hpp"
@@ -111,6 +112,7 @@ namespace se3
   struct ConstraintRotationalSubspace
   {
     SPATIAL_TYPEDEF_NO_TEMPLATE(ConstraintRotationalSubspace);
+    enum { NV = 3, Options = 0 };
     typedef traits<ConstraintRotationalSubspace>::JointMotion JointMotion;
     typedef traits<ConstraintRotationalSubspace>::JointForce JointForce;
     typedef traits<ConstraintRotationalSubspace>::DenseBase DenseBase;
@@ -119,6 +121,8 @@ namespace se3
     // Motion operator* (const MotionSpherical & vj) const
     // { return ??; }
 
+    int nv_impl() const { return NV; }
+    
     struct TransposeConst
     {
       Force::Vector3 operator* (const Force & phi)
@@ -218,8 +222,15 @@ namespace se3
     Motion_t v;
     Bias_t c;
 
+    F_t F;
+
     JointDataSpherical () : M(1)
     {}
+
+    JointDataDense<NQ, NV> toDense_impl() const
+    {
+      return JointDataDense<NQ, NV>(S, M, v, c, F);
+    }
   }; // struct JointDataSpherical
 
   struct JointModelSpherical : public JointModelBase<JointModelSpherical>
@@ -227,6 +238,15 @@ namespace se3
     typedef JointSpherical Joint;
     SE3_JOINT_TYPEDEF;
 
+    using JointModelBase<JointModelSpherical>::id;
+    using JointModelBase<JointModelSpherical>::idx_q;
+    using JointModelBase<JointModelSpherical>::idx_v;
+    using JointModelBase<JointModelSpherical>::lowerPosLimit;
+    using JointModelBase<JointModelSpherical>::upperPosLimit;
+    using JointModelBase<JointModelSpherical>::maxEffortLimit;
+    using JointModelBase<JointModelSpherical>::maxVelocityLimit;
+    using JointModelBase<JointModelSpherical>::setIndexes;
+
     JointData createData() const { return JointData(); }
 
     void calc (JointData & data,
@@ -249,6 +269,23 @@ namespace se3
       const JointData::Quaternion quat(Eigen::Matrix<double,4,1> (q.tail <4> ())); // TODO
       data.M.rotation (quat.matrix ());
     }
+
+    JointModelDense<NQ, NV> toDense_impl() const
+    {
+      return JointModelDense<NQ, NV>( id(),
+                                      idx_q(),
+                                      idx_v(),
+                                      lowerPosLimit(),
+                                      upperPosLimit(),
+                                      maxEffortLimit(),
+                                      maxVelocityLimit()
+                                    );
+    }
+
+    bool operator == (const JointModelSpherical& /*Ohter*/) const
+    {
+      return true; // TODO ?? used to bind variant in python
+    }
   }; // struct JointModelSpherical
 
 } // namespace se3
diff --git a/src/multibody/joint/joint-translation.hpp b/src/multibody/joint/joint-translation.hpp
index 0ac6209bdd..8ee94c1e08 100644
--- a/src/multibody/joint/joint-translation.hpp
+++ b/src/multibody/joint/joint-translation.hpp
@@ -19,6 +19,7 @@
 #define __se3_joint_translation_hpp__
 
 #include "pinocchio/multibody/joint/joint-base.hpp"
+#include "pinocchio/multibody/joint/joint-dense.hpp"
 #include "pinocchio/multibody/constraint.hpp"
 #include "pinocchio/spatial/inertia.hpp"
 #include "pinocchio/spatial/skew.hpp"
@@ -117,6 +118,7 @@ namespace se3
   struct ConstraintTranslationSubspace : ConstraintBase < ConstraintTranslationSubspace >
   {
     SPATIAL_TYPEDEF_NO_TEMPLATE(ConstraintTranslationSubspace);
+    enum { NV = 3, Options = 0 };
     typedef traits<ConstraintTranslationSubspace>::JointMotion JointMotion;
     typedef traits<ConstraintTranslationSubspace>::JointForce JointForce;
     typedef traits<ConstraintTranslationSubspace>::DenseBase DenseBase;
@@ -125,6 +127,7 @@ namespace se3
     Motion operator* (const MotionTranslation & vj) const
     { return Motion (vj (), Motion::Vector3::Zero ()); }
 
+    int nv_impl() const { return NV; }
 
     struct ConstraintTranspose
     {
@@ -235,7 +238,14 @@ namespace se3
     Motion_t v;
     Bias_t c;
 
+    F_t F; // TODO if not used anymore, clean F_t
+
     JointDataTranslation () : M(1) {}
+
+    JointDataDense<NQ, NV> toDense_impl() const
+    {
+      return JointDataDense<NQ, NV>(S, M, v, c, F);
+    }
   }; // struct JointDataTranslation
 
   struct JointModelTranslation : public JointModelBase<JointModelTranslation>
@@ -243,6 +253,15 @@ namespace se3
     typedef JointTranslation Joint;
     SE3_JOINT_TYPEDEF;
 
+    using JointModelBase<JointModelTranslation>::id;
+    using JointModelBase<JointModelTranslation>::idx_q;
+    using JointModelBase<JointModelTranslation>::idx_v;
+    using JointModelBase<JointModelTranslation>::lowerPosLimit;
+    using JointModelBase<JointModelTranslation>::upperPosLimit;
+    using JointModelBase<JointModelTranslation>::maxEffortLimit;
+    using JointModelBase<JointModelTranslation>::maxVelocityLimit;
+    using JointModelBase<JointModelTranslation>::setIndexes;
+
     JointData createData() const { return JointData(); }
 
     void calc (JointData & data,
@@ -257,6 +276,23 @@ namespace se3
       data.M.translation (qs.segment<NQ> (idx_q ()));
       data.v () = vs.segment<NQ> (idx_v ());
     }
+
+    JointModelDense<NQ, NV> toDense_impl() const
+    {
+      return JointModelDense<NQ, NV>( id(),
+                                      idx_q(),
+                                      idx_v(),
+                                      lowerPosLimit(),
+                                      upperPosLimit(),
+                                      maxEffortLimit(),
+                                      maxVelocityLimit()
+                                    );
+    }
+
+    bool operator == (const JointModelTranslation& /*Ohter*/) const
+    {
+      return true; // TODO ?? used to bind variant in python
+    }
   }; // struct JointModelTranslation
   
 } // namespace se3
diff --git a/src/multibody/joint/joint-variant.hpp b/src/multibody/joint/joint-variant.hpp
index 8372f4969c..8d4a21b8fe 100644
--- a/src/multibody/joint/joint-variant.hpp
+++ b/src/multibody/joint/joint-variant.hpp
@@ -29,8 +29,8 @@
 
 namespace se3
 {
-  typedef boost::variant< JointModelRX, JointModelRY, JointModelRZ, JointModelRevoluteUnaligned, JointModelSpherical, JointModelSphericalZYX, JointModelPX, JointModelPY, JointModelPZ, JointModelFreeFlyer, JointModelPlanar, JointModelTranslation> JointModelVariant;
-  typedef boost::variant< JointDataRX, JointDataRY, JointDataRZ, JointDataRevoluteUnaligned, JointDataSpherical, JointDataSphericalZYX, JointDataPX, JointDataPY, JointDataPZ, JointDataFreeFlyer, JointDataPlanar, JointDataTranslation> JointDataVariant;
+  typedef boost::variant< JointModelRX, JointModelRY, JointModelRZ, JointModelRevoluteUnaligned, JointModelSpherical, JointModelSphericalZYX, JointModelPX, JointModelPY, JointModelPZ, JointModelFreeFlyer, JointModelPlanar, JointModelTranslation,JointModelDense<-1,-1> > JointModelVariant;
+  typedef boost::variant< JointDataRX, JointDataRY, JointDataRZ, JointDataRevoluteUnaligned, JointDataSpherical, JointDataSphericalZYX, JointDataPX, JointDataPY, JointDataPZ, JointDataFreeFlyer, JointDataPlanar, JointDataTranslation,JointDataDense<-1,-1> > JointDataVariant;
 
   typedef std::vector<JointModelVariant> JointModelVector;
   typedef std::vector<JointDataVariant> JointDataVector;
diff --git a/src/simulation/compute-all-terms.hpp b/src/simulation/compute-all-terms.hpp
index a490770f01..cdd8bb0543 100644
--- a/src/simulation/compute-all-terms.hpp
+++ b/src/simulation/compute-all-terms.hpp
@@ -79,7 +79,7 @@ namespace se3
         data.oMi[i] = data.liMi[i];
       }
 
-      data.J.block(0,jmodel.idx_v(),6,jmodel.nv()) = data.oMi[i].act(jdata.S());
+      jmodel.jointCols(data.J) = data.oMi[i].act(jdata.S());
 
       data.a[i]  = jdata.c() + (data.v[i] ^ jdata.v());
       data.a[i] += data.liMi[i].actInv(data.a[parent]);
@@ -113,14 +113,14 @@ namespace se3
       const Model::Index & parent = model.parents[i];
 
       /* F[1:6,i] = Y*S */
-      data.Fcrb[i].block<6,JointModel::NV>(0,jmodel.idx_v()) = data.Ycrb[i] * jdata.S();
+      jmodel.jointCols(data.Fcrb[i]) = data.Ycrb[i] * jdata.S();
 
       /* M[i,SUBTREE] = S'*F[1:6,SUBTREE] */
       data.M.block(jmodel.idx_v(),jmodel.idx_v(),jmodel.nv(),data.nvSubtree[i])
       = jdata.S().transpose()*data.Fcrb[i].block(0,jmodel.idx_v(),6,data.nvSubtree[i]);
 
 
-      jmodel.jointForce(data.nle)  = jdata.S().transpose()*data.f[i];
+      jmodel.jointVelocitySelector(data.nle)  = jdata.S().transpose()*data.f[i];
       if(parent>0)
       {
         /*   Yli += liXi Yi */
diff --git a/src/spatial/inertia.hpp b/src/spatial/inertia.hpp
index 2a88c72175..e7bf36cec5 100644
--- a/src/spatial/inertia.hpp
+++ b/src/spatial/inertia.hpp
@@ -184,8 +184,8 @@ namespace se3
     {
       Matrix6 M;
       const Matrix3 & c_cross = (skew(c));
-      M.template block<3,3>(LINEAR, LINEAR ).template setZero ();
-      M.template block<3,3>(LINEAR, LINEAR ).template diagonal ().template fill (m);
+      M.template block<3,3>(LINEAR, LINEAR ).setZero ();
+      M.template block<3,3>(LINEAR, LINEAR ).diagonal ().fill (m);
       M.template block<3,3>(ANGULAR,LINEAR ) = m * c_cross;
       M.template block<3,3>(LINEAR, ANGULAR) = -M.template block<3,3> (ANGULAR, LINEAR);
       M.template block<3,3>(ANGULAR,ANGULAR) = (Matrix3)(I - M.template block<3,3>(ANGULAR, LINEAR) * c_cross);
diff --git a/unittest/joints.cpp b/unittest/joints.cpp
index b0498eb61e..f7ec4a27d4 100644
--- a/unittest/joints.cpp
+++ b/unittest/joints.cpp
@@ -1016,3 +1016,47 @@ BOOST_AUTO_TEST_CASE ( test_merge_body )
 }
 
 BOOST_AUTO_TEST_SUITE_END ()
+
+BOOST_AUTO_TEST_SUITE ( JointDense )
+
+BOOST_AUTO_TEST_CASE ( toJointModelDense )
+{
+  using namespace se3;
+
+
+  JointModelRX jmodel;
+  jmodel.setIndexes (2, 0, 0);
+
+  JointModelDense<JointModelBase<JointModelRX>::NQ, JointModelBase<JointModelRX>::NV> jmd(jmodel.id(), jmodel.idx_q(), jmodel.idx_v(), jmodel.lowerPosLimit(),
+                          jmodel.upperPosLimit(), jmodel.maxEffortLimit(), jmodel.maxVelocityLimit());
+  JointModelDense<JointModelBase<JointModelRX>::NQ, JointModelBase<JointModelRX>::NV> jmd2 = jmodel.toDense();
+  (void)jmd; (void)jmd2;
+
+  assert(jmd.idx_q() == jmodel.idx_q() && "The comparison of the joint index in configuration space failed");
+  assert(jmd.idx_q() == jmd2.idx_q() && "The comparison of the joint index in  configuration space failed");
+
+  assert(jmd.idx_v() == jmodel.idx_v() && "The comparison of the joint index in velocity space failed");
+  assert(jmd.idx_v() == jmd2.idx_v() && "The comparison of the joint index in  velocity space failed");
+
+  assert(jmd.id() == jmodel.id() && "The comparison of the joint index in model's kinematic tree failed");
+  assert(jmd.id() == jmd2.id() && "The comparison of the joint index in model's kinematic tree failed");
+
+}
+
+BOOST_AUTO_TEST_CASE ( toJointDataDense )
+{
+  using namespace se3;
+
+  JointModelRX jmodel;
+  jmodel.setIndexes (2, 0, 0);
+
+  JointDataRX jdata;
+
+  JointDataDense< JointDataBase<JointModelRX::JointData>::NQ,
+                  JointDataBase<JointModelRX::JointData>::NV
+                                                                      > jdd = jdata.toDense();
+
+  assert(jdata.S.nv() == jdd.S.nv() && "");
+
+}
+BOOST_AUTO_TEST_SUITE_END ()