sot-talos-balance  2.0.5
Collection of dynamic-graph entities aimed at implementing balance control on talos.
appli_dcmZmpControl_file.py
Go to the documentation of this file.
1 # flake8: noqa
2 from math import sqrt
3 
4 import numpy as np
5 from dynamic_graph import plug
6 from dynamic_graph.sot.core import (
7  SOT,
8  Derivator_of_Vector,
9  FeaturePosture,
10  MatrixHomoToPoseQuaternion,
11  Task,
12 )
13 from dynamic_graph.sot.core.matrix_util import matrixToTuple
14 from dynamic_graph.sot.core.meta_tasks_kine import (
15  MetaTaskKine6d,
16  MetaTaskKineCom,
17  gotoNd,
18 )
19 from dynamic_graph.sot.dynamic_pinocchio import DynamicPinocchio
20 from dynamic_graph.tracer_real_time import TracerRealTime
21 from rospkg import RosPack
22 
23 import dynamic_graph.sot_talos_balance.talos.base_estimator_conf as base_estimator_conf
24 import dynamic_graph.sot_talos_balance.talos.control_manager_conf as cm_conf
25 import dynamic_graph.sot_talos_balance.talos.ft_calibration_conf as ft_conf
26 import dynamic_graph.sot_talos_balance.talos.parameter_server_conf as param_server_conf
27 from dynamic_graph.sot_talos_balance.create_entities_utils import *
28 
29 cm_conf.CTRL_MAX = 100.0 # temporary hack
30 
31 robot.timeStep = robot.device.getTimeStep()
32 dt = robot.timeStep
33 
34 # --- Pendulum parameters
35 robot_name = "robot"
36 robot.dynamic.com.recompute(0)
37 robotDim = robot.dynamic.getDimension()
38 mass = robot.dynamic.data.mass[0]
39 h = robot.dynamic.com.value[2]
40 g = 9.81
41 omega = sqrt(g / h)
42 
43 # --- Parameter server
44 fill_parameter_server(robot.param_server, param_server_conf, dt)
45 
46 # --- Initial feet and waist
47 robot.dynamic.createOpPoint("LF", robot.OperationalPointsMap["left-ankle"])
48 robot.dynamic.createOpPoint("RF", robot.OperationalPointsMap["right-ankle"])
49 robot.dynamic.createOpPoint("WT", robot.OperationalPointsMap["waist"])
50 robot.dynamic.LF.recompute(0)
51 robot.dynamic.RF.recompute(0)
52 robot.dynamic.WT.recompute(0)
53 
54 # -------------------------- DESIRED TRAJECTORY --------------------------
55 
56 rospack = RosPack()
57 
58 data_folder = rospack.get_path("sot-talos-balance") + "/data/"
59 folder = data_folder + test_folder + "/"
60 
61 # --- Trajectory generators
62 
63 # --- General trigger
64 robot.triggerTrajGen = BooleanIdentity("triggerTrajGen")
65 robot.triggerTrajGen.sin.value = 0
66 
67 # --- CoM
68 robot.comTrajGen = create_com_trajectory_generator(dt, robot)
69 robot.comTrajGen.x.recompute(0) # trigger computation of initial value
70 robot.comTrajGen.playTrajectoryFile(folder + "CoM.dat")
71 plug(robot.triggerTrajGen.sout, robot.comTrajGen.trigger)
72 
73 # --- Left foot
74 robot.lfTrajGen = create_pose_rpy_trajectory_generator(dt, robot, "LF")
75 robot.lfTrajGen.x.recompute(0) # trigger computation of initial value
76 
77 robot.lfToMatrix = PoseRollPitchYawToMatrixHomo("lf2m")
78 plug(robot.lfTrajGen.x, robot.lfToMatrix.sin)
79 robot.lfTrajGen.playTrajectoryFile(folder + "LeftFoot.dat")
80 plug(robot.triggerTrajGen.sout, robot.lfTrajGen.trigger)
81 
82 # --- Right foot
83 robot.rfTrajGen = create_pose_rpy_trajectory_generator(dt, robot, "RF")
84 robot.rfTrajGen.x.recompute(0) # trigger computation of initial value
85 
86 robot.rfToMatrix = PoseRollPitchYawToMatrixHomo("rf2m")
87 plug(robot.rfTrajGen.x, robot.rfToMatrix.sin)
88 robot.rfTrajGen.playTrajectoryFile(folder + "RightFoot.dat")
89 plug(robot.triggerTrajGen.sout, robot.rfTrajGen.trigger)
90 
91 # --- ZMP
92 robot.zmpTrajGen = create_zmp_trajectory_generator(dt, robot)
93 robot.zmpTrajGen.x.recompute(0) # trigger computation of initial value
94 # robot.zmpTrajGen.playTrajectoryFile(folder + 'ZMP.dat')
95 plug(robot.triggerTrajGen.sout, robot.zmpTrajGen.trigger)
96 
97 # --- Waist
98 robot.waistTrajGen = create_orientation_rpy_trajectory_generator(dt, robot, "WT")
99 robot.waistTrajGen.x.recompute(0) # trigger computation of initial value
100 
101 robot.waistMix = Mix_of_vector("waistMix")
102 robot.waistMix.setSignalNumber(3)
103 robot.waistMix.addSelec(1, 0, 3)
104 robot.waistMix.addSelec(2, 3, 3)
105 robot.waistMix.default.value = [0.0] * 6
106 robot.waistMix.signal("sin1").value = [0.0] * 3
107 plug(robot.waistTrajGen.x, robot.waistMix.signal("sin2"))
108 
109 robot.waistToMatrix = PoseRollPitchYawToMatrixHomo("w2m")
110 plug(robot.waistMix.sout, robot.waistToMatrix.sin)
111 robot.waistTrajGen.playTrajectoryFile(folder + "WaistOrientation.dat")
112 plug(robot.triggerTrajGen.sout, robot.waistTrajGen.trigger)
113 
114 # --- Interface with controller entities
115 
116 wp = DummyWalkingPatternGenerator("dummy_wp")
117 wp.init()
118 wp.omega.value = omega
119 # wp.waist.value = robot.dynamic.WT.value # wait receives a full homogeneous matrix, but only the rotational part is controlled
120 # wp.footLeft.value = robot.dynamic.LF.value
121 # wp.footRight.value = robot.dynamic.RF.value
122 # wp.com.value = robot.dynamic.com.value
123 # wp.vcom.value = [0.]*3
124 # wp.acom.value = [0.]*3
125 plug(robot.waistToMatrix.sout, wp.waist)
126 plug(robot.lfToMatrix.sout, wp.footLeft)
127 plug(robot.rfToMatrix.sout, wp.footRight)
128 plug(robot.comTrajGen.x, wp.com)
129 plug(robot.comTrajGen.dx, wp.vcom)
130 plug(robot.comTrajGen.ddx, wp.acom)
131 # plug(robot.zmpTrajGen.x, wp.zmp)
132 
133 robot.wp = wp
134 
135 # --- Compute the values to use them in initialization
136 robot.wp.comDes.recompute(0)
137 robot.wp.dcmDes.recompute(0)
138 robot.wp.zmpDes.recompute(0)
139 
140 # -------------------------- ESTIMATION --------------------------
141 
142 # --- Base Estimation
143 robot.device_filters = create_device_filters(robot, dt)
144 robot.imu_filters = create_imu_filters(robot, dt)
145 robot.base_estimator = create_base_estimator(robot, dt, base_estimator_conf)
146 
147 robot.m2qLF = MatrixHomoToPoseQuaternion("m2qLF")
148 plug(robot.dynamic.LF, robot.m2qLF.sin)
149 plug(robot.m2qLF.sout, robot.base_estimator.lf_ref_xyzquat)
150 robot.m2qRF = MatrixHomoToPoseQuaternion("m2qRF")
151 plug(robot.dynamic.RF, robot.m2qRF.sin)
152 plug(robot.m2qRF.sout, robot.base_estimator.rf_ref_xyzquat)
153 
154 # robot.be_filters = create_be_filters(robot, dt)
155 
156 
157 
158 # rf = SimpleReferenceFrame('rf')
159 # rf.init(robot_name)
160 # plug(robot.dynamic.LF, rf.footLeft)
161 # plug(robot.dynamic.RF, rf.footRight)
162 # rf.reset.value = 1
163 # robot.rf = rf
164 
165 
172 
173 # --- Conversion
174 e2q = EulerToQuat("e2q")
175 plug(robot.base_estimator.q, e2q.euler)
176 robot.e2q = e2q
177 
178 # --- Kinematic computations
179 robot.rdynamic = DynamicPinocchio("real_dynamics")
180 robot.rdynamic.setModel(robot.dynamic.model)
181 robot.rdynamic.setData(robot.rdynamic.model.createData())
182 plug(robot.base_estimator.q, robot.rdynamic.position)
183 robot.rdynamic.velocity.value = [0.0] * robotDim
184 robot.rdynamic.acceleration.value = [0.0] * robotDim
185 
186 # --- CoM Estimation
187 cdc_estimator = DcmEstimator("cdc_estimator")
188 cdc_estimator.init(dt, robot_name)
189 plug(robot.e2q.quaternion, cdc_estimator.q)
190 plug(robot.base_estimator.v, cdc_estimator.v)
191 robot.cdc_estimator = cdc_estimator
192 
193 # --- DCM Estimation
194 estimator = DummyDcmEstimator("dummy")
195 estimator.omega.value = omega
196 estimator.mass.value = 1.0
197 plug(robot.cdc_estimator.c, estimator.com)
198 plug(robot.cdc_estimator.dc, estimator.momenta)
199 estimator.init()
200 robot.estimator = estimator
201 
202 # --- Force calibration
203 robot.ftc = create_ft_calibrator(robot, ft_conf)
204 
205 # --- ZMP estimation
206 zmp_estimator = SimpleZmpEstimator("zmpEst")
207 robot.rdynamic.createOpPoint("sole_LF", "left_sole_link")
208 robot.rdynamic.createOpPoint("sole_RF", "right_sole_link")
209 plug(robot.rdynamic.sole_LF, zmp_estimator.poseLeft)
210 plug(robot.rdynamic.sole_RF, zmp_estimator.poseRight)
211 plug(robot.ftc.left_foot_force_out, zmp_estimator.wrenchLeft)
212 plug(robot.ftc.right_foot_force_out, zmp_estimator.wrenchRight)
213 zmp_estimator.init()
214 robot.zmp_estimator = zmp_estimator
215 
216 # -------------------------- ADMITTANCE CONTROL --------------------------
217 
218 # --- DCM controller
219 Kp_dcm = [8.0] * 3
220 Ki_dcm = [0.0, 0.0, 0.0] # zero (to be set later)
221 gamma_dcm = 0.2
222 
223 dcm_controller = DcmController("dcmCtrl")
224 
225 dcm_controller.Kp.value = Kp_dcm
226 dcm_controller.Ki.value = Ki_dcm
227 dcm_controller.decayFactor.value = gamma_dcm
228 dcm_controller.mass.value = mass
229 dcm_controller.omega.value = omega
230 
231 plug(robot.cdc_estimator.c, dcm_controller.com)
232 plug(robot.estimator.dcm, dcm_controller.dcm)
233 
234 plug(robot.wp.zmpDes, dcm_controller.zmpDes)
235 plug(robot.wp.dcmDes, dcm_controller.dcmDes)
236 
237 dcm_controller.init(dt)
238 
239 robot.dcm_control = dcm_controller
240 
241 Ki_dcm = [1.0, 1.0, 1.0] # this value is employed later
242 
243 # --- CoM admittance controller
244 Kp_adm = [0.0, 0.0, 0.0] # zero (to be set later)
245 
246 com_admittance_control = ComAdmittanceController("comAdmCtrl")
247 com_admittance_control.Kp.value = Kp_adm
248 plug(robot.zmp_estimator.zmp, com_admittance_control.zmp)
249 com_admittance_control.zmpDes.value = (
250  robot.wp.zmpDes.value
251 ) # should be plugged to robot.dcm_control.zmpRef
252 plug(robot.wp.acomDes, com_admittance_control.ddcomDes)
253 
254 com_admittance_control.init(dt)
255 com_admittance_control.setState(robot.wp.comDes.value, [0.0, 0.0, 0.0])
256 
257 robot.com_admittance_control = com_admittance_control
258 
259 Kp_adm = [15.0, 15.0, 0.0] # this value is employed later
260 
261 # --- Control Manager
262 robot.cm = create_ctrl_manager(cm_conf, dt, robot_name="robot")
263 robot.cm.addCtrlMode("sot_input")
264 robot.cm.setCtrlMode("all", "sot_input")
265 robot.cm.addEmergencyStopSIN("zmp")
266 
267 # -------------------------- SOT CONTROL --------------------------
268 
269 # --- Upper body
270 robot.taskUpperBody = Task("task_upper_body")
271 robot.taskUpperBody.feature = FeaturePosture("feature_upper_body")
272 
273 q = list(robot.dynamic.position.value)
274 robot.taskUpperBody.feature.state.value = q
275 robot.taskUpperBody.feature.posture.value = q
276 
277 # robotDim = robot.dynamic.getDimension() # 38
278 robot.taskUpperBody.feature.selectDof(18, True)
279 robot.taskUpperBody.feature.selectDof(19, True)
280 robot.taskUpperBody.feature.selectDof(20, True)
281 robot.taskUpperBody.feature.selectDof(21, True)
282 robot.taskUpperBody.feature.selectDof(22, True)
283 robot.taskUpperBody.feature.selectDof(23, True)
284 robot.taskUpperBody.feature.selectDof(24, True)
285 robot.taskUpperBody.feature.selectDof(25, True)
286 robot.taskUpperBody.feature.selectDof(26, True)
287 robot.taskUpperBody.feature.selectDof(27, True)
288 robot.taskUpperBody.feature.selectDof(28, True)
289 robot.taskUpperBody.feature.selectDof(29, True)
290 robot.taskUpperBody.feature.selectDof(30, True)
291 robot.taskUpperBody.feature.selectDof(31, True)
292 robot.taskUpperBody.feature.selectDof(32, True)
293 robot.taskUpperBody.feature.selectDof(33, True)
294 robot.taskUpperBody.feature.selectDof(34, True)
295 robot.taskUpperBody.feature.selectDof(35, True)
296 robot.taskUpperBody.feature.selectDof(36, True)
297 robot.taskUpperBody.feature.selectDof(37, True)
298 
299 robot.taskUpperBody.controlGain.value = 100.0
300 robot.taskUpperBody.add(robot.taskUpperBody.feature.name)
301 plug(robot.dynamic.position, robot.taskUpperBody.feature.state)
302 
303 # --- CONTACTS
304 # define contactLF and contactRF
305 robot.contactLF = MetaTaskKine6d(
306  "contactLF", robot.dynamic, "LF", robot.OperationalPointsMap["left-ankle"]
307 )
308 robot.contactLF.feature.frame("desired")
309 robot.contactLF.gain.setConstant(300)
310 plug(robot.wp.footLeftDes, robot.contactLF.featureDes.position) # .errorIN?
311 locals()["contactLF"] = robot.contactLF
312 
313 robot.contactRF = MetaTaskKine6d(
314  "contactRF", robot.dynamic, "RF", robot.OperationalPointsMap["right-ankle"]
315 )
316 robot.contactRF.feature.frame("desired")
317 robot.contactRF.gain.setConstant(300)
318 plug(robot.wp.footRightDes, robot.contactRF.featureDes.position) # .errorIN?
319 locals()["contactRF"] = robot.contactRF
320 
321 # --- COM height
322 robot.taskComH = MetaTaskKineCom(robot.dynamic, name="comH")
323 plug(robot.wp.comDes, robot.taskComH.featureDes.errorIN)
324 robot.taskComH.task.controlGain.value = 100.0
325 robot.taskComH.feature.selec.value = "100"
326 
327 # --- COM
328 robot.taskCom = MetaTaskKineCom(robot.dynamic)
329 plug(robot.com_admittance_control.comRef, robot.taskCom.featureDes.errorIN)
330 plug(robot.com_admittance_control.dcomRef, robot.taskCom.featureDes.errordotIN)
331 robot.taskCom.task.controlGain.value = 0
332 robot.taskCom.task.setWithDerivative(True)
333 robot.taskCom.feature.selec.value = "011"
334 
335 # --- Waist
336 robot.keepWaist = MetaTaskKine6d(
337  "keepWaist", robot.dynamic, "WT", robot.OperationalPointsMap["waist"]
338 )
339 robot.keepWaist.feature.frame("desired")
340 robot.keepWaist.gain.setConstant(300)
341 plug(robot.wp.waistDes, robot.keepWaist.featureDes.position)
342 robot.keepWaist.feature.selec.value = "111000"
343 locals()["keepWaist"] = robot.keepWaist
344 
345 # # --- Posture
346 # robot.taskPos = Task ('task_pos')
347 # robot.taskPos.feature = FeaturePosture('feature_pos')
348 #
349 # q = list(robot.dynamic.position.value)
350 # robot.taskPos.feature.state.value = q
351 # robot.taskPos.feature.posture.value = q
352 
353 # robotDim = robot.dynamic.getDimension() # 38
354 # robot.taskPos.feature.selectDof(6,True)
355 # robot.taskPos.feature.selectDof(7,True)
356 # robot.taskPos.feature.selectDof(8,True)
357 # robot.taskPos.feature.selectDof(9,True)
358 # robot.taskPos.feature.selectDof(10,True)
359 # robot.taskPos.feature.selectDof(11,True)
360 # robot.taskPos.feature.selectDof(12,True)
361 # robot.taskPos.feature.selectDof(13,True)
362 # robot.taskPos.feature.selectDof(14,True)
363 # robot.taskPos.feature.selectDof(15,True)
364 # robot.taskPos.feature.selectDof(16,True)
365 # robot.taskPos.feature.selectDof(17,True)
366 # robot.taskPos.feature.selectDof(18,True)
367 # robot.taskPos.feature.selectDof(19,True)
368 # robot.taskPos.feature.selectDof(20,True)
369 # robot.taskPos.feature.selectDof(21,True)
370 # robot.taskPos.feature.selectDof(22,True)
371 # robot.taskPos.feature.selectDof(23,True)
372 # robot.taskPos.feature.selectDof(24,True)
373 # robot.taskPos.feature.selectDof(25,True)
374 # robot.taskPos.feature.selectDof(26,True)
375 # robot.taskPos.feature.selectDof(27,True)
376 # robot.taskPos.feature.selectDof(28,True)
377 # robot.taskPos.feature.selectDof(29,True)
378 # robot.taskPos.feature.selectDof(30,True)
379 # robot.taskPos.feature.selectDof(31,True)
380 # robot.taskPos.feature.selectDof(32,True)
381 # robot.taskPos.feature.selectDof(33,True)
382 # robot.taskPos.feature.selectDof(34,True)
383 # robot.taskPos.feature.selectDof(35,True)
384 # robot.taskPos.feature.selectDof(36,True)
385 # robot.taskPos.feature.selectDof(37,True)
386 
387 # robot.taskPos.controlGain.value = 100.0
388 # robot.taskPos.add(robot.taskPos.feature.name)
389 # plug(robot.dynamic.position, robot.taskPos.feature.state)
390 
391 # --- SOT solver
392 robot.sot = SOT("sot")
393 robot.sot.setSize(robot.dynamic.getDimension())
394 
395 # --- Plug SOT control to device through control manager
396 plug(robot.sot.control, robot.cm.ctrl_sot_input)
397 plug(robot.cm.u_safe, robot.device.control)
398 
399 robot.sot.push(robot.taskUpperBody.name)
400 robot.sot.push(robot.contactRF.task.name)
401 robot.sot.push(robot.contactLF.task.name)
402 robot.sot.push(robot.taskComH.task.name)
403 robot.sot.push(robot.taskCom.task.name)
404 robot.sot.push(robot.keepWaist.task.name)
405 # robot.sot.push(robot.taskPos.name)
406 # robot.device.control.recompute(0) # this crashes as it employs joint sensors which are not ready yet
407 
408 # --- Fix robot.dynamic inputs
409 plug(robot.device.velocity, robot.dynamic.velocity)
410 robot.dvdt = Derivator_of_Vector("dv_dt")
411 robot.dvdt.dt.value = dt
412 plug(robot.device.velocity, robot.dvdt.sin)
413 plug(robot.dvdt.sout, robot.dynamic.acceleration)
414 
415 # -------------------------- PLOTS --------------------------
416 
417 # --- ROS PUBLISHER
418 robot.publisher = create_rospublish(robot, "robot_publisher")
419 
420 create_topic(robot.publisher, robot.device, "state", robot=robot, data_type="vector")
421 create_topic(
422  robot.publisher, robot.base_estimator, "q", robot=robot, data_type="vector"
423 )
424 # create_topic(robot.publisher, robot.stf, 'q', robot = robot, data_type='vector')
425 
426 create_topic(
427  robot.publisher, robot.comTrajGen, "x", robot=robot, data_type="vector"
428 ) # generated CoM
429 create_topic(
430  robot.publisher, robot.comTrajGen, "dx", robot=robot, data_type="vector"
431 ) # generated CoM velocity
432 create_topic(
433  robot.publisher, robot.comTrajGen, "ddx", robot=robot, data_type="vector"
434 ) # generated CoM acceleration
435 
436 create_topic(
437  robot.publisher, robot.wp, "comDes", robot=robot, data_type="vector"
438 ) # desired CoM
439 
440 create_topic(
441  robot.publisher, robot.cdc_estimator, "c", robot=robot, data_type="vector"
442 ) # estimated CoM
443 create_topic(
444  robot.publisher, robot.cdc_estimator, "dc", robot=robot, data_type="vector"
445 ) # estimated CoM velocity
446 
447 create_topic(
448  robot.publisher,
449  robot.com_admittance_control,
450  "comRef",
451  robot=robot,
452  data_type="vector",
453 ) # reference CoM
454 create_topic(
455  robot.publisher, robot.dynamic, "com", robot=robot, data_type="vector"
456 ) # resulting SOT CoM
457 
458 create_topic(
459  robot.publisher, robot.dcm_control, "dcmDes", robot=robot, data_type="vector"
460 ) # desired DCM
461 create_topic(
462  robot.publisher, robot.estimator, "dcm", robot=robot, data_type="vector"
463 ) # estimated DCM
464 
465 create_topic(
466  robot.publisher, robot.zmpTrajGen, "x", robot=robot, data_type="vector"
467 ) # generated ZMP
468 create_topic(
469  robot.publisher, robot.wp, "zmpDes", robot=robot, data_type="vector"
470 ) # desired ZMP
471 create_topic(
472  robot.publisher, robot.dynamic, "zmp", robot=robot, data_type="vector"
473 ) # SOT ZMP
474 create_topic(
475  robot.publisher, robot.zmp_estimator, "zmp", robot=robot, data_type="vector"
476 ) # estimated ZMP
477 create_topic(
478  robot.publisher, robot.dcm_control, "zmpRef", robot=robot, data_type="vector"
479 ) # reference ZMP
480 
481 # create_topic(robot.publisher, robot.device, 'forceLLEG', robot = robot, data_type='vector') # measured left wrench
482 # create_topic(robot.publisher, robot.device, 'forceRLEG', robot = robot, data_type='vector') # measured right wrench
483 
484 # create_topic(robot.publisher, robot.device_filters.ft_LF_filter, 'x_filtered', robot = robot, data_type='vector') # filtered left wrench
485 # create_topic(robot.publisher, robot.device_filters.ft_RF_filter, 'x_filtered', robot = robot, data_type='vector') # filtered right wrench
486 
487 create_topic(
488  robot.publisher, robot.waistTrajGen, "x", robot=robot, data_type="vector"
489 ) # desired waist orientation
490 
491 create_topic(
492  robot.publisher, robot.lfTrajGen, "x", robot=robot, data_type="vector"
493 ) # desired left foot pose
494 create_topic(
495  robot.publisher, robot.rfTrajGen, "x", robot=robot, data_type="vector"
496 ) # desired right foot pose
497 
498 create_topic(
499  robot.publisher, robot.ftc, "left_foot_force_out", robot=robot, data_type="vector"
500 ) # calibrated left wrench
501 create_topic(
502  robot.publisher, robot.ftc, "right_foot_force_out", robot=robot, data_type="vector"
503 ) # calibrated right wrench
504 
505 create_topic(
506  robot.publisher, robot.dynamic, "LF", robot=robot, data_type="matrixHomo"
507 ) # left foot
508 create_topic(
509  robot.publisher, robot.dynamic, "RF", robot=robot, data_type="matrixHomo"
510 ) # right foot
511 
512 # --- TRACER
513 robot.tracer = TracerRealTime("com_tracer")
514 robot.tracer.setBufferSize(80 * (2**20))
515 robot.tracer.open("/tmp", "dg_", ".dat")
516 robot.device.after.addSignal("{0}.triger".format(robot.tracer.name))
517 
518 addTrace(robot.tracer, robot.wp, "comDes") # desired CoM
519 
520 addTrace(robot.tracer, robot.cdc_estimator, "c") # estimated CoM
521 addTrace(robot.tracer, robot.cdc_estimator, "dc") # estimated CoM velocity
522 
523 addTrace(robot.tracer, robot.com_admittance_control, "comRef") # reference CoM
524 addTrace(robot.tracer, robot.dynamic, "com") # resulting SOT CoM
525 
526 addTrace(robot.tracer, robot.dcm_control, "dcmDes") # desired DCM
527 addTrace(robot.tracer, robot.estimator, "dcm") # estimated DCM
528 
529 addTrace(robot.tracer, robot.dcm_control, "zmpDes") # desired ZMP
530 addTrace(robot.tracer, robot.dynamic, "zmp") # SOT ZMP
531 addTrace(robot.tracer, robot.zmp_estimator, "zmp") # estimated ZMP
532 addTrace(robot.tracer, robot.dcm_control, "zmpRef") # reference ZMP
533 
534 addTrace(robot.tracer, robot.ftc, "left_foot_force_out") # calibrated left wrench
535 addTrace(robot.tracer, robot.ftc, "right_foot_force_out") # calibrated right wrench
536 
537 addTrace(robot.tracer, robot.dynamic, "LF") # left foot
538 addTrace(robot.tracer, robot.dynamic, "RF") # right foot
539 
540 robot.tracer.start()
sot_talos_balance.create_entities_utils.create_ctrl_manager
def create_ctrl_manager(conf, dt, robot_name="robot")
Definition: create_entities_utils.py:341
create_entities_utils
sot_talos_balance.create_entities_utils.create_ft_calibrator
def create_ft_calibrator(robot, conf)
Definition: create_entities_utils.py:683
sot_talos_balance.create_entities_utils.create_base_estimator
def create_base_estimator(robot, dt, conf, robot_name="robot")
Definition: create_entities_utils.py:352
sot_talos_balance.create_entities_utils.create_com_trajectory_generator
def create_com_trajectory_generator(dt, robot)
Definition: create_entities_utils.py:120
sot_talos_balance.create_entities_utils.addTrace
def addTrace(tracer, entity, signalName)
Definition: create_entities_utils.py:402
sot_talos_balance.create_entities_utils.create_pose_rpy_trajectory_generator
def create_pose_rpy_trajectory_generator(dt, robot, signal_name)
Definition: create_entities_utils.py:162
sot_talos_balance.create_entities_utils.create_zmp_trajectory_generator
def create_zmp_trajectory_generator(dt, robot)
Definition: create_entities_utils.py:128
sot_talos_balance.create_entities_utils.create_topic
def create_topic(rospub, entity, signalName, robot=None, data_type="vector")
Definition: create_entities_utils.py:454
sot_talos_balance.create_entities_utils.create_orientation_rpy_trajectory_generator
def create_orientation_rpy_trajectory_generator(dt, robot, signal_name)
Definition: create_entities_utils.py:150
sot_talos_balance.create_entities_utils.fill_parameter_server
def fill_parameter_server(param_server, conf, dt, robot_name="robot")
Definition: create_entities_utils.py:576
sot_talos_balance.create_entities_utils.create_imu_filters
def create_imu_filters(robot, dt)
Definition: create_entities_utils.py:388
sot_talos_balance.create_entities_utils.create_rospublish
def create_rospublish(robot, name)
Definition: create_entities_utils.py:446
sot_talos_balance.create_entities_utils.create_device_filters
def create_device_filters(robot, dt)
Definition: create_entities_utils.py:279