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