appli_dcmZmpControl_file.py 17.5 KB
Newer Older
1
2
3
# flake8: noqa
from math import sqrt

4
5
6
7
8
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 import plug
9
from dynamic_graph.sot.core.derivator import Derivator_of_Vector
10
from dynamic_graph.sot.core.feature_posture import FeaturePosture
11
12
from dynamic_graph.sot.core.matrix_util import matrixToTuple
from dynamic_graph.sot.core.meta_tasks_kine import MetaTaskKine6d, MetaTaskKineCom, gotoNd
13
14
from dynamic_graph.sot.core.sot import SOT
from dynamic_graph.sot.core.task import Task
15
from dynamic_graph.sot.dynamic_pinocchio import DynamicPinocchio
16
from dynamic_graph.sot_talos_balance.create_entities_utils import *
17
18
19
from dynamic_graph.tracer_real_time import TracerRealTime
from rospkg import RosPack

Guilhem Saurel's avatar
Guilhem Saurel committed
20
21

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

26
27
28
29
30
31
32
33
    # --- 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
34

35
36
    # --- Parameter server
    robot.param_server = create_parameter_server(param_server_conf, dt)
Guilhem Saurel's avatar
Guilhem Saurel committed
37

38
39
40
41
    # --- 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'])
42
    robot.dynamic.add_signals()
43
44
45
    robot.dynamic.LF.recompute(0)
    robot.dynamic.RF.recompute(0)
    robot.dynamic.WT.recompute(0)
Guilhem Saurel's avatar
Guilhem Saurel committed
46

47
    # -------------------------- DESIRED TRAJECTORY --------------------------
Guilhem Saurel's avatar
Guilhem Saurel committed
48

49
    rospack = RosPack()
Guilhem Saurel's avatar
Guilhem Saurel committed
50

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

54
    # --- Trajectory generators
Guilhem Saurel's avatar
Guilhem Saurel committed
55

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

60
61
62
63
64
    # --- 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
65

66
67
68
    # --- 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
69

70
71
72
73
74
75
76
77
    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
78

79
80
81
82
    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
83

84
85
86
87
88
    # --- 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
89

90
91
92
    # --- 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
93

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

102
103
104
105
    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
106

107
    # --- Interface with controller entities
Guilhem Saurel's avatar
Guilhem Saurel committed
108

109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
    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
125

126
    robot.wp = wp
Guilhem Saurel's avatar
Guilhem Saurel committed
127

128
129
130
131
    # --- 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
132

133
    # -------------------------- ESTIMATION --------------------------
Guilhem Saurel's avatar
Guilhem Saurel committed
134

135
136
137
138
    # --- 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
139

140
141
142
143
144
145
146
147
148
149
150
    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
151

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

161
162
163
164
165
166
    # --- 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
167

168
169
170
171
172
173
174
175
176
177
178
    # --- 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
179

180
181
182
183
    # --- ZMP estimation
    zmp_estimator = SimpleZmpEstimator("zmpEst")
    robot.rdynamic.createOpPoint('sole_LF', 'left_sole_link')
    robot.rdynamic.createOpPoint('sole_RF', 'right_sole_link')
184
    robot.rdynamic.add_signals()
185
186
187
188
189
190
    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
    # --- DCM controller
195
196
    Kp_dcm = np.array([8.0] * 3)
    Ki_dcm = np.array([0.0, 0.0, 0.0])  # zero (to be set later)
197
    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 = np.array([1.0, 1.0, 1.0])  # this value is employed later
Guilhem Saurel's avatar
Guilhem Saurel committed
218

219
    # --- CoM admittance controller
220
    Kp_adm = np.array([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
    com_admittance_control.init(dt)
229
    com_admittance_control.setState(robot.wp.comDes.value, np.array([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
    q = robot.dynamic.position.value
246
247
    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
    # --- Plug SOT control to device through control manager
316
317
    robot.cm.add_commands()
    robot.cm.add_signals()
318
319
    plug(robot.sot.control, robot.cm.ctrl_sot_input)
    plug(robot.cm.u_safe, robot.device.control)
Guilhem Saurel's avatar
Guilhem Saurel committed
320

321
322
323
324
325
326
    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
327

328
329
330
331
332
333
    # --- 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
334

335
    # -------------------------- PLOTS --------------------------
Guilhem Saurel's avatar
Guilhem Saurel committed
336

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

340
341
342
    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
343

344
345
    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
346
347
348
    create_topic(robot.publisher, robot.comTrajGen, 'ddx', robot=robot,
                 data_type='vector')  # generated CoM acceleration

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

351
352
    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
353
354
355

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

358
359
    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
360

361
362
363
364
365
366
    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
367
368
369
    create_topic(robot.publisher, robot.waistTrajGen, 'x', robot=robot,
                 data_type='vector')  # desired waist orientation

370
371
    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
372
373
374
375
376
377

    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

378
379
    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
380

381
382
383
384
385
    # --- 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
386

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

389
390
    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
391

392
393
    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
394

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

398
399
400
401
    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
402

403
404
    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
405

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

409
    robot.tracer.start()