diff --git a/plot/plot_wrenchDistrib.xml b/plot/plot_wrenchDistrib.xml
new file mode 100644
index 0000000000000000000000000000000000000000..f70c034aa8afa388431d7599f5593a073801b30a
--- /dev/null
+++ b/plot/plot_wrenchDistrib.xml
@@ -0,0 +1,107 @@
+<?xml version='1.0' encoding='UTF-8'?>
+<root>
+ <tabbed_widget name="Main Window" parent="main_window">
+  <plotmatrix tab_name="plot" columns="3" rows="2">
+   <plot col="0" row="0">
+    <range bottom="-0.066212" right="51.116000" top="2.714707" left="41.117000"/>
+    <limitY/>
+    <curve G="0" name="/sot/distribute/wrenchLeft/data.0" custom_transform="noTransform" B="255" R="0"/>
+    <curve G="160" name="/sot/ftc/left_foot_force_out/data.0" custom_transform="noTransform" B="164" R="160"/>
+    <transform value="noTransform"/>
+   </plot>
+   <plot col="0" row="1">
+    <range bottom="-2.745565" right="51.116000" top="0.066965" left="41.117000"/>
+    <limitY/>
+    <curve G="0" name="/sot/distribute/wrenchRight/data.0" custom_transform="noTransform" B="255" R="255"/>
+    <curve G="0" name="/sot/ftc/right_foot_force_out/data.0" custom_transform="noTransform" B="255" R="0"/>
+    <transform value="noTransform"/>
+   </plot>
+   <plot col="1" row="0">
+    <range bottom="-39.112365" right="51.116000" top="0.953960" left="41.117000"/>
+    <limitY/>
+    <curve G="128" name="/sot/distribute/wrenchLeft/data.1" custom_transform="noTransform" B="0" R="0"/>
+    <curve G="0" name="/sot/ftc/left_foot_force_out/data.1" custom_transform="noTransform" B="0" R="128"/>
+    <transform value="noTransform"/>
+   </plot>
+   <plot col="1" row="1">
+    <range bottom="-0.952687" right="51.116000" top="39.060148" left="41.117000"/>
+    <limitY/>
+    <curve G="0" name="/sot/distribute/wrenchRight/data.1" custom_transform="noTransform" B="128" R="0"/>
+    <curve G="128" name="/sot/ftc/right_foot_force_out/data.1" custom_transform="noTransform" B="0" R="0"/>
+    <transform value="noTransform"/>
+   </plot>
+   <plot col="2" row="0">
+    <range bottom="400.779123" right="51.116000" top="443.812853" left="41.117000"/>
+    <limitY/>
+    <curve G="0" name="/sot/distribute/wrenchLeft/data.2" custom_transform="noTransform" B="0" R="255"/>
+    <curve G="128" name="/sot/ftc/left_foot_force_out/data.2" custom_transform="noTransform" B="0" R="128"/>
+    <transform value="noTransform"/>
+   </plot>
+   <plot col="2" row="1">
+    <range bottom="442.325429" right="51.116000" top="461.762601" left="41.117000"/>
+    <limitY/>
+    <curve G="128" name="/sot/distribute/wrenchRight/data.2" custom_transform="noTransform" B="128" R="0"/>
+    <curve G="0" name="/sot/ftc/right_foot_force_out/data.2" custom_transform="noTransform" B="0" R="255"/>
+    <transform value="noTransform"/>
+   </plot>
+  </plotmatrix>
+  <currentPlotMatrix index="0"/>
+ </tabbed_widget>
+ <use_relative_time_offset enabled="1"/>
+ <Plugins>
+  <DataLoad_CSV>
+   <default time_axis=""/>
+  </DataLoad_CSV>
+  <DataLoad_ROS_bags>
+   <selected_topics list=""/>
+  </DataLoad_ROS_bags>
+  <DataLoad_ULog>
+   <no_params/>
+  </DataLoad_ULog>
+  <ROS_Topic_Streamer>
+   <selected_topics list="/sot/distribute/wrenchLeft;/sot/distribute/wrenchRight;/sot/ftc/left_foot_force_out;/sot/ftc/right_foot_force_out"/>
+  </ROS_Topic_Streamer>
+  <RosoutPublisherROS/>
+  <TopicPublisherROS/>
+ </Plugins>
+ <previouslyLoadedStreamer name="ROS_Topic_Streamer"/>
+ <customMathEquations/>
+ <snippets>
+  <snippet name="1st_derivative">
+   <global>var prevX = 0
+var prevY = 0</global>
+   <equation>dx = time - prevX
+dy = value - prevY
+prevX = time
+prevY = value
+
+return dy/dx</equation>
+  </snippet>
+  <snippet name="1st_order_lowpass">
+   <global>var prevY = 0
+var alpha = 0.1</global>
+   <equation>prevY = alpha * value + (1.-alpha) * prevY
+
+return prevY</equation>
+  </snippet>
+  <snippet name="sum_A_B">
+   <global></global>
+   <equation>return $$PLOT_A$$ + $$PLOT_B$$</equation>
+  </snippet>
+  <snippet name="yaw_from_quaternion">
+   <global>// source: https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles
+
+function quaternionToYaw(x, y, z, w)
+{
+  // yaw (z-axis rotation)
+  t1 = 2.0 * (w * z + x * y);
+  t2 = 1.0 - 2.0 * (y * y + z * z);
+  yaw = Math.atan2(t1, t2);
+
+  return yaw
+}</global>
+   <equation>return quaternionToYaw(x, y, z, w);</equation>
+  </snippet>
+ </snippets>
+</root>
+
diff --git a/python/sot_talos_balance/test/appli_feet_admittance.py b/python/sot_talos_balance/test/appli_feet_admittance.py
index bc604415f2472cd4fc1c43ddf1bbd72dce1cc627..53f19b3e63266f6e39c192e13a6fe87ef0ddb764 100644
--- a/python/sot_talos_balance/test/appli_feet_admittance.py
+++ b/python/sot_talos_balance/test/appli_feet_admittance.py
@@ -8,6 +8,7 @@ from dynamic_graph.sot.core import Task, FeaturePosture
 from dynamic_graph.sot.core.matrix_util import matrixToTuple
 from dynamic_graph import plug
 from dynamic_graph.sot.core import SOT
