diff --git a/python/2013_coursens/p105_demo.py b/python/2013_coursens/p105_demo.py
new file mode 100644
index 0000000000000000000000000000000000000000..69de4ea6771fec023707f9fedad75fedfa25d4c4
--- /dev/null
+++ b/python/2013_coursens/p105_demo.py
@@ -0,0 +1,437 @@
+# ______________________________________________________________________________
+# ******************************************************************************
+# ______________________________________________________________________________
+# ******************************************************************************
+import sys
+execfile('/home/nmansard/.pythonrc')
+
+sys.path.append('..')
+from dynamic_graph import plug
+from dynamic_graph.sot.core import *
+from dynamic_graph.sot.core.math_small_entities import Derivator_of_Matrix
+from dynamic_graph.sot.dynamics import *
+from dynamic_graph.sot.dyninv import *
+import dynamic_graph.script_shortcuts
+from dynamic_graph.script_shortcuts import optionalparentheses
+from dynamic_graph.matlab import matlab
+from dynamic_graph.sot.core.matrix_util import matrixToTuple, vectorToTuple,rotate, matrixToRPY
+from dynamic_graph.sot.core.meta_task_6d import MetaTask6d,toFlags
+from dynamic_graph.sot.core.meta_tasks import setGain
+from dynamic_graph.sot.core.meta_tasks_kine import *
+from dynamic_graph.sot.core.meta_task_posture import MetaTaskKinePosture
+from dynamic_graph.sot.core.meta_task_visual_point import MetaTaskVisualPoint
+from dynamic_graph.sot.core.utils.viewer_helper import addRobotViewer,VisualPinger,updateComDisplay
+from dynamic_graph.sot.core.utils.attime import attime,ALWAYS,refset,sigset
+from numpy import *
+
+from history import History
+
+from robotSpecific import pkgDataRootDir,modelName,robotDimension,initialConfig,gearRatio,inertiaRotor
+
+# --- ROBOT SIMU ---------------------------------------------------------------
+# --- ROBOT SIMU ---------------------------------------------------------------
+# --- ROBOT SIMU ---------------------------------------------------------------
+
+robotName = 'hrp14small'
+robotDim=robotDimension[robotName]
+robot = RobotSimu("robot")
+robot.resize(robotDim)
+dt=5e-3
+
+from robotSpecific import halfSittingConfig
+x0=-0.00949035111398315034
+y0=0
+z0=0.64870185118253043
+halfSittingConfig[robotName] = (x0,y0,z0,0,0,0)+halfSittingConfig[robotName][6:]
+
+q0=list(halfSittingConfig[robotName])
+initialConfig[robotName]=tuple(q0)
+
+robot.set( initialConfig[robotName] )
+addRobotViewer(robot,small=True,verbose=True)
+
+#-------------------------------------------------------------------------------
+#----- MAIN LOOP ---------------------------------------------------------------
+#-------------------------------------------------------------------------------
+from dynamic_graph.sot.core.utils.thread_interruptible_loop import loopInThread,loopShortcuts
+@loopInThread
+def inc():
+    robot.increment(dt)
+    attime.run(robot.control.time)
+    updateComDisplay(robot,dyn.com)
+    history.record()
+
+runner=inc()
+[go,stop,next,n]=loopShortcuts(runner)
+
+#-----------------------------------------------------------------------------
+#---- DYN --------------------------------------------------------------------
+#-----------------------------------------------------------------------------
+modelDir  = pkgDataRootDir[robotName]
+xmlDir    = pkgDataRootDir[robotName]
+specificitiesPath = xmlDir + '/HRP2SpecificitiesSmall.xml'
+jointRankPath     = xmlDir + '/HRP2LinkJointRankSmall.xml'
+
+dyn = Dynamic("dyn")
+dyn.setFiles(modelDir, modelName[robotName],specificitiesPath,jointRankPath)
+dyn.parse()
+
+dyn.inertiaRotor.value = inertiaRotor[robotName]
+dyn.gearRatio.value    = gearRatio[robotName]
+
+plug(robot.state,dyn.position)
+dyn.velocity.value = robotDim*(0.,)
+dyn.acceleration.value = robotDim*(0.,)
+
+dyn.ffposition.unplug()
+dyn.ffvelocity.unplug()
+dyn.ffacceleration.unplug()
+
+dyn.setProperty('ComputeBackwardDynamics','true')
+dyn.setProperty('ComputeAccelerationCoM','true')
+
+robot.control.unplug()
+
+
+# ---- SOT ---------------------------------------------------------------------
+# ---- SOT ---------------------------------------------------------------------
+# ---- SOT ---------------------------------------------------------------------
+# The solver SOTH of dyninv is used, but normally, the SOT solver should be sufficient
+from dynamic_graph.sot.dyninv import SolverKine
+def toList(sot):
+    return map(lambda x: x[1:-1],sot.dispStack().split('|')[1:])
+SolverKine.toList = toList
+sot = SolverKine('sot')
+sot.setSize(robotDim)
+plug(sot.control,robot.control)
+
+# ---- TASKS -------------------------------------------------------------------
+# ---- TASKS -------------------------------------------------------------------
+# ---- TASKS -------------------------------------------------------------------
+
+
+# ---- TASK GRIP ---
+taskRH=MetaTaskKine6d('rh',dyn,'rh','right-wrist')
+handMgrip=eye(4); handMgrip[0:3,3] = (0,0,-0.21)
+taskRH.opmodif = matrixToTuple(handMgrip)
+taskRH.feature.frame('desired')
+
+taskLH=MetaTaskKine6d('lh',dyn,'lh','left-wrist')
+taskLH.opmodif = matrixToTuple(handMgrip)
+taskLH.feature.frame('desired')
+
+# --- STATIC COM (if not walking)
+taskCom = MetaTaskKineCom(dyn)
+
+# --- TASK AVOID
+taskShoulder=MetaTaskKine6d('shoulder',dyn,'shoulder','LARM_JOINT0')
+taskShoulder.feature.frame('desired')
+gotoNd(taskShoulder,(0,0,0),'010')
+taskShoulder.task = TaskInequality('taskShoulderAvoid')
+taskShoulder.task.add(taskShoulder.feature.name)
+taskShoulder.task.referenceInf.value = (-10,)    # Xmin, Ymin
+taskShoulder.task.referenceSup.value = (0.20,)    # Xmin, Ymin
+taskShoulder.task.dt.value=dt
+taskShoulder.task.controlGain.value = 0.9
+
+taskElbow=MetaTaskKine6d('elbow',dyn,'elbow','LARM_JOINT3')
+taskElbow.feature.frame('desired')
+gotoNd(taskElbow,(0,0,0),'010')
+taskElbow.task = TaskInequality('taskElbowAvoid')
+taskElbow.task.add(taskElbow.feature.name)
+taskElbow.task.referenceInf.value = (-10,)    # Xmin, Ymin
+taskElbow.task.referenceSup.value = (0.20,)    # Xmin, Ymin
+taskElbow.task.dt.value=dt
+taskElbow.task.controlGain.value = 0.9
+
+
+# --- TASK SUPPORT --------------------------------------------------
+featureSupport    = FeatureGeneric('featureSupport')
+plug(dyn.com,featureSupport.errorIN)
+plug(dyn.Jcom,featureSupport.jacobianIN)
+
+taskSupport=TaskInequality('taskSupport')
+taskSupport.add(featureSupport.name)
+taskSupport.selec.value = '011'
+taskSupport.referenceInf.value = (-0.08,-0.15,0)    # Xmin, Ymin
+taskSupport.referenceSup.value = (0.11,0.15,0)  # Xmax, Ymax
+taskSupport.dt.value=dt
+
+# --- TASK SUPPORT SMALL --------------------------------------------
+featureSupportSmall = FeatureGeneric('featureSupportSmall')
+plug(dyn.com,featureSupportSmall.errorIN)
+plug(dyn.Jcom,featureSupportSmall.jacobianIN)
+
+taskSupportSmall=TaskInequality('taskSupportSmall')
+taskSupportSmall.add(featureSupportSmall.name)
+taskSupportSmall.selec.value = '011'
+#taskSupportSmall.referenceInf.value = (-0.02,-0.02,0)    # Xmin, Ymin
+#askSupportSmall.referenceSup.value = (0.02,0.02,0)  # Xmax, Ymax
+taskSupportSmall.referenceInf.value = (-0.02,-0.05,0)    # Xmin, Ymin
+taskSupportSmall.referenceSup.value = (0.02,0.05,0)  # Xmax, Ymax
+taskSupportSmall.dt.value=dt
+
+# --- POSTURE ---
+taskPosture = MetaTaskKinePosture(dyn)
+
+# --- GAZE ---
+taskGaze = MetaTaskVisualPoint('gaze',dyn,'head','gaze')
+# Head to camera matrix transform
+# Camera RU headMcam=array([[0.0,0.0,1.0,0.0825],[1.,0.0,0.0,-0.029],[0.0,1.,0.0,0.102],[0.0,0.0,0.0,1.0]])
+# Camera LL 
+headMcam=array([[0.0,0.0,1.0,0.081],[1.,0.0,0.0,0.072],[0.0,1.,0.0,0.031],[0.0,0.0,0.0,1.0]])
+headMcam = dot(headMcam,rotate('x',10*pi/180))
+taskGaze.opmodif = matrixToTuple(headMcam)
+
+# --- FOV ---
+taskFoV = MetaTaskVisualPoint('FoV',dyn,'head','gaze')
+taskFoV.opmodif = matrixToTuple(headMcam)
+
+taskFoV.task=TaskInequality('taskFoVineq')
+taskFoV.task.add(taskFoV.feature.name)
+[Xmax,Ymax]=[0.38,0.28]
+taskFoV.task.referenceInf.value = (-Xmax,-Ymax)    # Xmin, Ymin
+taskFoV.task.referenceSup.value = (Xmax,Ymax)  # Xmax, Ymax
+taskFoV.task.dt.value=dt
+taskFoV.task.controlGain.value=0.01
+taskFoV.featureDes.xy.value = (0,0)
+
+
+# --- Task Joint Limits -----------------------------------------
+dyn.upperJl.recompute(0)
+dyn.lowerJl.recompute(0)
+taskJL = TaskJointLimits('taskJL')
+plug(dyn.position,taskJL.position)
+taskJL.controlGain.value = 10
+taskJL.referenceInf.value = dyn.lowerJl.value
+taskJL.referenceSup.value = dyn.upperJl.value
+taskJL.dt.value = dt
+taskJL.selec.value = toFlags(range(6,22)+range(22,28)+range(29,35))
+
+
+# --- CONTACTS
+# define contactLF and contactRF
+for name,joint in [ ['LF','left-ankle'], ['RF','right-ankle' ] ]:
+    contact = MetaTaskKine6d('contact'+name,dyn,name,joint)
+    contact.feature.frame('desired')
+    contact.gain.setConstant(10)
+    locals()['contact'+name] = contact
+
+# --- TRACER -----------------------------------------------------------------
+from dynamic_graph.tracer import *
+tr = Tracer('tr')
+tr.open('/tmp/','','.dat')
+tr.start()
+robot.after.addSignal('tr.triger')
+
+tr.add('dyn.com','com')
+
+history = History(dyn,1)
+
+# --- SHORTCUTS ----------------------------------------------------------------
+qn = taskJL.normalizedPosition
+@optionalparentheses
+def pqn(details=True):
+    ''' Display the normalized configuration vector. '''
+    qn.recompute(robot.state.time)
+    s = [ "{0:.1f}".format(v) for v in qn.value]
+    if details:
+        print("Rleg: "+" ".join(s[:6]))
+        print("Lleg: "+" ".join(s[6:12]))
+        print("Body: "+" ".join(s[12:16]))
+        print("Rarm: "+" ".join(s[16:23]))
+        print("Larm :"+" ".join(s[23:30]))
+    else:
+        print(" ".join(s[:30]))
+
+
+def jlbound(t=None):
+    '''Display the velocity bound induced by the JL as a double-column matrix.'''
+    if t==None: t=robot.state.time
+    taskJL.task.recompute(t)
+    return matrix([ [float(x),float(y)] for x,y
+                    in [ c.split(',') for c in taskJL.task.value[6:-3].split('),(') ] ])
+
+def p6d(R,t):
+    M=eye(4)
+    M[0:3,0:3]=R
+    M[0:3,3]=t
+    return M
+
+def push(task):
+    if isinstance(task,str): taskName=task
+    elif "task" in task.__dict__:  taskName=task.task.name
+    else: taskName=task.name
+    if taskName not in sot.toList():
+        sot.push(taskName)
+        if taskName!="taskposture" and "taskposture" in sot.toList():
+            sot.down("taskposture")
+
+
+def pop(task):
+    if isinstance(task,str): taskName=task
+    elif "task" in task.__dict__:  taskName=task.task.name
+    else: taskName=task.name
+    if taskName in sot.toList(): sot.rm(taskName)
+
+
+# --- DISPLAY ------------------------------------------------------------------
+# --- DISPLAY ------------------------------------------------------------------
+# --- DISPLAY ------------------------------------------------------------------
+RAD=pi/180
+comproj = [0.1,-0.95,1.6]
+#robot.viewer.updateElementConfig('footproj',[0.5,0.15,1.6+0.08,0,-pi/2,0 ])
+robot.viewer.updateElementConfig('footproj',comproj+[0,-pi/2,0 ])
+robot.viewer.updateElementConfig('zmp2',[0,0,-10,0,0,0])
+
+
+class BallPosition:
+    def __init__(self,xyz=(0,-1.1,0.9)):
+        self.ball = xyz
+        self.prec = 0
+        self.t = 0
+        self.duration = 0
+        self.f = 0
+        self.xyz= self.ball
+        
+    def move(self,xyz,duration=50):
+        self.prec = self.ball
+        self.ball = xyz
+        self.t = 0
+        self.duration = duration
+        self.changeTargets()
+
+        if duration>0:
+            self.f = lambda : self.moveDisplay()
+            attime(ALWAYS,self.f)
+        else:
+            self.moveDisplay()
+
+    def moveDisplay(self):
+        tau = 1.0 if self.duration<=0 else float(self.t) / self.duration
+        xyz = tau * array(self.ball) + (1-tau) * array(self.prec)
+        robot.viewer.updateElementConfig('zmp',vectorToTuple(xyz)+(0,0,0))
+
+        self.t += 1
+        if self.t>self.duration and self.duration>0:
+            attime.stop(self.f)
+        self.xyz= xyz
+        
+    def changeTargets(self):
+        gotoNd(taskRH,self.ball,'111',(4.9,0.9,0.01,0.9))
+        taskFoV.goto3D(self.ball)
+
+b = BallPosition()
+
+
+tr.add(taskJL.name+".normalizedPosition","qn")
+tr.add('dyn.com','com')
+tr.add(taskRH.task.name+'.error','erh')
+tr.add(taskFoV.feature.name+'.error','fov')
+tr.add(taskFoV.task.name+'.normalizedPosition','fovn')
+tr.add(taskSupportSmall.name+".normalizedPosition",'comsmalln')
+tr.add(taskSupport.name+".normalizedPosition",'comn')
+
+robot.after.addSignal(taskJL.name+".normalizedPosition")
+robot.after.addSignal(taskSupportSmall.name+".normalizedPosition")
+robot.after.addSignal(taskFoV.task.name+".normalizedPosition")
+robot.after.addSignal(taskFoV.feature.name+'.error')
+robot.after.addSignal(taskSupport.name+".normalizedPosition")
+
+# --- RUN ----------------------------------------------------------------------
+# --- RUN ----------------------------------------------------------------------
+# --- RUN ----------------------------------------------------------------------
+
+dyn.com.recompute(0)
+taskCom.featureDes.errorIN.value = dyn.com.value
+taskCom.task.controlGain.value = 10
+
+ball = BallPosition((0,-1.1,0.9))
+
+push(taskRH)
+ball.move((0.5,-0.2,1.3),0)
+
+next()
+next()
+next()
+ 
+def config(ref=0):
+    if ref==0:
+        print '''Only the task RH'''
+        none
+    elif ref==1:
+        print '''Task RH + foot constraint, balance is kept'''
+        sot.addContact(contactRF)
+        sot.addContact(contactLF)
+    elif ref==2:
+        print '''Task RH + foot constraint, balance is lost'''
+        sot.addContact(contactRF)
+        sot.addContact(contactLF)
+        ball.move((0.15,-0.2,1.3),0)
+    elif ref==3:
+        print '''Task RH + foot constraint + COM='''
+        sot.addContact(contactRF)
+        sot.addContact(contactLF)
+        push(taskCom)
+        ball.move((0.15,-0.2,1.3),0)
+    elif ref==4:
+        print '''Task RH + foot constraint + COM= + JL'''
+        qu =  list(dyn.upperJl.value)
+        qu[23]=-0.3
+        taskJL.referenceSup.value =tuple(qu)
+        push(taskJL)
+        sot.addContact(contactRF)
+        sot.addContact(contactLF)
+        push(taskCom)
+        ball.move((0.15,-0.2,1.3),0)
+    elif ref==5:
+        print '''Task RH + foot constraint + COM<'''
+        sot.addContact(contactRF)
+        sot.addContact(contactLF)
+        push(taskSupport)
+        ball.move((0.15,-0.2,1.3),0)
+    elif ref==6:
+        print '''Task RH + foot constraint + COM= + SINGULARITE '''
+        print '''(press 4x i to reach it)'''
+        sot.addContact(contactRF)
+        sot.addContact(contactLF)
+        push(taskCom)
+        ball.move((0.6,-0.2,1.3),0)
+    elif ref==7:
+        print '''Task RH + foot constraint + COM= + SINGULARITE + DAMPING'''
+        sot.addContact(contactRF)
+        sot.addContact(contactLF)
+        push(taskCom)
+        ball.move((0.9,-0.2,1.3),0)
+        sot.down(taskRH.task.name)
+        sot.down(taskRH.task.name)
+        sot.damping.value = 0.1
+
+    else:
+        print '''Not a correct config.'''
+        return
+    go()
+c=config
+
+@optionalparentheses
+def i():
+    xyz=ball.xyz
+    xyz[0] += 0.1
+    ball.move(vectorToTuple(xyz),30)
+
+
+def menu(ref=0):
+    print '\n\n\n\n\n\n\n\n\n'
+    print '''0: Only the task RH'''
+    print '''1: Task RH + foot constraint, balance is kept'''
+    print '''2: Task RH + foot constraint, balance is lost'''
+    print '''3: Task RH + foot constraint + COM='''
+    print '''4: Task RH + foot constraint + COM= + JL'''
+    print '''5: Task RH + foot constraint + COM<'''
+    print '''6: Task RH + foot constraint + COM= + SINGULARITE '''
+    print '''7: Task RH + foot constraint + COM= + SINGULARITE + DAMPING'''
+    print ''
+    uinp = raw_input('   Config? >> ')
+    config(int(uinp))
+
+menu()
diff --git a/python/2013_coursens/p105_walk.py b/python/2013_coursens/p105_walk.py
new file mode 100644
index 0000000000000000000000000000000000000000..d7c70731a061c5530b05096e66949d85f9028f35
--- /dev/null
+++ b/python/2013_coursens/p105_walk.py
@@ -0,0 +1,386 @@
+# ______________________________________________________________________________
+# ******************************************************************************
+# ______________________________________________________________________________
+# ******************************************************************************
+
+sys.path.append('..')
+from dynamic_graph import plug
+from dynamic_graph.sot.core import *
+from dynamic_graph.sot.core.math_small_entities import Derivator_of_Matrix
+from dynamic_graph.sot.dynamics import *
+from dynamic_graph.sot.dyninv import *
+import dynamic_graph.script_shortcuts
+from dynamic_graph.script_shortcuts import optionalparentheses
+from dynamic_graph.matlab import matlab
+from dynamic_graph.sot.core.matrix_util import matrixToTuple, vectorToTuple
+from dynamic_graph.sot.core.meta_task_6d import MetaTask6d,toFlags
+from dynamic_graph.sot.core.meta_tasks import setGain
+from dynamic_graph.sot.core.meta_tasks_kine import *
+from dynamic_graph.sot.core.meta_task_posture import MetaTaskKinePosture
+from dynamic_graph.sot.core.utils.viewer_helper import addRobotViewer,VisualPinger,updateComDisplay
+from dynamic_graph.sot.core.utils.attime import attime,ALWAYS,refset,sigset
+from numpy import *
+
+from history import History
+
+from robotSpecific import pkgDataRootDir,modelName,robotDimension,initialConfig,gearRatio,inertiaRotor
+
+# --- ROBOT SIMU ---------------------------------------------------------------
+# --- ROBOT SIMU ---------------------------------------------------------------
+# --- ROBOT SIMU ---------------------------------------------------------------
+
+robotName = 'hrp14small'
+robotDim=robotDimension[robotName]
+robot = RobotSimu("robot")
+robot.resize(robotDim)
+dt=5e-3
+
+from robotSpecific import halfSittingConfig
+x0=-0.00949035111398315034
+y0=0
+z0=0.64870185118253043  #0.6487018512
+halfSittingConfig[robotName] = (x0,y0,z0,0,0,0)+halfSittingConfig[robotName][6:]
+
+initialConfig[robotName]=halfSittingConfig[robotName]
+
+initialConfig[robotName]=(-1.9487374883906774e-27, -1.7176468466207308e-27, 0.63516120560912981, 4.9861897636537844e-20, -1.0588941103603052e-18, 1.2364668742710116e-19, -4.8902218174850969e-18, 2.4342667073361099e-18, -0.48729641533290902, 0.97459283066581781, -0.48729641533290879, -9.861508377534219e-19, -2.359096854752087e-18, -1.3129513558333748e-17, -0.48729641533290935, 0.97459283066581859, -0.48729641533290929, 1.6873551935899719e-17, 0.050059280167850009, 0.010000806681109737, -0.12770543788413571, 0.21393414582231227, 0.1568991096333438, -0.12050409251208098, -0.036557035976426684, -1.0972074690967477, -0.01444727776027852, -0.64702871259334449, 0.17550310601489358, 0.90762433294316336, 0.14017481565796389, 0.0052476440775108182, -0.23339033298135339, -0.00061821588820589444, 0.054877110564186989, 0.17414673138839104)
+
+# Work nicely, but q0 is uggly.
+initialConfig[robotName]=(0.00049921279479388854, 0.00049996060172474786, 0.63500107965860886, 8.1671945780155143e-23, 1.5720102693719503e-23, -4.8579693842636446e-23, 9.5431180449191677e-21, -0.00094329030861069489, -0.48680514099116468, 0.97549398127625753, -0.48868884028509291, 0.00094329030861069489, 6.989934206013467e-21, -0.00094334907227440175, -0.48703981005325936, 0.97596355410326363, -0.48892374405000433, 0.00094334907227440175, 0.11348807526440016, 0.20083116658525665, -0.0010545697535446316, -0.029186560447927903, 0.32171157730517314, -0.14204521593028116, -0.10741283049826153, -0.68485604962994939, -0.044874689963830129, -0.078235128020448866, 0.17272683925193663, 0.94207249294658169, 0.23722916347594097, 0.02069536375553379, -0.42067142923482798, -0.00032889180672932412, 0.019378691610578484, 0.17645563918444634)
+
+initialConfig[robotName]=(0.00049999999992054859, 0.00049999999999627166, 0.6350000000004179, -1.3576245230323505e-16, -1.2271661091679395e-14, -7.0755021448112835e-21, 1.1590588873416236e-17, -0.00094336656172265174, -0.48680748131139595, 0.97550163614052687, -0.48869415482911871, 0.00094336656172278759, 1.159528247642137e-17, -0.0009434253350073629, -0.48704216746084605, 0.97597124353230558, -0.48892907607144726, 0.00094342533500749864, 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)
+
+
+robot.set( initialConfig[robotName] )
+addRobotViewer(robot,small=True,verbose=True)
+
+#-------------------------------------------------------------------------------
+#----- MAIN LOOP ---------------------------------------------------------------
+#-------------------------------------------------------------------------------
+from dynamic_graph.sot.core.utils.thread_interruptible_loop import loopInThread,loopShortcuts
+@loopInThread
+def inc():
+    robot.increment(dt)
+    attime.run(robot.control.time)
+    updateComDisplay(robot,dyn.com)
+
+runner=inc()
+[go,stop,next,n]=loopShortcuts(runner)
+
+#-----------------------------------------------------------------------------
+#---- DYN --------------------------------------------------------------------
+#-----------------------------------------------------------------------------
+modelDir  = pkgDataRootDir[robotName]
+xmlDir    = pkgDataRootDir[robotName]
+specificitiesPath = xmlDir + '/HRP2SpecificitiesSmall.xml'
+jointRankPath     = xmlDir + '/HRP2LinkJointRankSmall.xml'
+
+dyn = Dynamic("dyn")
+dyn.setFiles(modelDir, modelName[robotName],specificitiesPath,jointRankPath)
+dyn.parse()
+
+dyn.inertiaRotor.value = inertiaRotor[robotName]
+dyn.gearRatio.value    = gearRatio[robotName]
+
+plug(robot.state,dyn.position)
+dyn.velocity.value = robotDim*(0.,)
+dyn.acceleration.value = robotDim*(0.,)
+
+dyn.ffposition.unplug()
+dyn.ffvelocity.unplug()
+dyn.ffacceleration.unplug()
+
+dyn.setProperty('ComputeBackwardDynamics','true')
+dyn.setProperty('ComputeAccelerationCoM','true')
+
+robot.control.unplug()
+
+# --- PG ---------------------------------------------------------
+# --- PG ---------------------------------------------------------
+# --- PG ---------------------------------------------------------
+from dynamic_graph.sot.pattern_generator.meta_pg import MetaPG
+pg = MetaPG(dyn)
+pg.plugZmp(robot)
+
+
+# ---- SOT ---------------------------------------------------------------------
+# ---- SOT ---------------------------------------------------------------------
+# ---- SOT ---------------------------------------------------------------------
+# The solver SOTH of dyninv is used, but normally, the SOT solver should be sufficient
+from dynamic_graph.sot.dyninv import SolverKine
+sot = SolverKine('sot')
+sot.setSize(robotDim)
+plug(sot.control,robot.control)
+
+# ---- TASKS -------------------------------------------------------------------
+# ---- TASKS -------------------------------------------------------------------
+# ---- TASKS -------------------------------------------------------------------
+
+
+# ---- TASK GRIP ---
+taskRH=MetaTaskKine6d('rh',dyn,'rh','right-wrist')
+handMgrip=eye(4); handMgrip[0:3,3] = (0,0,-0.17)
+taskRH.opmodif = matrixToTuple(handMgrip)
+
+# --- STATIC COM (if not walking)
+taskCom0 = MetaTaskKineCom(dyn)
+
+# --- POSTURE ---
+taskPosture = MetaTaskKinePosture(dyn)
+
+# --- GAZE ---
+taskGaze = MetaTaskKine6d('gaze',dyn,'head','gaze')
+# Head to camera matrix transform
+headMcam=array([[0.0,0.0,1.0,0.0825],[1.,0.0,0.0,-0.029],[0.0,1.,0.0,0.102],[0.0,0.0,0.0,1.0]])
+taskGaze.opmodif = matrixToTuple(headMcam)
+taskGaze.feature.frame('current')
+taskGaze.feature.selec.value = '011'
+
+# --- Task Joint Limits -----------------------------------------
+dyn.upperJl.recompute(0)
+dyn.lowerJl.recompute(0)
+taskJL = TaskJointLimits('taskJL')
+plug(dyn.position,taskJL.position)
+taskJL.controlGain.value = 10
+taskJL.referenceInf.value = dyn.lowerJl.value
+#taskJL.referenceInf.value = ( 0, 0, 0, 0, 0, 0, -0.785398163000, -0.610865238000, -2.181661565000, -0.034906585000, -1.308996939000, -0.349065850000, -0.523598775000, -0.349065850000, -2.181661565000, -0.034906585000, -1.308996939000, -0.610865238000,                          -0.785398163000, 0.01, -0.785398163000, -0.523598775000, -3.141592654000, -1.658062789000, -1.605702912000, -2.391101075000, -1.605702912000, -1.518400000000, 0, -3.141592654000, -0.174532925000, -1.605702912000, -2.391101075000, -1.605702912000, -1.605702912000, 0 )
+
+taskJL.referenceSup.value = dyn.upperJl.value
+taskJL.dt.value = dt
+taskJL.selec.value = toFlags(range(6,22)+range(22,28)+range(29,35))
+
+# ---- WAIST TASK ---
+taskWaist=MetaTask6d('waist',dyn,'waist','waist')
+pg.plugWaistTask(taskWaist)
+taskWaist.task.controlGain.value = 5
+
+# --- TASK COM ---
+taskCom = MetaTaskKineCom(dyn,"compd")
+plug(pg.comRef.ref,taskCom.featureDes.errorIN)
+plug(pg.pg.dcomref,taskCom.featureDes.errordotIN)
+taskCom.task = TaskPD('taskComPD')
+taskCom.task.add(taskCom.feature.name)
+plug(taskCom.feature.errordot,taskCom.task.errorDot)
+plug(taskCom.task.error,taskCom.gain.error)
+plug(taskCom.gain.gain,taskCom.task.controlGain)
+taskCom.gain.setConstant(40)
+taskCom.task.setBeta(-1)
+taskCom.feature.selec.value = '011'
+
+# --- TASK FEET
+taskRF=MetaTask6d('RF',dyn,'RF','right-ankle')
+plug(pg.pg.rightfootref,taskRF.featureDes.position)
+taskRF.task.controlGain.value = 5
+
+taskLF=MetaTask6d('LF',dyn,'LF','left-ankle')
+plug(pg.pg.leftfootref,taskLF.featureDes.position)
+taskLF.task.controlGain.value = 5
+
+# --- CONTACTS
+# define contactLF and contactRF
+for name,joint in [ ['LF','left-ankle'], ['RF','right-ankle' ] ]:
+    contact = MetaTaskKine6d('contact'+name,dyn,name,joint)
+    contact.feature.frame('desired')
+    contact.gain.setConstant(10)
+    locals()['contact'+name] = contact
+
+# --- TRACER -----------------------------------------------------------------
+from dynamic_graph.tracer import *
+tr = Tracer('tr')
+tr.open('/tmp/','','.dat')
+tr.start()
+robot.after.addSignal('tr.triger')
+
+tr.add(taskRF.featureDes.name+'.position','refr')
+tr.add(taskLF.featureDes.name+'.position','refl')
+tr.add('dyn.com','com')
+tr.add(taskRH.task.name+'.error','erh')
+
+tr.add(taskJL.name+".normalizedPosition","qn")
+tr.add("robot.state","q")
+tr.add("robot.control","qdot")
+robot.after.addSignal(taskJL.name+".normalizedPosition")
+
+# --- SHORTCUTS ----------------------------------------------------------------
+qn = taskJL.normalizedPosition
+@optionalparentheses
+def pqn(details=True):
+    ''' Display the normalized configuration vector. '''
+    qn.recompute(robot.state.time)
+    s = [ "{0:.1f}".format(v) for v in qn.value]
+    if details:
+        print("Rleg: "+" ".join(s[:6]))
+        print("Lleg: "+" ".join(s[6:12]))
+        print("Body: "+" ".join(s[12:16]))
+        print("Rarm: "+" ".join(s[16:22]))
+        print("Larm :"+" ".join(s[22:28]))
+    else:
+        print(" ".join(s[:30]))
+
+
+def jlbound(t=None):
+    '''Display the velocity bound induced by the JL as a double-column matrix.'''
+    if t==None: t=robot.state.time
+    taskJL.task.recompute(t)
+    return matrix([ [float(x),float(y)] for x,y
+                    in [ c.split(',') for c in taskJL.task.value[6:-3].split('),(') ] ])
+
+# --- RUN ----------------------------------------------------------------------
+# --- RUN ----------------------------------------------------------------------
+# --- RUN ----------------------------------------------------------------------
+
+#sot.addContact(contactRF)
+#sot.addContact(contactLF)
+#gotoNd(taskWaist,(0,0,0),'111011',(1,))
+#taskWaist.feature.frame('desired')
+#sot.push(taskWaist.task.name)
+#taskCom0.featureDes.errorIN.value = dyn.com.value
+#taskCom0.task.controlGain.value = 10
+#sot.push(taskCom0.task.name)
+
+# --- HERDT PG AND START -------------------------------------------------------
+# Set the algorithm generating the ZMP reference trajectory to Herdt's one.
+pg.startHerdt()
+# You can now modifiy the speed of the robot using set pg.pg.velocitydes [3]( x, y, yaw)
+pg.pg.velocitydes.value =(0.1,0.0,0.0)
+
+#ball0 = (2.5,-0.3,0.5)
+ball0 = (0.5,-2.3,0.5)
+w=(1/1.3,1/1.8,1/3.9)
+h=(0.1,0.07,0.3)
+
+sot.damping.value = 5e-2
+sot.push(taskJL.name)
+sot.push(taskWaist.task.name)
+sot.push(taskRF.task.name)
+sot.push(taskLF.task.name)
+sot.push(taskCom.task.name)
+taskPosture.gotoq((5,1,0.05,0.9),rhand=[1,])
+sot.push(taskPosture.task.name)
+gotoNd(taskGaze,ball0,'011',(10,1,0.01,0.9))
+sot.push(taskGaze.task.name)
+gotoNd(taskRH,ball0,'011',(10,1,0.01,0.9))
+sot.push(taskRH.task.name)
+
+# --- SCRIPT EVENTS ----------------------------------------------------------
+#from random import random
+import numpy.random as nprand
+
+
+class BallTraj:
+    def __init__(self,dt,ball0,v0=(0.0,0.0,0.0)):
+        self.dt = dt
+        self.pball=array(ball0)
+        self.vball=array(v0)
+        self.bounce = 0
+        self.z0 = 0.02
+        self.elasticZ = 0.99
+        self.elasticXY = 0.3
+        self.gravity=(0,0,-3.0)
+        nprand.seed(4)
+    def __call__(self,t):
+        self.vball += self.dt*array(self.gravity)
+        self.pball += self.dt*self.vball
+        if self.pball[2]<self.z0:
+            self.vball[2] =  self.elasticZ * abs(self.vball[2])
+            self.pball[2] = self.z0
+            self.vball[0:2] = (nprand.rand(2)-0.5)*self.elasticXY
+        return vectorToTuple(self.pball)
+ballTraj = BallTraj(dt,ball0)
+
+def trackTheBall(ball=None):
+    if ball==None:
+        t=robot.state.time/200.0
+        #ball = h * cos(array(w)*t) + ball0
+        ball = ballTraj(t)
+    if isinstance(ball,ndarray): ball=vectorToTuple(ball)
+    gotoNd(taskRH,ball)
+    gotoNd(taskGaze,ball)
+    robot.viewer.updateElementConfig('zmp',ball+(0,0,0))
+
+fixed=False
+if fixed:          trackTheBall()
+else:              attime(ALWAYS,trackTheBall)
+
+def ballInHand():
+    #rh = array(dyn.rh.value)[0:3,3]
+    robot.after.addSignal(taskRH.feature.name + '.position')
+    rh = array(taskRH.feature.position.value)[0:3,3]
+    robot.viewer.updateElementConfig('zmp',vectorToTuple(rh)+(0,0,0))
+
+#@attime(3450)
+def goForTheBall():
+    '''The hand goes for the hand.'''
+    taskRH.feature.selec.value = '111'
+    setGain(taskRH.gain,(5,0.5,0.03,0.9))
+    taskWaist.feature.selec.value = '011000'
+    ballTraj.elasticXY = 0.0
+    ballTraj.gravity = (0,0,-3.0)
+    attime(ALWAYS,graspIf)
+
+#@attime(3400) 
+def clearAttimePeriodic():
+    '''Stop the PG.'''
+    del attime.events[ALWAYS]
+    attime(ALWAYS,trackTheBall)
+    pg.pg.velocitydes.value = (0,0,0)
+
+#@attime(3900)
+def grasp():
+    '''Ball is in the hand.'''
+    del attime.events[ALWAYS]
+    attime(ALWAYS,ballInHand)
+    sot.rm(taskRH.task.name)
+    sot.rm(taskGaze.task.name)
+    q0 = halfSittingConfig[robotName]
+    taskPosture.gotoq((5,1,0.05,0.9),q0,rarm=[],larm=[],chest=[],head=[],rhand=[0,])
+    setGain(taskWaist.gain,(2,0.2,0.03,0.9))
+    taskWaist.feature.selec.value = '011100'
+
+
+def graspIf():
+    if linalg.norm(array(taskRH.task.error.value))<1e-2:
+        print grasp.__doc__
+        grasp()
+
+@optionalparentheses
+def now(t=None):
+    if t==None: t=robot.state.time
+    attime(t+1,clearAttimePeriodic)
+    attime(t+50,goForTheBall)
+    #attime(t+500,grasp)
+
+now(3250)
+
+# ------------------------------------------------------------------------------
+from math import atan2,pi
+def potFieldPG(target,pos,gain,obstacles):
+    ''' Compute the velocity of the PG as the gradient of a potential field in
+    the 2D plan.'''
+    
+    e = target-pos
+    waRwo = (array(dyn.waist.value)[0:2,0:2]).T
+    wo_vref = gain*e
+    wa_vref = dot(waRwo , wo_vref)
+    vmax = 0.2
+    vmax = array([0.3,0.1])
+    if abs(wa_vref[0])>vmax[0]:  wa_vref *= vmax[0]/abs(wa_vref[0])
+    if abs(wa_vref[1])>vmax[1]:  wa_vref *= vmax[1]/abs(wa_vref[1])
+    return wa_vref
+
+def pgv():
+    t = robot.state.time
+    if t%50!=1: return
+    ball = array(taskRH.ref)[0:2,3]
+    v = potFieldPG( ball, array(dyn.com.value)[0:2],array((1,1)), [])
+    dth = atan2(v[1],v[0])
+    vref = (v[0],v[1], 0.2*dth )
+    pg.pg.velocitydes.value = vref
+
+attime(ALWAYS,pgv)
+
+#next()
+go()
+
+
+
diff --git a/python/2013_coursens/p124_sing.py b/python/2013_coursens/p124_sing.py
new file mode 100644
index 0000000000000000000000000000000000000000..52a21a64765791aa7041e603a56a55c347b6dd60
--- /dev/null
+++ b/python/2013_coursens/p124_sing.py
@@ -0,0 +1,302 @@
+# ______________________________________________________________________________
+# ******************************************************************************
+# ______________________________________________________________________________
+# ******************************************************************************
+import sys
+execfile('/home/nmansard/.pythonrc')
+
+sys.path.append('..')
+from dynamic_graph import plug
+from dynamic_graph.sot.core import *
+from dynamic_graph.sot.core.math_small_entities import Derivator_of_Matrix
+from dynamic_graph.sot.dynamics import *
+from dynamic_graph.sot.dyninv import *
+import dynamic_graph.script_shortcuts
+from dynamic_graph.script_shortcuts import optionalparentheses
+from dynamic_graph.matlab import matlab
+from dynamic_graph.sot.core.matrix_util import matrixToTuple, vectorToTuple,rotate, matrixToRPY
+from dynamic_graph.sot.core.meta_task_6d import MetaTask6d,toFlags
+from dynamic_graph.sot.core.meta_tasks import setGain
+from dynamic_graph.sot.core.meta_tasks_kine import *
+from dynamic_graph.sot.core.meta_task_posture import MetaTaskKinePosture
+from dynamic_graph.sot.core.meta_task_visual_point import MetaTaskVisualPoint
+from dynamic_graph.sot.core.utils.viewer_helper import addRobotViewer,VisualPinger,updateComDisplay
+from dynamic_graph.sot.core.utils.attime import attime,ALWAYS,refset,sigset
+from numpy import *
+
+from dynamic_graph.sot.core.utils.history import History
+
+from robotSpecific import pkgDataRootDir,modelName,robotDimension,initialConfig,gearRatio,inertiaRotor
+
+# --- ROBOT SIMU ---------------------------------------------------------------
+# --- ROBOT SIMU ---------------------------------------------------------------
+# --- ROBOT SIMU ---------------------------------------------------------------
+
+robotName = 'hrp14small'
+robotDim=robotDimension[robotName]
+robot = RobotSimu("robot")
+robot.resize(robotDim)
+dt=5e-3
+
+from robotSpecific import halfSittingConfig
+x0=-0.00949035111398315034
+y0=0
+z0=0.64870185118253043
+halfSittingConfig[robotName] = (x0,y0,z0,0,0,0)+halfSittingConfig[robotName][6:]
+
+q0=list(halfSittingConfig[robotName])
+initialConfig[robotName]=tuple(q0)
+
+robot.set( initialConfig[robotName] )
+addRobotViewer(robot,small=True,verbose=True)
+
+#-------------------------------------------------------------------------------
+#----- MAIN LOOP ---------------------------------------------------------------
+#-------------------------------------------------------------------------------
+from dynamic_graph.sot.core.utils.thread_interruptible_loop import loopInThread,loopShortcuts
+@loopInThread
+def inc():
+    robot.increment(dt)
+    attime.run(robot.control.time)
+    updateComDisplay(robot,dyn.com)
+    history.record()
+
+runner=inc()
+[go,stop,next,n]=loopShortcuts(runner)
+
+#-----------------------------------------------------------------------------
+#---- DYN --------------------------------------------------------------------
+#-----------------------------------------------------------------------------
+modelDir  = pkgDataRootDir[robotName]
+xmlDir    = pkgDataRootDir[robotName]
+specificitiesPath = xmlDir + '/HRP2SpecificitiesSmall.xml'
+jointRankPath     = xmlDir + '/HRP2LinkJointRankSmall.xml'
+
+dyn = Dynamic("dyn")
+dyn.setFiles(modelDir, modelName[robotName],specificitiesPath,jointRankPath)
+dyn.parse()
+
+dyn.inertiaRotor.value = inertiaRotor[robotName]
+dyn.gearRatio.value    = gearRatio[robotName]
+
+plug(robot.state,dyn.position)
+dyn.velocity.value = robotDim*(0.,)
+dyn.acceleration.value = robotDim*(0.,)
+
+dyn.ffposition.unplug()
+dyn.ffvelocity.unplug()
+dyn.ffacceleration.unplug()
+
+dyn.setProperty('ComputeBackwardDynamics','true')
+dyn.setProperty('ComputeAccelerationCoM','true')
+
+robot.control.unplug()
+
+
+# ---- SOT ---------------------------------------------------------------------
+# ---- SOT ---------------------------------------------------------------------
+# ---- SOT ---------------------------------------------------------------------
+# The solver SOTH of dyninv is used, but normally, the SOT solver should be sufficient
+from dynamic_graph.sot.dyninv import SolverKine
+def toList(sot):
+    return map(lambda x: x[1:-1],sot.dispStack().split('|')[1:])
+SolverKine.toList = toList
+sot = SolverKine('sot')
+sot.setSize(robotDim)
+plug(sot.control,robot.control)
+
+# ---- TASKS -------------------------------------------------------------------
+# ---- TASKS -------------------------------------------------------------------
+# ---- TASKS -------------------------------------------------------------------
+
+# ---- TASK GRIP ---
+taskRH=MetaTaskKine6d('rh',dyn,'rh','right-wrist')
+handMgrip=eye(4); handMgrip[0:3,3] = (0,0,-0.21)
+taskRH.opmodif = matrixToTuple(handMgrip)
+taskRH.feature.frame('desired')
+
+taskLH=MetaTaskKine6d('lh',dyn,'lh','left-wrist')
+taskLH.opmodif = matrixToTuple(handMgrip)
+taskLH.feature.frame('desired')
+
+# --- STATIC COM (if not walking)
+taskCom = MetaTaskKineCom(dyn)
+
+# --- TASK AVOID
+taskShoulder=MetaTaskKine6d('shoulder',dyn,'shoulder','LARM_JOINT0')
+taskShoulder.feature.frame('desired')
+gotoNd(taskShoulder,(0,0,0),'010')
+taskShoulder.task = TaskInequality('taskShoulderAvoid')
+taskShoulder.task.add(taskShoulder.feature.name)
+taskShoulder.task.referenceInf.value = (-10,)    # Xmin, Ymin
+taskShoulder.task.referenceSup.value = (0.20,)    # Xmin, Ymin
+taskShoulder.task.dt.value=dt
+taskShoulder.task.controlGain.value = 0.9
+
+taskElbow=MetaTaskKine6d('elbow',dyn,'elbow','LARM_JOINT3')
+taskElbow.feature.frame('desired')
+gotoNd(taskElbow,(0,0,0),'010')
+taskElbow.task = TaskInequality('taskElbowAvoid')
+taskElbow.task.add(taskElbow.feature.name)
+taskElbow.task.referenceInf.value = (-10,)    # Xmin, Ymin
+taskElbow.task.referenceSup.value = (0.20,)    # Xmin, Ymin
+taskElbow.task.dt.value=dt
+taskElbow.task.controlGain.value = 0.9
+
+# --- TASK SUPPORT --------------------------------------------------
+featureSupport    = FeatureGeneric('featureSupport')
+plug(dyn.com,featureSupport.errorIN)
+plug(dyn.Jcom,featureSupport.jacobianIN)
+
+taskSupport=TaskInequality('taskSupport')
+taskSupport.add(featureSupport.name)
+taskSupport.selec.value = '011'
+taskSupport.referenceInf.value = (-0.08,-0.15,0)    # Xmin, Ymin
+taskSupport.referenceSup.value = (0.11,0.15,0)  # Xmax, Ymax
+taskSupport.dt.value=dt
+
+# --- POSTURE ---
+taskPosture = MetaTaskKinePosture(dyn)
+
+# --- GAZE ---
+taskGaze = MetaTaskVisualPoint('gaze',dyn,'head','gaze')
+# Head to camera matrix transform
+# Camera RU headMcam=array([[0.0,0.0,1.0,0.0825],[1.,0.0,0.0,-0.029],[0.0,1.,0.0,0.102],[0.0,0.0,0.0,1.0]])
+# Camera LL 
+headMcam=array([[0.0,0.0,1.0,0.081],[1.,0.0,0.0,0.072],[0.0,1.,0.0,0.031],[0.0,0.0,0.0,1.0]])
+headMcam = dot(headMcam,rotate('x',10*pi/180))
+taskGaze.opmodif = matrixToTuple(headMcam)
+
+# --- CONTACTS
+# define contactLF and contactRF
+for name,joint in [ ['LF','left-ankle'], ['RF','right-ankle' ] ]:
+    contact = MetaTaskKine6d('contact'+name,dyn,name,joint)
+    contact.feature.frame('desired')
+    contact.gain.setConstant(10)
+    locals()['contact'+name] = contact
+
+# --- TRACER -----------------------------------------------------------------
+from dynamic_graph.tracer import *
+tr = Tracer('tr')
+tr.open('/tmp/','','.dat')
+tr.start()
+robot.after.addSignal('tr.triger')
+
+tr.add('dyn.com','com')
+
+history = History(dyn,1)
+
+# --- SHORTCUTS ----------------------------------------------------------------
+def p6d(R,t):
+    M=eye(4)
+    M[0:3,0:3]=R
+    M[0:3,3]=t
+    return M
+
+def push(task):
+    if isinstance(task,str): taskName=task
+    elif "task" in task.__dict__:  taskName=task.task.name
+    else: taskName=task.name
+    if taskName not in sot.toList():
+        sot.push(taskName)
+        if taskName!="taskposture" and "taskposture" in sot.toList():
+            sot.down("taskposture")
+
+
+def pop(task):
+    if isinstance(task,str): taskName=task
+    elif "task" in task.__dict__:  taskName=task.task.name
+    else: taskName=task.name
+    if taskName in sot.toList(): sot.rm(taskName)
+
+
+# --- DISPLAY ------------------------------------------------------------------
+# --- DISPLAY ------------------------------------------------------------------
+# --- DISPLAY ------------------------------------------------------------------
+RAD=pi/180
+comproj = [0.1,-0.95,1.6]
+#robot.viewer.updateElementConfig('footproj',[0.5,0.15,1.6+0.08,0,-pi/2,0 ])
+robot.viewer.updateElementConfig('footproj',comproj+[0,-pi/2,0 ])
+robot.viewer.updateElementConfig('zmp2',[0,0,-10,0,0,0])
+
+class BallPosition:
+    def __init__(self,xyz=(0,-1.1,0.9)):
+        self.ball = xyz
+        self.prec = 0
+        self.t = 0
+        self.duration = 0
+        self.f = 0
+        self.xyz= self.ball
+        
+    def move(self,xyz,duration=50):
+        self.prec = self.ball
+        self.ball = xyz
+        self.t = 0
+        self.duration = duration
+        self.changeTargets()
+
+        if duration>0:
+            self.f = lambda : self.moveDisplay()
+            attime(ALWAYS,self.f)
+        else:
+            self.moveDisplay()
+
+    def moveDisplay(self):
+        tau = 1.0 if self.duration<=0 else float(self.t) / self.duration
+        xyz = tau * array(self.ball) + (1-tau) * array(self.prec)
+        robot.viewer.updateElementConfig('zmp',vectorToTuple(xyz)+(0,0,0))
+
+        self.t += 1
+        if self.t>self.duration and self.duration>0:
+            attime.stop(self.f)
+        self.xyz= xyz
+        
+    def changeTargets(self):
+        gotoNd(taskRH,self.ball,'111',(4.9,0.9,0.01,0.9))
+
+b = BallPosition()
+
+tr.add('dyn.com','com')
+tr.add(taskRH.task.name+'.error','erh')
+tr.add(taskSupport.name+".normalizedPosition",'comn')
+
+robot.after.addSignal(taskSupport.name+".normalizedPosition")
+
+# --- RUN ----------------------------------------------------------------------
+# --- RUN ----------------------------------------------------------------------
+# --- RUN ----------------------------------------------------------------------
+
+dyn.com.recompute(0)
+taskCom.featureDes.errorIN.value = dyn.com.value
+taskCom.task.controlGain.value = 10
+
+taskPosture.gotoq(1,halfSittingConfig[robotName],chest=[],head=[],rleg=[],lleg=[],rarm=[],larm=[])
+
+ball = BallPosition((0,-1.1,0.9))
+
+#sot.damping.value = 0.1
+
+sot.addContact(contactRF)
+sot.addContact(contactLF)
+push(taskRH)
+
+ball.move((1.2,-0.4,1),0)
+
+next()
+next()
+next()
+
+def stop570():
+    '''Singularity reached, COM disable, press go to continue.'''
+    stop()
+    next()
+    next()
+    next()
+    pop(taskCom)
+
+uinp = raw_input('   Enable COM regulation [Y/n]? >> ')
+if uinp!='n': 
+    push(taskCom)
+    attime(570,stop570)
+
+go()
diff --git a/python/2013_coursens/p96_sing.py b/python/2013_coursens/p96_sing.py
new file mode 100644
index 0000000000000000000000000000000000000000..e7211e89ba54650bbb8e1a409992899fe4bb2cdc
--- /dev/null
+++ b/python/2013_coursens/p96_sing.py
@@ -0,0 +1,318 @@
+# ______________________________________________________________________________
+# ******************************************************************************
+# ______________________________________________________________________________
+# ******************************************************************************
+import sys
+execfile('/home/nmansard/.pythonrc')
+
+sys.path.append('..')
+from dynamic_graph import plug
+from dynamic_graph.sot.core import *
+from dynamic_graph.sot.core.math_small_entities import Derivator_of_Matrix
+from dynamic_graph.sot.dynamics import *
+from dynamic_graph.sot.dyninv import *
+import dynamic_graph.script_shortcuts
+from dynamic_graph.script_shortcuts import optionalparentheses
+from dynamic_graph.matlab import matlab
+from dynamic_graph.sot.core.matrix_util import matrixToTuple, vectorToTuple,rotate, matrixToRPY
+from dynamic_graph.sot.core.meta_task_6d import MetaTask6d,toFlags
+from dynamic_graph.sot.core.meta_tasks import setGain
+from dynamic_graph.sot.core.meta_tasks_kine import *
+from dynamic_graph.sot.core.meta_task_posture import MetaTaskKinePosture
+from dynamic_graph.sot.core.meta_task_visual_point import MetaTaskVisualPoint
+from dynamic_graph.sot.core.utils.viewer_helper import addRobotViewer,VisualPinger,updateComDisplay
+from dynamic_graph.sot.core.utils.attime import attime,ALWAYS,refset,sigset
+from numpy import *
+
+from dynamic_graph.sot.core.utils.history import History
+
+from robotSpecific import pkgDataRootDir,modelName,robotDimension,initialConfig,gearRatio,inertiaRotor
+
+# --- ROBOT SIMU ---------------------------------------------------------------
+# --- ROBOT SIMU ---------------------------------------------------------------
+# --- ROBOT SIMU ---------------------------------------------------------------
+
+robotName = 'hrp14small'
+robotDim=robotDimension[robotName]
+robot = RobotSimu("robot")
+robot.resize(robotDim)
+dt=5e-3
+
+from robotSpecific import halfSittingConfig
+x0=-0.00949035111398315034
+y0=0
+z0=0.64870185118253043
+halfSittingConfig[robotName] = (x0,y0,z0,0,0,0)+halfSittingConfig[robotName][6:]
+
+q0=list(halfSittingConfig[robotName])
+initialConfig[robotName]=tuple(q0)
+
+robot.set( initialConfig[robotName] )
+addRobotViewer(robot,small=True,verbose=True)
+
+#-------------------------------------------------------------------------------
+#----- MAIN LOOP ---------------------------------------------------------------
+#-------------------------------------------------------------------------------
+from dynamic_graph.sot.core.utils.thread_interruptible_loop import loopInThread,loopShortcuts
+@loopInThread
+def inc():
+    robot.increment(dt)
+    attime.run(robot.control.time)
+    updateComDisplay(robot,dyn.com)
+    history.record()
+
+runner=inc()
+[go,stop,next,n]=loopShortcuts(runner)
+
+#-----------------------------------------------------------------------------
+#---- DYN --------------------------------------------------------------------
+#-----------------------------------------------------------------------------
+modelDir  = pkgDataRootDir[robotName]
+xmlDir    = pkgDataRootDir[robotName]
+specificitiesPath = xmlDir + '/HRP2SpecificitiesSmall.xml'
+jointRankPath     = xmlDir + '/HRP2LinkJointRankSmall.xml'
+
+dyn = Dynamic("dyn")
+dyn.setFiles(modelDir, modelName[robotName],specificitiesPath,jointRankPath)
+dyn.parse()
+
+dyn.inertiaRotor.value = inertiaRotor[robotName]
+dyn.gearRatio.value    = gearRatio[robotName]
+
+plug(robot.state,dyn.position)
+dyn.velocity.value = robotDim*(0.,)
+dyn.acceleration.value = robotDim*(0.,)
+
+dyn.ffposition.unplug()
+dyn.ffvelocity.unplug()
+dyn.ffacceleration.unplug()
+
+dyn.setProperty('ComputeBackwardDynamics','true')
+dyn.setProperty('ComputeAccelerationCoM','true')
+
+robot.control.unplug()
+
+
+# ---- SOT ---------------------------------------------------------------------
+# ---- SOT ---------------------------------------------------------------------
+# ---- SOT ---------------------------------------------------------------------
+# The solver SOTH of dyninv is used, but normally, the SOT solver should be sufficient
+from dynamic_graph.sot.dyninv import SolverKine
+def toList(sot):
+    return map(lambda x: x[1:-1],sot.dispStack().split('|')[1:])
+SolverKine.toList = toList
+sot = SolverKine('sot')
+sot.setSize(robotDim)
+plug(sot.control,robot.control)
+
+# ---- TASKS -------------------------------------------------------------------
+# ---- TASKS -------------------------------------------------------------------
+# ---- TASKS -------------------------------------------------------------------
+
+# ---- TASK GRIP ---
+taskRH=MetaTaskKine6d('rh',dyn,'rh','right-wrist')
+handMgrip=eye(4); handMgrip[0:3,3] = (0,0,-0.21)
+taskRH.opmodif = matrixToTuple(handMgrip)
+taskRH.feature.frame('desired')
+
+taskLH=MetaTaskKine6d('lh',dyn,'lh','left-wrist')
+taskLH.opmodif = matrixToTuple(handMgrip)
+taskLH.feature.frame('desired')
+
+# --- STATIC COM (if not walking)
+taskCom = MetaTaskKineCom(dyn)
+
+# --- TASK AVOID
+taskShoulder=MetaTaskKine6d('shoulder',dyn,'shoulder','LARM_JOINT0')
+taskShoulder.feature.frame('desired')
+gotoNd(taskShoulder,(0,0,0),'010')
+taskShoulder.task = TaskInequality('taskShoulderAvoid')
+taskShoulder.task.add(taskShoulder.feature.name)
+taskShoulder.task.referenceInf.value = (-10,)    # Xmin, Ymin
+taskShoulder.task.referenceSup.value = (0.20,)    # Xmin, Ymin
+taskShoulder.task.dt.value=dt
+taskShoulder.task.controlGain.value = 0.9
+
+taskElbow=MetaTaskKine6d('elbow',dyn,'elbow','LARM_JOINT3')
+taskElbow.feature.frame('desired')
+gotoNd(taskElbow,(0,0,0),'010')
+taskElbow.task = TaskInequality('taskElbowAvoid')
+taskElbow.task.add(taskElbow.feature.name)
+taskElbow.task.referenceInf.value = (-10,)    # Xmin, Ymin
+taskElbow.task.referenceSup.value = (0.20,)    # Xmin, Ymin
+taskElbow.task.dt.value=dt
+taskElbow.task.controlGain.value = 0.9
+
+# --- TASK SUPPORT --------------------------------------------------
+featureSupport    = FeatureGeneric('featureSupport')
+plug(dyn.com,featureSupport.errorIN)
+plug(dyn.Jcom,featureSupport.jacobianIN)
+
+taskSupport=TaskInequality('taskSupport')
+taskSupport.add(featureSupport.name)
+taskSupport.selec.value = '011'
+taskSupport.referenceInf.value = (-0.08,-0.15,0)    # Xmin, Ymin
+taskSupport.referenceSup.value = (0.11,0.15,0)  # Xmax, Ymax
+taskSupport.dt.value=dt
+
+# --- POSTURE ---
+taskPosture = MetaTaskKinePosture(dyn)
+
+# --- CONTACTS
+# define contactLF and contactRF
+for name,joint in [ ['LF','left-ankle'], ['RF','right-ankle' ] ]:
+    contact = MetaTaskKine6d('contact'+name,dyn,name,joint)
+    contact.feature.frame('desired')
+    contact.gain.setConstant(10)
+    locals()['contact'+name] = contact
+
+# --- TRACER -----------------------------------------------------------------
+from dynamic_graph.tracer import *
+tr = Tracer('tr')
+tr.open('/tmp/','','.dat')
+tr.start()
+robot.after.addSignal('tr.triger')
+
+tr.add('dyn.com','com')
+
+history = History(dyn,1)
+
+# --- SHORTCUTS ----------------------------------------------------------------
+def p6d(R,t):
+    M=eye(4)
+    M[0:3,0:3]=R
+    M[0:3,3]=t
+    return M
+
+def push(task):
+    if isinstance(task,str): taskName=task
+    elif "task" in task.__dict__:  taskName=task.task.name
+    else: taskName=task.name
+    if taskName not in sot.toList():
+        sot.push(taskName)
+        if taskName!="taskposture" and "taskposture" in sot.toList():
+            sot.down("taskposture")
+
+
+def pop(task):
+    if isinstance(task,str): taskName=task
+    elif "task" in task.__dict__:  taskName=task.task.name
+    else: taskName=task.name
+    if taskName in sot.toList(): sot.rm(taskName)
+
+
+# --- DISPLAY ------------------------------------------------------------------
+# --- DISPLAY ------------------------------------------------------------------
+# --- DISPLAY ------------------------------------------------------------------
+RAD=pi/180
+comproj = [0.1,-0.95,1.6]
+#robot.viewer.updateElementConfig('footproj',[0.5,0.15,1.6+0.08,0,-pi/2,0 ])
+robot.viewer.updateElementConfig('footproj',comproj+[0,-pi/2,0 ])
+robot.viewer.updateElementConfig('zmp2',[0,0,-10,0,0,0])
+
+class BallPosition:
+    def __init__(self,xyz=(0,-1.1,0.9)):
+        self.ball = xyz
+        self.prec = 0
+        self.t = 0
+        self.duration = 0
+        self.f = 0
+        self.xyz= self.ball
+        
+    def move(self,xyz,duration=50):
+        self.prec = self.ball
+        self.ball = xyz
+        self.t = 0
+        self.duration = duration
+        self.changeTargets()
+
+        if duration>0:
+            self.f = lambda : self.moveDisplay()
+            attime(ALWAYS,self.f)
+        else:
+            self.moveDisplay()
+
+    def moveDisplay(self):
+        tau = 1.0 if self.duration<=0 else float(self.t) / self.duration
+        xyz = tau * array(self.ball) + (1-tau) * array(self.prec)
+        robot.viewer.updateElementConfig('zmp',vectorToTuple(xyz)+(0,0,0))
+
+        self.t += 1
+        if self.t>self.duration and self.duration>0:
+            attime.stop(self.f)
+        self.xyz= xyz
+        
+    def changeTargets(self):
+        gotoNd(taskRH,self.ball,'111',(4.9,0.9,0.01,0.9))
+
+b = BallPosition()
+
+tr.add('dyn.com','com')
+tr.add(taskRH.task.name+'.error','erh')
+tr.add(taskSupport.name+".normalizedPosition",'comn')
+
+robot.after.addSignal(taskSupport.name+".normalizedPosition")
+
+# --- RUN ----------------------------------------------------------------------
+# --- RUN ----------------------------------------------------------------------
+# --- RUN ----------------------------------------------------------------------
+
+dyn.com.recompute(0)
+taskCom.featureDes.errorIN.value = dyn.com.value
+taskCom.task.controlGain.value = 10
+
+taskPosture.gotoq(1,halfSittingConfig[robotName],chest=[],head=[],rleg=[],lleg=[],rarm=[],larm=[])
+
+ball = BallPosition((0,-1.1,0.9))
+
+sot.damping.value = 0.1
+
+sot.addContact(contactRF)
+sot.addContact(contactLF)
+push(taskCom)
+push(taskRH)
+push(taskPosture)
+
+ball.move((0.5,-0.4,1.2),0)
+
+next()
+next()
+next()
+
+q0=(-0.013448627761452581, -0.0024679718155040885, 0.63088361991552966, -0.018777584993745728, -0.060941237730978988, 0.037292111609228754, -0.037510492296196345, 0.021074469596564682, -0.45832797319827417, 0.99704625676400405, -0.47711515439441948, -0.0045892937844194013, -0.0375232834669115, 0.021281798238735231, -0.47292880929139702, 1.00958227703776, -0.47505006770217456, -0.0047970164517906902, 0.072094769948381279, 0.011059478621976536, -0.00011090786283310123, 0.0010171428665263665, -0.52826992474554724, -0.45608428199514117, 0.042817559290084496, -1.2032584278571674, 0.18143720449535519, -0.89040683552343491, -0.029527723367927538, 0.24934327906079795, 0.15262968598049148, -0.0042541976395581915, -0.52055386436392925, 4.8770209960872496e-05, 0.00060030850701126319, 0.57928762243102871)
+
+ball.move((0.5,-0.2,1.2),0)
+taskPosture.gotoq(1,tuple(q0),chest=[],head=[],rleg=[],lleg=[],rarm=[],larm=[])
+
+class Swing:
+    def __init__(self):
+        self.w=1.0/200.0*5.0
+        self.th=30*RAD
+        self.first=False
+        self.Kdot = 0.1/200/0.2
+
+    def __call__(self):
+        t=robot.control.time
+        q=list(robot.state.value)
+        if not self.first:
+            self.t0=t
+            self.q0=q
+            self.th=self.q0[23]
+            self.first=True
+        q[23] = self.th*cos(self.w*(t-self.t0))
+        taskPosture.gotoq(10,tuple(q),chest=[],head=[],rleg=[],lleg=[],rarm=[],larm=[])
+        
+        self.th+=self.Kdot*self.th
+
+
+    def start(self):
+        robot.displayList.append(swing)
+
+swing=Swing()
+
+@optionalparentheses
+def n():swing.start()
+print '\n\n\nPress n to relax the elbow.'''
+
+go()
+
diff --git a/python/2013_coursens/xp2_kineana.py b/python/2013_coursens/xp2_kineana.py
new file mode 100644
index 0000000000000000000000000000000000000000..a3462847cd72f44f48c200e041c36e6cc15cc37f
--- /dev/null
+++ b/python/2013_coursens/xp2_kineana.py
@@ -0,0 +1,491 @@
+# ______________________________________________________________________________
+# ******************************************************************************
+# ______________________________________________________________________________
+# ******************************************************************************
+import sys
+execfile('/home/nmansard/.pythonrc')
+
+sys.path.append('..')
+from dynamic_graph import plug
+from dynamic_graph.sot.core import *
+from dynamic_graph.sot.core.math_small_entities import Derivator_of_Matrix
+from dynamic_graph.sot.dynamics import *
+from dynamic_graph.sot.dyninv import *
+import dynamic_graph.script_shortcuts
+from dynamic_graph.script_shortcuts import optionalparentheses
+from dynamic_graph.matlab import matlab
+from dynamic_graph.sot.core.matrix_util import matrixToTuple, vectorToTuple,rotate, matrixToRPY
+from dynamic_graph.sot.core.meta_task_6d import MetaTask6d,toFlags
+from dynamic_graph.sot.core.meta_tasks import setGain
+from dynamic_graph.sot.core.meta_tasks_kine import *
+from dynamic_graph.sot.core.meta_task_posture import MetaTaskKinePosture
+from dynamic_graph.sot.core.meta_task_visual_point import MetaTaskVisualPoint
+from dynamic_graph.sot.core.utils.viewer_helper import addRobotViewer,VisualPinger,updateComDisplay
+from dynamic_graph.sot.core.utils.attime import attime,ALWAYS,refset,sigset
+from numpy import *
+
+from history import History
+
+from robotSpecific import pkgDataRootDir,modelName,robotDimension,initialConfig,gearRatio,inertiaRotor
+
+# --- ROBOT SIMU ---------------------------------------------------------------
+# --- ROBOT SIMU ---------------------------------------------------------------
+# --- ROBOT SIMU ---------------------------------------------------------------
+
+robotName = 'hrp14small'
+robotDim=robotDimension[robotName]
+robot = RobotSimu("robot")
+robot.resize(robotDim)
+dt=5e-3
+
+from robotSpecific import halfSittingConfig
+x0=-0.00949035111398315034
+y0=0
+z0=0.64870185118253043  #0.6487018512
+halfSittingConfig[robotName] = (x0,y0,z0,0,0,0)+halfSittingConfig[robotName][6:]
+
+q0=list(halfSittingConfig[robotName])
+initialConfig[robotName]=tuple(q0)
+
+q0[20]=-1
+
+# Object is centered
+#q0 = (-0.0270855404604077, 0.0058984313447739307, 0.64596879258040218, 0.051646911457782906, -0.01849697876637613, -0.26694316842988736, 0.26764629500934506, -0.059954849690319403, -0.49830573060840277, 0.90083178777804507, -0.37102964759029244, 0.01502407314938717, 0.26738768265871021, -0.051757611229943074, -0.42500685369248403, 0.87129911754613565, -0.41481029199661462, 0.0068227689907173691, -0.57713162476592372, 0.14306500463299326, -0.78539800046042108, 0.23559202790602107, 0.25461821795561479, -0.1618842307250519, 0.0026405447564797208, -0.5275700723685236, 2.0836791732190813e-05, -0.00077505993604308769, 0.21347640028265377, 0.25464098548316366, 0.18983684206548987, 0.0028852330073878755, -0.52655196479787658, -1.5442864197634829e-05, -0.00056592322550752386, 0.13166002981175504)
+
+#initialConfig[robotName]=tuple(q0)
+
+
+robot.set( initialConfig[robotName] )
+addRobotViewer(robot,small=True,verbose=True)
+
+#-------------------------------------------------------------------------------
+#----- MAIN LOOP ---------------------------------------------------------------
+#-------------------------------------------------------------------------------
+from dynamic_graph.sot.core.utils.thread_interruptible_loop import loopInThread,loopShortcuts
+@loopInThread
+def inc():
+    robot.increment(dt)
+    attime.run(robot.control.time)
+    updateComDisplay(robot,dyn.com)
+    history.record()
+
+runner=inc()
+[go,stop,next,n]=loopShortcuts(runner)
+
+#-----------------------------------------------------------------------------
+#---- DYN --------------------------------------------------------------------
+#-----------------------------------------------------------------------------
+modelDir  = pkgDataRootDir[robotName]
+xmlDir    = pkgDataRootDir[robotName]
+specificitiesPath = xmlDir + '/HRP2SpecificitiesSmall.xml'
+jointRankPath     = xmlDir + '/HRP2LinkJointRankSmall.xml'
+
+dyn = Dynamic("dyn")
+dyn.setFiles(modelDir, modelName[robotName],specificitiesPath,jointRankPath)
+dyn.parse()
+
+dyn.inertiaRotor.value = inertiaRotor[robotName]
+dyn.gearRatio.value    = gearRatio[robotName]
+
+plug(robot.state,dyn.position)
+dyn.velocity.value = robotDim*(0.,)
+dyn.acceleration.value = robotDim*(0.,)
+
+dyn.ffposition.unplug()
+dyn.ffvelocity.unplug()
+dyn.ffacceleration.unplug()
+
+dyn.setProperty('ComputeBackwardDynamics','true')
+dyn.setProperty('ComputeAccelerationCoM','true')
+
+robot.control.unplug()
+
+# --- PG ---------------------------------------------------------
+# --- PG ---------------------------------------------------------
+# --- PG ---------------------------------------------------------
+from dynamic_graph.sot.pattern_generator.meta_pg import MetaPG
+pg = MetaPG(dyn)
+pg.plugZmp(robot)
+
+
+# ---- SOT ---------------------------------------------------------------------
+# ---- SOT ---------------------------------------------------------------------
+# ---- SOT ---------------------------------------------------------------------
+# The solver SOTH of dyninv is used, but normally, the SOT solver should be sufficient
+from dynamic_graph.sot.dyninv import SolverKine
+def toList(sot):
+    return map(lambda x: x[1:-1],sot.dispStack().split('|')[1:])
+SolverKine.toList = toList
+sot = SolverKine('sot')
+sot.setSize(robotDim)
+plug(sot.control,robot.control)
+
+# ---- TASKS -------------------------------------------------------------------
+# ---- TASKS -------------------------------------------------------------------
+# ---- TASKS -------------------------------------------------------------------
+
+
+# ---- TASK GRIP ---
+taskRH=MetaTaskKine6d('rh',dyn,'rh','right-wrist')
+handMgrip=eye(4); handMgrip[0:3,3] = (0,0,-0.21)
+taskRH.opmodif = matrixToTuple(handMgrip)
+taskRH.feature.frame('desired')
+
+taskLH=MetaTaskKine6d('lh',dyn,'lh','left-wrist')
+taskLH.opmodif = matrixToTuple(handMgrip)
+taskLH.feature.frame('desired')
+
+# --- STATIC COM (if not walking)
+taskCom = MetaTaskKineCom(dyn)
+
+# --- TASK AVOID
+taskShoulder=MetaTaskKine6d('shoulder',dyn,'shoulder','LARM_JOINT0')
+taskShoulder.feature.frame('desired')
+gotoNd(taskShoulder,(0,0,0),'010')
+taskShoulder.task = TaskInequality('taskShoulderAvoid')
+taskShoulder.task.add(taskShoulder.feature.name)
+taskShoulder.task.referenceInf.value = (-10,)    # Xmin, Ymin
+taskShoulder.task.referenceSup.value = (0.20,)    # Xmin, Ymin
+taskShoulder.task.dt.value=dt
+taskShoulder.task.controlGain.value = 0.9
+
+taskElbow=MetaTaskKine6d('elbow',dyn,'elbow','LARM_JOINT3')
+taskElbow.feature.frame('desired')
+gotoNd(taskElbow,(0,0,0),'010')
+taskElbow.task = TaskInequality('taskElbowAvoid')
+taskElbow.task.add(taskElbow.feature.name)
+taskElbow.task.referenceInf.value = (-10,)    # Xmin, Ymin
+taskElbow.task.referenceSup.value = (0.20,)    # Xmin, Ymin
+taskElbow.task.dt.value=dt
+taskElbow.task.controlGain.value = 0.9
+
+
+# --- TASK SUPPORT --------------------------------------------------
+featureSupport    = FeatureGeneric('featureSupport')
+plug(dyn.com,featureSupport.errorIN)
+plug(dyn.Jcom,featureSupport.jacobianIN)
+
+taskSupport=TaskInequality('taskSupport')
+taskSupport.add(featureSupport.name)
+taskSupport.selec.value = '011'
+taskSupport.referenceInf.value = (-0.08,-0.15,0)    # Xmin, Ymin
+taskSupport.referenceSup.value = (0.11,0.15,0)  # Xmax, Ymax
+taskSupport.dt.value=dt
+
+# --- TASK SUPPORT SMALL --------------------------------------------
+featureSupportSmall = FeatureGeneric('featureSupportSmall')
+plug(dyn.com,featureSupportSmall.errorIN)
+plug(dyn.Jcom,featureSupportSmall.jacobianIN)
+
+taskSupportSmall=TaskInequality('taskSupportSmall')
+taskSupportSmall.add(featureSupportSmall.name)
+taskSupportSmall.selec.value = '011'
+#taskSupportSmall.referenceInf.value = (-0.02,-0.02,0)    # Xmin, Ymin
+#askSupportSmall.referenceSup.value = (0.02,0.02,0)  # Xmax, Ymax
+taskSupportSmall.referenceInf.value = (-0.02,-0.05,0)    # Xmin, Ymin
+taskSupportSmall.referenceSup.value = (0.02,0.05,0)  # Xmax, Ymax
+taskSupportSmall.dt.value=dt
+
+# --- POSTURE ---
+taskPosture = MetaTaskKinePosture(dyn)
+
+# --- GAZE ---
+taskGaze = MetaTaskVisualPoint('gaze',dyn,'head','gaze')
+# Head to camera matrix transform
+# Camera RU headMcam=array([[0.0,0.0,1.0,0.0825],[1.,0.0,0.0,-0.029],[0.0,1.,0.0,0.102],[0.0,0.0,0.0,1.0]])
+# Camera LL 
+headMcam=array([[0.0,0.0,1.0,0.081],[1.,0.0,0.0,0.072],[0.0,1.,0.0,0.031],[0.0,0.0,0.0,1.0]])
+headMcam = dot(headMcam,rotate('x',10*pi/180))
+taskGaze.opmodif = matrixToTuple(headMcam)
+
+# --- FOV ---
+taskFoV = MetaTaskVisualPoint('FoV',dyn,'head','gaze')
+taskFoV.opmodif = matrixToTuple(headMcam)
+
+taskFoV.task=TaskInequality('taskFoVineq')
+taskFoV.task.add(taskFoV.feature.name)
+[Xmax,Ymax]=[0.38,0.28]
+taskFoV.task.referenceInf.value = (-Xmax,-Ymax)    # Xmin, Ymin
+taskFoV.task.referenceSup.value = (Xmax,Ymax)  # Xmax, Ymax
+taskFoV.task.dt.value=dt
+taskFoV.task.controlGain.value=0.01
+taskFoV.featureDes.xy.value = (0,0)
+
+# --- ORIENTATION -------------------------------------------------------------
+from dynamic_graph.sot.core.feature_vector3 import FeatureVector3
+featureVectorRH = FeatureVector3('fv3rh')
+plug(dyn.rh,featureVectorRH.position)
+plug(dyn.Jrh,featureVectorRH.Jq)
+featureVectorRH.vector.value = (1,0,0)
+featureVectorRH.positionRef.value = (1,0,0)
+taskVectorRH = Task('taskVectorRH')
+taskVectorRH.add(featureVectorRH.name)
+taskVectorRH.controlGain.value = 10
+
+featureVectorLH = FeatureVector3('fv3lh')
+plug(dyn.lh,featureVectorLH.position)
+plug(dyn.Jlh,featureVectorLH.Jq)
+featureVectorLH.vector.value = (1,0,0)
+featureVectorLH.positionRef.value = (1,0,0)
+taskVectorLH = Task('taskVectorLH')
+taskVectorLH.add(featureVectorLH.name)
+gainVectorLH = GainAdaptive("gainVectorLH")
+plug(taskVectorLH.error,gainVectorLH.error)
+plug(gainVectorLH.gain,taskVectorLH.controlGain)
+setGain(gainVectorLH,(10,1.5,0.01,0.9))
+
+# Control the orientation of the gripper toward the continuous handle.
+taskRHOrient=MetaTaskKine6d('rhorient',dyn,'rh','right-wrist')
+taskRHOrient.opmodif = matrixToTuple(handMgrip)
+taskRHOrient.feature.frame('current')
+taskRHOrient.feature.selec.value = '011'
+taskRHOrient.featureVector = FeatureVector3('feature-rhorient-v3')
+plug(taskRHOrient.feature.position,taskRHOrient.featureVector.position)
+plug(taskRHOrient.feature.Jq,taskRHOrient.featureVector.Jq)
+taskRHOrient.featureVector.vector.value = (1,0,0)
+taskRHOrient.task.add(taskRHOrient.featureVector.name)
+setGain(taskRHOrient.gain,1)
+
+taskLHOrient=MetaTaskKine6d('lhorient',dyn,'lh','left-wrist')
+taskLHOrient.opmodif = matrixToTuple(handMgrip)
+taskLHOrient.feature.frame('current')
+taskLHOrient.feature.selec.value = '011'
+taskLHOrient.featureVector = FeatureVector3('feature-lhorient-v3')
+plug(taskLHOrient.feature.position,taskLHOrient.featureVector.position)
+plug(taskLHOrient.feature.Jq,taskLHOrient.featureVector.Jq)
+taskLHOrient.featureVector.vector.value = (1,0,0)
+taskLHOrient.task.add(taskLHOrient.featureVector.name)
+setGain(taskLHOrient.gain,1)
+
+# --- Task Joint Limits -----------------------------------------
+dyn.upperJl.recompute(0)
+dyn.lowerJl.recompute(0)
+taskJL = TaskJointLimits('taskJL')
+plug(dyn.position,taskJL.position)
+taskJL.controlGain.value = 10
+taskJL.referenceInf.value = dyn.lowerJl.value
+taskJL.referenceSup.value = dyn.upperJl.value
+taskJL.dt.value = dt
+taskJL.selec.value = toFlags(range(6,22)+range(22,28)+range(29,35))
+
+# ---- WAIST TASK ---
+taskWaist=MetaTask6d('waist',dyn,'waist','waist')
+pg.plugWaistTask(taskWaist)
+taskWaist.task.controlGain.value = 5
+
+# --- CONTACTS
+# define contactLF and contactRF
+for name,joint in [ ['LF','left-ankle'], ['RF','right-ankle' ] ]:
+    contact = MetaTaskKine6d('contact'+name,dyn,name,joint)
+    contact.feature.frame('desired')
+    contact.gain.setConstant(10)
+    locals()['contact'+name] = contact
+
+# --- TRACER -----------------------------------------------------------------
+from dynamic_graph.tracer import *
+tr = Tracer('tr')
+tr.open('/tmp/','','.dat')
+tr.start()
+robot.after.addSignal('tr.triger')
+
+tr.add('dyn.com','com')
+
+history = History(dyn,1)
+
+# --- SHORTCUTS ----------------------------------------------------------------
+qn = taskJL.normalizedPosition
+@optionalparentheses
+def pqn(details=True):
+    ''' Display the normalized configuration vector. '''
+    qn.recompute(robot.state.time)
+    s = [ "{0:.1f}".format(v) for v in qn.value]
+    if details:
+        print("Rleg: "+" ".join(s[:6]))
+        print("Lleg: "+" ".join(s[6:12]))
+        print("Body: "+" ".join(s[12:16]))
+        print("Rarm: "+" ".join(s[16:23]))
+        print("Larm :"+" ".join(s[23:30]))
+    else:
+        print(" ".join(s[:30]))
+
+
+def jlbound(t=None):
+    '''Display the velocity bound induced by the JL as a double-column matrix.'''
+    if t==None: t=robot.state.time
+    taskJL.task.recompute(t)
+    return matrix([ [float(x),float(y)] for x,y
+                    in [ c.split(',') for c in taskJL.task.value[6:-3].split('),(') ] ])
+
+def p6d(R,t):
+    M=eye(4)
+    M[0:3,0:3]=R
+    M[0:3,3]=t
+    return M
+
+def push(task):
+    if isinstance(task,str): taskName=task
+    elif "task" in task.__dict__:  taskName=task.task.name
+    else: taskName=task.name
+    if taskName not in sot.toList():
+        sot.push(taskName)
+        if taskName!="taskposture" and "taskposture" in sot.toList():
+            sot.down("taskposture")
+
+
+def pop(task):
+    if isinstance(task,str): taskName=task
+    elif "task" in task.__dict__:  taskName=task.task.name
+    else: taskName=task.name
+    if taskName in sot.toList(): sot.rm(taskName)
+
+
+# --- DISPLAY ------------------------------------------------------------------
+# --- DISPLAY ------------------------------------------------------------------
+# --- DISPLAY ------------------------------------------------------------------
+RAD=pi/180
+comproj = [0.1,-0.95,1.6]
+#robot.viewer.updateElementConfig('footproj',[0.5,0.15,1.6+0.08,0,-pi/2,0 ])
+robot.viewer.updateElementConfig('footproj',comproj+[0,-pi/2,0 ])
+robot.viewer.updateElementConfig('zmp2',[0,0,-10,0,0,0])
+
+def updateComProj(com=None):
+    if com==None:
+        [x,y,z]=com=dyn.com.value
+    else:
+        [x,y,z]=com
+    p=array(comproj)+[0,y,-x]
+    robot.viewer.updateElementConfig('zmp2',vectorToTuple(p)+(0,0,0 ))
+
+robot.displayList.append( updateComProj)
+
+
+def displayFov(recompute=False):
+    if recompute: taskGaze.proj.transfo.recompute(robot.state.time)
+    robot.viewer.updateElementConfig('fov',matrixToRPY(taskGaze.proj.transfo.value))
+displayFov(True)
+robot.after.addSignal(taskGaze.proj.name+'.transfo')
+robot.displayList.append(displayFov)
+
+
+class BallPosition:
+    def __init__(self,xyz=(0,-1.1,0.9)):
+        self.ball = xyz
+        self.prec = 0
+        self.t = 0
+        self.duration = 0
+        self.f = 0
+        self.xyz= self.ball
+        
+    def move(self,xyz,duration=50):
+        self.prec = self.ball
+        self.ball = xyz
+        self.t = 0
+        self.duration = duration
+        self.changeTargets()
+
+        if duration>0:
+            self.f = lambda : self.moveDisplay()
+            attime(ALWAYS,self.f)
+        else:
+            self.moveDisplay()
+
+    def moveDisplay(self):
+        tau = 1.0 if self.duration<=0 else float(self.t) / self.duration
+        xyz = tau * array(self.ball) + (1-tau) * array(self.prec)
+        robot.viewer.updateElementConfig('zmp',vectorToTuple(xyz)+(0,0,0))
+
+        self.t += 1
+        if self.t>self.duration and self.duration>0:
+            attime.stop(self.f)
+        self.xyz= xyz
+        
+    def changeTargets(self):
+        gotoNd(taskRH,self.ball,'111',(4.9,0.9,0.01,0.9))
+        taskFoV.goto3D(self.ball)
+
+b = BallPosition()
+
+
+tr.add(taskJL.name+".normalizedPosition","qn")
+tr.add('dyn.com','com')
+tr.add(taskRH.task.name+'.error','erh')
+tr.add(taskFoV.feature.name+'.error','fov')
+tr.add(taskFoV.task.name+'.normalizedPosition','fovn')
+tr.add(taskSupportSmall.name+".normalizedPosition",'comsmalln')
+tr.add(taskSupport.name+".normalizedPosition",'comn')
+
+robot.after.addSignal(taskJL.name+".normalizedPosition")
+robot.after.addSignal(taskSupportSmall.name+".normalizedPosition")
+robot.after.addSignal(taskFoV.task.name+".normalizedPosition")
+robot.after.addSignal(taskFoV.feature.name+'.error')
+robot.after.addSignal(taskSupport.name+".normalizedPosition")
+
+# --- RUN ----------------------------------------------------------------------
+# --- RUN ----------------------------------------------------------------------
+# --- RUN ----------------------------------------------------------------------
+taskCom.featureDes.errorIN.value = dyn.com.value
+taskCom.task.controlGain.value = 10
+
+taskPosture.gotoq(1,array(q0),chest=[],head=[],rleg=[],lleg=[],rarm=[],larm=[])
+
+sot.push(taskJL.name)
+sot.addContact(contactRF)
+sot.addContact(contactLF)
+sot.push(taskSupport.name)
+
+ball = BallPosition((0,-1.1,0.9))
+
+tw=TaskWeight('tw')
+
+'''
+taskFoV.task.controlGain.value=0.05
+taskSupportSmall.controlGain.value=0.02
+tw.add(taskSupportSmall.name,1)
+tw.add(taskFoV.task.name,10)
+sot.damping.value = 0.01
+'''
+
+taskFoV.task.controlGain.value=0.05
+taskSupportSmall.controlGain.value=0.02
+tw.add(taskSupportSmall.name,1)
+tw.add(taskFoV.task.name,3)
+sot.damping.value = 0.01
+#sot.damping.value = 0.6
+#ball.move((0.1,-1.2,0.9))
+
+push(taskRH)
+push(tw)
+
+ball.move((0.5,-0.2,1.3),0)
+attime(400,lambda:ball.move((1,0.5,0.9)))
+attime(1000,lambda: ball.move((0.6,0.,0.8)))
+attime(1400,lambda: ball.move((0.1,-1.2,0.9)))
+attime(1401,lambda: sigset(sot.damping,0.1))
+attime(1900,lambda: sigset(sot.damping,0.9))
+attime(2080,stop)
+
+#for t in [400,1000,1400]: attime(t,stop)
+#import time
+#for t in [1000]: attime(t,lambda: time.sleep(1))
+#time.sleep(2)
+
+for t in filter(lambda x: isinstance(x,int),attime.events.keys()): attime(t,lambda:sot.resetAset() )
+print filter(lambda x: isinstance(x,int),attime.events.keys())
+# --- DG ---
+
+'''
+taskSupportSmall.controlGain.value=0.1
+taskJL.controlGain.value = 1
+taskSupport.controlGain.value = 1
+
+del attime.events[1900]
+del attime.events[1401]
+
+attime(1800,lambda: sigset(sot.damping,0.9))
+attime(1920,lambda: sigset(sot.damping,0.1))
+attime(1920, lambda: pop(tw))
+attime(850,lambda: sigset(taskSupportSmall.controlGain,0.01))
+'''
+
+go()
+