sot-talos-balance  2.0.5
Collection of dynamic-graph entities aimed at implementing balance control on talos.
motor_parameters.py
Go to the documentation of this file.
1 # -*- coding: utf-8 -*-
2 """
3 Created on Mon Feb 9 13:55:16 2015
4 
5 @author: adelpret
6 """
7 from numpy import zeros as zeros
8 
9 NJ = 32
10 
11 GEAR_RATIOS = (
12  150.0,
13  101.0,
14  100.0,
15  144.0,
16  100.0,
17  101.0,
18  150.0,
19  101.0,
20  100.0,
21  144.0,
22  100.0,
23  101.0,
24  100.0,
25  100.0,
26  100.0,
27  100.0,
28  100.0,
29  100.0,
30  100.0,
31  100.0,
32  100.0,
33  100.0,
34  100.0,
35  100.0,
36  100.0,
37  100.0,
38  100.0,
39  100.0,
40  100.0,
41  100.0,
42  100.0,
43  200.0,
44 )
45 ROTOR_INERTIAS = (
46  8.6e-6,
47  34e-6,
48  34.0e-5,
49  61.0e-5,
50  34.0e-6,
51  21.0e-6,
52  8.6e-6,
53  34e-6,
54  34.0e-5,
55  61.0e-5,
56  34.0e-6,
57  21.0e-6,
58  34.0e-6,
59  34.0e-6,
60  34.0e-6,
61  34.0e-6,
62  21.0e-6,
63  21.0e-6,
64  5.8e-6,
65  1.3e-6,
66  1.3e-07,
67  1.3,
68  34.0e-6,
69  34.0e-6,
70  21.0e-6,
71  21.0e-6,
72  5.8e-6,
73  1.3e-6,
74  1.3e-07,
75  1.3,
76  1.3,
77  1.3,
78 )
79 
80 # New motor parameters with current measurment
81 # (used by ForceTorqueEstimator and JointTorqueController)
82 Kt_p = zeros(NJ) + 1.0
83 Kt_n = zeros(NJ) + 1.0
84 Kf_p = zeros(NJ)
85 Kf_n = zeros(NJ)
86 Kv_p = zeros(NJ)
87 Kv_n = zeros(NJ)
88 Ka_p = zeros(NJ)
89 Ka_n = zeros(NJ)
90 
91 deadzone = zeros(NJ)
92 K_bemf = zeros(NJ)
93 # used by ControlManager to compensate back-EMF
94 cur_sens_gains = zeros(NJ) + 1.0
95 
96 cur_sens_gains[0] = 1.256534 # Using 20161114_135332_rhy_static
97 deadzone[0] = 0.588671 # Using 20161114_135332_rhy_static
98 deadzone[0] = 0.575506 # Using 20161114_143152_rhy_vel
99 K_bemf[0] = 1.169309 # [Amp/Rad.s-1] Using 20161114_143152_rhy_vel
100 Kt_p[0] = 0.055302 # Using 20161114_135332_rhy_static
101 Kt_n[0] = 0.053055 # Using 20161114_135332_rhy_static
102 Kv_p[0] = 0.777466 # Using 20161114_143152_rhy_vel
103 Kv_n[0] = 0.779525 # Using 20161114_143152_rhy_vel
104 Kf_p[0] = 0.463513 # Using 20161114_143152_rhy_vel
105 Kf_n[0] = 0.231212 # Using 20161114_143152_rhy_vel
106 
107 cur_sens_gains[1] = 0.997354 # Using 20161114_144232_rhr_static
108 deadzone[1] = 0.571099 # Using 20161114_144232_rhr_static
109 deadzone[1] = 0.564055 # Using 20161114_150356_rhr_vel
110 K_bemf[1] = 1.120005 # [Amp/Rad.s-1] Using 20161114_150356_rhr_vel
111 Kt_p[1] = 0.061092 # Using 20161114_144232_rhr_static
112 Kt_n[1] = 0.057921 # Using 20161114_144232_rhr_static
113 Kv_p[1] = 0.583937 # Using 20161114_150356_rhr_vel
114 Kv_n[1] = 0.471164 # Using 20161114_150356_rhr_vel
115 Kf_p[1] = 0.139620 # Using 20161114_150356_rhr_vel
116 Kf_n[1] = 0.622044 # Using 20161114_150356_rhr_vel
117 
118 cur_sens_gains[2] = 0.884396 # Using 20161114_150722_rhp_static
119 deadzone[2] = 0.671750 # Using 20161114_150722_rhp_static
120 deadzone[2] = 0.461780 # Using 20161114_151812_rhp_vel
121 K_bemf[2] = 1.171485 # [Amp/Rad.s-1] Using 20161114_151812_rhp_vel
122 Kt_p[2] = 0.093924 # Using 20161114_150722_rhp_static
123 Kt_n[2] = 0.074335 # Using 20161114_150722_rhp_static
124 Kv_p[2] = 0.236554 # Using 20161114_151812_rhp_vel
125 Kv_n[2] = 0.200306 # Using 20161114_151812_rhp_vel
126 Kf_p[2] = 0.322370 # Using 20161114_151812_rhp_vel
127 Kf_n[2] = 0.955010 # Using 20161114_151812_rhp_vel
128 
129 cur_sens_gains[3] = 0.898618 # Using 20161114_152140_rk_static
130 deadzone[3] = 0.611894 # Using 20161114_152140_rk_static
131 deadzone[3] = 0.594111 # Using 20161114_153220_rk_vel
132 K_bemf[3] = 1.062028 # [Amp/Rad.s-1] Using 20161114_153220_rk_vel
133 Kt_p[3] = 0.074025 # Using 20161114_152140_rk_static
134 Kt_n[3] = 0.070763 # Using 20161114_152140_rk_static
135 Kv_p[3] = 0.310712 # Using 20161114_153220_rk_vel
136 Kv_n[3] = 0.302653 # Using 20161114_153220_rk_vel
137 Kf_p[3] = 0.562304 # Using 20161114_153220_rk_vel
138 Kf_n[3] = 0.590003 # Using 20161114_153220_rk_vel
139 
140 cur_sens_gains[4] = 0.989997 # Using 20161114_153739_rap_static
141 deadzone[4] = 0.485198 # Using 20161114_153739_rap_static
142 deadzone[4] = 0.615647 # Using 20161114_154559_rap_vel
143 K_bemf[4] = 0.787735 # [Amp/Rad.s-1] Using 20161114_154559_rap_vel
144 Kt_p[4] = 0.082806 # Using 20161114_153739_rap_static
145 Kt_n[4] = 0.088764 # Using 20161114_153739_rap_static
146 Kv_p[4] = 0.222277 # Using 20161114_154559_rap_vel
147 Kv_n[4] = 0.225662 # Using 20161114_154559_rap_vel
148 Kf_p[4] = 0.559252 # Using 20161114_154559_rap_vel
149 Kf_n[4] = 0.259040 # Using 20161114_154559_rap_vel
150 
151 cur_sens_gains[5] = 1.007775 # Using 20161114_154945_rar_static
152 deadzone[5] = 0.595250 # Using 20161114_154945_rar_static
153 deadzone[5] = 0.617705 # Using 20161114_160038_rar_vel
154 K_bemf[5] = 0.456973 # [Amp/Rad.s-1] Using 20161114_160038_rar_vel
155 Kt_p[5] = 0.155565 # Using 20161114_154945_rar_static
156 Kt_n[5] = 0.156830 # Using 20161114_154945_rar_static
157 Kv_p[5] = 0.511848 # Using 20161114_160038_rar_vel
158 Kv_n[5] = 0.517686 # Using 20161114_160038_rar_vel
159 Kf_p[5] = 0.564020 # Using 20161114_160038_rar_vel
160 Kf_n[5] = 0.321447 # Using 20161114_160038_rar_vel
161 
162 cur_sens_gains[6] = 1.250000 # Using 20171002_163413_lhy_static (forced by hand)
163 deadzone[6] = 0.295741 # Using 20171002_163413_lhy_static
164 deadzone[6] = 0.627764 # Using 20171002_151718_lhy_vel
165 K_bemf[6] = 1.168112 # [Amp/Rad.s-1] Using 20171002_151718_lhy_vel
166 Kt_p[6] = 0.057042 # Using 20171002_163413_lhy_static
167 Kt_n[6] = 0.054307 # Using 20171002_163413_lhy_static
168 Kv_p[6] = 0.833485 # Using 20171002_151718_lhy_vel
169 Kv_n[6] = 0.835893 # Using 20171002_151718_lhy_vel
170 Kf_p[6] = 0.444239 # Using 20171002_151718_lhy_vel
171 Kf_n[6] = 0.251280 # Using 20171002_151718_lhy_vel
172 
173 cur_sens_gains[7] = 1.004305 # Using 20171002_164436_lhr_static
174 deadzone[7] = 0.640682 # Using 20171002_164436_lhr_static
175 deadzone[7] = 0.601328 # Using 20171002_153334_lhr_vel
176 K_bemf[7] = 1.161687 # [Amp/Rad.s-1] Using 20171002_153334_lhr_vel
177 Kt_p[7] = 0.069472 # Using 20171002_164436_lhr_static
178 Kt_n[7] = 0.058696 # Using 20171002_164436_lhr_static
179 Kv_p[7] = 0.363950 # Using 20171002_153334_lhr_vel
180 Kv_n[7] = 0.421599 # Using 20171002_153334_lhr_vel
181 Kf_p[7] = 0.606632 # Using 20171002_153334_lhr_vel
182 Kf_n[7] = 0.095194 # Using 20171002_153334_lhr_vel
183 
184 cur_sens_gains[8] = 0.901796 # Using 20171002_165335_lhp_static
185 deadzone[8] = 0.456045 # Using 20171002_165335_lhp_static
186 deadzone[8] = 0.607772 # Using 20171002_154449_lhp_vel
187 K_bemf[8] = 0.975598 # [Amp/Rad.s-1] Using 20171002_154449_lhp_vel
188 Kt_p[8] = 0.076617 # Using 20171002_165335_lhp_static
189 Kt_n[8] = 0.087909 # Using 20171002_165335_lhp_static
190 Kv_p[8] = 0.122464 # Using 20171002_154449_lhp_vel
191 Kv_n[8] = 0.181315 # Using 20171002_154449_lhp_vel
192 Kf_p[8] = 0.597530 # Using 20171002_154449_lhp_vel
193 Kf_n[8] = 0.375285 # Using 20171002_154449_lhp_vel
194 
195 cur_sens_gains[9] = 0.897802 # Using 20170113_151748_lk_static
196 deadzone[9] = 0.628737 # Using 20170113_151748_lk_static
197 deadzone[9] = 0.621934 # Using 20170113_152924_lk_const_vel
198 K_bemf[9] = 1.057187 # [Amp/Rad.s-1] Using 20170113_152924_lk_const_vel
199 Kt_p[9] = 0.071932 # Using 20170113_151748_lk_static
200 Kt_n[9] = 0.065418 # Using 20170113_151748_lk_static
201 Kv_p[9] = 0.331607 # Using 20170113_152924_lk_const_vel
202 Kv_n[9] = 0.351136 # Using 20170113_152924_lk_const_vel
203 Kf_p[9] = 0.363123 # Using 20170113_152924_lk_const_vel
204 Kf_n[9] = 0.699079 # Using 20170113_152924_lk_const_vel
205 
206 cur_sens_gains[10] = 0.978598 # Using 20170113_154007_lap_static
207 deadzone[10] = 0.577901 # Using 20170113_154007_lap_static
208 deadzone[10] = 0.582928 # Using 20170113_154834_lap_const_vel
209 K_bemf[10] = 0.806259 # [Amp/Rad.s-1] Using 20170113_154834_lap_const_vel
210 Kt_p[10] = 0.092789 # Using 20170113_154007_lap_static
211 Kt_n[10] = 0.093060 # Using 20170113_154007_lap_static
212 Kv_p[10] = 0.542156 # Using 20170113_154834_lap_const_vel
213 Kv_n[10] = 0.469827 # Using 20170113_154834_lap_const_vel
214 Kf_p[10] = 0.063421 # Using 20170113_154834_lap_const_vel
215 Kf_n[10] = 0.806020 # Using 20170113_154834_lap_const_vel
216 
217 cur_sens_gains[11] = 0.995794 # Using 20170113_155150_lar_static
218 deadzone[11] = 0.643120 # Using 20170113_155150_lar_static
219 deadzone[11] = 0.645305 # Using 20170113_160057_lar_const_vel
220 K_bemf[11] = 0.474213 # [Amp/Rad.s-1] Using 20170113_160057_lar_const_vel
221 Kt_p[11] = 0.147175 # Using 20170113_155150_lar_static
222 Kt_n[11] = 0.150098 # Using 20170113_155150_lar_static
223 Kv_p[11] = 0.342627 # Using 20170113_160057_lar_const_vel
224 Kv_n[11] = 0.351680 # Using 20170113_160057_lar_const_vel
225 Kf_p[11] = 0.263935 # Using 20170113_160057_lar_const_vel
226 Kf_n[11] = 0.430722 # Using 20170113_160057_lar_const_vel
227 
228 # take averages for p and n
229 for i in range(NJ):
230  Kt_av = (Kt_n[i] + Kt_p[i]) / 2
231  Kt_n[i] = Kt_av
232  Kt_p[i] = Kt_av
233 
234  Kv_av = (Kv_n[i] + Kv_p[i]) / 2
235  Kv_n[i] = Kv_av
236  Kv_p[i] = Kv_av
237 
238  Kf_av = (Kf_n[i] + Kf_p[i]) / 2
239  Kf_n[i] = Kf_av
240  Kf_p[i] = Kf_av