blob: f4319833a9ba15ce942ea01094cea8f622960019 [file] [log] [blame]
Austin Schuhce7e03d2020-11-20 22:32:44 -08001import frc971.control_loops.python.controls as controls
Austin Schuh3c542312013-02-24 01:53:50 -08002import numpy
Austin Schuh572ff402015-11-08 12:17:50 -08003import os
Austin Schuh3c542312013-02-24 01:53:50 -08004
Austin Schuhbcce26a2018-03-26 23:41:24 -07005
Tyler Chatow6738c362019-02-16 14:12:30 -08006class Constant(object):
Austin Schuhe8ca06a2020-03-07 22:27:39 -08007 def __init__(self, name, formatt, value, comment=None):
Tyler Chatow6738c362019-02-16 14:12:30 -08008 self.name = name
9 self.formatt = formatt
10 self.value = value
11 self.formatToType = {}
12 self.formatToType['%f'] = "double"
13 self.formatToType['%d'] = "int"
Austin Schuhe8ca06a2020-03-07 22:27:39 -080014 if comment is None:
15 self.comment = ""
16 else:
17 self.comment = comment + "\n"
Tyler Chatow6738c362019-02-16 14:12:30 -080018
19 def Render(self, loop_type):
20 typestring = self.formatToType[self.formatt]
21 if loop_type == 'float' and typestring == 'double':
22 typestring = loop_type
Austin Schuhe8ca06a2020-03-07 22:27:39 -080023 return str("\n%sstatic constexpr %s %s = "+ self.formatt +";\n") % \
24 (self.comment, typestring, self.name, self.value)
Ben Fredrickson1b45f782014-02-23 07:44:36 +000025
26
Austin Schuhe3490622013-03-13 01:24:30 -070027class ControlLoopWriter(object):
Tyler Chatow6738c362019-02-16 14:12:30 -080028 def __init__(self,
29 gain_schedule_name,
30 loops,
31 namespaces=None,
32 write_constants=False,
33 plant_type='StateFeedbackPlant',
34 observer_type='StateFeedbackObserver',
35 scalar_type='double'):
36 """Constructs a control loop writer.
Austin Schuhe3490622013-03-13 01:24:30 -070037
Tyler Chatow6738c362019-02-16 14:12:30 -080038 Args:
39 gain_schedule_name: string, Name of the overall controller.
40 loops: array[ControlLoop], a list of control loops to gain schedule
41 in order.
42 namespaces: array[string], a list of names of namespaces to nest in
43 order. If None, the default will be used.
44 plant_type: string, The C++ type of the plant.
45 observer_type: string, The C++ type of the observer.
46 scalar_type: string, The C++ type of the base scalar.
47 """
48 self._gain_schedule_name = gain_schedule_name
49 self._loops = loops
50 if namespaces:
51 self._namespaces = namespaces
52 else:
53 self._namespaces = ['frc971', 'control_loops']
Austin Schuhe3490622013-03-13 01:24:30 -070054
Tyler Chatow6738c362019-02-16 14:12:30 -080055 self._namespace_start = '\n'.join(
56 ['namespace %s {' % name for name in self._namespaces])
Austin Schuh86093ad2016-02-06 14:29:34 -080057
Tyler Chatow6738c362019-02-16 14:12:30 -080058 self._namespace_end = '\n'.join([
59 '} // namespace %s' % name for name in reversed(self._namespaces)
60 ])
Austin Schuh25933852014-02-23 02:04:13 -080061
Tyler Chatow6738c362019-02-16 14:12:30 -080062 self._constant_list = []
63 self._plant_type = plant_type
64 self._observer_type = observer_type
65 self._scalar_type = scalar_type
Austin Schuh25933852014-02-23 02:04:13 -080066
Tyler Chatow6738c362019-02-16 14:12:30 -080067 def AddConstant(self, constant):
68 """Adds a constant to write.
Austin Schuhe3490622013-03-13 01:24:30 -070069
Tyler Chatow6738c362019-02-16 14:12:30 -080070 Args:
71 constant: Constant, the constant to add to the header.
72 """
73 self._constant_list.append(constant)
Brian Silvermane51ad632014-01-08 15:12:29 -080074
Tyler Chatow6738c362019-02-16 14:12:30 -080075 def _TopDirectory(self):
76 return self._namespaces[0]
Austin Schuhe3490622013-03-13 01:24:30 -070077
Tyler Chatow6738c362019-02-16 14:12:30 -080078 def _HeaderGuard(self, header_file):
79 return ('_'.join([namespace.upper() for namespace in self._namespaces])
80 + '_' + os.path.basename(header_file).upper().replace(
81 '.', '_').replace('/', '_') + '_')
Austin Schuhe3490622013-03-13 01:24:30 -070082
Tyler Chatow6738c362019-02-16 14:12:30 -080083 def Write(self, header_file, cc_file):
84 """Writes the loops to the specified files."""
85 self.WriteHeader(header_file)
86 self.WriteCC(os.path.basename(header_file), cc_file)
Austin Schuhe3490622013-03-13 01:24:30 -070087
Tyler Chatow6738c362019-02-16 14:12:30 -080088 def _GenericType(self, typename, extra_args=None):
89 """Returns a loop template using typename for the type."""
90 num_states = self._loops[0].A.shape[0]
91 num_inputs = self._loops[0].B.shape[1]
92 num_outputs = self._loops[0].C.shape[0]
93 if extra_args is not None:
94 extra_args = ', ' + extra_args
95 else:
96 extra_args = ''
97 if self._scalar_type != 'double':
98 extra_args += ', ' + self._scalar_type
99 return '%s<%d, %d, %d%s>' % (typename, num_states, num_inputs,
100 num_outputs, extra_args)
Austin Schuh32501832017-02-25 18:32:56 -0800101
Tyler Chatow6738c362019-02-16 14:12:30 -0800102 def _ControllerType(self):
103 """Returns a template name for StateFeedbackController."""
104 return self._GenericType('StateFeedbackController')
Austin Schuhe3490622013-03-13 01:24:30 -0700105
Tyler Chatow6738c362019-02-16 14:12:30 -0800106 def _ObserverType(self):
107 """Returns a template name for StateFeedbackObserver."""
108 return self._GenericType(self._observer_type)
Austin Schuh20388b62017-11-23 22:40:46 -0800109
Tyler Chatow6738c362019-02-16 14:12:30 -0800110 def _LoopType(self):
111 """Returns a template name for StateFeedbackLoop."""
112 num_states = self._loops[0].A.shape[0]
113 num_inputs = self._loops[0].B.shape[1]
114 num_outputs = self._loops[0].C.shape[0]
Austin Schuh20388b62017-11-23 22:40:46 -0800115
Tyler Chatow6738c362019-02-16 14:12:30 -0800116 return 'StateFeedbackLoop<%d, %d, %d, %s, %s, %s>' % (
117 num_states, num_inputs, num_outputs, self._scalar_type,
118 self._PlantType(), self._ObserverType())
Austin Schuhe3490622013-03-13 01:24:30 -0700119
Tyler Chatow6738c362019-02-16 14:12:30 -0800120 def _PlantType(self):
121 """Returns a template name for StateFeedbackPlant."""
122 return self._GenericType(self._plant_type)
Austin Schuhe3490622013-03-13 01:24:30 -0700123
Tyler Chatow6738c362019-02-16 14:12:30 -0800124 def _PlantCoeffType(self):
125 """Returns a template name for StateFeedbackPlantCoefficients."""
126 return self._GenericType(self._plant_type + 'Coefficients')
Austin Schuhe3490622013-03-13 01:24:30 -0700127
Tyler Chatow6738c362019-02-16 14:12:30 -0800128 def _ControllerCoeffType(self):
129 """Returns a template name for StateFeedbackControllerCoefficients."""
130 return self._GenericType('StateFeedbackControllerCoefficients')
Austin Schuh32501832017-02-25 18:32:56 -0800131
Tyler Chatow6738c362019-02-16 14:12:30 -0800132 def _ObserverCoeffType(self):
133 """Returns a template name for StateFeedbackObserverCoefficients."""
134 return self._GenericType(self._observer_type + 'Coefficients')
Austin Schuh32501832017-02-25 18:32:56 -0800135
Tyler Chatow6738c362019-02-16 14:12:30 -0800136 def WriteHeader(self, header_file):
137 """Writes the header file to the file named header_file."""
138 with open(header_file, 'w') as fd:
139 header_guard = self._HeaderGuard(header_file)
140 fd.write('#ifndef %s\n'
141 '#define %s\n\n' % (header_guard, header_guard))
142 fd.write(
143 '#include \"frc971/control_loops/state_feedback_loop.h\"\n')
Ravago Jones26f7ad02021-02-05 15:45:59 -0800144 if (self._plant_type == 'StateFeedbackHybridPlant'
145 or self._observer_type == 'HybridKalman'):
Tyler Chatow6738c362019-02-16 14:12:30 -0800146 fd.write(
147 '#include \"frc971/control_loops/hybrid_state_feedback_loop.h\"\n'
148 )
Austin Schuh4cc4fe22017-11-23 19:13:09 -0800149
Tyler Chatow6738c362019-02-16 14:12:30 -0800150 fd.write('\n')
Austin Schuhe3490622013-03-13 01:24:30 -0700151
Tyler Chatow6738c362019-02-16 14:12:30 -0800152 fd.write(self._namespace_start)
Ben Fredrickson1b45f782014-02-23 07:44:36 +0000153
Tyler Chatow6738c362019-02-16 14:12:30 -0800154 for const in self._constant_list:
155 fd.write(const.Render(self._scalar_type))
Ben Fredrickson1b45f782014-02-23 07:44:36 +0000156
Tyler Chatow6738c362019-02-16 14:12:30 -0800157 fd.write('\n\n')
158 for loop in self._loops:
159 fd.write(loop.DumpPlantHeader(self._PlantCoeffType()))
160 fd.write('\n')
161 fd.write(loop.DumpControllerHeader(self._scalar_type))
162 fd.write('\n')
163 fd.write(loop.DumpObserverHeader(self._ObserverCoeffType()))
164 fd.write('\n')
Austin Schuhe3490622013-03-13 01:24:30 -0700165
Tyler Chatow6738c362019-02-16 14:12:30 -0800166 fd.write('%s Make%sPlant();\n\n' % (self._PlantType(),
167 self._gain_schedule_name))
Austin Schuhe3490622013-03-13 01:24:30 -0700168
Tyler Chatow6738c362019-02-16 14:12:30 -0800169 fd.write('%s Make%sController();\n\n' % (self._ControllerType(),
170 self._gain_schedule_name))
Austin Schuh32501832017-02-25 18:32:56 -0800171
Tyler Chatow6738c362019-02-16 14:12:30 -0800172 fd.write('%s Make%sObserver();\n\n' % (self._ObserverType(),
173 self._gain_schedule_name))
Austin Schuh32501832017-02-25 18:32:56 -0800174
Tyler Chatow6738c362019-02-16 14:12:30 -0800175 fd.write('%s Make%sLoop();\n\n' % (self._LoopType(),
176 self._gain_schedule_name))
Austin Schuhe3490622013-03-13 01:24:30 -0700177
Tyler Chatow6738c362019-02-16 14:12:30 -0800178 fd.write(self._namespace_end)
179 fd.write('\n\n')
180 fd.write("#endif // %s\n" % header_guard)
Austin Schuhe3490622013-03-13 01:24:30 -0700181
Tyler Chatow6738c362019-02-16 14:12:30 -0800182 def WriteCC(self, header_file_name, cc_file):
183 """Writes the cc file to the file named cc_file."""
184 with open(cc_file, 'w') as fd:
185 fd.write('#include \"%s/%s\"\n' % (os.path.join(*self._namespaces),
186 header_file_name))
187 fd.write('\n')
James Kuszmaul03be1242020-02-21 14:52:04 -0800188 fd.write('#include <chrono>\n')
Tyler Chatow6738c362019-02-16 14:12:30 -0800189 fd.write('#include <vector>\n')
190 fd.write('\n')
191 fd.write(
192 '#include \"frc971/control_loops/state_feedback_loop.h\"\n')
193 fd.write('\n')
194 fd.write(self._namespace_start)
195 fd.write('\n\n')
196 for loop in self._loops:
197 fd.write(
198 loop.DumpPlant(self._PlantCoeffType(), self._scalar_type))
199 fd.write('\n')
Austin Schuhe3490622013-03-13 01:24:30 -0700200
Tyler Chatow6738c362019-02-16 14:12:30 -0800201 for loop in self._loops:
202 fd.write(loop.DumpController(self._scalar_type))
203 fd.write('\n')
Austin Schuhe3490622013-03-13 01:24:30 -0700204
Tyler Chatow6738c362019-02-16 14:12:30 -0800205 for loop in self._loops:
206 fd.write(
207 loop.DumpObserver(self._ObserverCoeffType(),
208 self._scalar_type))
209 fd.write('\n')
Austin Schuh32501832017-02-25 18:32:56 -0800210
Tyler Chatow6738c362019-02-16 14:12:30 -0800211 fd.write('%s Make%sPlant() {\n' % (self._PlantType(),
212 self._gain_schedule_name))
213 fd.write(' ::std::vector< ::std::unique_ptr<%s>> plants(%d);\n' %
214 (self._PlantCoeffType(), len(self._loops)))
215 for index, loop in enumerate(self._loops):
Ravago Jones26f7ad02021-02-05 15:45:59 -0800216 fd.write(' plants[%d] = ::std::unique_ptr<%s>(new %s(%s));\n'
217 % (index, self._PlantCoeffType(),
218 self._PlantCoeffType(), loop.PlantFunction()))
Austin Schuhb02bf5b2021-07-31 21:28:21 -0700219 fd.write(' return %s(std::move(plants));\n' % self._PlantType())
Tyler Chatow6738c362019-02-16 14:12:30 -0800220 fd.write('}\n\n')
Austin Schuhe3490622013-03-13 01:24:30 -0700221
Tyler Chatow6738c362019-02-16 14:12:30 -0800222 fd.write('%s Make%sController() {\n' % (self._ControllerType(),
223 self._gain_schedule_name))
224 fd.write(
225 ' ::std::vector< ::std::unique_ptr<%s>> controllers(%d);\n' %
226 (self._ControllerCoeffType(), len(self._loops)))
227 for index, loop in enumerate(self._loops):
228 fd.write(
Ravago Jones26f7ad02021-02-05 15:45:59 -0800229 ' controllers[%d] = ::std::unique_ptr<%s>(new %s(%s));\n'
230 % (index, self._ControllerCoeffType(),
231 self._ControllerCoeffType(), loop.ControllerFunction()))
Austin Schuhb02bf5b2021-07-31 21:28:21 -0700232 fd.write(' return %s(std::move(controllers));\n' %
233 self._ControllerType())
Tyler Chatow6738c362019-02-16 14:12:30 -0800234 fd.write('}\n\n')
Austin Schuh32501832017-02-25 18:32:56 -0800235
Tyler Chatow6738c362019-02-16 14:12:30 -0800236 fd.write('%s Make%sObserver() {\n' % (self._ObserverType(),
237 self._gain_schedule_name))
238 fd.write(' ::std::vector< ::std::unique_ptr<%s>> observers(%d);\n'
239 % (self._ObserverCoeffType(), len(self._loops)))
240 for index, loop in enumerate(self._loops):
241 fd.write(
242 ' observers[%d] = ::std::unique_ptr<%s>(new %s(%s));\n'
243 % (index, self._ObserverCoeffType(),
244 self._ObserverCoeffType(), loop.ObserverFunction()))
Austin Schuhb02bf5b2021-07-31 21:28:21 -0700245 fd.write(
246 ' return %s(std::move(observers));\n' % self._ObserverType())
Tyler Chatow6738c362019-02-16 14:12:30 -0800247 fd.write('}\n\n')
Austin Schuh32501832017-02-25 18:32:56 -0800248
Tyler Chatow6738c362019-02-16 14:12:30 -0800249 fd.write('%s Make%sLoop() {\n' % (self._LoopType(),
250 self._gain_schedule_name))
251 fd.write(
252 ' return %s(Make%sPlant(), Make%sController(), Make%sObserver());\n'
253 % (self._LoopType(), self._gain_schedule_name,
254 self._gain_schedule_name, self._gain_schedule_name))
255 fd.write('}\n\n')
Austin Schuhe3490622013-03-13 01:24:30 -0700256
Tyler Chatow6738c362019-02-16 14:12:30 -0800257 fd.write(self._namespace_end)
258 fd.write('\n')
Austin Schuhe3490622013-03-13 01:24:30 -0700259
260
Austin Schuh3c542312013-02-24 01:53:50 -0800261class ControlLoop(object):
Tyler Chatow6738c362019-02-16 14:12:30 -0800262 def __init__(self, name):
263 """Constructs a control loop object.
Austin Schuh3c542312013-02-24 01:53:50 -0800264
Tyler Chatow6738c362019-02-16 14:12:30 -0800265 Args:
266 name: string, The name of the loop to use when writing the C++ files.
267 """
268 self._name = name
Austin Schuh64433f12022-02-21 19:40:38 -0800269 self.delayed_u = False
Austin Schuhb5d302f2019-01-20 20:51:19 -0800270
Tyler Chatow6738c362019-02-16 14:12:30 -0800271 @property
272 def name(self):
273 """Returns the name"""
274 return self._name
Austin Schuh3c542312013-02-24 01:53:50 -0800275
Tyler Chatow6738c362019-02-16 14:12:30 -0800276 def ContinuousToDiscrete(self, A_continuous, B_continuous, dt):
277 """Calculates the discrete time values for A and B.
Austin Schuhc1f68892013-03-16 17:06:27 -0700278
Tyler Chatow6738c362019-02-16 14:12:30 -0800279 Args:
280 A_continuous: numpy.matrix, The continuous time A matrix
281 B_continuous: numpy.matrix, The continuous time B matrix
282 dt: float, The time step of the control loop
Austin Schuhc1f68892013-03-16 17:06:27 -0700283
Tyler Chatow6738c362019-02-16 14:12:30 -0800284 Returns:
285 (A, B), numpy.matrix, the control matricies.
286 """
287 return controls.c2d(A_continuous, B_continuous, dt)
Austin Schuh3c542312013-02-24 01:53:50 -0800288
Tyler Chatow6738c362019-02-16 14:12:30 -0800289 def InitializeState(self):
290 """Sets X, Y, and X_hat to zero defaults."""
Austin Schuh43b9ae92020-02-29 23:08:38 -0800291 self.X = numpy.matrix(numpy.zeros((self.A.shape[0], 1)))
Tyler Chatow6738c362019-02-16 14:12:30 -0800292 self.Y = self.C * self.X
Austin Schuh43b9ae92020-02-29 23:08:38 -0800293 self.X_hat = numpy.matrix(numpy.zeros((self.A.shape[0], 1)))
Austin Schuh64433f12022-02-21 19:40:38 -0800294 self.last_U = numpy.matrix(numpy.zeros((self.B.shape[1], 1)))
Austin Schuh3c542312013-02-24 01:53:50 -0800295
Tyler Chatow6738c362019-02-16 14:12:30 -0800296 def PlaceControllerPoles(self, poles):
297 """Places the controller poles.
Austin Schuh3c542312013-02-24 01:53:50 -0800298
Tyler Chatow6738c362019-02-16 14:12:30 -0800299 Args:
300 poles: array, An array of poles. Must be complex conjegates if they have
301 any imaginary portions.
302 """
303 self.K = controls.dplace(self.A, self.B, poles)
Austin Schuh3c542312013-02-24 01:53:50 -0800304
Tyler Chatow6738c362019-02-16 14:12:30 -0800305 def PlaceObserverPoles(self, poles):
306 """Places the observer poles.
Austin Schuh3c542312013-02-24 01:53:50 -0800307
Tyler Chatow6738c362019-02-16 14:12:30 -0800308 Args:
309 poles: array, An array of poles. Must be complex conjegates if they have
310 any imaginary portions.
311 """
312 self.L = controls.dplace(self.A.T, self.C.T, poles).T
Sabina Davis3922dfa2018-02-10 23:10:05 -0800313
Tyler Chatow6738c362019-02-16 14:12:30 -0800314 def Update(self, U):
315 """Simulates one time step with the provided U."""
316 #U = numpy.clip(U, self.U_min, self.U_max)
Austin Schuh64433f12022-02-21 19:40:38 -0800317 if self.delayed_u:
318 self.X = self.A * self.X + self.B * self.last_U
319 self.Y = self.C * self.X + self.D * self.last_U
320 self.last_U = U.copy()
321 else:
322 self.X = self.A * self.X + self.B * U
323 self.Y = self.C * self.X + self.D * U
Austin Schuh3c542312013-02-24 01:53:50 -0800324
Tyler Chatow6738c362019-02-16 14:12:30 -0800325 def PredictObserver(self, U):
326 """Runs the predict step of the observer update."""
Austin Schuh64433f12022-02-21 19:40:38 -0800327 if self.delayed_u:
328 self.X_hat = (self.A * self.X_hat + self.B * self.last_U)
329 self.last_U = U.copy()
330 else:
331 self.X_hat = (self.A * self.X_hat + self.B * U)
Austin Schuh1a387962015-01-31 16:36:20 -0800332
Tyler Chatow6738c362019-02-16 14:12:30 -0800333 def CorrectObserver(self, U):
334 """Runs the correct step of the observer update."""
335 if hasattr(self, 'KalmanGain'):
336 KalmanGain = self.KalmanGain
337 else:
338 KalmanGain = numpy.linalg.inv(self.A) * self.L
Austin Schuh64433f12022-02-21 19:40:38 -0800339 if self.delayed_u:
340 self.X_hat += KalmanGain * (self.Y - self.C * self.X_hat -
341 self.D * self.last_U)
Tyler Chatow6738c362019-02-16 14:12:30 -0800342 else:
Austin Schuh64433f12022-02-21 19:40:38 -0800343 self.X_hat += KalmanGain * (self.Y - self.C * self.X_hat -
344 self.D * U)
Austin Schuh3c542312013-02-24 01:53:50 -0800345
Tyler Chatow6738c362019-02-16 14:12:30 -0800346 def _DumpMatrix(self, matrix_name, matrix, scalar_type):
347 """Dumps the provided matrix into a variable called matrix_name.
Austin Schuh3c542312013-02-24 01:53:50 -0800348
Tyler Chatow6738c362019-02-16 14:12:30 -0800349 Args:
350 matrix_name: string, The variable name to save the matrix to.
351 matrix: The matrix to dump.
352 scalar_type: The C++ type to use for the scalar in the matrix.
Austin Schuh3c542312013-02-24 01:53:50 -0800353
Tyler Chatow6738c362019-02-16 14:12:30 -0800354 Returns:
355 string, The C++ commands required to populate a variable named matrix_name
356 with the contents of matrix.
357 """
358 ans = [
Ravago Jones26f7ad02021-02-05 15:45:59 -0800359 ' Eigen::Matrix<%s, %d, %d> %s;\n' %
360 (scalar_type, matrix.shape[0], matrix.shape[1], matrix_name)
Tyler Chatow6738c362019-02-16 14:12:30 -0800361 ]
Austin Schuh5ea48472021-02-02 20:46:41 -0800362 for x in range(matrix.shape[0]):
363 for y in range(matrix.shape[1]):
Tyler Chatow6738c362019-02-16 14:12:30 -0800364 write_type = repr(matrix[x, y])
365 if scalar_type == 'float':
Austin Schuh085eab92020-11-26 13:54:51 -0800366 if '.' not in write_type and 'e' not in write_type:
Tyler Chatow6738c362019-02-16 14:12:30 -0800367 write_type += '.0'
368 write_type += 'f'
369 ans.append(
370 ' %s(%d, %d) = %s;\n' % (matrix_name, x, y, write_type))
Austin Schuh3c542312013-02-24 01:53:50 -0800371
Tyler Chatow6738c362019-02-16 14:12:30 -0800372 return ''.join(ans)
Austin Schuh3c542312013-02-24 01:53:50 -0800373
Tyler Chatow6738c362019-02-16 14:12:30 -0800374 def DumpPlantHeader(self, plant_coefficient_type):
375 """Writes out a c++ header declaration which will create a Plant object.
Austin Schuh3c542312013-02-24 01:53:50 -0800376
Tyler Chatow6738c362019-02-16 14:12:30 -0800377 Returns:
378 string, The header declaration for the function.
379 """
380 return '%s Make%sPlantCoefficients();\n' % (plant_coefficient_type,
381 self._name)
Austin Schuh3c542312013-02-24 01:53:50 -0800382
Tyler Chatow6738c362019-02-16 14:12:30 -0800383 def DumpPlant(self, plant_coefficient_type, scalar_type):
384 """Writes out a c++ function which will create a PlantCoefficients object.
Austin Schuh3c542312013-02-24 01:53:50 -0800385
Tyler Chatow6738c362019-02-16 14:12:30 -0800386 Returns:
387 string, The function which will create the object.
388 """
389 ans = [
390 '%s Make%sPlantCoefficients() {\n' % (plant_coefficient_type,
391 self._name)
392 ]
Austin Schuh3c542312013-02-24 01:53:50 -0800393
Tyler Chatow6738c362019-02-16 14:12:30 -0800394 ans.append(self._DumpMatrix('C', self.C, scalar_type))
395 ans.append(self._DumpMatrix('D', self.D, scalar_type))
396 ans.append(self._DumpMatrix('U_max', self.U_max, scalar_type))
397 ans.append(self._DumpMatrix('U_min', self.U_min, scalar_type))
Austin Schuh3c542312013-02-24 01:53:50 -0800398
Austin Schuh64433f12022-02-21 19:40:38 -0800399 delayed_u_string = "true" if self.delayed_u else "false"
Tyler Chatow6738c362019-02-16 14:12:30 -0800400 if plant_coefficient_type.startswith('StateFeedbackPlant'):
401 ans.append(self._DumpMatrix('A', self.A, scalar_type))
402 ans.append(self._DumpMatrix('B', self.B, scalar_type))
403 ans.append(
James Kuszmaul03be1242020-02-21 14:52:04 -0800404 ' const std::chrono::nanoseconds dt(%d);\n' % (self.dt * 1e9))
405 ans.append(
Tyler Chatow6738c362019-02-16 14:12:30 -0800406 ' return %s'
Austin Schuh64433f12022-02-21 19:40:38 -0800407 '(A, B, C, D, U_max, U_min, dt, %s);\n' % (plant_coefficient_type, delayed_u_string))
Tyler Chatow6738c362019-02-16 14:12:30 -0800408 elif plant_coefficient_type.startswith('StateFeedbackHybridPlant'):
409 ans.append(
410 self._DumpMatrix('A_continuous', self.A_continuous,
411 scalar_type))
412 ans.append(
413 self._DumpMatrix('B_continuous', self.B_continuous,
414 scalar_type))
415 ans.append(' return %s'
Austin Schuh64433f12022-02-21 19:40:38 -0800416 '(A_continuous, B_continuous, C, D, U_max, U_min, %s);\n' %
417 (plant_coefficient_type, delayed_u_string))
Tyler Chatow6738c362019-02-16 14:12:30 -0800418 else:
419 glog.fatal('Unsupported plant type %s', plant_coefficient_type)
Austin Schuh3ad5ed82017-02-25 21:36:19 -0800420
Tyler Chatow6738c362019-02-16 14:12:30 -0800421 ans.append('}\n')
422 return ''.join(ans)
Austin Schuh3c542312013-02-24 01:53:50 -0800423
Tyler Chatow6738c362019-02-16 14:12:30 -0800424 def PlantFunction(self):
425 """Returns the name of the plant coefficient function."""
426 return 'Make%sPlantCoefficients()' % self._name
Austin Schuh3c542312013-02-24 01:53:50 -0800427
Tyler Chatow6738c362019-02-16 14:12:30 -0800428 def ControllerFunction(self):
429 """Returns the name of the controller function."""
430 return 'Make%sControllerCoefficients()' % self._name
Austin Schuh32501832017-02-25 18:32:56 -0800431
Tyler Chatow6738c362019-02-16 14:12:30 -0800432 def ObserverFunction(self):
433 """Returns the name of the controller function."""
434 return 'Make%sObserverCoefficients()' % self._name
Austin Schuhe3490622013-03-13 01:24:30 -0700435
Tyler Chatow6738c362019-02-16 14:12:30 -0800436 def DumpControllerHeader(self, scalar_type):
437 """Writes out a c++ header declaration which will create a Controller object.
Austin Schuh3c542312013-02-24 01:53:50 -0800438
Tyler Chatow6738c362019-02-16 14:12:30 -0800439 Returns:
440 string, The header declaration for the function.
441 """
442 num_states = self.A.shape[0]
443 num_inputs = self.B.shape[1]
444 num_outputs = self.C.shape[0]
445 return 'StateFeedbackControllerCoefficients<%d, %d, %d, %s> %s;\n' % (
446 num_states, num_inputs, num_outputs, scalar_type,
447 self.ControllerFunction())
Austin Schuh3c542312013-02-24 01:53:50 -0800448
Tyler Chatow6738c362019-02-16 14:12:30 -0800449 def DumpController(self, scalar_type):
450 """Returns a c++ function which will create a Controller object.
Austin Schuh3c542312013-02-24 01:53:50 -0800451
Tyler Chatow6738c362019-02-16 14:12:30 -0800452 Returns:
453 string, The function which will create the object.
454 """
455 num_states = self.A.shape[0]
456 num_inputs = self.B.shape[1]
457 num_outputs = self.C.shape[0]
458 ans = [
459 'StateFeedbackControllerCoefficients<%d, %d, %d, %s> %s {\n' %
460 (num_states, num_inputs, num_outputs, scalar_type,
461 self.ControllerFunction())
462 ]
Austin Schuh3c542312013-02-24 01:53:50 -0800463
Tyler Chatow6738c362019-02-16 14:12:30 -0800464 ans.append(self._DumpMatrix('K', self.K, scalar_type))
465 if not hasattr(self, 'Kff'):
466 self.Kff = numpy.matrix(numpy.zeros(self.K.shape))
Austin Schuh86093ad2016-02-06 14:29:34 -0800467
Tyler Chatow6738c362019-02-16 14:12:30 -0800468 ans.append(self._DumpMatrix('Kff', self.Kff, scalar_type))
Austin Schuh3c542312013-02-24 01:53:50 -0800469
Tyler Chatow6738c362019-02-16 14:12:30 -0800470 ans.append(
471 ' return StateFeedbackControllerCoefficients<%d, %d, %d, %s>'
472 '(K, Kff);\n' % (num_states, num_inputs, num_outputs, scalar_type))
473 ans.append('}\n')
474 return ''.join(ans)
Austin Schuh32501832017-02-25 18:32:56 -0800475
Tyler Chatow6738c362019-02-16 14:12:30 -0800476 def DumpObserverHeader(self, observer_coefficient_type):
477 """Writes out a c++ header declaration which will create a Observer object.
Austin Schuh32501832017-02-25 18:32:56 -0800478
Tyler Chatow6738c362019-02-16 14:12:30 -0800479 Returns:
480 string, The header declaration for the function.
481 """
Ravago Jones26f7ad02021-02-05 15:45:59 -0800482 return '%s %s;\n' % (observer_coefficient_type,
483 self.ObserverFunction())
Austin Schuh32501832017-02-25 18:32:56 -0800484
Tyler Chatow6738c362019-02-16 14:12:30 -0800485 def DumpObserver(self, observer_coefficient_type, scalar_type):
486 """Returns a c++ function which will create a Observer object.
Austin Schuh32501832017-02-25 18:32:56 -0800487
Tyler Chatow6738c362019-02-16 14:12:30 -0800488 Returns:
489 string, The function which will create the object.
490 """
491 ans = [
492 '%s %s {\n' % (observer_coefficient_type, self.ObserverFunction())
493 ]
Austin Schuh32501832017-02-25 18:32:56 -0800494
Austin Schuh64433f12022-02-21 19:40:38 -0800495 delayed_u_string = "true" if self.delayed_u else "false"
Tyler Chatow6738c362019-02-16 14:12:30 -0800496 if observer_coefficient_type.startswith('StateFeedbackObserver'):
497 if hasattr(self, 'KalmanGain'):
498 KalmanGain = self.KalmanGain
499 Q = self.Q
500 R = self.R
501 else:
502 KalmanGain = numpy.linalg.inv(self.A) * self.L
503 Q = numpy.zeros(self.A.shape)
504 R = numpy.zeros((self.C.shape[0], self.C.shape[0]))
505 ans.append(self._DumpMatrix('KalmanGain', KalmanGain, scalar_type))
506 ans.append(self._DumpMatrix('Q', Q, scalar_type))
507 ans.append(self._DumpMatrix('R', R, scalar_type))
Austin Schuh64433f12022-02-21 19:40:38 -0800508 ans.append(' return %s(KalmanGain, Q, R, %s);\n' %
509 (observer_coefficient_type, delayed_u_string))
Sabina Davis3922dfa2018-02-10 23:10:05 -0800510
Tyler Chatow6738c362019-02-16 14:12:30 -0800511 elif observer_coefficient_type.startswith('HybridKalman'):
512 ans.append(
513 self._DumpMatrix('Q_continuous', self.Q_continuous,
514 scalar_type))
515 ans.append(
516 self._DumpMatrix('R_continuous', self.R_continuous,
517 scalar_type))
518 ans.append(
519 self._DumpMatrix('P_steady_state', self.P_steady_state,
520 scalar_type))
521 ans.append(
Austin Schuh64433f12022-02-21 19:40:38 -0800522 ' return %s(Q_continuous, R_continuous, P_steady_state, %s);\n' %
523 (observer_coefficient_type, delayed_u_string))
Tyler Chatow6738c362019-02-16 14:12:30 -0800524 else:
525 glog.fatal('Unsupported observer type %s',
526 observer_coefficient_type)
Austin Schuh32501832017-02-25 18:32:56 -0800527
Tyler Chatow6738c362019-02-16 14:12:30 -0800528 ans.append('}\n')
529 return ''.join(ans)
530
Austin Schuh3ad5ed82017-02-25 21:36:19 -0800531
532class HybridControlLoop(ControlLoop):
Tyler Chatow6738c362019-02-16 14:12:30 -0800533 def __init__(self, name):
534 super(HybridControlLoop, self).__init__(name=name)
Austin Schuh3ad5ed82017-02-25 21:36:19 -0800535
Tyler Chatow6738c362019-02-16 14:12:30 -0800536 def Discretize(self, dt):
537 [self.A, self.B, self.Q, self.R] = \
538 controls.kalmd(self.A_continuous, self.B_continuous,
539 self.Q_continuous, self.R_continuous, dt)
Austin Schuh3ad5ed82017-02-25 21:36:19 -0800540
Tyler Chatow6738c362019-02-16 14:12:30 -0800541 def PredictHybridObserver(self, U, dt):
542 self.Discretize(dt)
Austin Schuh64433f12022-02-21 19:40:38 -0800543 if self.delayed_u:
544 self.X_hat = self.A * self.X_hat + self.B * self.last_U
545 self.last_U = U.copy()
546 else:
547 self.X_hat = self.A * self.X_hat + self.B * U
548
Tyler Chatow6738c362019-02-16 14:12:30 -0800549 self.P = (self.A * self.P * self.A.T + self.Q)
Austin Schuh3ad5ed82017-02-25 21:36:19 -0800550
Tyler Chatow6738c362019-02-16 14:12:30 -0800551 def CorrectHybridObserver(self, U):
552 Y_bar = self.Y - self.C * self.X_hat
553 C_t = self.C.T
554 S = self.C * self.P * C_t + self.R
555 self.KalmanGain = self.P * C_t * numpy.linalg.inv(S)
556 self.X_hat = self.X_hat + self.KalmanGain * Y_bar
557 self.P = (numpy.eye(len(self.A)) - self.KalmanGain * self.C) * self.P
558
559 def InitializeState(self):
560 super(HybridControlLoop, self).InitializeState()
561 if hasattr(self, 'Q_steady_state'):
562 self.P = self.Q_steady_state
563 else:
564 self.P = numpy.matrix(
565 numpy.zeros((self.A.shape[0], self.A.shape[0])))
Campbell Crowley33e0e3d2017-12-27 17:55:40 -0800566
567
568class CIM(object):
Tyler Chatow6738c362019-02-16 14:12:30 -0800569 def __init__(self):
570 # Stall Torque in N m
571 self.stall_torque = 2.42
572 # Stall Current in Amps
573 self.stall_current = 133.0
574 # Free Speed in rad/s
575 self.free_speed = 5500.0 / 60.0 * 2.0 * numpy.pi
576 # Free Current in Amps
577 self.free_current = 4.7
578 # Resistance of the motor
579 self.resistance = 12.0 / self.stall_current
580 # Motor velocity constant
581 self.Kv = (
582 self.free_speed / (12.0 - self.resistance * self.free_current))
583 # Torque constant
584 self.Kt = self.stall_torque / self.stall_current
Lee Mracek97fc8af2018-01-13 04:38:52 -0500585
586
587class MiniCIM(object):
Tyler Chatow6738c362019-02-16 14:12:30 -0800588 def __init__(self):
589 # Stall Torque in N m
590 self.stall_torque = 1.41
591 # Stall Current in Amps
592 self.stall_current = 89.0
593 # Free Speed in rad/s
594 self.free_speed = 5840.0 / 60.0 * 2.0 * numpy.pi
595 # Free Current in Amps
596 self.free_current = 3.0
597 # Resistance of the motor
598 self.resistance = 12.0 / self.stall_current
599 # Motor velocity constant
600 self.Kv = (
601 self.free_speed / (12.0 - self.resistance * self.free_current))
602 # Torque constant
603 self.Kt = self.stall_torque / self.stall_current
Austin Schuhf173eb82018-01-20 23:32:30 -0800604
milind-uf70e8e12021-10-02 12:36:00 -0700605 # Motor inertia in kg m^2
606 self.motor_inertia = 0.0001634
607
Austin Schuhf173eb82018-01-20 23:32:30 -0800608
Austin Schuhb5d302f2019-01-20 20:51:19 -0800609class NMotor(object):
610 def __init__(self, motor, n):
611 """Gangs together n motors."""
612 self.motor = motor
613 self.stall_torque = motor.stall_torque * n
614 self.stall_current = motor.stall_current * n
615 self.free_speed = motor.free_speed
616
617 self.free_current = motor.free_current * n
618 self.resistance = motor.resistance / n
619 self.Kv = motor.Kv
620 self.Kt = motor.Kt
Austin Schuh36bb8e32019-02-18 15:02:57 -0800621 self.motor_inertia = motor.motor_inertia * n
Austin Schuhb5d302f2019-01-20 20:51:19 -0800622
623
624class Vex775Pro(object):
625 def __init__(self):
626 # Stall Torque in N m
627 self.stall_torque = 0.71
628 # Stall Current in Amps
629 self.stall_current = 134.0
630 # Free Speed in rad/s
631 self.free_speed = 18730.0 / 60.0 * 2.0 * numpy.pi
632 # Free Current in Amps
633 self.free_current = 0.7
634 # Resistance of the motor
635 self.resistance = 12.0 / self.stall_current
636 # Motor velocity constant
Tyler Chatow6738c362019-02-16 14:12:30 -0800637 self.Kv = (
638 self.free_speed / (12.0 - self.resistance * self.free_current))
Austin Schuhb5d302f2019-01-20 20:51:19 -0800639 # Torque constant
640 self.Kt = self.stall_torque / self.stall_current
641 # Motor inertia in kg m^2
642 self.motor_inertia = 0.00001187
643
644
Austin Schuhf173eb82018-01-20 23:32:30 -0800645class BAG(object):
Tyler Chatow6738c362019-02-16 14:12:30 -0800646 # BAG motor specs available at http://motors.vex.com/vexpro-motors/bag-motor
647 def __init__(self):
648 # Stall Torque in (N m)
649 self.stall_torque = 0.43
650 # Stall Current in (Amps)
651 self.stall_current = 53.0
652 # Free Speed in (rad/s)
653 self.free_speed = 13180.0 / 60.0 * 2.0 * numpy.pi
654 # Free Current in (Amps)
655 self.free_current = 1.8
656 # Resistance of the motor (Ohms)
657 self.resistance = 12.0 / self.stall_current
658 # Motor velocity constant (radians / (sec * volt))
659 self.Kv = (
660 self.free_speed / (12.0 - self.resistance * self.free_current))
661 # Torque constant (N * m / A)
662 self.Kt = self.stall_torque / self.stall_current
663 # Motor inertia in kg m^2
664 self.motor_inertia = 0.000006
665
Brian Silverman6260c092018-01-14 15:21:36 -0800666
667class MN3510(object):
Tyler Chatow6738c362019-02-16 14:12:30 -0800668 def __init__(self):
669 # http://www.robotshop.com/en/t-motor-navigator-mn3510-360kv-brushless-motor.html#Specifications
670 # Free Current in Amps
671 self.free_current = 0.0
672 # Resistance of the motor
673 self.resistance = 0.188
674 # Stall Current in Amps
675 self.stall_current = 14.0 / self.resistance
676 # Motor velocity constant
677 self.Kv = 360.0 / 60.0 * (2.0 * numpy.pi)
678 # Torque constant Nm / A
679 self.Kt = 1.0 / self.Kv
680 # Stall Torque in N m
681 self.stall_torque = self.Kt * self.stall_current
James Kuszmaulef0c18a2020-01-12 15:44:20 -0800682
683
684class Falcon(object):
685 """Class representing the VexPro Falcon 500 motor.
686
687 All numbers based on data from
688 https://www.vexrobotics.com/vexpro/falcon-500."""
689
690 def __init__(self):
691 # Stall Torque in N m
692 self.stall_torque = 4.69
693 # Stall Current in Amps
694 self.stall_current = 257.0
695 # Free Speed in rad / sec
696 self.free_speed = 6380.0 / 60.0 * 2.0 * numpy.pi
697 # Free Current in Amps
698 self.free_current = 1.5
699 # Resistance of the motor, divided by 2 to account for the 2 motors
700 self.resistance = 12.0 / self.stall_current
701 # Motor velocity constant
Ravago Jones26f7ad02021-02-05 15:45:59 -0800702 self.Kv = (
703 self.free_speed / (12.0 - self.resistance * self.free_current))
James Kuszmaulef0c18a2020-01-12 15:44:20 -0800704 # Torque constant
705 self.Kt = self.stall_torque / self.stall_current
Austin Schuhc1c957a2020-02-20 17:47:58 -0800706 # Motor inertia in kg m^2
707 # Diameter of 1.9", weight of: 100 grams
708 # TODO(austin): Get a number from Scott Westbrook for the mass
Ravago Jones26f7ad02021-02-05 15:45:59 -0800709 self.motor_inertia = 0.1 * ((0.95 * 0.0254)**2.0)