sot-talos-balance  2.0.5
Collection of dynamic-graph entities aimed at implementing balance control on talos.
appli_waistControl.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  MatrixHomoToPoseQuaternion,
11  Task,
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 from dynamic_graph.sot_talos_balance.simple_controller_6d import SimpleController6d
28 
29 cm_conf.CTRL_MAX = 1000.0 # temporary hack
30 
31 robot.timeStep = robot.device.getTimeStep()
32 dt = robot.timeStep
33 
34 # --- Pendulum parameters
35 robot_name = "robot"
36 robot.dynamic.com.recompute(0)
37 robotDim = robot.dynamic.getDimension()
38 mass = robot.dynamic.data.mass[0]
39 h = robot.dynamic.com.value[2]
40 g = 9.81
41 omega = sqrt(g / h)
42 
43 # --- Parameter server
44 robot.param_server = create_parameter_server(param_server_conf, dt)
45 
46 # --- Initial feet and waist
47 robot.dynamic.createOpPoint("LF", robot.OperationalPointsMap["left-ankle"])
48 robot.dynamic.createOpPoint("RF", robot.OperationalPointsMap["right-ankle"])
49 robot.dynamic.createOpPoint("WT", robot.OperationalPointsMap["waist"])
50 robot.dynamic.LF.recompute(0)
51 robot.dynamic.RF.recompute(0)
52 robot.dynamic.WT.recompute(0)
53 
54 # -------------------------- DESIRED TRAJECTORY --------------------------
55 
56 # --- Trajectory generators
57 
58 # --- CoM
59 robot.comTrajGen = create_com_trajectory_generator(dt, robot)
60 
61 # --- Left foot
62 robot.lfTrajGen = create_pose_rpy_trajectory_generator(dt, robot, "LF")
63 # robot.lfTrajGen.x.recompute(0) # trigger computation of initial value
64 robot.lfToMatrix = PoseRollPitchYawToMatrixHomo("lf2m")
65 plug(robot.lfTrajGen.x, robot.lfToMatrix.sin)
66 
67 # --- Right foot
68 robot.rfTrajGen = create_pose_rpy_trajectory_generator(dt, robot, "RF")
69 # robot.rfTrajGen.x.recompute(0) # trigger computation of initial value
70 robot.rfToMatrix = PoseRollPitchYawToMatrixHomo("rf2m")
71 plug(robot.rfTrajGen.x, robot.rfToMatrix.sin)
72 
73 # --- Waist
74 robot.waistTrajGen = create_orientation_rpy_trajectory_generator(dt, robot, "WT")
75 # robot.waistTrajGen.x.recompute(0) # trigger computation of initial value
76 
77 robot.waistMix = Mix_of_vector("waistMix")
78 robot.waistMix.setSignalNumber(3)
79 robot.waistMix.addSelec(1, 0, 3)
80 robot.waistMix.addSelec(2, 3, 3)
81 robot.waistMix.default.value = [0.0] * 6
82 robot.waistMix.signal("sin1").value = [0.0] * 3
83 plug(robot.waistTrajGen.x, robot.waistMix.signal("sin2"))
84 
85 robot.waistToMatrix = PoseRollPitchYawToMatrixHomo("w2m")
86 plug(robot.waistMix.sout, robot.waistToMatrix.sin)
87 
88 # --- Rho
89 robot.rhoTrajGen = create_scalar_trajectory_generator(dt, 0.5, "rhoTrajGen")
90 robot.rhoScalar = Component_of_vector("rho_scalar")
91 robot.rhoScalar.setIndex(0)
92 plug(robot.rhoTrajGen.x, robot.rhoScalar.sin)
93 
94 # --- Interface with controller entities
95 
96 wp = DummyWalkingPatternGenerator("dummy_wp")
97 wp.init()
98 wp.omega.value = omega
99 plug(robot.waistToMatrix.sout, wp.waist)
100 plug(robot.lfToMatrix.sout, wp.footLeft)
101 plug(robot.rfToMatrix.sout, wp.footRight)
102 plug(robot.comTrajGen.x, wp.com)
103 plug(robot.comTrajGen.dx, wp.vcom)
104 plug(robot.comTrajGen.ddx, wp.acom)
105 
106 robot.wp = wp
107 
108 # --- Compute the values to use them in initialization
109 robot.wp.comDes.recompute(0)
110 robot.wp.dcmDes.recompute(0)
111 robot.wp.zmpDes.recompute(0)
112 
113 # -------------------------- ESTIMATION --------------------------
114 
115 # --- Base Estimation
116 robot.device_filters = create_device_filters(robot, dt)
117 robot.imu_filters = create_imu_filters(robot, dt)
118 robot.base_estimator = create_base_estimator(robot, dt, base_estimator_conf)
119 
120 robot.m2qLF = MatrixHomoToPoseQuaternion("m2qLF")
121 plug(robot.dynamic.LF, robot.m2qLF.sin)
122 plug(robot.m2qLF.sout, robot.base_estimator.lf_ref_xyzquat)
123 robot.m2qRF = MatrixHomoToPoseQuaternion("m2qRF")
124 plug(robot.dynamic.RF, robot.m2qRF.sin)
125 plug(robot.m2qRF.sout, robot.base_estimator.rf_ref_xyzquat)
126 
127 # robot.be_filters = create_be_filters(robot, dt)
128 
129 # --- Conversion
130 e2q = EulerToQuat("e2q")
131 plug(robot.base_estimator.q, e2q.euler)
132 robot.e2q = e2q
133 
134 # --- Kinematic computations
135 robot.rdynamic = DynamicPinocchio("real_dynamics")
136 robot.rdynamic.setModel(robot.dynamic.model)
137 robot.rdynamic.setData(robot.rdynamic.model.createData())
138 # plug(robot.base_estimator.q,robot.rdynamic.position)
139 
140 robot.baseselec = Selec_of_vector("base_selec")
141 robot.baseselec.selec(0, 6)
142 plug(robot.base_estimator.q, robot.baseselec.sin)
143 plug(robot.baseselec.sout, robot.rdynamic.ffposition)
144 
145 plug(robot.device.state, robot.rdynamic.position)
146 robot.rdynamic.velocity.value = [0.0] * robotDim
147 robot.rdynamic.acceleration.value = [0.0] * robotDim
148 
149 # --- CoM Estimation
150 cdc_estimator = DcmEstimator("cdc_estimator")
151 cdc_estimator.init(dt, robot_name)
152 plug(robot.e2q.quaternion, cdc_estimator.q)
153 plug(robot.base_estimator.v, cdc_estimator.v)
154 robot.cdc_estimator = cdc_estimator
155 
156 # --- DCM Estimation
157 estimator = DummyDcmEstimator("dummy")
158 estimator.omega.value = omega
159 estimator.mass.value = 1.0
160 plug(robot.cdc_estimator.c, estimator.com)
161 plug(robot.cdc_estimator.dc, estimator.momenta)
162 estimator.init()
163 robot.estimator = estimator
164 
165 # --- Force calibration
166 robot.ftc = create_ft_calibrator(robot, ft_conf)
167 
168 # --- ZMP estimation
169 zmp_estimator = SimpleZmpEstimator("zmpEst")
170 robot.rdynamic.createOpPoint("sole_LF", "left_sole_link")
171 robot.rdynamic.createOpPoint("sole_RF", "right_sole_link")
172 plug(robot.rdynamic.sole_LF, zmp_estimator.poseLeft)
173 plug(robot.rdynamic.sole_RF, zmp_estimator.poseRight)
174 plug(robot.ftc.left_foot_force_out, zmp_estimator.wrenchLeft)
175 plug(robot.ftc.right_foot_force_out, zmp_estimator.wrenchRight)
176 zmp_estimator.init()
177 robot.zmp_estimator = zmp_estimator
178 
179 # --- Control Manager
180 robot.cm = create_ctrl_manager(cm_conf, dt, robot_name="robot")
181 robot.cm.addCtrlMode("sot_input")
182 robot.cm.setCtrlMode("all", "sot_input")
183 
184 # -------------------------- SOT CONTROL --------------------------
185 
186 # --- Upper body
187 robot.taskUpperBody = Task("task_upper_body")
188 robot.taskUpperBody.feature = FeaturePosture("feature_upper_body")
189 
190 q = list(robot.dynamic.position.value)
191 robot.taskUpperBody.feature.state.value = q
192 robot.taskUpperBody.feature.posture.value = q
193 
194 # robotDim = robot.dynamic.getDimension() # 38
195 robot.taskUpperBody.feature.selectDof(18, True)
196 robot.taskUpperBody.feature.selectDof(19, True)
197 robot.taskUpperBody.feature.selectDof(20, True)
198 robot.taskUpperBody.feature.selectDof(21, True)
199 robot.taskUpperBody.feature.selectDof(22, True)
200 robot.taskUpperBody.feature.selectDof(23, True)
201 robot.taskUpperBody.feature.selectDof(24, True)
202 robot.taskUpperBody.feature.selectDof(25, True)
203 robot.taskUpperBody.feature.selectDof(26, True)
204 robot.taskUpperBody.feature.selectDof(27, True)
205 robot.taskUpperBody.feature.selectDof(28, True)
206 robot.taskUpperBody.feature.selectDof(29, True)
207 robot.taskUpperBody.feature.selectDof(30, True)
208 robot.taskUpperBody.feature.selectDof(31, True)
209 robot.taskUpperBody.feature.selectDof(32, True)
210 robot.taskUpperBody.feature.selectDof(33, True)
211 robot.taskUpperBody.feature.selectDof(34, True)
212 robot.taskUpperBody.feature.selectDof(35, True)
213 robot.taskUpperBody.feature.selectDof(36, True)
214 robot.taskUpperBody.feature.selectDof(37, True)
215 
216 robot.taskUpperBody.controlGain.value = 100.0
217 robot.taskUpperBody.add(robot.taskUpperBody.feature.name)
218 plug(robot.dynamic.position, robot.taskUpperBody.feature.state)
219 
220 # --- CONTACTS
221 # define contactLF and contactRF
222 robot.contactLF = MetaTaskKine6d(
223  "contactLF", robot.rdynamic, "LF", robot.OperationalPointsMap["left-ankle"]
224 )
225 robot.contactLF.feature.frame("desired")
226 robot.contactLF.gain.setConstant(1.0)
227 plug(robot.wp.footLeftDes, robot.contactLF.featureDes.position) # .errorIN?
228 locals()["contactLF"] = robot.contactLF
229 
230 robot.contactRF = MetaTaskKine6d(
231  "contactRF", robot.rdynamic, "RF", robot.OperationalPointsMap["right-ankle"]
232 )
233 robot.contactRF.feature.frame("desired")
234 robot.contactRF.gain.setConstant(1.0)
235 plug(robot.wp.footRightDes, robot.contactRF.featureDes.position) # .errorIN?
236 locals()["contactRF"] = robot.contactRF
237 
238 
243 
244 # --- COM
245 robot.taskCom = MetaTaskKineCom(robot.rdynamic)
246 plug(robot.wp.comDes, robot.taskCom.featureDes.errorIN)
247 robot.taskCom.task.controlGain.value = 1.0
248 # robot.taskCom.feature.selec.value = '011'
249 
250 # --- Waist
251 robot.keepWaist = MetaTaskKine6d(
252  "keepWaist", robot.rdynamic, "WT", robot.OperationalPointsMap["waist"]
253 )
254 robot.keepWaist.feature.frame("desired")
255 robot.keepWaist.gain.setConstant(1.0)
256 plug(robot.wp.waistDes, robot.keepWaist.featureDes.position)
257 robot.keepWaist.feature.selec.value = "111000"
258 locals()["keepWaist"] = robot.keepWaist
259 
260 # --- SOT solver
261 robot.sot = SOT("sot")
262 robot.sot.setSize(robot.dynamic.getDimension())
263 
264 # --- Plug SOT control to device through control manager
265 plug(robot.sot.control, robot.cm.ctrl_sot_input)
266 plug(robot.cm.u_safe, robot.device.control)
267 
268 robot.sot.push(robot.taskUpperBody.name)
269 robot.sot.push(robot.contactRF.task.name)
270 robot.sot.push(robot.contactLF.task.name)
271 # robot.sot.push(robot.taskComH.task.name)
272 robot.sot.push(robot.taskCom.task.name)
273 robot.sot.push(robot.keepWaist.task.name)
274 # robot.sot.push(robot.taskWaist.name)
275 
276 # --- Fix robot.dynamic inputs
277 # robot.baseselec = Selec_of_vector('baseselec')
278 # robot.baseselec.selec(0, 6)
279 # plug(robot.base_estimator.q, robot.baseselec.sin)
280 # plug(robot.baseselec.sout, robot.dynamic.ffposition)
281 
282 plug(robot.device.velocity, robot.dynamic.velocity)
283 robot.dvdt = Derivator_of_Vector("dv_dt")
284 robot.dvdt.dt.value = dt
285 plug(robot.device.velocity, robot.dvdt.sin)
286 plug(robot.dvdt.sout, robot.dynamic.acceleration)
287 
288 # -------------------------- PLOTS --------------------------
289 
290 # --- ROS PUBLISHER
291 robot.publisher = create_rospublish(robot, "robot_publisher")
292 
293 create_topic(
294  robot.publisher, robot.wp, "comDes", robot=robot, data_type="vector"
295 ) # desired CoM
296 
297 create_topic(
298  robot.publisher, robot.cdc_estimator, "c", robot=robot, data_type="vector"
299 ) # estimated CoM
300 create_topic(
301  robot.publisher, robot.cdc_estimator, "dc", robot=robot, data_type="vector"
302 ) # estimated CoM velocity
303 
304 create_topic(
305  robot.publisher, robot.dynamic, "com", robot=robot, data_type="vector"
306 ) # resulting SOT CoM
307 
308 create_topic(
309  robot.publisher, robot.estimator, "dcm", robot=robot, data_type="vector"
310 ) # estimated DCM
311 
312 create_topic(
313  robot.publisher, robot.dynamic, "zmp", robot=robot, data_type="vector"
314 ) # SOT ZMP
315 create_topic(
316  robot.publisher, robot.zmp_estimator, "zmp", robot=robot, data_type="vector"
317 ) # estimated ZMP
318 
319 # create_topic(robot.publisher, robot.device, 'forceLLEG', robot = robot, data_type='vector') # measured left wrench
320 # create_topic(robot.publisher, robot.device, 'forceRLEG', robot = robot, data_type='vector') # measured right wrench
321 
322 # create_topic(robot.publisher, robot.device_filters.ft_LF_filter, 'x_filtered', robot = robot, data_type='vector') # filtered left wrench
323 # create_topic(robot.publisher, robot.device_filters.ft_RF_filter, 'x_filtered', robot = robot, data_type='vector') # filtered right wrench
324 
325 create_topic(
326  robot.publisher, robot.ftc, "left_foot_force_out", robot=robot, data_type="vector"
327 ) # calibrated left wrench
328 create_topic(
329  robot.publisher, robot.ftc, "right_foot_force_out", robot=robot, data_type="vector"
330 ) # calibrated right wrench
331 
332 create_topic(
333  robot.publisher, robot.zmp_estimator, "copRight", robot=robot, data_type="vector"
334 )
335 
336 create_topic(
337  robot.publisher, robot.zmp_estimator, "copLeft", robot=robot, data_type="vector"
338 )
339 
340 
345 
346 # addTrace(robot.tracer, robot.wp, 'comDes') # desired CoM
347 
348 # addTrace(robot.tracer, robot.cdc_estimator, 'c') # estimated CoM
349 # addTrace(robot.tracer, robot.cdc_estimator, 'dc') # estimated CoM velocity
350 
351 # addTrace(robot.tracer, robot.dynamic, 'com') # resulting SOT CoM
352 
353 # addTrace(robot.tracer, robot.estimator, 'dcm') # estimated DCM
354 
355 # addTrace(robot.tracer, robot.dynamic, 'zmp') # SOT ZMP
356 # addTrace(robot.tracer, robot.zmp_estimator, 'zmp') # estimated ZMP
357 
358 # addTrace(robot.tracer, robot.ftc, 'left_foot_force_out') # calibrated left wrench
359 # addTrace(robot.tracer, robot.ftc, 'right_foot_force_out') # calibrated right wrench
360 
361 # 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.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