diff --git a/CMakeLists.txt b/CMakeLists.txt
index c8983c98967220fc883c4f81a898b8591748ab55..74c00dfb2ebefbc31a0dd7500786db94beeceeb2 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -163,6 +163,8 @@ IF(BUILD_PYTHON_INTERFACE)
     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_com.py
+                  python/${SOTTALOSBALANCE_PYNAME}/test/appli_static_com.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
diff --git a/python/sot_talos_balance/create_entities_utils.py b/python/sot_talos_balance/create_entities_utils.py
index b931684b5f78e252e6c605416b53c92d3521f173..047ad0d0a9b2ad7296c02912caf53175904381a8 100644
--- a/python/sot_talos_balance/create_entities_utils.py
+++ b/python/sot_talos_balance/create_entities_utils.py
@@ -273,8 +273,8 @@ def create_topic(rospub, entity, signalName, robot=None, data_type='vector'):
 def create_dummy_dcm_estimator(robot):
     from math import sqrt
     estimator = DummyDcmEstimator("dummy")
-    mass = robot.dynamic.data.mass[0]
     robot.dynamic.com.recompute(0)
+    mass = robot.dynamic.data.mass[0]
     h = robot.dynamic.com.value[2]
     g = 9.81
     omega = sqrt(g/h)
@@ -302,8 +302,8 @@ def create_com_admittance_controller(Kp,dt,robot):
 def create_dcm_controller(Kp,Ki,dt,robot,dcmSignal):
     from math import sqrt
     controller = DcmController("dcmCtrl")
-    mass = robot.dynamic.data.mass[0]
     robot.dynamic.com.recompute(0)
+    mass = robot.dynamic.data.mass[0]
     h = robot.dynamic.com.value[2]
     g = 9.81
     omega = sqrt(g/h)
@@ -327,8 +327,8 @@ def create_dcm_controller(Kp,Ki,dt,robot,dcmSignal):
 def create_dcm_com_controller(Kp,Ki,dt,robot,dcmSignal):
     from math import sqrt
     controller = DcmComController("dcmComCtrl")
-    mass = robot.dynamic.data.mass[0]
     robot.dynamic.com.recompute(0)
+    mass = robot.dynamic.data.mass[0]
     h = robot.dynamic.com.value[2]
     g = 9.81
     omega = sqrt(g/h)
diff --git a/python/sot_talos_balance/test/appli_dcmZmpControl.py b/python/sot_talos_balance/test/appli_dcmZmpControl.py
new file mode 100644
index 0000000000000000000000000000000000000000..eafcec350ee1655122eb75e357cad8426ad7b40d
--- /dev/null
+++ b/python/sot_talos_balance/test/appli_dcmZmpControl.py
@@ -0,0 +1,75 @@
+'''Test CoM admittance control as described in paper.'''
+from sot_talos_balance.create_entities_utils import create_com_admittance_controller, create_dummy_dcm_estimator, create_dcm_controller
+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 time import sleep
+
+from dynamic_graph.tracer_real_time import TracerRealTime
+from sot_talos_balance.create_entities_utils import addTrace, dump_tracer
+
+dt = robot.timeStep;
+
+# --- Dummy estimator
+robot.estimator = create_dummy_dcm_estimator(robot)
+
+# --- DCM controller
+Kp_dcm = [500.0,500.0,500.0]
+Ki_dcm = [0.0,0.0,0.0]
+robot.dcm_control = create_dcm_controller(Kp_dcm,Ki_dcm,dt,robot,robot.estimator.dcm)
+
+# --- Admittance controller
+Kp_adm = [0.0,0.0,0.0]
+robot.com_admittance_control = create_com_admittance_controller(Kp_adm,dt,robot)
+
+# --- 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(300)
+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(300)
+robot.contactRF.keep()
+locals()['contactRF'] = robot.contactRF
+
+# --- COM
+robot.taskCom = MetaTaskKineCom(robot.dynamic)
+robot.dynamic.com.recompute(0)
+plug(robot.com_admittance_control.comRef,robot.taskCom.featureDes.errorIN)
+plug(robot.com_admittance_control.dcomRef,robot.taskCom.featureDes.errordotIN)
+robot.taskCom.task.controlGain.value = 0
+robot.taskCom.task.setWithDerivative(True)
+
+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.contactLF.task.name)
+robot.sot.push(robot.taskCom.task.name)
+robot.device.control.recompute(0)
+
+# --- TRACER
+robot.tracer = TracerRealTime("zmp_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}.zmpRef'.format(robot.dcm_control.name))
+robot.device.after.addSignal('{0}.zmp'.format(robot.dynamic.name)) # why needed?
+robot.device.after.addSignal('{0}.comRef'.format(robot.com_admittance_control.name)) # why needed?
+
+addTrace(robot.tracer, robot.dynamic, 'zmp')
+addTrace(robot.tracer, robot.dcm_control, 'zmpRef')
+addTrace(robot.tracer, robot.estimator, 'dcm')
+addTrace(robot.tracer, robot.dynamic, 'com')
+addTrace(robot.tracer, robot.com_admittance_control, 'comRef')
+
+# SIMULATION
+
+robot.tracer.start()
+
diff --git a/python/sot_talos_balance/test/appli_static_com.py b/python/sot_talos_balance/test/appli_static_com.py
new file mode 100644
index 0000000000000000000000000000000000000000..902ce8abb5cdabeda44c52441f613e42fca66852
--- /dev/null
+++ b/python/sot_talos_balance/test/appli_static_com.py
@@ -0,0 +1,79 @@
+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.dynamic)
+robot.taskPos.featureDes.errorIN.value = robot.dynamic.position.value
+robot.taskPos.task.controlGain.value = 100
+
+# --- 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 = 100
+
+# --- 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
+
+# --- 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_dcmZmpControl.py b/python/sot_talos_balance/test/test_dcmZmpControl.py
index 02bc8fa1b49dbd428712872cfd464f3fdb9cd380..62bdf99c5945107941b244f4fd14e4404cec6633 100644
--- a/python/sot_talos_balance/test/test_dcmZmpControl.py
+++ b/python/sot_talos_balance/test/test_dcmZmpControl.py
@@ -1,142 +1,70 @@
-'''Test CoM admittance control as described in paper.'''
-from sot_talos_balance.create_entities_utils import create_com_admittance_controller, create_dummy_dcm_estimator, create_dcm_controller
-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 time import sleep
-import sys
-import os
 
-from dynamic_graph.tracer_real_time import TracerRealTime
-from sot_talos_balance.create_entities_utils import addTrace, dump_tracer
+from sot_talos_balance.utils.gazebo_utils import apply_force
+
 import matplotlib.pyplot as plt
 import numpy as np
 
-from sot_talos_balance.utils.gazebo_utils import apply_force
-
-def main(robot):
-    dt = robot.timeStep;
-
-    # --- 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 = 0
-    robot.taskCom.task.setWithDerivative(True)
-
-    # --- Dummy estimator
-    robot.estimator = create_dummy_dcm_estimator(robot)
-
-    # --- DCM controller
-    Kp_dcm = [500.0,500.0,500.0]
-    Ki_dcm = [0.0,0.0,0.0]
-    robot.dcm_control = create_dcm_controller(Kp_dcm,Ki_dcm,dt,robot,robot.estimator.dcm)
-
-    # --- Admittance controller
-    Kp_adm = [0.0,0.0,0.0]
-    robot.com_admittance_control = create_com_admittance_controller(Kp_adm,dt,robot)
-
-    # --- 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(300)
-    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(300)
-    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.contactLF.task.name)
-    robot.sot.push(robot.taskCom.task.name)
-    robot.device.control.recompute(0)
-
-    # --- TRACER
-    robot.tracer = TracerRealTime("zmp_tracer")
-    robot.tracer.setBufferSize(80*(2**20))
-    robot.tracer.open('/tmp','dg_','.dat')
-    robot.device.after.addSignal('{0}.triger'.format(robot.tracer.name))
-
-    addTrace(robot.tracer, robot.dynamic, 'zmp')
-    addTrace(robot.tracer, robot.dcm_control, 'zmpRef')
-    addTrace(robot.tracer, robot.estimator, 'dcm')
-    addTrace(robot.tracer, robot.dynamic, 'com')
-    addTrace(robot.tracer, robot.com_admittance_control, 'comRef')
-
-    # SIMULATION
-
-    # begin with constant references
-    plug(robot.com_admittance_control.comRef,robot.taskCom.featureDes.errorIN)
-    plug(robot.com_admittance_control.dcomRef,robot.taskCom.featureDes.errordotIN)
-    sleep(1.0)
-    os.system("rosservice call \start_dynamic_graph")
-    sleep(2.0)
-
-    # connect ZMP control signal and reset controllers
-    plug(robot.dcm_control.zmpRef,robot.com_admittance_control.zmpDes)
-
-    robot.com_admittance_control.setState(robot.dynamic.com.value,[0.0,0.0,0.0])
-    robot.com_admittance_control.Kp.value = [10.0,10.0,0.0]
-    robot.dcm_control.resetDcmIntegralError()
-    robot.dcm_control.Ki.value = [1.0,1.0,0.0]
-
-    robot.tracer.start()
-
-    sleep(5.0)
-
-    # kick the robot on the chest to test its stability
-    apply_force([-1000.0,0,0],0.01)
-
-    sleep(5.0)
-
-    dump_tracer(robot.tracer)
-
-	  # --- DISPLAY
-    dcm_data = np.loadtxt('/tmp/dg_'+robot.estimator.name+'-dcm.dat')
-    zmp_data = np.loadtxt('/tmp/dg_'+robot.dynamic.name+'-zmp.dat')
-    zmpDes_data = np.loadtxt('/tmp/dg_'+robot.dcm_control.name+'-zmpRef.dat')
-    com_data = np.loadtxt('/tmp/dg_'+robot.dynamic.name+'-com.dat')
-    comDes_data = np.loadtxt('/tmp/dg_'+robot.com_admittance_control.name+'-comRef.dat')
-
-    plt.figure()
-    plt.plot(dcm_data[:,1],'b-')
-    plt.plot(dcm_data[:,2],'r-')
-    plt.title('DCM')
-    plt.legend(['x','y'])
-
-    plt.figure()
-    plt.plot(com_data[:,1],'b-')
-    plt.plot(comDes_data[:,1],'b--')
-    plt.plot(com_data[:,2],'r-')
-    plt.plot(comDes_data[:,2],'r--')
-    plt.plot(com_data[:,3],'g-')
-    plt.plot(comDes_data[:,3],'g--')
-    plt.title('COM real vs desired')
-    plt.legend(['Real x','Desired x','Real y','Desired y','Real z','Desired z'])
-
-    plt.figure()
-    plt.plot(zmp_data[:,1],'b-')
-    plt.plot(zmpDes_data[:,1],'b--')
-    plt.plot(zmp_data[:,2],'r-')
-    plt.plot(zmpDes_data[:,2],'r--')
-    plt.title('ZMP real vs desired')
-    plt.legend(['Real x','Desired x','Real y','Desired y'])
-
-    plt.figure()
-    plt.plot(zmp_data[:,1] - zmpDes_data[:,1],'b-')
-    plt.plot(zmp_data[:,2] - zmpDes_data[:,2],'r-')
-    plt.title('ZMP error')
-    plt.legend(['Error on x','Error on y'])
-
-    plt.show()
+run_test('appli_dcmZmpControl.py')
+
+sleep(5.0)
+
+# connect ZMP control signal and reset controllers
+runCommandClient('plug(robot.dcm_control.zmpRef,robot.com_admittance_control.zmpDes)')
+runCommandClient('robot.com_admittance_control.setState(robot.dynamic.com.value,[0.0,0.0,0.0])')
+runCommandClient('robot.com_admittance_control.Kp.value = [20.0,10.0,0.0]')
+runCommandClient('robot.dcm_control.resetDcmIntegralError()')
+runCommandClient('robot.dcm_control.Ki.value = [1.0,1.0,0.0]')
+
+sleep(5.0)
+
+print('Kick the robot...')
+apply_force([-1000.0,0,0],0.01)
+print('... kick!')
+
+sleep(5.0)
+
+runCommandClient('dump_tracer(robot.tracer)')
+
+# --- DISPLAY
+dcm_data    = np.loadtxt('/tmp/dg_' + evalCommandClient('robot.estimator.name')+ '-dcm.dat')
+zmp_data    = np.loadtxt('/tmp/dg_' + evalCommandClient('robot.dynamic.name') + '-zmp.dat')
+zmpDes_data = np.loadtxt('/tmp/dg_' + evalCommandClient('robot.dcm_control.name') + '-zmpRef.dat')
+com_data    = np.loadtxt('/tmp/dg_' + evalCommandClient('robot.dynamic.name') + '-com.dat')
+comDes_data = np.loadtxt('/tmp/dg_' + evalCommandClient('robot.com_admittance_control.name') + '-comRef.dat')
+
+plt.ion()
+
+plt.figure()
+plt.plot(dcm_data[:,1],'b-')
+plt.plot(dcm_data[:,2],'r-')
+plt.title('DCM')
+plt.legend(['x','y'])
+
+plt.figure()
+plt.plot(com_data[:,1],'b-')
+plt.plot(comDes_data[:,1],'b--')
+plt.plot(com_data[:,2],'r-')
+plt.plot(comDes_data[:,2],'r--')
+plt.plot(com_data[:,3],'g-')
+plt.plot(comDes_data[:,3],'g--')
+plt.title('COM real vs desired')
+plt.legend(['Real x','Desired x','Real y','Desired y','Real z','Desired z'])
+
+plt.figure()
+plt.plot(zmp_data[:,1],'b-')
+plt.plot(zmpDes_data[:,1],'b--')
+plt.plot(zmp_data[:,2],'r-')
+plt.plot(zmpDes_data[:,2],'r--')
+plt.title('ZMP real vs desired')
+plt.legend(['Real x','Desired x','Real y','Desired y'])
+
+plt.figure()
+plt.plot(zmp_data[:,1] - zmpDes_data[:,1],'b-')
+plt.plot(zmp_data[:,2] - zmpDes_data[:,2],'r-')
+plt.title('ZMP error')
+plt.legend(['Error on x','Error on y'])
+
+raw_input("Wait before leaving the simulation")
 
diff --git a/python/sot_talos_balance/test/test_static_com.py b/python/sot_talos_balance/test/test_static_com.py
new file mode 100644
index 0000000000000000000000000000000000000000..67b7c8bbc0353458615c5aa8fbba78c6a1a8229b
--- /dev/null
+++ b/python/sot_talos_balance/test/test_static_com.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_com.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")
+
+