diff --git a/python/sot_talos_balance/test/appli_dcmZmpControl_distribute.py b/python/sot_talos_balance/test/appli_dcmZmpControl_distribute.py
index 3259b17c8a71f43020d5e473584089c212c43ab4..31ba4e26d2b2b3ecfa9df80261c6ea1eddae8ea2 100644
--- a/python/sot_talos_balance/test/appli_dcmZmpControl_distribute.py
+++ b/python/sot_talos_balance/test/appli_dcmZmpControl_distribute.py
@@ -1,6 +1,8 @@
 from sot_talos_balance.create_entities_utils import *
 import sot_talos_balance.talos.parameter_server_conf   as param_server_conf
+import sot_talos_balance.talos.control_manager_conf    as cm_conf
 import sot_talos_balance.talos.base_estimator_conf     as base_estimator_conf
+import sot_talos_balance.talos.ft_calibration_conf     as ft_conf
 from dynamic_graph.sot.core.meta_tasks_kine import MetaTaskKine6d, MetaTaskKineCom, gotoNd
 from dynamic_graph.sot.core import Task, FeaturePosture
 from dynamic_graph.sot.core.matrix_util import matrixToTuple
@@ -99,14 +101,17 @@ plug(robot.cdc_estimator.dc,estimator.momenta)
 estimator.init()
 robot.estimator = estimator
 
+# --- Force calibration
+robot.ftc = create_ft_calibrator(robot,ft_conf)
+
 # --- ZMP estimation
 zmp_estimator = SimpleZmpEstimator("zmpEst")
 robot.rdynamic.createOpPoint('sole_LF','left_sole_link')
 robot.rdynamic.createOpPoint('sole_RF','right_sole_link')
 plug(robot.rdynamic.sole_LF,zmp_estimator.poseLeft)
 plug(robot.rdynamic.sole_RF,zmp_estimator.poseRight)
-plug(robot.device_filters.ft_LF_filter.x_filtered,zmp_estimator.wrenchLeft)
-plug(robot.device_filters.ft_RF_filter.x_filtered,zmp_estimator.wrenchRight)
+plug(robot.ftc.left_foot_force_out,zmp_estimator.wrenchLeft)
+plug(robot.ftc.right_foot_force_out,zmp_estimator.wrenchRight)
 zmp_estimator.init()
 robot.zmp_estimator = zmp_estimator
 
@@ -135,7 +140,7 @@ dcm_controller.init(dt)
 
 robot.dcm_control = dcm_controller
 
-Ki_dcm = [0.1,0.1,0.1] # this value is employed later
+Ki_dcm = [1.0,1.0,1.0] # this value is employed later
 
 # --- Distribute wrench
 distribute = DistributeWrench('distribute')
@@ -158,7 +163,14 @@ com_admittance_control.setState(comDes,[0.0,0.0,0.0])
 
 robot.com_admittance_control = com_admittance_control
 
-Kp_adm = [0.1,0.1,0.0] # this value is employed later
+Kp_adm = [20.0,10.0,0.0] # this value is employed later
+
+# --- Control Manager
+robot.cm = create_ctrl_manager(cm_conf, dt, robot_name='robot')
+robot.cm.addCtrlMode('sot_input')
+robot.cm.setCtrlMode('all','sot_input')
+robot.cm.addEmergencyStopSIN('zmp')
+robot.cm.addEmergencyStopSIN('distribute')
 
 # -------------------------- SOT CONTROL --------------------------
 
@@ -274,7 +286,10 @@ locals()['keepWaist'] = robot.keepWaist
 # --- SOT solver
 robot.sot = SOT('sot')
 robot.sot.setSize(robot.dynamic.getDimension())
-plug(robot.sot.control,robot.device.control)
+
+# --- Plug SOT control to device through control manager
+plug(robot.sot.control,robot.cm.ctrl_sot_input)
+plug(robot.cm.u_safe,robot.device.control)
 
 robot.sot.push(robot.taskUpperBody.name)
 robot.sot.push(robot.contactRF.task.name)
@@ -321,6 +336,15 @@ create_topic(robot.publisher, robot.distribute, 'wrenchLeft', robot = robot, dat
 create_topic(robot.publisher, robot.distribute, 'wrenchRight', robot = robot, data_type='vector')         # reference right wrench
 create_topic(robot.publisher, robot.distribute, 'wrenchRef', robot = robot, data_type='vector')           # optimized reference wrench
 
+#create_topic(robot.publisher, robot.device, 'forceLLEG', robot = robot, data_type='vector')               # measured left wrench
+#create_topic(robot.publisher, robot.device, 'forceRLEG', robot = robot, data_type='vector')               # measured right wrench
+
+#create_topic(robot.publisher, robot.device_filters.ft_LF_filter, 'x_filtered', robot = robot, data_type='vector') # filtered left wrench
+#create_topic(robot.publisher, robot.device_filters.ft_RF_filter, 'x_filtered', robot = robot, data_type='vector') # filtered right wrench
+
+create_topic(robot.publisher, robot.ftc, 'left_foot_force_out', robot = robot, data_type='vector')  # calibrated left wrench
+create_topic(robot.publisher, robot.ftc, 'right_foot_force_out', robot = robot, data_type='vector') # calibrated right wrench
+
 # --- TRACER
 robot.tracer = TracerRealTime("zmp_tracer")
 robot.tracer.setBufferSize(80*(2**20))
diff --git a/python/sot_talos_balance/test/test_dcmZmpControl_distribute.py b/python/sot_talos_balance/test/test_dcmZmpControl_distribute.py
index c02179d5e0cbe95072feaf841ffd709a988d3c58..ee23ed7ef56705c6f5e4e38d077ff0d785543343 100644
--- a/python/sot_talos_balance/test/test_dcmZmpControl_distribute.py
+++ b/python/sot_talos_balance/test/test_dcmZmpControl_distribute.py
@@ -1,32 +1,25 @@
 '''Test CoM admittance control as described in paper.'''
-from sot_talos_balance.utils.run_test_utils import run_test, runCommandClient, evalCommandClient
+from sot_talos_balance.utils.run_test_utils import *
 from time import sleep
 
-from sot_talos_balance.utils.gazebo_utils import apply_force
-
 import matplotlib.pyplot as plt
 import numpy as np
 
 run_test('appli_dcmZmpControl_distribute.py')
 
-sleep(5.0)
+run_ft_calibration('robot.ftc')
+raw_input("Wait before running the test")
 
 # Connect ZMP reference and reset controllers
 print('Connect ZMP reference')
+runCommandClient('plug(robot.zmp_estimator.emergencyStop,robot.cm.emergencyStop_zmp)')
+runCommandClient('plug(robot.distribute.emergencyStop,robot.cm.emergencyStop_distribute)')
 runCommandClient('plug(robot.distribute.zmpRef,robot.com_admittance_control.zmpDes)')
 runCommandClient('robot.com_admittance_control.setState(comDes,[0.0,0.0,0.0])')
 runCommandClient('robot.com_admittance_control.Kp.value = Kp_adm')
 runCommandClient('robot.dcm_control.resetDcmIntegralError()')
 runCommandClient('robot.dcm_control.Ki.value = Ki_dcm')
 
-#sleep(5.0)
-
-#print('Kick the robot...')
-#apply_force([-1000.0,0,0],0.01)
-#print('... kick!')
-
-#sleep(5.0)
-
 sleep(30.0)
 
 runCommandClient('dump_tracer(robot.tracer)')