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