Skip to content
GitLab
Commit 3cc951d2 authored by Gabriele Buondonno's avatar Gabriele Buondonno
Browse files

[test_dcm_zmp_control] [test_dcm_zmp_control_distribute] Remove tracers

parent 8a0abc45
Pipeline #5488 failed with stage
in 26 minutes
Hide whitespace changes
Inline Side-by-side
python/sot_talos_balance/test/appli_dcm_zmp_control.py
View file @ 3cc951d2
......@@ -171,7 +171,7 @@ robot.cdc_estimator = cdc_estimator
# --- DCM Estimation
estimator = DummyDcmEstimator("dummy")
estimator.omega.value = omega
plug(robot.wp.omegaDes, estimator.omega)
estimator.mass.value = 1.0
plug(robot.cdc_estimator.c, estimator.com)
plug(robot.cdc_estimator.dc,estimator.momenta)
......@@ -205,7 +205,7 @@ dcm_controller.Kp.value = Kp_dcm
dcm_controller.Ki.value = Ki_dcm
dcm_controller.decayFactor.value = gamma_dcm
dcm_controller.mass.value = mass
dcm_controller.omega.value = omega
plug(robot.wp.omegaDes, dcm_controller.omega)
plug(robot.cdc_estimator.c,dcm_controller.com)
plug(robot.estimator.dcm,dcm_controller.dcm)
......@@ -367,33 +367,33 @@ create_topic(robot.publisher, robot.ftc, 'right_foot_force_out', robot = robot,
create_topic(robot.publisher, robot.dynamic, 'LF', robot = robot, data_type='matrixHomo') # left foot
create_topic(robot.publisher, robot.dynamic, 'RF', robot = robot, data_type='matrixHomo') # right foot
# --- 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))
## --- 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))
addTrace(robot.tracer, robot.wp, 'comDes') # desired CoM
#addTrace(robot.tracer, robot.wp, 'comDes') # desired CoM
addTrace(robot.tracer, robot.cdc_estimator, 'c') # estimated CoM
addTrace(robot.tracer, robot.cdc_estimator, 'dc') # estimated CoM velocity
#addTrace(robot.tracer, robot.cdc_estimator, 'c') # estimated CoM
#addTrace(robot.tracer, robot.cdc_estimator, 'dc') # estimated CoM velocity
addTrace(robot.tracer, robot.com_admittance_control, 'comRef') # reference CoM
addTrace(robot.tracer, robot.dynamic, 'com') # resulting SOT CoM
#addTrace(robot.tracer, robot.com_admittance_control, 'comRef') # reference CoM
#addTrace(robot.tracer, robot.dynamic, 'com') # resulting SOT CoM
addTrace(robot.tracer, robot.dcm_control, 'dcmDes') # desired DCM
addTrace(robot.tracer, robot.estimator, 'dcm') # estimated DCM
#addTrace(robot.tracer, robot.dcm_control, 'dcmDes') # desired DCM
#addTrace(robot.tracer, robot.estimator, 'dcm') # estimated DCM
addTrace(robot.tracer, robot.dcm_control, 'zmpDes') # desired ZMP
addTrace(robot.tracer, robot.dynamic, 'zmp') # SOT ZMP
addTrace(robot.tracer, robot.zmp_estimator, 'zmp') # estimated ZMP
addTrace(robot.tracer, robot.dcm_control, 'zmpRef') # reference ZMP
#addTrace(robot.tracer, robot.dcm_control, 'zmpDes') # desired ZMP
#addTrace(robot.tracer, robot.dynamic, 'zmp') # SOT ZMP
#addTrace(robot.tracer, robot.zmp_estimator, 'zmp') # estimated ZMP
#addTrace(robot.tracer, robot.dcm_control, 'zmpRef') # reference ZMP
addTrace(robot.tracer, robot.ftc, 'left_foot_force_out') # calibrated left wrench
addTrace(robot.tracer, robot.ftc, 'right_foot_force_out') # calibrated right wrench
#addTrace(robot.tracer, robot.ftc, 'left_foot_force_out') # calibrated left wrench
#addTrace(robot.tracer, robot.ftc, 'right_foot_force_out') # calibrated right wrench
addTrace(robot.tracer, robot.dynamic, 'LF') # left foot
addTrace(robot.tracer, robot.dynamic, 'RF') # right foot
#addTrace(robot.tracer, robot.dynamic, 'LF') # left foot
#addTrace(robot.tracer, robot.dynamic, 'RF') # right foot
robot.tracer.start()
#robot.tracer.start()
python/sot_talos_balance/test/appli_dcm_zmp_control_distribute.py
View file @ 3cc951d2
......@@ -404,38 +404,38 @@ create_topic(robot.publisher, robot.ftc, 'right_foot_force_out', robot = robot,
create_topic(robot.publisher, robot.dynamic, 'LF', robot = robot, data_type='matrixHomo') # left foot
create_topic(robot.publisher, robot.dynamic, 'RF', robot = robot, data_type='matrixHomo') # right foot
# --- 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))
## --- 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))
addTrace(robot.tracer, robot.wp, 'comDes') # desired CoM
#addTrace(robot.tracer, robot.wp, 'comDes') # desired CoM
addTrace(robot.tracer, robot.cdc_estimator, 'c') # estimated CoM
addTrace(robot.tracer, robot.cdc_estimator, 'dc') # estimated CoM velocity
#addTrace(robot.tracer, robot.cdc_estimator, 'c') # estimated CoM
#addTrace(robot.tracer, robot.cdc_estimator, 'dc') # estimated CoM velocity
addTrace(robot.tracer, robot.com_admittance_control, 'comRef') # reference CoM
addTrace(robot.tracer, robot.dynamic, 'com') # resulting SOT CoM
#addTrace(robot.tracer, robot.com_admittance_control, 'comRef') # reference CoM
#addTrace(robot.tracer, robot.dynamic, 'com') # resulting SOT CoM
addTrace(robot.tracer, robot.dcm_control, 'dcmDes') # desired DCM
addTrace(robot.tracer, robot.estimator, 'dcm') # estimated DCM
#addTrace(robot.tracer, robot.dcm_control, 'dcmDes') # desired DCM
#addTrace(robot.tracer, robot.estimator, 'dcm') # estimated DCM
addTrace(robot.tracer, robot.dcm_control, 'zmpDes') # desired ZMP
addTrace(robot.tracer, robot.dynamic, 'zmp') # SOT ZMP
addTrace(robot.tracer, robot.zmp_estimator, 'zmp') # estimated ZMP
addTrace(robot.tracer, robot.dcm_control, 'zmpRef') # reference ZMP
#addTrace(robot.tracer, robot.dcm_control, 'zmpDes') # desired ZMP
#addTrace(robot.tracer, robot.dynamic, 'zmp') # SOT ZMP
#addTrace(robot.tracer, robot.zmp_estimator, 'zmp') # estimated ZMP
#addTrace(robot.tracer, robot.dcm_control, 'zmpRef') # reference ZMP
addTrace(robot.tracer, robot.dcm_control, 'wrenchRef') # unoptimized reference wrench
addTrace(robot.tracer, robot.distribute, 'wrenchLeft') # reference left wrench
addTrace(robot.tracer, robot.distribute, 'wrenchRight') # reference right wrench
addTrace(robot.tracer, robot.distribute, 'wrenchRef') # optimized reference wrench
#addTrace(robot.tracer, robot.dcm_control, 'wrenchRef') # unoptimized reference wrench
#addTrace(robot.tracer, robot.distribute, 'wrenchLeft') # reference left wrench
#addTrace(robot.tracer, robot.distribute, 'wrenchRight') # reference right wrench
#addTrace(robot.tracer, robot.distribute, 'wrenchRef') # optimized reference wrench
addTrace(robot.tracer, robot.ftc, 'left_foot_force_out') # calibrated left wrench
addTrace(robot.tracer, robot.ftc, 'right_foot_force_out') # calibrated right wrench
#addTrace(robot.tracer, robot.ftc, 'left_foot_force_out') # calibrated left wrench
#addTrace(robot.tracer, robot.ftc, 'right_foot_force_out') # calibrated right wrench
addTrace(robot.tracer, robot.dynamic, 'LF') # left foot
addTrace(robot.tracer, robot.dynamic, 'RF') # right foot
#addTrace(robot.tracer, robot.dynamic, 'LF') # left foot
#addTrace(robot.tracer, robot.dynamic, 'RF') # right foot
robot.tracer.start()
#robot.tracer.start()
python/sot_talos_balance/test/test_dcm_zmp_control.py
View file @ 3cc951d2
......@@ -97,7 +97,9 @@ else:
else:
print("Not raising the foot")
raw_input("Wait before dumping the data")
#raw_input("Wait before dumping the data")
runCommandClient('dump_tracer(robot.tracer)')
#runCommandClient('dump_tracer(robot.tracer)')
print('Bye!')
python/sot_talos_balance/test/test_dcm_zmp_control_distribute.py
View file @ 3cc951d2
......@@ -5,22 +5,18 @@ from time import sleep
from sys import argv
test_folder, sot_talos_balance_folder = get_file_folder(argv)
use_distribute = ask_for_confirmation("Use output of force distribution?")
print( ("Using" if use_distribute else "Not using") + " output of force distribution" )
run_test('appli_dcm_zmp_control_distribute.py')
run_ft_calibration('robot.ftc')
use_force_distribution = ask_for_confirmation("Use output of force distribution?")
if use_force_distribution:
print("Using output of force distribution")
else:
print("Not using output of force distribution")
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)')
if use_force_distribution:
if use_distribute:
runCommandClient('plug(robot.distribute.emergencyStop,robot.cm.emergencyStop_distribute)')
runCommandClient('plug(robot.distribute.zmpRef,robot.com_admittance_control.zmpDes)')
else:
......@@ -112,7 +108,9 @@ else:
else:
print("Not raising the foot")
raw_input("Wait before dumping the data")
#raw_input("Wait before dumping the data")
#runCommandClient('dump_tracer(robot.tracer)')
runCommandClient('dump_tracer(robot.tracer)')
print('Bye!')
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment