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