From b7518e7af5d9acf0e210c7b0102d69298d6ff3d3 Mon Sep 17 00:00:00 2001
From: Francois Keith <keith@lirmm.fr>
Date: Fri, 15 Feb 2013 18:57:28 +0100
Subject: [PATCH] First draft for all python scripts.

Not completely using new macros.
---
 python/ros/ros-dynromeo.py  | 145 +++++++++++++
 python/ros/ros-kineromeo.py | 116 ++++++++++
 python/ros/ros-p105_demo.py | 420 ++++++++++++++++++++++++++++++++++++
 python/ros/ros-walkromeo.py |  97 +++++++++
 4 files changed, 778 insertions(+)
 create mode 100644 python/ros/ros-dynromeo.py
 create mode 100644 python/ros/ros-kineromeo.py
 create mode 100644 python/ros/ros-p105_demo.py
 create mode 100644 python/ros/ros-walkromeo.py

diff --git a/python/ros/ros-dynromeo.py b/python/ros/ros-dynromeo.py
new file mode 100644
index 0000000..1d2e474
--- /dev/null
+++ b/python/ros/ros-dynromeo.py
@@ -0,0 +1,145 @@
+# ______________________________________________________________________________
+# ******************************************************************************
+# 
+# ______________________________________________________________________________
+# ******************************************************************************
+
+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.utils.thread_interruptible_loop import loopInThread,loopShortcuts
+from dynamic_graph.sot.dyninv.meta_task_dyn_6d import MetaTaskDyn6d
+
+from numpy import *
+
+
+from dynamic_graph.sot.romeo.romeo import *
+robot = Robot('robot', device=RobotDynSimu('robot'))
+plug(robot.device.state, robot.dynamic.position)
+
+from dynamic_graph.ros import *
+ros = Ros(robot)
+
+# --- ROBOT SIMU ---------------------------------------------------------------
+#robotName = 'romeo'
+#robotDim=robotDimension[robotName]
+#robot = RobotDynSimu("romeo")
+#robot.resize(robotDim)
+dt=5e-3
+
+
+#-------------------------------------------------------------------------------
+#----- MAIN LOOP ---------------------------------------------------------------
+#-------------------------------------------------------------------------------
+@loopInThread
+def inc():
+    robot.device.increment(dt)
+
+runner=inc()
+[go,stop,next,n]=loopShortcuts(runner)
+
+# --- shortcuts -------------------------------------------------
+@optionalparentheses
+def q():
+    if 'dyn' in globals(): print robot.dynamic.ffposition.__repr__()
+    print robot.state.__repr__()
+@optionalparentheses
+def qdot(): print robot.control.__repr__()
+@optionalparentheses
+def t(): print robot.state.time-1
+@optionalparentheses
+def iter():         print 'iter = ',robot.state.time
+@optionalparentheses
+def status():       print runner.isPlay
+
+# --- DYN ----------------------------------------------------------------------
+
+plug(robot.device.state,   robot.dynamic.position)
+plug(robot.device.velocity,robot.dynamic.velocity)
+robot.dynamic.acceleration.value = robot.dimension*(0.,)
+
+robot.dynamic.ffposition.unplug()
+robot.dynamic.ffvelocity.unplug()
+robot.dynamic.ffacceleration.unplug()
+
+robot.dynamic.setProperty('ComputeBackwardDynamics','true')
+robot.dynamic.setProperty('ComputeAccelerationCoM','true')
+
+robot.device.control.unplug()
+
+# --- Task Dyn -----------------------------------------
+# Task right hand
+task=MetaTaskDyn6d('rh',robot.dynamic,'task')
+task.ref = ((0,0,-1,0.22),(0,1,0,-0.37),(1,0,0,.74),(0,0,0,1))
+
+# Task LFoot: Move the right foot up.
+taskLF=MetaTaskDyn6d('lf',robot.dynamic,'lf','left-ankle')
+taskLF.ref = ((1,0,0,0.0),(0,1,0,+0.29),(0,0,1,.15),(0,0,0,1))
+
+# --- TASK COM ------------------------------------------------------
+robot.dynamic.setProperty('ComputeCoM','true')
+
+featureCom    = FeatureGeneric('featureCom')
+featureComDes = FeatureGeneric('featureComDes')
+plug(robot.dynamic.com,featureCom.errorIN)
+plug(robot.dynamic.Jcom,featureCom.jacobianIN)
+featureCom.setReference('featureComDes')
+featureComDes.errorIN.value = (0.0478408688115,-0.0620357207995,0.684865189311)
+
+taskCom = TaskDynPD('taskCom')
+taskCom.add('featureCom')
+plug(robot.dynamic.velocity,taskCom.qdot)
+taskCom.dt.value = 1e-3
+
+gCom = GainAdaptive('gCom')
+plug(taskCom.error,gCom.error)
+plug(gCom.gain,taskCom.controlGain)
+# Work from .04 to .09 gCom.set 1050 45 125e3
+# Good behavior gCom.set 1080 15 125e3
+# Low gains gCom.set 400 1 39e3
+# Current choice
+gCom.set(1050,45,125e3)
+
+# ---- CONTACT -----------------------------------------
+# Left foot contact
+contactLF = MetaTaskDyn6d('contact_lleg',robot.dynamic,'lf','left-ankle')
+contactLF.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.feature.frame('desired')
+
+# Right foot contact
+contactRF = MetaTaskDyn6d('contact_rleg',robot.dynamic,'rf','right-ankle')
+contactRF.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))
+contactRF.feature.frame('desired')
+
+# --- SOT Dyn OpSpaceH --------------------------------------
+# SOT controller.
+sot = SolverOpSpace('sot')
+sot.setSize(robot.dimension-6)
+#sot.damping.value = 2e-2
+sot.breakFactor.value = 10
+
+plug(robot.dynamic.inertiaReal,sot.matrixInertia)
+plug(robot.dynamic.dynamicDrift,sot.dyndrift)
+plug(robot.dynamic.velocity,sot.velocity)
+
+plug(sot.control,robot.device.control)
+# For the integration of q = int(int(qddot)).
+plug(sot.acceleration,robot.device.acceleration)
+
+# --- RUN ------------------------------------------------
+#  changes the posture of romeo while asserting that it keeps its balance
+featureComDes.errorIN.value = (0.05,  0.1,   0.675)
+
+sot.addContactFromTask(contactLF.task.name,'LF')
+sot._LF_p.value = contactLF.support
+sot.addContactFromTask(contactRF.task.name,'RF')
+sot._RF_p.value = contactRF.support
+sot.push('taskCom')
+
+# go
+
diff --git a/python/ros/ros-kineromeo.py b/python/ros/ros-kineromeo.py
new file mode 100644
index 0000000..ef99d5d
--- /dev/null
+++ b/python/ros/ros-kineromeo.py
@@ -0,0 +1,116 @@
+# -*- coding: utf-8 -*-
+# Copyright 2011, Florent Lamiraux, Thomas Moulard, JRL, CNRS/AIST
+#
+# This file is part of dynamic-graph.
+# dynamic-graph is free software: you can redistribute it and/or
+# modify it under the terms of the GNU Lesser General Public License
+# as published by the Free Software Foundation, either version 3 of
+# the License, or (at your option) any later version.
+#
+# dynamic-graph is distributed in the hope that it will be useful, but
+# WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+# General Lesser Public License for more details.  You should have
+# received a copy of the GNU Lesser General Public License along with
+# dynamic-graph. If not, see <http://www.gnu.org/licenses/>.
+
+from dynamic_graph.sot.romeo.romeo import *
+robot = Robot('robot')
+plug(robot.device.state, robot.dynamic.position)
+
+from dynamic_graph.ros import *
+ros = Ros(robot)
+
+
+# Create a solver.
+# from dynamic_graph.sot.dynamics.solver import Solver
+# solver = Solver(robot)
+
+from dynamic_graph.sot.core.matrix_util import matrixToTuple, vectorToTuple,rotate, matrixToRPY
+from dynamic_graph.sot.core.meta_tasks_kine import *
+from numpy import *
+
+
+# --- ROBOT SIMU ---------------------------------------------------------------
+# robotName = 'romeo'
+# robotDim=robotDimension[robotName]
+# robot = RobotSimu("romeo")
+# robot.resize(robotDim)
+dt=5e-3
+
+# robot.set( initialConfig[robotName] )
+# addRobotViewer(robot,small=True,small_extra=24,verbose=False)
+
+# --- ROMEO HANDS ---------------------------------------------------------------
+# robot.gripper=0.0
+# RobotClass = RobotSimu
+# RobotClass.stateFullSizeOld = RobotClass.stateFullSize
+# RobotClass.stateFullSize = lambda x: [float(v) for v in x.state.value+24*(x.gripper,)]
+
+#-------------------------------------------------------------------------------
+#----- MAIN LOOP ---------------------------------------------------------------
+#-------------------------------------------------------------------------------
+from dynamic_graph.sot.core.utils.thread_interruptible_loop import loopInThread,loopShortcuts
+@loopInThread
+def inc():
+    robot.device.increment(dt)
+
+runner=inc()
+[go,stop,next,n]=loopShortcuts(runner)
+
+# ---- SOT ---------------------------------------------------------------------
+# ---- SOT ---------------------------------------------------------------------
+# ---- SOT ---------------------------------------------------------------------
+
+sot = SOT('sot')
+sot.setNumberDofs(robot.dimension)
+plug(sot.control,robot.device.control)
+
+# ---- TASKS -------------------------------------------------------------------
+# ---- TASKS -------------------------------------------------------------------
+# ---- TASKS -------------------------------------------------------------------
+
+
+# ---- TASK GRIP ---
+taskRH=MetaTaskKine6d('rh',robot.dynamic,'right-wrist','right-wrist')
+handMgrip=eye(4); handMgrip[0:3,3] = (0.1,0,0)
+taskRH.opmodif = matrixToTuple(handMgrip)
+taskRH.feature.frame('desired')
+# robot.tasks['right-wrist'].add(taskRH.feature.name)
+
+# --- STATIC COM (if not walking)
+# taskCom = MetaTaskKineCom(robot.dynamic)
+# robot.dynamic.com.recompute(0)
+# taskCom.featureDes.errorIN.value = robot.dynamic.com.value
+# taskCom.task.controlGain.value = 10
+
+robot.createCenterOfMassFeatureAndTask(
+    'featureCom', 'featureComDef', 'comTask',
+    selec = '011',
+    gain = 10)
+
+
+# --- CONTACTS
+# define contactLF and contactRF
+for name,joint in [ ['LF','left-ankle'], ['RF','right-ankle' ] ]:
+    contact = MetaTaskKine6d('contact'+name,robot.dynamic,name,joint)
+    contact.feature.frame('desired')
+    contact.gain.setConstant(10)
+    contact.keep()
+    locals()['contact'+name] = contact
+
+# --- RUN ----------------------------------------------------------------------
+
+target=(0.5,-0.2,0.8)
+gotoNd(taskRH,target,'111',(4.9,0.9,0.01,0.9))
+
+# sot.push(taskCom.task.name)
+# sot.push(robot.tasks['right-wrist'].name)
+
+sot.push(contactRF.task.name)
+sot.push(contactLF.task.name)
+sot.push((robot.comTask.name))
+sot.push(taskRH.task.name)
+
+# go()
+
diff --git a/python/ros/ros-p105_demo.py b/python/ros/ros-p105_demo.py
new file mode 100644
index 0000000..bdef006
--- /dev/null
+++ b/python/ros/ros-p105_demo.py
@@ -0,0 +1,420 @@
+# ______________________________________________________________________________
+# ******************************************************************************
+# ______________________________________________________________________________
+# ******************************************************************************
+
+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.core import feature_vector3
+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.attime import attime,ALWAYS,refset,sigset
+from dynamic_graph.tracer import Tracer
+from numpy import *
+
+from dynamic_graph.sot.core.utils.history import History
+
+from dynamic_graph.sot.romeo.romeo import *
+robot = Robot('robot')
+plug(robot.device.state, robot.dynamic.position)
+
+from dynamic_graph.ros import *
+ros = Ros(robot)
+
+
+# --- ROBOT SIMU ---------------------------------------------------------------
+# --- ROBOT SIMU ---------------------------------------------------------------
+# --- ROBOT SIMU ---------------------------------------------------------------
+
+dt=5e-3
+q0=list(robot.halfSitting)
+
+#-------------------------------------------------------------------------------
+#----- MAIN LOOP ---------------------------------------------------------------
+#-------------------------------------------------------------------------------
+from dynamic_graph.sot.core.utils.thread_interruptible_loop import loopInThread,loopShortcuts
+@loopInThread
+def inc():
+    robot.device.increment(dt)
+    attime.run(robot.device.control.time)
+    history.record()
+
+runner=inc()
+[go,stop,next,n]=loopShortcuts(runner)
+
+#-----------------------------------------------------------------------------
+#---- DYN --------------------------------------------------------------------
+#-----------------------------------------------------------------------------
+
+robot.dynamic.velocity.value = robot.dimension*(0.,)
+robot.dynamic.acceleration.value = robot.dimension*(0.,)
+
+robot.dynamic.ffposition.unplug()
+robot.dynamic.ffvelocity.unplug()
+robot.dynamic.ffacceleration.unplug()
+
+robot.dynamic.setProperty('ComputeBackwardDynamics','true')
+robot.dynamic.setProperty('ComputeAccelerationCoM','true')
+
+robot.device.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(robot.dimension)
+plug(sot.control,robot.device.control)
+
+# ---- TASKS -------------------------------------------------------------------
+# ---- TASKS -------------------------------------------------------------------
+# ---- TASKS -------------------------------------------------------------------
+
+
+# ---- TASK GRIP ---
+taskRH=MetaTaskKine6d('rh',robot.dynamic,'rh','right-wrist')
+handMgrip=eye(4); handMgrip[0:3,3] = (0.07,-0.02,0)
+taskRH.opmodif = matrixToTuple(handMgrip)
+taskRH.feature.frame('desired')
+
+taskLH=MetaTaskKine6d('lh',robot.dynamic,'lh','left-wrist')
+taskLH.opmodif = matrixToTuple(handMgrip)
+taskLH.feature.frame('desired')
+
+# --- STATIC COM (if not walking)
+taskCom = MetaTaskKineCom(robot.dynamic)
+
+# --- TASK AVOID
+taskShoulder=MetaTaskKine6d('shoulder',robot.dynamic,'shoulder','LShoulderPitch')
+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',robot.dynamic,'elbow','LElbowYaw')
+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(robot.dynamic.com,featureSupport.errorIN)
+plug(robot.dynamic.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(robot.dynamic.com,featureSupportSmall.errorIN)
+plug(robot.dynamic.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(robot.dynamic)
+
+# --- GAZE ---
+taskGaze = MetaTaskVisualPoint('gaze',robot.dynamic,'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',robot.dynamic,'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 -----------------------------------------
+robot.dynamic.upperJl.recompute(0)
+robot.dynamic.lowerJl.recompute(0)
+taskJL = TaskJointLimits('taskJL')
+plug(robot.dynamic.position,taskJL.position)
+taskJL.controlGain.value = 10
+taskJL.referenceInf.value = robot.dynamic.lowerJl.value
+taskJL.referenceSup.value = robot.dynamic.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,robot.dynamic,name,joint)
+    contact.feature.frame('desired')
+    contact.gain.setConstant(10)
+    locals()['contact'+name] = contact
+
+# --- TRACER -----------------------------------------------------------------
+tr = Tracer('tr')
+tr.open('/tmp/','','.dat')
+tr.start()
+robot.device.after.addSignal('tr.triger')
+
+# tr.add('dyn2.com','com')
+
+history = History(robot.dynamic,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.device.after.addSignal(taskJL.name+".normalizedPosition")
+# robot.device.after.addSignal(taskSupportSmall.name+".normalizedPosition")
+# robot.device.after.addSignal(taskFoV.task.name+".normalizedPosition")
+# robot.device.after.addSignal(taskFoV.feature.name+'.error')
+# robot.device.after.addSignal(taskSupport.name+".normalizedPosition")
+
+# --- RUN ----------------------------------------------------------------------
+# --- RUN ----------------------------------------------------------------------
+# --- RUN ----------------------------------------------------------------------
+
+robot.dynamic.com.recompute(0)
+taskCom.featureDes.errorIN.value = robot.dynamic.com.value
+taskCom.task.controlGain.value = 10
+
+ball = BallPosition((0,-1.1,0.9))
+
+push(taskRH)
+ball.move((0.5,-0.2,1.0),0)
+
+next()
+next()
+next()
+ 
+def config(ref=0):
+    if ref==0:
+        print '''Only the task RH'''
+    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)
+        print 'pouet'
+    elif ref==3:
+        print '''Task RH + foot constraint + COM='''
+        sot.addContact(contactRF)
+        sot.addContact(contactLF)
+        push(taskCom)
+        ball.move((0.15,0.1,1),0)
+    elif ref==4:
+        print '''Task RH + foot constraint + COM= + JL'''
+        qu =  list(robot.dynamic.upperJl.value)
+        qu[19]=0
+        taskJL.referenceSup.value =tuple(qu)
+        push(taskJL)
+        sot.addContact(contactRF)
+        sot.addContact(contactLF)
+        push(taskCom)
+        ball.move((0.15,0.1,1),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.15,-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.15,-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)
+
+
+
+# Add a signal 'mySignal' to rosExport which
+#  will get published into the topic 'myTopic'.
+# The associated timestamp associated with accessible through
+# the 'myTopicTimestamp' signal.
+#ros.rosExport.add('vector3Stamped', 'ball.xyz', 'ballPos')
+#robot.device.after
+
+
+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/ros/ros-walkromeo.py b/python/ros/ros-walkromeo.py
new file mode 100644
index 0000000..c3fb4d8
--- /dev/null
+++ b/python/ros/ros-walkromeo.py
@@ -0,0 +1,97 @@
+# ______________________________________________________________________________
+# ******************************************************************************
+# A simple Herdt walking pattern generator for Romeo.
+# ______________________________________________________________________________
+# ******************************************************************************
+
+from dynamic_graph import plug
+from dynamic_graph.sot.core import *
+from dynamic_graph.sot.dynamics import *
+from dynamic_graph.sot.core.matrix_util import matrixToTuple, vectorToTuple,rotate, matrixToRPY
+from dynamic_graph.sot.core.meta_tasks_kine import *
+from dynamic_graph.sot.core.utils.thread_interruptible_loop import loopInThread,loopShortcuts
+from dynamic_graph.sot.dyninv import SolverKine
+
+from dynamic_graph.sot.romeo.romeo import *
+robot = Robot('robot')
+plug(robot.device.state, robot.dynamic.position)
+
+
+
+from dynamic_graph.ros import *
+ros = Ros(robot)
+
+
+# Create a solver.
+# from dynamic_graph.sot.dynamics.solver import Solver
+# solver = Solver(robot)
+
+from dynamic_graph.sot.core.matrix_util import matrixToTuple, vectorToTuple,rotate, matrixToRPY
+from dynamic_graph.sot.core.meta_tasks_kine import *
+from numpy import *
+
+
+# --- ROBOT SIMU ---------------------------------------------------------------
+dt=5e-3
+
+
+#-------------------------------------------------------------------------------
+#----- MAIN LOOP ---------------------------------------------------------------
+#-------------------------------------------------------------------------------
+from dynamic_graph.sot.core.utils.thread_interruptible_loop import loopInThread,loopShortcuts
+@loopInThread
+def inc():
+    robot.device.increment(dt)
+
+runner=inc()
+[go,stop,next,n]=loopShortcuts(runner)
+
+# --- PG ---------------------------------------------------------
+from dynamic_graph.sot.pattern_generator.meta_pg import MetaPG
+pg = MetaPG(robot.dynamic)
+pg.plugZmp(robot.device)
+
+# ---- SOT ---------------------------------------------------------------------
+# The basic SOT solver would work too.
+sot = SolverKine('sot')
+sot.setSize(robot.dimension)
+plug(sot.control,robot.device.control)
+
+# ---- TASKS -------------------------------------------------------------------
+# ---- WAIST TASK ---
+taskWaist=MetaTask6d('waist',robot.dynamic,'waist','waist')
+pg.plugWaistTask(taskWaist)
+taskWaist.task.controlGain.value = 5
+taskWaist.feature.selec.value = '011100'
+
+# --- TASK COM ---
+taskCom = MetaTaskKineCom(robot.dynamic,"compd")
+pg.plugComTask(taskCom)
+taskCom.feature.selec.value = '011'
+
+# --- TASK FEET
+taskRF=MetaTask6d('rf',robot.dynamic,'rf','right-ankle')
+plug(pg.pg.rightfootref,taskRF.featureDes.position)
+taskRF.task.controlGain.value = 40
+
+taskLF=MetaTask6d('lf',robot.dynamic,'lf','left-ankle')
+plug(pg.pg.leftfootref,taskLF.featureDes.position)
+taskLF.task.controlGain.value = 40
+
+# --- RUN ----------------------------------------------------------------------
+# --- RUN ----------------------------------------------------------------------
+# --- RUN ----------------------------------------------------------------------
+sot.push(taskWaist.task.name)
+sot.push(taskRF.task.name)
+sot.push(taskLF.task.name)
+sot.push(taskCom.task.name)
+
+# --- HERDT PG AND START -------------------------------------------------------
+# Set the algorithm generating the ZMP reference trajectory to Herdt's one.
+pg.startHerdt(False)
+# 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)
+
+go()
+
+
-- 
GitLab