sot-talos-balance  2.0.5
Collection of dynamic-graph entities aimed at implementing balance control on talos.
appli_dcmZmpControl_feedback.py
Go to the documentation of this file.
1 # flake8: noqa
2 from math import sqrt
3 
4 from dynamic_graph import plug
5 from dynamic_graph.sot.core import SOT, Derivator_of_Vector, FeaturePosture, Task
6 from dynamic_graph.sot.core.matrix_util import matrixToTuple
7 from dynamic_graph.sot.core.meta_tasks_kine import (
8  MetaTaskKine6d,
9  MetaTaskKineCom,
10  gotoNd,
11 )
12 from dynamic_graph.tracer_real_time import TracerRealTime
13 
14 import dynamic_graph.sot_talos_balance.talos.base_estimator_conf as base_estimator_conf
15 import dynamic_graph.sot_talos_balance.talos.control_manager_conf as cm_conf
16 import dynamic_graph.sot_talos_balance.talos.ft_calibration_conf as ft_conf
17 import dynamic_graph.sot_talos_balance.talos.parameter_server_conf as param_server_conf
18 from dynamic_graph.sot_talos_balance.create_entities_utils import *
19 
20 robot.timeStep = robot.device.getTimeStep()
21 dt = robot.timeStep
22 
23 # -------------------------- DESIRED TRAJECTORY --------------------------
24 
25 # --- Desired values
26 
27 # --- Desired CoM
28 robot.dynamic.com.recompute(0)
29 comDes = robot.dynamic.com.value
30 
31 
36 
37 # --- Desired DCM and ZMP
38 dcmDes = comDes
39 zmpDes = comDes[:2] + (0.0,)
40 
41 # --- Desired CoM acceleration
42 ddcomDes = (0.0, 0.0, 0.0)
43 
44 # --- Pendulum parameters
45 robot_name = "robot"
46 robotDim = robot.dynamic.getDimension()
47 mass = robot.dynamic.data.mass[0]
48 h = robot.dynamic.com.value[2]
49 g = 9.81
50 omega = sqrt(g / h)
51 
52 # -------------------------- ESTIMATION --------------------------
53 
54 # --- REAL ESTIMATION - diagnostics only
55 
56 # --- Base Estimation
57 robot.param_server = create_parameter_server(param_server_conf, dt)
58 robot.device_filters = create_device_filters(robot, dt)
59 robot.imu_filters = create_imu_filters(robot, dt)
60 robot.base_estimator = create_base_estimator(robot, dt, base_estimator_conf)
61 # robot.be_filters = create_be_filters(robot, dt)
62 
63 # --- Conversion
64 e2q = EulerToQuat("e2q")
65 plug(robot.base_estimator.q, e2q.euler)
66 robot.e2q = e2q
67 
68 # --- CoM Estimation
69 cdc_estimator = DcmEstimator("cdc_estimator")
70 cdc_estimator.init(dt, robot_name)
71 plug(robot.e2q.quaternion, cdc_estimator.q)
72 plug(robot.base_estimator.v, cdc_estimator.v)
73 robot.cdc_estimator = cdc_estimator
74 
75 # --- SOT ESTIMATION
76 
77 # --- DCM Estimation
78 estimator = DummyDcmEstimator("dummy")
79 estimator.omega.value = omega
80 estimator.mass.value = mass
81 plug(robot.dynamic.com, estimator.com)
82 plug(robot.dynamic.momenta, estimator.momenta)
83 estimator.init()
84 robot.estimator = estimator
85 
86 # --- dc Estimation - diagnostics only
87 estimatorDc = DummyDcmEstimator("dcest")
88 estimatorDc.omega.value = 1.0
89 estimatorDc.mass.value = mass
90 estimatorDc.com.value = (0.0, 0.0, 0.0)
91 plug(robot.dynamic.momenta, estimatorDc.momenta)
92 estimatorDc.init()
93 robot.estimatorDc = estimatorDc
94 
95 # --- filters - already created
96 # filters = Bunch()
97 # filters.ft_RF_filter = create_butter_lp_filter_Wn_04_N_2("ft_RF_filter", dt, 6)
98 # filters.ft_LF_filter = create_butter_lp_filter_Wn_04_N_2("ft_LF_filter", dt, 6)
99 # plug(robot.device.forceRLEG, filters.ft_RF_filter.x)
100 # plug(robot.device.forceLLEG, filters.ft_LF_filter.x)
101 # robot.device_filters = filters
102 
103 # --- Force calibration
104 robot.ftc = create_ft_calibrator(robot, ft_conf)
105 
106 # --- ZMP estimation
107 zmp_estimator = SimpleZmpEstimator("zmpEst")
108 robot.dynamic.createOpPoint("sole_LF", "left_sole_link")
109 robot.dynamic.createOpPoint("sole_RF", "right_sole_link")
110 plug(robot.dynamic.sole_LF, zmp_estimator.poseLeft)
111 plug(robot.dynamic.sole_RF, zmp_estimator.poseRight)
112 plug(robot.ftc.left_foot_force_out, zmp_estimator.wrenchLeft)
113 plug(robot.ftc.right_foot_force_out, zmp_estimator.wrenchRight)
114 zmp_estimator.init()
115 robot.zmp_estimator = zmp_estimator
116 
117 # -------------------------- ADMITTANCE CONTROL --------------------------
118 
119 # --- DCM controller
120 Kp_dcm = [5.0, 5.0, 5.0]
121 Ki_dcm = [0.0, 0.0, 0.0] # zero (to be set later)
122 gamma_dcm = 0.2
123 
124 dcm_controller = DcmController("dcmCtrl")
125 
126 dcm_controller.Kp.value = Kp_dcm
127 dcm_controller.Ki.value = Ki_dcm
128 dcm_controller.decayFactor.value = gamma_dcm
129 dcm_controller.mass.value = mass
130 dcm_controller.omega.value = omega
131 
132 plug(robot.dynamic.com, dcm_controller.com)
133 plug(robot.estimator.dcm, dcm_controller.dcm)
134 
135 dcm_controller.zmpDes.value = zmpDes # plug a signal here
136 dcm_controller.dcmDes.value = dcmDes # plug a signal here
137 
138 dcm_controller.init(dt)
139 
140 robot.dcm_control = dcm_controller
141 
142 Ki_dcm = [1.0, 1.0, 1.0] # this value is employed later
143 
144 # --- CoM admittance controller
145 Kp_adm = [0.0, 0.0, 0.0] # zero (to be set later)
146 
147 com_admittance_control = ComAdmittanceController("comAdmCtrl")
148 com_admittance_control.Kp.value = Kp_adm
149 plug(robot.zmp_estimator.zmp, com_admittance_control.zmp)
150 com_admittance_control.zmpDes.value = (
151  zmpDes # should be plugged to robot.dcm_control.zmpRef
152 )
153 com_admittance_control.ddcomDes.value = ddcomDes # plug a signal here
154 
155 com_admittance_control.init(dt)
156 com_admittance_control.setState(comDes, [0.0, 0.0, 0.0])
157 
158 robot.com_admittance_control = com_admittance_control
159 
160 Kp_adm = [20.0, 10.0, 0.0] # this value is employed later
161 
162 # --- Control Manager
163 robot.cm = create_ctrl_manager(cm_conf, dt, robot_name="robot")
164 robot.cm.addCtrlMode("sot_input")
165 robot.cm.setCtrlMode("all", "sot_input")
166 robot.cm.addEmergencyStopSIN("zmp")
167 
168 # -------------------------- SOT CONTROL --------------------------
169 
170 # --- Upper body
171 robot.taskUpperBody = Task("task_upper_body")
172 robot.taskUpperBody.feature = FeaturePosture("feature_upper_body")
173 
174 q = list(robot.dynamic.position.value)
175 robot.taskUpperBody.feature.state.value = q
176 robot.taskUpperBody.feature.posture.value = q
177 
178 # robotDim = robot.dynamic.getDimension() # 38
179 robot.taskUpperBody.feature.selectDof(18, True)
180 robot.taskUpperBody.feature.selectDof(19, True)
181 robot.taskUpperBody.feature.selectDof(20, True)
182 robot.taskUpperBody.feature.selectDof(21, True)
183 robot.taskUpperBody.feature.selectDof(22, True)
184 robot.taskUpperBody.feature.selectDof(23, True)
185 robot.taskUpperBody.feature.selectDof(24, True)
186 robot.taskUpperBody.feature.selectDof(25, True)
187 robot.taskUpperBody.feature.selectDof(26, True)
188 robot.taskUpperBody.feature.selectDof(27, True)
189 robot.taskUpperBody.feature.selectDof(28, True)
190 robot.taskUpperBody.feature.selectDof(29, True)
191 robot.taskUpperBody.feature.selectDof(30, True)
192 robot.taskUpperBody.feature.selectDof(31, True)
193 robot.taskUpperBody.feature.selectDof(32, True)
194 robot.taskUpperBody.feature.selectDof(33, True)
195 robot.taskUpperBody.feature.selectDof(34, True)
196 robot.taskUpperBody.feature.selectDof(35, True)
197 robot.taskUpperBody.feature.selectDof(36, True)
198 robot.taskUpperBody.feature.selectDof(37, True)
199 
200 robot.taskUpperBody.controlGain.value = 100.0
201 robot.taskUpperBody.add(robot.taskUpperBody.feature.name)
202 plug(robot.dynamic.position, robot.taskUpperBody.feature.state)
203 
204 # --- CONTACTS
205 # define contactLF and contactRF
206 robot.contactLF = MetaTaskKine6d(
207  "contactLF", robot.dynamic, "LF", robot.OperationalPointsMap["left-ankle"]
208 )
209 robot.contactLF.feature.frame("desired")
210 robot.contactLF.gain.setConstant(300)
211 robot.contactLF.keep()
212 locals()["contactLF"] = robot.contactLF
213 
214 robot.contactRF = MetaTaskKine6d(
215  "contactRF", robot.dynamic, "RF", robot.OperationalPointsMap["right-ankle"]
216 )
217 robot.contactRF.feature.frame("desired")
218 robot.contactRF.gain.setConstant(300)
219 robot.contactRF.keep()
220 locals()["contactRF"] = robot.contactRF
221 
222 # --- COM
223 robot.taskCom = MetaTaskKineCom(robot.dynamic)
224 plug(robot.com_admittance_control.comRef, robot.taskCom.featureDes.errorIN)
225 plug(robot.com_admittance_control.dcomRef, robot.taskCom.featureDes.errordotIN)
226 robot.taskCom.task.controlGain.value = 0
227 robot.taskCom.task.setWithDerivative(True)
228 
229 # --- Waist
230 robot.keepWaist = MetaTaskKine6d(
231  "keepWaist", robot.dynamic, "WT", robot.OperationalPointsMap["waist"]
232 )
233 robot.keepWaist.feature.frame("desired")
234 robot.keepWaist.gain.setConstant(300)
235 robot.keepWaist.keep()
236 robot.keepWaist.feature.selec.value = "111000"
237 locals()["keepWaist"] = robot.keepWaist
238 
239 # # --- Posture
240 # robot.taskPos = Task ('task_pos')
241 # robot.taskPos.feature = FeaturePosture('feature_pos')
242 #
243 # q = list(robot.dynamic.position.value)
244 # robot.taskPos.feature.state.value = q
245 # robot.taskPos.feature.posture.value = q
246 
247 # robotDim = robot.dynamic.getDimension() # 38
248 # robot.taskPos.feature.selectDof(6,True)
249 # robot.taskPos.feature.selectDof(7,True)
250 # robot.taskPos.feature.selectDof(8,True)
251 # robot.taskPos.feature.selectDof(9,True)
252 # robot.taskPos.feature.selectDof(10,True)
253 # robot.taskPos.feature.selectDof(11,True)
254 # robot.taskPos.feature.selectDof(12,True)
255 # robot.taskPos.feature.selectDof(13,True)
256 # robot.taskPos.feature.selectDof(14,True)
257 # robot.taskPos.feature.selectDof(15,True)
258 # robot.taskPos.feature.selectDof(16,True)
259 # robot.taskPos.feature.selectDof(17,True)
260 # robot.taskPos.feature.selectDof(18,True)
261 # robot.taskPos.feature.selectDof(19,True)
262 # robot.taskPos.feature.selectDof(20,True)
263 # robot.taskPos.feature.selectDof(21,True)
264 # robot.taskPos.feature.selectDof(22,True)
265 # robot.taskPos.feature.selectDof(23,True)
266 # robot.taskPos.feature.selectDof(24,True)
267 # robot.taskPos.feature.selectDof(25,True)
268 # robot.taskPos.feature.selectDof(26,True)
269 # robot.taskPos.feature.selectDof(27,True)
270 # robot.taskPos.feature.selectDof(28,True)
271 # robot.taskPos.feature.selectDof(29,True)
272 # robot.taskPos.feature.selectDof(30,True)
273 # robot.taskPos.feature.selectDof(31,True)
274 # robot.taskPos.feature.selectDof(32,True)
275 # robot.taskPos.feature.selectDof(33,True)
276 # robot.taskPos.feature.selectDof(34,True)
277 # robot.taskPos.feature.selectDof(35,True)
278 # robot.taskPos.feature.selectDof(36,True)
279 # robot.taskPos.feature.selectDof(37,True)
280 
281 # robot.taskPos.controlGain.value = 100.0
282 # robot.taskPos.add(robot.taskPos.feature.name)
283 # plug(robot.dynamic.position, robot.taskPos.feature.state)
284 
285 # --- SOT solver
286 robot.sot = SOT("sot")
287 robot.sot.setSize(robot.dynamic.getDimension())
288 
289 # --- Plug SOT control to device through control manager
290 plug(robot.sot.control, robot.cm.ctrl_sot_input)
291 plug(robot.cm.u_safe, robot.device.control)
292 
293 robot.sot.push(robot.taskUpperBody.name)
294 robot.sot.push(robot.contactRF.task.name)
295 robot.sot.push(robot.contactLF.task.name)
296 robot.sot.push(robot.taskCom.task.name)
297 robot.sot.push(robot.keepWaist.task.name)
298 # robot.sot.push(robot.taskPos.name)
299 robot.device.control.recompute(0)
300 
301 # --- Fix robot.dynamic inputs
302 plug(robot.device.velocity, robot.dynamic.velocity)
303 robot.dvdt = Derivator_of_Vector("dv_dt")
304 robot.dvdt.dt.value = dt
305 plug(robot.device.velocity, robot.dvdt.sin)
306 plug(robot.dvdt.sout, robot.dynamic.acceleration)
307 
308 # -------------------------- PLOTS --------------------------
309 
310 # --- ROS PUBLISHER
311 robot.publisher = create_rospublish(robot, "robot_publisher")
312 
313 rospub_signalName = "{0}_{1}".format("fake", "comDes")
314 topicname = "/sot/{0}/{1}".format("fake", "comDes")
315 robot.publisher.add("vector", rospub_signalName, topicname)
316 plug(
317  robot.dcm_control.dcmDes, robot.publisher.signal(rospub_signalName)
318 ) # desired CoM (workaround)
319 
320 create_topic(
321  robot.publisher, robot.cdc_estimator, "c", robot=robot, data_type="vector"
322 ) # estimated CoM
323 create_topic(
324  robot.publisher, robot.cdc_estimator, "dc", robot=robot, data_type="vector"
325 ) # estimated CoM velocity
326 
327 create_topic(
328  robot.publisher,
329  robot.com_admittance_control,
330  "comRef",
331  robot=robot,
332  data_type="vector",
333 ) # reference CoM
334 create_topic(
335  robot.publisher, robot.dynamic, "com", robot=robot, data_type="vector"
336 ) # resulting SOT CoM
337 
338 create_topic(
339  robot.publisher, robot.dcm_control, "dcmDes", robot=robot, data_type="vector"
340 ) # desired DCM
341 create_topic(
342  robot.publisher, robot.estimator, "dcm", robot=robot, data_type="vector"
343 ) # estimated DCM
344 
345 create_topic(
346  robot.publisher, robot.estimatorDc, "dcm", robot=robot, data_type="vector"
347 ) # dc from sot
348 
349 create_topic(
350  robot.publisher, robot.dcm_control, "zmpDes", robot=robot, data_type="vector"
351 ) # desired ZMP
352 create_topic(
353  robot.publisher, robot.dynamic, "zmp", robot=robot, data_type="vector"
354 ) # SOT ZMP
355 create_topic(
356  robot.publisher, robot.zmp_estimator, "zmp", robot=robot, data_type="vector"
357 ) # estimated ZMP
358 create_topic(
359  robot.publisher, robot.dcm_control, "zmpRef", robot=robot, data_type="vector"
360 ) # reference ZMP
361 
362 # create_topic(robot.publisher, robot.device, 'forceLLEG', robot = robot, data_type='vector') # measured left wrench
363 # create_topic(robot.publisher, robot.device, 'forceRLEG', robot = robot, data_type='vector') # measured right wrench
364 
365 # create_topic(robot.publisher, robot.device_filters.ft_LF_filter, 'x_filtered', robot = robot, data_type='vector') # filtered left wrench
366 # create_topic(robot.publisher, robot.device_filters.ft_RF_filter, 'x_filtered', robot = robot, data_type='vector') # filtered right wrench
367 
368 create_topic(
369  robot.publisher, robot.ftc, "left_foot_force_out", robot=robot, data_type="vector"
370 ) # calibrated left wrench
371 create_topic(
372  robot.publisher, robot.ftc, "right_foot_force_out", robot=robot, data_type="vector"
373 ) # calibrated right wrench
374 
375 # --- TRACER
376 robot.tracer = TracerRealTime("zmp_tracer")
377 robot.tracer.setBufferSize(80 * (2**20))
378 robot.tracer.open("/tmp", "dg_", ".dat")
379 robot.device.after.addSignal("{0}.triger".format(robot.tracer.name))
380 
381 addTrace(robot.tracer, robot.dcm_control, "dcmDes") # desired CoM (workaround)
382 addTrace(robot.tracer, robot.cdc_estimator, "c") # estimated CoM
383 addTrace(robot.tracer, robot.com_admittance_control, "comRef") # reference CoM
384 addTrace(robot.tracer, robot.dynamic, "com") # resulting SOT CoM
385 
386 # addTrace(robot.tracer, robot.dcm_control, 'dcmDes') # desired DCM (already added)
387 addTrace(robot.tracer, robot.estimator, "dcm") # estimated DCM
388 
389 addTrace(robot.tracer, robot.dcm_control, "zmpDes") # desired ZMP
390 addTrace(robot.tracer, robot.zmp_estimator, "zmp") # estimated ZMP
391 addTrace(robot.tracer, robot.dcm_control, "zmpRef") # reference ZMP
392 addTrace(robot.tracer, robot.dynamic, "zmp") # SOT ZMP
393 
394 # -------------------------- SIMULATION --------------------------
395 
396 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.addTrace
def addTrace(tracer, entity, signalName)
Definition: create_entities_utils.py:402
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_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