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