sot-talos-balance  2.0.5
Collection of dynamic-graph entities aimed at implementing balance control on talos.
appli_dcmCoupledAnkleControl.py
Go to the documentation of this file.
1 # flake8: noqa
2 from math import pi, 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.coupled_admittance_controller import (
22  CoupledAdmittanceController,
23 )
24 from dynamic_graph.sot_talos_balance.create_entities_utils import *
25 from dynamic_graph.sot_talos_balance.meta_task_joint import MetaTaskKineJoint
26 from dynamic_graph.sot_talos_balance.saturation import Saturation
27 
28 cm_conf.CTRL_MAX = 1000.0 # temporary hack
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 # --- Ankle velocity saturation
54 kSat = 10.0
55 qLimSat = [pi / 6]
56 dqLimSat = [0.3]
57 
58 # -------------------------- DESIRED TRAJECTORY --------------------------
59 
60 # --- Trajectory generators
61 
62 # --- CoM
63 robot.comTrajGen = create_com_trajectory_generator(dt, robot)
64 
65 # --- Left foot
66 robot.lfTrajGen = create_pose_rpy_trajectory_generator(dt, robot, "LF")
67 # robot.lfTrajGen.x.recompute(0) # trigger computation of initial value
68 robot.lfToMatrix = PoseRollPitchYawToMatrixHomo("lf2m")
69 plug(robot.lfTrajGen.x, robot.lfToMatrix.sin)
70 
71 # --- Right foot
72 robot.rfTrajGen = create_pose_rpy_trajectory_generator(dt, robot, "RF")
73 # robot.rfTrajGen.x.recompute(0) # trigger computation of initial value
74 robot.rfToMatrix = PoseRollPitchYawToMatrixHomo("rf2m")
75 plug(robot.rfTrajGen.x, robot.rfToMatrix.sin)
76 
77 # --- Waist
78 robot.waistTrajGen = create_orientation_rpy_trajectory_generator(dt, robot, "WT")
79 # robot.waistTrajGen.x.recompute(0) # trigger computation of initial value
80 
81 robot.waistMix = Mix_of_vector("waistMix")
82 robot.waistMix.setSignalNumber(3)
83 robot.waistMix.addSelec(1, 0, 3)
84 robot.waistMix.addSelec(2, 3, 3)
85 robot.waistMix.default.value = [0.0] * 6
86 robot.waistMix.signal("sin1").value = [0.0] * 3
87 plug(robot.waistTrajGen.x, robot.waistMix.signal("sin2"))
88 
89 robot.waistToMatrix = PoseRollPitchYawToMatrixHomo("w2m")
90 plug(robot.waistMix.sout, robot.waistToMatrix.sin)
91 
92 # --- Rho
93 robot.rhoTrajGen = create_scalar_trajectory_generator(dt, 0.5, "rhoTrajGen")
94 robot.rhoScalar = Component_of_vector("rho_scalar")
95 robot.rhoScalar.setIndex(0)
96 plug(robot.rhoTrajGen.x, robot.rhoScalar.sin)
97 
98 # --- Phase
99 robot.phaseTrajGen = create_scalar_trajectory_generator(dt, 0.5, "phaseTrajGen")
100 robot.phaseScalar = Component_of_vector("phase_scalar")
101 robot.phaseScalar.setIndex(0)
102 plug(robot.phaseTrajGen.x, robot.phaseScalar.sin)
103 
104 # --- Interface with controller entities
105 
106 wp = DummyWalkingPatternGenerator("dummy_wp")
107 wp.init()
108 wp.omega.value = omega
109 plug(robot.waistToMatrix.sout, wp.waist)
110 plug(robot.lfToMatrix.sout, wp.footLeft)
111 plug(robot.rfToMatrix.sout, wp.footRight)
112 plug(robot.comTrajGen.x, wp.com)
113 plug(robot.comTrajGen.dx, wp.vcom)
114 plug(robot.comTrajGen.ddx, wp.acom)
115 
116 robot.wp = wp
117 
118 # --- Compute the values to use them in initialization
119 robot.wp.comDes.recompute(0)
120 robot.wp.dcmDes.recompute(0)
121 robot.wp.zmpDes.recompute(0)
122 
123 # -------------------------- ESTIMATION --------------------------
124 
125 # --- Base Estimation
126 robot.device_filters = create_device_filters(robot, dt)
127 robot.imu_filters = create_imu_filters(robot, dt)
128 robot.base_estimator = create_base_estimator(robot, dt, base_estimator_conf)
129 # robot.be_filters = create_be_filters(robot, dt)
130 
131 # --- Reference frame
132 
133 rf = SimpleReferenceFrame("rf")
134 rf.init(robot_name)
135 plug(robot.dynamic.LF, rf.footLeft)
136 plug(robot.dynamic.RF, rf.footRight)
137 robot.rf = rf
138 
139 # --- State transformation
140 stf = StateTransformation("stf")
141 stf.init()
142 plug(robot.rf.referenceFrame, stf.referenceFrame)
143 plug(robot.base_estimator.q, stf.q_in)
144 plug(robot.base_estimator.v, stf.v_in)
145 robot.stf = stf
146 
147 # --- Conversion
148 e2q = EulerToQuat("e2q")
149 plug(robot.stf.q, e2q.euler)
150 robot.e2q = e2q
151 
152 # --- Kinematic computations
153 robot.rdynamic = DynamicPinocchio("real_dynamics")
154 robot.rdynamic.setModel(robot.dynamic.model)
155 robot.rdynamic.setData(robot.rdynamic.model.createData())
156 plug(robot.stf.q, robot.rdynamic.position)
157 robot.rdynamic.velocity.value = [0.0] * robotDim
158 robot.rdynamic.acceleration.value = [0.0] * robotDim
159 
160 # --- CoM Estimation
161 cdc_estimator = DcmEstimator("cdc_estimator")
162 cdc_estimator.init(dt, robot_name)
163 plug(robot.e2q.quaternion, cdc_estimator.q)
164 plug(robot.stf.v, cdc_estimator.v)
165 robot.cdc_estimator = cdc_estimator
166 
167 # --- DCM Estimation
168 estimator = DummyDcmEstimator("dummy")
169 estimator.omega.value = omega
170 estimator.mass.value = 1.0
171 plug(robot.cdc_estimator.c, estimator.com)
172 plug(robot.cdc_estimator.dc, estimator.momenta)
173 estimator.init()
174 robot.estimator = estimator
175 
176 # --- Force calibration
177 robot.ftc = create_ft_calibrator(robot, ft_conf)
178 
179 # --- ZMP estimation
180 zmp_estimator = SimpleZmpEstimator("zmpEst")
181 robot.rdynamic.createOpPoint("sole_LF", "left_sole_link")
182 robot.rdynamic.createOpPoint("sole_RF", "right_sole_link")
183 plug(robot.rdynamic.sole_LF, zmp_estimator.poseLeft)
184 plug(robot.rdynamic.sole_RF, zmp_estimator.poseRight)
185 plug(robot.ftc.left_foot_force_out, zmp_estimator.wrenchLeft)
186 plug(robot.ftc.right_foot_force_out, zmp_estimator.wrenchRight)
187 zmp_estimator.init()
188 robot.zmp_estimator = zmp_estimator
189 
190 # -------------------------- ADMITTANCE CONTROL --------------------------
191 
192 # --- DCM controller
193 Kp_dcm = [5.0, 5.0, 5.0]
194 Ki_dcm = [0.0, 0.0, 0.0] # zero (to be set later)
195 gamma_dcm = 0.2
196 
197 dcm_controller = DcmController("dcmCtrl")
198 
199 dcm_controller.Kp.value = Kp_dcm
200 dcm_controller.Ki.value = Ki_dcm
201 dcm_controller.decayFactor.value = gamma_dcm
202 dcm_controller.mass.value = mass
203 dcm_controller.omega.value = omega
204 
205 plug(robot.cdc_estimator.c, dcm_controller.com)
206 plug(robot.estimator.dcm, dcm_controller.dcm)
207 
208 plug(robot.wp.zmpDes, dcm_controller.zmpDes)
209 plug(robot.wp.dcmDes, dcm_controller.dcmDes)
210 
211 dcm_controller.init(dt)
212 
213 robot.dcm_control = dcm_controller
214 
215 Ki_dcm = [1.0, 1.0, 1.0] # this value is employed later
216 
217 # --- Distribute wrench
218 distribute = create_simple_distribute_wrench()
219 plug(robot.e2q.quaternion, distribute.q)
220 plug(robot.dcm_control.wrenchRef, distribute.wrenchDes)
221 plug(robot.rhoScalar.sout, distribute.rho)
222 distribute.init(robot_name)
223 robot.distribute = distribute
224 
225 # --- Ankle admittance controllers
226 LeftPitchJoint = 4
227 LeftRollJoint = 5
228 RightPitchJoint = 10
229 RightRollJoint = 11
230 
231 kSumPitch = [1e-3]
232 kSumRoll = [0.0]
233 kDiffPitch = [1e-6]
234 kDiffRoll = [0.0]
235 
236 # --- Pitch
237 controller = CoupledAdmittanceController("pitchController")
238 controller.kSum.value = kSumPitch
239 controller.kDiff.value = kDiffPitch
240 
241 # Right pitch
242 robot.tauRP = Selec_of_vector("tauRP")
243 robot.tauRP.selec(RightPitchJoint, RightPitchJoint + 1)
244 plug(robot.device.ptorque, robot.tauRP.sin)
245 plug(robot.tauRP.sout, controller.tauR)
246 
247 robot.tauDesRP = Selec_of_vector("tauDesRP")
248 robot.tauDesRP.selec(4, 5)
249 plug(robot.distribute.ankleWrenchRight, robot.tauDesRP.sin)
250 
251 robot.multRP = Multiply_double_vector("multRP")
252 robot.multRP.sin1.value = -1.0
253 plug(robot.tauDesRP.sout, robot.multRP.sin2)
254 plug(robot.multRP.sout, controller.tauDesR)
255 
256 # Left pitch
257 robot.tauLP = Selec_of_vector("tauLP")
258 robot.tauLP.selec(LeftPitchJoint, LeftPitchJoint + 1)
259 plug(robot.device.ptorque, robot.tauLP.sin)
260 plug(robot.tauLP.sout, controller.tauL)
261 
262 robot.tauDesLP = Selec_of_vector("tauDesLP")
263 robot.tauDesLP.selec(4, 5)
264 plug(robot.distribute.ankleWrenchLeft, robot.tauDesLP.sin)
265 
266 robot.multLP = Multiply_double_vector("multLP")
267 robot.multLP.sin1.value = -1.0
268 plug(robot.tauDesLP.sout, robot.multLP.sin2)
269 plug(robot.multLP.sout, controller.tauDesL)
270 
271 robot.pitchController = controller
272 
273 # --- Roll
274 controller = CoupledAdmittanceController("rollController")
275 controller.kSum.value = kSumRoll
276 controller.kDiff.value = kDiffRoll
277 
278 # Right roll
279 robot.tauRR = Selec_of_vector("tauRR")
280 robot.tauRR.selec(RightRollJoint, RightRollJoint + 1)
281 plug(robot.device.ptorque, robot.tauRR.sin)
282 plug(robot.tauRR.sout, controller.tauR)
283 
284 robot.tauDesRR = Selec_of_vector("tauDesRR")
285 robot.tauDesRR.selec(3, 4)
286 plug(robot.distribute.ankleWrenchRight, robot.tauDesRR.sin)
287 
288 robot.multRR = Multiply_double_vector("multRR")
289 robot.multRR.sin1.value = -1.0
290 plug(robot.tauDesRR.sout, robot.multRR.sin2)
291 plug(robot.multRR.sout, controller.tauDesR)
292 
293 # Left roll
294 robot.tauLR = Selec_of_vector("tauLR")
295 robot.tauLR.selec(LeftRollJoint, LeftRollJoint + 1)
296 plug(robot.device.ptorque, robot.tauLR.sin)
297 plug(robot.tauLR.sout, controller.tauL)
298 
299 robot.tauDesLR = Selec_of_vector("tauDesLR")
300 robot.tauDesLR.selec(3, 4)
301 plug(robot.distribute.ankleWrenchLeft, robot.tauDesLR.sin)
302 
303 robot.multLR = Multiply_double_vector("multLR")
304 robot.multLR.sin1.value = -1.0
305 plug(robot.tauDesLR.sout, robot.multLR.sin2)
306 plug(robot.multLR.sout, controller.tauDesL)
307 
308 robot.rollController = controller
309 
310 # --- ANKLE TASKS
311 robot.qRP = Selec_of_vector("qRP")
312 robot.qRP.selec(RightPitchJoint + 6, RightPitchJoint + 7)
313 plug(robot.device.state, robot.qRP.sin)
314 
315 robot.saturationRP = Saturation("saturationRP")
316 plug(robot.qRP.sout, robot.saturationRP.x)
317 plug(robot.pitchController.dqRefR, robot.saturationRP.y)
318 robot.saturationRP.k.value = kSat
319 robot.saturationRP.xLim.value = qLimSat
320 robot.saturationRP.yLim.value = dqLimSat
321 
322 robot.taskRP = MetaTaskKineJoint(robot.dynamic, RightPitchJoint + 6)
323 robot.taskRP.task.controlGain.value = 0
324 robot.taskRP.task.setWithDerivative(True)
325 robot.taskRP.featureDes.errorIN.value = [0.0]
326 plug(robot.saturationRP.yOut, robot.taskRP.featureDes.errordotIN)
327 
328 robot.qRR = Selec_of_vector("qRR")
329 robot.qRR.selec(RightRollJoint + 6, RightRollJoint + 7)
330 plug(robot.device.state, robot.qRR.sin)
331 
332 robot.saturationRR = Saturation("saturationRR")
333 plug(robot.qRR.sout, robot.saturationRR.x)
334 plug(robot.rollController.dqRefR, robot.saturationRR.y)
335 robot.saturationRR.k.value = kSat
336 robot.saturationRR.xLim.value = qLimSat
337 robot.saturationRR.yLim.value = dqLimSat
338 
339 robot.taskRR = MetaTaskKineJoint(robot.dynamic, RightRollJoint + 6)
340 robot.taskRR.task.controlGain.value = 0
341 robot.taskRR.task.setWithDerivative(True)
342 robot.taskRR.featureDes.errorIN.value = [0.0]
343 plug(robot.saturationRR.yOut, robot.taskRR.featureDes.errordotIN)
344 # plug(robot.rollController.dqRefR, robot.taskRR.featureDes.errordotIN)
345 
346 robot.qLP = Selec_of_vector("qLP")
347 robot.qLP.selec(LeftPitchJoint + 6, LeftPitchJoint + 7)
348 plug(robot.device.state, robot.qLP.sin)
349 
350 robot.saturationLP = Saturation("saturationLP")
351 plug(robot.qLP.sout, robot.saturationLP.x)
352 plug(robot.pitchController.dqRefL, robot.saturationLP.y)
353 robot.saturationLP.k.value = kSat
354 robot.saturationLP.xLim.value = qLimSat
355 robot.saturationLP.yLim.value = dqLimSat
356 
357 robot.taskLP = MetaTaskKineJoint(robot.dynamic, LeftPitchJoint + 6)
358 robot.taskLP.task.controlGain.value = 0
359 robot.taskLP.task.setWithDerivative(True)
360 robot.taskLP.featureDes.errorIN.value = [0.0]
361 plug(robot.saturationLP.yOut, robot.taskLP.featureDes.errordotIN)
362 # plug(robot.pitchController.dqRefL, robot.taskLP.featureDes.errordotIN)
363 
364 robot.qLR = Selec_of_vector("qLR")
365 robot.qLR.selec(LeftRollJoint + 6, LeftRollJoint + 7)
366 plug(robot.device.state, robot.qLR.sin)
367 
368 robot.saturationLR = Saturation("saturationLR")
369 plug(robot.qLR.sout, robot.saturationLR.x)
370 plug(robot.rollController.dqRefL, robot.saturationLR.y)
371 robot.saturationLR.k.value = kSat
372 robot.saturationLR.xLim.value = qLimSat
373 robot.saturationLR.yLim.value = dqLimSat
374 
375 robot.taskLR = MetaTaskKineJoint(robot.dynamic, LeftRollJoint + 6)
376 robot.taskLR.task.controlGain.value = 0
377 robot.taskLR.task.setWithDerivative(True)
378 robot.taskLR.featureDes.errorIN.value = [0.0]
379 # plug(robot.rollController.dqRefL, robot.taskLR.featureDes.errordotIN)
380 plug(robot.saturationLR.yOut, robot.taskLR.featureDes.errordotIN)
381 
382 # --- Control Manager
383 robot.cm = create_ctrl_manager(cm_conf, dt, robot_name="robot")
384 robot.cm.addCtrlMode("sot_input")
385 robot.cm.setCtrlMode("all", "sot_input")
386 robot.cm.addEmergencyStopSIN("zmp")
387 robot.cm.addEmergencyStopSIN("distribute")
388 
389 # -------------------------- SOT CONTROL --------------------------
390 # --- UPPER BODY POSTURE
391 robot.taskUpperBody = Task("task_upper_body")
392 robot.taskUpperBody.feature = FeaturePosture("feature_upper_body")
393 
394 q = list(robot.dynamic.position.value)
395 robot.taskUpperBody.feature.state.value = q
396 robot.taskUpperBody.feature.posture.value = q
397 
398 robot.taskUpperBody.feature.selectDof(18, True)
399 robot.taskUpperBody.feature.selectDof(19, True)
400 robot.taskUpperBody.feature.selectDof(20, True)
401 robot.taskUpperBody.feature.selectDof(21, True)
402 robot.taskUpperBody.feature.selectDof(22, True)
403 robot.taskUpperBody.feature.selectDof(23, True)
404 robot.taskUpperBody.feature.selectDof(24, True)
405 robot.taskUpperBody.feature.selectDof(25, True)
406 robot.taskUpperBody.feature.selectDof(26, True)
407 robot.taskUpperBody.feature.selectDof(27, True)
408 robot.taskUpperBody.feature.selectDof(28, True)
409 robot.taskUpperBody.feature.selectDof(29, True)
410 robot.taskUpperBody.feature.selectDof(30, True)
411 robot.taskUpperBody.feature.selectDof(31, True)
412 robot.taskUpperBody.feature.selectDof(32, True)
413 robot.taskUpperBody.feature.selectDof(33, True)
414 robot.taskUpperBody.feature.selectDof(34, True)
415 robot.taskUpperBody.feature.selectDof(35, True)
416 robot.taskUpperBody.feature.selectDof(36, True)
417 robot.taskUpperBody.feature.selectDof(37, True)
418 
419 robot.taskUpperBody.controlGain.value = 100.0
420 robot.taskUpperBody.add(robot.taskUpperBody.feature.name)
421 plug(robot.dynamic.position, robot.taskUpperBody.feature.state)
422 
423 # --- CONTACTS
424 robot.contactLF = MetaTaskKine6d(
425  "contactLF", robot.dynamic, "LF", robot.OperationalPointsMap["left-ankle"]
426 )
427 robot.contactLF.feature.frame("desired")
428 robot.contactLF.gain.setConstant(300)
429 plug(robot.wp.footLeftDes, robot.contactLF.featureDes.position)
430 locals()["contactLF"] = robot.contactLF
431 
432 robot.contactRF = MetaTaskKine6d(
433  "contactRF", robot.dynamic, "RF", robot.OperationalPointsMap["right-ankle"]
434 )
435 robot.contactRF.feature.frame("desired")
436 robot.contactRF.gain.setConstant(300)
437 plug(robot.wp.footRightDes, robot.contactRF.featureDes.position)
438 locals()["contactRF"] = robot.contactRF
439 
440 # --- COM
441 robot.taskCom = MetaTaskKineCom(robot.dynamic)
442 plug(robot.wp.comDes, robot.taskCom.featureDes.errorIN)
443 robot.taskCom.task.controlGain.value = 10
444 robot.taskCom.feature.selec.value = "100"
445 
446 # --- Waist
447 robot.keepWaist = MetaTaskKine6d(
448  "keepWaist", robot.dynamic, "WT", robot.OperationalPointsMap["waist"]
449 )
450 robot.keepWaist.feature.frame("desired")
451 robot.keepWaist.gain.setConstant(300)
452 plug(robot.wp.waistDes, robot.keepWaist.featureDes.position)
453 robot.keepWaist.feature.selec.value = "111000"
454 locals()["keepWaist"] = robot.keepWaist
455 
456 # --- SOT solver
457 robot.sot = SOT("sot")
458 robot.sot.setSize(robot.dynamic.getDimension())
459 
460 # --- Plug SOT control to device through control manager
461 plug(robot.sot.control, robot.cm.ctrl_sot_input)
462 plug(robot.cm.u_safe, robot.device.control)
463 # plug(robot.sot.control, robot.device.control)
464 
465 robot.sot.push(robot.taskUpperBody.name)
466 robot.sot.push(robot.contactRF.task.name)
467 robot.sot.push(robot.contactLF.task.name)
468 robot.sot.push(robot.taskCom.task.name)
469 robot.sot.push(robot.taskRP.task.name)
470 robot.sot.push(robot.taskRR.task.name)
471 robot.sot.push(robot.taskLP.task.name)
472 robot.sot.push(robot.taskLR.task.name)
473 robot.sot.push(robot.keepWaist.task.name)
474 
475 # --- Fix robot.dynamic inputs
476 plug(robot.device.velocity, robot.dynamic.velocity)
477 robot.dvdt = Derivator_of_Vector("dv_dt")
478 robot.dvdt.dt.value = dt
479 plug(robot.device.velocity, robot.dvdt.sin)
480 plug(robot.dvdt.sout, robot.dynamic.acceleration)
481 
482 # -------------------------- PLOTS --------------------------
483 
484 # --- ROS PUBLISHER
485 robot.publisher = create_rospublish(robot, "robot_publisher")
486 
487 create_topic(
488  robot.publisher, robot.wp, "comDes", robot=robot, data_type="vector"
489 ) # desired CoM
490 
491 create_topic(
492  robot.publisher, robot.cdc_estimator, "c", robot=robot, data_type="vector"
493 ) # estimated CoM
494 create_topic(
495  robot.publisher, robot.cdc_estimator, "dc", robot=robot, data_type="vector"
496 ) # estimated CoM velocity
497 
498 create_topic(
499  robot.publisher, robot.dynamic, "com", robot=robot, data_type="vector"
500 ) # resulting SOT CoM
501 
502 create_topic(
503  robot.publisher, robot.dcm_control, "dcmDes", robot=robot, data_type="vector"
504 ) # desired DCM
505 create_topic(
506  robot.publisher, robot.estimator, "dcm", robot=robot, data_type="vector"
507 ) # estimated DCM
508 
509 create_topic(
510  robot.publisher, robot.dcm_control, "zmpDes", robot=robot, data_type="vector"
511 ) # desired ZMP
512 create_topic(
513  robot.publisher, robot.dynamic, "zmp", robot=robot, data_type="vector"
514 ) # SOT ZMP
515 create_topic(
516  robot.publisher, robot.zmp_estimator, "zmp", robot=robot, data_type="vector"
517 ) # estimated ZMP
518 create_topic(
519  robot.publisher, robot.dcm_control, "zmpRef", robot=robot, data_type="vector"
520 ) # reference ZMP
521 
522 create_topic(
523  robot.publisher, robot.dcm_control, "wrenchRef", robot=robot, data_type="vector"
524 ) # unoptimized reference wrench
525 create_topic(
526  robot.publisher, robot.distribute, "wrenchLeft", robot=robot, data_type="vector"
527 ) # reference left wrench
528 create_topic(
529  robot.publisher, robot.distribute, "wrenchRight", robot=robot, data_type="vector"
530 ) # reference right wrench
531 create_topic(
532  robot.publisher, robot.distribute, "wrenchRef", robot=robot, data_type="vector"
533 ) # optimized reference wrench
534 
535 create_topic(
536  robot.publisher, robot.ftc, "left_foot_force_out", robot=robot, data_type="vector"
537 ) # calibrated left wrench
538 create_topic(
539  robot.publisher, robot.ftc, "right_foot_force_out", robot=robot, data_type="vector"
540 ) # calibrated right wrench
541 
542 create_topic(
543  robot.publisher, robot.pitchController, "tauL", robot=robot, data_type="vector"
544 )
545 create_topic(
546  robot.publisher, robot.pitchController, "tauDesL", robot=robot, data_type="vector"
547 )
548 create_topic(
549  robot.publisher, robot.pitchController, "dqRefL", robot=robot, data_type="vector"
550 )
551 create_topic(
552  robot.publisher, robot.pitchController, "tauR", robot=robot, data_type="vector"
553 )
554 create_topic(
555  robot.publisher, robot.pitchController, "tauDesR", robot=robot, data_type="vector"
556 )
557 create_topic(
558  robot.publisher, robot.pitchController, "dqRefR", robot=robot, data_type="vector"
559 )
560 create_topic(
561  robot.publisher, robot.pitchController, "tauSum", robot=robot, data_type="vector"
562 )
563 create_topic(
564  robot.publisher, robot.pitchController, "tauDiff", robot=robot, data_type="vector"
565 )
566 create_topic(
567  robot.publisher, robot.pitchController, "tauDesSum", robot=robot, data_type="vector"
568 )
569 create_topic(
570  robot.publisher,
571  robot.pitchController,
572  "tauDesDiff",
573  robot=robot,
574  data_type="vector",
575 )
576 
577 create_topic(
578  robot.publisher, robot.rollController, "tauL", robot=robot, data_type="vector"
579 )
580 create_topic(
581  robot.publisher, robot.rollController, "tauDesL", robot=robot, data_type="vector"
582 )
583 create_topic(
584  robot.publisher, robot.rollController, "dqRefL", robot=robot, data_type="vector"
585 )
586 create_topic(
587  robot.publisher, robot.rollController, "tauR", robot=robot, data_type="vector"
588 )
589 create_topic(
590  robot.publisher, robot.rollController, "tauDesR", robot=robot, data_type="vector"
591 )
592 create_topic(
593  robot.publisher, robot.rollController, "dqRefR", robot=robot, data_type="vector"
594 )
595 create_topic(
596  robot.publisher, robot.rollController, "tauSum", robot=robot, data_type="vector"
597 )
598 create_topic(
599  robot.publisher, robot.rollController, "tauDiff", robot=robot, data_type="vector"
600 )
601 create_topic(
602  robot.publisher, robot.rollController, "tauDesSum", robot=robot, data_type="vector"
603 )
604 create_topic(
605  robot.publisher, robot.rollController, "tauDesDiff", robot=robot, data_type="vector"
606 )
607 
608 create_topic(
609  robot.publisher, robot.saturationRP, "yOut", robot=robot, data_type="vector"
610 )
611 create_topic(
612  robot.publisher, robot.saturationRR, "yOut", robot=robot, data_type="vector"
613 )
614 create_topic(
615  robot.publisher, robot.saturationLP, "yOut", robot=robot, data_type="vector"
616 )
617 create_topic(
618  robot.publisher, robot.saturationLR, "yOut", robot=robot, data_type="vector"
619 )
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