sot-talos-balance  2.0.5
Collection of dynamic-graph entities aimed at implementing balance control on talos.
appli_base_estimator_integrator.py
Go to the documentation of this file.
1 # flake8: noqa
2 from math import sqrt
3 
4 import numpy as np
5 
6 import dynamic_graph.sot_talos_balance.talos.base_estimator_conf as base_estimator_conf
7 import dynamic_graph.sot_talos_balance.talos.control_manager_conf as cm_conf
8 import dynamic_graph.sot_talos_balance.talos.ft_calibration_conf as ft_conf
9 import dynamic_graph.sot_talos_balance.talos.parameter_server_conf as param_server_conf
10 from dynamic_graph import plug
11 from dynamic_graph.sot.core import (
12  SOT,
13  Derivator_of_Vector,
14  FeaturePosture,
15  MatrixHomoToPoseQuaternion,
16  Task,
17 )
18 from dynamic_graph.sot.core.matrix_util import matrixToTuple
19 from dynamic_graph.sot.core.meta_tasks_kine import (
20  MetaTaskKine6d,
21  MetaTaskKineCom,
22  gotoNd,
23 )
24 from dynamic_graph.sot.dynamics_pinocchio import DynamicPinocchio
25 from dynamic_graph.tracer_real_time import TracerRealTime
26 from dynamic_graph.sot_talos_balance.create_entities_utils import *
27 
28 robot.timeStep = robot.device.getTimeStep()
29 dt = robot.timeStep
30 robot.device.setControlInputType("noInteg")
31 
32 # --- Pendulum parameters
33 robot_name = "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(param_server_conf, dt)
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 folder = None
55 if test_folder is not None:
56  if sot_talos_balance_folder:
57  from rospkg import RosPack
58 
59  rospack = RosPack()
60 
61  folder = rospack.get_path("sot-talos-balance") + "/data/" + test_folder
62  else:
63  folder = test_folder
64  if folder[-1] != "/":
65  folder += "/"
66 
67 # --- Trajectory generators
68 
69 # --- General trigger
70 robot.triggerTrajGen = BooleanIdentity("triggerTrajGen")
71 robot.triggerTrajGen.sin.value = 0
72 
73 # --- CoM
74 robot.comTrajGen = create_com_trajectory_generator(dt, robot)
75 robot.comTrajGen.x.recompute(0) # trigger computation of initial value
76 plug(robot.triggerTrajGen.sout, robot.comTrajGen.trigger)
77 
78 # --- Left foot
79 robot.lfTrajGen = create_pose_rpy_trajectory_generator(dt, robot, "LF")
80 robot.lfTrajGen.x.recompute(0) # trigger computation of initial value
81 
82 robot.lfToMatrix = PoseRollPitchYawToMatrixHomo("lf2m")
83 plug(robot.lfTrajGen.x, robot.lfToMatrix.sin)
84 plug(robot.triggerTrajGen.sout, robot.lfTrajGen.trigger)
85 
86 # --- Right foot
87 robot.rfTrajGen = create_pose_rpy_trajectory_generator(dt, robot, "RF")
88 robot.rfTrajGen.x.recompute(0) # trigger computation of initial value
89 
90 robot.rfToMatrix = PoseRollPitchYawToMatrixHomo("rf2m")
91 plug(robot.rfTrajGen.x, robot.rfToMatrix.sin)
92 plug(robot.triggerTrajGen.sout, robot.rfTrajGen.trigger)
93 
94 # --- ZMP
95 robot.zmpTrajGen = create_zmp_trajectory_generator(dt, robot)
96 robot.zmpTrajGen.x.recompute(0) # trigger computation of initial value
97 plug(robot.triggerTrajGen.sout, robot.zmpTrajGen.trigger)
98 
99 # --- Waist
100 robot.waistTrajGen = create_orientation_rpy_trajectory_generator(dt, robot, "WT")
101 robot.waistTrajGen.x.recompute(0) # trigger computation of initial value
102 
103 robot.waistMix = Mix_of_vector("waistMix")
104 robot.waistMix.setSignalNumber(3)
105 robot.waistMix.addSelec(1, 0, 3)
106 robot.waistMix.addSelec(2, 3, 3)
107 robot.waistMix.default.value = [0.0] * 6
108 robot.waistMix.signal("sin1").value = [0.0] * 3
109 plug(robot.waistTrajGen.x, robot.waistMix.signal("sin2"))
110 
111 robot.waistToMatrix = PoseRollPitchYawToMatrixHomo("w2m")
112 plug(robot.waistMix.sout, robot.waistToMatrix.sin)
113 plug(robot.triggerTrajGen.sout, robot.waistTrajGen.trigger)
114 
115 # --- Load files
116 if folder is not None:
117  robot.comTrajGen.playTrajectoryFile(folder + "CoM.dat")
118  robot.lfTrajGen.playTrajectoryFile(folder + "LeftFoot.dat")
119  robot.rfTrajGen.playTrajectoryFile(folder + "RightFoot.dat")
120  # robot.zmpTrajGen.playTrajectoryFile(folder + 'ZMP.dat')
121  robot.waistTrajGen.playTrajectoryFile(folder + "WaistOrientation.dat")
122 
123 # --- Interface with controller entities
124 
125 wp = DummyWalkingPatternGenerator("dummy_wp")
126 wp.init()
127 wp.omega.value = omega
128 plug(robot.waistToMatrix.sout, wp.waist)
129 plug(robot.lfToMatrix.sout, wp.footLeft)
130 plug(robot.rfToMatrix.sout, wp.footRight)
131 plug(robot.comTrajGen.x, wp.com)
132 plug(robot.comTrajGen.dx, wp.vcom)
133 plug(robot.comTrajGen.ddx, wp.acom)
134 # if folder is not None:
135 # plug(robot.zmpTrajGen.x, wp.zmp)
136 
137 robot.wp = wp
138 
139 # --- Compute the values to use them in initialization
140 robot.wp.comDes.recompute(0)
141 robot.wp.dcmDes.recompute(0)
142 robot.wp.zmpDes.recompute(0)
143 
144 # -------------------------- ESTIMATION --------------------------
145 
146 # --- Base Estimation
147 robot.device_filters = create_device_filters(robot, dt)
148 robot.imu_filters = create_imu_filters(robot, dt)
149 robot.base_estimator = create_base_estimator(robot, dt, base_estimator_conf)
150 
151 robot.m2qLF = MatrixHomoToPoseQuaternion("m2qLF")
152 plug(robot.dynamic.LF, robot.m2qLF.sin)
153 plug(robot.m2qLF.sout, robot.base_estimator.lf_ref_xyzquat)
154 robot.m2qRF = MatrixHomoToPoseQuaternion("m2qRF")
155 plug(robot.dynamic.RF, robot.m2qRF.sin)
156 plug(robot.m2qRF.sout, robot.base_estimator.rf_ref_xyzquat)
157 
158 # robot.be_filters = create_be_filters(robot, dt)
159 
160 # -------------------------- SOT CONTROL --------------------------
161 
162 # --- Upper body
163 robot.taskUpperBody = Task("task_upper_body")
164 robot.taskUpperBody.feature = FeaturePosture("feature_upper_body")
165 
166 q = list(robot.dynamic.position.value)
167 robot.taskUpperBody.feature.state.value = q
168 robot.taskUpperBody.feature.posture.value = q
169 
170 robotDim = robot.dynamic.getDimension() # 38
171 for i in range(18, robotDim):
172  robot.taskUpperBody.feature.selectDof(i, True)
173 
174 robot.taskUpperBody.controlGain.value = 100.0
175 robot.taskUpperBody.add(robot.taskUpperBody.feature.name)
176 plug(robot.dynamic.position, robot.taskUpperBody.feature.state)
177 
178 # --- CONTACTS
179 # define contactLF and contactRF
180 robot.contactLF = MetaTaskKine6d(
181  "contactLF", robot.dynamic, "LF", robot.OperationalPointsMap["left-ankle"]
182 )
183 robot.contactLF.feature.frame("desired")
184 robot.contactLF.gain.setConstant(300)
185 plug(robot.wp.footLeftDes, robot.contactLF.featureDes.position) # .errorIN?
186 locals()["contactLF"] = robot.contactLF
187 
188 robot.contactRF = MetaTaskKine6d(
189  "contactRF", robot.dynamic, "RF", robot.OperationalPointsMap["right-ankle"]
190 )
191 robot.contactRF.feature.frame("desired")
192 robot.contactRF.gain.setConstant(300)
193 plug(robot.wp.footRightDes, robot.contactRF.featureDes.position) # .errorIN?
194 locals()["contactRF"] = robot.contactRF
195 
196 # --- COM
197 robot.taskCom = MetaTaskKineCom(robot.dynamic)
198 plug(robot.wp.comDes, robot.taskCom.featureDes.errorIN)
199 robot.taskCom.task.controlGain.value = 100.0
200 
201 # --- Waist
202 robot.keepWaist = MetaTaskKine6d(
203  "keepWaist", robot.dynamic, "WT", robot.OperationalPointsMap["waist"]
204 )
205 robot.keepWaist.feature.frame("desired")
206 robot.keepWaist.gain.setConstant(300)
207 plug(robot.wp.waistDes, robot.keepWaist.featureDes.position)
208 robot.keepWaist.feature.selec.value = "111000"
209 locals()["keepWaist"] = robot.keepWaist
210 
211 # --- SOT solver
212 robot.sot = SOT("sot")
213 robot.sot.setSize(robot.dynamic.getDimension())
214 
215 # --- State integrator
216 robot.integrate = SimpleStateIntegrator("integrate")
217 robot.integrate.init(dt)
218 robot.integrate.setState(robot.device.state.value)
219 robot.integrate.setVelocity(robot.dynamic.getDimension() * [0.0])
220 
221 # --- Plug SOT control to device through state integrator
222 plug(robot.sot.control, robot.integrate.control)
223 plug(robot.integrate.state, robot.device.control)
224 
225 robot.sot.push(robot.taskUpperBody.name)
226 robot.sot.push(robot.contactRF.task.name)
227 robot.sot.push(robot.contactLF.task.name)
228 robot.sot.push(robot.taskCom.task.name)
229 robot.sot.push(robot.keepWaist.task.name)
230 
231 # --- Delay
232 robot.delay_vel = DelayVector("delay_vel")
233 robot.delay_vel.setMemory(robot.dynamic.getDimension() * [0.0])
234 robot.device.before.addSignal(robot.delay_vel.name + ".current")
235 plug(robot.sot.control, robot.delay_vel.sin)
236 
237 # --- Plug integrator instead of device
238 plug(robot.delay_vel.previous, robot.vselec.sin)
239 
240 # --- Fix robot.dynamic inputs
241 plug(robot.delay_vel.previous, robot.dynamic.velocity)
242 robot.dvdt = Derivator_of_Vector("dv_dt")
243 robot.dvdt.dt.value = dt
244 plug(robot.delay_vel.previous, robot.dvdt.sin)
245 plug(robot.dvdt.sout, robot.dynamic.acceleration)
246 
247 # -------------------------- PLOTS --------------------------
248 
249 # --- ROS PUBLISHER
250 robot.publisher = create_rospublish(robot, "robot_publisher")
251 
252 create_topic(robot.publisher, robot.device, "state", robot=robot, data_type="vector")
253 create_topic(
254  robot.publisher, robot.base_estimator, "q", robot=robot, data_type="vector"
255 )
256 create_topic(
257  robot.publisher, robot.imu_filters, "imu_quat", robot=robot, data_type="vector"
258 )
259 
260 create_topic(
261  robot.publisher, robot.base_estimator, "w_lf", robot=robot, data_type="double"
262 )
263 create_topic(
264  robot.publisher, robot.base_estimator, "w_rf", robot=robot, data_type="double"
265 )
266 create_topic(
267  robot.publisher,
268  robot.base_estimator,
269  "w_lf_filtered",
270  robot=robot,
271  data_type="double",
272 )
273 create_topic(
274  robot.publisher,
275  robot.base_estimator,
276  "w_rf_filtered",
277  robot=robot,
278  data_type="double",
279 )
280 
281 create_topic(
282  robot.publisher, robot.device, "forceLLEG", robot=robot, data_type="vector"
283 )
284 create_topic(
285  robot.publisher,
286  robot.device_filters.ft_LF_filter,
287  "x_filtered",
288  robot=robot,
289  data_type="vector",
290 )
291 
292 create_topic(
293  robot.publisher, robot.device, "forceRLEG", robot=robot, data_type="vector"
294 )
295 create_topic(
296  robot.publisher,
297  robot.device_filters.ft_RF_filter,
298  "x_filtered",
299  robot=robot,
300  data_type="vector",
301 )
302 
303 create_topic(
304  robot.publisher, robot.base_estimator, "lf_xyzquat", robot=robot, data_type="vector"
305 )
306 create_topic(
307  robot.publisher, robot.base_estimator, "rf_xyzquat", robot=robot, data_type="vector"
308 )
309 
310 # create_topic(robot.publisher, robot.base_estimator, 'lf_ref_xyzquat', robot = robot, data_type='vector')
311 # create_topic(robot.publisher, robot.base_estimator, 'rf_ref_xyzquat', robot = robot, data_type='vector')
create_entities_utils
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_zmp_trajectory_generator
def create_zmp_trajectory_generator(dt, robot)
Definition: create_entities_utils.py:128
dynamicgraph::sot::talos_balance::DelayVector
Delay< Vector, int > DelayVector
Definition: delay.hh:101
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_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