appli_dcmZmpControl_file.py 17.2 KB
Newer Older
1
2
3
4
5
6
7
8
9
10
11
12
13
14
# flake8: noqa
from math import sqrt

from dynamic_graph.sot.core.task import Task
from dynamic_graph.sot.core.feature_posture import FeaturePosture
from dynamic_graph.sot.core.sot import SOT
from dynamic_graph.sot.core.derivator import Derivator_of_Vector

from dynamic_graph.sot.core.matrix_util import matrixToTuple
from dynamic_graph.sot.core.meta_tasks_kine import MetaTaskKine6d, MetaTaskKineCom, gotoNd
from dynamic_graph.sot.dynamic_pinocchio import DynamicPinocchio
from dynamic_graph.tracer_real_time import TracerRealTime
from rospkg import RosPack

15
16
17
18
19
import dynamic_graph.sot_talos_balance.talos.base_estimator_conf as base_estimator_conf
import dynamic_graph.sot_talos_balance.talos.control_manager_conf as cm_conf
import dynamic_graph.sot_talos_balance.talos.ft_calibration_conf as ft_conf
import dynamic_graph.sot_talos_balance.talos.parameter_server_conf as param_server_conf
from dynamic_graph.sot_talos_balance.create_entities_utils import *
20
21
22

from dynamic_graph import plug

Guilhem Saurel's avatar
Guilhem Saurel committed
23
24

def init_sot_talos_balance(robot, test_folder):
25
    cm_conf.CTRL_MAX = 1000.0  # temporary hack
Guilhem Saurel's avatar
Guilhem Saurel committed
26
    dt = robot.device.getTimeStep()
27
    robot.timeStep = dt
Guilhem Saurel's avatar
Guilhem Saurel committed
28

29
30
31
32
33
34
35
36
    # --- Pendulum parameters
    robot_name = 'robot'
    robot.dynamic.com.recompute(0)
    robotDim = robot.dynamic.getDimension()
    mass = robot.dynamic.data.mass[0]
    h = robot.dynamic.com.value[2]
    g = 9.81
    omega = sqrt(g / h)
Guilhem Saurel's avatar
Guilhem Saurel committed
37

38
39
    # --- Parameter server
    robot.param_server = create_parameter_server(param_server_conf, dt)
Guilhem Saurel's avatar
Guilhem Saurel committed
40

41
42
43
44
45
46
47
    # --- Initial feet and waist
    robot.dynamic.createOpPoint('LF', robot.OperationalPointsMap['left-ankle'])
    robot.dynamic.createOpPoint('RF', robot.OperationalPointsMap['right-ankle'])
    robot.dynamic.createOpPoint('WT', robot.OperationalPointsMap['waist'])
    robot.dynamic.LF.recompute(0)
    robot.dynamic.RF.recompute(0)
    robot.dynamic.WT.recompute(0)
Guilhem Saurel's avatar
Guilhem Saurel committed
48

49
    # -------------------------- DESIRED TRAJECTORY --------------------------
Guilhem Saurel's avatar
Guilhem Saurel committed
50

51
    rospack = RosPack()
Guilhem Saurel's avatar
Guilhem Saurel committed
52

53
54
    data_folder = rospack.get_path('sot-talos-balance') + "/data/"
    folder = data_folder + test_folder + '/'
Guilhem Saurel's avatar
Guilhem Saurel committed
55

56
    # --- Trajectory generators
Guilhem Saurel's avatar
Guilhem Saurel committed
57

58
59
60
    # --- General trigger
    robot.triggerTrajGen = BooleanIdentity('triggerTrajGen')
    robot.triggerTrajGen.sin.value = 0
Guilhem Saurel's avatar
Guilhem Saurel committed
61

62
63
64
65
66
    # --- CoM
    robot.comTrajGen = create_com_trajectory_generator(dt, robot)
    robot.comTrajGen.x.recompute(0)  # trigger computation of initial value
    robot.comTrajGen.playTrajectoryFile(folder + 'CoM.dat')
    plug(robot.triggerTrajGen.sout, robot.comTrajGen.trigger)
Guilhem Saurel's avatar
Guilhem Saurel committed
67

68
69
70
    # --- Left foot
    robot.lfTrajGen = create_pose_rpy_trajectory_generator(dt, robot, 'LF')
    robot.lfTrajGen.x.recompute(0)  # trigger computation of initial value
Guilhem Saurel's avatar
Guilhem Saurel committed
71

72
73
74
75
76
77
78
79
    robot.lfToMatrix = PoseRollPitchYawToMatrixHomo('lf2m')
    plug(robot.lfTrajGen.x, robot.lfToMatrix.sin)
    robot.lfTrajGen.playTrajectoryFile(folder + 'LeftFoot.dat')
    plug(robot.triggerTrajGen.sout, robot.lfTrajGen.trigger)

    # --- Right foot
    robot.rfTrajGen = create_pose_rpy_trajectory_generator(dt, robot, 'RF')
    robot.rfTrajGen.x.recompute(0)  # trigger computation of initial value
Guilhem Saurel's avatar
Guilhem Saurel committed
80

81
82
83
84
    robot.rfToMatrix = PoseRollPitchYawToMatrixHomo('rf2m')
    plug(robot.rfTrajGen.x, robot.rfToMatrix.sin)
    robot.rfTrajGen.playTrajectoryFile(folder + 'RightFoot.dat')
    plug(robot.triggerTrajGen.sout, robot.rfTrajGen.trigger)
Guilhem Saurel's avatar
Guilhem Saurel committed
85

86
87
88
89
90
    # --- ZMP
    robot.zmpTrajGen = create_zmp_trajectory_generator(dt, robot)
    robot.zmpTrajGen.x.recompute(0)  # trigger computation of initial value
    # robot.zmpTrajGen.playTrajectoryFile(folder + 'ZMP.dat')
    plug(robot.triggerTrajGen.sout, robot.zmpTrajGen.trigger)
Guilhem Saurel's avatar
Guilhem Saurel committed
91

92
93
94
    # --- Waist
    robot.waistTrajGen = create_orientation_rpy_trajectory_generator(dt, robot, 'WT')
    robot.waistTrajGen.x.recompute(0)  # trigger computation of initial value
Guilhem Saurel's avatar
Guilhem Saurel committed
95

96
97
98
99
100
101
102
    robot.waistMix = Mix_of_vector("waistMix")
    robot.waistMix.setSignalNumber(3)
    robot.waistMix.addSelec(1, 0, 3)
    robot.waistMix.addSelec(2, 3, 3)
    robot.waistMix.default.value = [0.0] * 6
    robot.waistMix.signal("sin1").value = [0.0] * 3
    plug(robot.waistTrajGen.x, robot.waistMix.signal("sin2"))
Guilhem Saurel's avatar
Guilhem Saurel committed
103

104
105
106
107
    robot.waistToMatrix = PoseRollPitchYawToMatrixHomo('w2m')
    plug(robot.waistMix.sout, robot.waistToMatrix.sin)
    robot.waistTrajGen.playTrajectoryFile(folder + 'WaistOrientation.dat')
    plug(robot.triggerTrajGen.sout, robot.waistTrajGen.trigger)
Guilhem Saurel's avatar
Guilhem Saurel committed
108

109
    # --- Interface with controller entities
Guilhem Saurel's avatar
Guilhem Saurel committed
110

