sot-talos-balance  2.0.5
Collection of dynamic-graph entities aimed at implementing balance control on talos.
appli_feet_admittance.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  Task,
11  operator,
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 robot.comTrajGen = create_com_trajectory_generator(dt, robot)
55 
56 robot.lfTrajGen = create_pose_rpy_trajectory_generator(dt, robot, "LF")
57 robot.lfToMatrix = PoseRollPitchYawToMatrixHomo("lf2m")
58 plug(robot.lfTrajGen.x, robot.lfToMatrix.sin)
59 
60 robot.rfTrajGen = create_pose_rpy_trajectory_generator(dt, robot, "RF")
61 robot.rfToMatrix = PoseRollPitchYawToMatrixHomo("rf2m")
62 plug(robot.rfTrajGen.x, robot.rfToMatrix.sin)
63 
64 # --- Interface with controller entities
65 
66 wp = DummyWalkingPatternGenerator("dummy_wp")
67 wp.init()
68 wp.omega.value = omega
69 wp.waist.value = (
70  robot.dynamic.WT.value
71 ) # waist receives a full homogeneous matrix, but only the rotational part is controlled
72 plug(robot.lfToMatrix.sout, wp.footLeft)
73 plug(robot.rfToMatrix.sout, wp.footRight)
74 plug(robot.comTrajGen.x, wp.com)
75 plug(robot.comTrajGen.dx, wp.vcom)
76 plug(robot.comTrajGen.ddx, wp.acom)
77 robot.wp = wp
78 
79 # --- Compute the values to use them in initialization
80 robot.wp.comDes.recompute(0)
81 robot.wp.dcmDes.recompute(0)
82 robot.wp.zmpDes.recompute(0)
83 
84 # -------------------------- ESTIMATION --------------------------
85 
86 # --- Base Estimation
87 robot.device_filters = create_device_filters(robot, dt)
88 robot.imu_filters = create_imu_filters(robot, dt)
89 robot.base_estimator = create_base_estimator(robot, dt, base_estimator_conf)
90 # robot.be_filters = create_be_filters(robot, dt)
91 
92 # --- Reference frame
93 
94 rf = SimpleReferenceFrame("rf")
95 rf.init(robot_name)
96 plug(robot.dynamic.LF, rf.footLeft)
97 plug(robot.dynamic.RF, rf.footRight)
98 robot.rf = rf
99 
100 # --- State transformation
101 stf = StateTransformation("stf")
102 stf.init()
103 plug(robot.rf.referenceFrame, stf.referenceFrame)
104 plug(robot.base_estimator.q, stf.q_in)
105 plug(robot.base_estimator.v, stf.v_in)
106 robot.stf = stf
107 
108 # --- Conversion
109 e2q = EulerToQuat("e2q")
110 plug(robot.stf.q, e2q.euler)
111 robot.e2q = e2q
112 
113 # --- Kinematic computations
114 robot.rdynamic = DynamicPinocchio("real_dynamics")
115 robot.rdynamic.setModel(robot.dynamic.model)
116 robot.rdynamic.setData(robot.rdynamic.model.createData())
117 plug(robot.stf.q, robot.rdynamic.position)
118 robot.rdynamic.velocity.value = [0.0] * robotDim
119 robot.rdynamic.acceleration.value = [0.0] * robotDim
120 
121 # --- CoM Estimation
122 cdc_estimator = DcmEstimator("cdc_estimator")
123 cdc_estimator.init(dt, robot_name)
124 plug(robot.e2q.quaternion, cdc_estimator.q)
125 plug(robot.stf.v, cdc_estimator.v)
126 robot.cdc_estimator = cdc_estimator
127 
128 # --- DCM Estimation
129 estimator = DummyDcmEstimator("dummy")
130 estimator.omega.value = omega
131 estimator.mass.value = 1.0
132 plug(robot.cdc_estimator.c, estimator.com)
133 plug(robot.cdc_estimator.dc, estimator.momenta)
134 estimator.init()
135 robot.estimator = estimator
136 
137 # --- Force calibration
138 robot.ftc = create_ft_calibrator(robot, ft_conf)
139 
140 # --- ZMP estimation
141 zmp_estimator = SimpleZmpEstimator("zmpEst")
142 robot.rdynamic.createOpPoint("sole_LF", "left_sole_link")
143 robot.rdynamic.createOpPoint("sole_RF", "right_sole_link")
144 plug(robot.rdynamic.sole_LF, zmp_estimator.poseLeft)
145 plug(robot.rdynamic.sole_RF, zmp_estimator.poseRight)
146 plug(robot.ftc.left_foot_force_out, zmp_estimator.wrenchLeft)
147 plug(robot.ftc.right_foot_force_out, zmp_estimator.wrenchRight)
148 zmp_estimator.init()
149 robot.zmp_estimator = zmp_estimator
150 
151 # -------------------------- ADMITTANCE CONTROL --------------------------
152 
153 # --- CoM control
154 Kp_com = [0.0] * 2 + [4.0]
155 
156 comErr = operator.Substract_of_vector("comErr")
157 plug(robot.wp.comDes, comErr.sin1)
158 plug(robot.cdc_estimator.c, comErr.sin2)
159 robot.comErr = comErr
160 
161 comControl = operator.Multiply_of_vector("comControl")
162 comControl.sin0.value = Kp_com
163 plug(robot.comErr.sout, comControl.sin1)
164 robot.comControl = comControl
165 
166 forceControl = operator.Add_of_vector("forceControl")
167 plug(robot.comControl.sout, forceControl.sin1)
168 forceControl.sin2.value = [0.0, 0.0, mass * g]
169 robot.forceControl = forceControl
170 
171 wrenchControl = operator.Mix_of_vector("wrenchControl")
172 wrenchControl.setSignalNumber(3)
173 wrenchControl.addSelec(1, 0, 3)
174 wrenchControl.addSelec(2, 3, 3)
175 wrenchControl.default.value = [0.0] * 6
176 plug(robot.forceControl.sout, wrenchControl.signal("sin1"))
177 wrenchControl.signal("sin2").value = [0.0] * 3
178 robot.wrenchControl = wrenchControl
179 
180 # --- Distribute wrench
181 distribute = SimpleDistributeWrench("distribute")
182 plug(robot.e2q.quaternion, distribute.q)
183 distribute.rho.value = 0.5
184 plug(robot.wrenchControl.sout, distribute.wrenchDes)
185 distribute.init(robot_name)
186 robot.distribute = distribute
187 
188 # --- Trick to command both feet controllers at the same time
189 robot.admBF_Kp = Selec_of_vector("admBF_Kp")
190 robot.admBF_Kp.selec(0, 6)
191 robot.admBF_Kp.sin.value = [0.0] * 2 + [3.0] + [0.0] * 3
192 
193 robot.admBF_dqSaturation = Selec_of_vector("admBF_dqSaturation")
194 robot.admBF_dqSaturation.selec(0, 6)
195 robot.admBF_dqSaturation.sin.value = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
196 
197 # --- Left foot
198 controller = AdmittanceControllerEndEffector("admLF")
199 
200 plug(robot.ftc.left_foot_force_out, controller.force)
201 plug(robot.e2q.quaternion, controller.q)
202 plug(robot.admBF_Kp.sout, controller.Kp)
203 controller.Kd.value = [1.0] * 6
204 plug(robot.distribute.wrenchLeft, controller.w_forceDes)
205 plug(robot.admBF_dqSaturation.sout, controller.dqSaturation)
206 
207 controller.init(dt, "right_sole_link", "leg_right_6_joint")
208 
209 robot.admLF = controller
210 
211 # --- Right foot
212 controller = AdmittanceControllerEndEffector("admRF")
213 
214 plug(robot.ftc.right_foot_force_out, controller.force)
215 plug(robot.e2q.quaternion, controller.q)
216 plug(robot.admBF_Kp.sout, controller.Kp)
217 controller.Kd.value = [1.0] * 6
218 plug(robot.distribute.wrenchRight, controller.w_forceDes)
219 plug(robot.admBF_dqSaturation.sout, controller.dqSaturation)
220 
221 controller.init(dt, "left_sole_link", "leg_right_6_joint")
222 
223 robot.admRF = controller
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 
270 # --- Left foot
271 robot.contactLF = MetaTaskKine6d(
272  "contactLF", robot.dynamic, "LF", robot.OperationalPointsMap["left-ankle"]
273 )
274 # handMgrip = np.eye(4)
275 # handMgrip[0:3, 3] = (0.1, 0, 0)
276 # robot.contactLF.opmodif = matrixToTuple(handMgrip)
277 robot.contactLF.feature.frame("desired")
278 # robot.contactLF.feature.selec.value = '111111'
279 robot.contactLF.task.setWithDerivative(True)
280 robot.contactLF.task.controlGain.value = 0
281 robot.contactLF.feature.position.value = np.eye(4)
282 robot.contactLF.feature.velocity.value = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
283 robot.contactLF.featureDes.position.value = np.eye(4)
284 # robot.contactLF.gain.setConstant(300)
285 # plug(robot.wp.footLeftDes, robot.contactLF.featureDes.position) #.errorIN?
286 plug(robot.admLF.dq, robot.contactLF.featureDes.velocity)
287 locals()["contactLF"] = robot.contactLF
288 
289 # --- Right foot
290 robot.contactRF = MetaTaskKine6d(
291  "contactRF", robot.dynamic, "RF", robot.OperationalPointsMap["right-ankle"]
292 )
293 # handMgrip = np.eye(4)
294 # handMgrip[0:3, 3] = (0.1, 0, 0)
295 # robot.contactRF.opmodif = matrixToTuple(handMgrip)
296 robot.contactRF.feature.frame("desired")
297 # robot.contactRF.feature.selec.value = '111111'
298 robot.contactRF.task.setWithDerivative(True)
299 robot.contactRF.task.controlGain.value = 0
300 robot.contactRF.feature.position.value = np.eye(4)
301 robot.contactRF.feature.velocity.value = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
302 robot.contactRF.featureDes.position.value = np.eye(4)
303 # robot.contactRF.gain.setConstant(300)
304 # plug(robot.wp.footLeftDes, robot.contactRF.featureDes.position) #.errorIN?
305 plug(robot.admRF.dq, robot.contactRF.featureDes.velocity)
306 locals()["contactRF"] = robot.contactRF
307 
308 # --- COM
309 robot.taskCom = MetaTaskKineCom(robot.dynamic)
310 plug(robot.wp.comDes, robot.taskCom.featureDes.errorIN)
311 robot.taskCom.task.controlGain.value = 10
312 robot.taskCom.feature.selec.value = "011" # needed ?
313 
314 # --- Waist
315 robot.keepWaist = MetaTaskKine6d(
316  "keepWaist", robot.dynamic, "WT", robot.OperationalPointsMap["waist"]
317 )
318 robot.keepWaist.feature.frame("desired")
319 robot.keepWaist.gain.setConstant(300)
320 plug(robot.wp.waistDes, robot.keepWaist.featureDes.position)
321 robot.keepWaist.feature.selec.value = "111000"
322 locals()["keepWaist"] = robot.keepWaist
323 
324 # # --- Posture
325 # robot.taskPos = Task ('task_pos')
326 # robot.taskPos.feature = FeaturePosture('feature_pos')
327 #
328 # q = list(robot.dynamic.position.value)
329 # robot.taskPos.feature.state.value = q
330 # robot.taskPos.feature.posture.value = q
331 
332 # robotDim = robot.dynamic.getDimension() # 38
333 # robot.taskPos.feature.selectDof(6,True)
334 # robot.taskPos.feature.selectDof(7,True)
335 # robot.taskPos.feature.selectDof(8,True)
336 # robot.taskPos.feature.selectDof(9,True)
337 # robot.taskPos.feature.selectDof(10,True)
338 # robot.taskPos.feature.selectDof(11,True)
339 # robot.taskPos.feature.selectDof(12,True)
340 # robot.taskPos.feature.selectDof(13,True)
341 # robot.taskPos.feature.selectDof(14,True)
342 # robot.taskPos.feature.selectDof(15,True)
343 # robot.taskPos.feature.selectDof(16,True)
344 # robot.taskPos.feature.selectDof(17,True)
345 # robot.taskPos.feature.selectDof(18,True)
346 # robot.taskPos.feature.selectDof(19,True)
347 # robot.taskPos.feature.selectDof(20,True)
348 # robot.taskPos.feature.selectDof(21,True)
349 # robot.taskPos.feature.selectDof(22,True)
350 # robot.taskPos.feature.selectDof(23,True)
351 # robot.taskPos.feature.selectDof(24,True)
352 # robot.taskPos.feature.selectDof(25,True)
353 # robot.taskPos.feature.selectDof(26,True)
354 # robot.taskPos.feature.selectDof(27,True)
355 # robot.taskPos.feature.selectDof(28,True)
356 # robot.taskPos.feature.selectDof(29,True)
357 # robot.taskPos.feature.selectDof(30,True)
358 # robot.taskPos.feature.selectDof(31,True)
359 # robot.taskPos.feature.selectDof(32,True)
360 # robot.taskPos.feature.selectDof(33,True)
361 # robot.taskPos.feature.selectDof(34,True)
362 # robot.taskPos.feature.selectDof(35,True)
363 # robot.taskPos.feature.selectDof(36,True)
364 # robot.taskPos.feature.selectDof(37,True)
365 
366 # robot.taskPos.controlGain.value = 100.0
367 # robot.taskPos.add(robot.taskPos.feature.name)
368 # plug(robot.dynamic.position, robot.taskPos.feature.state)
369 
370 # --- SOT solver
371 robot.sot = SOT("sot")
372 robot.sot.setSize(robot.dynamic.getDimension())
373 
374 # --- Plug SOT control to device through control manager
375 plug(robot.sot.control, robot.cm.ctrl_sot_input)
376 plug(robot.cm.u_safe, robot.device.control)
377 
378 robot.sot.push(robot.taskUpperBody.name)
379 robot.sot.push(robot.contactRF.task.name)
380 robot.sot.push(robot.contactLF.task.name)
381 robot.sot.push(robot.taskCom.task.name)
382 robot.sot.push(robot.keepWaist.task.name)
383 # robot.sot.push(robot.taskPos.name)
384 # robot.device.control.recompute(0) # this crashes as it employs joint sensors which are not ready yet
385 
386 # --- Fix robot.dynamic inputs
387 plug(robot.device.velocity, robot.dynamic.velocity)
388 robot.dvdt = Derivator_of_Vector("dv_dt")
389 robot.dvdt.dt.value = dt
390 plug(robot.device.velocity, robot.dvdt.sin)
391 plug(robot.dvdt.sout, robot.dynamic.acceleration)
392 
393 # -------------------------- PLOTS --------------------------
394 
395 # --- ROS PUBLISHER
396 robot.publisher = create_rospublish(robot, "robot_publisher")
397 
398 create_topic(
399  robot.publisher, robot.wp, "comDes", robot=robot, data_type="vector"
400 ) # desired CoM
401 
402 create_topic(
403  robot.publisher, robot.cdc_estimator, "c", robot=robot, data_type="vector"
404 ) # estimated CoM
405 create_topic(
406  robot.publisher, robot.cdc_estimator, "dc", robot=robot, data_type="vector"
407 ) # estimated CoM velocity
408 
409 create_topic(
410  robot.publisher, robot.dynamic, "com", robot=robot, data_type="vector"
411 ) # resulting SOT CoM
412 
413 create_topic(
414  robot.publisher, robot.estimator, "dcm", robot=robot, data_type="vector"
415 ) # estimated DCM
416 
417 create_topic(
418  robot.publisher, robot.dynamic, "zmp", robot=robot, data_type="vector"
419 ) # SOT ZMP
420 create_topic(
421  robot.publisher, robot.zmp_estimator, "zmp", robot=robot, data_type="vector"
422 ) # estimated ZMP
423 
424 # create_topic(robot.publisher, robot.device, 'forceLLEG', robot = robot, data_type='vector') # measured left wrench
425 # create_topic(robot.publisher, robot.device, 'forceRLEG', robot = robot, data_type='vector') # measured right wrench
426 
427 # create_topic(robot.publisher, robot.device_filters.ft_LF_filter, 'x_filtered', robot = robot, data_type='vector') # filtered left wrench
428 # create_topic(robot.publisher, robot.device_filters.ft_RF_filter, 'x_filtered', robot = robot, data_type='vector') # filtered right wrench
429 
430 create_topic(
431  robot.publisher, robot.distribute, "wrenchLeft", robot=robot, data_type="vector"
432 )
433 create_topic(
434  robot.publisher, robot.distribute, "wrenchRight", robot=robot, data_type="vector"
435 )
436 
437 create_topic(
438  robot.publisher, robot.ftc, "left_foot_force_out", robot=robot, data_type="vector"
439 ) # calibrated left wrench
440 create_topic(
441  robot.publisher, robot.ftc, "right_foot_force_out", robot=robot, data_type="vector"
442 ) # calibrated right wrench
443 
444 # --- TRACER
445 robot.tracer = TracerRealTime("com_tracer")
446 robot.tracer.setBufferSize(80 * (2**20))
447 robot.tracer.open("/tmp", "dg_", ".dat")
448 robot.device.after.addSignal("{0}.triger".format(robot.tracer.name))
449 
450 addTrace(robot.tracer, robot.wp, "comDes") # desired CoM
451 
452 addTrace(robot.tracer, robot.cdc_estimator, "c") # estimated CoM
453 addTrace(robot.tracer, robot.cdc_estimator, "dc") # estimated CoM velocity
454 
455 addTrace(robot.tracer, robot.dynamic, "com") # resulting SOT CoM
456 
457 addTrace(robot.tracer, robot.estimator, "dcm") # estimated DCM
458 
459 addTrace(robot.tracer, robot.dynamic, "zmp") # SOT ZMP
460 addTrace(robot.tracer, robot.zmp_estimator, "zmp") # estimated ZMP
461 
462 addTrace(robot.tracer, robot.ftc, "left_foot_force_out") # calibrated left wrench
463 addTrace(robot.tracer, robot.ftc, "right_foot_force_out") # calibrated right wrench
464 
465 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_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