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