From 218c85acde243f0bd66beb28768694733a17dee1 Mon Sep 17 00:00:00 2001
From: Mansard <nmansard@laas.fr>
Date: Sun, 11 Sep 2011 12:10:39 +0200
Subject: [PATCH] Added zmp in history.
---
python/history.py | 39 +++++++++++++++++++++++++++------------
1 file changed, 27 insertions(+), 12 deletions(-)
diff --git a/python/history.py b/python/history.py
index bf43296..29657c6 100644
--- a/python/history.py
+++ b/python/history.py
@@ -1,32 +1,47 @@
+from numpy import *
+from matrix_util import vectorToTuple
+
class History:
- def __init__(self,robot,freq=100):
- self.robot = robot
+ def __init__(self,dynEnt,freq=100,zmpSig=None):
self.q = list()
self.qdot = list()
self.zmp = list()
self.freq=freq
+ self.zmpSig = zmpSig
+ self.dynEnt = dynEnt
+ self.withZmp = (self.zmpSig!=None) and ("waist" in map(lambda x:x.name,self.dynEnt.signals()) )
def record(self):
- i=self.robot.state.time
+ i=self.dynEnt.position.time
if i%self.freq == 0:
- self.q.append(robot.state.value)
- self.qdot.append(robot.state.value)
- self.zmp.append(list(vectorToTuple(matrix(dyn.waist.value).I*matrix(zmp.zmp.value+(1,)).T)))
+ self.q.append(self.dynEnt.position.value)
+ self.qdot.append(self.dynEnt.velocity.value)
+ if self.withZmp:
+ waMwo = matrix(self.dynEnt.waist.value).I
+ wo_z=matrix(self.zmpSig.value+(1,)).T
+ self.zmp.append(list(vectorToTuple(waMwo*wo_z)))
def restore(self,t):
- if not t in self.q.keys():
- print "Time ",t," has not been stored (freq is ",self.freq,")."
- return
+ t= int(t/self.freq)
print "robot.set(",self.q[t],")"
print "robot.setVelocity(",self.qdot[t],")"
print "robot.state.time = ",t
def dumpToOpenHRP(self,baseName = "dyninv",sample = 1):
filePos = open(baseName+'.pos','w')
fileRPY = open(baseName+'.hip','w')
- fileZMP = open(baseName+'.zmp','w')
sampleT = 0.005
- for nT,(q,z) in enumerate(zip(self.q,self.zmp)):
- fileZMP.write(str(sampleT*nT)+' '+str(z[0])+' '+str(z[1])+' '+str(z[2])+'\n')
+ for nT,q in enumerate(self.q):
fileRPY.write(str(sampleT*nT)+' '+str(q[3])+' '+str(q[4])+' '+str(q[5])+'\n')
filePos.write(str(sampleT*nT)+' ')
for j in range(6,36):
filePos.write(str(q[j])+' ')
filePos.write(10*' 0'+'\n')
+ if self.withZmp:
+ fileZMP = open(baseName+'.zmp','w')
+ for nT,z in enumerate(self.zmp):
+ fileZMP.write(str(sampleT*nT)+' '+str(z[0])+' '+str(z[1])+' '+str(z[2])+'\n')
+
+ filePos0 = open(baseName+'.pos0','w')
+ filePos0.write( "seq.sendMsg(\":joint-angles " )
+ q0 = self.q[0]
+ for x in q0[6:36]:
+ filePos0.write( str(x*180.0/pi)+' ' )
+ filePos0.write( " 0 0 0 0 0 0 0 0 0 0 5\")")
--
GitLab