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