diff --git a/python/mocap/mocap_parser.py b/python/mocap/mocap_parser.py
new file mode 100644
index 0000000000000000000000000000000000000000..01ac026755b811ff265809871e62493a341dbb93
--- /dev/null
+++ b/python/mocap/mocap_parser.py
@@ -0,0 +1,339 @@
+'''
+==========================================================================================
+ Mocap position files replay -- author O. Ramos -- rewrite NMsd
+ ---------------------------------
+
+ Read the positions files coming from testHuma, and make them available for
+ control or display.
+
+ The files are read and stored at the beginning, so that any position of the file
+ can be accessed at any time, that is, 'iter' can be changed to see the robot
+ at a different moment of time
+
+=========================================================================================
+'''
+
+import sys
+import time
+from numpy import *
+
+
+
+#--------------------------------------------------------------------------
+#--------------------------------------------------------------------------
+#--------------------------------------------------------------------------
+
+# Convert matrix to tuple
+def matrixToTuple(M):
+ tmp = M.tolist()
+ res = []
+ for i in tmp:
+ res.append(tuple(i))
+ return tuple(res)
+
+# Convert from Roll, Pitch, Yaw to transformation Matrix
+def rpy2tr(r,p,y):
+ mat = matrix(rotate('z',r))*matrix(rotate('y',p))*matrix(rotate('x',y))
+ return matrixToTuple(mat)
+
+# Get the distance btw the position components of 2 transf matrices
+def distVector(M2,M1):
+ px = M1[0][3] - M2[0][3]
+ py = M1[1][3] - M2[1][3]
+ pz = M1[2][3] - M2[2][3]
+ return [px,py,pz]
+
+# Obtain an orthonormal matrix SO3 using the given vector as its first column
+# (it computes Gram Schmidt to obtain an orthonormal basis using the
+# first vector and 2 other 'random' vectors)
+
+def generateOrthonormalM(v1):
+ v2 = matrix([random(),random(),random()])
+ v3 = matrix([random(),random(),random()])
+
+ v1 = matrix(v1)
+ e1 = v1/linalg.norm(v1)
+
+ u2 = v2-inner(v2,e1)*e1
+ e2 = u2/linalg.norm(u2)
+
+ u3 = v3-inner(v3,e1)*e1-inner(v3,e2)*e2
+ e3 = u3/linalg.norm(u3)
+
+ e1=e1.tolist(); e2=e2.tolist(); e3=e3.tolist()
+ M = ( (e1[0][0],e2[0][0],e3[0][0]), (e1[0][1],e2[0][1],e3[0][1]), (e1[0][2],e2[0][2],e3[0][2]) )
+ return M
+
+# Convert from Transformation Matrix to Roll,Pitch,Yaw
+def tr2rpy(M):
+ m = sqrt(M[2][1]**2+M[2][2]**2)
+ p = arctan2(-M[2][0],m)
+
+ if abs( p-pi/2 ) < 0.001:
+ r = 0
+ y = arctan2(M[0][1],M[1][1])
+ elif abs( p+pi/2 ) < 0.001:
+ r = 0
+ y = -arctan2(M[0][1],M[1][1])
+ else:
+ r = arctan2(M[1][0],M[0][0])
+ y = arctan2(M[2][1],M[2][2])
+
+ return [float(r),float(p),float(y)]
+
+def matrixToRPY( M ):
+ '''
+ Convert a 4x4 homogeneous matrix to a 6x1 rpy pose vector.
+ '''
+ rot = tr2rpy(M)
+ return [ M[0][3], M[1][3], M[2][3], rot[2],rot[1],rot[0]]
+
+#--------------------------------------------------------------------------
+#--------------------------------------------------------------------------
+#--------------------------------------------------------------------------
+
+class JointData:
+ def __init__(self):
+ self.name = ""
+ self.positions = []
+ self.Ki = None
+ self.displayObject = ""
+
+
+class LinkData:
+ def __init__(self):
+ self.body1 = ""
+ self.body2 = ""
+ self.displayObject = ""
+
+class MocapParser:
+ # Mocap variables
+ M = 1
+ KM = 2
+ MK = 3
+ KMK = 4
+ classicalLinkMap = \
+ {'lleg0':['waist','Lleg1'], 'lleg1':['Lleg1','Lleg2'], 'lleg2':['Lleg2','Lfoot'], \
+ 'rleg0':['waist','Rleg1'], 'rleg1':['Rleg1','Rleg2'], 'rleg2':['Rleg2','Rfoot'],\
+ 'lshoulder':['chest','Larm1'], 'larm1':['Larm1','Larm2'], 'larm2':['Larm2','Lhand'],\
+ 'rshoulder':['chest','Rarm1'], 'rarm1':['Rarm1','Rarm2'], 'rarm2':['Rarm2','Rhand'],\
+ 'head':['head','chest'], 'chest':['chest','waist'] }
+ # Constructer:
+ def __init__(self, mocapDir=None, skeletumFileName=None):
+ self.qs = []
+ self.Kw = None
+ self.jointsNumber = 0
+ self.joints = None
+ self.links = {}
+ self.method = MocapParser.M
+ self.iter = 0
+ self.increment = +1
+ self.delayTime = 0.0
+ self.with_dispJoints = True
+ self.with_dispRobot = True
+ self.with_dispLinks = False
+ self.with_dispNumber = False
+ self.maxTime = -1
+
+ if skeletumFileName!=None: self.openQFile( skeletumFileName)
+ if mocapDir!=None:
+ self.openKwFile( mocapDir+'Kw.dat')
+ self.openKiFile( mocapDir+'Ki.dat')
+ self. openMocapPositionFile( mocapDir )
+
+ # --- read files ----
+ def openKwFile(self,KwFile):
+ '''
+ Try to open the Transformation Matrices WRM (corresponding to the coordinate
+ system of each joint).
+ '''
+ fileKw = open(KwFile,'r')
+ linesKw = fileKw.readlines()
+ self.Kw = matrix(eval(linesKw[0].split()[2][5:]))
+
+ def openKiFile(self,KiFile):
+ '''
+ Ki contains one line for each joint, with its name and the offset matrix Ki.
+ Try to open and read Ki and set the names for the joints (using Ki.dat).
+ '''
+ self.jointsNumber = len( open(KiFile,'r').readlines() )
+ self.joints = [ JointData() for i in range(self.jointsNumber) ]
+ try:
+ KiFile = open(KiFile,'r')
+ for val in range(self.jointsNumber):
+ KiLine = KiFile.readline()[5:].split(' ')
+ self.joints[val].Ki = matrix( eval(KiLine[1][5:]) )
+ self.joints[val].name = KiLine[0]
+ except:
+ print " -- There is no Ki --"
+ sys.exit(0)
+
+
+ def checkMaxTime( self,l,msg=None ):
+ if self.maxTime<0: self.maxTime = l
+ elif l<self.maxTime:
+ if msg==None: msg = ''
+ else: msg = 'in '+msg
+ print 'Time length is not concordant',msg,'. Reducing it.'
+ self.maxTime = len(self.qs)
+
+
+ def openQFile(self,skeletumFileName):
+ self.qs = []
+ for l in open(skeletumFileName,'r').readlines():
+ self.qs.append( list( eval(l.split()[0][4:]) ) ) # + 10*[0.0]
+ self.checkMaxTime( len(self.qs) )
+
+ def openMocapPositionFile(self,dataFolder):
+ for i,j in enumerate(self.joints):
+ for l in open(dataFolder+'WRM_'+j.name,'r').readlines():
+ j.positions.append( eval(l[5:]) )
+ self.checkMaxTime( len(j.positions),'joint '+str(i) )
+
+ # --- Other init ---
+ def setLinksMap( self, linkDef = classicalLinkMap ):
+ self.links = dict()
+ for name,(b1,b2) in linkDef.iteritems():
+ self.links[ name ] = LinkData()
+ for i,j in enumerate(self.joints):
+ if j.name==b1: self.links[name].body1 = i
+ if j.name==b2: self.links[name].body2 = i
+
+ def assignDisplayObjects(self):
+ for i,j in enumerate(self.joints):
+ j.displayObject = 'axis'+str(i+1)
+ for n,l in self.links.iteritems():
+ l.displayObject = n
+
+ # --- data access
+ def jointPosition_M(self,joint,iter):
+ return self.joints[joint].positions[iter]
+ def jointPosition_MK(self,joint,iter):
+ M = matrix(self.jointPosition_M(joint,iter))
+ return matrixToTuple( M*self.joints[joint].Ki )
+ def jointPosition_KMK(self,joint,iter):
+ M = matrix(self.jointPosition_M(joint,iter))
+ return matrixToTuple( self.Kw*M*self.joints[joint].Ki )
+ def jointPosition_KM(self,joint,iter):
+ M = matrix(self.jointPosition_M(joint,iter))
+ return matrixToTuple( self.Kw*M )
+
+ def setPositionMethod(self,method): # M KM KMK
+ if( method=='M'): self.method = self.M
+ if( method=='KM'): self.method = self.KM
+ if( method=='MK'): self.method = self.MK
+ if( method=='KMK'): self.method = self.KMK
+ def jointPosition(self,j,t):
+ if self.method == self.M:
+ return self.jointPosition_M(j,t)
+ elif self.method == self.KM:
+ return self.jointPosition_KM(j,t)
+ elif self.method == self.MK:
+ return self.jointPosition_MK(j,t)
+ elif self.method == self.KMK:
+ return self.jointPosition_KMK(j,t)
+ def jointRPY( self,j,t): return matrixToRPY( self.jointPosition(j,t) )
+
+ def linkPosition(self,l,t):
+ # Get the 'vector' joining the positions of two consecutive transformation matrices
+ # and use it to generate a orthonormal matrix (rotation matrix), then get roll, pitch, yaw
+ M1 = self.jointPosition( self.links[l].body1,t )
+ M2 = self.jointPosition( self.links[l].body2,t )
+ return [ M1[0][3],M1[1][3],M1[2][3] ]+ tr2rpy( generateOrthonormalM( distVector(M1,M2) ) )
+
+ # --- iter update
+ def forward( self ) : self.increment = +1
+ def backward( self ) : self.increment = -1
+ def incTime( self ):
+ self.iter += self.increment
+ if self.iter<0:
+ print " -- Start of file reached (when going backwards). --"
+ self.increment = 0
+ self.iter=0
+ elif self.iter >= self.maxTime:
+ print " -- End of file containing the positions. --"
+ self.increment = 0
+ self.iter=self.maxTime-1
+
+ # --- display update
+ def setRobotViewer( self, rview, robotName = None ):
+ self.rview = rview
+ if robotName==None: self.robotName = "hrp"
+ else: self.robotName = robotName
+ self.hideLinks()
+ self.hideJoints()
+
+ def dispJoints( self ):
+ if self.with_dispJoints:
+ for j in range(self.jointsNumber):
+ self.rview.updateElementConfig( self.joints[j].displayObject,self.jointRPY(j,self.iter) )
+ def dispLinks( self ):
+ if self.with_dispLinks:
+ for n,l in self.links.iteritems():
+ self.rview.updateElementConfig( l.displayObject,self.linkPosition(n,self.iter) )
+ def dispRobot(self):
+ if self.with_dispRobot:
+ self.rview.updateElementConfig( self.robotName, list(self.qs[self.iter])+10*[0,] )
+ def hideJoints( self ):
+ for j in self.joints:
+ self.rview.updateElementConfig( j.displayObject,[ 0, 0, -1, 0, 0, 0 ])
+ def hideLinks( self ):
+ for j in self.links.values():
+ self.rview.updateElementConfig( j.displayObject,[ 0, 0, -1, 0, 0, 0 ])
+ def alignHrpFrames( self ):
+ namesT1 = ['Larm2','Lhand','Lleg1','Lleg2','Rarm2','Rhand','Rleg1','Rleg2','head','chest']
+ namesT2 = ['Larm1','Rarm1']
+ transf1 = matrix(rotate('x',-pi))*matrix(rotate('z',-pi/2))
+ transf2 = matrix(rotate('y',pi/2))*matrix(rotate('z',pi/2))
+ for joint in self.joints:
+ if joint.name in namesT1: joint.Ki = joints.Ki*transf1
+ if joint.name in namesT2: joint.Ki = joints.Ki*transf2
+
+ def update( self ):
+ self.incTime()
+ if self.delayTime>0: time.sleep( self.delayTime )
+ if self.with_dispNumber: print self.iter
+ self.refresh()
+ def refresh( self ):
+ self.dispJoints()
+ self.dispLinks()
+ self.dispRobot()
+
+
+ #=======================================================================================
+ # Helper functions
+ #=======================================================================================
+
+
+ def hlp_mocapCoord(self):
+ self.with_dispJoints = not self.with_dispJoints
+ self.hideJoints()
+
+ def hlp_skeleton(self):
+ self.with_dispLinks = not self.with_dispLinks
+ self.hideLinks()
+
+ def hlp_showNumber(self):
+ self.with_dispNumber = not self.with_dispNumber
+
+ def hlp_toggle(self):
+ if self.method == self.M:
+ self.method = self.MK
+ elif self.method == self.MK:
+ self.method = self.KMK
+ elif self.method == self.KMK:
+ self.method = self.KM
+ elif self.method == self.KM:
+ self.method = self.M
+ print self.method
+ self.disp_toggle()
+
+ def disp_toggle(self):
+ if self.method == self.MK:
+ print " --- wRm*Ki --- "
+ elif self.method == self.KMK:
+ print " --- Kw*wRm*Ki --- "
+ elif self.method == self.KM:
+ print " --- Kw*wRm --- "
+ elif self.method == self.M:
+ print " --- wRm --- "
diff --git a/python/mocap/replay_mocap.py b/python/mocap/replay_mocap.py
new file mode 100644
index 0000000000000000000000000000000000000000..5cf3df2135953571406c77c6ef2c168ffad71c36
--- /dev/null
+++ b/python/mocap/replay_mocap.py
@@ -0,0 +1,115 @@
+'''
+============================================================
+ Replay qs and WRM -- author Oscar Ramos (rewr. NMsd)
+ ---------------------------------
+
+ The program uses the Mocap Parser to replay the mocap on robot viewer.
+ Object are expected on robot viewer config file:
+ -- hrp: the robot vrml
+ -- axis1->axis15: the center (with orientation) of the joints.
+ -- one object per link, named from the link-map.
+
+ Use it as:
+ py playJointsFile.py [options] args
+ Options:
+ - go: start the simulation
+ - stop: stop the simulation
+ - next: single step of simulation
+ - forward: play forward (by default)
+ - backward: play backward
+ - replay: start from the beginning
+ - s: show/hide the skeleton from the mocap
+ - n: show/hide frame numbers (display on screen)
+ - t: toggle between WRM, WRM*Ki, Kw*WRM*Ki, Kw*WRM
+
+ Some variables can be changed iteratively:
+ - mp.delayTime: float, to change the time between frames (default=0.0 seconds)
+ - mp.iter: int, to go to a certain frame number
+
+=================================================================
+'''
+
+from optparse import OptionParser
+from mocap_parser import MocapParser
+sys.path.append('/home/nmansard/src/hpp2/sot-dyninv/python')
+from dynamic_graph.script_shortcuts import optionalparentheses
+from random import random
+
+
+# --- PARSER INIT ------------------------------------------------------------------
+# --- PARSER INIT ------------------------------------------------------------------
+# --- PARSER INIT ------------------------------------------------------------------
+
+usage = "usage: py postureOpTask.py [options] args"
+parser = OptionParser(usage=usage)
+parser.add_option("-p", dest="opti_q_file", help="Specification of qs from mocap optimization (outputJoints)")
+parser.add_option("-o", dest="opFolder", help="Folder containing the operational points (WRM,Ki,KW)")
+parser.add_option("-d", dest="delayTime", help="Delay time between iterations (in sec). Default is %default", \
+ type=float, default="0")
+parser.add_option("-s", dest="server", help="Name of server (CORBA or XML-RPC). Default is %default", \
+ metavar="NAME_SERVER", default="CORBA")
+(options, args) = parser.parse_args()
+parser.print_help()
+print '\n -------------------------------------------------------------------------'
+
+
+# --- DISPLAY INIT --------------------------------------------------------------------
+# --- DISPLAY INIT --------------------------------------------------------------------
+# --- DISPLAY INIT --------------------------------------------------------------------
+
+# Initialize robotviewer, if possible
+try:
+ import robotviewer
+ clt = robotviewer.client(options.server) # default is CORBA
+except:
+ print " -- WARNING!: No robot viewer (or not correct server)."
+ class RobotViewerFaked:
+ def update(*args): void
+ def updateElementConfig(*args): void
+ clt =RobotViewerFaked()
+
+
+# --- PARSER INIT ------------------------------------------------------------------
+# --- PARSER INIT ------------------------------------------------------------------
+# --- PARSER INIT ------------------------------------------------------------------
+
+mp = MocapParser(options.opFolder,options.opti_q_file)
+mp.setLinksMap()
+mp.assignDisplayObjects()
+mp.setRobotViewer( clt )
+mp.delayTime = options.delayTime
+#mp.dispLinks = True
+
+#=======================================================================================
+# THREAD
+#=======================================================================================
+
+from ThreadInterruptibleLoop import *
+@loopInThread
+def loop():
+ mp.update()
+runner=loop()
+
+@optionalparentheses
+def go(): runner.play()
+@optionalparentheses
+def stop(): runner.pause()
+@optionalparentheses
+def next(): mp.update()
+@optionalparentheses
+def forward():hlp_forward()
+@optionalparentheses
+def backward():hlp_backward()
+@optionalparentheses
+def replay():
+ mp.iter=0
+ mp.forward()
+ runner.play()
+@optionalparentheses
+def s(): mp.hlp_skeleton()
+@optionalparentheses
+def m(): mp.hlp_mocapCoord()
+@optionalparentheses
+def n(): mp.hlp_showNumber()
+@optionalparentheses
+def t(): mp.hlp_toggle()