remove stuff dependent on urdf

include/sot-dynamic/dynamic.h

@@ -47,7 +47,6 @@
 /* PINOCCHIO */
 #include <pinocchio/multibody/model.hpp>
 #include <pinocchio/multibody/joint/joint-variant.hpp>
-#include <pinocchio/parsers/urdf.hpp>
 #include <pinocchio/algorithm/rnea.hpp>
 #include <pinocchio/algorithm/jacobian.hpp>
 #include <pinocchio/algorithm/frames.hpp>

src/python-module-py.cpp

@@ -86,10 +86,6 @@ namespace dynamicgraph{
       pointer1 = PyCObject_AsVoidPtr(object);
       Dynamic* dyn_entity = (Dynamic*) pointer1;
 
-      std::string msg("Error in obtaining pinocchio model");
-      PyObject* dgpyError =
-	PyErr_NewException(const_cast<char*>(msg.c_str()), NULL, NULL);
-      
       try {
 	se3::python::ModelHandler cppModelHandle = 
 	  boost::python::extract<se3::python::ModelHandler>(pyPinocchioObject);

src/sot-dynamic.cpp

@@ -286,21 +286,29 @@ dg::Vector Dynamic::getPinocchioPos(int time)
 
   if( freeFlyerPositionSIN) {
     dg::Vector qFF=freeFlyerPositionSIN.access(time);
+
+
     q.resize(qJoints.size() + 7);
-    urdf::Rotation rot;
-    rot.setFromRPY(qFF(3),qFF(4),qFF(5));
-    double x,y,z,w;
-    rot.getQuaternion(x,y,z,w);
-    q << qFF(0),qFF(1),qFF(2),x,y,z,w,qJoints;
+
+    Eigen::Quaternion<double> quat = Eigen::AngleAxisd(qFF(5),Eigen::Vector3d::UnitZ())*
+      Eigen::AngleAxisd(qFF(4),Eigen::Vector3d::UnitY())*
+      Eigen::AngleAxisd(qFF(3),Eigen::Vector3d::UnitX());
+    
+    q << qFF(0),qFF(1),qFF(2),
+      quat.x(),quat.y(),quat.z(),quat.w(),
+      qJoints;
   }
   else if (se3::nv(m_model->joints[1]) == 6){
     dg::Vector qFF = qJoints.head<6>();
-    urdf::Rotation rot;
-    rot.setFromRPY(qFF(3),qFF(4),qFF(5));
-    double x,y,z,w;
-    rot.getQuaternion(x,y,z,w);
     q.resize(qJoints.size()+1);
-    q << qFF(0),qFF(1),qFF(2),x,y,z,w,qJoints.segment(6,qJoints.size()-6);
+
+    Eigen::Quaternion<double> quat = Eigen::AngleAxisd(qFF(5),Eigen::Vector3d::UnitZ())*
+      Eigen::AngleAxisd(qFF(4),Eigen::Vector3d::UnitY())*
+      Eigen::AngleAxisd(qFF(3),Eigen::Vector3d::UnitX());
+    q << qFF(0),qFF(1),qFF(2),
+      quat.x(),quat.y(),quat.z(),quat.w(),
+      qJoints.segment(6,qJoints.size()-6);
+
     assert(q.size() == m_model->nq);
   }
   else {

unitTesting/kineromeo.py

@@ -5,6 +5,7 @@
 #
 # ______________________________________________________________________________
 # ******************************************************************************
+import pinocchio as se3
 from dynamic_graph import plug
 from dynamic_graph.sot.core import *
 from dynamic_graph.sot.dynamics import *
@@ -24,7 +25,7 @@ set_printoptions(suppress=True, precision=7)
 #Define robotName, urdfpath and initialConfig
 
 robotName = 'romeo'
-urdfpath = "/local/rbudhira/git/proyan/sot-pinocchio/unitTesting/romeoNoToes.urdf"
+urdfpath = "romeoNoToes.urdf"
 
 initialConfig = (0, 0, .840252, 0, 0, 0,                                 # FF
                  0,  0,  -0.3490658,  0.6981317,  -0.3490658,   0,       # LLEG
@@ -35,13 +36,24 @@ initialConfig = (0, 0, .840252, 0, 0, 0,                                 # FF
                  1.5, -0.6,   0.5,  1.05, -0.4, -0.3, -0.2,              # RARM
                  )
 
+
+#-----------------------------------------------------------------------------
+#---- PINOCCHIO MODEL AND DATA --------------------------------------------------------------------
+#-----------------------------------------------------------------------------
+pinocchioModel = se3.buildModelFromUrdf(urdfpath,se3.JointModelFreeFlyer())
+pinocchioData = pinocchioModel.createData()
+
 #-----------------------------------------------------------------------------
 #---- DYN --------------------------------------------------------------------
 #-----------------------------------------------------------------------------
 dyn = Dynamic("dyn")
+dyn.setModel(pinocchioModel)
+dyn.setData(pinocchioData)
 
 dyn.displayModel()
 
+
+robotDim = dyn.getDimension()
 inertiaRotor = (0,)*6+(5e-4,)*31
 gearRatio =  (0,)*6+(200,)*31
 dyn.inertiaRotor.value = inertiaRotor