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
...@@ -171,7 +171,7 @@ robot.cdc_estimator = cdc_estimator ...@@ -171,7 +171,7 @@ robot.cdc_estimator = cdc_estimator
# --- DCM Estimation # --- DCM Estimation
estimator = DummyDcmEstimator("dummy") estimator = DummyDcmEstimator("dummy")
estimator.omega.value = omega plug(robot.wp.omegaDes, estimator.omega)
estimator.mass.value = 1.0 estimator.mass.value = 1.0
plug(robot.cdc_estimator.c, estimator.com) plug(robot.cdc_estimator.c, estimator.com)
plug(robot.cdc_estimator.dc,estimator.momenta) plug(robot.cdc_estimator.dc,estimator.momenta)
...@@ -205,7 +205,7 @@ dcm_controller.Kp.value = Kp_dcm ...@@ -205,7 +205,7 @@ dcm_controller.Kp.value = Kp_dcm
dcm_controller.Ki.value = Ki_dcm dcm_controller.Ki.value = Ki_dcm
dcm_controller.decayFactor.value = gamma_dcm dcm_controller.decayFactor.value = gamma_dcm
dcm_controller.mass.value = mass 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.cdc_estimator.c,dcm_controller.com)
plug(robot.estimator.dcm,dcm_controller.dcm) plug(robot.estimator.dcm,dcm_controller.dcm)
...@@ -367,33 +367,33 @@ create_topic(robot.publisher, robot.ftc, 'right_foot_force_out', robot = robot, ...@@ -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, 'LF', robot = robot, data_type='matrixHomo') # left foot
create_topic(robot.publisher, robot.dynamic, 'RF', robot = robot, data_type='matrixHomo') # right foot create_topic(robot.publisher, robot.dynamic, 'RF', robot = robot, data_type='matrixHomo') # right foot
# --- TRACER ## --- TRACER
robot.tracer = TracerRealTime("com_tracer") #robot.tracer = TracerRealTime("com_tracer")
robot.tracer.setBufferSize(80*(2**20)) #robot.tracer.setBufferSize(80*(2**20))
robot.tracer.open('/tmp','dg_','.dat') #robot.tracer.open('/tmp','dg_','.dat')
robot.device.after.addSignal('{0}.triger'.format(robot.tracer.name)) #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, 'c') # estimated CoM
addTrace(robot.tracer, robot.cdc_estimator, 'dc') # estimated CoM velocity #addTrace(robot.tracer, robot.cdc_estimator, 'dc') # estimated CoM velocity
addTrace(robot.tracer, robot.com_admittance_control, 'comRef') # reference CoM #addTrace(robot.tracer, robot.com_admittance_control, 'comRef') # reference CoM
addTrace(robot.tracer, robot.dynamic, 'com') # resulting SOT CoM #addTrace(robot.tracer, robot.dynamic, 'com') # resulting SOT CoM
addTrace(robot.tracer, robot.dcm_control, 'dcmDes') # desired DCM #addTrace(robot.tracer, robot.dcm_control, 'dcmDes') # desired DCM
addTrace(robot.tracer, robot.estimator, 'dcm') # estimated DCM #addTrace(robot.tracer, robot.estimator, 'dcm') # estimated DCM
addTrace(robot.tracer, robot.dcm_control, 'zmpDes') # desired ZMP #addTrace(robot.tracer, robot.dcm_control, 'zmpDes') # desired ZMP
addTrace(robot.tracer, robot.dynamic, 'zmp') # SOT ZMP #addTrace(robot.tracer, robot.dynamic, 'zmp') # SOT ZMP
addTrace(robot.tracer, robot.zmp_estimator, 'zmp') # estimated 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, 'zmpRef') # reference ZMP
addTrace(robot.tracer, robot.ftc, 'left_foot_force_out') # calibrated left 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, 'right_foot_force_out') # calibrated right wrench
addTrace(robot.tracer, robot.dynamic, 'LF') # left foot #addTrace(robot.tracer, robot.dynamic, 'LF') # left foot
addTrace(robot.tracer, robot.dynamic, 'RF') # right foot #addTrace(robot.tracer, robot.dynamic, 'RF') # right foot
robot.tracer.start() #robot.tracer.start()
...@@ -404,38 +404,38 @@ create_topic(robot.publisher, robot.ftc, 'right_foot_force_out', robot = robot, ...@@ -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, 'LF', robot = robot, data_type='matrixHomo') # left foot
create_topic(robot.publisher, robot.dynamic, 'RF', robot = robot, data_type='matrixHomo') # right foot create_topic(robot.publisher, robot.dynamic, 'RF', robot = robot, data_type='matrixHomo') # right foot
# --- TRACER ## --- TRACER
robot.tracer = TracerRealTime("com_tracer") #robot.tracer = TracerRealTime("com_tracer")
robot.tracer.setBufferSize(80*(2**20)) #robot.tracer.setBufferSize(80*(2**20))
robot.tracer.open('/tmp','dg_','.dat') #robot.tracer.open('/tmp','dg_','.dat')
robot.device.after.addSignal('{0}.triger'.format(robot.tracer.name)) #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, 'c') # estimated CoM
addTrace(robot.tracer, robot.cdc_estimator, 'dc') # estimated CoM velocity #addTrace(robot.tracer, robot.cdc_estimator, 'dc') # estimated CoM velocity
addTrace(robot.tracer, robot.com_admittance_control, 'comRef') # reference CoM #addTrace(robot.tracer, robot.com_admittance_control, 'comRef') # reference CoM
addTrace(robot.tracer, robot.dynamic, 'com') # resulting SOT CoM #addTrace(robot.tracer, robot.dynamic, 'com') # resulting SOT CoM
addTrace(robot.tracer, robot.dcm_control, 'dcmDes') # desired DCM #addTrace(robot.tracer, robot.dcm_control, 'dcmDes') # desired DCM
addTrace(robot.tracer, robot.estimator, 'dcm') # estimated DCM #addTrace(robot.tracer, robot.estimator, 'dcm') # estimated DCM
addTrace(robot.tracer, robot.dcm_control, 'zmpDes') # desired ZMP #addTrace(robot.tracer, robot.dcm_control, 'zmpDes') # desired ZMP
addTrace(robot.tracer, robot.dynamic, 'zmp') # SOT ZMP #addTrace(robot.tracer, robot.dynamic, 'zmp') # SOT ZMP
addTrace(robot.tracer, robot.zmp_estimator, 'zmp') # estimated 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, 'zmpRef') # reference ZMP
addTrace(robot.tracer, robot.dcm_control, 'wrenchRef') # unoptimized 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, 'wrenchLeft') # reference left wrench
addTrace(robot.tracer, robot.distribute, 'wrenchRight') # reference right wrench #addTrace(robot.tracer, robot.distribute, 'wrenchRight') # reference right wrench
addTrace(robot.tracer, robot.distribute, 'wrenchRef') # optimized reference 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, 'left_foot_force_out') # calibrated left wrench
addTrace(robot.tracer, robot.ftc, 'right_foot_force_out') # calibrated right 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, 'LF') # left foot
addTrace(robot.tracer, robot.dynamic, 'RF') # right foot #addTrace(robot.tracer, robot.dynamic, 'RF') # right foot
robot.tracer.start() #robot.tracer.start()
...@@ -97,7 +97,9 @@ else: ...@@ -97,7 +97,9 @@ else:
else: else:
print("Not raising the foot") 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!')
...@@ -5,22 +5,18 @@ from time import sleep ...@@ -5,22 +5,18 @@ from time import sleep
from sys import argv from sys import argv
test_folder, sot_talos_balance_folder = get_file_folder(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_test('appli_dcm_zmp_control_distribute.py')
run_ft_calibration('robot.ftc') 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") raw_input("Wait before running the test")
# Connect ZMP reference and reset controllers # Connect ZMP reference and reset controllers
print('Connect ZMP reference') print('Connect ZMP reference')
runCommandClient('plug(robot.zmp_estimator.emergencyStop,robot.cm.emergencyStop_zmp)') 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.emergencyStop,robot.cm.emergencyStop_distribute)')
runCommandClient('plug(robot.distribute.zmpRef,robot.com_admittance_control.zmpDes)') runCommandClient('plug(robot.distribute.zmpRef,robot.com_admittance_control.zmpDes)')
else: else:
...@@ -112,7 +108,9 @@ else: ...@@ -112,7 +108,9 @@ else:
else: else:
print("Not raising the foot") 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