111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
    wp = DummyWalkingPatternGenerator('dummy_wp')
    wp.init()
    wp.omega.value = omega
    #wp.waist.value = robot.dynamic.WT.value          # wait receives a full homogeneous matrix, but only the rotational part is controlled
    #wp.footLeft.value = robot.dynamic.LF.value
    #wp.footRight.value = robot.dynamic.RF.value
    #wp.com.value  = robot.dynamic.com.value
    #wp.vcom.value = [0.]*3
    #wp.acom.value = [0.]*3
    plug(robot.waistToMatrix.sout, wp.waist)
    plug(robot.lfToMatrix.sout, wp.footLeft)
    plug(robot.rfToMatrix.sout, wp.footRight)
    plug(robot.comTrajGen.x, wp.com)
    plug(robot.comTrajGen.dx, wp.vcom)
    plug(robot.comTrajGen.ddx, wp.acom)
    #plug(robot.zmpTrajGen.x, wp.zmp)
Guilhem Saurel's avatar
Guilhem Saurel committed
127

128
    robot.wp = wp
Guilhem Saurel's avatar
Guilhem Saurel committed
129

130
131
132
133
    # --- Compute the values to use them in initialization
    robot.wp.comDes.recompute(0)
    robot.wp.dcmDes.recompute(0)
    robot.wp.zmpDes.recompute(0)
Guilhem Saurel's avatar
Guilhem Saurel committed
134

135
    # -------------------------- ESTIMATION --------------------------
Guilhem Saurel's avatar
Guilhem Saurel committed
136

137
138
139
140
    # --- Base Estimation
    robot.device_filters = create_device_filters(robot, dt)
    robot.imu_filters = create_imu_filters(robot, dt)
    robot.base_estimator = create_base_estimator(robot, dt, base_estimator_conf)
Guilhem Saurel's avatar
Guilhem Saurel committed
141

142
143
144
145
146
147
148
149
150
151
152
    robot.m2qLF = MatrixHomoToPoseQuaternion('m2qLF')
    plug(robot.dynamic.LF, robot.m2qLF.sin)
    plug(robot.m2qLF.sout, robot.base_estimator.lf_ref_xyzquat)
    robot.m2qRF = MatrixHomoToPoseQuaternion('m2qRF')
    plug(robot.dynamic.RF, robot.m2qRF.sin)
    plug(robot.m2qRF.sout, robot.base_estimator.rf_ref_xyzquat)

    # --- Conversion
    e2q = EulerToQuat('e2q')
    plug(robot.base_estimator.q, e2q.euler)
    robot.e2q = e2q
Guilhem Saurel's avatar
Guilhem Saurel committed
153

154
155
156
157
158
159
160
    # --- Kinematic computations
    robot.rdynamic = DynamicPinocchio("real_dynamics")
    robot.rdynamic.setModel(robot.dynamic.model)
    robot.rdynamic.setData(robot.rdynamic.model.createData())
    plug(robot.base_estimator.q, robot.rdynamic.position)
    robot.rdynamic.velocity.value = [0.0] * robotDim
    robot.rdynamic.acceleration.value = [0.0] * robotDim
Guilhem Saurel's avatar
Guilhem Saurel committed
161

162
163
164
165
166
167
    # --- CoM Estimation
    cdc_estimator = DcmEstimator('cdc_estimator')
    cdc_estimator.init(dt, robot_name)
    plug(robot.e2q.quaternion, cdc_estimator.q)
    plug(robot.base_estimator.v, cdc_estimator.v)
    robot.cdc_estimator = cdc_estimator
Guilhem Saurel's avatar
Guilhem Saurel committed
168

169
170
171
172
173
174
175
176
177
178
179
    # --- DCM Estimation
    estimator = DummyDcmEstimator("dummy")
    estimator.omega.value = omega
    estimator.mass.value = 1.0
    plug(robot.cdc_estimator.c, estimator.com)
    plug(robot.cdc_estimator.dc, estimator.momenta)
    estimator.init()
    robot.estimator = estimator

    # --- Force calibration
    robot.ftc = create_ft_calibrator(robot, ft_conf)
Guilhem Saurel's avatar
Guilhem Saurel committed
180

181
182
183
184
185
186
187
188
189
190
    # --- ZMP estimation
    zmp_estimator = SimpleZmpEstimator("zmpEst")
    robot.rdynamic.createOpPoint('sole_LF', 'left_sole_link')
    robot.rdynamic.createOpPoint('sole_RF', 'right_sole_link')
    plug(robot.rdynamic.sole_LF, zmp_estimator.poseLeft)
    plug(robot.rdynamic.sole_RF, zmp_estimator.poseRight)
    plug(robot.ftc.left_foot_force_out, zmp_estimator.wrenchLeft)
    plug(robot.ftc.right_foot_force_out, zmp_estimator.wrenchRight)
    zmp_estimator.init()
    robot.zmp_estimator = zmp_estimator
Guilhem Saurel's avatar
Guilhem Saurel committed
191

192
    # -------------------------- ADMITTANCE CONTROL --------------------------
Guilhem Saurel's avatar
Guilhem Saurel committed
193

194
195
196
197
    # --- DCM controller
    Kp_dcm = [8.0] * 3
    Ki_dcm = [0.0, 0.0, 0.0]  # zero (to be set later)
    gamma_dcm = 0.2
Guilhem Saurel's avatar
Guilhem Saurel committed
198

199
    dcm_controller = DcmController("dcmCtrl")
Guilhem Saurel's avatar
Guilhem Saurel committed
200

201
202
203
204
205
    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
Guilhem Saurel's avatar
Guilhem Saurel committed
206

207
208
    plug(robot.cdc_estimator.c, dcm_controller.com)
    plug(robot.estimator.dcm, dcm_controller.dcm)
Guilhem Saurel's avatar
Guilhem Saurel committed
209

210
211
    plug(robot.wp.zmpDes, dcm_controller.zmpDes)
    plug(robot.wp.dcmDes, dcm_controller.dcmDes)
Guilhem Saurel's avatar
Guilhem Saurel committed
212

213
    dcm_controller.init(dt)
Guilhem Saurel's avatar
Guilhem Saurel committed
214

215
    robot.dcm_control = dcm_controller
Guilhem Saurel's avatar
Guilhem Saurel committed
216

217
    Ki_dcm = [1.0, 1.0, 1.0]  # this value is employed later
Guilhem Saurel's avatar
Guilhem Saurel committed
218

219
220
    # --- CoM admittance controller
    Kp_adm = [0.0, 0.0, 0.0]  # zero (to be set later)
Guilhem Saurel's avatar
Guilhem Saurel committed
221

222
223
224
225
226
    com_admittance_control = ComAdmittanceController("comAdmCtrl")
    com_admittance_control.Kp.value = Kp_adm
    plug(robot.zmp_estimator.zmp, com_admittance_control.zmp)
    com_admittance_control.zmpDes.value = robot.wp.zmpDes.value  # should be plugged to robot.dcm_control.zmpRef
    plug(robot.wp.acomDes, com_admittance_control.ddcomDes)
Guilhem Saurel's avatar
Guilhem Saurel committed
227

228
229
    com_admittance_control.init(dt)
    com_admittance_control.setState(robot.wp.comDes.value, [0.0, 0.0, 0.0])
Guilhem Saurel's avatar
Guilhem Saurel committed
230

231
    robot.com_admittance_control = com_admittance_control
Guilhem Saurel's avatar
Guilhem Saurel committed
232

233
234
235
236
237
    # --- Control Manager
    robot.cm = create_ctrl_manager(cm_conf, dt, robot_name='robot')
    robot.cm.addCtrlMode('sot_input')
    robot.cm.setCtrlMode('all', 'sot_input')
    robot.cm.addEmergencyStopSIN('zmp')
Guilhem Saurel's avatar
Guilhem Saurel committed
238

239
    # -------------------------- SOT CONTROL --------------------------
Guilhem Saurel's avatar
Guilhem Saurel committed
240

241
242
243
    # --- Upper body
    robot.taskUpperBody = Task('task_upper_body')
    robot.taskUpperBody.feature = FeaturePosture('feature_upper_body')
