sot-talos-balance  2.0.5
Collection of dynamic-graph entities aimed at implementing balance control on talos.
appli_joystick_dcmZmpControl.py
Go to the documentation of this file.
1 # flake8: noqa
2 
3 import sys
4 
5 from math import sqrt
6 
7 import numpy as np
8 from dynamic_graph import plug
9 from dynamic_graph.sot.core import SOT, Derivator_of_Vector, FeaturePosture
10 from dynamic_graph.sot.core import MatrixHomoToPoseQuaternion, Task, RPYToMatrix
11 from dynamic_graph.sot.core.matrix_util import matrixToTuple
12 from dynamic_graph.sot.core.meta_tasks_kine import MetaTaskKine6d
13 from dynamic_graph.sot.core.meta_tasks_kine import MetaTaskKineCom, gotoNd
14 
15 from dynamic_graph.sot.dynamic_pinocchio import DynamicPinocchio
16 from dynamic_graph.tracer_real_time import TracerRealTime
17 from rospkg import RosPack
18 
19 import dynamic_graph.sot_talos_balance.talos.base_estimator_conf as base_estimator_conf
20 import dynamic_graph.sot_talos_balance.talos.control_manager_conf as cm_conf
21 import dynamic_graph.sot_talos_balance.talos.ft_calibration_conf as ft_conf
22 import dynamic_graph.sot_talos_balance.talos.parameter_server_conf as param_server_conf
23 from dynamic_graph.sot_talos_balance.create_entities_utils import *
24 
25 from dynamic_graph.sot.pattern_generator import PatternGenerator
26 
27 # This will only work in simulation ! WARNING !
28 cm_conf.CTRL_MAX = 100.0
29 
30 robot.timeStep = robot.device.getTimeStep()
31 dt = robot.timeStep
32 
33 # --- Pendulum parameters
34 robot_name = "robot"
35 robot.dynamic.com.recompute(0)
36 robotDim = robot.dynamic.getDimension()
37 mass = robot.dynamic.data.mass[0]
38 h = robot.dynamic.com.value[2]
39 g = 9.81
40 omega = sqrt(g / h)
41 
42 # --- Parameter server
43 robot.param_server = create_parameter_server(param_server_conf, dt)
44 
45 # --- Initial feet and waist
46 robot.dynamic.createOpPoint("LF", robot.OperationalPointsMap["left-ankle"])
47 robot.dynamic.createOpPoint("RF", robot.OperationalPointsMap["right-ankle"])
48 robot.dynamic.createOpPoint("WT", robot.OperationalPointsMap["waist"])
49 robot.dynamic.LF.recompute(0)
50 robot.dynamic.RF.recompute(0)
51 robot.dynamic.WT.recompute(0)
52 
53 # -------------------------- PATTERN GENERATOR --------------------------
54 
55 robot.pg = PatternGenerator("pg")
56 
57 # Specify the robot model TODO: use get_parameter through ParameterServer
58 # DO USE FILE ANYMORE
59 rospack = RosPack()
60 talos_data_folder = rospack.get_path("talos_data")
61 robot.pg.setURDFpath(talos_data_folder + "/urdf/talos_reduced_wpg.urdf")
62 robot.pg.setSRDFpath(talos_data_folder + "/srdf/talos_wpg.srdf")
63 
64 robot.pg.buildModel()
65 
66 robot.pg.parseCmd(":samplingperiod 0.005")
67 robot.pg.parseCmd(":previewcontroltime 1.6")
68 robot.pg.parseCmd(":omega 0.0")
69 robot.pg.parseCmd(":stepheight 0.05")
70 robot.pg.parseCmd(":doublesupporttime 0.2")
71 robot.pg.parseCmd(":singlesupporttime 1.0")
72 robot.pg.parseCmd(":armparameters 0.5")
73 robot.pg.parseCmd(":LimitsFeasibility 0.0")
74 robot.pg.parseCmd(":ZMPShiftParameters 0.015 0.015 0.015 0.015")
75 robot.pg.parseCmd(":TimeDistributeParameters 2.0 3.5 1.7 3.0")
76 robot.pg.parseCmd(":UpperBodyMotionParameters -0.1 -1.0 0.0")
77 robot.pg.parseCmd(":comheight 0.876681")
78 robot.pg.parseCmd(":setVelReference 0.1 0.0 0.0")
79 
80 robot.pg.parseCmd(":SetAlgoForZmpTrajectory Naveau")
81 
82 plug(robot.dynamic.position, robot.pg.position)
83 plug(robot.dynamic.com, robot.pg.com)
84 plug(robot.dynamic.LF, robot.pg.leftfootcurrentpos)
85 plug(robot.dynamic.RF, robot.pg.rightfootcurrentpos)
86 robotDim = len(robot.dynamic.velocity.value)
87 robot.pg.motorcontrol.value = robotDim * (0,)
88 robot.pg.zmppreviouscontroller.value = (0, 0, 0)
89 
90 robot.pg.initState()
91 
92 robot.pg.parseCmd(":setDSFeetDistance 0.162")
93 
94 robot.pg.parseCmd(":NaveauOnline")
95 robot.pg.parseCmd(":numberstepsbeforestop 2")
96 robot.pg.parseCmd(":setfeetconstraint XY 0.091 0.0489")
97 
98 robot.pg.parseCmd(":deleteallobstacles")
99 robot.pg.parseCmd(":feedBackControl false")
100 robot.pg.parseCmd(":useDynamicFilter true")
101 
102 robot.pg.velocitydes.value = (0.1, 0.0, 0.0) # DEFAULT VALUE (0.1,0.0,0.0)
103 
104 # -------------------------- TRIGGER --------------------------
105 
106 robot.triggerPG = BooleanIdentity("triggerPG")
107 robot.triggerPG.sin.value = 0
108 plug(robot.triggerPG.sout, robot.pg.trigger)
109 
110 # -------------------------- Interface with controller entities --------------------------
111 
112 wp = DummyWalkingPatternGenerator("dummy_wp")
113 wp.init()
114 wp.omega.value = omega
115 
116 # Plug Pattern generator to the dummy one.
117 plug(robot.pg.waistattitudematrixabsolute, wp.waist)
118 plug(robot.pg.leftfootref, wp.footLeft)
119 plug(robot.pg.rightfootref, wp.footRight)
120 plug(robot.pg.comref, wp.com)
121 plug(robot.pg.dcomref, wp.vcom)
122 plug(robot.pg.ddcomref, wp.acom)
123 
124 robot.wp = wp
125 
126 # --- Compute the values to use them in initialization
127 robot.wp.comDes.recompute(0)
128 robot.wp.dcmDes.recompute(0)
129 robot.wp.zmpDes.recompute(0)
130 
131 # -------------------------- ESTIMATION --------------------------
132 
133 # --- Base Estimation
134 robot.device_filters = create_device_filters(robot, dt)
135 robot.imu_filters = create_imu_filters(robot, dt)
136 robot.base_estimator = create_base_estimator(robot, dt, base_estimator_conf)
137 
138 robot.m2qLF = MatrixHomoToPoseQuaternion("m2qLF")
139 plug(robot.dynamic.LF, robot.m2qLF.sin)
140 plug(robot.m2qLF.sout, robot.base_estimator.lf_ref_xyzquat)
141 robot.m2qRF = MatrixHomoToPoseQuaternion("m2qRF")
142 plug(robot.dynamic.RF, robot.m2qRF.sin)
143 plug(robot.m2qRF.sout, robot.base_estimator.rf_ref_xyzquat)
144 
145 # --- Conversion
146 e2q = EulerToQuat("e2q")
147 plug(robot.base_estimator.q, e2q.euler)
148 robot.e2q = e2q
149 
150 # --- Kinematic computations
151 robot.rdynamic = DynamicPinocchio("real_dynamics")
152 robot.rdynamic.setModel(robot.dynamic.model)
153 robot.rdynamic.setData(robot.rdynamic.model.createData())
154 plug(robot.base_estimator.q, robot.rdynamic.position)
155 robot.rdynamic.velocity.value = [0.0] * robotDim
156 robot.rdynamic.acceleration.value = [0.0] * robotDim
157 
158 # --- CoM Estimation
159 cdc_estimator = DcmEstimator("cdc_estimator")
160 cdc_estimator.init(dt, robot_name)
161 plug(robot.e2q.quaternion, cdc_estimator.q)
162 plug(robot.base_estimator.v, cdc_estimator.v)
163 robot.cdc_estimator = cdc_estimator
164 
165 # --- DCM Estimation
166 estimator = DummyDcmEstimator("dummy")
167 estimator.omega.value = omega
168 estimator.mass.value = 1.0
169 plug(robot.cdc_estimator.c, estimator.com)
170 plug(robot.cdc_estimator.dc, estimator.momenta)
171 estimator.init()
172 robot.estimator = estimator
173 
174 # --- Force calibration
175 robot.ftc = create_ft_calibrator(robot, ft_conf)
176 
177 # --- ZMP estimation
178 zmp_estimator = SimpleZmpEstimator("zmpEst")
179 robot.rdynamic.createOpPoint("sole_LF", "left_sole_link")
180 robot.rdynamic.createOpPoint("sole_RF", "right_sole_link")
181 plug(robot.rdynamic.sole_LF, zmp_estimator.poseLeft)
182 plug(robot.rdynamic.sole_RF, zmp_estimator.poseRight)
183 plug(robot.ftc.left_foot_force_out, zmp_estimator.wrenchLeft)
184 plug(robot.ftc.right_foot_force_out, zmp_estimator.wrenchRight)
185 zmp_estimator.init()
186 robot.zmp_estimator = zmp_estimator
187 
188 # -------------------------- ADMITTANCE CONTROL --------------------------
189 
190 # --- DCM controller
191 Kp_dcm = [8.0] * 3
192 Ki_dcm = [0.0, 0.0, 0.0] # zero (to be set later)
193 gamma_dcm = 0.2
194 
195 dcm_controller = DcmController("dcmCtrl")
196 
197 dcm_controller.Kp.value = Kp_dcm
198 dcm_controller.Ki.value = Ki_dcm
199 dcm_controller.decayFactor.value = gamma_dcm
200 dcm_controller.mass.value = mass
201 dcm_controller.omega.value = omega
202 
203 plug(robot.cdc_estimator.c, dcm_controller.com)
204 plug(robot.estimator.dcm, dcm_controller.dcm)
205 
206 plug(robot.wp.zmpDes, dcm_controller.zmpDes)
207 plug(robot.wp.dcmDes, dcm_controller.dcmDes)
208 
209 dcm_controller.init(dt)
210 
211 robot.dcm_control = dcm_controller
212 
213 Ki_dcm = [1.0, 1.0, 1.0] # this value is employed later
214 
215 # --- CoM admittance controller
216 Kp_adm = [0.0, 0.0, 0.0] # zero (to be set later)
217 
218 com_admittance_control = ComAdmittanceController("comAdmCtrl")
219 com_admittance_control.Kp.value = Kp_adm
220 plug(robot.zmp_estimator.zmp, com_admittance_control.zmp)
221 com_admittance_control.zmpDes.value = (
222  robot.wp.zmpDes.value
223 ) # should be plugged to robot.dcm_control.zmpRef
224 plug(robot.wp.acomDes, com_admittance_control.ddcomDes)
225 
226 com_admittance_control.init(dt)
227 com_admittance_control.setState(robot.wp.comDes.value, [0.0, 0.0, 0.0])
228 
229 robot.com_admittance_control = com_admittance_control
230 
231 Kp_adm = [15.0, 15.0, 0.0] # this value is employed later
232 
233 # --- Control Manager
234 robot.cm = create_ctrl_manager(cm_conf, dt, robot_name="robot")
235 robot.cm.addCtrlMode("sot_input")
236 robot.cm.setCtrlMode("all", "sot_input")
237 robot.cm.addEmergencyStopSIN("zmp")
238 
239 # -------------------------- SOT CONTROL --------------------------
240 
241 # --- Upper body
242 robot.taskUpperBody = Task("task_upper_body")
243 robot.taskUpperBody.feature = FeaturePosture("feature_upper_body")
244 
245 q = list(robot.dynamic.position.value)
246 robot.taskUpperBody.feature.state.value = q
247 robot.taskUpperBody.feature.posture.value = q
248 
249 
250 
251 # robotDim = robot.dynamic.getDimension() # 38
252 robot.taskUpperBody.feature.selectDof(18, True)
253 robot.taskUpperBody.feature.selectDof(19, True)
254 robot.taskUpperBody.feature.selectDof(20, True)
255 robot.taskUpperBody.feature.selectDof(21, True)
256 robot.taskUpperBody.feature.selectDof(22, True)
257 robot.taskUpperBody.feature.selectDof(23, True)
258 robot.taskUpperBody.feature.selectDof(24, True)
259 robot.taskUpperBody.feature.selectDof(25, True)
260 robot.taskUpperBody.feature.selectDof(26, True)
261 robot.taskUpperBody.feature.selectDof(27, True)
262 robot.taskUpperBody.feature.selectDof(28, True)
263 robot.taskUpperBody.feature.selectDof(29, True)
264 robot.taskUpperBody.feature.selectDof(30, True)
265 robot.taskUpperBody.feature.selectDof(31, True)
266 robot.taskUpperBody.feature.selectDof(32, True)
267 robot.taskUpperBody.feature.selectDof(33, True)
268 robot.taskUpperBody.feature.selectDof(34, True)
269 robot.taskUpperBody.feature.selectDof(35, True)
270 robot.taskUpperBody.feature.selectDof(36, True)
271 robot.taskUpperBody.feature.selectDof(37, True)
272 
273 robot.taskUpperBody.controlGain.value = 100.0
274 robot.taskUpperBody.add(robot.taskUpperBody.feature.name)
275 plug(robot.dynamic.position, robot.taskUpperBody.feature.state)
276 
277 # --- CONTACTS
278 # define contactLF and contactRF
279 robot.contactLF = MetaTaskKine6d(
280  "contactLF", robot.dynamic, "LF", robot.OperationalPointsMap["left-ankle"]
281 )
282 robot.contactLF.feature.frame("desired")
283 robot.contactLF.gain.setConstant(300)
284 plug(robot.wp.footLeftDes, robot.contactLF.featureDes.position) # .errorIN?
285 locals()["contactLF"] = robot.contactLF
286 
287 robot.contactRF = MetaTaskKine6d(
288  "contactRF", robot.dynamic, "RF", robot.OperationalPointsMap["right-ankle"]
289 )
290 robot.contactRF.feature.frame("desired")
291 robot.contactRF.gain.setConstant(300)
292 plug(robot.wp.footRightDes, robot.contactRF.featureDes.position) # .errorIN?
293 locals()["contactRF"] = robot.contactRF
294 
295 # --- COM height
296 robot.taskComH = MetaTaskKineCom(robot.dynamic, name="comH")
297 plug(robot.wp.comDes, robot.taskComH.featureDes.errorIN)
298 robot.taskComH.task.controlGain.value = 100.0
299 robot.taskComH.feature.selec.value = "100"
300 
301 # --- COM
302 robot.taskCom = MetaTaskKineCom(robot.dynamic)
303 plug(robot.com_admittance_control.comRef, robot.taskCom.featureDes.errorIN)
304 plug(robot.com_admittance_control.dcomRef, robot.taskCom.featureDes.errordotIN)
305 robot.taskCom.task.controlGain.value = 0
306 robot.taskCom.task.setWithDerivative(True)
307 robot.taskCom.feature.selec.value = "011"
308 
309 # --- Waist
310 
311 robot.keepWaist = MetaTaskKine6d(
312  "keepWaist", robot.dynamic, "WT", robot.OperationalPointsMap["waist"]
313 )
314 robot.keepWaist.feature.frame("desired")
315 robot.keepWaist.gain.setConstant(300)
316 plug(robot.wp.waistDes, robot.keepWaist.featureDes.position) # de base
317 robot.keepWaist.feature.selec.value = "111000"
318 locals()["keepWaist"] = robot.keepWaist
319 
320 # --- SOT solver
321 robot.sot = SOT("sot")
322 robot.sot.setSize(robot.dynamic.getDimension())
323 
324 # --- Plug SOT control to device through control manager
325 plug(robot.sot.control, robot.cm.ctrl_sot_input)
326 plug(robot.cm.u_safe, robot.device.control)
327 
328 robot.sot.push(robot.taskUpperBody.name)
329 robot.sot.push(robot.contactRF.task.name)
330 robot.sot.push(robot.contactLF.task.name)
331 robot.sot.push(robot.taskComH.task.name)
332 robot.sot.push(robot.taskCom.task.name)
333 robot.sot.push(robot.keepWaist.task.name)
334 
335 # --- Fix robot.dynamic inputs
336 plug(robot.device.velocity, robot.dynamic.velocity)
337 robot.dvdt = Derivator_of_Vector("dv_dt")
338 robot.dvdt.dt.value = dt
339 plug(robot.device.velocity, robot.dvdt.sin)
340 plug(robot.dvdt.sout, robot.dynamic.acceleration)
341 
342 # -------------------------- PLOTS --------------------------
343 
344 # --- ROS PUBLISHER
345 
346 
347 
348 robot.publisher = create_rospublish(robot, "robot_publisher")
349 
350 
351 create_topic(
352  robot.publisher, robot.pg, "comref", robot=robot, data_type="vector"
353 ) # desired CoM
354 create_topic(robot.publisher, robot.pg, "dcomref", robot=robot, data_type="vector")
355 create_topic(robot.publisher, robot.wp, "waist", robot=robot, data_type="matrixHomo")
356 create_topic(
357  robot.publisher,
358  robot.keepWaist.featureDes,
359  "position",
360  robot=robot,
361  data_type="matrixHomo",
362 )
363 create_topic(robot.publisher, robot.dynamic, "WT", robot=robot, data_type="matrixHomo")
364 create_topic(
365  robot.publisher,
366  robot.pg,
367  "waistattitudematrixabsolute",
368  robot=robot,
369  data_type="matrixHomo",
370 )
371 
372 create_topic(
373  robot.publisher, robot.pg, "leftfootref", robot=robot, data_type="matrixHomo"
374 )
375 create_topic(robot.publisher, robot.wp, "footLeft", robot=robot, data_type="matrixHomo")
376 create_topic(
377  robot.publisher, robot.pg, "rightfootref", robot=robot, data_type="matrixHomo"
378 )
379 create_topic(
380  robot.publisher, robot.wp, "footRight", robot=robot, data_type="matrixHomo"
381 )
382 
383 # create_topic(robot.publisher, robot.pg, 'leftfootref', robot = robot, data_type='vector')
384 # plug(robot.pg.leftfootref, wp.footLeft)
385 # plug(robot.pg.rightfootref, wp.footRight)
386 # plug(robot.pg.comref, wp.com)
387 # plug(robot.pg.dcomref, wp.vcom)
388 # plug(robot.pg.ddcomref, wp.acom)
389 
390 
391 # create_topic(robot.publisher, robot.device, 'state', robot=robot, data_type='vector')
392 # create_topic(robot.publisher, robot.base_estimator, 'q', robot=robot, data_type='vector')
393 
434 robot.tracer = TracerRealTime("com_tracer")
435 robot.tracer.setBufferSize(80 * (2**20))
436 robot.tracer.open("/tmp", "dg_", ".dat")
437 
438 robot.device.after.addSignal("{0}.triger".format(robot.tracer.name))
439 addTrace(robot.tracer, robot.pg, "waistattitudeabsolute")
440 
441 addTrace(robot.tracer, robot.wp, "comDes") # desired CoM
442 
443 addTrace(robot.tracer, robot.cdc_estimator, "c") # estimated CoM
444 addTrace(robot.tracer, robot.cdc_estimator, "dc") # estimated CoM velocity
445 
446 # addTrace(robot.tracer, robot.com_admittance_control, 'comRef') # reference CoM
447 
448 addTrace(robot.tracer, robot.pg, "comref")
449 addTrace(robot.tracer, robot.pg, "dcomref")
450 addTrace(robot.tracer, robot.pg, "ddcomref")
451 
452 addTrace(robot.tracer, robot.pg, "rightfootref")
453 addTrace(robot.tracer, robot.pg, "leftfootref")
454 
455 addTrace(robot.tracer, robot.pg, "rightfootcontact")
456 addTrace(robot.tracer, robot.pg, "leftfootcontact")
457 addTrace(robot.tracer, robot.pg, "SupportFoot")
458 
459 addTrace(robot.tracer, robot.dynamic, "com") # resulting SOT CoM
460 addTrace(robot.tracer, robot.dynamic, "LF") # left foot
461 addTrace(robot.tracer, robot.dynamic, "RF") # right foot
462 
463 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