diff --git a/src/dynamic_graph/sot/dynamics/__init__.py b/src/dynamic_graph/sot/dynamics/__init__.py
index e6c844330beb70644a1349feb3c17b7c87be3af9..0637534c072d51d78f6c7e3e574886f19c784227 100755
--- a/src/dynamic_graph/sot/dynamics/__init__.py
+++ b/src/dynamic_graph/sot/dynamics/__init__.py
@@ -1,6 +1,8 @@
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:]])
diff --git a/src/sot-dynamic.cpp b/src/sot-dynamic.cpp
index 69b4ad821efaff6492e17377ed92e905fcf2fe73..48b643280b8ab36831c04286ece2f91a970d058e 100644
--- a/src/sot-dynamic.cpp
+++ b/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);
diff --git a/unitTesting/kineromeo.py b/unitTesting/kineromeo.py
index e7eac08072a047a7d987d1747a5dff214788852e..4ac7ecc6af8b5944f782b06aaba329e1e2546ee8 100644
--- a/unitTesting/kineromeo.py
+++ b/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)