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