diff --git a/python/mocap/slide.py b/python/mocap/slide.py
index d37e15f0359ff5c1e425b9c29e0dd574a7ea6aeb..795c821114a0855eb98342312a77827c407fa961 100644
--- a/python/mocap/slide.py
+++ b/python/mocap/slide.py
@@ -19,12 +19,15 @@ from dynamic_graph.script_shortcuts import optionalparentheses
from dynamic_graph.matlab import matlab
sys.path.append('..')
from dynamic_graph.sot.core.meta_task_6d import toFlags
+from meta_tasks_dyn import *
from meta_task_dyn_6d import MetaTaskDyn6d
from attime import attime
from numpy import *
from robotSpecific import pkgDataRootDir,modelName,robotDimension,initialConfig,gearRatio,inertiaRotor
from matrix_util import matrixToTuple, vectorToTuple
from history import History
+from zmp_estimator import ZmpEstimator
+from viewer_helper import addRobotViewer,VisualPinger
#-----------------------------------------------------------------------------
# --- ROBOT SIMU -------------------------------------------------------------
@@ -35,62 +38,17 @@ robotDim = robotDimension[robotName]
RobotClass = RobotDynSimu
robot = RobotClass("robot")
robot.resize(robotDim)
+addRobotViewer(robot,small=True,verbose=False)
dt = 5e-3
-
-
-#half sitting
-#qhs=matrix((0,0,0,0,0,0, 0,0,-26,50,-24,0,0,0,-26,50,-24,0,0,0,0,0,15,10,0,-30,0,0,10,15,-10,0,-30,0,0,10))
-#robot.set((0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -0.4537856055185257, 0.87266462599716477, -0.41887902047863906, 0.0, 0.0, 0.0, -0.4537856055185257, 0.87266462599716477, -0.41887902047863906, 0.0, 0.0, 0.0, 0.0, 0.0, 0.26179938779914941, 0.17453292519943295, 0.0, -0.52359877559829882, 0.0, 0.0, 0.17453292519943295, 0.26179938779914941, -0.17453292519943295, 0.0, -0.52359877559829882, 0.0, 0.0, 0.17453292519943295))
-
# 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))
-# ------------------------------------------------------------------------------
-# --- VIEWER -------------------------------------------------------------------
-# ------------------------------------------------------------------------------
-
-try:
- import robotviewer
-
- def stateFullSize(robot):
- return [float(val) for val in robot.state.value]+10*[0.0]
- RobotClass.stateFullSize = stateFullSize
- robot.viewer = robotviewer.client('XML-RPC')
- #robot.viewer = robotviewer.client('CORBA')
- # Check the connection
- robot.viewer.updateElementConfig('hrp',robot.stateFullSize())
-
- def refreshView( robot ):
- robot.viewer.updateElementConfig('hrp',robot.stateFullSize())
- RobotClass.refresh = refreshView
-
- def incrementView(robot,dt):
- robot.incrementNoView(dt)
- robot.refresh()
- #if zmp.zmp.time > 0:
- # robot.viewer.updateElementConfig('zmp',[zmp.zmp.value[0],zmp.zmp.value[1],0,0,0,0])
- RobotClass.incrementNoView = RobotClass.increment
- RobotClass.increment = incrementView
-
- def setView( robot,*args ):
- robot.setNoView(*args)
- robot.refresh()
- RobotClass.setNoView = RobotClass.set
- RobotClass.set = setView
- robot.refresh()
-
-except:
- print "No robot viewer, sorry."
- class RobotViewerFaked:
- def update(*args): void
- def updateElementConfig(*args): void
- robot.viewer = RobotViewerFaked()
-
#-------------------------------------------------------------------------------
#----- MAIN LOOP ---------------------------------------------------------------
#-------------------------------------------------------------------------------
+
class EllipseTemporal:
def __init__(self,tempo):
self.t0 = None
@@ -117,11 +75,10 @@ class EllipseTemporal:
traj = EllipseTemporal(robot.state)
-
def inc():
robot.increment(dt)
# Execute a function at time t, if specified with t.add(...)
- zmpup()
+ if 'refresh' in ZmpEstimator.__dict__: zmp.refresh()
attime.run(robot.control.time)
robot.viewer.updateElementConfig('zmp',[zmp.zmp.value[0],zmp.zmp.value[1],0,0,0,0])
if dyn.com.time >0:
@@ -148,7 +105,6 @@ def next(): inc()
# --- shortcuts -------------------------------------------------
@optionalparentheses
def q():
- if 'dyn' in globals(): print dyn.ffposition.__repr__()
print robot.state.__repr__()
@optionalparentheses
def qdot(): print robot.control.__repr__()
@@ -157,53 +113,11 @@ def iter(): print 'iter = ',robot.state.time
@optionalparentheses
def status(): print runner.isPlay
-
@optionalparentheses
def dump():
history.dumpToOpenHRP('openhrp/slide')
-
-
-
-# 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 )
-
-def goto6d(task,position,gain=None):
- M=eye(4)
- if( len(position)==3 ): M[0:3,3] = position
- else: print "Position 6D with rotation ... todo"
- task.feature.selec.value = "111111"
- if gain!=None: task.gain.setConstant(gain)
- task.featureDes.position.value = matrixToTuple(M)
-
-def gotoNd(task,position,selec,gain=None):
- M=eye(4)
- if isinstance(position,matrix): position = vectorToTuple(position)
- if( len(position)==3 ): M[0:3,3] = position
- else: print "Position 6D with rotation ... todo"
- if isinstance(selec,str): task.feature.selec.value = selec
- else: task.feature.selec.value = toFlags(selec)
- task.featureDes.position.value = matrixToTuple(M)
- if gain!=None:
- if len(gain)==1: task.gain.setConstant(gain)
- elif len(gain)==3: task.gain.set( gain[0],gain[1],gain[2])
- elif len(gain)==4: task.gain.setByPoint( gain[0],gain[1],gain[2],gain[3])
-
-def contact(contact,task=None):
- sot.addContactFromTask(contact.task.name,contact.name)
- sot.signal("_"+contact.name+"_p").value = contact.support
- if task!= None: sot.rm(task.task.name)
- contact.task.resetJacobianDerivative()
+attime.addPing( VisualPinger(robot.viewer) )
#-----------------------------------------------------------------------------
@@ -264,44 +178,7 @@ for task in [ taskWaist, taskChest, taskHead, taskrh, tasklh, taskrf, taskrfz ]:
task.gain.setConstant(50)
task.task.dt.value = dt
-
-
-#-----------------------------------------------------------------------------
-# --- OTHER TASKS ------------------------------------------------------------
-#-----------------------------------------------------------------------------
-
# --- TASK COM ------------------------------------------------------
-
-class MetaTaskDynCom(object):
- def __init__(self,dyn,dt,name="com"):
- self.dyn=dyn
- self.name=name
- dyn.setProperty('ComputeCoM','true')
-
- self.feature = FeatureGeneric('feature'+name)
- self.featureDes = FeatureGeneric('featureDes'+name)
- self.task = TaskDynPD('task'+name)
- self.gain = GainAdaptive('gain'+name)
-
- plug(dyn.com,self.feature.errorIN)
- plug(dyn.Jcom,self.feature.jacobianIN)
- self.feature.sdes.value = self.featureDes.name
-
- self.task.add(self.feature.name)
- plug(dyn.velocity,self.task.qdot)
- self.task.dt.value = dt
-
- plug(self.task.error,self.gain.error)
- plug(self.gain.gain,self.task.controlGain)
-
- @property
- def ref(self):
- return self.featureDes.errorIN.value
-
- @ref.setter
- def ref(self,v):
- self.featureDes.errorIN.value = v
-
taskCom = MetaTaskDynCom(dyn,dt)
@@ -348,6 +225,7 @@ taskLim.referenceVelSup.value = tuple([val*pi/180 for val in dqup])
#-----------------------------------------------------------------------------
sot = SolverDynReduced('sot')
+contact = AddContactHelper(sot)
sot.setSize(robotDim-6)
#sot.damping.value = 1e-2
sot.breakFactor.value = 20
@@ -370,59 +248,20 @@ plug(sot.acceleration,robot.acceleration)
contactLF = MetaTaskDyn6d('contact_lleg',dyn,'lf','left-ankle')
contactLF.feature.frame('desired')
contactLF.gain.setConstant(1000)
+contactLF.name = "LF"
# Right foot contact
contactRF = MetaTaskDyn6d('contact_rleg',dyn,'rf','right-ankle')
contactRF.feature.frame('desired')
+contactRF.name = "RF"
-# ((0.03,-0.03,-0.03,0.03),(-0.015,-0.015,0.015,0.015),(-0.105,-0.105,-0.105,-0.105))
-# ((0.03,-0.03,-0.03,0.03),(-0.015,-0.015,0.015,0.015),(-0.105,-0.105,-0.105,-0.105))
contactRF.support = ((0.11,-0.08,-0.08,0.11),(-0.045,-0.045,0.07,0.07),(-0.105,-0.105,-0.105,-0.105))
contactLF.support = ((0.11,-0.08,-0.08,0.11),(-0.07,-0.07,0.045,0.045),(-0.105,-0.105,-0.105,-0.105))
-#contactLF.support = ((0.11,-0.08,-0.08,0.11),(-0.07,-0.07,0.045,0.045),(-0.105,-0.105,-0.105,-0.105))
contactLF.support = ((0.03,-0.03,-0.03,0.03),(-0.015,-0.015,0.015,0.015),(-0.105,-0.105,-0.105,-0.105))
-contactLF.name = "LF"
-contactRF.name = "RF"
-#-----------------------------------------------------------------------------
#--- ZMP ---------------------------------------------------------------------
-#-----------------------------------------------------------------------------
zmp = ZmpEstimator('zmp')
-def computeZmp():
- p=zeros((4,0))
- f=zeros((0,1))
- if '_RF_p' in [s.name for s in sot.signals()]:
- Mr=matrix(dyn.rf.value)
- fr=matrix(sot._RF_fn.value).transpose()
- pr=matrix(sot._RF_p.value+((1,1,1,1),))
- p=hstack((p,Mr*pr))
- f=vstack((f,fr))
- if '_LF_p' in [s.name for s in sot.signals()]:
- Ml=matrix(dyn.lf.value)
- fl=matrix(sot._LF_fn.value).transpose()
- pl=matrix(sot._LF_p.value+((1,1,1,1),))
- p=hstack((p,Ml*pl))
- f=vstack((f,fl))
- zmp=p*f/sum(f)
- return zmp
-def zmpup():
- zmp.zmp.value = tuple(computeZmp()[0:3].transpose().tolist()[0])
- zmp.zmp.time = sot.solution.time
-
-@optionalparentheses
-def pl():
- if '_LF_p' in [s.name for s in sot.signals()]:
- print 'checkin'
- Ml=matrix(dyn.lf.value)
- pl=matrix(sot._LF_p.value+((1,1,1,1),))
- return matlab( matrixToTuple((Ml*pl)[0:3,:]) ).resstr
-@optionalparentheses
-def pr():
- if '_RF_p' in [s.name for s in sot.signals()]:
- Mr=matrix(dyn.rf.value)
- pr=matrix(sot._RF_p.value+((1,1,1,1),))
- return matlab( matrixToTuple( (Mr*pr)[0:3,:] ))
-
+zmp.declare(sot,dyn)
#-----------------------------------------------------------------------------
# --- TRACE ------------------------------------------------------------------
@@ -458,15 +297,8 @@ robot.after.addSignal('dyn.waist')
robot.after.addSignal('taskLim.normalizedPosition')
tr.add('taskLim.normalizedPosition','qn')
-
-
history = History(dyn,1,zmp.zmp)
-
-
-#-----------------------------------------------------------------------------
-# --- FUNCTIONS TO PUSH/PULL/UP/DOWN TASKS -----------------------------------
-
#-----------------------------------------------------------------------------
# --- RUN --------------------------------------------------------------------
#-----------------------------------------------------------------------------
@@ -480,8 +312,6 @@ contact(contactRF)
#taskCom.feature.selec.value = "111"
taskCom.gain.setByPoint(100,10,0.005,0.8)
-#taskCom.gain.setConstant(2)
-
rfz0=0.020
rf0=matrix((0.0095,-0.095,rfz0))
@@ -489,7 +319,6 @@ rf0=matrix((0.0095,-0.095,rfz0))
#traj.set( vectorToTuple(rf0[0,0:2]),(0.35,-0.2),1200 )
traj.set( vectorToTuple(rf0[0,0:2]),(0.4,-0.42),1200 )
-
mrf=eye(4)
mrf[0:3,3] = (0,0,-0.105)
taskrf.opmodif = matrixToTuple(mrf)
@@ -513,7 +342,6 @@ taskrfz.feature.frame('desired')
taskHead.feature.selec.value='011000'
taskHead.featureDes.position.value = matrixToTuple(eye(4))
-
sot.push(taskLim.name)
sot.push(taskHead.task.name)
plug(robot.state,sot.position)
@@ -522,7 +350,6 @@ plug(robot.state,sot.position)
sigset = ( lambda s,v : s.__class__.value.__set__(s,v) )
refset = ( lambda mt,v : mt.__class__.ref.__set__(mt,v) )
-
attime(2
,(lambda : sot.push(taskCom.task.name),"Add COM")
,(lambda : sigset(taskCom.feature.selec, "11"),"COM XY")