First test with variant?

CMakeLists.txt

@@ -71,6 +71,9 @@ PKG_CONFIG_USE_DEPENDENCY(tspatial eigenpy)
 ADD_EXECUTABLE(rnea EXCLUDE_FROM_ALL unittest/rnea.cpp)
 PKG_CONFIG_USE_DEPENDENCY(rnea eigenpy)
 
+ADD_EXECUTABLE(variant EXCLUDE_FROM_ALL unittest/variant.cpp)
+PKG_CONFIG_USE_DEPENDENCY(variant eigenpy)
+
 SETUP_PROJECT_FINALIZE()
 SETUP_PROJECT_CPACK()
 

src/multibody/joint.hpp

@@ -37,7 +37,7 @@ namespace se3
   {
     typedef typename traits<Joint>::JointData JointData;
 
-    JointData createData() const { return static_cast<Joint*>(this)->createData(); }
+    JointData createData() const { return static_cast<const Joint*>(this)->createData(); }
     void calc( JointData& data, 
 	       const Eigen::VectorXd & qs, 
 	       const Eigen::VectorXd & vs, 
@@ -51,11 +51,17 @@ namespace se3
   };
 
 
+  struct JointDataRX;
+  struct JointModelRX;
+
+  struct JointDataFreeFlyer;
+  struct JointModelFreeFlyer;
+
+
+
   /* --- REVOLUTE X --------------------------------------------------------- */
   /* --- REVOLUTE X --------------------------------------------------------- */
   /* --- REVOLUTE X --------------------------------------------------------- */
-  struct JointDataRX;
-  struct JointModelRX;
 
   template<>
   struct traits<JointDataRX>
@@ -93,6 +99,8 @@ namespace se3
     JointDataRX() : M(1)
    {
       S << 0,0,0,1,0,0;
+      M.translation(SE3::Vector3::Zero());
+      v.linear(Motion::Vector3::Zero());
     }
   };
 
@@ -142,6 +150,88 @@ namespace se3
   };
 
 
+
+  /* --- REVOLUTE FF -------------------------------------------------------- */
+  /* --- REVOLUTE FF -------------------------------------------------------- */
+  /* --- REVOLUTE FF -------------------------------------------------------- */
+
+  template<>
+  struct traits<JointDataFreeFlyer>
+  {
+    struct BiasZero {};
+    friend const Motion & operator+ ( const Motion& v, const BiasZero&) { return v; }
+   
+    typedef Eigen::Matrix<double,6,6> Matrix6;
+    typedef Eigen::Quaternion<double> Quaternion;
+    typedef Eigen::Matrix<double,3,1> Vector3;
+
+    typedef Matrix6 Constraint_t;
+    typedef SE3 Transformation_t;
+    typedef Motion Velocity_t;
+    typedef BiasZero Bias_t;
+    typedef JointModelFreeFlyer JointModel;
+    typedef Eigen::Matrix<double,6,1> JointMotion_t;
+  };
+
+  struct JointDataFreeFlyer : public JointDataBase<JointDataFreeFlyer>
+  {
+    typedef typename traits<JointDataFreeFlyer>::Constraint_t Constraint_t;
+    typedef typename traits<JointDataFreeFlyer>::Transformation_t Transformation_t;
+    typedef typename traits<JointDataFreeFlyer>::Velocity_t Velocity_t;
+    typedef typename traits<JointDataFreeFlyer>::Bias_t Bias_t;
+    typedef typename traits<JointDataFreeFlyer>::JointMotion_t JointMotion_t;
+    typedef typename traits<JointDataFreeFlyer>::JointModel JointModel;
+
+    typedef Eigen::Matrix<double,6,6> Matrix6;
+    typedef Eigen::Quaternion<double> Quaternion;
+    typedef Eigen::Matrix<double,3,1> Vector3;
+    
+    Constraint_t S;
+    Transformation_t M;
+    Velocity_t v;
+    Bias_t c;
+    JointMotion_t qdd;
+
+    Quaternion rotation;
+
+    JointDataFreeFlyer() : M(1)
+    {
+     S = Eigen::Matrix<double,6,6>::Identity();
+    }
+  };
+
+  template<>
+  struct traits<JointModelFreeFlyer>
+  {
+    typedef JointDataFreeFlyer JointData;
+  };
+
+  struct JointModelFreeFlyer : public JointModelBase<JointModelFreeFlyer>
+  {
+    typedef traits<JointModelFreeFlyer>::JointData JointData;
+
+    static const int nq = 7;
+    static const int nv = 6;
+    int idx_q,idx_v;
+
+    JointModelFreeFlyer() : idx_q(-1),idx_v(-1) {} // Default constructor for std::vector
+    JointModelFreeFlyer( int index_q,int index_v ) : idx_q(index_q),idx_v(index_v) {}
+
+    JointData createData() const { return JointData(); }
+    void calc( JointData& data, 
+	       const Eigen::VectorXd & qs, 
+	       const Eigen::VectorXd & vs, 
+	       const Eigen::VectorXd & as ) const
+    {
+      Eigen::VectorXd::ConstFixedSegmentReturnType<nq>::Type q = qs.segment<nq>(idx_q);
+      data.v = vs .segment<nv>(idx_v);
+      data.qdd = as.segment<nv>(idx_v);
+
+      JointData::Quaternion quat(Eigen::Matrix<double,4,1>(q.tail(4)));
+      data.M = SE3(quat.matrix(),q.head<3>());
+    }
+  };
+
 } // namespace se3
 
 #endif // ifndef __se3_joint_hpp__