+from dynamic_graph.sot.core import operator
 from math import sqrt
 import numpy as np
 
@@ -42,19 +43,23 @@ robot.dynamic.WT.recompute(0)
 
 # --- Trajectory generators
 robot.comTrajGen = create_com_trajectory_generator(dt,robot)
-robot.lfPosTrajGen  = create_position_trajectory_generator(dt, robot, 'LF')
-robot.rfPosTrajGen  = create_position_trajectory_generator(dt, robot, 'RF')
+
+robot.lfTrajGen  = create_pose_rpy_trajectory_generator(dt, robot, 'LF')
+robot.lfToMatrix = PoseRollPitchYawToMatrixHomo('lf2m')
+plug(robot.lfTrajGen.x, robot.lfToMatrix.sin)
+
+robot.rfTrajGen  = create_pose_rpy_trajectory_generator(dt, robot, 'RF')
+robot.rfToMatrix = PoseRollPitchYawToMatrixHomo('rf2m')
+plug(robot.rfTrajGen.x, robot.rfToMatrix.sin)
 
 # --- Interface with controller entities
 
 wp = DummyWalkingPatternGenerator('dummy_wp')
 wp.init()
 wp.omega.value = omega
-wp.waist.value = robot.dynamic.WT.value          # wait receives a full homogeneous matrix, but only the rotational part is controlled
-wp.footLeft.value = robot.dynamic.LF.value
-wp.footRight.value = robot.dynamic.RF.value
-plug(robot.lfPosTrajGen.x, wp.footPositionLeft)  # if given, footPositionLeft overrides the translation in footLeft
-plug(robot.rfPosTrajGen.x, wp.footPositionRight) # if given, footPositionRight overrides the translation in footRight
+wp.waist.value = robot.dynamic.WT.value          # waist receives a full homogeneous matrix, but only the rotational part is controlled
+plug(robot.lfToMatrix.sout, wp.footLeft)
+plug(robot.rfToMatrix.sout, wp.footRight)
 plug(robot.comTrajGen.x, wp.com)
 plug(robot.comTrajGen.dx, wp.vcom)
 plug(robot.comTrajGen.ddx, wp.acom)
@@ -134,15 +139,59 @@ robot.zmp_estimator = zmp_estimator
 
 # -------------------------- ADMITTANCE CONTROL --------------------------
 
