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