Skip to content
Snippets Groups Projects
Commit b11567be authored by Rohan Budhiraja's avatar Rohan Budhiraja
Browse files

run framesforwardkinematics before finding position signal value

parent 39a3db9c
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
src/dynamic_graph/sot/dynamics/__init__.py
+ 53
− 0
View file @ b11567be
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
+ 19
− 12
View file @ b11567be
......@@ -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
+ 34
− 36
View file @ b11567be
......@@ -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)
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