+# --- CoM control
+Kp_com = [0.]*2 + [4.]
+
+comErr = operator.Substract_of_vector('comErr')
+plug(robot.wp.comDes, comErr.sin1)
+plug(robot.cdc_estimator.c, comErr.sin2)
+robot.comErr = comErr
+
+comControl = operator.Multiply_of_vector('comControl')
+comControl.sin0.value = Kp_com
+plug(robot.comErr.sout, comControl.sin1)
+robot.comControl = comControl
+
+forceControl = operator.Add_of_vector('forceControl')
+plug(robot.comControl.sout, forceControl.sin1)
+forceControl.sin2.value = [0.0, 0.0, mass*g]
+robot.forceControl = forceControl
+
+wrenchControl = operator.Mix_of_vector('wrenchControl')
+wrenchControl.setSignalNumber(3)
+wrenchControl.addSelec(1, 0, 3)
+wrenchControl.addSelec(2, 3, 3)
+wrenchControl.default.value = [0.0]*6
+plug(robot.forceControl.sout, wrenchControl.signal("sin1"))
+wrenchControl.signal("sin2").value = [0.0]*3
+robot.wrenchControl = wrenchControl
+
+# --- Distribute wrench
+distribute = SimpleDistributeWrench('distribute')
+plug(robot.e2q.quaternion, distribute.q)
+distribute.rho.value = 0.5
+plug(robot.wrenchControl.sout, distribute.wrenchDes)
+distribute.init(robot_name)
+robot.distribute = distribute
+
+# --- Trick to command both feet controllers at the same time
+robot.admBF_Kp = Selec_of_vector('admBF_Kp')
+robot.admBF_Kp.selec(0,6)
+robot.admBF_Kp.sin.value = [0.0]*2 + [3.0] + [0.0]*3
+
+robot.admBF_dqSaturation = Selec_of_vector('admBF_dqSaturation')
+robot.admBF_dqSaturation.selec(0,6)
+robot.admBF_dqSaturation.sin.value = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
+
 # --- Left foot
 controller = AdmittanceControllerEndEffector("admLF")
 
 plug(robot.ftc.left_foot_force_out, controller.force)
 plug(robot.e2q.quaternion, controller.q)
-controller.Kp.value = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
-controller.Kd.value = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
-controller.w_forceDes.value = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
-controller.dqSaturation.value = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
+plug(robot.admBF_Kp.sout, controller.Kp)
+controller.Kd.value = [1.0] * 6
+plug(robot.distribute.wrenchLeft, controller.w_forceDes)
+plug(robot.admBF_dqSaturation.sout, controller.dqSaturation)
 
 controller.init(dt, "right_sole_link", 'leg_right_6_joint')
 
@@ -153,10 +202,10 @@ controller = AdmittanceControllerEndEffector("admRF")
 
 plug(robot.ftc.right_foot_force_out, controller.force)
 plug(robot.e2q.quaternion, controller.q)
-controller.Kp.value = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
-controller.Kd.value = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
-controller.w_forceDes.value = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
-controller.dqSaturation.value = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
+plug(robot.admBF_Kp.sout, controller.Kp)
+controller.Kd.value = [1.0] * 6
+plug(robot.distribute.wrenchRight, controller.w_forceDes)
+plug(robot.admBF_dqSaturation.sout, controller.dqSaturation)
 
 controller.init(dt, "left_sole_link", 'leg_right_6_joint')
 
@@ -245,7 +294,7 @@ locals()['contactRF'] = robot.contactRF
 robot.taskCom = MetaTaskKineCom(robot.dynamic)
 plug(robot.wp.comDes,robot.taskCom.featureDes.errorIN)
 robot.taskCom.task.controlGain.value = 10
-robot.taskCom.task.setWithDerivative(True)
+robot.taskCom.feature.selec.value = '011' # needed ?
 
 # --- Waist
 robot.keepWaist = MetaTaskKine6d('keepWaist',robot.dynamic,'WT',robot.OperationalPointsMap['waist'])
@@ -348,6 +397,10 @@ create_topic(robot.publisher, robot.zmp_estimator, 'zmp', robot = robot, data_ty
 #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.distribute, 'wrenchLeft', robot = robot, data_type='vector')
+create_topic(robot.publisher, robot.distribute, 'wrenchRight', robot = robot, data_type='vector')
+
+
 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
 
diff --git a/python/sot_talos_balance/test/test_feet_admittance.py b/python/sot_talos_balance/test/test_feet_admittance.py
index 2e774304983b605dd316436bfcb6cf60a1a8edde..116cfa1b490e4851d5d7524632a31aff85ccfb0e 100644
--- a/python/sot_talos_balance/test/test_feet_admittance.py
+++ b/python/sot_talos_balance/test/test_feet_admittance.py
@@ -7,6 +7,9 @@ run_test('appli_feet_admittance.py')
 run_ft_calibration('robot.ftc')
 raw_input("Wait before running the test")
 
+print('Set saturation value')
+runCommandClient('robot.admBF_dqSaturation.sin.value = [0.0, 0.0, 0.01, 0.0, 0.0, 0.0]')
+
 raw_input("Wait before dumping the data")
 
 runCommandClient('dump_tracer(robot.tracer)')