run framesforwardkinematics before finding position signal value

src/dynamic_graph/sot/dynamics/__init__.py

 from dynamic import Dynamic
 from angle_estimator import AngleEstimator
 from zmp_from_forces import ZmpFromForces
+import numpy as np
+from numpy import arctan2, arcsin, sin, cos, sqrt
 
 DynamicOld = Dynamic
 
@@ -13,3 +15,54 @@ class Dynamic (DynamicOld):
     def setModel(self, pinocchio_model):
         dynamic.wrap.set_pinocchio_model(self.obj,pinocchio_model)
         return
+
+def fromSotToPinocchio(q_sot, freeflyer=True):
+    if freeflyer:
+        [r,p,y] = q_sot[3:6]
+        cr = cos(r)
+        cp = cos(p)
+        cy = cos(y)
+        sr = sin(r)
+        sp = sin(p)
+        sy = sin(y)
+
+        rotmat = np.matrix([[cy*cp, cy*sp*sr-sy*cr, cy*sp*cr+sy*sr],
+                           [sy*cp, sy*sp*sr+cy*cr, sy*sp*cr-cy*sr],
+                           [-sp, cp*sr, cp*cr]])
+
+        d0 = rotmat[0,0]
+        d1 = rotmat[1,1]
+        d2 = rotmat[2,2]
+        rr = 1.0+d0+d1+d2
+
+        if rr>0:
+            s = 0.5 / sqrt(rr)
+            _x = (rotmat[2,1] - rotmat[1,2]) * s
+            _y = (rotmat[0,2] - rotmat[2,0]) * s
+            _z = (rotmat[1,0] - rotmat[0,1]) * s
+            _r = 0.25 / s
+        else:
+            #Trace is less than zero, so need to determine which
+            #major diagonal is largest
+            if ((d0 > d1) and (d0 > d2)):
+                s = 0.5 / sqrt(1 + d0 - d1 - d2)
+                _x = 0.5 * s
+                _y = (rotmat[0,1] + rotmat[1,0]) * s
+                _z = (rotmat[0,2] + rotmat[2,0]) * s
+                _r = (rotmat[1,2] + rotmat[2,1]) * s
+            elif (d1 > d2):
+                s = 0.5 / sqrt(1 + d0 - d1 - d2)
+                _x = (rotmat[0,1] + rotmat[1,0]) * s
+                _y = 0.5 * s
+                _z = (rotmat[1,2] + rotmat[2,1]) * s
+                _r = (rotmat[0,2] + rotmat[2,0]) * s
+            else:
+                s = 0.5 / sqrt(1 + d0 - d1 - d2)
+                _x = (rotmat[0,2] + rotmat[2,0]) * s
+                _y = (rotmat[1,2] + rotmat[2,1]) * s
+                _z = 0.5 * s
+                _r = (rotmat[0,1] + rotmat[1,0]) * s
+
+        return np.matrix([q_sot[0:3]+(_x,_y,_z,_r)+q_sot[6:]])
+    else:
+        return np.matrix([q_sot[0:]])

src/sot-dynamic.cpp

@@ -29,6 +29,7 @@
 #include <pinocchio/algorithm/center-of-mass.hpp>
 #include <pinocchio/spatial/motion.hpp>
 #include <pinocchio/algorithm/crba.hpp>
+#include <pinocchio/multibody/model.hpp>
 
 #include <dynamic-graph/all-commands.h>
 
@@ -287,7 +288,6 @@ dg::Vector Dynamic::getPinocchioPos(int time)
   if( freeFlyerPositionSIN) {
     dg::Vector qFF=freeFlyerPositionSIN.access(time);
 
-
     q.resize(qJoints.size() + 7);
 
     Eigen::Quaternion<double> quat = Eigen::AngleAxisd(qFF(5),Eigen::Vector3d::UnitZ())*
@@ -308,15 +308,14 @@ dg::Vector Dynamic::getPinocchioPos(int time)
     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 {
     q.resize(qJoints.size());
     q=qJoints;
   }
-
-  sotDEBUGOUT(15) <<"Position out"<<q<<std::endl;
+  sotDEBUG(15) <<"Position out"<<q<<std::endl;
+  sotDEBUGOUT(15);
   return q;
 }
 
@@ -636,16 +635,19 @@ computeGenericEndeffJacobian(bool isFrame, int jointId,dg::Matrix& res,int time
   //In local coordinates.
   se3::computeJacobians(*m_model,*m_data,this->getPinocchioPos(time));
   se3::Data::Matrix6x m_output = Eigen::MatrixXd::Zero(6,m_model->nv);
-
+  std::string temp;
   if(isFrame){
     se3::framesForwardKinematics(*m_model,*m_data);
     se3::getFrameJacobian<true>(*m_model,*m_data,(se3::Model::Index)jointId,m_output);
+    temp = m_model->getFrameName((se3::Model::Index)jointId);
+  }
+  else {
+    temp = m_model->getJointName((se3::Model::Index)jointId);
+    se3::getJacobian<true>(*m_model,*m_data,(se3::Model::Index)jointId,m_output);
   }
-  else se3::getJacobian<true>(*m_model,*m_data,(se3::Model::Index)jointId,m_output);
-
   res = m_output;
+  sotDEBUG(25) << "EndEffJacobian for "<<temp<<" is "<<m_output<<std::endl;
   sotDEBUGOUT(25);
-
   return res;
 }
 
@@ -655,13 +657,17 @@ computeGenericPosition(bool isFrame, int jointId, MatrixHomogeneous& res, int ti
   sotDEBUGIN(25);
   assert(m_data);
   newtonEulerSINTERN(time);
+  std::string temp;
   if(isFrame){
-    //TODO: Confirm if we need this. Already being called when calculating jacobian
-    //se3::framesForwardKinematics(m_model,*m_data);
+    se3::framesForwardKinematics(*m_model,*m_data);
+    res.matrix()= m_data->oMf[jointId].toHomogeneousMatrix();
+    temp = m_model->getFrameName((se3::Model::Index)jointId);
+  }
+  else{
     res.matrix()= m_data->oMi[jointId].toHomogeneousMatrix();
+    temp = m_model->getJointName((se3::Model::Index)jointId);
   }
-  else res.matrix()= m_data->oMi[jointId].toHomogeneousMatrix();
-
+  sotDEBUG(25)<<"For "<<temp<<" with id: "<<jointId<<" position is "<<res<<std::endl;
   sotDEBUGOUT(25);
   return res;
 }
@@ -698,6 +704,7 @@ computeNewtonEuler( int& dummy,int time )
   sotDEBUGIN(15);
   assert(m_model);
   assert(m_data);
+
   
   const Eigen::VectorXd q=getPinocchioPos(time);
   const Eigen::VectorXd v=getPinocchioVel(time);

unitTesting/kineromeo.py

@@ -5,7 +5,6 @@
 #
 # ______________________________________________________________________________
 # ******************************************************************************
-import pinocchio as se3
 from dynamic_graph import plug
 from dynamic_graph.sot.core import *
 from dynamic_graph.sot.dynamics import *
@@ -13,7 +12,7 @@ import dynamic_graph.script_shortcuts
 from dynamic_graph.script_shortcuts import optionalparentheses
 from dynamic_graph.sot.core.matrix_util import matrixToTuple, vectorToTuple,rotate, matrixToRPY
 from dynamic_graph.sot.core.meta_tasks_kine import *
-from dynamic_graph.sot.core.utils.viewer_helper import addRobotViewer,VisualPinger,updateComDisplay
+#from dynamic_graph.sot.core.utils.viewer_helper import addRobotViewer,VisualPinger,updateComDisplay
 from numpy import *
 
 set_printoptions(suppress=True, precision=7)
@@ -24,9 +23,17 @@ set_printoptions(suppress=True, precision=7)
 
 #Define robotName, urdfpath and initialConfig
 
-robotName = 'romeo'
-urdfpath = "romeoNoToes.urdf"
+#Taking input from pinocchio
+from pinocchio.romeo_wrapper import RomeoWrapper
+import pinocchio as se3
 
