sot-talos-balance  2.0.5
Collection of dynamic-graph entities aimed at implementing balance control on talos.
appli_dcmZmpWaistControl.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.ft_calibration_conf as ft_conf
25 import dynamic_graph.sot_talos_balance.talos.parameter_server_conf as param_server_conf
26 from dynamic_graph.sot_talos_balance.create_entities_utils import *
27 from dynamic_graph.sot_talos_balance.simple_controller_6d import SimpleController6d
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 # -------------------------- DESIRED TRAJECTORY --------------------------
55 
56 # --- Trajectory generators
57 
58 # --- CoM
59 robot.comTrajGen = create_com_trajectory_generator(dt, robot)
60 
61 # --- Left foot
62 robot.lfTrajGen = create_pose_rpy_trajectory_generator(dt, robot, "LF")
63 # robot.lfTrajGen.x.recompute(0) # trigger computation of initial value
64 robot.lfToMatrix = PoseRollPitchYawToMatrixHomo("lf2m")
65 plug(robot.lfTrajGen.x, robot.lfToMatrix.sin)
66 
67 # --- Right foot
68 robot.rfTrajGen = create_pose_rpy_trajectory_generator(dt, robot, "RF")
69 # robot.rfTrajGen.x.recompute(0) # trigger computation of initial value
70 robot.rfToMatrix = PoseRollPitchYawToMatrixHomo("rf2m")
71 plug(robot.rfTrajGen.x, robot.rfToMatrix.sin)
72 
73 # --- Waist
74 robot.waistTrajGen = create_orientation_rpy_trajectory_generator(dt, robot, "WT")
75 # robot.waistTrajGen.x.recompute(0) # trigger computation of initial value
76 
77 robot.waistMix = Mix_of_vector("waistMix")
78 robot.waistMix.setSignalNumber(3)
79 robot.waistMix.addSelec(1, 0, 3)
80 robot.waistMix.addSelec(2, 3, 3)
81 robot.waistMix.default.value = [0.0] * 6
82 robot.waistMix.signal("sin1").value = [0.0] * 3
83 plug(robot.waistTrajGen.x, robot.waistMix.signal("sin2"))
84 
85 robot.waistToMatrix = PoseRollPitchYawToMatrixHomo("w2m")
86 plug(robot.waistMix.sout, robot.waistToMatrix.sin)
87 
88 # --- Rho
89 robot.rhoTrajGen = create_scalar_trajectory_generator(dt, 0.5, "rhoTrajGen")
90 robot.rhoScalar = Component_of_vector("rho_scalar")
91 robot.rhoScalar.setIndex(0)
92 plug(robot.rhoTrajGen.x, robot.rhoScalar.sin)
93 
94 # --- Interface with controller entities
95 
96 wp = DummyWalkingPatternGenerator("dummy_wp")
97 wp.init()
98 wp.omega.value = omega
99 plug(robot.waistToMatrix.sout, wp.waist)
100 plug(robot.lfToMatrix.sout, wp.footLeft)
101 plug(robot.rfToMatrix.sout, wp.footRight)
102 plug(robot.comTrajGen.x, wp.com)
103 plug(robot.comTrajGen.dx, wp.vcom)
104 plug(robot.comTrajGen.ddx, wp.acom)
105 
106 robot.wp = wp
107 
108 # --- Compute the values to use them in initialization
109 robot.wp.comDes.recompute(0)
110 robot.wp.dcmDes.recompute(0)
111 robot.wp.zmpDes.recompute(0)
112 
113 # -------------------------- ESTIMATION --------------------------
114 
115 # --- Base Estimation
116 robot.device_filters = create_device_filters(robot, dt)
117 robot.imu_filters = create_imu_filters(robot, dt)
118 robot.base_estimator = create_base_estimator(robot, dt, base_estimator_conf)
119 
120 robot.m2qLF = MatrixHomoToPoseQuaternion("m2qLF")
121 plug(robot.dynamic.LF, robot.m2qLF.sin)
122 plug(robot.m2qLF.sout, robot.base_estimator.lf_ref_xyzquat)
123 robot.m2qRF = MatrixHomoToPoseQuaternion("m2qRF")
124 plug(robot.dynamic.RF, robot.m2qRF.sin)
125 plug(robot.m2qRF.sout, robot.base_estimator.rf_ref_xyzquat)
126 
127 # robot.be_filters = create_be_filters(robot, dt)
128 
129 
130 
131 # rf = SimpleReferenceFrame('rf')
132 # rf.init(robot_name)
133 # plug(robot.dynamic.LF, rf.footLeft)
134 # plug(robot.dynamic.RF, rf.footRight)
135 # rf.reset.value = 1
136 # robot.rf = rf
137 
138 
145 
146 # --- Conversion
147 e2q = EulerToQuat("e2q")
148 plug(robot.base_estimator.q, e2q.euler)
149 robot.e2q = e2q
150 
151 # --- Kinematic computations
152 robot.rdynamic = DynamicPinocchio("real_dynamics")
153 robot.rdynamic.setModel(robot.dynamic.model)
154 robot.rdynamic.setData(robot.rdynamic.model.createData())
155 # plug(robot.base_estimator.q,robot.rdynamic.position)
156 
157 robot.baseselec = Selec_of_vector("base_selec")
158 robot.baseselec.selec(0, 6)
159 plug(robot.base_estimator.q, robot.baseselec.sin)
160 plug(robot.baseselec.sout, robot.rdynamic.ffposition)
161 
162 plug(robot.device.state, robot.rdynamic.position)
163 robot.rdynamic.velocity.value = [0.0] * robotDim
164 robot.rdynamic.acceleration.value = [0.0] * robotDim
165 
166 # --- CoM Estimation
167 cdc_estimator = DcmEstimator("cdc_estimator")
168 cdc_estimator.init(dt, robot_name)
169 plug(robot.e2q.quaternion, cdc_estimator.q)
170 plug(robot.base_estimator.v, cdc_estimator.v)
171 robot.cdc_estimator = cdc_estimator
172 
173 # --- DCM Estimation
174 estimator = DummyDcmEstimator("dummy")
175 estimator.omega.value = omega
176 estimator.mass.value = 1.0
177 plug(robot.cdc_estimator.c, estimator.com)
178 plug(robot.cdc_estimator.dc, estimator.momenta)
179 estimator.init()
180 robot.estimator = estimator
181 
182 # --- Force calibration
183 robot.ftc = create_ft_calibrator(robot, ft_conf)
184 
185 # --- ZMP estimation
186 zmp_estimator = SimpleZmpEstimator("zmpEst")
187 robot.rdynamic.createOpPoint("sole_LF", "left_sole_link")
188 robot.rdynamic.createOpPoint("sole_RF", "right_sole_link")
189 plug(robot.rdynamic.sole_LF, zmp_estimator.poseLeft)
190 plug(robot.rdynamic.sole_RF, zmp_estimator.poseRight)
191 plug(robot.ftc.left_foot_force_out, zmp_estimator.wrenchLeft)
192 plug(robot.ftc.right_foot_force_out, zmp_estimator.wrenchRight)
193 zmp_estimator.init()
194 robot.zmp_estimator = zmp_estimator
195 
196 # -------------------------- ADMITTANCE CONTROL --------------------------
197 
198 # --- DCM controller
199 Kp_dcm = [5.0, 5.0, 5.0]
200 Ki_dcm = [0.0, 0.0, 0.0] # zero (to be set later)
201 gamma_dcm = 0.2
202 
203 dcm_controller = DcmController("dcmCtrl")
204 
205 dcm_controller.Kp.value = Kp_dcm
206 dcm_controller.Ki.value = Ki_dcm
207 dcm_controller.decayFactor.value = gamma_dcm
208 dcm_controller.mass.value = mass
209 dcm_controller.omega.value = omega
210 
211 plug(robot.cdc_estimator.c, dcm_controller.com)
212 plug(robot.estimator.dcm, dcm_controller.dcm)
213 
214 plug(robot.wp.zmpDes, dcm_controller.zmpDes)
215 plug(robot.wp.dcmDes, dcm_controller.dcmDes)
216 
217 dcm_controller.init(dt)
218 
219 robot.dcm_control = dcm_controller
220 
221 Ki_dcm = [1.0, 1.0, 1.0] # this value is employed later
222 
223 # --- CoM admittance controller
224 Kp_adm = [0.0, 0.0, 0.0] # zero (to be set later)
225 
226 com_admittance_control = ComAdmittanceController("comAdmCtrl")
227 com_admittance_control.Kp.value = Kp_adm
228 plug(robot.zmp_estimator.zmp, com_admittance_control.zmp)
229 com_admittance_control.zmpDes.value = (
230  robot.wp.zmpDes.value
231 ) # should be plugged to robot.dcm_control.zmpRef
232 plug(robot.wp.acomDes, com_admittance_control.ddcomDes)
233 
234 com_admittance_control.init(dt)
235 com_admittance_control.setState(robot.wp.comDes.value, [0.0, 0.0, 0.0])
236 
237 robot.com_admittance_control = com_admittance_control
238 
239 Kp_adm = [20.0, 10.0, 0.0] # this value is employed later
240 
241 # --- Control Manager
242 robot.cm = create_ctrl_manager(cm_conf, dt, robot_name="robot")
243 robot.cm.addCtrlMode("sot_input")
244 robot.cm.setCtrlMode("all", "sot_input")
245 robot.cm.addEmergencyStopSIN("zmp")
246 
247 # -------------------------- SOT CONTROL --------------------------
248 
249 # --- Upper body
250 robot.taskUpperBody = Task("task_upper_body")
251 robot.taskUpperBody.feature = FeaturePosture("feature_upper_body")
252 
253 q = list(robot.dynamic.position.value)
254 robot.taskUpperBody.feature.state.value = q
255 robot.taskUpperBody.feature.posture.value = q
256 
257 # robotDim = robot.dynamic.getDimension() # 38
258 robot.taskUpperBody.feature.selectDof(18, True)
259 robot.taskUpperBody.feature.selectDof(19, True)
260 robot.taskUpperBody.feature.selectDof(20, True)
261 robot.taskUpperBody.feature.selectDof(21, True)
262 robot.taskUpperBody.feature.selectDof(22, True)
263 robot.taskUpperBody.feature.selectDof(23, True)
264 robot.taskUpperBody.feature.selectDof(24, True)
265 robot.taskUpperBody.feature.selectDof(25, True)
266 robot.taskUpperBody.feature.selectDof(26, True)
267 robot.taskUpperBody.feature.selectDof(27, True)
268 robot.taskUpperBody.feature.selectDof(28, True)
269 robot.taskUpperBody.feature.selectDof(29, True)
270 robot.taskUpperBody.feature.selectDof(30, True)
271 robot.taskUpperBody.feature.selectDof(31, True)
272 robot.taskUpperBody.feature.selectDof(32, True)
273 robot.taskUpperBody.feature.selectDof(33, True)
274 robot.taskUpperBody.feature.selectDof(34, True)
275 robot.taskUpperBody.feature.selectDof(35, True)
276 robot.taskUpperBody.feature.selectDof(36, True)
277 robot.taskUpperBody.feature.selectDof(37, True)
278 
279 robot.taskUpperBody.controlGain.value = 100.0
280 robot.taskUpperBody.add(robot.taskUpperBody.feature.name)
281 plug(robot.dynamic.position, robot.taskUpperBody.feature.state)
282 
283 # --- CONTACTS
284 # define contactLF and contactRF
285 robot.contactLF = MetaTaskKine6d(
286  "contactLF", robot.rdynamic, "LF", robot.OperationalPointsMap["left-ankle"]
287 )
288 robot.contactLF.feature.frame("desired")
289 robot.contactLF.gain.setConstant(0)
290 plug(robot.wp.footLeftDes, robot.contactLF.featureDes.position) # .errorIN?
291 locals()["contactLF"] = robot.contactLF
292 
293 robot.contactRF = MetaTaskKine6d(
294  "contactRF", robot.rdynamic, "RF", robot.OperationalPointsMap["right-ankle"]
295 )
296 robot.contactRF.feature.frame("desired")
297 robot.contactRF.gain.setConstant(0)
298 plug(robot.wp.footRightDes, robot.contactRF.featureDes.position) # .errorIN?
299 locals()["contactRF"] = robot.contactRF
300 
301 # --- COM height
302 robot.taskComH = MetaTaskKineCom(robot.rdynamic, name="comH")
303 plug(robot.wp.comDes, robot.taskComH.featureDes.errorIN)
304 robot.taskComH.task.controlGain.value = 1.0
305 robot.taskComH.feature.selec.value = "100"
306 
307 # --- COM
308 robot.taskCom = MetaTaskKineCom(robot.rdynamic)
309 plug(robot.com_admittance_control.comRef, robot.taskCom.featureDes.errorIN)
310 plug(robot.com_admittance_control.dcomRef, robot.taskCom.featureDes.errordotIN)
311 robot.taskCom.task.controlGain.value = 0
312 robot.taskCom.task.setWithDerivative(True)
313 robot.taskCom.feature.selec.value = "011"
314 
315 # --- Waist
316 robot.keepWaist = MetaTaskKine6d(
317  "keepWaist", robot.rdynamic, "WT", robot.OperationalPointsMap["waist"]
318 )
319 robot.keepWaist.feature.frame("desired")
320 robot.keepWaist.gain.setConstant(1.0)
321 plug(robot.wp.waistDes, robot.keepWaist.featureDes.position)
322 robot.keepWaist.feature.selec.value = "111000"
323 locals()["keepWaist"] = robot.keepWaist
324 
325 # --- SOT solver
326 robot.sot = SOT("sot")
327 robot.sot.setSize(robot.dynamic.getDimension())
328 
329 # --- Plug SOT control to device through control manager
330 plug(robot.sot.control, robot.cm.ctrl_sot_input)
331 plug(robot.cm.u_safe, robot.device.control)
332 
333 robot.sot.push(robot.taskUpperBody.name)
334 robot.sot.push(robot.contactRF.task.name)
335 robot.sot.push(robot.contactLF.task.name)
336 robot.sot.push(robot.taskComH.task.name)
337 robot.sot.push(robot.taskCom.task.name)
338 robot.sot.push(robot.keepWaist.task.name)
339 # robot.sot.push(robot.taskWaist.name)
340 
341 # --- Fix robot.dynamic inputs
342 plug(robot.device.velocity, robot.dynamic.velocity)
343 robot.dvdt = Derivator_of_Vector("dv_dt")
344 robot.dvdt.dt.value = dt
345 plug(robot.device.velocity, robot.dvdt.sin)
346 plug(robot.dvdt.sout, robot.dynamic.acceleration)
347 
348 # -------------------------- PLOTS --------------------------
349 
350 # --- ROS PUBLISHER
351 robot.publisher = create_rospublish(robot, "robot_publisher")
352 
353 create_topic(
354  robot.publisher, robot.wp, "comDes", robot=robot, data_type="vector"
355 ) # desired CoM
356 
357 create_topic(
358  robot.publisher, robot.cdc_estimator, "c", robot=robot, data_type="vector"
359 ) # estimated CoM
360 create_topic(
361  robot.publisher, robot.cdc_estimator, "dc", robot=robot, data_type="vector"
362 ) # estimated CoM velocity
363 
364 create_topic(
365  robot.publisher, robot.dynamic, "com", robot=robot, data_type="vector"
366 ) # resulting SOT CoM
367 
368 create_topic(
369  robot.publisher, robot.dcm_control, "dcmDes", robot=robot, data_type="vector"
370 ) # desired DCM
371 create_topic(
372  robot.publisher, robot.estimator, "dcm", robot=robot, data_type="vector"
373 ) # estimated DCM
374 
375 create_topic(
376  robot.publisher, robot.dcm_control, "zmpDes", robot=robot, data_type="vector"
377 ) # desired ZMP
378 create_topic(
379  robot.publisher, robot.dynamic, "zmp", robot=robot, data_type="vector"
380 ) # SOT ZMP
381 create_topic(
382  robot.publisher, robot.zmp_estimator, "zmp", robot=robot, data_type="vector"
383 ) # estimated ZMP
384 create_topic(
385  robot.publisher, robot.dcm_control, "zmpRef", robot=robot, data_type="vector"
386 ) # reference ZMP
387 
388 create_topic(
389  robot.publisher, robot.dcm_control, "wrenchRef", robot=robot, data_type="vector"
390 ) # unoptimized reference wrench
391 
392 # create_topic(robot.publisher, robot.device, 'forceLLEG', robot = robot, data_type='vector') # measured left wrench
393 # create_topic(robot.publisher, robot.device, 'forceRLEG', robot = robot, data_type='vector') # measured right wrench
394 
395 # create_topic(robot.publisher, robot.device_filters.ft_LF_filter, 'x_filtered', robot = robot, data_type='vector') # filtered left wrench
396 # create_topic(robot.publisher, robot.device_filters.ft_RF_filter, 'x_filtered', robot = robot, data_type='vector') # filtered right wrench
397 
398 create_topic(
399  robot.publisher, robot.ftc, "left_foot_force_out", robot=robot, data_type="vector"
400 ) # calibrated left wrench
401 create_topic(
402  robot.publisher, robot.ftc, "right_foot_force_out", robot=robot, data_type="vector"
403 ) # calibrated right wrench
404 
405 create_topic(
406  robot.publisher, robot.zmp_estimator, "copRight", robot=robot, data_type="vector"
407 )
408 create_topic(
409  robot.publisher, robot.zmp_estimator, "copLeft", robot=robot, data_type="vector"
410 )
411 
412 
417 
418 # addTrace(robot.tracer, robot.wp, 'comDes') # desired CoM
419 
420 # addTrace(robot.tracer, robot.cdc_estimator, 'c') # estimated CoM
421 # addTrace(robot.tracer, robot.cdc_estimator, 'dc') # estimated CoM velocity
422 
423 # addTrace(robot.tracer, robot.com_admittance_control, 'comRef') # reference CoM
424 # addTrace(robot.tracer, robot.dynamic, 'com') # resulting SOT CoM
425 
426 # addTrace(robot.tracer, robot.dcm_control, 'dcmDes') # desired DCM
427 # addTrace(robot.tracer, robot.estimator, 'dcm') # estimated DCM
428 
429 # addTrace(robot.tracer, robot.dcm_control, 'zmpDes') # desired ZMP
430 # addTrace(robot.tracer, robot.dynamic, 'zmp') # SOT ZMP
431 # addTrace(robot.tracer, robot.zmp_estimator, 'zmp') # estimated ZMP
432 # addTrace(robot.tracer, robot.dcm_control, 'zmpRef') # reference ZMP
433 
434 # addTrace(robot.tracer, robot.dcm_control, 'wrenchRef') # unoptimized reference wrench
435 
436 # addTrace(robot.tracer, robot.ftc, 'left_foot_force_out') # calibrated left wrench
437 # addTrace(robot.tracer, robot.ftc, 'right_foot_force_out') # calibrated right wrench
438 
439 # robot.tracer.start()
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