sot-talos-balance  2.0.5
Collection of dynamic-graph entities aimed at implementing balance control on talos.
appli_dcmCopControl.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 # --- Ankle controllers
233 LeftPitchJoint = 4
234 LeftRollJoint = 5
235 RightPitchJoint = 10
236 RightRollJoint = 11
237 
238 Kp_ankles = [1e-3] * 2
239 
240 # --- Right ankle
241 controller = AnkleAdmittanceController("rightController")
242 plug(robot.ftc.right_foot_force_out, controller.wrench)
243 controller.gainsXY.value = [0.0] * 2
244 plug(robot.distribute.copRight, controller.pRef)
245 controller.init()
246 robot.rightAnkleController = controller
247 
248 # --- Left ankle
249 controller = AnkleAdmittanceController("leftController")
250 plug(robot.ftc.left_foot_force_out, controller.wrench)
251 controller.gainsXY.value = [0.0] * 2
252 plug(robot.distribute.copLeft, controller.pRef)
253 controller.init()
254 robot.leftAnkleController = controller
255 
256 # --- Control Manager
257 robot.cm = create_ctrl_manager(cm_conf, dt, robot_name="robot")
258 robot.cm.addCtrlMode("sot_input")
259 robot.cm.setCtrlMode("all", "sot_input")
260 robot.cm.addEmergencyStopSIN("zmp")
261 robot.cm.addEmergencyStopSIN("distribute")
262 
263 # -------------------------- SOT CONTROL --------------------------
264 
265 # --- Upper body
266 robot.taskUpperBody = Task("task_upper_body")
267 robot.taskUpperBody.feature = FeaturePosture("feature_upper_body")
268 
269 q = list(robot.dynamic.position.value)
270 robot.taskUpperBody.feature.state.value = q
271 robot.taskUpperBody.feature.posture.value = q
272 
273 # robotDim = robot.dynamic.getDimension() # 38
274 robot.taskUpperBody.feature.selectDof(18, True)
275 robot.taskUpperBody.feature.selectDof(19, True)
276 robot.taskUpperBody.feature.selectDof(20, True)
277 robot.taskUpperBody.feature.selectDof(21, True)
278 robot.taskUpperBody.feature.selectDof(22, True)
279 robot.taskUpperBody.feature.selectDof(23, True)
280 robot.taskUpperBody.feature.selectDof(24, True)
281 robot.taskUpperBody.feature.selectDof(25, True)
282 robot.taskUpperBody.feature.selectDof(26, True)
283 robot.taskUpperBody.feature.selectDof(27, True)
284 robot.taskUpperBody.feature.selectDof(28, True)
285 robot.taskUpperBody.feature.selectDof(29, True)
286 robot.taskUpperBody.feature.selectDof(30, True)
287 robot.taskUpperBody.feature.selectDof(31, True)
288 robot.taskUpperBody.feature.selectDof(32, True)
289 robot.taskUpperBody.feature.selectDof(33, True)
290 robot.taskUpperBody.feature.selectDof(34, True)
291 robot.taskUpperBody.feature.selectDof(35, True)
292 robot.taskUpperBody.feature.selectDof(36, True)
293 robot.taskUpperBody.feature.selectDof(37, True)
294 
295 robot.taskUpperBody.controlGain.value = 100.0
296 robot.taskUpperBody.add(robot.taskUpperBody.feature.name)
297 plug(robot.dynamic.position, robot.taskUpperBody.feature.state)
298 
299 # --- CONTACTS
300 # define contactLF and contactRF
301 robot.contactLF = MetaTaskKine6d(
302  "contactLF", robot.dynamic, "LF", robot.OperationalPointsMap["left-ankle"]
303 )
304 robot.contactLF.feature.frame("desired")
305 robot.contactLF.gain.setConstant(300)
306 plug(robot.wp.footLeftDes, robot.contactLF.featureDes.position) # .errorIN?
307 plug(robot.leftAnkleController.vDes, robot.contactLF.featureDes.velocity)
308 robot.contactLF.task.setWithDerivative(True)
309 locals()["contactLF"] = robot.contactLF
310 
311 robot.contactRF = MetaTaskKine6d(
312  "contactRF", robot.dynamic, "RF", robot.OperationalPointsMap["right-ankle"]
313 )
314 robot.contactRF.feature.frame("desired")
315 robot.contactRF.gain.setConstant(300)
316 plug(robot.wp.footRightDes, robot.contactRF.featureDes.position) # .errorIN?
317 plug(robot.rightAnkleController.vDes, robot.contactRF.featureDes.velocity)
318 robot.contactRF.task.setWithDerivative(True)
319 locals()["contactRF"] = robot.contactRF
320 
321 # --- COM
322 robot.taskCom = MetaTaskKineCom(robot.dynamic)
323 plug(robot.wp.comDes, robot.taskCom.featureDes.errorIN)
324 robot.taskCom.task.controlGain.value = 10
325 
326 # --- Waist
327 robot.keepWaist = MetaTaskKine6d(
328  "keepWaist", robot.dynamic, "WT", robot.OperationalPointsMap["waist"]
329 )
330 robot.keepWaist.feature.frame("desired")
331 robot.keepWaist.gain.setConstant(300)
332 plug(robot.wp.waistDes, robot.keepWaist.featureDes.position)
333 robot.keepWaist.feature.selec.value = "111000"
334 locals()["keepWaist"] = robot.keepWaist
335 
336 # --- SOT solver
337 robot.sot = SOT("sot")
338 robot.sot.setSize(robot.dynamic.getDimension())
339 
340 # --- Plug SOT control to device through control manager
341 plug(robot.sot.control, robot.cm.ctrl_sot_input)
342 plug(robot.cm.u_safe, robot.device.control)
343 
344 robot.sot.push(robot.taskUpperBody.name)
345 robot.sot.push(robot.contactRF.task.name)
346 robot.sot.push(robot.contactLF.task.name)
347 robot.sot.push(robot.taskCom.task.name)
348 robot.sot.push(robot.keepWaist.task.name)
349 
350 # --- Fix robot.dynamic inputs
351 plug(robot.device.velocity, robot.dynamic.velocity)
352 robot.dvdt = Derivator_of_Vector("dv_dt")
353 robot.dvdt.dt.value = dt
354 plug(robot.device.velocity, robot.dvdt.sin)
355 plug(robot.dvdt.sout, robot.dynamic.acceleration)
356 
357 # -------------------------- PLOTS --------------------------
358 
359 # --- ROS PUBLISHER
360 robot.publisher = create_rospublish(robot, "robot_publisher")
361 
362 create_topic(
363  robot.publisher, robot.wp, "comDes", robot=robot, data_type="vector"
364 ) # desired CoM
365 
366 create_topic(
367  robot.publisher, robot.cdc_estimator, "c", robot=robot, data_type="vector"
368 ) # estimated CoM
369 create_topic(
370  robot.publisher, robot.cdc_estimator, "dc", robot=robot, data_type="vector"
371 ) # estimated CoM velocity
372 
373 create_topic(
374  robot.publisher, robot.dynamic, "com", robot=robot, data_type="vector"
375 ) # resulting SOT CoM
376 
377 create_topic(
378  robot.publisher, robot.dcm_control, "dcmDes", robot=robot, data_type="vector"
379 ) # desired DCM
380 create_topic(
381  robot.publisher, robot.estimator, "dcm", robot=robot, data_type="vector"
382 ) # estimated DCM
383 
384 create_topic(
385  robot.publisher, robot.dcm_control, "zmpDes", robot=robot, data_type="vector"
386 ) # desired ZMP
387 create_topic(
388  robot.publisher, robot.dynamic, "zmp", robot=robot, data_type="vector"
389 ) # SOT ZMP
390 create_topic(
391  robot.publisher, robot.zmp_estimator, "zmp", robot=robot, data_type="vector"
392 ) # estimated ZMP
393 create_topic(
394  robot.publisher, robot.dcm_control, "zmpRef", robot=robot, data_type="vector"
395 ) # reference ZMP
396 
397 create_topic(
398  robot.publisher, robot.dcm_control, "wrenchRef", robot=robot, data_type="vector"
399 ) # unoptimized reference wrench
400 create_topic(
401  robot.publisher, robot.distribute, "wrenchLeft", robot=robot, data_type="vector"
402 ) # reference left wrench
403 create_topic(
404  robot.publisher, robot.distribute, "wrenchRight", robot=robot, data_type="vector"
405 ) # reference right wrench
406 create_topic(
407  robot.publisher, robot.distribute, "wrenchRef", robot=robot, data_type="vector"
408 ) # optimized reference wrench
409 
410 # create_topic(robot.publisher, robot.device, 'forceLLEG', robot = robot, data_type='vector') # measured left wrench
411 # create_topic(robot.publisher, robot.device, 'forceRLEG', robot = robot, data_type='vector') # measured right wrench
412 
413 # create_topic(robot.publisher, robot.device_filters.ft_LF_filter, 'x_filtered', robot = robot, data_type='vector') # filtered left wrench
414 # create_topic(robot.publisher, robot.device_filters.ft_RF_filter, 'x_filtered', robot = robot, data_type='vector') # filtered right wrench
415 
416 create_topic(
417  robot.publisher, robot.ftc, "left_foot_force_out", robot=robot, data_type="vector"
418 ) # calibrated left wrench
419 create_topic(
420  robot.publisher, robot.ftc, "right_foot_force_out", robot=robot, data_type="vector"
421 ) # calibrated right wrench
422 
423 create_topic(
424  robot.publisher, robot.zmp_estimator, "copRight", robot=robot, data_type="vector"
425 )
426 create_topic(
427  robot.publisher, robot.rightAnkleController, "pRef", robot=robot, data_type="vector"
428 )
429 create_topic(
430  robot.publisher, robot.rightAnkleController, "dRP", robot=robot, data_type="vector"
431 )
432 
433 create_topic(
434  robot.publisher, robot.zmp_estimator, "copLeft", robot=robot, data_type="vector"
435 )
436 create_topic(
437  robot.publisher, robot.leftAnkleController, "pRef", robot=robot, data_type="vector"
438 )
439 create_topic(
440  robot.publisher, robot.leftAnkleController, "dRP", robot=robot, data_type="vector"
441 )
442 
443 
448 
449 # addTrace(robot.tracer, robot.wp, 'comDes') # desired CoM
450 
451 # addTrace(robot.tracer, robot.cdc_estimator, 'c') # estimated CoM
452 # addTrace(robot.tracer, robot.cdc_estimator, 'dc') # estimated CoM velocity
453 
454 # addTrace(robot.tracer, robot.com_admittance_control, 'comRef') # reference CoM
455 # addTrace(robot.tracer, robot.dynamic, 'com') # resulting SOT CoM
456 
457 # addTrace(robot.tracer, robot.dcm_control, 'dcmDes') # desired DCM
458 # addTrace(robot.tracer, robot.estimator, 'dcm') # estimated DCM
459 
460 # addTrace(robot.tracer, robot.dcm_control, 'zmpDes') # desired ZMP
461 # addTrace(robot.tracer, robot.dynamic, 'zmp') # SOT ZMP
462 # addTrace(robot.tracer, robot.zmp_estimator, 'zmp') # estimated ZMP
463 # addTrace(robot.tracer, robot.dcm_control, 'zmpRef') # reference ZMP
464 
465 # addTrace(robot.tracer, robot.dcm_control, 'wrenchRef') # unoptimized reference wrench
466 # addTrace(robot.tracer, robot.distribute, 'wrenchLeft') # reference left wrench
467 # addTrace(robot.tracer, robot.distribute, 'wrenchRight') # reference right wrench
468 # addTrace(robot.tracer, robot.distribute, 'wrenchRef') # optimized reference wrench
469 
470 # addTrace(robot.tracer, robot.ftc, 'left_foot_force_out') # calibrated left wrench
471 # addTrace(robot.tracer, robot.ftc, 'right_foot_force_out') # calibrated right wrench
472 
473 # 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