+#SET THE PATH TO THE URDF AND MESHES
+urdfPath = "~/git/sot/pinocchio/models/romeo.urdf"
+urdfDir = ["~/git/sot/pinocchio/models"]
+pinocchioRobot = RomeoWrapper(urdfPath, urdfDir, se3.JointModelFreeFlyer())
+robotName = 'romeo'
+pinocchioRobot.initDisplay(loadModel=True)
+pinocchioRobot.display(pinocchioRobot.q0)
 initialConfig = (0, 0, .840252, 0, 0, 0,                                 # FF
                  0,  0,  -0.3490658,  0.6981317,  -0.3490658,   0,       # LLEG
                  0,  0,  -0.3490658,  0.6981317,  -0.3490658,   0,       # RLEG
@@ -40,19 +47,18 @@ initialConfig = (0, 0, .840252, 0, 0, 0,                                 # FF
 #-----------------------------------------------------------------------------
 #---- PINOCCHIO MODEL AND DATA --------------------------------------------------------------------
 #-----------------------------------------------------------------------------
-pinocchioModel = se3.buildModelFromUrdf(urdfpath,se3.JointModelFreeFlyer())
-pinocchioData = pinocchioModel.createData()
+#pinocchioModel = se3.buildModelFromUrdf(urdfpath, se3.JointModelFreeFlyer())
+#pinocchioData = pinocchioModel.createData()
 
 #-----------------------------------------------------------------------------
 #---- DYN --------------------------------------------------------------------
 #-----------------------------------------------------------------------------
 dyn = Dynamic("dyn")
-dyn.setModel(pinocchioModel)
-dyn.setData(pinocchioData)
+dyn.setModel(pinocchioRobot.model)
+dyn.setData(pinocchioRobot.data)
 
 dyn.displayModel()
 
-
 robotDim = dyn.getDimension()
 inertiaRotor = (0,)*6+(5e-4,)*31
 gearRatio =  (0,)*6+(200,)*31
@@ -71,9 +77,10 @@ robot = RobotSimu(robotName)
 robot.resize(robotDim)
 dt=5e-3
 
-robot.set( initialConfig )
-addRobotViewer(robot, small=False)
+#TODO: This configuration follows xyzQuat format for freeflyer. Do something about it
+#initialConfig = zip(*(list(matrixToTuple(pinocchioRobot.q0))))[0]
 
+robot.set(initialConfig)
 plug(robot.state,dyn.position)
 
 # ------------------------------------------------------------------------------
@@ -84,6 +91,9 @@ sot.setSize(robotDim)
 plug(sot.control,robot.control)
 
 
+
+#--------------------------------DISPLAY-----------------------------------------
+
 # ------------------------------------------------------------------------------
 # ---- TASKS -------------------------------------------------------------------
 # ------------------------------------------------------------------------------
@@ -112,7 +122,12 @@ for name,joint in [ ['LF','LAnkleRoll'], ['RF','RAnkleRoll' ] ]:
 
 
 target = (0.5,-0.2,1.3)
-robot.viewer.updateElementConfig('zmp',target+(0,0,0))
+
+#addRobotViewer(robot, small=False)
+#robot.viewer.updateElementConfig('zmp',target+(0,0,0))
+
+
+
 gotoNd(taskRH,target,'111',(4.9,0.9,0.01,0.9))
 sot.push(contactRF.task.name)
 sot.push(contactLF.task.name)
@@ -123,27 +138,10 @@ sot.push(taskRH.task.name)
 #----- MAIN LOOP ---------------------------------------------------------------
 #-------------------------------------------------------------------------------
 
-from dynamic_graph.sot.core.utils.thread_interruptible_loop import loopInThread,loopShortcuts
-@loopInThread
-def inc():
-    robot.increment(dt)
-#    print "dyn_position_value_devel"
-#    print asarray(dyn.position.value)
-#    print "robot_control_value_devel"
-#    print asarray(robot.control.value)
-#    print "task_contactlf_jacobian_devel"
-#    print transpose(asarray(contactLF.feature.signal("jacobian").value))
-#    print "dyn_JLF_devel"
-#    print transpose(asarray(dyn.signal("JLF").value))
-runner=inc()
-[go,stop,next,n]=loopShortcuts(runner)
-
-print "dyn_position_value_devel"
-print asarray(dyn.position.value)
-print "robot_control_value_devel"
-print asarray(robot.control.value)
-print "task_contactlf_jacobian_devel"
-print transpose(asarray(contactLF.feature.signal("jacobian").value))
-print "dyn_JLF_devel"
-print transpose(asarray(dyn.signal("JLF").value))
-go()
+
+def runner(n):
+    for i in xrange(n):
+        robot.increment(dt)
+        pinocchioRobot.display(fromSotToPinocchio(robot.state.value))
+
+runner(1000)