Guilhem Saurel's avatar
Guilhem Saurel committed
244

245
246
247
    q = list(robot.dynamic.position.value)
    robot.taskUpperBody.feature.state.value = q
    robot.taskUpperBody.feature.posture.value = q
Guilhem Saurel's avatar
Guilhem Saurel committed
248

249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
    # robotDim = robot.dynamic.getDimension() # 38
    robot.taskUpperBody.feature.selectDof(18, True)
    robot.taskUpperBody.feature.selectDof(19, True)
    robot.taskUpperBody.feature.selectDof(20, True)
    robot.taskUpperBody.feature.selectDof(21, True)
    robot.taskUpperBody.feature.selectDof(22, True)
    robot.taskUpperBody.feature.selectDof(23, True)
    robot.taskUpperBody.feature.selectDof(24, True)
    robot.taskUpperBody.feature.selectDof(25, True)
    robot.taskUpperBody.feature.selectDof(26, True)
    robot.taskUpperBody.feature.selectDof(27, True)
    robot.taskUpperBody.feature.selectDof(28, True)
    robot.taskUpperBody.feature.selectDof(29, True)
    robot.taskUpperBody.feature.selectDof(30, True)
    robot.taskUpperBody.feature.selectDof(31, True)
    robot.taskUpperBody.feature.selectDof(32, True)
    robot.taskUpperBody.feature.selectDof(33, True)
    robot.taskUpperBody.feature.selectDof(34, True)
    robot.taskUpperBody.feature.selectDof(35, True)
    robot.taskUpperBody.feature.selectDof(36, True)
    robot.taskUpperBody.feature.selectDof(37, True)
Guilhem Saurel's avatar
Guilhem Saurel committed
270

271
272
273
    robot.taskUpperBody.controlGain.value = 100.0
    robot.taskUpperBody.add(robot.taskUpperBody.feature.name)
    plug(robot.dynamic.position, robot.taskUpperBody.feature.state)
Guilhem Saurel's avatar
Guilhem Saurel committed
274

275
276
277
278
279
280
281
    # --- CONTACTS
    #define contactLF and contactRF
    robot.contactLF = MetaTaskKine6d('contactLF', robot.dynamic, 'LF', robot.OperationalPointsMap['left-ankle'])
    robot.contactLF.feature.frame('desired')
    robot.contactLF.gain.setConstant(300)
    plug(robot.wp.footLeftDes, robot.contactLF.featureDes.position)  #.errorIN?
    locals()['contactLF'] = robot.contactLF
Guilhem Saurel's avatar
Guilhem Saurel committed
282

283
284
285
286
287
    robot.contactRF = MetaTaskKine6d('contactRF', robot.dynamic, 'RF', robot.OperationalPointsMap['right-ankle'])
    robot.contactRF.feature.frame('desired')
    robot.contactRF.gain.setConstant(300)
    plug(robot.wp.footRightDes, robot.contactRF.featureDes.position)  #.errorIN?
    locals()['contactRF'] = robot.contactRF
Guilhem Saurel's avatar
Guilhem Saurel committed
288

289
290
291
292
293
    # --- COM height
    robot.taskComH = MetaTaskKineCom(robot.dynamic, name='comH')
    plug(robot.wp.comDes, robot.taskComH.featureDes.errorIN)
    robot.taskComH.task.controlGain.value = 100.
    robot.taskComH.feature.selec.value = '100'
Guilhem Saurel's avatar
Guilhem Saurel committed
294

295
296
297
298
299
300
301
    # --- COM
    robot.taskCom = MetaTaskKineCom(robot.dynamic)
    plug(robot.com_admittance_control.comRef, robot.taskCom.featureDes.errorIN)
    plug(robot.com_admittance_control.dcomRef, robot.taskCom.featureDes.errordotIN)
    robot.taskCom.task.controlGain.value = 0
    robot.taskCom.task.setWithDerivative(True)
    robot.taskCom.feature.selec.value = '011'
