| import frc971.control_loops.python.controls as controls |
| import numpy |
| import os |
| import json |
| |
| |
| class Constant(object): |
| |
| def __init__(self, name, formatt, value, comment=None): |
| self.name = name |
| self.formatt = formatt |
| self.value = value |
| self.formatToType = {} |
| self.formatToType['%f'] = "double" |
| self.formatToType['%d'] = "int" |
| if comment is None: |
| self.comment = "" |
| else: |
| self.comment = comment + "\n" |
| |
| def Render(self, loop_type): |
| typestring = self.formatToType[self.formatt] |
| if loop_type == 'float' and typestring == 'double': |
| typestring = loop_type |
| return str("\n%sstatic constexpr %s %s = "+ self.formatt +";\n") % \ |
| (self.comment, typestring, self.name, self.value) |
| |
| |
| def MatrixToJson(matrix): |
| """Returns JSON representation of a numpy matrix.""" |
| return { |
| "rows": matrix.shape[0], |
| "cols": matrix.shape[1], |
| "storage_order": "ColMajor", |
| "data": numpy.array(matrix).flatten(order='F').tolist() |
| } |
| |
| |
| class ControlLoopWriter(object): |
| |
| def __init__(self, |
| gain_schedule_name, |
| loops, |
| namespaces=None, |
| write_constants=False, |
| plant_type='StateFeedbackPlant', |
| observer_type='StateFeedbackObserver', |
| scalar_type='double'): |
| """Constructs a control loop writer. |
| |
| Args: |
| gain_schedule_name: string, Name of the overall controller. |
| loops: array[ControlLoop], a list of control loops to gain schedule |
| in order. |
| namespaces: array[string], a list of names of namespaces to nest in |
| order. If None, the default will be used. |
| plant_type: string, The C++ type of the plant. |
| observer_type: string, The C++ type of the observer. |
| scalar_type: string, The C++ type of the base scalar. |
| """ |
| self._gain_schedule_name = gain_schedule_name |
| self._loops = loops |
| if namespaces: |
| self._namespaces = namespaces |
| else: |
| self._namespaces = ['frc971', 'control_loops'] |
| |
| self._namespace_start = '\n'.join( |
| ['namespace %s {' % name for name in self._namespaces]) |
| |
| self._namespace_end = '\n'.join([ |
| '} // namespace %s' % name for name in reversed(self._namespaces) |
| ]) |
| |
| self._constant_list = [] |
| self._plant_type = plant_type |
| self._observer_type = observer_type |
| self._scalar_type = scalar_type |
| |
| def AddConstant(self, constant): |
| """Adds a constant to write. |
| |
| Args: |
| constant: Constant, the constant to add to the header. |
| """ |
| self._constant_list.append(constant) |
| |
| def _TopDirectory(self): |
| return self._namespaces[0] |
| |
| def _HeaderGuard(self, header_file): |
| return ('_'.join([namespace.upper() |
| for namespace in self._namespaces]) + '_' + |
| os.path.basename(header_file).upper().replace( |
| '.', '_').replace('/', '_') + '_') |
| |
| def Write(self, header_file, cc_file, json_file=None): |
| """Writes the loops to the specified files.""" |
| self.WriteHeader(header_file) |
| self.WriteCC(os.path.basename(header_file), cc_file) |
| if json_file is not None: |
| self.WriteJson(json_file) |
| |
| def _GenericType(self, typename, extra_args=None): |
| """Returns a loop template using typename for the type.""" |
| num_states = self._loops[0].A.shape[0] |
| num_inputs = self._loops[0].B.shape[1] |
| num_outputs = self._loops[0].C.shape[0] |
| if extra_args is not None: |
| extra_args = ', ' + extra_args |
| else: |
| extra_args = '' |
| if self._scalar_type != 'double': |
| extra_args += ', ' + self._scalar_type |
| return '%s<%d, %d, %d%s>' % (typename, num_states, num_inputs, |
| num_outputs, extra_args) |
| |
| def _ControllerType(self): |
| """Returns a template name for StateFeedbackController.""" |
| return self._GenericType('StateFeedbackController') |
| |
| def _ObserverType(self): |
| """Returns a template name for StateFeedbackObserver.""" |
| return self._GenericType(self._observer_type) |
| |
| def _LoopType(self): |
| """Returns a template name for StateFeedbackLoop.""" |
| num_states = self._loops[0].A.shape[0] |
| num_inputs = self._loops[0].B.shape[1] |
| num_outputs = self._loops[0].C.shape[0] |
| |
| return 'StateFeedbackLoop<%d, %d, %d, %s, %s, %s>' % ( |
| num_states, num_inputs, num_outputs, self._scalar_type, |
| self._PlantType(), self._ObserverType()) |
| |
| def _PlantType(self): |
| """Returns a template name for StateFeedbackPlant.""" |
| return self._GenericType(self._plant_type) |
| |
| def _PlantCoeffType(self): |
| """Returns a template name for StateFeedbackPlantCoefficients.""" |
| return self._GenericType(self._plant_type + 'Coefficients') |
| |
| def _ControllerCoeffType(self): |
| """Returns a template name for StateFeedbackControllerCoefficients.""" |
| return self._GenericType('StateFeedbackControllerCoefficients') |
| |
| def _ObserverCoeffType(self): |
| """Returns a template name for StateFeedbackObserverCoefficients.""" |
| return self._GenericType(self._observer_type + 'Coefficients') |
| |
| def WriteHeader(self, header_file): |
| """Writes the header file to the file named header_file.""" |
| with open(header_file, 'w') as fd: |
| header_guard = self._HeaderGuard(header_file) |
| fd.write('#ifndef %s\n' |
| '#define %s\n\n' % (header_guard, header_guard)) |
| fd.write( |
| '#include \"frc971/control_loops/state_feedback_loop.h\"\n') |
| if (self._plant_type == 'StateFeedbackHybridPlant' |
| or self._observer_type == 'HybridKalman'): |
| fd.write( |
| '#include \"frc971/control_loops/hybrid_state_feedback_loop.h\"\n' |
| ) |
| |
| fd.write('\n') |
| |
| fd.write(self._namespace_start) |
| |
| for const in self._constant_list: |
| fd.write(const.Render(self._scalar_type)) |
| |
| fd.write('\n\n') |
| for loop in self._loops: |
| fd.write(loop.DumpPlantHeader(self._PlantCoeffType())) |
| fd.write('\n') |
| fd.write(loop.DumpControllerHeader(self._scalar_type)) |
| fd.write('\n') |
| fd.write(loop.DumpObserverHeader(self._ObserverCoeffType())) |
| fd.write('\n') |
| |
| fd.write('%s Make%sPlant();\n\n' % |
| (self._PlantType(), self._gain_schedule_name)) |
| |
| fd.write('%s Make%sController();\n\n' % |
| (self._ControllerType(), self._gain_schedule_name)) |
| |
| fd.write('%s Make%sObserver();\n\n' % |
| (self._ObserverType(), self._gain_schedule_name)) |
| |
| fd.write('%s Make%sLoop();\n\n' % |
| (self._LoopType(), self._gain_schedule_name)) |
| |
| fd.write(self._namespace_end) |
| fd.write('\n\n') |
| fd.write("#endif // %s\n" % header_guard) |
| |
| def WriteCC(self, header_file_name, cc_file): |
| """Writes the cc file to the file named cc_file.""" |
| with open(cc_file, 'w') as fd: |
| fd.write('#include \"%s/%s\"\n' % |
| (os.path.join(*self._namespaces), header_file_name)) |
| fd.write('\n') |
| fd.write('#include <chrono>\n') |
| fd.write('#include <vector>\n') |
| fd.write('\n') |
| fd.write( |
| '#include \"frc971/control_loops/state_feedback_loop.h\"\n') |
| fd.write('\n') |
| fd.write(self._namespace_start) |
| fd.write('\n\n') |
| for loop in self._loops: |
| fd.write( |
| loop.DumpPlant(self._PlantCoeffType(), self._scalar_type)) |
| fd.write('\n') |
| |
| for loop in self._loops: |
| fd.write(loop.DumpController(self._scalar_type)) |
| fd.write('\n') |
| |
| for loop in self._loops: |
| fd.write( |
| loop.DumpObserver(self._ObserverCoeffType(), |
| self._scalar_type)) |
| fd.write('\n') |
| |
| fd.write('%s Make%sPlant() {\n' % |
| (self._PlantType(), self._gain_schedule_name)) |
| fd.write(' ::std::vector< ::std::unique_ptr<%s>> plants(%d);\n' % |
| (self._PlantCoeffType(), len(self._loops))) |
| for index, loop in enumerate(self._loops): |
| fd.write( |
| ' plants[%d] = ::std::unique_ptr<%s>(new %s(%s));\n' % |
| (index, self._PlantCoeffType(), self._PlantCoeffType(), |
| loop.PlantFunction())) |
| fd.write(' return %s(std::move(plants));\n' % self._PlantType()) |
| fd.write('}\n\n') |
| |
| fd.write('%s Make%sController() {\n' % |
| (self._ControllerType(), self._gain_schedule_name)) |
| fd.write( |
| ' ::std::vector< ::std::unique_ptr<%s>> controllers(%d);\n' % |
| (self._ControllerCoeffType(), len(self._loops))) |
| for index, loop in enumerate(self._loops): |
| fd.write( |
| ' controllers[%d] = ::std::unique_ptr<%s>(new %s(%s));\n' |
| % (index, self._ControllerCoeffType(), |
| self._ControllerCoeffType(), loop.ControllerFunction())) |
| fd.write(' return %s(std::move(controllers));\n' % |
| self._ControllerType()) |
| fd.write('}\n\n') |
| |
| fd.write('%s Make%sObserver() {\n' % |
| (self._ObserverType(), self._gain_schedule_name)) |
| fd.write( |
| ' ::std::vector< ::std::unique_ptr<%s>> observers(%d);\n' % |
| (self._ObserverCoeffType(), len(self._loops))) |
| for index, loop in enumerate(self._loops): |
| fd.write( |
| ' observers[%d] = ::std::unique_ptr<%s>(new %s(%s));\n' % |
| (index, self._ObserverCoeffType(), |
| self._ObserverCoeffType(), loop.ObserverFunction())) |
| fd.write(' return %s(std::move(observers));\n' % |
| self._ObserverType()) |
| fd.write('}\n\n') |
| |
| fd.write('%s Make%sLoop() {\n' % |
| (self._LoopType(), self._gain_schedule_name)) |
| fd.write( |
| ' return %s(Make%sPlant(), Make%sController(), Make%sObserver());\n' |
| % (self._LoopType(), self._gain_schedule_name, |
| self._gain_schedule_name, self._gain_schedule_name)) |
| fd.write('}\n\n') |
| |
| fd.write(self._namespace_end) |
| fd.write('\n') |
| |
| def WriteJson(self, json_file): |
| """Writes a JSON file of the loop constants to the specified json_file.""" |
| loops = [] |
| for loop in self._loops: |
| loop_json = {} |
| loop_json["plant"] = loop.DumpPlantJson(self._PlantCoeffType()) |
| loop_json["controller"] = loop.DumpControllerJson() |
| loop_json["observer"] = loop.DumbObserverJson( |
| self._ObserverCoeffType()) |
| loops.append(loop_json) |
| with open(json_file, 'w') as f: |
| f.write(json.dumps(loops)) |
| |
| |
| class ControlLoop(object): |
| |
| def __init__(self, name): |
| """Constructs a control loop object. |
| |
| Args: |
| name: string, The name of the loop to use when writing the C++ files. |
| """ |
| self._name = name |
| self.delayed_u = 0 |
| |
| @property |
| def name(self): |
| """Returns the name""" |
| return self._name |
| |
| def ContinuousToDiscrete(self, A_continuous, B_continuous, dt): |
| """Calculates the discrete time values for A and B. |
| |
| Args: |
| A_continuous: numpy.matrix, The continuous time A matrix |
| B_continuous: numpy.matrix, The continuous time B matrix |
| dt: float, The time step of the control loop |
| |
| Returns: |
| (A, B), numpy.matrix, the control matricies. |
| """ |
| return controls.c2d(A_continuous, B_continuous, dt) |
| |
| def InitializeState(self): |
| """Sets X, Y, and X_hat to zero defaults.""" |
| self.X = numpy.matrix(numpy.zeros((self.A.shape[0], 1))) |
| self.Y = self.C * self.X |
| self.X_hat = numpy.matrix(numpy.zeros((self.A.shape[0], 1))) |
| self.last_U = numpy.matrix( |
| numpy.zeros((self.B.shape[1], max(1, self.delayed_u)))) |
| |
| def PlaceControllerPoles(self, poles): |
| """Places the controller poles. |
| |
| Args: |
| poles: array, An array of poles. Must be complex conjegates if they have |
| any imaginary portions. |
| """ |
| self.K = controls.dplace(self.A, self.B, poles) |
| |
| def PlaceObserverPoles(self, poles): |
| """Places the observer poles. |
| |
| Args: |
| poles: array, An array of poles. Must be complex conjegates if they have |
| any imaginary portions. |
| """ |
| self.L = controls.dplace(self.A.T, self.C.T, poles).T |
| |
| def Update(self, U): |
| """Simulates one time step with the provided U.""" |
| #U = numpy.clip(U, self.U_min, self.U_max) |
| if self.delayed_u > 0: |
| self.X = self.A * self.X + self.B * self.last_U[:, -1] |
| self.Y = self.C * self.X + self.D * self.last_U[:, -1] |
| self.last_U[:, 1:] = self.last_U[:, 0:-1] |
| self.last_U[:, 0] = U.copy() |
| else: |
| self.X = self.A * self.X + self.B * U |
| self.Y = self.C * self.X + self.D * U |
| |
| def PredictObserver(self, U): |
| """Runs the predict step of the observer update.""" |
| if self.delayed_u > 0: |
| self.X_hat = (self.A * self.X_hat + self.B * self.last_U[:, -1]) |
| self.last_U[:, 1:] = self.last_U[:, 0:-1] |
| self.last_U[:, 0] = U.copy() |
| else: |
| self.X_hat = (self.A * self.X_hat + self.B * U) |
| |
| def CorrectObserver(self, U): |
| """Runs the correct step of the observer update.""" |
| if hasattr(self, 'KalmanGain'): |
| KalmanGain = self.KalmanGain |
| else: |
| KalmanGain = numpy.linalg.inv(self.A) * self.L |
| if self.delayed_u > 0: |
| self.X_hat += KalmanGain * (self.Y - self.C * self.X_hat - |
| self.D * self.last_U[:, -1]) |
| else: |
| self.X_hat += KalmanGain * (self.Y - self.C * self.X_hat - |
| self.D * U) |
| |
| def _DumpMatrix(self, matrix_name, matrix, scalar_type): |
| """Dumps the provided matrix into a variable called matrix_name. |
| |
| Args: |
| matrix_name: string, The variable name to save the matrix to. |
| matrix: The matrix to dump. |
| scalar_type: The C++ type to use for the scalar in the matrix. |
| |
| Returns: |
| string, The C++ commands required to populate a variable named matrix_name |
| with the contents of matrix. |
| """ |
| ans = [ |
| ' Eigen::Matrix<%s, %d, %d> %s;\n' % |
| (scalar_type, matrix.shape[0], matrix.shape[1], matrix_name) |
| ] |
| for x in range(matrix.shape[0]): |
| for y in range(matrix.shape[1]): |
| write_type = repr(matrix[x, y]) |
| if scalar_type == 'float': |
| if '.' not in write_type and 'e' not in write_type: |
| write_type += '.0' |
| write_type += 'f' |
| ans.append(' %s(%d, %d) = %s;\n' % |
| (matrix_name, x, y, write_type)) |
| |
| return ''.join(ans) |
| |
| def DumpPlantHeader(self, plant_coefficient_type): |
| """Writes out a c++ header declaration which will create a Plant object. |
| |
| Returns: |
| string, The header declaration for the function. |
| """ |
| return '%s Make%sPlantCoefficients();\n' % (plant_coefficient_type, |
| self._name) |
| |
| def DumpPlantJson(self, plant_coefficient_type): |
| result = { |
| "c": MatrixToJson(self.C), |
| "d": MatrixToJson(self.D), |
| "u_min": MatrixToJson(self.U_min), |
| "u_max": MatrixToJson(self.U_max), |
| "u_limit_coefficient": MatrixToJson(self.U_limit_coefficient), |
| "u_limit_constant": MatrixToJson(self.U_limit_constant), |
| "delayed_u": self.delayed_u |
| } |
| if plant_coefficient_type.startswith('StateFeedbackPlant'): |
| result["a"] = MatrixToJson(self.A) |
| result["b"] = MatrixToJson(self.B) |
| result["dt"] = int(self.dt * 1e9) |
| elif plant_coefficient_type.startswith('StateFeedbackHybridPlant'): |
| result["a_continuous"] = MatrixToJson(self.A_continuous) |
| result["b_continuous"] = MatrixToJson(self.B_continuous) |
| else: |
| glog.fatal('Unsupported plant type %s', plant_coefficient_type) |
| return result |
| |
| def DumpPlant(self, plant_coefficient_type, scalar_type): |
| """Writes out a c++ function which will create a PlantCoefficients object. |
| |
| Returns: |
| string, The function which will create the object. |
| """ |
| ans = [ |
| '%s Make%sPlantCoefficients() {\n' % |
| (plant_coefficient_type, self._name) |
| ] |
| |
| num_states = self.A.shape[0] |
| num_inputs = self.B.shape[1] |
| num_outputs = self.C.shape[0] |
| |
| ans.append(self._DumpMatrix('C', self.C, scalar_type)) |
| ans.append(self._DumpMatrix('D', self.D, scalar_type)) |
| ans.append(self._DumpMatrix('U_max', self.U_max, scalar_type)) |
| ans.append(self._DumpMatrix('U_min', self.U_min, scalar_type)) |
| |
| if not hasattr(self, 'U_limit_coefficient'): |
| self.U_limit_coefficient = numpy.matrix( |
| numpy.zeros((num_inputs, num_states))) |
| |
| if not hasattr(self, 'U_limit_constant'): |
| self.U_limit_constant = self.U_max |
| |
| ans.append( |
| self._DumpMatrix('U_limit_coefficient', self.U_limit_coefficient, |
| scalar_type)) |
| ans.append( |
| self._DumpMatrix('U_limit_constant', self.U_limit_constant, |
| scalar_type)) |
| |
| delayed_u_string = str(self.delayed_u) |
| if plant_coefficient_type.startswith('StateFeedbackPlant'): |
| ans.append(self._DumpMatrix('A', self.A, scalar_type)) |
| ans.append(self._DumpMatrix('B', self.B, scalar_type)) |
| ans.append(' const std::chrono::nanoseconds dt(%d);\n' % |
| (self.dt * 1e9)) |
| ans.append( |
| ' return %s' |
| '(A, B, C, D, U_max, U_min, U_limit_coefficient, U_limit_constant, dt, %s);\n' |
| % (plant_coefficient_type, delayed_u_string)) |
| elif plant_coefficient_type.startswith('StateFeedbackHybridPlant'): |
| ans.append( |
| self._DumpMatrix('A_continuous', self.A_continuous, |
| scalar_type)) |
| ans.append( |
| self._DumpMatrix('B_continuous', self.B_continuous, |
| scalar_type)) |
| ans.append( |
| ' return %s' |
| '(A_continuous, B_continuous, C, D, U_max, U_min, U_limit_coefficient, U_limit_constant, %s);\n' |
| % (plant_coefficient_type, delayed_u_string)) |
| else: |
| glog.fatal('Unsupported plant type %s', plant_coefficient_type) |
| |
| ans.append('}\n') |
| return ''.join(ans) |
| |
| def PlantFunction(self): |
| """Returns the name of the plant coefficient function.""" |
| return 'Make%sPlantCoefficients()' % self._name |
| |
| def ControllerFunction(self): |
| """Returns the name of the controller function.""" |
| return 'Make%sControllerCoefficients()' % self._name |
| |
| def ObserverFunction(self): |
| """Returns the name of the controller function.""" |
| return 'Make%sObserverCoefficients()' % self._name |
| |
| def DumpControllerHeader(self, scalar_type): |
| """Writes out a c++ header declaration which will create a Controller object. |
| |
| Returns: |
| string, The header declaration for the function. |
| """ |
| num_states = self.A.shape[0] |
| num_inputs = self.B.shape[1] |
| num_outputs = self.C.shape[0] |
| return 'StateFeedbackControllerCoefficients<%d, %d, %d, %s> %s;\n' % ( |
| num_states, num_inputs, num_outputs, scalar_type, |
| self.ControllerFunction()) |
| |
| def DumpControllerJson(self): |
| result = {"k": MatrixToJson(self.K), "kff": MatrixToJson(self.Kff)} |
| return result |
| |
| def DumpController(self, scalar_type): |
| """Returns a c++ function which will create a Controller object. |
| |
| Returns: |
| string, The function which will create the object. |
| """ |
| num_states = self.A.shape[0] |
| num_inputs = self.B.shape[1] |
| num_outputs = self.C.shape[0] |
| ans = [ |
| 'StateFeedbackControllerCoefficients<%d, %d, %d, %s> %s {\n' % |
| (num_states, num_inputs, num_outputs, scalar_type, |
| self.ControllerFunction()) |
| ] |
| |
| ans.append(self._DumpMatrix('K', self.K, scalar_type)) |
| if not hasattr(self, 'Kff'): |
| self.Kff = numpy.matrix(numpy.zeros(self.K.shape)) |
| |
| ans.append(self._DumpMatrix('Kff', self.Kff, scalar_type)) |
| |
| ans.append( |
| ' return StateFeedbackControllerCoefficients<%d, %d, %d, %s>' |
| '(K, Kff);\n' % (num_states, num_inputs, num_outputs, scalar_type)) |
| ans.append('}\n') |
| return ''.join(ans) |
| |
| def DumpObserverHeader(self, observer_coefficient_type): |
| """Writes out a c++ header declaration which will create a Observer object. |
| |
| Returns: |
| string, The header declaration for the function. |
| """ |
| return '%s %s;\n' % (observer_coefficient_type, |
| self.ObserverFunction()) |
| |
| def GetObserverCoefficients(self): |
| if hasattr(self, 'KalmanGain'): |
| KalmanGain = self.KalmanGain |
| Q = self.Q |
| R = self.R |
| else: |
| KalmanGain = numpy.linalg.inv(self.A) * self.L |
| Q = numpy.zeros(self.A.shape) |
| R = numpy.zeros((self.C.shape[0], self.C.shape[0])) |
| return (KalmanGain, Q, R) |
| |
| def DumbObserverJson(self, observer_coefficient_type): |
| result = {"delayed_u": self.delayed_u} |
| if observer_coefficient_type.startswith('StateFeedbackObserver'): |
| KalmanGain, Q, R = self.GetObserverCoefficients() |
| result["kalman_gain"] = MatrixToJson(KalmanGain) |
| result["q"] = MatrixToJson(Q) |
| result["r"] = MatrixToJson(R) |
| elif observer_coefficient_type.startswith('HybridKalman'): |
| result["q_continuous"] = MatrixToJson(self.Q_continuous) |
| result["r_continuous"] = MatrixToJson(self.R_continuous) |
| result["p_steady_state"] = MatrixToJson(self.P_steady_state) |
| else: |
| glog.fatal('Unsupported plant type %s', observer_coefficient_type) |
| return result |
| |
| def DumpObserver(self, observer_coefficient_type, scalar_type): |
| """Returns a c++ function which will create a Observer object. |
| |
| Returns: |
| string, The function which will create the object. |
| """ |
| ans = [ |
| '%s %s {\n' % (observer_coefficient_type, self.ObserverFunction()) |
| ] |
| |
| delayed_u_string = str(self.delayed_u) |
| if observer_coefficient_type.startswith('StateFeedbackObserver'): |
| KalmanGain, Q, R = self.GetObserverCoefficients() |
| ans.append(self._DumpMatrix('KalmanGain', KalmanGain, scalar_type)) |
| ans.append(self._DumpMatrix('Q', Q, scalar_type)) |
| ans.append(self._DumpMatrix('R', R, scalar_type)) |
| ans.append(' return %s(KalmanGain, Q, R, %s);\n' % |
| (observer_coefficient_type, delayed_u_string)) |
| |
| elif observer_coefficient_type.startswith('HybridKalman'): |
| ans.append( |
| self._DumpMatrix('Q_continuous', self.Q_continuous, |
| scalar_type)) |
| ans.append( |
| self._DumpMatrix('R_continuous', self.R_continuous, |
| scalar_type)) |
| ans.append( |
| self._DumpMatrix('P_steady_state', self.P_steady_state, |
| scalar_type)) |
| ans.append( |
| ' return %s(Q_continuous, R_continuous, P_steady_state, %s);\n' |
| % (observer_coefficient_type, delayed_u_string)) |
| else: |
| glog.fatal('Unsupported observer type %s', |
| observer_coefficient_type) |
| |
| ans.append('}\n') |
| return ''.join(ans) |
| |
| |
| class HybridControlLoop(ControlLoop): |
| |
| def __init__(self, name): |
| super(HybridControlLoop, self).__init__(name=name) |
| |
| def Discretize(self, dt): |
| [self.A, self.B, self.Q, self.R] = \ |
| controls.kalmd(self.A_continuous, self.B_continuous, |
| self.Q_continuous, self.R_continuous, dt) |
| |
| def PredictHybridObserver(self, U, dt): |
| self.Discretize(dt) |
| if self.delayed_u > 0: |
| self.X_hat = self.A * self.X_hat + self.B * self.last_U[:, -1] |
| self.last_U[:, 1:] = self.last_U[:, 0:-1] |
| self.last_U[:, 0] = U.copy() |
| else: |
| self.X_hat = self.A * self.X_hat + self.B * U |
| |
| self.P = (self.A * self.P * self.A.T + self.Q) |
| |
| def CorrectHybridObserver(self, U): |
| Y_bar = self.Y - self.C * self.X_hat |
| C_t = self.C.T |
| S = self.C * self.P * C_t + self.R |
| self.KalmanGain = self.P * C_t * numpy.linalg.inv(S) |
| self.X_hat = self.X_hat + self.KalmanGain * Y_bar |
| self.P = (numpy.eye(len(self.A)) - self.KalmanGain * self.C) * self.P |
| |
| def InitializeState(self): |
| super(HybridControlLoop, self).InitializeState() |
| if hasattr(self, 'Q_steady_state'): |
| self.P = self.Q_steady_state |
| else: |
| self.P = numpy.matrix( |
| numpy.zeros((self.A.shape[0], self.A.shape[0]))) |
| |
| |
| class CIM(object): |
| |
| def __init__(self): |
| # Stall Torque in N m |
| self.stall_torque = 2.42 |
| # Stall Current in Amps |
| self.stall_current = 133.0 |
| # Free Speed in rad/s |
| self.free_speed = 5500.0 / 60.0 * 2.0 * numpy.pi |
| # Free Current in Amps |
| self.free_current = 4.7 |
| # Resistance of the motor |
| self.resistance = 12.0 / self.stall_current |
| # Motor velocity constant |
| self.Kv = (self.free_speed / |
| (12.0 - self.resistance * self.free_current)) |
| # Torque constant |
| self.Kt = self.stall_torque / self.stall_current |
| |
| |
| class MiniCIM(object): |
| |
| def __init__(self): |
| # Stall Torque in N m |
| self.stall_torque = 1.41 |
| # Stall Current in Amps |
| self.stall_current = 89.0 |
| # Free Speed in rad/s |
| self.free_speed = 5840.0 / 60.0 * 2.0 * numpy.pi |
| # Free Current in Amps |
| self.free_current = 3.0 |
| # Resistance of the motor |
| self.resistance = 12.0 / self.stall_current |
| # Motor velocity constant |
| self.Kv = (self.free_speed / |
| (12.0 - self.resistance * self.free_current)) |
| # Torque constant |
| self.Kt = self.stall_torque / self.stall_current |
| |
| # Motor inertia in kg m^2 |
| self.motor_inertia = 0.0001634 |
| |
| |
| class NMotor(object): |
| |
| def __init__(self, motor, n): |
| """Gangs together n motors.""" |
| self.motor = motor |
| self.stall_torque = motor.stall_torque * n |
| self.stall_current = motor.stall_current * n |
| self.free_speed = motor.free_speed |
| |
| self.free_current = motor.free_current * n |
| self.resistance = motor.resistance / n |
| self.Kv = motor.Kv |
| self.Kt = motor.Kt |
| self.motor_inertia = motor.motor_inertia * n |
| |
| |
| class Vex775Pro(object): |
| |
| def __init__(self): |
| # Stall Torque in N m |
| self.stall_torque = 0.71 |
| # Stall Current in Amps |
| self.stall_current = 134.0 |
| # Free Speed in rad/s |
| self.free_speed = 18730.0 / 60.0 * 2.0 * numpy.pi |
| # Free Current in Amps |
| self.free_current = 0.7 |
| # Resistance of the motor |
| self.resistance = 12.0 / self.stall_current |
| # Motor velocity constant |
| self.Kv = (self.free_speed / |
| (12.0 - self.resistance * self.free_current)) |
| # Torque constant |
| self.Kt = self.stall_torque / self.stall_current |
| # Motor inertia in kg m^2 |
| self.motor_inertia = 0.00001187 |
| |
| |
| class BAG(object): |
| # BAG motor specs available at http://motors.vex.com/vexpro-motors/bag-motor |
| def __init__(self): |
| # Stall Torque in (N m) |
| self.stall_torque = 0.43 |
| # Stall Current in (Amps) |
| self.stall_current = 53.0 |
| # Free Speed in (rad/s) |
| self.free_speed = 13180.0 / 60.0 * 2.0 * numpy.pi |
| # Free Current in (Amps) |
| self.free_current = 1.8 |
| # Resistance of the motor (Ohms) |
| self.resistance = 12.0 / self.stall_current |
| # Motor velocity constant (radians / (sec * volt)) |
| self.Kv = (self.free_speed / |
| (12.0 - self.resistance * self.free_current)) |
| # Torque constant (N * m / A) |
| self.Kt = self.stall_torque / self.stall_current |
| # Motor inertia in kg m^2 |
| self.motor_inertia = 0.000006 |
| |
| |
| class MN3510(object): |
| |
| def __init__(self): |
| # http://www.robotshop.com/en/t-motor-navigator-mn3510-360kv-brushless-motor.html#Specifications |
| # Free Current in Amps |
| self.free_current = 0.0 |
| # Resistance of the motor |
| self.resistance = 0.188 |
| # Stall Current in Amps |
| self.stall_current = 14.0 / self.resistance |
| # Motor velocity constant |
| self.Kv = 360.0 / 60.0 * (2.0 * numpy.pi) |
| # Torque constant Nm / A |
| self.Kt = 1.0 / self.Kv |
| # Stall Torque in N m |
| self.stall_torque = self.Kt * self.stall_current |
| |
| |
| class Falcon(object): |
| """Class representing the VexPro Falcon 500 motor. |
| |
| All numbers based on data from |
| https://www.vexrobotics.com/vexpro/falcon-500.""" |
| |
| def __init__(self): |
| # Stall Torque in N m |
| self.stall_torque = 4.69 |
| # Stall Current in Amps |
| self.stall_current = 257.0 |
| # Free Speed in rad / sec |
| self.free_speed = 6380.0 / 60.0 * 2.0 * numpy.pi |
| # Free Current in Amps |
| self.free_current = 1.5 |
| # Resistance of the motor, divided by 2 to account for the 2 motors |
| self.resistance = 12.0 / self.stall_current |
| # Motor velocity constant |
| self.Kv = (self.free_speed / |
| (12.0 - self.resistance * self.free_current)) |
| # Torque constant |
| self.Kt = self.stall_torque / self.stall_current |
| # Motor inertia in kg m^2 |
| # Diameter of 1.9", weight of: 100 grams |
| # TODO(austin): Get a number from Scott Westbrook for the mass |
| self.motor_inertia = 0.1 * ((0.95 * 0.0254)**2.0) |
| |
| |
| class KrakenFOC(object): |
| """Class representing the WCP Kraken X60 motor using |
| Field Oriented Controls (FOC) communication. |
| |
| All numbers based on data from |
| https://wcproducts.com/products/kraken. |
| """ |
| |
| def __init__(self): |
| # Stall Torque in N m |
| self.stall_torque = 9.37 |
| # Stall Current in Amps |
| self.stall_current = 483.0 |
| # Free Speed in rad / sec |
| self.free_speed = 5800.0 / 60.0 * 2.0 * numpy.pi |
| # Free Current in Amps |
| self.free_current = 2.0 |
| # Resistance of the motor, divided by 2 to account for the 2 motors |
| self.resistance = 12.0 / self.stall_current |
| # Motor velocity constant |
| self.Kv = (self.free_speed / |
| (12.0 - self.resistance * self.free_current)) |
| # Torque constant |
| self.Kt = self.stall_torque / self.stall_current |
| # Motor inertia in kg m^2 |
| # Diameter of 1.9", weight of: 100 grams |
| # TODO(Filip): Update motor inertia for Kraken, currently using Falcon motor inertia |
| self.motor_inertia = 0.1 * ((0.95 * 0.0254)**2.0) |