unittest/rnea.cpp

@@ -114,34 +114,12 @@ int main()
     {
   for( int i=1;i<model.nbody;++i )
     {
-      // JointModelRX & jmodel = model.joints[i];
-      // JointDataRX & jdata = data.joints[i];
-      // jmodel.calc(jdata,q,v,a);
-
-      // const Model::Index & parent = model.parents[i];
-      // const SE3 & liMi = data.liMi[i] = model.jointPlacements[i]*jdata.M;
-      
-      // if(parent>0) data.oMi[i] = data.oMi[parent]*liMi;
-      // else         data.oMi[i] = liMi;
-
-      // data.v[i] = jdata.v;
-      // if(parent>0) data.v[i] += liMi.actInv(data.v[parent]);
-
-      // data.a[i] =  Motion(jdata.S*jdata.qdd) + jdata.c + data.v[i].cross(jdata.v); 
-      // if(parent>0) data.a[i] += liMi.actInv(data.a[parent]);
-
-      // data.f[i] = model.inertias[i]*data.a[i] + model.inertias[i].vxiv(data.v[i]); // -f_ext
       rneaForwardStep(model,data,model.joints[i],data.joints[i],i,q,v,a);
     }
 
   for( int i=model.nbody-1;i>0;--i )
     {
-      const Model::Index & parent = model.parents[i];
-      JointModelRX & jmodel = model.joints[i];
-      
-      data.tau.segment(jmodel.idx_v,jmodel.nv) = data.joints[i].S.transpose()*data.f[i].toVector();
-      if(parent>0) data.f[parent] += data.liMi[i].act(data.f[i]);
-      //rneaBackwardStep(model,data,model.joints[i],data.joints[i],i);
+      rneaBackwardStep(model,data,model.joints[i],data.joints[i],i);
     }
     }
   timer.toc(std::cout,1000);

unittest/variant.cpp

+#include "pinocchio/spatial/fwd.hpp"
+#include "pinocchio/spatial/se3.hpp"
+#include "pinocchio/multibody/joint.hpp"
+#include "pinocchio/multibody/model.hpp"
+
+#include <iostream>
+
+#include "pinocchio/tools/timer.hpp"
+
+#include <boost/variant.hpp>
+
+
+using namespace se3;
+typedef boost::variant< JointModelRX,JointModelFreeFlyer> JointModel;
+typedef boost::variant< JointDataRX,JointDataFreeFlyer> JointData;
+
+
+class CreateJointData: public boost::static_visitor<JointData>
+{
+public:
+  template<typename D>
+  JointData operator()(const se3::JointModelBase<D> & jmodel) const
+  { return JointData(jmodel.createData()); }
+
+  static JointData run( const JointModel & jmodel)
+  { return boost::apply_visitor( CreateJointData(), jmodel ); }
+};
+
+
+int main()
+{
+  using namespace Eigen;
+
+
+  JointModel jmodel = JointModelRX(0,0);
+  const JointData & jdata = CreateJointData::run(jmodel); //x boost::apply_visitor( CreateJointData(), jmodel );
+
+
+  se3::Model model;
+}