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