From 466b6662a1e631bbbb5eca06a9cad0729bb036bc Mon Sep 17 00:00:00 2001
From: Mansard <nmansard@laas.fr>
Date: Tue, 27 Sep 2011 19:12:03 +0200
Subject: [PATCH] ivigit.
---
python/mocap/footprint_parser.py | 53 ++++++++++++++++++++++++++++++++
1 file changed, 53 insertions(+)
create mode 100644 python/mocap/footprint_parser.py
diff --git a/python/mocap/footprint_parser.py b/python/mocap/footprint_parser.py
new file mode 100644
index 0000000..9edf96d
--- /dev/null
+++ b/python/mocap/footprint_parser.py
@@ -0,0 +1,53 @@
+'''
+Read the file coming from the Oscar footprint extraction, and
+display the position at the proper time interval, using the RF and LF
+objects of robotviewer.
+'''
+from numpy import *
+
+
+# --- FOOT PRINT PARSER ------------------------------------------------------------
+class FootPrintParser:
+ def __init__(self,filename,dt,robotviewer,offset=None):
+ self.file = open(filename,'r')
+ self.dt=dt
+ self.events ={}
+ self.clt = robotviewer
+
+ R=eye(3); a=0
+ if offset!=None:
+ if len(offset)==2:
+ R[0:2,2]=offset
+ elif len(offset)==3:
+ R[0:2,2]=offset[0:2]
+ a=offset[2]
+ c=cos(a); s=sin(a)
+ R[0,0]=c; R[1,1]=c
+ R[0,1]=s; R[1,0]=-s
+ elif len(offset)==4:
+ R[0:2,2]=offset[0:2]
+ a=offset[2]; scale=offset[3]
+ c=cos(a)*scale; s=sin(a)*scale
+ R[0,0]=c; R[1,1]=c
+ R[0,1]=s; R[1,0]=-s
+ R=matrix(R)
+
+ for l in self.file.readlines():
+ (t,foot,x,y,theta)=[ float(x) for x in l.split()]
+ if foot==1: foot='LF'
+ else: foot='RF'
+
+ if (x,y,theta) == (0,0,0): (x,y,theta)= (-100,-100,0)
+ p=R*matrix((x,y,1)).T; theta-=a
+ x=float(p[0,0]); y=float(p[1,0])
+
+ t=int(round(t*dt))
+ if not t in self.events.keys(): self.events[t] = []
+ self.events[t].append( (foot, [x,y,0,0,0,theta] ) )
+ self.update(0)
+
+ def update(self,iterTime):
+ if not iterTime in self.events.keys(): return
+ for (foot, pos) in self.events[iterTime]:
+ self.clt.updateElementConfig(foot,pos)
+
--
GitLab