Merge pull request #43 from fvalenza/topic/JointDense

Topic/joint dense

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

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>() ;
 

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]) 

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());
     }
 
   };

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();
     }
 
   };

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]);
     }
   };

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]);
     }
   };

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]);
     }
   };

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;

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"