Slightly modify test_dcmZmpControl

CMakeLists.txt

@@ -181,6 +181,7 @@ IF(BUILD_PYTHON_INTERFACE)
                   python/${SOTTALOSBALANCE_PYNAME}/test/test_dcmComControl.py
                   python/${SOTTALOSBALANCE_PYNAME}/test/test_dcmComZmpControl.py
                   python/${SOTTALOSBALANCE_PYNAME}/test/test_dcmZmpControl.py
+                  python/${SOTTALOSBALANCE_PYNAME}/test/appli_dcmZmpControl.py
                   python/${SOTTALOSBALANCE_PYNAME}/test/test_comAdmittance.py
                   python/${SOTTALOSBALANCE_PYNAME}/test/test_admittance_single_joint.py
                   python/${SOTTALOSBALANCE_PYNAME}/test/appli_admittance_single_joint.py

python/sot_talos_balance/test/appli_dcmZmpControl.py

-'''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
@@ -16,12 +14,12 @@ 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)
+Ki_dcm = [1.0,1.0,0.0] # to be set later
+robot.dcm_control = create_dcm_controller(Kp_dcm,[0.0]*3,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)
+Kp_adm = [20.0,10.0,0.0] # to be set later
+robot.com_admittance_control = create_com_admittance_controller([0.0]*3,dt,robot)
 
 # --- CONTACTS
 #define contactLF and contactRF

python/sot_talos_balance/test/test_dcmZmpControl.py

+'''Test CoM admittance control as described in paper.'''
 from sot_talos_balance.utils.run_test_utils import run_test, runCommandClient, evalCommandClient
 from time import sleep
 
@@ -10,12 +11,13 @@ run_test('appli_dcmZmpControl.py')
 
 sleep(5.0)
 
-# connect ZMP control signal and reset controllers
+# Connect ZMP reference and reset controllers
+print('Connect ZMP reference')
 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.com_admittance_control.Kp.value = Kp_adm')
 runCommandClient('robot.dcm_control.resetDcmIntegralError()')
-runCommandClient('robot.dcm_control.Ki.value = [1.0,1.0,0.0]')
+runCommandClient('robot.dcm_control.Ki.value = Ki_dcm')
 
 sleep(5.0)
 
@@ -28,7 +30,7 @@ sleep(5.0)
 runCommandClient('dump_tracer(robot.tracer)')
 
 # --- DISPLAY
-dcm_data    = np.loadtxt('/tmp/dg_' + evalCommandClient('robot.estimator.name')+ '-dcm.dat')
+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')