sot-talos-balance  2.0.5
Collection of dynamic-graph entities aimed at implementing balance control on talos.
appli_dcmZmpControl_distribute.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 
22 import dynamic_graph.sot_talos_balance.talos.base_estimator_conf as base_estimator_conf
23 import dynamic_graph.sot_talos_balance.talos.control_manager_conf as cm_conf
24 import dynamic_graph.sot_talos_balance.talos.distribute_conf as distribute_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 from dynamic_graph.sot_talos_balance.round_double_to_int import RoundDoubleToInt
29 
30 robot.timeStep = robot.device.getTimeStep()
31 dt = robot.timeStep
32 
33 # --- Pendulum parameters
34 robot_name = "robot"
35 robot.dynamic.com.recompute(0)
36 robotDim = robot.dynamic.getDimension()
37 mass = robot.dynamic.data.mass[0]
38 h = robot.dynamic.com.value[2]
39 g = 9.81
40 omega = sqrt(g / h)
41 
42 # --- Parameter server
43 robot.param_server = create_parameter_server(param_server_conf, dt)
44 
45 # --- Initial feet and waist
46 robot.dynamic.createOpPoint("LF", robot.OperationalPointsMap["left-ankle"])
47 robot.dynamic.createOpPoint("RF", robot.OperationalPointsMap["right-ankle"])
48 robot.dynamic.createOpPoint("WT", robot.OperationalPointsMap["waist"])
49 robot.dynamic.LF.recompute(0)
50 robot.dynamic.RF.recompute(0)
51 robot.dynamic.WT.recompute(0)
52 
53 # -------------------------- DESIRED TRAJECTORY --------------------------
54 
55 # --- Trajectory generators
56 
57 # --- CoM
58 robot.comTrajGen = create_com_trajectory_generator(dt, robot)
59 
60 # --- Left foot
61 robot.lfTrajGen = create_pose_rpy_trajectory_generator(dt, robot, "LF")
62 # robot.lfTrajGen.x.recompute(0) # trigger computation of initial value
63 robot.lfToMatrix = PoseRollPitchYawToMatrixHomo("lf2m")
64 plug(robot.lfTrajGen.x, robot.lfToMatrix.sin)
65 
66 # --- Right foot
67 robot.rfTrajGen = create_pose_rpy_trajectory_generator(dt, robot, "RF")
68 # robot.rfTrajGen.x.recompute(0) # trigger computation of initial value
69 robot.rfToMatrix = PoseRollPitchYawToMatrixHomo("rf2m")
70 plug(robot.rfTrajGen.x, robot.rfToMatrix.sin)
71 
72 # --- Waist
73 robot.waistTrajGen = create_orientation_rpy_trajectory_generator(dt, robot, "WT")
74 # robot.waistTrajGen.x.recompute(0) # trigger computation of initial value
75 
76 robot.waistMix = Mix_of_vector("waistMix")
77 robot.waistMix.setSignalNumber(3)
78 robot.waistMix.addSelec(1, 0, 3)
79 robot.waistMix.addSelec(2, 3, 3)
80 robot.waistMix.default.value = [0.0] * 6
81 robot.waistMix.signal("sin1").value = [0.0] * 3
82 plug(robot.waistTrajGen.x, robot.waistMix.signal("sin2"))
83 
84 robot.waistToMatrix = PoseRollPitchYawToMatrixHomo("w2m")
85 plug(robot.waistMix.sout, robot.waistToMatrix.sin)
86 
87 # --- Rho
88 robot.rhoTrajGen = create_scalar_trajectory_generator(dt, 0.5, "rhoTrajGen")
89 robot.rhoScalar = Component_of_vector("rho_scalar")
90 robot.rhoScalar.setIndex(0)
91 plug(robot.rhoTrajGen.x, robot.rhoScalar.sin)
92 
93 # --- Phase
94 robot.phaseTrajGen = create_scalar_trajectory_generator(dt, 0.0, "phaseTrajGen")
95 robot.phaseScalar = Component_of_vector("phase_scalar")
96 robot.phaseScalar.setIndex(0)
97 plug(robot.phaseTrajGen.x, robot.phaseScalar.sin)
98 robot.phaseInt = RoundDoubleToInt("phase_int")
99 plug(robot.phaseScalar.sout, robot.phaseInt.sin)
100 
101 # --- Interface with controller entities
102 
103 wp = DummyWalkingPatternGenerator("dummy_wp")
104 wp.init()
105 wp.omega.value = omega
106 plug(robot.rhoScalar.sout, wp.rho)
107 plug(robot.phaseInt.sout, wp.phase)
108 plug(robot.waistToMatrix.sout, wp.waist)
109 plug(robot.lfToMatrix.sout, wp.footLeft)
110 plug(robot.rfToMatrix.sout, wp.footRight)
111 plug(robot.comTrajGen.x, wp.com)
112 plug(robot.comTrajGen.dx, wp.vcom)
113 plug(robot.comTrajGen.ddx, wp.acom)
114 
115 robot.wp = wp
116 
117 # --- Compute the values to use them in initialization
118 robot.wp.comDes.recompute(0)
119 robot.wp.dcmDes.recompute(0)
120 robot.wp.zmpDes.recompute(0)
121 
122 # -------------------------- ESTIMATION --------------------------
123 
124 # --- Base Estimation
125 robot.device_filters = create_device_filters(robot, dt)
126 robot.imu_filters = create_imu_filters(robot, dt)
127 robot.base_estimator = create_base_estimator(robot, dt, base_estimator_conf)
128 
129 robot.m2qLF = MatrixHomoToPoseQuaternion("m2qLF")
130 plug(robot.dynamic.LF, robot.m2qLF.sin)
131 plug(robot.m2qLF.sout, robot.base_estimator.lf_ref_xyzquat)
132 robot.m2qRF = MatrixHomoToPoseQuaternion("m2qRF")
133 plug(robot.dynamic.RF, robot.m2qRF.sin)
134 plug(robot.m2qRF.sout, robot.base_estimator.rf_ref_xyzquat)
135 
136 # robot.be_filters = create_be_filters(robot, dt)
137 
138 
139 
140 # rf = SimpleReferenceFrame('rf')
141 # rf.init(robot_name)
142 # plug(robot.dynamic.LF, rf.footLeft)
143 # plug(robot.dynamic.RF, rf.footRight)
144 # rf.reset.value = 1
145 # robot.rf = rf
146 
147 
154 
155 # --- Conversion
156 e2q = EulerToQuat("e2q")
157 plug(robot.base_estimator.q, e2q.euler)
158 robot.e2q = e2q
159 
160 # --- Kinematic computations
161 robot.rdynamic = DynamicPinocchio("real_dynamics")
162 robot.rdynamic.setModel(robot.dynamic.model)
163 robot.rdynamic.setData(robot.rdynamic.model.createData())
164 plug(robot.base_estimator.q, robot.rdynamic.position)
165 robot.rdynamic.velocity.value = [0.0] * robotDim
166 robot.rdynamic.acceleration.value = [0.0] * robotDim
167 
168 # --- CoM Estimation
169 cdc_estimator = DcmEstimator("cdc_estimator")
170 cdc_estimator.init(dt, robot_name)
171 plug(robot.e2q.quaternion, cdc_estimator.q)
172 plug(robot.base_estimator.v, cdc_estimator.v)
173 robot.cdc_estimator = cdc_estimator
174 
175 # --- DCM Estimation
176 estimator = DummyDcmEstimator("dummy")
177 plug(robot.wp.omegaDes, estimator.omega)
178 estimator.mass.value = 1.0
179 plug(robot.cdc_estimator.c, estimator.com)
180 plug(robot.cdc_estimator.dc, estimator.momenta)
181 estimator.init()
182 robot.estimator = estimator
183 
184 # --- Force calibration
185 robot.ftc = create_ft_calibrator(robot, ft_conf)
186 
187 # --- ZMP estimation
188 zmp_estimator = SimpleZmpEstimator("zmpEst")
189 robot.rdynamic.createOpPoint("sole_LF", "left_sole_link")
190 robot.rdynamic.createOpPoint("sole_RF", "right_sole_link")
191 plug(robot.rdynamic.sole_LF, zmp_estimator.poseLeft)
192 plug(robot.rdynamic.sole_RF, zmp_estimator.poseRight)
193 plug(robot.ftc.left_foot_force_out, zmp_estimator.wrenchLeft)
194 plug(robot.ftc.right_foot_force_out, zmp_estimator.wrenchRight)
195 zmp_estimator.init()
196 robot.zmp_estimator = zmp_estimator
197 
198 # -------------------------- ADMITTANCE CONTROL --------------------------
199 
200 # --- DCM controller
201 Kp_dcm = [5.0, 5.0, 5.0]
202 Ki_dcm = [0.0, 0.0, 0.0] # zero (to be set later)
203 gamma_dcm = 0.2
204 
205 dcm_controller = DcmController("dcmCtrl")
206 
207 dcm_controller.Kp.value = Kp_dcm
208 dcm_controller.Ki.value = Ki_dcm
209 dcm_controller.decayFactor.value = gamma_dcm
210 dcm_controller.mass.value = mass
211 plug(robot.wp.omegaDes, dcm_controller.omega)
212 
213 plug(robot.cdc_estimator.c, dcm_controller.com)
214 plug(robot.estimator.dcm, dcm_controller.dcm)
215 
216 plug(robot.wp.zmpDes, dcm_controller.zmpDes)
217 plug(robot.wp.dcmDes, dcm_controller.dcmDes)
218 
219 dcm_controller.init(dt)
220 
221 robot.dcm_control = dcm_controller
222 
223 Ki_dcm = [1.0, 1.0, 1.0] # this value is employed later
224 
225 # --- Distribute wrench
226 distribute = create_distribute_wrench(distribute_conf)
227 plug(robot.e2q.quaternion, distribute.q)
228 plug(robot.dcm_control.wrenchRef, distribute.wrenchDes)
229 plug(robot.wp.rhoDes, distribute.rho)
230 plug(robot.wp.phaseDes, distribute.phase)
231 distribute.init(robot_name)
232 robot.distribute = distribute
233 
234 # --- CoM admittance controller
235 Kp_adm = [0.0, 0.0, 0.0] # zero (to be set later)
236 
237 com_admittance_control = ComAdmittanceController("comAdmCtrl")
238 com_admittance_control.Kp.value = Kp_adm
239 plug(robot.zmp_estimator.zmp, com_admittance_control.zmp)
240 com_admittance_control.zmpDes.value = (
241  robot.wp.zmpDes.value
242 ) # should be plugged to robot.distribute.zmpRef
243 plug(robot.wp.acomDes, com_admittance_control.ddcomDes)
244 
245 com_admittance_control.init(dt)
246 com_admittance_control.setState(robot.wp.comDes.value, [0.0, 0.0, 0.0])
247 
248 robot.com_admittance_control = com_admittance_control
249 
250 Kp_adm = [20.0, 10.0, 0.0] # this value is employed later
251 
252 # --- Control Manager
253 robot.cm = create_ctrl_manager(cm_conf, dt, robot_name="robot")
254 robot.cm.addCtrlMode("sot_input")
255 robot.cm.setCtrlMode("all", "sot_input")
256 robot.cm.addEmergencyStopSIN("zmp")
257 robot.cm.addEmergencyStopSIN("distribute")
258 
259 # -------------------------- SOT CONTROL --------------------------
260 
261 # --- Upper body
262 robot.taskUpperBody = Task("task_upper_body")
263 robot.taskUpperBody.feature = FeaturePosture("feature_upper_body")
264 
265 q = list(robot.dynamic.position.value)
266 robot.taskUpperBody.feature.state.value = q
267 robot.taskUpperBody.feature.posture.value = q
268 
269 # robotDim = robot.dynamic.getDimension() # 38
270 robot.taskUpperBody.feature.selectDof(18, True)
271 robot.taskUpperBody.feature.selectDof(19, True)
272 robot.taskUpperBody.feature.selectDof(20, True)
273 robot.taskUpperBody.feature.selectDof(21, True)
274 robot.taskUpperBody.feature.selectDof(22, True)
275 robot.taskUpperBody.feature.selectDof(23, True)
276 robot.taskUpperBody.feature.selectDof(24, True)
277 robot.taskUpperBody.feature.selectDof(25, True)
278 robot.taskUpperBody.feature.selectDof(26, True)
279 robot.taskUpperBody.feature.selectDof(27, True)
280 robot.taskUpperBody.feature.selectDof(28, True)
281 robot.taskUpperBody.feature.selectDof(29, True)
282 robot.taskUpperBody.feature.selectDof(30, True)
283 robot.taskUpperBody.feature.selectDof(31, True)
284 robot.taskUpperBody.feature.selectDof(32, True)
285 robot.taskUpperBody.feature.selectDof(33, True)
286 robot.taskUpperBody.feature.selectDof(34, True)
287 robot.taskUpperBody.feature.selectDof(35, True)
288 robot.taskUpperBody.feature.selectDof(36, True)
289 robot.taskUpperBody.feature.selectDof(37, 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 = 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 # # --- Posture
338 # robot.taskPos = Task ('task_pos')
339 # robot.taskPos.feature = FeaturePosture('feature_pos')
340 #
341 # q = list(robot.dynamic.position.value)
342 # robot.taskPos.feature.state.value = q
343 # robot.taskPos.feature.posture.value = q
344 
345 # robotDim = robot.dynamic.getDimension() # 38
346 # robot.taskPos.feature.selectDof(6,True)
347 # robot.taskPos.feature.selectDof(7,True)
348 # robot.taskPos.feature.selectDof(8,True)
349 # robot.taskPos.feature.selectDof(9,True)
350 # robot.taskPos.feature.selectDof(10,True)
351 # robot.taskPos.feature.selectDof(11,True)
352 # robot.taskPos.feature.selectDof(12,True)
353 # robot.taskPos.feature.selectDof(13,True)
354 # robot.taskPos.feature.selectDof(14,True)
355 # robot.taskPos.feature.selectDof(15,True)
356 # robot.taskPos.feature.selectDof(16,True)
357 # robot.taskPos.feature.selectDof(17,True)
358 # robot.taskPos.feature.selectDof(18,True)
359 # robot.taskPos.feature.selectDof(19,True)
360 # robot.taskPos.feature.selectDof(20,True)
361 # robot.taskPos.feature.selectDof(21,True)
362 # robot.taskPos.feature.selectDof(22,True)
363 # robot.taskPos.feature.selectDof(23,True)
364 # robot.taskPos.feature.selectDof(24,True)
365 # robot.taskPos.feature.selectDof(25,True)
366 # robot.taskPos.feature.selectDof(26,True)
367 # robot.taskPos.feature.selectDof(27,True)
368 # robot.taskPos.feature.selectDof(28,True)
369 # robot.taskPos.feature.selectDof(29,True)
370 # robot.taskPos.feature.selectDof(30,True)
371 # robot.taskPos.feature.selectDof(31,True)
372 # robot.taskPos.feature.selectDof(32,True)
373 # robot.taskPos.feature.selectDof(33,True)
374 # robot.taskPos.feature.selectDof(34,True)
375 # robot.taskPos.feature.selectDof(35,True)
376 # robot.taskPos.feature.selectDof(36,True)
377 # robot.taskPos.feature.selectDof(37,True)
378 
379 # robot.taskPos.controlGain.value = 100.0
380 # robot.taskPos.add(robot.taskPos.feature.name)
381 # plug(robot.dynamic.position, robot.taskPos.feature.state)
382 
383 # --- SOT solver
384 robot.sot = SOT("sot")
385 robot.sot.setSize(robot.dynamic.getDimension())
386 
387 # --- Plug SOT control to device through control manager
388 plug(robot.sot.control, robot.cm.ctrl_sot_input)
389 plug(robot.cm.u_safe, robot.device.control)
390 
391 robot.sot.push(robot.taskUpperBody.name)
392 robot.sot.push(robot.contactRF.task.name)
393 robot.sot.push(robot.contactLF.task.name)
394 robot.sot.push(robot.taskComH.task.name)
395 robot.sot.push(robot.taskCom.task.name)
396 robot.sot.push(robot.keepWaist.task.name)
397 # robot.sot.push(robot.taskPos.name)
398 # robot.device.control.recompute(0) # this crashes as it employs joint sensors which are not ready yet
399 
400 # --- Fix robot.dynamic inputs
401 plug(robot.device.velocity, robot.dynamic.velocity)
402 robot.dvdt = Derivator_of_Vector("dv_dt")
403 robot.dvdt.dt.value = dt
404 plug(robot.device.velocity, robot.dvdt.sin)
405 plug(robot.dvdt.sout, robot.dynamic.acceleration)
406 
407 # -------------------------- PLOTS --------------------------
408 
409 # --- ROS PUBLISHER
410 robot.publisher = create_rospublish(robot, "robot_publisher")
411 
412 create_topic(robot.publisher, robot.device, "state", robot=robot, data_type="vector")
413 create_topic(
414  robot.publisher, robot.base_estimator, "q", robot=robot, data_type="vector"
415 )
416 # create_topic(robot.publisher, robot.stf, 'q', robot = robot, data_type='vector')
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.dcm_control, "zmpDes", robot=robot, data_type="vector"
449 ) # desired ZMP
450 create_topic(
451  robot.publisher, robot.dynamic, "zmp", robot=robot, data_type="vector"
452 ) # SOT ZMP
453 create_topic(
454  robot.publisher, robot.zmp_estimator, "zmp", robot=robot, data_type="vector"
455 ) # estimated ZMP
456 create_topic(
457  robot.publisher, robot.dcm_control, "zmpRef", robot=robot, data_type="vector"
458 ) # reference ZMP
459 
460 create_topic(
461  robot.publisher, robot.dcm_control, "wrenchRef", robot=robot, data_type="vector"
462 ) # unoptimized reference wrench
463 create_topic(
464  robot.publisher, robot.distribute, "wrenchLeft", robot=robot, data_type="vector"
465 ) # reference left wrench
466 create_topic(
467  robot.publisher, robot.distribute, "wrenchRight", robot=robot, data_type="vector"
468 ) # reference right wrench
469 create_topic(
470  robot.publisher, robot.distribute, "wrenchRef", robot=robot, data_type="vector"
471 ) # optimized reference wrench
472 
473 # create_topic(robot.publisher, robot.device, 'forceLLEG', robot = robot, data_type='vector') # measured left wrench
474 # create_topic(robot.publisher, robot.device, 'forceRLEG', robot = robot, data_type='vector') # measured right wrench
475 
476 # create_topic(robot.publisher, robot.device_filters.ft_LF_filter, 'x_filtered', robot = robot, data_type='vector') # filtered left wrench
477 # create_topic(robot.publisher, robot.device_filters.ft_RF_filter, 'x_filtered', robot = robot, data_type='vector') # filtered right wrench
478 
479 create_topic(
480  robot.publisher, robot.ftc, "left_foot_force_out", robot=robot, data_type="vector"
481 ) # calibrated left wrench
482 create_topic(
483  robot.publisher, robot.ftc, "right_foot_force_out", robot=robot, data_type="vector"
484 ) # calibrated right wrench
485 
486 # --- TRACER
487 robot.tracer = TracerRealTime("com_tracer")
488 robot.tracer.setBufferSize(80 * (2**20))
489 robot.tracer.open("/tmp", "dg_", ".dat")
490 robot.device.after.addSignal("{0}.triger".format(robot.tracer.name))
491 
492 addTrace(robot.tracer, robot.wp, "comDes") # desired CoM
493 
494 addTrace(robot.tracer, robot.cdc_estimator, "c") # estimated CoM
495 addTrace(robot.tracer, robot.cdc_estimator, "dc") # estimated CoM velocity
496 
497 addTrace(robot.tracer, robot.com_admittance_control, "comRef") # reference CoM
498 addTrace(robot.tracer, robot.dynamic, "com") # resulting SOT CoM
499 
500 addTrace(robot.tracer, robot.dcm_control, "dcmDes") # desired DCM
501 addTrace(robot.tracer, robot.estimator, "dcm") # estimated DCM
502 
503 addTrace(robot.tracer, robot.dcm_control, "zmpDes") # desired ZMP
504 addTrace(robot.tracer, robot.dynamic, "zmp") # SOT ZMP
505 addTrace(robot.tracer, robot.zmp_estimator, "zmp") # estimated ZMP
506 addTrace(robot.tracer, robot.dcm_control, "zmpRef") # reference ZMP
507 
508 addTrace(robot.tracer, robot.dcm_control, "wrenchRef") # unoptimized reference wrench
509 addTrace(robot.tracer, robot.distribute, "wrenchLeft") # reference left wrench
510 addTrace(robot.tracer, robot.distribute, "wrenchRight") # reference right wrench
511 addTrace(robot.tracer, robot.distribute, "wrenchRef") # optimized reference wrench
512 
513 addTrace(robot.tracer, robot.ftc, "left_foot_force_out") # calibrated left wrench
514 addTrace(robot.tracer, robot.ftc, "right_foot_force_out") # calibrated right wrench
515 
516 robot.tracer.start()
sot_talos_balance.create_entities_utils.create_distribute_wrench
def create_distribute_wrench(conf)
Definition: create_entities_utils.py:640
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_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_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.create_device_filters
def create_device_filters(robot, dt)
Definition: create_entities_utils.py:279