diff --git a/python/mocap/imit.py b/python/mocap/imit.py
index 654c4cde165aec7fbd441fe25d443725a4b79da5..efd99916e7b3f961c8964eb3eecade7b1876f65c 100644
--- a/python/mocap/imit.py
+++ b/python/mocap/imit.py
@@ -22,7 +22,7 @@ from meta_task_dyn_6d import MetaTaskDyn6d
from attime import attime
from numpy import *
from robotSpecific import pkgDataRootDir,modelName,robotDimension,initialConfig,gearRatio,inertiaRotor
-from mocap_parser import MocapParser,MocapParserTimed
+from mocap_parser import MocapParser,MocapParserScaled
from matrix_util import matrixToTuple
#-----------------------------------------------------------------------------
@@ -34,7 +34,7 @@ robotDim = robotDimension[robotName]
RobotClass = RobotDynSimu
robot = RobotClass("robot")
robot.resize(robotDim)
-dt = 1e-3
+dt = 5e-3
# Initial configuration
robot.set( ( 0,0,0.6487,0,0,0,0,0,-0.453786,0.872665,-0.418879,0,0,0,-0.453786,0.872665,-0.418879,0,0,0,0,0,0.261799,-0.174533,0,-0.523599,0,0,0.174533,0.261799,0.174533,0,-0.523599,0,0,0.174533 ) )
@@ -42,6 +42,27 @@ robot.set( ( 0,0,0.6487,0,0,0,0,0,-0.453786,0.872665,-0.418879,0,0,0,-0.453786,0
# Init config, both hands in place, com in the center of the polygon.
#robot.set((0.047605478333881616, 0.0069670567778655663, 0.70217819698128325, 0.033271671746594733, -0.14633177925107063, -0.0043665416399416524, 0.0060395105648764135, -0.044264063566775931, 0.10843367835692867, 0.20411254610434074, -0.16600602326644109, 0.011755310912801515, 0.0060134908716506048, -0.044087946756041933, 0.21121791020744213, -0.0023194171097636559, -0.06235944098204433, 0.011577286094383481, -0.031867567519300713, 0.0034122542878611364, -0.00018994268250045398, 0.017828376138565111, 0.57116020077409424, -0.20030039713333259, -0.035022771905715559, -1.4854699434782164, 0.033323186696575108, -0.98274234563896556, 0.1745446596832258, 0.67308873547891679, 0.22219955983304274, 0.048809070665574876, -1.412474883412113, -0.083661057118875753, -0.78534673513887354, 0.17410999723730466))
+# Similar initial position
+robot.set((0.029999999999999954, -9.1828441355437758e-29, 0.63000000000000145, 1.4360295221127874e-28, -8.7850988124233508e-28, -3.5290660360498378e-28, 3.5290667992075291e-28, 4.3262691761853982e-19, -0.46492814154439754, 1.0079478744182508, -0.54301973287385275, -4.326269177621427e-19, 3.5290664066979039e-28, 6.5287166994574407e-20, -0.46492814154439754, 1.0079478744182508, -0.54301973287385275, -6.5287167138177328e-20, 1.2924296127032732e-27, 0.15167281550539663, -9.5043422474809308e-28, -0.15167281550539663, 0.5642849052442489, -0.35455784714971755, -0.24227191641099344, -1.3414948262245163, -0.02295302606649734, -0.88270884805420247, 0.17453299999999999, 0.56428357448356325, 0.35455662057462251, 0.24227231972524182, -1.3414929885300777, 0.022954869149019692, -0.88271337036701469, 0.17453299999999999))
+'''
+References for the initial position
+taskrh.ref = ((0, 0, -1, 0.18), (0, 1, 0, -0.41), (1, 0, 0, 0.8), (0.0, 0.0, 0.0, 1.0))
+tasklh.ref = ((0, 0, -1, 0.18), (0, 0.1, 0, 0.41), (1, 0, 0, 0.8), (0.0, 0.0, 0.0, 1.0))
+taskHead.ref = ((1,0,0,0.02),(0,1,0,0),(0,0,1,0),(0,0,0,1))
+taskWaist.ref = ((1,0,0,0.03),(0,1,0,0),(0,0,1,0.63),(0,0,0,1))
+'''
+
+# Similar initial position with hand forward
+robot.set((-0.033328803958899381, -0.0019839923040723341, 0.62176527349722499, 2.379901541270165e-05, 0.037719492175904465, 0.00043085147714449579, -0.00028574496353126724, 0.0038294370786961648, -0.64798319906979551, 1.0552418879542016, -0.44497846451873851, -0.0038397195926379991, -0.00028578259876671871, 0.0038284398205732629, -0.64712828871069394, 1.0534202525984278, -0.4440117393779604, -0.0038387216246160054, 0.00014352031102944824, 0.013151503268540811, -0.00057411504064861592, -0.050871000025766742, 0.21782780288481224, -0.37965640592672439, -0.14072647716213352, -1.1942332339530364, 0.0055454863752273523, -0.66956710808008013, 0.1747981826611808, 0.21400703176352612, 0.38370527720078107, 0.14620204468509851, -1.1873407322935838, -0.0038746980026940735, -0.66430172366423146, 0.17500428384087438))
+'''
+taskrh.ref = ((0, 0, -1, 0.25), (0, 1, 0, -0.41), (1, 0, 0, 0.8), (0.0, 0.0, 0.0, 1.0))
+tasklh.ref = ((0, 0, -1, 0.25), (0, 0.1, 0, 0.41), (1, 0, 0, 0.8), (0.0, 0.0, 0.0, 1.0))
+taskHead.ref = ((1,0,0,0.02),(0,1,0,0),(0,0,1,0),(0,0,0,1))
+taskWaist.ref = ((1,0,0,0.),(0,1,0,0),(0,0,1,0.63),(0,0,0,1))
+featureComDes.errorIN.value = ( 0.01, 0., 0.8 )
+gCom.setConstant( 50.0 )
+'''
+
# ------------------------------------------------------------------------------
# --- VIEWER -------------------------------------------------------------------
# ------------------------------------------------------------------------------
@@ -94,6 +115,10 @@ def inc():
# Execute a function at time t, if specified with t.add(...)
attime.run(robot.control.time)
zmpup()
+ robot.viewer.updateElementConfig('zmp',[zmp.zmp.value[0],zmp.zmp.value[1],0,0,0,0])
+ if dyn.com.time >0:
+ robot.viewer.updateElementConfig('com',[dyn.com.value[0],dyn.com.value[1],0,0,0,0])
+
from ThreadInterruptibleLoop import *
@loopInThread
@@ -138,6 +163,19 @@ def t():
mp.hlp_toggle()
mp.refresh()
+# Add a visual output when an event is called.
+class Ping:
+ def __init__(self):
+ self.pos = 1
+ self.refresh()
+ def refresh(self):
+ robot.viewer.updateElementConfig('pong', [ 0, 0.9, self.pos*0.1, 0, 0, 0 ])
+ def __call__(self):
+ self.pos += 1
+ self.refresh()
+ping=Ping()
+attime.addPing( ping )
+
#-----------------------------------------------------------------------------
#---- DYN --------------------------------------------------------------------
@@ -173,30 +211,31 @@ robot.control.unplug()
#-----------------------------------------------------------------------------
-class MocapTracker(MocapParserTimed):
+class MocapTracker(MocapParserScaled):
class TaskAssociation:
def __init__(self,task,mocap):
self.task = task
self.mocap = mocap
def __init__(self,*args):
- MocapParserTimed.__init__(self,*args)
+ MocapParserScaled.__init__(self,*args)
self.jointMap = dict()
+ self.posture = None
def addJointMap( self,jointName,metaTask ):
joint=None
- for i,j in enumerate(self.joints):
- if j.name==jointName: joint=i
- if joint==None:
+ if not jointName in self.joints:
print "Error, joint name ",jointName," does not correspond to a mocap node."
- self.jointMap[jointName] = MocapTracker.TaskAssociation(metaTask,joint)
+ return
+ self.jointMap[jointName] = MocapTracker.TaskAssociation(metaTask,jointName)
def rmJointMap( self,jointName):
- if jointName in jointMap.keys():
- del jointMap[jointName]
+ if jointName in self.jointMap.keys():
+ del self.jointMap[jointName]
else: print "Error, joint name ",jointName," is not stored yet."
def update(self):
- MocapParserTimed.update(self)
+ MocapParserScaled.update(self)
for n,p in self.jointMap.iteritems():
p.task.ref = self.jointPosition(p.mocap)
-
+ if self.posture != None:
+ self.posture.value = self.qs[self.iter]
#-----------------------------------------------------------------------------
mp = MocapTracker('yoga/','yoga/outputJointsYogaTR')
mp.setLinksMap()
@@ -208,6 +247,9 @@ mp.with_dispLinks = False
mp.hideLinks()
mp.delayTime = 0
+mp.alignHrpFrames()
+for joint in mp.joints.values():
+ joint.Ki[0:3,3] *= 0
#-----------------------------------------------------------------------------
@@ -220,10 +262,11 @@ taskChest = MetaTaskDyn6d('taskChest',dyn,'chest','chest')
taskHead = MetaTaskDyn6d('taskHead',dyn,'head','gaze')
taskrh = MetaTaskDyn6d('rh',dyn,'rh','right-wrist')
tasklh = MetaTaskDyn6d('lh',dyn,'lh','left-wrist')
+taskrf = MetaTaskDyn6d('rf',dyn,'rf','right-ankle')
-for task in [ taskWaist, taskChest, taskHead, taskrh, tasklh ]:
+for task in [ taskWaist, taskChest, taskHead, taskrh, tasklh, taskrf ]:
taskWaist.feature.frame('current')
- taskWaist.gain.setConstant(100)
+ taskWaist.gain.setConstant(50)
taskWaist.task.dt.value = dt
#-----------------------------------------------------------------------------
@@ -328,7 +371,7 @@ def pr():
from dynamic_graph.tracer import *
tr = Tracer('tr')
-tr.open('/tmp/','step_','.dat')
+tr.open('/tmp/','yoga_','.dat')
tr.add('dyn.com','com')
tr.add('dyn.waist','waist')
@@ -356,22 +399,31 @@ robot.after.addSignal('sot.forcesNormal')
# --- RUN --------------------------------------------------------------------
#-----------------------------------------------------------------------------
-mp.setPositionMethod("KM")
+mp.setPositionMethod("KMK")
mp.refresh()
mp.pause()
mp.timeScale = 2
+mp.spaceScale = 0.9
+# Initial position of the mocap reference
dyn.rf.recompute(0)
dyn.lf.recompute(0)
Mrfr = matrix(dyn.rf.value)
-Mrfm = matrix(mp.jointPosition_M(8,0))
+Mrfm = matrix(mp.jointPosition_M("Rfoot",0))
Mlfr = matrix(dyn.lf.value)
-Mlfm = matrix(mp.jointPosition_M(8,0))
-#mp.Kw = Mrfr*Mrfm.I
-#mp.Kw[0:3,0:3] = eye(3)
+Mlfm = matrix(mp.jointPosition_M("Lfoot",0))
mp.Kw[0:3,0:3] = eye(3)
-mp.Kw[0:3,3] = Mrfr[0:3,3]-Mrfm[0:3,3]
+mp.Kw[0:3,3] = (Mrfr[0:3,3]-Mrfm[0:3,3] + Mlfr[0:3,3]-Mlfm[0:3,3])/2
+
+for mocap,task in ( ("Lhand",tasklh),("Rhand",taskrh),("head",taskHead)):
+ wmMm = matrix(mp.jointPosition_KM(mocap,0))
+ task.feature.position.recompute(0)
+ wrMr = matrix(task.feature.position.value)
+ mp.joints[mocap].Ki = wmMm.I*wrMr
+ mp.joints[mocap].Ki[0:3,3] *= 0
+ wmMm = matrix(mp.jointPosition_KMK(mocap,0))
+ mp.joints[mocap].Kw = wrMr*wmMm.I*mp.joints[mocap].Kw
def updateMocap():
mp.update()
@@ -385,27 +437,59 @@ sot._LF_p.value = supportLF
featureComDes.errorIN.value = ( 0.01, 0., 0.8077 )
featureCom.selec.value = "11"
-gCom.setConstant( 500.0 )
-sot.push('taskCom')
+#gCom.setConstant( 500.0 )
-sot.push(taskrh.task.name)
-sot.push(tasklh.task.name)
taskrh.feature.selec.value = '111'
-taskrh.gain.setConstant(500)
+#taskrh.gain.setConstant(500)
+mrh=eye(4)
+mrh[0:3,3] = (0,0,-0.08)
+taskrh.opmodif = matrixToTuple(mrh)
+
tasklh.feature.selec.value = '111'
-tasklh.gain.setConstant(500)
+#tasklh.gain.setConstant(500)
+tasklh.opmodif = matrixToTuple(mrh)
-mp.addJointMap("Rhand",taskrh)
-mp.addJointMap("Lhand",tasklh)
+#taskHead.gain.setConstant(500)
+taskHead.feature.selec.value = '111000'
+taskrf.feature.selec.value = '111111'
+mrf=eye(4)
+mrf[0:3,3] = (0,0,-0.05)
+taskrf.opmodif = matrixToTuple(mrf)
+sot.push('taskCom')
+#sot.push(taskrh.task.name)
+#sot.push(tasklh.task.name)
+#sot.push(taskHead.task.name)
+#sot.push(taskWaist.task.name)
+
+#mp.addJointMap("Rhand",taskrh)
+#mp.addJointMap("Lhand",tasklh)
+mp.addJointMap("head",taskHead)
+mp.addJointMap("Rfoot",taskrf)
+mp.posture = sot.posture
+plug(robot.state,sot.position)
+sot.breakFactor.value = 20
+
+featureComDes.errorIN.value = ( 0.01, 0.09, 0.8077 )
+gCom.setConstant(100)
+
+mp.forward()
inc()
-#go()
+go()
-#attime(2, lambda: mp.forward())
-#attime(2, lambda: taskrh.gain.setConstant(2/dt))
-#attime(2, lambda: tasklh.gain.setConstant(2/dt))
+sigset = ( lambda s,v : s.__class__.value.__set__(s,v) )
+#attime(200, lambda: mp.forward())
+#attime(200, lambda: taskrh.gain.setConstant(2/dt))
+#attime(200, lambda: tasklh.gain.setConstant(2/dt))
+#attime(100, lambda: sigset( featureComDes.errorIN,(0.01,0.1,0.8)) )
+
+#attime(40*mp.timeScale, lambda: contactRF.feature.keep() )
+attime(40*mp.timeScale, lambda: sot.rmContact("RF"),"Remove RF contact" )
+attime(40*mp.timeScale, lambda: sot.push(taskrf.task.name), "Start to track RF mocap" )
+
+rfFinal = eye(4); rfFinal[0:3,3] = (0.02,-0.12,0.105-0.05)
+attime(1050*mp.timeScale,
+ (lambda: sigset(taskrf.featureDes.position,matrixToTuple(rfFinal)), "RF to final position"),
+ (mp.rmJointMap("Rfoot"),"Stop tracking RF mocap"))
-mrh=eye(4)
-mrh[0:3,3] = (0,0,-0.2)
-taskrh.opmodif = matrixToTuple(mrh)