diff --git a/CMakeLists.txt b/CMakeLists.txt
index 870033bef0e3abde4c9f518f549e86ffad3b8025..47eaee3aa67d0e6dcc165d8ba48a787590bb5546 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -161,6 +161,7 @@ IF(BUILD_PYTHON_INTERFACE)
     INSTALL(FILES python/${SOTTALOSBALANCE_PYNAME}/test/__init__.py
                   python/${SOTTALOSBALANCE_PYNAME}/test/link_state_publisher.py
                   python/${SOTTALOSBALANCE_PYNAME}/test/test_COMTraj_checkfeedback.py
+                  python/${SOTTALOSBALANCE_PYNAME}/test/appli_COMTraj_checkfeedback.py
                   python/${SOTTALOSBALANCE_PYNAME}/test/test_ffSubscriber.py
                   python/${SOTTALOSBALANCE_PYNAME}/test/test_zmpEstimator.py
                   python/${SOTTALOSBALANCE_PYNAME}/test/test_dcmComControl.py
diff --git a/python/sot_talos_balance/test/appli_COMTraj_checkfeedback.py b/python/sot_talos_balance/test/appli_COMTraj_checkfeedback.py
new file mode 100644
index 0000000000000000000000000000000000000000..0d3e0bfdbdd2fca3e71b25edcdb15a798e296e4a
--- /dev/null
+++ b/python/sot_talos_balance/test/appli_COMTraj_checkfeedback.py
@@ -0,0 +1,73 @@
+from sot_talos_balance.create_entities_utils import create_com_trajectory_generator
+from dynamic_graph.sot.core.meta_tasks_kine import MetaTaskKine6d, MetaTaskKineCom, gotoNd
+from dynamic_graph import plug
+from dynamic_graph.sot.core import SOT
+
+from dynamic_graph.ros import RosSubscribe
+from dynamic_graph.sot.dynamics_pinocchio import DynamicPinocchio
+
+from dynamic_graph.tracer_real_time import TracerRealTime
+from sot_talos_balance.create_entities_utils import addTrace, dump_tracer
+
+dt = robot.timeStep;
+robot.comTrajGen = create_com_trajectory_generator(dt,robot);
+
+robot.subscriber = RosSubscribe("base_subscriber")
+robot.subscriber.add("vector","position","/sot/base_link/position")
+robot.subscriber.add("vector","velocity","/sot/base_link/velocity")
+
+robot.rdynamic = DynamicPinocchio("real_dynamics")
+robot.rdynamic.setModel(robot.dynamic.model)
+robot.rdynamic.setData(robot.rdynamic.model.createData())
+
+plug(robot.device.robotState,robot.rdynamic.position)
+plug(robot.subscriber.position,robot.rdynamic.ffposition)
+
+plug(robot.device.robotVelocity,robot.rdynamic.velocity)
+plug(robot.subscriber.velocity,robot.rdynamic.ffvelocity)
+
+robot.rdynamic.acceleration.value = [0.0]*38
+
+# --- COM
+robot.taskCom = MetaTaskKineCom(robot.dynamic)
+robot.dynamic.com.recompute(0)
+robot.taskCom.featureDes.errorIN.value = robot.dynamic.com.value
+robot.taskCom.task.controlGain.value = 10
+
+# --- CONTACTS
+#define contactLF and contactRF
+robot.contactLF = MetaTaskKine6d('contactLF',robot.dynamic,'LF',robot.OperationalPointsMap['left-ankle'])
+robot.contactLF.feature.frame('desired')
+robot.contactLF.gain.setConstant(100)
+robot.contactLF.keep()
+locals()['contactLF'] = robot.contactLF
+
+robot.contactRF = MetaTaskKine6d('contactRF',robot.dynamic,'RF',robot.OperationalPointsMap['right-ankle'])
+robot.contactRF.feature.frame('desired')
+robot.contactRF.gain.setConstant(100)
+robot.contactRF.keep()
+locals()['contactRF'] = robot.contactRF
+
+robot.sot = SOT('sot')
+robot.sot.setSize(robot.dynamic.getDimension())
+plug(robot.sot.control,robot.device.control)
+plug(robot.comTrajGen.x,    robot.taskCom.featureDes.errorIN)
+
+robot.sot.push(robot.contactRF.task.name)
+robot.sot.push(robot.contactLF.task.name)
+robot.sot.push(robot.taskCom.task.name)
+robot.device.control.recompute(0)
+
+# --- TRACER
+robot.tracer = TracerRealTime("com_tracer")
+robot.tracer.setBufferSize(80*(2**20))
+robot.tracer.open('/tmp','dg_','.dat')
+robot.device.after.addSignal('{0}.triger'.format(robot.tracer.name))
+robot.device.after.addSignal('{0}.com'.format(robot.rdynamic.name))
+
+addTrace(robot.tracer, robot.comTrajGen, 'x')
+addTrace(robot.tracer, robot.dynamic, 'com')
+addTrace(robot.tracer, robot.rdynamic, 'com')
+
+robot.tracer.start()
+
diff --git a/python/sot_talos_balance/test/link_state_publisher.py b/python/sot_talos_balance/test/link_state_publisher.py
index 1c5cb7904a08a9e7470151e8d01cd0cf5821c41c..50aac24c7831a71c0a7bcd8c96d7e4e65a2b2cc5 100644
--- a/python/sot_talos_balance/test/link_state_publisher.py
+++ b/python/sot_talos_balance/test/link_state_publisher.py
@@ -18,8 +18,15 @@ class LinkStatePublisher(threading.Thread):
         self.rate = rate
         self.prefix = prefix
         self.rpy = rpy
+        self._stop = False
         rospy.init_node(self.name, anonymous=True)
 
+    def stop(self):
+        self._stop = True
+
+    def stopped(self):
+        return self._stop
+
     def run(self):
         topic_pos = self.prefix + '/' + self.link_name + '/position'
         topic_vel = self.prefix + '/' + self.link_name + '/velocity'
@@ -31,7 +38,7 @@ class LinkStatePublisher(threading.Thread):
         get_link_state = rospy.ServiceProxy('/gazebo/get_link_state', GetLinkState)
 
         rate = rospy.Rate(self.rate)
-        while not rospy.is_shutdown():
+        while ( not self.stopped() ) and ( not rospy.is_shutdown() ):
             link_state_msg = get_link_state(link_name = self.link_name)
             link_state = link_state_msg.link_state
 
diff --git a/python/sot_talos_balance/test/test_COMTraj_checkfeedback.py b/python/sot_talos_balance/test/test_COMTraj_checkfeedback.py
index ef5253887775046540c1e448de4663e0a51d13e9..3d8cd9740d627ca58151c3ef36a02326dbd4b21f 100644
--- a/python/sot_talos_balance/test/test_COMTraj_checkfeedback.py
+++ b/python/sot_talos_balance/test/test_COMTraj_checkfeedback.py
@@ -1,141 +1,78 @@
-from sot_talos_balance.create_entities_utils import create_com_trajectory_generator
-from dynamic_graph.sot.core.meta_tasks_kine import MetaTaskKine6d, MetaTaskKineCom, gotoNd
-from dynamic_graph.sot.core.matrix_util import matrixToTuple
-from dynamic_graph import plug
-from dynamic_graph.sot.core import SOT
-from numpy import eye
+from sot_talos_balance.utils.run_test_utils import run_test, runCommandClient, evalCommandClient
+from sot_talos_balance.test.link_state_publisher import LinkStatePublisher
 from time import sleep
-import os
 
-from dynamic_graph.ros import RosSubscribe
-from dynamic_graph.sot.dynamics_pinocchio import DynamicPinocchio
-
-from dynamic_graph.tracer_real_time import TracerRealTime
-from sot_talos_balance.create_entities_utils import addTrace, dump_tracer
 import matplotlib.pyplot as plt
 import numpy as np
 
-def main(robot):
-    dt = robot.timeStep;
-    robot.comTrajGen = create_com_trajectory_generator(dt,robot);
-
-    robot.subscriber = RosSubscribe("base_subscriber")
-    robot.subscriber.add("vector","position","/sot/base_link/position")
-    robot.subscriber.add("vector","velocity","/sot/base_link/velocity")
-
-    robot.rdynamic = DynamicPinocchio("real_dynamics")
-    robot.rdynamic.setModel(robot.dynamic.model)
-    robot.rdynamic.setData(robot.rdynamic.model.createData())
-
-    plug(robot.device.robotState,robot.rdynamic.position)
-    plug(robot.subscriber.position,robot.rdynamic.ffposition)
-
-    plug(robot.device.robotVelocity,robot.rdynamic.velocity)
-    plug(robot.subscriber.velocity,robot.rdynamic.ffvelocity)
-
-    robot.rdynamic.acceleration.value = [0.0]*38
-
-    # --- COM
-    robot.taskCom = MetaTaskKineCom(robot.dynamic)
-    robot.dynamic.com.recompute(0)
-    robot.taskCom.featureDes.errorIN.value = robot.dynamic.com.value
-    robot.taskCom.task.controlGain.value = 10
-
-    # --- CONTACTS
-    #define contactLF and contactRF
-    robot.contactLF = MetaTaskKine6d('contactLF',robot.dynamic,'LF',robot.OperationalPointsMap['left-ankle'])
-    robot.contactLF.feature.frame('desired')
-    robot.contactLF.gain.setConstant(100)
-    robot.contactLF.keep()
-    locals()['contactLF'] = robot.contactLF
-
-    robot.contactRF = MetaTaskKine6d('contactRF',robot.dynamic,'RF',robot.OperationalPointsMap['right-ankle'])
-    robot.contactRF.feature.frame('desired')
-    robot.contactRF.gain.setConstant(100)
-    robot.contactRF.keep()
-    locals()['contactRF'] = robot.contactRF
-
-    robot.sot = SOT('sot')
-    robot.sot.setSize(robot.dynamic.getDimension())
-    plug(robot.sot.control,robot.device.control)
-
-    robot.sot.push(robot.contactRF.task.name)
-    robot.sot.push(robot.taskCom.task.name)
-    robot.sot.push(robot.contactLF.task.name)
-    robot.device.control.recompute(0)
-
-    # --- TRACER
-    robot.tracer = TracerRealTime("com_tracer")
-    robot.tracer.setBufferSize(80*(2**20))
-    robot.tracer.open('/tmp','dg_','.dat')
-    robot.device.after.addSignal('{0}.triger'.format(robot.tracer.name))
-    robot.device.after.addSignal('{0}.com'.format(robot.rdynamic.name))
-
-    addTrace(robot.tracer, robot.comTrajGen, 'x')
-    addTrace(robot.tracer, robot.dynamic, 'com')
-    addTrace(robot.tracer, robot.rdynamic, 'com')
-
-    robot.tracer.start()
-
-    plug(robot.comTrajGen.x,    robot.taskCom.featureDes.errorIN);
-    sleep(1.0);
-    os.system("rosservice call \start_dynamic_graph")
-    sleep(2.0);
-    robot.comTrajGen.move(1,-0.025,4.0);
-    sleep(5.0);
-    robot.comTrajGen.startSinusoid(1,0.05,8.0);
-    sleep(5.0);
-
-    dump_tracer(robot.tracer)
-
-	  # --- DISPLAY
-    comDes_data = np.loadtxt('/tmp/dg_'+robot.comTrajGen.name+'-x.dat')
-    comSot_data = np.loadtxt('/tmp/dg_'+robot.dynamic.name+'-com.dat')
-    com_data    = np.loadtxt('/tmp/dg_'+robot.rdynamic.name+'-com.dat')
-    
-
-    plt.figure()
-    plt.plot(com_data[:,1],'b-')
-    plt.plot(comDes_data[:,1],'b--')
-    plt.plot(comSot_data[:,1],'b:')
-    plt.plot(com_data[:,2],'r-')
-    plt.plot(comDes_data[:,2],'r--')
-    plt.plot(comSot_data[:,2],'r:')
-    plt.plot(com_data[:,3],'g-')
-    plt.plot(comDes_data[:,3],'g--')
-    plt.plot(comSot_data[:,3],'g:')
-    plt.title('COM real vs desired vs SOT')
-    plt.legend(['Real x','Desired x','SOT x','Real y','Desired y','SOT y','Real z','Desired z','SOT z'])
-
-    plt.figure()
-    plt.plot(com_data[:,1],'b-')
-    plt.title('COM real x')
-    plt.figure()
-    plt.plot(comDes_data[:,1],'b--')
-    plt.title('COM desired x')
-    plt.figure()
-    plt.plot(comSot_data[:,1],'b:')
-    plt.title('COM SOT x')
-
-    plt.figure()
-    plt.plot(com_data[:,2],'r-')
-    plt.title('COM real y')
-    plt.figure()
-    plt.plot(comDes_data[:,2],'r--')
-    plt.title('COM desired y')
-    plt.figure()
-    plt.plot(comSot_data[:,2],'r:')
-    plt.title('COM SOT y')
-
-    plt.figure()
-    plt.plot(com_data[:,3],'g-')
-    plt.title('COM real z')
-    plt.figure()
-    plt.plot(comDes_data[:,3],'g--')
-    plt.title('COM desired z')
-    plt.figure()
-    plt.plot(comSot_data[:,3],'g:')
-    plt.title('COM SOT z')
-
-    plt.show()
+pub = LinkStatePublisher('base_link',1000)
+print("Starting link state publisher...")
+pub.start()
+print("Link state publisher started")
+raw_input("Wait before running the test")
+
+run_test('appli_COMTraj_checkfeedback.py')
+
+sleep(2.0)
+runCommandClient('robot.comTrajGen.move(1,-0.025,4.0)')
+sleep(5.0)
+runCommandClient('robot.comTrajGen.startSinusoid(1,0.05,8.0)')
+sleep(5.0)
+runCommandClient('dump_tracer(robot.tracer)')
+
+# --- DISPLAY
+comDes_data = np.loadtxt('/tmp/dg_' + evalCommandClient('robot.comTrajGen.name') + '-x.dat')
+comSot_data = np.loadtxt('/tmp/dg_' + evalCommandClient('robot.dynamic.name') + '-com.dat')
+com_data    = np.loadtxt('/tmp/dg_' + evalCommandClient('robot.rdynamic.name') + '-com.dat')
+
+plt.ion()
+
+plt.figure()
+plt.plot(com_data[:,1],'b-')
+plt.plot(comDes_data[:,1],'b--')
+plt.plot(comSot_data[:,1],'b:')
+plt.plot(com_data[:,2],'r-')
+plt.plot(comDes_data[:,2],'r--')
+plt.plot(comSot_data[:,2],'r:')
+plt.plot(com_data[:,3],'g-')
+plt.plot(comDes_data[:,3],'g--')
+plt.plot(comSot_data[:,3],'g:')
+plt.title('COM real vs desired vs SOT')
+plt.legend(['Real x','Desired x','SOT x','Real y','Desired y','SOT y','Real z','Desired z','SOT z'])
+
+plt.figure()
+plt.plot(com_data[:,1],'b-')
+plt.title('COM real x')
+plt.figure()
+plt.plot(comDes_data[:,1],'b--')
+plt.title('COM desired x')
+plt.figure()
+plt.plot(comSot_data[:,1],'b:')
+plt.title('COM SOT x')
+
+plt.figure()
+plt.plot(com_data[:,2],'r-')
+plt.title('COM real y')
+plt.figure()
+plt.plot(comDes_data[:,2],'r--')
+plt.title('COM desired y')
+plt.figure()
+plt.plot(comSot_data[:,2],'r:')
+plt.title('COM SOT y')
+
+plt.figure()
+plt.plot(com_data[:,3],'g-')
+plt.title('COM real z')
+plt.figure()
+plt.plot(comDes_data[:,3],'g--')
+plt.title('COM desired z')
+plt.figure()
+plt.plot(comSot_data[:,3],'g:')
+plt.title('COM SOT z')
+
+raw_input("Wait before leaving the simulation")
+print("Stopping link state publisher...")
+pub.stop()
+sleep(0.1)
+print("Link state publisher stopped")
 
diff --git a/python/sot_talos_balance/utils/run_test_utils.py b/python/sot_talos_balance/utils/run_test_utils.py
index 1b836791e282e80868d690346ad2685369c254d0..a0e756a5c5cd4b70abc88e0084670d0eb5ffead4 100644
--- a/python/sot_talos_balance/utils/run_test_utils.py
+++ b/python/sot_talos_balance/utils/run_test_utils.py
@@ -40,8 +40,6 @@ def run_test(appli):
 
         runCommandStartDynamicGraph = rospy.ServiceProxy('start_dynamic_graph', Empty)
 
-        rospy.loginfo("Stack of Tasks launched")
-
         initCode = open( appli, "r").read().split("\n")
     
         rospy.loginfo("Stack of Tasks launched")