Skip to content
GitLab
Projects
Groups
Snippets
/
Help
Help
Support
Community forum
Keyboard shortcuts
?
Submit feedback
Contribute to GitLab
Sign in / Register
Toggle navigation
Menu
Open sidebar
loco-3d
sot-talos-balance
Commits
3cc951d2
Commit
3cc951d2
authored
Sep 03, 2019
by
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
Changes
4
Pipelines
3
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_
distribut
ion
:
if
use_distribut
e
:
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!
'
)
Write
Preview
Supports
Markdown
0%
Try again
or
attach a new file
.
Cancel
You are about to add
0
people
to the discussion. Proceed with caution.
Finish editing this message first!
Cancel
Please
register
or
sign in
to comment