Commit b11567be authored by Rohan Budhiraja's avatar Rohan Budhiraja
Browse files

run framesforwardkinematics before finding position signal value

parent 39a3db9c
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:]])
......@@ -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);
......
......@@ -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)
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment