Skip to content
Snippets Groups Projects
Commit 51ffe3ed authored by Gabriele Buondonno's avatar Gabriele Buondonno
Browse files

Slightly modify test_dcmZmpControl

parent 4389280d
No related branches found
No related tags found
No related merge requests found
......@@ -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
......
'''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
......
'''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')
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment