diff --git a/CMakeLists.txt b/CMakeLists.txt
index a2471062c92e7cfaf4133e0dfc4849cd09aa3d9e..c8983c98967220fc883c4f81a898b8591748ab55 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -161,6 +161,12 @@ IF(BUILD_PYTHON_INTERFACE)
                 DESTINATION ${PYTHON_SITELIB}/${SOTTALOSBALANCE_PYNAME}/talos)
   IF(BUILD_TEST)
     INSTALL(FILES python/${SOTTALOSBALANCE_PYNAME}/test/__init__.py
+                  python/${SOTTALOSBALANCE_PYNAME}/test/test_config_feedback_gazebo.py
+                  python/${SOTTALOSBALANCE_PYNAME}/test/appli_config_feedback_gazebo.py
+                  python/${SOTTALOSBALANCE_PYNAME}/test/test_static_sensorfeedback.py
+                  python/${SOTTALOSBALANCE_PYNAME}/test/appli_static_sensorfeedback.py
+                  python/${SOTTALOSBALANCE_PYNAME}/test/test_static_feedback_gazebo.py
+                  python/${SOTTALOSBALANCE_PYNAME}/test/appli_static_feedback_gazebo.py
                   python/${SOTTALOSBALANCE_PYNAME}/test/test_COMTraj_feedback_gazebo.py
                   python/${SOTTALOSBALANCE_PYNAME}/test/appli_COMTraj_feedback_gazebo.py
                   python/${SOTTALOSBALANCE_PYNAME}/test/test_COMTraj_sensorfeedback.py
diff --git a/python/sot_talos_balance/create_entities_utils.py b/python/sot_talos_balance/create_entities_utils.py
index 129eee73ed543f91f0d934604f3193269256d5fe..b931684b5f78e252e6c605416b53c92d3521f173 100644
--- a/python/sot_talos_balance/create_entities_utils.py
+++ b/python/sot_talos_balance/create_entities_utils.py
@@ -408,9 +408,8 @@ def create_zmp_estimator(robot):
     estimator = SimpleZmpEstimator("zmpEst")
     plug(robot.dynamic.LF,estimator.poseLeft)
     plug(robot.dynamic.RF,estimator.poseRight)
-    # force sensors are not mapped correctly
-    plug(robot.device.forceRLEG,estimator.wrenchLeft)
-    plug(robot.device.forceRARM,estimator.wrenchRight)
+    plug(robot.device.forceLLEG,estimator.wrenchLeft)
+    plug(robot.device.forceRLEG,estimator.wrenchRight)
 
     estimator.init()
     return estimator
diff --git a/python/sot_talos_balance/test/appli_config_feedback_gazebo.py b/python/sot_talos_balance/test/appli_config_feedback_gazebo.py
new file mode 100644
index 0000000000000000000000000000000000000000..6275121f3af78bc4c97b748393d24a20c34a4e0a
--- /dev/null
+++ b/python/sot_talos_balance/test/appli_config_feedback_gazebo.py
@@ -0,0 +1,69 @@
+from sot_talos_balance.create_entities_utils import *
+from sot_talos_balance.meta_task_config import MetaTaskKineConfig
+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.sot.core.operator import Mix_of_vector
+
+from dynamic_graph.ros import RosSubscribe
+from dynamic_graph.sot.dynamics_pinocchio import DynamicPinocchio
+
+dt = robot.timeStep;
+
+# --- Subscriber
+robot.subscriber = RosSubscribe("base_subscriber")
+robot.subscriber.add("vector","position","/sot/base_link/position")
+robot.subscriber.add("vector","velocity","/sot/base_link/velocity")
+
+# --- Merger
+robot.statemix = Mix_of_vector("statemix")
+
+robot.statemix.setSignalNumber(3)
+
+robot.statemix.addSelec(1,0,6)
+robot.statemix.addSelec(2,6,robot.dynamic.getDimension()-6)
+
+robot.statemix.default.value=[0.0]*robot.dynamic.getDimension()
+plug(robot.subscriber.position,robot.statemix.signal("sin1"))
+plug(robot.device.joint_angles,robot.statemix.signal("sin2"))
+
+# --- Dynamic pinocchio
+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]*robot.dynamic.getDimension()
+
+# --- Posture
+robot.taskPos = MetaTaskKineConfig(robot.rdynamic)
+robot.taskPos.featureDes.errorIN.value = robot.dynamic.position.value
+robot.taskPos.task.controlGain.value = 10
+plug(robot.statemix.sout,robot.taskPos.feature.errorIN)
+
+# --- SOLVER
+robot.sot = SOT('sot')
+robot.sot.setSize(robot.dynamic.getDimension())
+robot.sot.push(robot.taskPos.task.name)
+
+robot.device.control.value = [0.0]*robot.dynamic.getDimension()
+
+# --- 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}.state'.format(robot.device.name))
+robot.device.after.addSignal('{0}.sout'.format(robot.statemix.name))
+robot.device.after.addSignal('{0}.errorIN'.format(robot.taskPos.featureDes.name))
+
+addTrace(robot.tracer, robot.taskPos.featureDes, 'errorIN')
+addTrace(robot.tracer, robot.device, 'state')
+addTrace(robot.tracer, robot.statemix, 'sout')
+
+robot.tracer.start()
diff --git a/python/sot_talos_balance/test/appli_static_feedback_gazebo.py b/python/sot_talos_balance/test/appli_static_feedback_gazebo.py
new file mode 100644
index 0000000000000000000000000000000000000000..2153f53a8ca59ffcf1dd14aa2586be6c9c6896c4
--- /dev/null
+++ b/python/sot_talos_balance/test/appli_static_feedback_gazebo.py
@@ -0,0 +1,80 @@
+from sot_talos_balance.create_entities_utils import *
+from sot_talos_balance.meta_task_config import MetaTaskKineConfig
+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.sot.core.operator import Mix_of_vector
+
+from dynamic_graph.ros import RosSubscribe
+from dynamic_graph.sot.dynamics_pinocchio import DynamicPinocchio
+
+dt = robot.timeStep;
+
+# --- Subscriber
+robot.subscriber = RosSubscribe("base_subscriber")
+robot.subscriber.add("vector","position","/sot/base_link/position")
+robot.subscriber.add("vector","velocity","/sot/base_link/velocity")
+
+# --- Dynamic pinocchio
+robot.rdynamic = DynamicPinocchio("real_dynamics")
+robot.rdynamic.setModel(robot.dynamic.model)
+robot.rdynamic.setData(robot.rdynamic.model.createData())
+
+plug(robot.device.state,robot.rdynamic.position)
+plug(robot.subscriber.position,robot.rdynamic.ffposition)
+
+plug(robot.device.velocity,robot.rdynamic.velocity)
+plug(robot.subscriber.velocity,robot.rdynamic.ffvelocity)
+
+robot.rdynamic.acceleration.value = [0.0]*robot.dynamic.getDimension()
+
+# --- Posture
+#robot.taskPos = MetaTaskKineConfig(robot.rdynamic)
+#robot.taskPos.featureDes.errorIN.value = robot.dynamic.position.value
+#robot.taskPos.task.controlGain.value = 100
+#plug(robot.state,robot.taskPos.feature.errorIN)
+
+# --- COM
+robot.taskCom = MetaTaskKineCom(robot.rdynamic)
+robot.dynamic.com.recompute(0)
+robot.taskCom.featureDes.errorIN.value = robot.dynamic.com.value
+robot.taskCom.task.controlGain.value = 100
+
+# --- CONTACTS
+#define contactLF and contactRF
+robot.contactLF = MetaTaskKine6d('contactLF',robot.rdynamic,'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.rdynamic,'RF',robot.OperationalPointsMap['right-ankle'])
+robot.contactRF.feature.frame('desired')
+robot.contactRF.gain.setConstant(100)
+# robot.contactRF.keep()
+locals()['contactRF'] = robot.contactRF
+
+# --- SOLVER
+robot.sot = SOT('sot')
+robot.sot.setSize(robot.dynamic.getDimension())
+robot.sot.push(robot.contactRF.task.name)
+robot.sot.push(robot.contactLF.task.name)
+robot.sot.push(robot.taskCom.task.name)
+#robot.sot.push(robot.taskPos.task.name)
+
+robot.device.control.value = [0.0]*robot.dynamic.getDimension()
+
+# --- 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))
+robot.device.after.addSignal('{0}.com'.format(robot.dynamic.name))
+robot.device.after.addSignal('{0}.errorIN'.format(robot.taskCom.featureDes.name))
+
+addTrace(robot.tracer, robot.taskCom.featureDes, 'errorIN')
+addTrace(robot.tracer, robot.dynamic, 'com')
+addTrace(robot.tracer, robot.rdynamic, 'com')
+
+robot.tracer.start()
diff --git a/python/sot_talos_balance/test/appli_static_sensorfeedback.py b/python/sot_talos_balance/test/appli_static_sensorfeedback.py
new file mode 100644
index 0000000000000000000000000000000000000000..54f3fdc80307c02774ced71ed51602eec0718f9c
--- /dev/null
+++ b/python/sot_talos_balance/test/appli_static_sensorfeedback.py
@@ -0,0 +1,89 @@
+from sot_talos_balance.create_entities_utils import *
+from sot_talos_balance.meta_task_config import MetaTaskKineConfig
+import sot_talos_balance.control_manager_conf as param_server_conf
+from sot_talos_balance.talos import base_estimator_conf
+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.sot.core.operator import Selec_of_vector
+
+from dynamic_graph.ros import RosSubscribe
+from dynamic_graph.sot.dynamics_pinocchio import DynamicPinocchio
+
+dt = robot.timeStep;
+
+# --- Estimators
+robot.param_server            = create_parameter_server(param_server_conf,dt)
+robot.device_filters          = create_device_filters(robot, dt)
+robot.imu_filters             = create_imu_filters(robot, dt)
+robot.base_estimator          = create_base_estimator(robot, dt, base_estimator_conf)
+robot.be_filters              = create_be_filters(robot, dt)
+robot.dcm_estimator           = create_dcm_estimator(robot, dt)
+
+robot.baseselec = Selec_of_vector("base_selec")
+robot.baseselec.selec(0,6)
+plug(robot.base_estimator.q,robot.baseselec.sin)
+
+# --- Dynamic pinocchio
+robot.rdynamic = DynamicPinocchio("real_dynamics")
+robot.rdynamic.setModel(robot.dynamic.model)
+robot.rdynamic.setData(robot.rdynamic.model.createData())
+
+plug(robot.device.state,robot.rdynamic.position)
+plug(robot.baseselec.sout,robot.rdynamic.ffposition)
+
+robot.rdynamic.velocity.value = [0.0]*robot.dynamic.getDimension()
+
+robot.rdynamic.acceleration.value = [0.0]*robot.dynamic.getDimension()
+
+# --- Posture
+#robot.taskPos = MetaTaskKineConfig(robot.rdynamic)
+#robot.taskPos.featureDes.errorIN.value = robot.dynamic.position.value
+#robot.taskPos.task.controlGain.value = 100
+#plug(robot.state,robot.taskPos.feature.errorIN)
+
+# --- COM
+robot.taskCom = MetaTaskKineCom(robot.rdynamic)
+robot.dynamic.com.recompute(0)
+robot.taskCom.featureDes.errorIN.value = robot.dynamic.com.value
+robot.taskCom.task.controlGain.value = 100
+# plug(robot.dcm_estimator.c, robot.taskCom.feature.errorIN)
+
+# --- CONTACTS
+#define contactLF and contactRF
+robot.contactLF = MetaTaskKine6d('contactLF',robot.rdynamic,'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.rdynamic,'RF',robot.OperationalPointsMap['right-ankle'])
+robot.contactRF.feature.frame('desired')
+robot.contactRF.gain.setConstant(100)
+# robot.contactRF.keep()
+locals()['contactRF'] = robot.contactRF
+
+# --- SOLVER
+robot.sot = SOT('sot')
+robot.sot.setSize(robot.dynamic.getDimension())
+robot.sot.push(robot.contactRF.task.name)
+robot.sot.push(robot.contactLF.task.name)
+robot.sot.push(robot.taskCom.task.name)
+#robot.sot.push(robot.taskPos.task.name)
+
+robot.device.control.value = [0.0]*robot.dynamic.getDimension()
+
+# --- 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))
+robot.device.after.addSignal('{0}.com'.format(robot.dynamic.name))
+robot.device.after.addSignal('{0}.errorIN'.format(robot.taskCom.featureDes.name))
+
+addTrace(robot.tracer, robot.taskCom.featureDes, 'errorIN')
+addTrace(robot.tracer, robot.dynamic, 'com')
+addTrace(robot.tracer, robot.rdynamic, 'com')
+
+robot.tracer.start()
diff --git a/python/sot_talos_balance/test/test_config_feedback_gazebo.py b/python/sot_talos_balance/test/test_config_feedback_gazebo.py
new file mode 100644
index 0000000000000000000000000000000000000000..74dc0fd2a0ce2a5b84e7a64ccce08c9b0de1afe2
--- /dev/null
+++ b/python/sot_talos_balance/test/test_config_feedback_gazebo.py
@@ -0,0 +1,95 @@
+from sot_talos_balance.utils.run_test_utils import run_test, runCommandClient, evalCommandClient
+from sot_talos_balance.utils.gazebo_utils import GazeboLinkStatePublisher
+from time import sleep
+
+import matplotlib.pyplot as plt
+import numpy as np
+
+pub = GazeboLinkStatePublisher('base_link',5000)
+print("Starting Gazebo link state publisher...")
+pub.start()
+print("Gazebo link state publisher started")
+raw_input("Wait before running the test")
+
+run_test('appli_config_feedback_gazebo.py')
+
+# wait for sensor values to be ready
+raw_input("Wait before plugging the SOT")
+
+# set initial conditions from sensor readings
+runCommandClient('robot.statemix.sout.recompute(0)')
+runCommandClient('robot.device.set(robot.statemix.sout.value)')
+runCommandClient('robot.taskPos.featureDes.errorIN.value = robot.device.state.value')
+
+# plug the SOT
+runCommandClient('plug(robot.sot.control,robot.device.control)')
+
+sleep(5.0)
+runCommandClient('dump_tracer(robot.tracer)')
+
+# --- DISPLAY
+posDes_data = np.loadtxt('/tmp/dg_' + evalCommandClient('robot.taskPos.featureDes.name') + '-errorIN.dat')
+posSot_data = np.loadtxt('/tmp/dg_' + evalCommandClient('robot.device.name') + '-state.dat')
+pos_data    = np.loadtxt('/tmp/dg_' + evalCommandClient('robot.statemix.name') + '-sout.dat')
+
+posErr_data = pos_data - posDes_data
+
+plt.ion()
+
+plt.figure()
+plt.plot(pos_data[:,1],'b-')
+plt.plot(posDes_data[:,1],'b--')
+plt.plot(posSot_data[:,1],'b:')
+plt.plot(pos_data[:,2],'r-')
+plt.plot(posDes_data[:,2],'r--')
+plt.plot(posSot_data[:,2],'r:')
+plt.plot(pos_data[:,3],'g-')
+plt.plot(posDes_data[:,3],'g--')
+plt.plot(posSot_data[:,3],'g:')
+plt.title('Pos 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(posErr_data[:,1],'b-')
+plt.plot(posErr_data[:,2],'r-')
+plt.plot(posErr_data[:,3],'g-')
+plt.title('Pos error')
+plt.legend(['x','y','z'])
+
+plt.figure()
+plt.plot(pos_data[:,1],'b-')
+plt.title('Pos real x')
+plt.figure()
+plt.plot(posDes_data[:,1],'b--')
+plt.title('Pos desired x')
+plt.figure()
+plt.plot(posSot_data[:,1],'b:')
+plt.title('Pos SOT x')
+
+plt.figure()
+plt.plot(pos_data[:,2],'r-')
+plt.title('Pos real y')
+plt.figure()
+plt.plot(posDes_data[:,2],'r--')
+plt.title('Pos desired y')
+plt.figure()
+plt.plot(posSot_data[:,2],'r:')
+plt.title('Pos SOT y')
+
+plt.figure()
+plt.plot(pos_data[:,3],'g-')
+plt.title('Pos real z')
+plt.figure()
+plt.plot(posDes_data[:,3],'g--')
+plt.title('Pos desired z')
+plt.figure()
+plt.plot(posSot_data[:,3],'g:')
+plt.title('Pos SOT z')
+
+raw_input("Wait before leaving the simulation")
+print("Stopping Gazebo link state publisher...")
+pub.stop()
+sleep(0.1)
+print("Gazebo link state publisher stopped")
+
+
diff --git a/python/sot_talos_balance/test/test_static_feedback_gazebo.py b/python/sot_talos_balance/test/test_static_feedback_gazebo.py
new file mode 100644
index 0000000000000000000000000000000000000000..d1cdb4cbcee83ae2e8997695efeb0f8542d44309
--- /dev/null
+++ b/python/sot_talos_balance/test/test_static_feedback_gazebo.py
@@ -0,0 +1,97 @@
+from sot_talos_balance.utils.run_test_utils import run_test, runCommandClient, evalCommandClient
+from sot_talos_balance.utils.gazebo_utils import GazeboLinkStatePublisher
+from time import sleep
+
+import matplotlib.pyplot as plt
+import numpy as np
+
+pub = GazeboLinkStatePublisher('base_link',5000)
+print("Starting Gazebo link state publisher...")
+pub.start()
+print("Gazebo link state publisher started")
+raw_input("Wait before running the test")
+
+run_test('appli_static_feedback_gazebo.py')
+
+# wait for sensor values to be ready
+raw_input("Wait before plugging the SOT")
+
+# set initial conditions from sensor readings
+runCommandClient('robot.rdynamic.com.recompute(0)')
+runCommandClient('robot.taskCom.featureDes.errorIN.value = robot.rdynamic.com.value')
+runCommandClient('robot.device.set(robot.subscriber.position.value+robot.device.state.value[6:])')
+runCommandClient('robot.contactLF.keep()')
+runCommandClient('robot.contactRF.keep()')
+
+# plug the SOT
+runCommandClient('plug(robot.sot.control,robot.device.control)')
+
+sleep(5.0)
+runCommandClient('dump_tracer(robot.tracer)')
+
+# --- DISPLAY
+comDes_data = np.loadtxt('/tmp/dg_' + evalCommandClient('robot.taskCom.featureDes.name') + '-errorIN.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')
+
+comErr_data = com_data - comDes_data
+
+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(comErr_data[:,1],'b-')
+plt.plot(comErr_data[:,2],'r-')
+plt.plot(comErr_data[:,3],'g-')
+plt.title('COM error')
+plt.legend(['x','y','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 Gazebo link state publisher...")
+pub.stop()
+sleep(0.1)
+print("Gazebo link state publisher stopped")
+
+
diff --git a/python/sot_talos_balance/test/test_static_sensorfeedback.py b/python/sot_talos_balance/test/test_static_sensorfeedback.py
new file mode 100644
index 0000000000000000000000000000000000000000..5300bbb493f7cbf422456505047a026c2752d3c5
--- /dev/null
+++ b/python/sot_talos_balance/test/test_static_sensorfeedback.py
@@ -0,0 +1,85 @@
+from sot_talos_balance.utils.run_test_utils import run_test, runCommandClient, evalCommandClient
+from time import sleep
+
+import matplotlib.pyplot as plt
+import numpy as np
+
+run_test('appli_static_sensorfeedback.py')
+
+# wait for sensor values to be ready
+raw_input("Wait before plugging the SOT")
+
+# set initial conditions from sensor readings
+runCommandClient('robot.dcm_estimator.c.recompute(0)')
+runCommandClient('robot.taskCom.featureDes.errorIN.value = robot.rdynamic.com.value')
+runCommandClient('robot.device.set(robot.base_estimator.q.value[:6]+robot.device.state.value[6:])')
+runCommandClient('robot.contactLF.keep()')
+runCommandClient('robot.contactRF.keep()')
+
+# plug the SOT
+runCommandClient('plug(robot.sot.control,robot.device.control)')
+
+sleep(5.0)
+runCommandClient('dump_tracer(robot.tracer)')
+
+# --- DISPLAY
+comDes_data = np.loadtxt('/tmp/dg_' + evalCommandClient('robot.taskCom.featureDes.name') + '-errorIN.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')
+
+comErr_data = com_data - comDes_data
+
+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(comErr_data[:,1],'b-')
+plt.plot(comErr_data[:,2],'r-')
+plt.plot(comErr_data[:,3],'g-')
+plt.title('COM error')
+plt.legend(['x','y','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")
+
diff --git a/python/sot_talos_balance/utils/gazebo_utils.py b/python/sot_talos_balance/utils/gazebo_utils.py
index 3ddcca62bae4f4a5f1c4fc384949973909b57014..8de30769ba50309923b29ac2dcd7787fbfb66381 100644
--- a/python/sot_talos_balance/utils/gazebo_utils.py
+++ b/python/sot_talos_balance/utils/gazebo_utils.py
@@ -29,13 +29,13 @@ def quat2list(v):
 
 class GazeboLinkStatePublisher(threading.Thread):
     '''Utility class reading the state of a given link from Gazebo and publishing on a topic.'''
-    def __init__(self, link_name, rate, rpy=True, prefix='/sot'):
+    def __init__(self, link_name, rate, euler='sxyz', prefix='/sot'):
         super(GazeboLinkStatePublisher, self).__init__(name = link_name+"_publisher")
         self.daemon = True
         self.link_name = link_name
         self.rate = rate
         self.prefix = prefix
-        self.rpy = rpy
+        self.euler = euler
         self._stop = False
         rospy.init_node(self.name, anonymous=True)
     def stop(self):
@@ -61,13 +61,11 @@ class GazeboLinkStatePublisher(threading.Thread):
 
             cartesian = vec2list(link_state.pose.position)
             orientation = quat2list(link_state.pose.orientation)
-            if self.rpy:
-                orientation = list(euler_from_quaternion(orientation,'rzyx'))
+            if self.euler:
+                orientation = list(euler_from_quaternion(orientation,self.euler))
             position = cartesian + orientation
-            msg_pos = VectorMsg(position)
 
             velocity = vec2list(link_state.twist.linear) + vec2list(link_state.twist.angular)
-            msg_vel = VectorMsg(velocity)
 
             pub_pos.publish(position)
             pub_vel.publish(velocity)
diff --git a/python/sot_talos_balance/utils/run_test_utils.py b/python/sot_talos_balance/utils/run_test_utils.py
index a0e756a5c5cd4b70abc88e0084670d0eb5ffead4..070fefc2a2dcc5b0214c59cd8b410adbd98e4989 100644
--- a/python/sot_talos_balance/utils/run_test_utils.py
+++ b/python/sot_talos_balance/utils/run_test_utils.py
@@ -11,7 +11,17 @@ from std_srvs.srv import *
 from dynamic_graph_bridge.srv import *
 from dynamic_graph_bridge_msgs.srv import *
 
-runCommandClient = rospy.ServiceProxy('run_command', RunCommand)
+_runCommandClient = rospy.ServiceProxy('run_command', RunCommand)
+
+def runCommandClient(code):
+    out = _runCommandClient(code)
+    if out.standardoutput or out.standarderror:
+        print("command: " + code)
+        if out.standardoutput:
+            print("standardoutput: " + out.standardoutput)
+        if out.standarderror:
+            print("standarderror: "  + out.standarderror)
+    return out
 
 def evalCommandClient(code):
     return eval(runCommandClient(code).result)
@@ -22,10 +32,8 @@ def launch_script(code,title,description = ""):
     rospy.loginfo(code)
     for line in code:
         if line != '' and line[0] != '#':
-            print(line)
             answer = runCommandClient(str(line))
             rospy.logdebug(answer)
-            print(answer)
     rospy.loginfo("...done with "+title)
 
 def run_test(appli):