Guilhem Saurel's avatar
Guilhem Saurel committed
302

303
304
305
306
307
308
309
310
311
312
313
    # --- Waist
    robot.keepWaist = MetaTaskKine6d('keepWaist', robot.dynamic, 'WT', robot.OperationalPointsMap['waist'])
    robot.keepWaist.feature.frame('desired')
    robot.keepWaist.gain.setConstant(300)
    plug(robot.wp.waistDes, robot.keepWaist.featureDes.position)
    robot.keepWaist.feature.selec.value = '111000'
    locals()['keepWaist'] = robot.keepWaist

    # --- SOT solver
    robot.sot = SOT('sot')
    robot.sot.setSize(robot.dynamic.getDimension())
Guilhem Saurel's avatar
Guilhem Saurel committed
314

315
316
317
    # --- Plug SOT control to device through control manager
    plug(robot.sot.control, robot.cm.ctrl_sot_input)
    plug(robot.cm.u_safe, robot.device.control)
Guilhem Saurel's avatar
Guilhem Saurel committed
318

319
320
321
322
323
324
    robot.sot.push(robot.taskUpperBody.name)
    robot.sot.push(robot.contactRF.task.name)
    robot.sot.push(robot.contactLF.task.name)
    robot.sot.push(robot.taskComH.task.name)
    robot.sot.push(robot.taskCom.task.name)
    robot.sot.push(robot.keepWaist.task.name)
Guilhem Saurel's avatar
Guilhem Saurel committed
325

326
327
328
329
330
331
    # --- Fix robot.dynamic inputs
    plug(robot.device.velocity, robot.dynamic.velocity)
    robot.dvdt = Derivator_of_Vector("dv_dt")
    robot.dvdt.dt.value = dt
    plug(robot.device.velocity, robot.dvdt.sin)
    plug(robot.dvdt.sout, robot.dynamic.acceleration)
Guilhem Saurel's avatar
Guilhem Saurel committed
332

333
    # -------------------------- PLOTS --------------------------
Guilhem Saurel's avatar
Guilhem Saurel committed
334

335
336
    # --- ROS PUBLISHER
    robot.publisher = create_rospublish(robot, 'robot_publisher')
Guilhem Saurel's avatar
Guilhem Saurel committed
337

338
339
340
    create_topic(robot.publisher, robot.device, 'state', robot=robot, data_type='vector')
    create_topic(robot.publisher, robot.base_estimator, 'q', robot=robot, data_type='vector')
    #create_topic(robot.publisher, robot.stf, 'q', robot = robot, data_type='vector')
Guilhem Saurel's avatar
Guilhem Saurel committed
341

342
343
    create_topic(robot.publisher, robot.comTrajGen, 'x', robot=robot, data_type='vector')  # generated CoM
    create_topic(robot.publisher, robot.comTrajGen, 'dx', robot=robot, data_type='vector')  # generated CoM velocity
Guilhem Saurel's avatar
Guilhem Saurel committed
344
345
346
    create_topic(robot.publisher, robot.comTrajGen, 'ddx', robot=robot,
                 data_type='vector')  # generated CoM acceleration

347
    create_topic(robot.publisher, robot.wp, 'comDes', robot=robot, data_type='vector')  # desired CoM
Guilhem Saurel's avatar
Guilhem Saurel committed
348

349
350
    create_topic(robot.publisher, robot.cdc_estimator, 'c', robot=robot, data_type='vector')  # estimated CoM
    create_topic(robot.publisher, robot.cdc_estimator, 'dc', robot=robot, data_type='vector')  # estimated CoM velocity
Guilhem Saurel's avatar
Guilhem Saurel committed
351
352
353

    create_topic(robot.publisher, robot.com_admittance_control, 'comRef', robot=robot,
                 data_type='vector')  # reference CoM
