sot-talos-balance  2.0.5
Collection of dynamic-graph entities aimed at implementing balance control on talos.
appli_ankle_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.dynamics_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 baseEstimatorConf
23 import dynamic_graph.sot_talos_balance.talos.control_manager_conf as controlManagerConf
24 import dynamic_graph.sot_talos_balance.talos.ft_calibration_conf as forceConf
25 import dynamic_graph.sot_talos_balance.talos.parameter_server_conf as paramServerConfig
26 from dynamic_graph.sot_talos_balance.create_entities_utils import *
27 from dynamic_graph.sot_talos_balance.meta_task_joint import MetaTaskKineJoint
28 
29 robot.timeStep = robot.device.getTimeStep()
30 dt = robot.timeStep
31 
32 # --- Pendulum parameters
33 robotName = "robot"
34 robot.dynamic.com.recompute(0)
35 robotDim = robot.dynamic.getDimension()
36 mass = robot.dynamic.data.mass[0]
37 h = robot.dynamic.com.value[2]
38 g = 9.81
39 omega = sqrt(g / h)
40 
41 # --- Parameter server
42 robot.param_server = create_parameter_server(paramServerConfig, robot.timeStep)
43 
44 # --- Initial feet and waist
45 robot.dynamic.createOpPoint("LF", robot.OperationalPointsMap["left-ankle"])
46 robot.dynamic.createOpPoint("RF", robot.OperationalPointsMap["right-ankle"])
47 robot.dynamic.createOpPoint("WT", robot.OperationalPointsMap["waist"])
48 robot.dynamic.LF.recompute(0)
49 robot.dynamic.RF.recompute(0)
50 robot.dynamic.WT.recompute(0)
51 
52 # -------------------------- DESIRED TRAJECTORY --------------------------
53 
54 # --- Trajectory generators
55 robot.comTrajGen = create_com_trajectory_generator(dt, robot)
56 
57 robot.lfTrajGen = create_pose_rpy_trajectory_generator(dt, robot, "LF")
58 robot.lfToMatrix = PoseRollPitchYawToMatrixHomo("lf2m")
59 plug(robot.lfTrajGen.x, robot.lfToMatrix.sin)
60 
61 robot.rfTrajGen = create_pose_rpy_trajectory_generator(dt, robot, "RF")
62 robot.rfToMatrix = PoseRollPitchYawToMatrixHomo("rf2m")
63 plug(robot.rfTrajGen.x, robot.rfToMatrix.sin)
64 
65 # --- Interface with controller entities
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.baseEstimator = create_base_estimator(robot, dt, baseEstimatorConf)
90 
91 # --- Reference frame
92 rf = SimpleReferenceFrame("rf")
93 rf.init(robotName)
94 plug(robot.dynamic.LF, rf.footLeft)
95 plug(robot.dynamic.RF, rf.footRight)
96 robot.rf = rf
97 
98 # --- State transformation
99 stf = StateTransformation("stf")
100 stf.init()
101 plug(robot.rf.referenceFrame, stf.referenceFrame)
102 plug(robot.baseEstimator.q, stf.q_in)
103 plug(robot.baseEstimator.v, stf.v_in)
104 robot.stf = stf
105 
106 # --- Conversion
107 e2q = EulerToQuat("e2q")
108 plug(robot.stf.q, e2q.euler)
109 robot.e2q = e2q
110 
111 # --- Kinematic computations
112 robot.rdynamic = DynamicPinocchio("real_dynamics")
113 robot.rdynamic.setModel(robot.dynamic.model)
114 robot.rdynamic.setData(robot.rdynamic.model.createData())
115 plug(robot.stf.q, robot.rdynamic.position)
116 robot.rdynamic.velocity.value = [0.0] * robotDim
117 robot.rdynamic.acceleration.value = [0.0] * robotDim
118 
119 # --- CoM Estimation
120 cdc_estimator = DcmEstimator("cdc_estimator")
121 cdc_estimator.init(dt, robotName)
122 plug(robot.e2q.quaternion, cdc_estimator.q)
123 plug(robot.stf.v, cdc_estimator.v)
124 robot.cdc_estimator = cdc_estimator
125 
126 # --- DCM Estimation
127 estimator = DummyDcmEstimator("dummy")
128 estimator.omega.value = omega
129 estimator.mass.value = 1.0
130 plug(robot.cdc_estimator.c, estimator.com)
131 plug(robot.cdc_estimator.dc, estimator.momenta)
132 estimator.init()
133 robot.estimator = estimator
134 
135 # --- Force calibration
136 robot.ftc = create_ft_calibrator(robot, forceConf)
137 
138 # --- ZMP estimation
139 zmp_estimator = SimpleZmpEstimator("zmpEst")
140 robot.rdynamic.createOpPoint("sole_LF", "left_sole_link")
141 robot.rdynamic.createOpPoint("sole_RF", "right_sole_link")
142 plug(robot.rdynamic.sole_LF, zmp_estimator.poseLeft)
143 plug(robot.rdynamic.sole_RF, zmp_estimator.poseRight)
144 plug(robot.ftc.left_foot_force_out, zmp_estimator.wrenchLeft)
145 plug(robot.ftc.right_foot_force_out, zmp_estimator.wrenchRight)
146 zmp_estimator.init()
147 robot.zmp_estimator = zmp_estimator
148 
149 # -------------------------- ADMITTANCE CONTROL --------------------------
150 
151 # --- CoM control
152 Kp_com = [0.0] * 2 + [4.0]
153 
154 comErr = operator.Substract_of_vector("comErr")
155 plug(robot.wp.comDes, comErr.sin1)
156 plug(robot.cdc_estimator.c, comErr.sin2)
157 robot.comErr = comErr
158 
159 comControl = operator.Multiply_of_vector("comControl")
160 comControl.sin0.value = Kp_com
161 plug(robot.comErr.sout, comControl.sin1)
162 robot.comControl = comControl
163 
164 forceControl = operator.Add_of_vector("forceControl")
165 plug(robot.comControl.sout, forceControl.sin1)
166 forceControl.sin2.value = [0.0, 0.0, mass * g]
167 robot.forceControl = forceControl
168 
169 wrenchControl = operator.Mix_of_vector("wrenchControl")
170 wrenchControl.setSignalNumber(3)
171 wrenchControl.addSelec(1, 0, 3)
172 wrenchControl.addSelec(2, 3, 3)
173 wrenchControl.default.value = [0.0] * 6
174 plug(robot.forceControl.sout, wrenchControl.signal("sin1"))
175 wrenchControl.signal("sin2").value = [0.0] * 3
176 robot.wrenchControl = wrenchControl
177 
178 # --- Distribute wrench
179 distribute = DistributeWrench("distribute")
180 plug(robot.e2q.quaternion, distribute.q)
181 plug(robot.wrenchControl.sout, distribute.wrenchDes)
182 distribute.init(robotName)
183 robot.wrenchDistributor = distribute
184 
185 # --- Left foot
186 robot.leftAnkleController = create_ankle_admittance_controller(
187  [0, 0], robot, "left", "leftController"
188 )
189 
190 # --- Right foot
191 robot.rightAnkleController = create_ankle_admittance_controller(
192  [0, 0], robot, "right", "rightController"
193 )
194 
195 # --- Control Manager
196 robot.controlManager = create_ctrl_manager(controlManagerConf, dt, robot_name="robot")
197 robot.controlManager.addCtrlMode("sot_input")
198 robot.controlManager.setCtrlMode("all", "sot_input")
199 robot.controlManager.addEmergencyStopSIN("zmp")
200 
201 # -------------------------- SOT CONTROL --------------------------
202 
203 # --- Upper body
204 robot.taskUpperBody = Task("task_upper_body")
205 robot.taskUpperBody.feature = FeaturePosture("feature_upper_body")
206 
207 q = list(robot.dynamic.position.value)
208 robot.taskUpperBody.feature.state.value = q
209 robot.taskUpperBody.feature.posture.value = q
210 
211 # robotDim = robot.dynamic.getDimension() # 38
212 robot.taskUpperBody.feature.selectDof(18, True)
213 robot.taskUpperBody.feature.selectDof(19, True)
214 robot.taskUpperBody.feature.selectDof(20, True)
215 robot.taskUpperBody.feature.selectDof(21, True)
216 robot.taskUpperBody.feature.selectDof(22, True)
217 robot.taskUpperBody.feature.selectDof(23, True)
218 robot.taskUpperBody.feature.selectDof(24, True)
219 robot.taskUpperBody.feature.selectDof(25, True)
220 robot.taskUpperBody.feature.selectDof(26, True)
221 robot.taskUpperBody.feature.selectDof(27, True)
222 robot.taskUpperBody.feature.selectDof(28, True)
223 robot.taskUpperBody.feature.selectDof(29, True)
224 robot.taskUpperBody.feature.selectDof(30, True)
225 robot.taskUpperBody.feature.selectDof(31, True)
226 robot.taskUpperBody.feature.selectDof(32, True)
227 robot.taskUpperBody.feature.selectDof(33, True)
228 robot.taskUpperBody.feature.selectDof(34, True)
229 robot.taskUpperBody.feature.selectDof(35, True)
230 robot.taskUpperBody.feature.selectDof(36, True)
231 robot.taskUpperBody.feature.selectDof(37, True)
232 
233 robot.taskUpperBody.controlGain.value = 100.0
234 robot.taskUpperBody.add(robot.taskUpperBody.feature.name)
235 plug(robot.dynamic.position, robot.taskUpperBody.feature.state)
236 
237 # --- RIGHT ANKLE PITCH
238 RightPitchJoint = 16
239 
240 rightPitchSelec = Selec_of_vector("rightPitchSelec")
241 rightPitchSelec.selec(1, 2)
242 plug(robot.rightAnkleController.dRP, rightPitchSelec.sin)
243 
244 robot.rightAnklePitchTask = MetaTaskKineJoint(robot.dynamic, RightPitchJoint)
245 robot.rightAnklePitchTask.featureDes.errorIN.value = [0.0]
246 robot.rightAnklePitchTask.task.controlGain.value = 0
247 robot.rightAnklePitchTask.task.setWithDerivative(True)
248 plug(rightPitchSelec.sout, robot.rightAnklePitchTask.featureDes.errordotIN)
249 
250 # --- RIGHT ANKLE ROLL
251 RightRollJoint = 17
252 
253 rightRollSelec = Selec_of_vector("rightRollSelec")
254 rightRollSelec.selec(0, 1)
255 plug(robot.rightAnkleController.dRP, rightRollSelec.sin)
256 
257 robot.rightAnkleRollTask = MetaTaskKineJoint(robot.dynamic, RightRollJoint)
258 robot.rightAnkleRollTask.featureDes.errorIN.value = [0.0]
259 robot.rightAnkleRollTask.task.controlGain.value = 0
260 robot.rightAnkleRollTask.task.setWithDerivative(True)
261 plug(rightRollSelec.sout, robot.rightAnkleRollTask.featureDes.errordotIN)
262 
263 # --- LEFT ANKLE PITCH
264 LeftPitchJoint = 10
265 
266 leftPitchSelec = Selec_of_vector("leftPitchSelec")
267 leftPitchSelec.selec(1, 2)
268 plug(robot.leftAnkleController.dRP, leftPitchSelec.sin)
269 
270 robot.leftAnklePitchTask = MetaTaskKineJoint(robot.dynamic, LeftPitchJoint)
271 robot.leftAnklePitchTask.featureDes.errorIN.value = [0.0]
272 robot.leftAnklePitchTask.task.controlGain.value = 0
273 robot.leftAnklePitchTask.task.setWithDerivative(True)
274 plug(leftPitchSelec.sout, robot.leftAnklePitchTask.featureDes.errordotIN)
275 
276 # --- LEFT ANKLE ROLL
277 LeftRollJoint = 11
278 
279 leftRollSelec = Selec_of_vector("leftRollSelec")
280 leftRollSelec.selec(0, 1)
281 plug(robot.leftAnkleController.dRP, leftRollSelec.sin)
282 
283 robot.leftAnkleRollTask = MetaTaskKineJoint(robot.dynamic, LeftRollJoint)
284 robot.leftAnkleRollTask.featureDes.errorIN.value = [0.0]
285 robot.leftAnkleRollTask.task.controlGain.value = 0
286 robot.leftAnkleRollTask.task.setWithDerivative(True)
287 plug(leftRollSelec.sout, robot.leftAnkleRollTask.featureDes.errordotIN)
288 
289 # --- CONTACTS
290 # define contactLF and contactRF
291 robot.contactLF = MetaTaskKine6d(
292  "contactLF", robot.dynamic, "LF", robot.OperationalPointsMap["left-ankle"]
293 )
294 robot.contactLF.feature.frame("desired")
295 robot.contactLF.gain.setConstant(100)
296 robot.contactLF.keep()
297 locals()["contactLF"] = robot.contactLF
298 
299 robot.contactRF = MetaTaskKine6d(
300  "contactRF", robot.dynamic, "RF", robot.OperationalPointsMap["right-ankle"]
301 )
302 robot.contactRF.feature.frame("desired")
303 robot.contactRF.gain.setConstant(100)
304 robot.contactRF.keep()
305 locals()["contactRF"] = robot.contactRF
306 
307 # --- COM
308 robot.taskCom = MetaTaskKineCom(robot.dynamic)
309 plug(robot.wp.comDes, robot.taskCom.featureDes.errorIN)
310 robot.taskCom.task.controlGain.value = 10
311 robot.taskCom.feature.selec.value = "011" # needed ?
312 
313 # --- Waist
314 robot.keepWaist = MetaTaskKine6d(
315  "keepWaist", robot.dynamic, "WT", robot.OperationalPointsMap["waist"]
316 )
317 robot.keepWaist.feature.frame("desired")
318 robot.keepWaist.gain.setConstant(300)
319 plug(robot.wp.waistDes, robot.keepWaist.featureDes.position)
320 robot.keepWaist.feature.selec.value = "111000"
321 locals()["keepWaist"] = robot.keepWaist
322 
323 # --- SOT solver
324 robot.sot = SOT("sot")
325 robot.sot.setSize(robot.dynamic.getDimension())
326 
327 # --- Plug SOT control to device through control manager
328 plug(robot.sot.control, robot.controlManager.ctrl_sot_input)
329 plug(robot.controlManager.u_safe, robot.device.control)
330 
331 robot.sot.push(robot.taskUpperBody.name)
332 robot.sot.push(robot.contactRF.task.name)
333 robot.sot.push(robot.contactLF.task.name)
334 robot.sot.push(robot.taskCom.task.name)
335 robot.sot.push(robot.keepWaist.task.name)
336 # robot.sot.push(robot.taskPos.name)
337 # robot.device.control.recompute(0) # this crashes as it employs joint sensors which are not ready yet
338 
339 # --- Fix robot.dynamic inputs
340 plug(robot.device.velocity, robot.dynamic.velocity)
341 robot.dvdt = Derivator_of_Vector("dv_dt")
342 robot.dvdt.dt.value = dt
343 plug(robot.device.velocity, robot.dvdt.sin)
344 plug(robot.dvdt.sout, robot.dynamic.acceleration)
345 
346 # -------------------------- PLOTS --------------------------
347 
348 # --- ROS PUBLISHER
349 robot.publisher = create_rospublish(robot, "robot_publisher")
350 
351 create_topic(
352  robot.publisher, robot.wp, "comDes", robot=robot, data_type="vector"
353 ) # desired CoM
354 
355 create_topic(
356  robot.publisher, robot.cdc_estimator, "c", robot=robot, data_type="vector"
357 ) # estimated CoM
358 create_topic(
359  robot.publisher, robot.cdc_estimator, "dc", robot=robot, data_type="vector"
360 ) # estimated CoM velocity
361 
362 create_topic(
363  robot.publisher, robot.dynamic, "com", robot=robot, data_type="vector"
364 ) # resulting SOT CoM
365 
366 create_topic(
367  robot.publisher, robot.estimator, "dcm", robot=robot, data_type="vector"
368 ) # estimated DCM
369 
370 create_topic(
371  robot.publisher, robot.dynamic, "zmp", robot=robot, data_type="vector"
372 ) # SOT ZMP
373 create_topic(
374  robot.publisher, robot.zmp_estimator, "zmp", robot=robot, data_type="vector"
375 ) # estimated ZMP
376 
377 # create_topic(robot.publisher, robot.device, 'forceLLEG', robot = robot, data_type='vector') # measured left wrench
378 # create_topic(robot.publisher, robot.device, 'forceRLEG', robot = robot, data_type='vector') # measured right wrench
379 
380 # create_topic(robot.publisher, robot.device_filters.ft_LF_filter, 'x_filtered', robot = robot, data_type='vector') # filtered left wrench
381 # create_topic(robot.publisher, robot.device_filters.ft_RF_filter, 'x_filtered', robot = robot, data_type='vector') # filtered right wrench
382 
383 create_topic(
384  robot.publisher,
385  robot.wrenchDistributor,
386  "wrenchLeft",
387  robot=robot,
388  data_type="vector",
389 )
390 create_topic(
391  robot.publisher,
392  robot.wrenchDistributor,
393  "wrenchRight",
394  robot=robot,
395  data_type="vector",
396 )
397 
398 create_topic(
399  robot.publisher, robot.ftc, "left_foot_force_out", robot=robot, data_type="vector"
400 ) # calibrated left wrench
401 create_topic(
402  robot.publisher, robot.ftc, "right_foot_force_out", robot=robot, data_type="vector"
403 ) # calibrated right wrench
404 
405 # --- TRACER
406 robot.tracer = TracerRealTime("com_tracer")
407 robot.tracer.setBufferSize(80 * (2**20))
408 robot.tracer.open("/tmp", "dg_", ".dat")
409 robot.device.after.addSignal("{0}.triger".format(robot.tracer.name))
410 
411 addTrace(robot.tracer, robot.wp, "comDes") # desired CoM
412 
413 addTrace(robot.tracer, robot.cdc_estimator, "c") # estimated CoM
414 addTrace(robot.tracer, robot.cdc_estimator, "dc") # estimated CoM velocity
415 
416 addTrace(robot.tracer, robot.dynamic, "com") # resulting SOT CoM
417 
418 addTrace(robot.tracer, robot.estimator, "dcm") # estimated DCM
419 
420 addTrace(robot.tracer, robot.dynamic, "zmp") # SOT ZMP
421 addTrace(robot.tracer, robot.zmp_estimator, "zmp") # estimated ZMP
422 
423 addTrace(robot.tracer, robot.ftc, "left_foot_force_out") # calibrated left wrench
424 addTrace(robot.tracer, robot.ftc, "right_foot_force_out") # calibrated right wrench
425 
426 robot.tracer.start()
sot_talos_balance.create_entities_utils.create_ankle_admittance_controller
def create_ankle_admittance_controller(gains, robot, side, name)
Definition: create_entities_utils.py:255
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