sot-talos-balance  2.0.5
Collection of dynamic-graph entities aimed at implementing balance control on talos.
appli_dcmSingleCopControl.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 SOT, Derivator_of_Vector, FeaturePosture, Task
7 from dynamic_graph.sot.core.matrix_util import matrixToTuple
8 from dynamic_graph.sot.core.meta_tasks_kine import (
9  MetaTaskKine6d,
10  MetaTaskKineCom,
11  gotoNd,
12 )
13 from dynamic_graph.sot.core.operator import Multiply_double_vector
14 from dynamic_graph.sot.dynamic_pinocchio import DynamicPinocchio
15 from dynamic_graph.tracer_real_time import TracerRealTime
16 
17 import dynamic_graph.sot_talos_balance.talos.base_estimator_conf as base_estimator_conf
18 import dynamic_graph.sot_talos_balance.talos.control_manager_conf as cm_conf
19 import dynamic_graph.sot_talos_balance.talos.ft_calibration_conf as ft_conf
20 import dynamic_graph.sot_talos_balance.talos.parameter_server_conf as param_server_conf
21 from dynamic_graph.sot_talos_balance.create_entities_utils import *
22 from dynamic_graph.sot_talos_balance.meta_task_joint import MetaTaskKineJoint
23 
24 cm_conf.CTRL_MAX = 10.0 # temporary hack
25 
26 robot.timeStep = robot.device.getTimeStep()
27 dt = robot.timeStep
28 
29 # --- Pendulum parameters
30 robot_name = "robot"
31 robot.dynamic.com.recompute(0)
32 robotDim = robot.dynamic.getDimension()
33 mass = robot.dynamic.data.mass[0]
34 h = robot.dynamic.com.value[2]
35 g = 9.81
36 omega = sqrt(g / h)
37 
38 # --- Parameter server
39 robot.param_server = create_parameter_server(param_server_conf, dt)
40 
41 # --- Initial feet and waist
42 robot.dynamic.createOpPoint("LF", robot.OperationalPointsMap["left-ankle"])
43 robot.dynamic.createOpPoint("RF", robot.OperationalPointsMap["right-ankle"])
44 robot.dynamic.createOpPoint("WT", robot.OperationalPointsMap["waist"])
45 robot.dynamic.LF.recompute(0)
46 robot.dynamic.RF.recompute(0)
47 robot.dynamic.WT.recompute(0)
48 
49 # -------------------------- DESIRED TRAJECTORY --------------------------
50 
51 # --- Trajectory generators
52 
53 # --- CoM
54 robot.comTrajGen = create_com_trajectory_generator(dt, robot)
55 
56 # --- Left foot
57 robot.lfTrajGen = create_pose_rpy_trajectory_generator(dt, robot, "LF")
58 # robot.lfTrajGen.x.recompute(0) # trigger computation of initial value
59 robot.lfToMatrix = PoseRollPitchYawToMatrixHomo("lf2m")
60 plug(robot.lfTrajGen.x, robot.lfToMatrix.sin)
61 
62 # --- Right foot
63 robot.rfTrajGen = create_pose_rpy_trajectory_generator(dt, robot, "RF")
64 # robot.rfTrajGen.x.recompute(0) # trigger computation of initial value
65 robot.rfToMatrix = PoseRollPitchYawToMatrixHomo("rf2m")
66 plug(robot.rfTrajGen.x, robot.rfToMatrix.sin)
67 
68 # --- Waist
69 robot.waistTrajGen = create_orientation_rpy_trajectory_generator(dt, robot, "WT")
70 # robot.waistTrajGen.x.recompute(0) # trigger computation of initial value
71 
72 robot.waistMix = Mix_of_vector("waistMix")
73 robot.waistMix.setSignalNumber(3)
74 robot.waistMix.addSelec(1, 0, 3)
75 robot.waistMix.addSelec(2, 3, 3)
76 robot.waistMix.default.value = [0.0] * 6
77 robot.waistMix.signal("sin1").value = [0.0] * 3
78 plug(robot.waistTrajGen.x, robot.waistMix.signal("sin2"))
79 
80 robot.waistToMatrix = PoseRollPitchYawToMatrixHomo("w2m")
81 plug(robot.waistMix.sout, robot.waistToMatrix.sin)
82 
83 # --- Rho
84 robot.rhoTrajGen = create_scalar_trajectory_generator(dt, 0.5, "rhoTrajGen")
85 robot.rhoScalar = Component_of_vector("rho_scalar")
86 robot.rhoScalar.setIndex(0)
87 plug(robot.rhoTrajGen.x, robot.rhoScalar.sin)
88 
89 # --- Phase
90 robot.phaseTrajGen = create_scalar_trajectory_generator(dt, 0.5, "phaseTrajGen")
91 robot.phaseScalar = Component_of_vector("phase_scalar")
92 robot.phaseScalar.setIndex(0)
93 plug(robot.phaseTrajGen.x, robot.phaseScalar.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 # robot.be_filters = create_be_filters(robot, dt)
121 
122 # --- Reference frame
123 
124 rf = SimpleReferenceFrame("rf")
125 rf.init(robot_name)
126 plug(robot.dynamic.LF, rf.footLeft)
127 plug(robot.dynamic.RF, rf.footRight)
128 robot.rf = rf
129 
130 # --- State transformation
131 stf = StateTransformation("stf")
132 stf.init()
133 plug(robot.rf.referenceFrame, stf.referenceFrame)
134 plug(robot.base_estimator.q, stf.q_in)
135 plug(robot.base_estimator.v, stf.v_in)
136 robot.stf = stf
137 
138 # --- Conversion
139 e2q = EulerToQuat("e2q")
140 plug(robot.stf.q, e2q.euler)
141 robot.e2q = e2q
142 
143 # --- Kinematic computations
144 robot.rdynamic = DynamicPinocchio("real_dynamics")
145 robot.rdynamic.setModel(robot.dynamic.model)
146 robot.rdynamic.setData(robot.rdynamic.model.createData())
147 plug(robot.stf.q, robot.rdynamic.position)
148 robot.rdynamic.velocity.value = [0.0] * robotDim
149 robot.rdynamic.acceleration.value = [0.0] * robotDim
150 
151 # --- CoM Estimation
152 cdc_estimator = DcmEstimator("cdc_estimator")
153 cdc_estimator.init(dt, robot_name)
154 plug(robot.e2q.quaternion, cdc_estimator.q)
155 plug(robot.stf.v, cdc_estimator.v)
156 robot.cdc_estimator = cdc_estimator
157 
158 # --- DCM Estimation
159 estimator = DummyDcmEstimator("dummy")
160 estimator.omega.value = omega
161 estimator.mass.value = 1.0
162 plug(robot.cdc_estimator.c, estimator.com)
163 plug(robot.cdc_estimator.dc, estimator.momenta)
164 estimator.init()
165 robot.estimator = estimator
166 
167 # --- Force calibration
168 robot.ftc = create_ft_calibrator(robot, ft_conf)
169 
170 # --- ZMP estimation
171 zmp_estimator = SimpleZmpEstimator("zmpEst")
172 robot.rdynamic.createOpPoint("sole_LF", "left_sole_link")
173 robot.rdynamic.createOpPoint("sole_RF", "right_sole_link")
174 plug(robot.rdynamic.sole_LF, zmp_estimator.poseLeft)
175 plug(robot.rdynamic.sole_RF, zmp_estimator.poseRight)
176 plug(robot.ftc.left_foot_force_out, zmp_estimator.wrenchLeft)
177 plug(robot.ftc.right_foot_force_out, zmp_estimator.wrenchRight)
178 zmp_estimator.init()
179 robot.zmp_estimator = zmp_estimator
180 
181 # -------------------------- ADMITTANCE CONTROL --------------------------
182 
183 # --- DCM controller
184 Kp_dcm = [5.0, 5.0, 5.0]
185 Ki_dcm = [0.0, 0.0, 0.0] # zero (to be set later)
186 gamma_dcm = 0.2
187 
188 dcm_controller = DcmController("dcmCtrl")
189 
190 dcm_controller.Kp.value = Kp_dcm
191 dcm_controller.Ki.value = Ki_dcm
192 dcm_controller.decayFactor.value = gamma_dcm
193 dcm_controller.mass.value = mass
194 dcm_controller.omega.value = omega
195 
196 plug(robot.cdc_estimator.c, dcm_controller.com)
197 plug(robot.estimator.dcm, dcm_controller.dcm)
198 
199 plug(robot.wp.zmpDes, dcm_controller.zmpDes)
200 plug(robot.wp.dcmDes, dcm_controller.dcmDes)
201 
202 dcm_controller.init(dt)
203 
204 robot.dcm_control = dcm_controller
205 
206 Ki_dcm = [1.0, 1.0, 1.0] # this value is employed later
207 
208 # --- Distribute wrench
209 distribute = create_distribute_wrench(base_estimator_conf)
210 plug(robot.e2q.quaternion, distribute.q)
211 plug(robot.dcm_control.wrenchRef, distribute.wrenchDes)
212 plug(robot.rhoScalar.sout, distribute.rho)
213 distribute.init(robot_name)
214 robot.distribute = distribute
215 
216 # --- Ankle controllers
217 LeftPitchJoint = 4
218 LeftRollJoint = 5
219 RightPitchJoint = 10
220 RightRollJoint = 11
221 
222 Kp_ankles = [1e-3] * 2
223 
224 # --- Right ankle
225 controller = AnkleAdmittanceController("rightController")
226 plug(robot.ftc.right_foot_force_out, controller.wrench)
227 controller.gainsXY.value = Kp_ankles
228 plug(robot.distribute.copRight, controller.pRef)
229 controller.init()
230 robot.rightAnkleController = controller
231 
232 # --- RIGHT ANKLE PITCH
233 robot.rightPitchSelec = Selec_of_vector("rightPitchSelec")
234 robot.rightPitchSelec.selec(1, 2)
235 plug(robot.rightAnkleController.dRP, robot.rightPitchSelec.sin)
236 
237 robot.rightAnklePitchTask = MetaTaskKineJoint(robot.dynamic, RightPitchJoint + 6)
238 robot.rightAnklePitchTask.task.controlGain.value = 0
239 robot.rightAnklePitchTask.task.setWithDerivative(True)
240 robot.rightAnklePitchTask.featureDes.errorIN.value = [0.0]
241 plug(robot.rightPitchSelec.sout, robot.rightAnklePitchTask.featureDes.errordotIN)
242 
243 # --- RIGHT ANKLE ROLL
244 robot.rightRollSelec = Selec_of_vector("rightRollSelec")
245 robot.rightRollSelec.selec(0, 1)
246 plug(robot.rightAnkleController.dRP, robot.rightRollSelec.sin)
247 
248 robot.rightAnkleRollTask = MetaTaskKineJoint(robot.dynamic, RightRollJoint + 6)
249 robot.rightAnkleRollTask.task.controlGain.value = 0
250 robot.rightAnkleRollTask.task.setWithDerivative(True)
251 robot.rightAnkleRollTask.featureDes.errorIN.value = [0.0]
252 plug(robot.rightRollSelec.sout, robot.rightAnkleRollTask.featureDes.errordotIN)
253 
254 # --- Control Manager
255 robot.cm = create_ctrl_manager(cm_conf, dt, robot_name="robot")
256 robot.cm.addCtrlMode("sot_input")
257 robot.cm.setCtrlMode("all", "sot_input")
258 robot.cm.addEmergencyStopSIN("zmp")
259 robot.cm.addEmergencyStopSIN("distribute")
260 
261 # -------------------------- SOT CONTROL --------------------------
262 
263 # --- Upper body
264 robot.taskUpperBody = Task("task_upper_body")
265 robot.taskUpperBody.feature = FeaturePosture("feature_upper_body")
266 
267 q = list(robot.dynamic.position.value)
268 robot.taskUpperBody.feature.state.value = q
269 robot.taskUpperBody.feature.posture.value = q
270 
271 # robotDim = robot.dynamic.getDimension() # 38
272 robot.taskUpperBody.feature.selectDof(18, True)
273 robot.taskUpperBody.feature.selectDof(19, True)
274 robot.taskUpperBody.feature.selectDof(20, True)
275 robot.taskUpperBody.feature.selectDof(21, True)
276 robot.taskUpperBody.feature.selectDof(22, True)
277 robot.taskUpperBody.feature.selectDof(23, True)
278 robot.taskUpperBody.feature.selectDof(24, True)
279 robot.taskUpperBody.feature.selectDof(25, True)
280 robot.taskUpperBody.feature.selectDof(26, True)
281 robot.taskUpperBody.feature.selectDof(27, True)
282 robot.taskUpperBody.feature.selectDof(28, True)
283 robot.taskUpperBody.feature.selectDof(29, True)
284 robot.taskUpperBody.feature.selectDof(30, True)
285 robot.taskUpperBody.feature.selectDof(31, True)
286 robot.taskUpperBody.feature.selectDof(32, True)
287 robot.taskUpperBody.feature.selectDof(33, True)
288 robot.taskUpperBody.feature.selectDof(34, True)
289 robot.taskUpperBody.feature.selectDof(35, True)
290 robot.taskUpperBody.feature.selectDof(36, True)
291 robot.taskUpperBody.feature.selectDof(37, True)
292 
293 robot.taskUpperBody.controlGain.value = 100.0
294 robot.taskUpperBody.add(robot.taskUpperBody.feature.name)
295 plug(robot.dynamic.position, robot.taskUpperBody.feature.state)
296 
297 # --- CONTACTS
298 # define contactLF and contactRF
299 robot.contactLF = MetaTaskKine6d(
300  "contactLF", robot.dynamic, "LF", robot.OperationalPointsMap["left-ankle"]
301 )
302 robot.contactLF.feature.frame("desired")
303 robot.contactLF.gain.setConstant(300)
304 plug(robot.wp.footLeftDes, robot.contactLF.featureDes.position) # .errorIN?
305 locals()["contactLF"] = robot.contactLF
306 
307 robot.contactRF = MetaTaskKine6d(
308  "contactRF", robot.dynamic, "RF", robot.OperationalPointsMap["right-ankle"]
309 )
310 robot.contactRF.feature.frame("desired")
311 robot.contactRF.gain.setConstant(300)
312 plug(robot.wp.footRightDes, robot.contactRF.featureDes.position) # .errorIN?
313 # robot.contactRF.feature.selec.value = '000111'
314 locals()["contactRF"] = robot.contactRF
315 
316 # --- COM
317 robot.taskCom = MetaTaskKineCom(robot.dynamic)
318 plug(robot.wp.comDes, robot.taskCom.featureDes.errorIN)
319 robot.taskCom.task.controlGain.value = 10
320 robot.taskCom.feature.selec.value = "100"
321 
322 # --- Waist
323 robot.keepWaist = MetaTaskKine6d(
324  "keepWaist", robot.dynamic, "WT", robot.OperationalPointsMap["waist"]
325 )
326 robot.keepWaist.feature.frame("desired")
327 robot.keepWaist.gain.setConstant(300)
328 plug(robot.wp.waistDes, robot.keepWaist.featureDes.position)
329 robot.keepWaist.feature.selec.value = "111000"
330 locals()["keepWaist"] = robot.keepWaist
331 
332 # --- SOT solver
333 robot.sot = SOT("sot")
334 robot.sot.setSize(robot.dynamic.getDimension())
335 
336 # --- Plug SOT control to device through control manager
337 plug(robot.sot.control, robot.cm.ctrl_sot_input)
338 plug(robot.cm.u_safe, robot.device.control)
339 
340 robot.sot.push(robot.taskUpperBody.name)
341 robot.sot.push(robot.contactRF.task.name)
342 robot.sot.push(robot.contactLF.task.name)
343 robot.sot.push(robot.taskCom.task.name)
344 robot.sot.push(robot.rightAnklePitchTask.task.name)
345 robot.sot.push(robot.rightAnkleRollTask.task.name)
346 robot.sot.push(robot.keepWaist.task.name)
347 
348 # --- Fix robot.dynamic inputs
349 plug(robot.device.velocity, robot.dynamic.velocity)
350 robot.dvdt = Derivator_of_Vector("dv_dt")
351 robot.dvdt.dt.value = dt
352 plug(robot.device.velocity, robot.dvdt.sin)
353 plug(robot.dvdt.sout, robot.dynamic.acceleration)
354 
355 # -------------------------- PLOTS --------------------------
356 
357 # --- ROS PUBLISHER
358 robot.publisher = create_rospublish(robot, "robot_publisher")
359 
360 create_topic(
361  robot.publisher, robot.wp, "comDes", robot=robot, data_type="vector"
362 ) # desired CoM
363 
364 create_topic(
365  robot.publisher, robot.cdc_estimator, "c", robot=robot, data_type="vector"
366 ) # estimated CoM
367 create_topic(
368  robot.publisher, robot.cdc_estimator, "dc", robot=robot, data_type="vector"
369 ) # estimated CoM velocity
370 
371 create_topic(
372  robot.publisher, robot.dynamic, "com", robot=robot, data_type="vector"
373 ) # resulting SOT CoM
374 
375 create_topic(
376  robot.publisher, robot.dcm_control, "dcmDes", robot=robot, data_type="vector"
377 ) # desired DCM
378 create_topic(
379  robot.publisher, robot.estimator, "dcm", robot=robot, data_type="vector"
380 ) # estimated DCM
381 
382 create_topic(
383  robot.publisher, robot.dcm_control, "zmpDes", robot=robot, data_type="vector"
384 ) # desired ZMP
385 create_topic(
386  robot.publisher, robot.dynamic, "zmp", robot=robot, data_type="vector"
387 ) # SOT ZMP
388 create_topic(
389  robot.publisher, robot.zmp_estimator, "zmp", robot=robot, data_type="vector"
390 ) # estimated ZMP
391 create_topic(
392  robot.publisher, robot.dcm_control, "zmpRef", robot=robot, data_type="vector"
393 ) # reference ZMP
394 
395 create_topic(
396  robot.publisher, robot.dcm_control, "wrenchRef", robot=robot, data_type="vector"
397 ) # unoptimized reference wrench
398 create_topic(
399  robot.publisher, robot.distribute, "wrenchLeft", robot=robot, data_type="vector"
400 ) # reference left wrench
401 create_topic(
402  robot.publisher, robot.distribute, "wrenchRight", robot=robot, data_type="vector"
403 ) # reference right wrench
404 create_topic(
405  robot.publisher, robot.distribute, "wrenchRef", robot=robot, data_type="vector"
406 ) # optimized reference wrench
407 
408 # create_topic(robot.publisher, robot.device, 'forceLLEG', robot = robot, data_type='vector') # measured left wrench
409 # create_topic(robot.publisher, robot.device, 'forceRLEG', robot = robot, data_type='vector') # measured right wrench
410 
411 # create_topic(robot.publisher, robot.device_filters.ft_LF_filter, 'x_filtered', robot = robot, data_type='vector') # filtered left wrench
412 # create_topic(robot.publisher, robot.device_filters.ft_RF_filter, 'x_filtered', robot = robot, data_type='vector') # filtered right wrench
413 
414 create_topic(
415  robot.publisher, robot.ftc, "left_foot_force_out", robot=robot, data_type="vector"
416 ) # calibrated left wrench
417 create_topic(
418  robot.publisher, robot.ftc, "right_foot_force_out", robot=robot, data_type="vector"
419 ) # calibrated right wrench
420 
421 create_topic(
422  robot.publisher, robot.zmp_estimator, "copRight", robot=robot, data_type="vector"
423 )
424 create_topic(
425  robot.publisher, robot.rightAnkleController, "pRef", robot=robot, data_type="vector"
426 )
427 create_topic(
428  robot.publisher, robot.rightAnkleController, "dRP", robot=robot, data_type="vector"
429 )
430 
431 
436 
437 # addTrace(robot.tracer, robot.wp, 'comDes') # desired CoM
438 
439 # addTrace(robot.tracer, robot.cdc_estimator, 'c') # estimated CoM
440 # addTrace(robot.tracer, robot.cdc_estimator, 'dc') # estimated CoM velocity
441 
442 # addTrace(robot.tracer, robot.com_admittance_control, 'comRef') # reference CoM
443 # addTrace(robot.tracer, robot.dynamic, 'com') # resulting SOT CoM
444 
445 # addTrace(robot.tracer, robot.dcm_control, 'dcmDes') # desired DCM
446 # addTrace(robot.tracer, robot.estimator, 'dcm') # estimated DCM
447 
448 # addTrace(robot.tracer, robot.dcm_control, 'zmpDes') # desired ZMP
449 # addTrace(robot.tracer, robot.dynamic, 'zmp') # SOT ZMP
450 # addTrace(robot.tracer, robot.zmp_estimator, 'zmp') # estimated ZMP
451 # addTrace(robot.tracer, robot.dcm_control, 'zmpRef') # reference ZMP
452 
453 # addTrace(robot.tracer, robot.dcm_control, 'wrenchRef') # unoptimized reference wrench
454 # addTrace(robot.tracer, robot.distribute, 'wrenchLeft') # reference left wrench
455 # addTrace(robot.tracer, robot.distribute, 'wrenchRight') # reference right wrench
456 # addTrace(robot.tracer, robot.distribute, 'wrenchRef') # optimized reference wrench
457 
458 # addTrace(robot.tracer, robot.ftc, 'left_foot_force_out') # calibrated left wrench
459 # addTrace(robot.tracer, robot.ftc, 'right_foot_force_out') # calibrated right wrench
460 
461 # 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