354
    create_topic(robot.publisher, robot.dynamic, 'com', robot=robot, data_type='vector')  # resulting SOT CoM
Guilhem Saurel's avatar
Guilhem Saurel committed
355

356
357
    create_topic(robot.publisher, robot.dcm_control, 'dcmDes', robot=robot, data_type='vector')  # desired DCM
    create_topic(robot.publisher, robot.estimator, 'dcm', robot=robot, data_type='vector')  # estimated DCM
Guilhem Saurel's avatar
Guilhem Saurel committed
358

359
360
361
362
363
364
    create_topic(robot.publisher, robot.zmpTrajGen, 'x', robot=robot, data_type='vector')  # generated ZMP
    create_topic(robot.publisher, robot.wp, 'zmpDes', robot=robot, data_type='vector')  # desired ZMP
    create_topic(robot.publisher, robot.dynamic, 'zmp', robot=robot, data_type='vector')  # SOT ZMP
    create_topic(robot.publisher, robot.zmp_estimator, 'zmp', robot=robot, data_type='vector')  # estimated ZMP
    create_topic(robot.publisher, robot.dcm_control, 'zmpRef', robot=robot, data_type='vector')  # reference ZMP

Guilhem Saurel's avatar
Guilhem Saurel committed
365
366
367
    create_topic(robot.publisher, robot.waistTrajGen, 'x', robot=robot,
                 data_type='vector')  # desired waist orientation

368
369
    create_topic(robot.publisher, robot.lfTrajGen, 'x', robot=robot, data_type='vector')  # desired left foot pose
    create_topic(robot.publisher, robot.rfTrajGen, 'x', robot=robot, data_type='vector')  # desired right foot pose
Guilhem Saurel's avatar
Guilhem Saurel committed
370
371
372
373
374
375

    create_topic(robot.publisher, robot.ftc, 'left_foot_force_out', robot=robot,
                 data_type='vector')  # calibrated left wrench
    create_topic(robot.publisher, robot.ftc, 'right_foot_force_out', robot=robot,
                 data_type='vector')  # calibrated right wrench

376
377
    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
Guilhem Saurel's avatar
Guilhem Saurel committed
378

379
380
381
382
383
    # --- 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))
Guilhem Saurel's avatar
Guilhem Saurel committed
384

385
    addTrace(robot.tracer, robot.wp, 'comDes')  # desired CoM
Guilhem Saurel's avatar
Guilhem Saurel committed
386

387
388
    addTrace(robot.tracer, robot.cdc_estimator, 'c')  # estimated CoM
    addTrace(robot.tracer, robot.cdc_estimator, 'dc')  # estimated CoM velocity
Guilhem Saurel's avatar
Guilhem Saurel committed
389

390
391
    addTrace(robot.tracer, robot.com_admittance_control, 'comRef')  # reference CoM
    addTrace(robot.tracer, robot.dynamic, 'com')  # resulting SOT CoM
Guilhem Saurel's avatar
Guilhem Saurel committed
392

393
394
    addTrace(robot.tracer, robot.dcm_control, 'dcmDes')  # desired DCM
    addTrace(robot.tracer, robot.estimator, 'dcm')  # estimated DCM
Guilhem Saurel's avatar
Guilhem Saurel committed
395

396
397
398
399
    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
Guilhem Saurel's avatar
Guilhem Saurel committed
400

401
402
    addTrace(robot.tracer, robot.ftc, 'left_foot_force_out')  # calibrated left wrench
    addTrace(robot.tracer, robot.ftc, 'right_foot_force_out')  # calibrated right wrench
Guilhem Saurel's avatar
Guilhem Saurel committed
403

404
405
    addTrace(robot.tracer, robot.dynamic, 'LF')  # left foot
    addTrace(robot.tracer, robot.dynamic, 'RF')  # right foot
Guilhem Saurel's avatar
Guilhem Saurel committed
406

407
    robot.tracer.start()