Reformat python and c++ files
Change-Id: I7d7d99a2094c2a9181ed882735b55159c14db3b0
diff --git a/frc971/control_loops/python/angular_system.py b/frc971/control_loops/python/angular_system.py
index a31390a..2b5a37e 100755
--- a/frc971/control_loops/python/angular_system.py
+++ b/frc971/control_loops/python/angular_system.py
@@ -9,6 +9,7 @@
class AngularSystemParams(object):
+
def __init__(self,
name,
motor,
@@ -35,6 +36,7 @@
class AngularSystem(control_loop.ControlLoop):
+
def __init__(self, params, name="AngularSystem"):
super(AngularSystem, self).__init__(name)
self.params = params
@@ -45,15 +47,15 @@
self.G = params.G
# Moment of inertia in kg m^2
- self.J = params.J + self.motor.motor_inertia / (self.G ** 2.0)
+ self.J = params.J + self.motor.motor_inertia / (self.G**2.0)
# Control loop time step
self.dt = params.dt
# State is [position, velocity]
# Input is [Voltage]
- C1 = self.motor.Kt / (self.G * self.G * self.motor.resistance *
- self.J * self.motor.Kv)
+ C1 = self.motor.Kt / (
+ self.G * self.G * self.motor.resistance * self.J * self.motor.Kv)
C2 = self.motor.Kt / (self.G * self.J * self.motor.resistance)
self.A_continuous = numpy.matrix([[0, 1], [0, -C1]])
@@ -116,6 +118,7 @@
class IntegralAngularSystem(AngularSystem):
+
def __init__(self, params, name="IntegralAngularSystem"):
super(IntegralAngularSystem, self).__init__(params, name=name)
@@ -136,10 +139,9 @@
self.A, self.B = self.ContinuousToDiscrete(self.A_continuous,
self.B_continuous, self.dt)
- self.Q = numpy.matrix(
- [[(self.params.kalman_q_pos**2.0), 0.0, 0.0],
- [0.0, (self.params.kalman_q_vel**2.0), 0.0],
- [0.0, 0.0, (self.params.kalman_q_voltage**2.0)]])
+ self.Q = numpy.matrix([[(self.params.kalman_q_pos**2.0), 0.0, 0.0],
+ [0.0, (self.params.kalman_q_vel**2.0), 0.0],
+ [0.0, 0.0, (self.params.kalman_q_voltage**2.0)]])
self.R = numpy.matrix([[(self.params.kalman_r_position**2.0)]])
@@ -197,8 +199,8 @@
vbat = 12.0
- goal = numpy.concatenate(
- (plant.X, numpy.matrix(numpy.zeros((1, 1)))), axis=0)
+ goal = numpy.concatenate((plant.X, numpy.matrix(numpy.zeros((1, 1)))),
+ axis=0)
profile = TrapezoidProfile(plant.dt)
profile.set_maximum_acceleration(max_acceleration)
@@ -257,8 +259,7 @@
goal = controller.A * goal + controller.B * ff_U
if U[0, 0] != U_uncapped[0, 0]:
- profile.MoveCurrentState(
- numpy.matrix([[goal[0, 0]], [goal[1, 0]]]))
+ profile.MoveCurrentState(numpy.matrix([[goal[0, 0]], [goal[1, 0]]]))
glog.debug('Time: %f', t_plot[-1])
glog.debug('goal_error %s', repr(end_goal - goal))
@@ -374,7 +375,8 @@
loop_writer.AddConstant(
control_loop.Constant('kOutputRatio', '%f', angular_system.G))
loop_writer.AddConstant(
- control_loop.Constant('kFreeSpeed', '%f', angular_system.motor.free_speed))
+ control_loop.Constant('kFreeSpeed', '%f',
+ angular_system.motor.free_speed))
loop_writer.Write(plant_files[0], plant_files[1])
integral_angular_system = IntegralAngularSystem(params,
diff --git a/frc971/control_loops/python/cim.py b/frc971/control_loops/python/cim.py
index fc39eb9..8c13eb0 100644
--- a/frc971/control_loops/python/cim.py
+++ b/frc971/control_loops/python/cim.py
@@ -3,44 +3,46 @@
from frc971.control_loops.python import control_loop
import numpy
+
class CIM(control_loop.ControlLoop):
- def __init__(self):
- super(CIM, self).__init__("CIM")
- # Stall Torque in N m
- self.stall_torque = 2.42
- # Stall Current in Amps
- self.stall_current = 133
- # Free Speed in RPM
- self.free_speed = 4650.0
- # Free Current in Amps
- self.free_current = 2.7
- # Moment of inertia of the CIM in kg m^2
- self.J = 0.0001
- # 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 / 60.0 * 2.0 * numpy.pi) /
- (12.0 - self.resistance * self.free_current))
- # Torque constant
- self.Kt = self.stall_torque / self.stall_current
- # Control loop time step
- self.dt = 0.005
- # State feedback matrices
- self.A_continuous = numpy.matrix(
- [[-self.Kt / self.Kv / (self.J * self.resistance)]])
- self.B_continuous = numpy.matrix(
- [[self.Kt / (self.J * self.resistance)]])
- self.C = numpy.matrix([[1]])
- self.D = numpy.matrix([[0]])
+ def __init__(self):
+ super(CIM, self).__init__("CIM")
+ # Stall Torque in N m
+ self.stall_torque = 2.42
+ # Stall Current in Amps
+ self.stall_current = 133
+ # Free Speed in RPM
+ self.free_speed = 4650.0
+ # Free Current in Amps
+ self.free_current = 2.7
+ # Moment of inertia of the CIM in kg m^2
+ self.J = 0.0001
+ # 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 / 60.0 * 2.0 * numpy.pi) /
+ (12.0 - self.resistance * self.free_current))
+ # Torque constant
+ self.Kt = self.stall_torque / self.stall_current
+ # Control loop time step
+ self.dt = 0.005
- self.A, self.B = self.ContinuousToDiscrete(self.A_continuous,
- self.B_continuous, self.dt)
+ # State feedback matrices
+ self.A_continuous = numpy.matrix(
+ [[-self.Kt / self.Kv / (self.J * self.resistance)]])
+ self.B_continuous = numpy.matrix(
+ [[self.Kt / (self.J * self.resistance)]])
+ self.C = numpy.matrix([[1]])
+ self.D = numpy.matrix([[0]])
- self.PlaceControllerPoles([0.01])
- self.PlaceObserverPoles([0.01])
+ self.A, self.B = self.ContinuousToDiscrete(self.A_continuous,
+ self.B_continuous, self.dt)
- self.U_max = numpy.matrix([[12.0]])
- self.U_min = numpy.matrix([[-12.0]])
+ self.PlaceControllerPoles([0.01])
+ self.PlaceObserverPoles([0.01])
- self.InitializeState()
+ self.U_max = numpy.matrix([[12.0]])
+ self.U_min = numpy.matrix([[-12.0]])
+
+ self.InitializeState()
diff --git a/frc971/control_loops/python/control_loop.py b/frc971/control_loops/python/control_loop.py
index e3e00b7..ac22821 100644
--- a/frc971/control_loops/python/control_loop.py
+++ b/frc971/control_loops/python/control_loop.py
@@ -2,548 +2,591 @@
import numpy
import os
-class Constant(object):
- def __init__ (self, name, formatt, value):
- self.name = name
- self.formatt = formatt
- self.value = value
- self.formatToType = {}
- self.formatToType['%f'] = "double"
- self.formatToType['%d'] = "int"
- def Render(self, loop_type):
- typestring = self.formatToType[self.formatt]
- if loop_type == 'float' and typestring == 'double':
- typestring = loop_type
- return str("\nstatic constexpr %s %s = "+ self.formatt +";\n") % \
- (typestring, self.name, self.value)
+class Constant(object):
+
+ def __init__(self, name, formatt, value):
+ self.name = name
+ self.formatt = formatt
+ self.value = value
+ self.formatToType = {}
+ self.formatToType['%f'] = "double"
+ self.formatToType['%d'] = "int"
+
+ def Render(self, loop_type):
+ typestring = self.formatToType[self.formatt]
+ if loop_type == 'float' and typestring == 'double':
+ typestring = loop_type
+ return str("\nstatic constexpr %s %s = "+ self.formatt +";\n") % \
+ (typestring, self.name, self.value)
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']
+ 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.
- self._namespace_start = '\n'.join(
- ['namespace %s {' % name for name in self._namespaces])
+ 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_end = '\n'.join(
- ['} // namespace %s' % name for name in reversed(self._namespaces)])
+ self._namespace_start = '\n'.join(
+ ['namespace %s {' % name for name in self._namespaces])
- self._constant_list = []
- self._plant_type = plant_type
- self._observer_type = observer_type
- self._scalar_type = scalar_type
+ self._namespace_end = '\n'.join([
+ '} // namespace %s' % name for name in reversed(self._namespaces)
+ ])
- def AddConstant(self, constant):
- """Adds a constant to write.
+ self._constant_list = []
+ self._plant_type = plant_type
+ self._observer_type = observer_type
+ self._scalar_type = scalar_type
- Args:
- constant: Constant, the constant to add to the header.
- """
- self._constant_list.append(constant)
+ def AddConstant(self, constant):
+ """Adds a constant to write.
- def _TopDirectory(self):
- return self._namespaces[0]
+ Args:
+ constant: Constant, the constant to add to the header.
+ """
+ self._constant_list.append(constant)
- def _HeaderGuard(self, header_file):
- return ('_'.join([namespace.upper() for namespace in self._namespaces]) + '_' +
- os.path.basename(header_file).upper()
- .replace('.', '_').replace('/', '_') + '_')
+ def _TopDirectory(self):
+ return self._namespaces[0]
- def Write(self, header_file, cc_file):
- """Writes the loops to the specified files."""
- self.WriteHeader(header_file)
- self.WriteCC(os.path.basename(header_file), cc_file)
+ def _HeaderGuard(self, header_file):
+ return ('_'.join([namespace.upper() for namespace in self._namespaces])
+ + '_' + os.path.basename(header_file).upper().replace(
+ '.', '_').replace('/', '_') + '_')
- 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 Write(self, header_file, cc_file):
+ """Writes the loops to the specified files."""
+ self.WriteHeader(header_file)
+ self.WriteCC(os.path.basename(header_file), cc_file)
- def _ControllerType(self):
- """Returns a template name for StateFeedbackController."""
- return self._GenericType('StateFeedbackController')
+ 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 _ObserverType(self):
- """Returns a template name for StateFeedbackObserver."""
- return self._GenericType(self._observer_type)
+ def _ControllerType(self):
+ """Returns a template name for StateFeedbackController."""
+ return self._GenericType('StateFeedbackController')
- 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]
+ def _ObserverType(self):
+ """Returns a template name for StateFeedbackObserver."""
+ return self._GenericType(self._observer_type)
- return 'StateFeedbackLoop<%d, %d, %d, %s, %s, %s>' % (
- num_states,
- num_inputs,
- num_outputs, self._scalar_type,
- self._PlantType(), self._ObserverType())
+ 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 _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 _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 _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 _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')
+ 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('\n')
- fd.write(self._namespace_start)
+ fd.write(self._namespace_start)
- for const in self._constant_list:
- fd.write(const.Render(self._scalar_type))
+ 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('\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%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%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%sObserver();\n\n' % (self._ObserverType(),
+ self._gain_schedule_name))
- fd.write('%s Make%sLoop();\n\n' %
- (self._LoopType(), 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)
+ 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 <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')
+ 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 <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.DumpController(self._scalar_type))
+ fd.write('\n')
- for loop in self._loops:
- fd.write(loop.DumpObserver(self._ObserverCoeffType(), 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(&plants);\n' % self._PlantType())
- fd.write('}\n\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(&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(&controllers);\n' % self._ControllerType())
- 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(&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(&observers);\n' % self._ObserverType())
- 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(&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('%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')
+ fd.write(self._namespace_end)
+ fd.write('\n')
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
+ def __init__(self, name):
+ """Constructs a control loop object.
- @property
- def name(self):
- """Returns the name"""
- return self._name
+ Args:
+ name: string, The name of the loop to use when writing the C++ files.
+ """
+ self._name = name
- def ContinuousToDiscrete(self, A_continuous, B_continuous, dt):
- """Calculates the discrete time values for A and B.
+ @property
+ def name(self):
+ """Returns the name"""
+ return self._name
- 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
+ def ContinuousToDiscrete(self, A_continuous, B_continuous, dt):
+ """Calculates the discrete time values for A and B.
- Returns:
- (A, B), numpy.matrix, the control matricies.
- """
- return controls.c2d(A_continuous, B_continuous, dt)
+ 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
- def InitializeState(self):
- """Sets X, Y, and X_hat to zero defaults."""
- self.X = numpy.zeros((self.A.shape[0], 1))
- self.Y = self.C * self.X
- self.X_hat = numpy.zeros((self.A.shape[0], 1))
+ Returns:
+ (A, B), numpy.matrix, the control matricies.
+ """
+ return controls.c2d(A_continuous, B_continuous, dt)
- def PlaceControllerPoles(self, poles):
- """Places the controller poles.
+ def InitializeState(self):
+ """Sets X, Y, and X_hat to zero defaults."""
+ self.X = numpy.zeros((self.A.shape[0], 1))
+ self.Y = self.C * self.X
+ self.X_hat = numpy.zeros((self.A.shape[0], 1))
- 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 PlaceControllerPoles(self, poles):
+ """Places the controller 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.K = controls.dplace(self.A, self.B, 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 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)
- self.X = self.A * self.X + self.B * U
- self.Y = self.C * self.X + self.D * U
+ def Update(self, U):
+ """Simulates one time step with the provided U."""
+ #U = numpy.clip(U, self.U_min, self.U_max)
+ 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."""
- self.X_hat = (self.A * self.X_hat + self.B * U)
+ def PredictObserver(self, U):
+ """Runs the predict step of the observer update."""
+ 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
- self.X_hat += KalmanGain * (self.Y - self.C * self.X_hat - self.D * 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
+ self.X_hat += KalmanGain * (self.Y - self.C * self.X_hat - self.D * U)
- def UpdateObserver(self, U):
- """Updates the observer given the provided U."""
- if hasattr(self, 'KalmanGain'):
- KalmanGain = self.KalmanGain
- else:
- KalmanGain = numpy.linalg.inv(self.A) * self.L
- self.X_hat = (self.A * self.X_hat + self.B * U +
- self.A * KalmanGain * (self.Y - self.C * self.X_hat - self.D * U))
+ def UpdateObserver(self, U):
+ """Updates the observer given the provided U."""
+ if hasattr(self, 'KalmanGain'):
+ KalmanGain = self.KalmanGain
+ else:
+ KalmanGain = numpy.linalg.inv(self.A) * self.L
+ self.X_hat = (self.A * self.X_hat + self.B * U + self.A * 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.
+ 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.
+ 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 xrange(matrix.shape[0]):
- for y in xrange(matrix.shape[1]):
- write_type = repr(matrix[x, y])
- if scalar_type == 'float':
- if '.' not in write_type:
- write_type += '.0'
- write_type += 'f'
- ans.append(' %s(%d, %d) = %s;\n' % (matrix_name, x, y, write_type))
+ 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 xrange(matrix.shape[0]):
+ for y in xrange(matrix.shape[1]):
+ write_type = repr(matrix[x, y])
+ if scalar_type == 'float':
+ if '.' 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)
+ return ''.join(ans)
- def DumpPlantHeader(self, plant_coefficient_type):
- """Writes out a c++ header declaration which will create a Plant object.
+ 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)
+ Returns:
+ string, The header declaration for the function.
+ """
+ return '%s Make%sPlantCoefficients();\n' % (plant_coefficient_type,
+ self._name)
- def DumpPlant(self, plant_coefficient_type, scalar_type):
- """Writes out a c++ function which will create a PlantCoefficients object.
+ 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)]
+ Returns:
+ string, The function which will create the object.
+ """
+ ans = [
+ '%s Make%sPlantCoefficients() {\n' % (plant_coefficient_type,
+ self._name)
+ ]
- 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))
+ 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 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(' return %s'
- '(A, B, C, D, U_max, U_min);\n' % (
- plant_coefficient_type))
- 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);\n' % (
- plant_coefficient_type))
- else:
- glog.fatal('Unsupported plant type %s', plant_coefficient_type)
+ 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(
+ ' return %s'
+ '(A, B, C, D, U_max, U_min);\n' % (plant_coefficient_type))
+ 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);\n' %
+ (plant_coefficient_type))
+ else:
+ glog.fatal('Unsupported plant type %s', plant_coefficient_type)
- ans.append('}\n')
- return ''.join(ans)
+ ans.append('}\n')
+ return ''.join(ans)
- def PlantFunction(self):
- """Returns the name of the plant coefficient function."""
- return 'Make%sPlantCoefficients()' % self._name
+ 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 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 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.
+ 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())
+ 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 DumpController(self, scalar_type):
- """Returns a c++ function which will create a Controller object.
+ 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())]
+ 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('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(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)
+ 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.
+ 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())
+ Returns:
+ string, The header declaration for the function.
+ """
+ return '%s %s;\n' % (observer_coefficient_type, self.ObserverFunction())
- def DumpObserver(self, observer_coefficient_type, scalar_type):
- """Returns a c++ function which will create a Observer object.
+ 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())]
+ Returns:
+ string, The function which will create the object.
+ """
+ ans = [
+ '%s %s {\n' % (observer_coefficient_type, self.ObserverFunction())
+ ]
- if observer_coefficient_type.startswith('StateFeedbackObserver'):
- 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]))
- 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);\n'
- % (observer_coefficient_type,))
+ if observer_coefficient_type.startswith('StateFeedbackObserver'):
+ 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]))
+ 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);\n' %
+ (observer_coefficient_type,))
- 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);\n' % (
- observer_coefficient_type,))
- else:
- glog.fatal('Unsupported observer type %s', observer_coefficient_type)
+ 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);\n' %
+ (observer_coefficient_type,))
+ else:
+ glog.fatal('Unsupported observer type %s',
+ observer_coefficient_type)
- ans.append('}\n')
- return ''.join(ans)
+ 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 __init__(self, name):
+ super(HybridControlLoop, self).__init__(name=name)
- def PredictHybridObserver(self, U, dt):
- self.Discretize(dt)
- self.X_hat = self.A * self.X_hat + self.B * U
- self.P = (self.A * self.P * self.A.T + self.Q)
+ 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 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 PredictHybridObserver(self, U, dt):
+ self.Discretize(dt)
+ self.X_hat = self.A * self.X_hat + self.B * U
+ self.P = (self.A * self.P * self.A.T + self.Q)
- 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])))
+ 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
+
+ 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
+
+ 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
class NMotor(object):
+
def __init__(self, motor, n):
"""Gangs together n motors."""
self.motor = motor
@@ -558,6 +601,7 @@
class Vex775Pro(object):
+
def __init__(self):
# Stall Torque in N m
self.stall_torque = 0.71
@@ -570,7 +614,8 @@
# 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))
+ 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
@@ -578,37 +623,40 @@
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
+ # 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
+
+ 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
diff --git a/frc971/control_loops/python/down_estimator.py b/frc971/control_loops/python/down_estimator.py
index 6db8f34..8efcff6 100644
--- a/frc971/control_loops/python/down_estimator.py
+++ b/frc971/control_loops/python/down_estimator.py
@@ -16,106 +16,108 @@
gflags.DEFINE_bool('plot', False, 'If true, plot the loop response.')
+
class DownEstimator(control_loop.ControlLoop):
- def __init__(self, name='DownEstimator'):
- super(DownEstimator, self).__init__(name)
- self.dt = 0.005
- # State is [gyro_angle, bias].
- # U is [gyro_x_velocity].
+ def __init__(self, name='DownEstimator'):
+ super(DownEstimator, self).__init__(name)
+ self.dt = 0.005
- self.A_continuous = numpy.matrix([[0, 1],
- [0, 0]])
+ # State is [gyro_angle, bias].
+ # U is [gyro_x_velocity].
- self.B_continuous = numpy.matrix([[1],
- [0]])
+ self.A_continuous = numpy.matrix([[0, 1], [0, 0]])
- self.A, self.B = self.ContinuousToDiscrete(
- self.A_continuous, self.B_continuous, self.dt)
+ self.B_continuous = numpy.matrix([[1], [0]])
- q_gyro_noise = 1e-6
- q_gyro_bias_noise = 1e-3
- self.Q = numpy.matrix([[1.0 / (q_gyro_noise ** 2.0), 0.0],
- [0.0, 1.0 / (q_gyro_bias_noise ** 2.0)]])
+ self.A, self.B = self.ContinuousToDiscrete(self.A_continuous,
+ self.B_continuous, self.dt)
- r_accelerometer_angle_noise = 5e+7
- self.R = numpy.matrix([[(r_accelerometer_angle_noise ** 2.0)]])
+ q_gyro_noise = 1e-6
+ q_gyro_bias_noise = 1e-3
+ self.Q = numpy.matrix([[1.0 / (q_gyro_noise**2.0), 0.0],
+ [0.0, 1.0 / (q_gyro_bias_noise**2.0)]])
- self.C = numpy.matrix([[1.0, 0.0]])
- self.D = numpy.matrix([[0]])
+ r_accelerometer_angle_noise = 5e+7
+ self.R = numpy.matrix([[(r_accelerometer_angle_noise**2.0)]])
- self.U_max = numpy.matrix([[numpy.pi]])
- self.U_min = numpy.matrix([[-numpy.pi]])
- self.K = numpy.matrix(numpy.zeros((1, 2)))
+ self.C = numpy.matrix([[1.0, 0.0]])
+ self.D = numpy.matrix([[0]])
- self.KalmanGain, self.Q_steady = controls.kalman(
- A=self.A, B=self.B, C=self.C, Q=self.Q, R=self.R)
+ self.U_max = numpy.matrix([[numpy.pi]])
+ self.U_min = numpy.matrix([[-numpy.pi]])
+ self.K = numpy.matrix(numpy.zeros((1, 2)))
- self.L = self.A * self.KalmanGain
+ self.KalmanGain, self.Q_steady = controls.kalman(
+ A=self.A, B=self.B, C=self.C, Q=self.Q, R=self.R)
- self.InitializeState()
+ self.L = self.A * self.KalmanGain
- def Update(self, accelerometer_x, accelerometer_y, accelerometer_z, gyro_x):
- acceleration = math.sqrt(
- accelerometer_x**2 + accelerometer_y**2 + accelerometer_z**2)
- if acceleration < 0.9 or acceleration > 1.1:
- glog.error('bad total acceleration %f' % acceleration)
- # TODO(Brian): Tune this?
- return
- accelerometer_angle = math.atan2(accelerometer_x, accelerometer_z)
- Z = numpy.matrix([[accelerometer_angle], [gyro_x]])
+ self.InitializeState()
- Y = Z - self.H * self.X_hat
- S = self.H * self.P * self.H.transpose() + self.R
- K = self.P * self.H.transpose() * numpy.linalg.inv(S)
- self.X_hat += K * Y
- self.P = (numpy.identity(K.shape[0]) - K * self.H) * self.P
+ def Update(self, accelerometer_x, accelerometer_y, accelerometer_z, gyro_x):
+ acceleration = math.sqrt(accelerometer_x**2 + accelerometer_y**2 +
+ accelerometer_z**2)
+ if acceleration < 0.9 or acceleration > 1.1:
+ glog.error('bad total acceleration %f' % acceleration)
+ # TODO(Brian): Tune this?
+ return
+ accelerometer_angle = math.atan2(accelerometer_x, accelerometer_z)
+ Z = numpy.matrix([[accelerometer_angle], [gyro_x]])
+
+ Y = Z - self.H * self.X_hat
+ S = self.H * self.P * self.H.transpose() + self.R
+ K = self.P * self.H.transpose() * numpy.linalg.inv(S)
+ self.X_hat += K * Y
+ self.P = (numpy.identity(K.shape[0]) - K * self.H) * self.P
+
def main(argv):
- argv = FLAGS(argv)
- glog.init()
+ argv = FLAGS(argv)
+ glog.init()
- estimator = DownEstimator()
+ estimator = DownEstimator()
- if FLAGS.plot:
- real_angles = [0]
- real_velocities = [0]
- estimated_angles = [0]
- estimated_velocities = [0]
- for _ in xrange(100):
- estimator.Predict(0)
- estimator.Update(numpy.sqrt(2) / 2.0, numpy.sqrt(2) / 2.0, 0, 0)
- real_angles.append(math.pi / 2)
- real_velocities.append(0)
- estimated_angles.append(estimator.X_hat[0, 0])
- estimated_velocities.append(estimator.X_hat[1, 0])
- angle = math.pi / 2
- velocity = 1
- for i in xrange(100):
- measured_velocity = velocity + (random.random() - 0.5) * 0.01 + 0.05
- estimator.Predict(measured_velocity)
- estimator.Update(math.sin(angle) + (random.random() - 0.5) * 0.02, 0,
- math.cos(angle) + (random.random() - 0.5) * 0.02,
- measured_velocity)
- real_angles.append(angle)
- real_velocities.append(velocity)
- estimated_angles.append(estimator.X_hat[0, 0])
- estimated_velocities.append(estimator.X_hat[1, 0])
- angle += velocity * estimator.dt
- pylab.plot(range(len(real_angles)), real_angles)
- pylab.plot(range(len(real_velocities)), real_velocities)
- pylab.plot(range(len(estimated_angles)), estimated_angles)
- pylab.plot(range(len(estimated_velocities)), estimated_velocities)
- pylab.show()
+ if FLAGS.plot:
+ real_angles = [0]
+ real_velocities = [0]
+ estimated_angles = [0]
+ estimated_velocities = [0]
+ for _ in xrange(100):
+ estimator.Predict(0)
+ estimator.Update(numpy.sqrt(2) / 2.0, numpy.sqrt(2) / 2.0, 0, 0)
+ real_angles.append(math.pi / 2)
+ real_velocities.append(0)
+ estimated_angles.append(estimator.X_hat[0, 0])
+ estimated_velocities.append(estimator.X_hat[1, 0])
+ angle = math.pi / 2
+ velocity = 1
+ for i in xrange(100):
+ measured_velocity = velocity + (random.random() - 0.5) * 0.01 + 0.05
+ estimator.Predict(measured_velocity)
+ estimator.Update(
+ math.sin(angle) + (random.random() - 0.5) * 0.02, 0,
+ math.cos(angle) + (random.random() - 0.5) * 0.02,
+ measured_velocity)
+ real_angles.append(angle)
+ real_velocities.append(velocity)
+ estimated_angles.append(estimator.X_hat[0, 0])
+ estimated_velocities.append(estimator.X_hat[1, 0])
+ angle += velocity * estimator.dt
+ pylab.plot(range(len(real_angles)), real_angles)
+ pylab.plot(range(len(real_velocities)), real_velocities)
+ pylab.plot(range(len(estimated_angles)), estimated_angles)
+ pylab.plot(range(len(estimated_velocities)), estimated_velocities)
+ pylab.show()
- if len(argv) != 3:
- print "Expected .h file name and .cc file name"
- else:
- namespaces = ['frc971', 'control_loops', 'drivetrain']
- kf_loop_writer = control_loop.ControlLoopWriter(
- "DownEstimator", [estimator],
- namespaces = namespaces)
- kf_loop_writer.Write(argv[1], argv[2])
+ if len(argv) != 3:
+ print "Expected .h file name and .cc file name"
+ else:
+ namespaces = ['frc971', 'control_loops', 'drivetrain']
+ kf_loop_writer = control_loop.ControlLoopWriter(
+ "DownEstimator", [estimator], namespaces=namespaces)
+ kf_loop_writer.Write(argv[1], argv[2])
+
if __name__ == '__main__':
- sys.exit(main(sys.argv))
+ sys.exit(main(sys.argv))
diff --git a/frc971/control_loops/python/drivetrain.py b/frc971/control_loops/python/drivetrain.py
index c59cbab..1da85bc 100644
--- a/frc971/control_loops/python/drivetrain.py
+++ b/frc971/control_loops/python/drivetrain.py
@@ -7,7 +7,9 @@
from matplotlib import pylab
import glog
+
class DrivetrainParams(object):
+
def __init__(self,
J,
mass,
@@ -107,6 +109,7 @@
class Drivetrain(control_loop.ControlLoop):
+
def __init__(self,
drivetrain_params,
name="Drivetrain",
@@ -190,8 +193,8 @@
# Resistance of the motor, divided by the number of motors.
# Motor velocity constant
- self.Kv = (self.free_speed /
- (12.0 - self.resistance * self.free_current))
+ self.Kv = (
+ self.free_speed / (12.0 - self.resistance * self.free_current))
# Torque constant
self.Kt = self.stall_torque / self.stall_current
@@ -215,14 +218,11 @@
# X will be of the format
# [[positionl], [velocityl], [positionr], velocityr]]
self.A_continuous = numpy.matrix(
- [[0, 1, 0, 0],
- [0, -self.mspl * self.tcl, 0, -self.msnr * self.tcr],
- [0, 0, 0, 1],
- [0, -self.msnl * self.tcl, 0, -self.mspr * self.tcr]])
+ [[0, 1, 0, 0], [0, -self.mspl * self.tcl, 0, -self.msnr * self.tcr],
+ [0, 0, 0, 1], [0, -self.msnl * self.tcl, 0,
+ -self.mspr * self.tcr]])
self.B_continuous = numpy.matrix(
- [[0, 0],
- [self.mspl * self.mpl, self.msnr * self.mpr],
- [0, 0],
+ [[0, 0], [self.mspl * self.mpl, self.msnr * self.mpr], [0, 0],
[self.msnl * self.mpl, self.mspr * self.mpr]])
self.C = numpy.matrix([[1, 0, 0, 0], [0, 0, 1, 0]])
self.D = numpy.matrix([[0, 0], [0, 0]])
@@ -232,10 +232,10 @@
def BuildDrivetrainController(self, q_pos, q_vel):
# Tune the LQR controller
- self.Q = numpy.matrix([[(1.0 / (q_pos**2.0)), 0.0, 0.0,
- 0.0], [0.0, (1.0 / (q_vel**2.0)), 0.0, 0.0],
- [0.0, 0.0, (1.0 / (q_pos**2.0)),
- 0.0], [0.0, 0.0, 0.0, (1.0 / (q_vel**2.0))]])
+ self.Q = numpy.matrix([[(1.0 / (q_pos**2.0)), 0.0, 0.0, 0.0],
+ [0.0, (1.0 / (q_vel**2.0)), 0.0, 0.0],
+ [0.0, 0.0, (1.0 / (q_pos**2.0)), 0.0],
+ [0.0, 0.0, 0.0, (1.0 / (q_vel**2.0))]])
self.R = numpy.matrix([[(1.0 / (12.0**2.0)), 0.0],
[0.0, (1.0 / (12.0**2.0))]])
@@ -254,6 +254,7 @@
class KFDrivetrain(Drivetrain):
+
def __init__(self,
drivetrain_params,
name="KFDrivetrain",
@@ -291,9 +292,10 @@
self.A_continuous[0:4, 0:4] = self.unaugmented_A_continuous
if self.force:
- self.A_continuous[0:4, 4:6] = numpy.matrix(
- [[0.0, 0.0], [self.mspl, self.msnl], [0.0, 0.0],
- [self.msnr, self.mspr]])
+ self.A_continuous[0:4, 4:6] = numpy.matrix([[0.0, 0.0],
+ [self.mspl, self.msnl],
+ [0.0, 0.0],
+ [self.msnr, self.mspr]])
q_voltage = drivetrain_params.kf_q_voltage * self.mpl
else:
self.A_continuous[0:4, 4:6] = self.unaugmented_B_continuous
@@ -307,28 +309,31 @@
self.B_continuous, self.dt)
if self.has_imu:
- self.C = numpy.matrix(
- [[1, 0, 0, 0, 0, 0, 0], [0, 0, 1, 0, 0, 0, 0], [
- 0, -0.5 / drivetrain_params.robot_radius, 0,
- 0.5 / drivetrain_params.robot_radius, 0, 0, 0
- ], [0, 0, 0, 0, 0, 0, 0]])
+ self.C = numpy.matrix([[1, 0, 0, 0, 0, 0, 0], [0, 0, 1, 0, 0, 0, 0],
+ [
+ 0, -0.5 / drivetrain_params.robot_radius,
+ 0, 0.5 / drivetrain_params.robot_radius,
+ 0, 0, 0
+ ], [0, 0, 0, 0, 0, 0, 0]])
gravity = 9.8
self.C[3, 0:6] = 0.5 * (
- self.A_continuous[1, 0:6] + self.A_continuous[3, 0:6]
- ) / gravity
+ self.A_continuous[1, 0:6] + self.A_continuous[3, 0:6]) / gravity
- self.D = numpy.matrix([[0, 0], [0, 0], [0, 0], [
- 0.5 *
- (self.B_continuous[1, 0] + self.B_continuous[3, 0]) / gravity,
- 0.5 *
- (self.B_continuous[1, 1] + self.B_continuous[3, 1]) / gravity
- ]])
+ self.D = numpy.matrix(
+ [[0, 0], [0, 0], [0, 0],
+ [
+ 0.5 * (self.B_continuous[1, 0] + self.B_continuous[3, 0]) /
+ gravity,
+ 0.5 * (self.B_continuous[1, 1] + self.B_continuous[3, 1]) /
+ gravity
+ ]])
else:
- self.C = numpy.matrix(
- [[1, 0, 0, 0, 0, 0, 0], [0, 0, 1, 0, 0, 0, 0], [
- 0, -0.5 / drivetrain_params.robot_radius, 0,
- 0.5 / drivetrain_params.robot_radius, 0, 0, 0
- ]])
+ self.C = numpy.matrix([[1, 0, 0, 0, 0, 0, 0], [0, 0, 1, 0, 0, 0, 0],
+ [
+ 0, -0.5 / drivetrain_params.robot_radius,
+ 0, 0.5 / drivetrain_params.robot_radius,
+ 0, 0, 0
+ ]])
self.D = numpy.matrix([[0, 0], [0, 0], [0, 0]])
@@ -337,12 +342,12 @@
q_encoder_uncertainty = 2.00
self.Q = numpy.matrix(
- [[(q_pos**2.0), 0.0, 0.0, 0.0, 0.0, 0.0,
- 0.0], [0.0, (q_vel**2.0), 0.0, 0.0, 0.0, 0.0,
- 0.0], [0.0, 0.0, (q_pos**2.0), 0.0, 0.0, 0.0,
- 0.0], [0.0, 0.0, 0.0, (q_vel**2.0), 0.0, 0.0, 0.0],
- [0.0, 0.0, 0.0, 0.0, (q_voltage**2.0), 0.0,
- 0.0], [0.0, 0.0, 0.0, 0.0, 0.0, (q_voltage**2.0), 0.0],
+ [[(q_pos**2.0), 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
+ [0.0, (q_vel**2.0), 0.0, 0.0, 0.0, 0.0, 0.0],
+ [0.0, 0.0, (q_pos**2.0), 0.0, 0.0, 0.0, 0.0],
+ [0.0, 0.0, 0.0, (q_vel**2.0), 0.0, 0.0, 0.0],
+ [0.0, 0.0, 0.0, 0.0, (q_voltage**2.0), 0.0, 0.0],
+ [0.0, 0.0, 0.0, 0.0, 0.0, (q_voltage**2.0), 0.0],
[0.0, 0.0, 0.0, 0.0, 0.0, 0.0, (q_encoder_uncertainty**2.0)]])
r_pos = 0.0001
@@ -365,7 +370,8 @@
# If we don't have an IMU, pad various matrices with zeros so that
# we can still have 4 measurement outputs.
if not self.has_imu:
- self.KalmanGain = numpy.hstack((self.KalmanGain, numpy.matrix(numpy.zeros((7, 1)))))
+ self.KalmanGain = numpy.hstack((self.KalmanGain,
+ numpy.matrix(numpy.zeros((7, 1)))))
self.C = numpy.vstack((self.C, numpy.matrix(numpy.zeros((1, 7)))))
self.D = numpy.vstack((self.D, numpy.matrix(numpy.zeros((1, 2)))))
Rtmp = numpy.zeros((4, 4))
@@ -401,8 +407,11 @@
self.InitializeState()
-def WriteDrivetrain(drivetrain_files, kf_drivetrain_files, year_namespace,
- drivetrain_params, scalar_type='double'):
+def WriteDrivetrain(drivetrain_files,
+ kf_drivetrain_files,
+ year_namespace,
+ drivetrain_params,
+ scalar_type='double'):
# Write the generated constants out to a file.
drivetrain_low_low = Drivetrain(
@@ -569,8 +578,7 @@
pylab.plot(range(200), close_loop_left, label='left position')
pylab.plot(range(200), close_loop_right, label='right position')
pylab.suptitle(
- 'Angular Move\nLeft position going to -1 and right position going to 1'
- )
+ 'Angular Move\nLeft position going to -1 and right position going to 1')
pylab.legend(loc='center right')
pylab.show()
diff --git a/frc971/control_loops/python/haptic_wheel.py b/frc971/control_loops/python/haptic_wheel.py
index 6c88e15..088b204 100755
--- a/frc971/control_loops/python/haptic_wheel.py
+++ b/frc971/control_loops/python/haptic_wheel.py
@@ -15,392 +15,443 @@
gflags.DEFINE_bool('plot', False, 'If true, plot the loop response.')
gflags.DEFINE_string('data', None, 'If defined, plot the provided CAN data')
-gflags.DEFINE_bool('rerun_kf', False, 'If true, rerun the KF. The torque in the data file will be interpreted as the commanded current.')
+gflags.DEFINE_bool(
+ 'rerun_kf', False,
+ 'If true, rerun the KF. The torque in the data file will be interpreted as the commanded current.'
+)
+
class SystemParams(object):
- def __init__(self, J, G, kP, kD, kCompensationTimeconstant, q_pos, q_vel,
- q_torque, current_limit):
- self.J = J
- self.G = G
- self.q_pos = q_pos
- self.q_vel = q_vel
- self.q_torque = q_torque
- self.kP = kP
- self.kD = kD
- self.kCompensationTimeconstant = kCompensationTimeconstant
- self.r_pos = 0.001
- self.current_limit = current_limit
- #[15.0, 0.25],
- #[10.0, 0.2],
- #[5.0, 0.13],
- #[3.0, 0.10],
- #[2.0, 0.08],
- #[1.0, 0.06],
- #[0.5, 0.05],
- #[0.25, 0.025],
+ def __init__(self, J, G, kP, kD, kCompensationTimeconstant, q_pos, q_vel,
+ q_torque, current_limit):
+ self.J = J
+ self.G = G
+ self.q_pos = q_pos
+ self.q_vel = q_vel
+ self.q_torque = q_torque
+ self.kP = kP
+ self.kD = kD
+ self.kCompensationTimeconstant = kCompensationTimeconstant
+ self.r_pos = 0.001
+ self.current_limit = current_limit
-kWheel = SystemParams(J=0.0008,
- G=(1.25 + 0.02) / 0.35,
- q_pos=0.001,
- q_vel=0.20,
- q_torque=0.005,
- kP=7.0,
- kD=0.0,
- kCompensationTimeconstant=0.95,
- current_limit=4.5)
-kTrigger = SystemParams(J=0.00025,
- G=(0.925 * 2.0 + 0.02) / 0.35,
- q_pos=0.001,
- q_vel=0.1,
- q_torque=0.005,
- kP=120.0,
- kD=1.8,
- kCompensationTimeconstant=0.95,
- current_limit=3.0)
+ #[15.0, 0.25],
+ #[10.0, 0.2],
+ #[5.0, 0.13],
+ #[3.0, 0.10],
+ #[2.0, 0.08],
+ #[1.0, 0.06],
+ #[0.5, 0.05],
+ #[0.25, 0.025],
+
+
+kWheel = SystemParams(
+ J=0.0008,
+ G=(1.25 + 0.02) / 0.35,
+ q_pos=0.001,
+ q_vel=0.20,
+ q_torque=0.005,
+ kP=7.0,
+ kD=0.0,
+ kCompensationTimeconstant=0.95,
+ current_limit=4.5)
+kTrigger = SystemParams(
+ J=0.00025,
+ G=(0.925 * 2.0 + 0.02) / 0.35,
+ q_pos=0.001,
+ q_vel=0.1,
+ q_torque=0.005,
+ kP=120.0,
+ kD=1.8,
+ kCompensationTimeconstant=0.95,
+ current_limit=3.0)
+
class HapticInput(control_loop.ControlLoop):
- def __init__(self, params=None, name='HapticInput'):
- # The defaults are for the steering wheel.
- super(HapticInput, self).__init__(name)
- motor = self.motor = control_loop.MN3510()
- # Moment of inertia of the wheel in kg m^2
- self.J = params.J
+ def __init__(self, params=None, name='HapticInput'):
+ # The defaults are for the steering wheel.
+ super(HapticInput, self).__init__(name)
+ motor = self.motor = control_loop.MN3510()
- # Control loop time step
- self.dt = 0.001
+ # Moment of inertia of the wheel in kg m^2
+ self.J = params.J
- # Gear ratio from the motor to the input.
- self.G = params.G
+ # Control loop time step
+ self.dt = 0.001
- self.A_continuous = numpy.matrix(numpy.zeros((2, 2)))
- self.A_continuous[1, 1] = 0
- self.A_continuous[0, 1] = 1
+ # Gear ratio from the motor to the input.
+ self.G = params.G
- self.B_continuous = numpy.matrix(numpy.zeros((2, 1)))
- self.B_continuous[1, 0] = motor.Kt * self.G / self.J
+ self.A_continuous = numpy.matrix(numpy.zeros((2, 2)))
+ self.A_continuous[1, 1] = 0
+ self.A_continuous[0, 1] = 1
- # State feedback matrices
- # [position, angular velocity]
- self.C = numpy.matrix([[1.0, 0.0]])
- self.D = numpy.matrix([[0.0]])
+ self.B_continuous = numpy.matrix(numpy.zeros((2, 1)))
+ self.B_continuous[1, 0] = motor.Kt * self.G / self.J
- self.A, self.B = self.ContinuousToDiscrete(
- self.A_continuous, self.B_continuous, self.dt)
+ # State feedback matrices
+ # [position, angular velocity]
+ self.C = numpy.matrix([[1.0, 0.0]])
+ self.D = numpy.matrix([[0.0]])
- self.U_max = numpy.matrix([[2.5]])
- self.U_min = numpy.matrix([[-2.5]])
+ self.A, self.B = self.ContinuousToDiscrete(self.A_continuous,
+ self.B_continuous, self.dt)
- self.L = numpy.matrix([[0.0], [0.0]])
- self.K = numpy.matrix([[0.0, 0.0]])
+ self.U_max = numpy.matrix([[2.5]])
+ self.U_min = numpy.matrix([[-2.5]])
- self.InitializeState()
+ self.L = numpy.matrix([[0.0], [0.0]])
+ self.K = numpy.matrix([[0.0, 0.0]])
+
+ self.InitializeState()
class IntegralHapticInput(HapticInput):
- def __init__(self, params=None, name="IntegralHapticInput"):
- super(IntegralHapticInput, self).__init__(name=name, params=params)
- self.A_continuous_unaugmented = self.A_continuous
- self.B_continuous_unaugmented = self.B_continuous
+ def __init__(self, params=None, name="IntegralHapticInput"):
+ super(IntegralHapticInput, self).__init__(name=name, params=params)
- self.A_continuous = numpy.matrix(numpy.zeros((3, 3)))
- self.A_continuous[0:2, 0:2] = self.A_continuous_unaugmented
- self.A_continuous[1, 2] = (1 / self.J)
+ self.A_continuous_unaugmented = self.A_continuous
+ self.B_continuous_unaugmented = self.B_continuous
- self.B_continuous = numpy.matrix(numpy.zeros((3, 1)))
- self.B_continuous[0:2, 0] = self.B_continuous_unaugmented
+ self.A_continuous = numpy.matrix(numpy.zeros((3, 3)))
+ self.A_continuous[0:2, 0:2] = self.A_continuous_unaugmented
+ self.A_continuous[1, 2] = (1 / self.J)
- self.C_unaugmented = self.C
- self.C = numpy.matrix(numpy.zeros((1, 3)))
- self.C[0:1, 0:2] = self.C_unaugmented
+ self.B_continuous = numpy.matrix(numpy.zeros((3, 1)))
+ self.B_continuous[0:2, 0] = self.B_continuous_unaugmented
- self.A, self.B = self.ContinuousToDiscrete(
- self.A_continuous, self.B_continuous, self.dt)
+ self.C_unaugmented = self.C
+ self.C = numpy.matrix(numpy.zeros((1, 3)))
+ self.C[0:1, 0:2] = self.C_unaugmented
- self.Q = numpy.matrix([[(params.q_pos ** 2.0), 0.0, 0.0],
- [0.0, (params.q_vel ** 2.0), 0.0],
- [0.0, 0.0, (params.q_torque ** 2.0)]])
+ self.A, self.B = self.ContinuousToDiscrete(self.A_continuous,
+ self.B_continuous, self.dt)
- self.R = numpy.matrix([[(params.r_pos ** 2.0)]])
+ self.Q = numpy.matrix([[(params.q_pos**2.0), 0.0, 0.0],
+ [0.0, (params.q_vel**2.0), 0.0],
+ [0.0, 0.0, (params.q_torque**2.0)]])
- self.KalmanGain, self.Q_steady = controls.kalman(
- A=self.A, B=self.B, C=self.C, Q=self.Q, R=self.R)
- self.L = self.A * self.KalmanGain
+ self.R = numpy.matrix([[(params.r_pos**2.0)]])
- self.K_unaugmented = self.K
- self.K = numpy.matrix(numpy.zeros((1, 3)))
- self.K[0, 0:2] = self.K_unaugmented
- self.K[0, 2] = 1.0 / (self.motor.Kt / (self.motor.resistance))
+ self.KalmanGain, self.Q_steady = controls.kalman(
+ A=self.A, B=self.B, C=self.C, Q=self.Q, R=self.R)
+ self.L = self.A * self.KalmanGain
- self.InitializeState()
+ self.K_unaugmented = self.K
+ self.K = numpy.matrix(numpy.zeros((1, 3)))
+ self.K[0, 0:2] = self.K_unaugmented
+ self.K[0, 2] = 1.0 / (self.motor.Kt / (self.motor.resistance))
+
+ self.InitializeState()
+
def ReadCan(filename):
- """Reads the candump in filename and returns the 4 fields."""
- trigger = []
- trigger_velocity = []
- trigger_torque = []
- trigger_current = []
- wheel = []
- wheel_velocity = []
- wheel_torque = []
- wheel_current = []
+ """Reads the candump in filename and returns the 4 fields."""
+ trigger = []
+ trigger_velocity = []
+ trigger_torque = []
+ trigger_current = []
+ wheel = []
+ wheel_velocity = []
+ wheel_torque = []
+ wheel_current = []
- trigger_request_time = [0.0]
- trigger_request_current = [0.0]
- wheel_request_time = [0.0]
- wheel_request_current = [0.0]
+ trigger_request_time = [0.0]
+ trigger_request_current = [0.0]
+ wheel_request_time = [0.0]
+ wheel_request_current = [0.0]
- with open(filename, 'r') as fd:
- for line in fd:
- data = line.split()
- can_id = int(data[1], 16)
- if can_id == 0:
- data = [int(d, 16) for d in data[3:]]
- trigger.append(((data[0] + (data[1] << 8)) - 32768) / 32768.0)
- trigger_velocity.append(((data[2] + (data[3] << 8)) - 32768) / 32768.0)
- trigger_torque.append(((data[4] + (data[5] << 8)) - 32768) / 32768.0)
- trigger_current.append(((data[6] + ((data[7] & 0x3f) << 8)) - 8192) / 8192.0)
- elif can_id == 1:
- data = [int(d, 16) for d in data[3:]]
- wheel.append(((data[0] + (data[1] << 8)) - 32768) / 32768.0)
- wheel_velocity.append(((data[2] + (data[3] << 8)) - 32768) / 32768.0)
- wheel_torque.append(((data[4] + (data[5] << 8)) - 32768) / 32768.0)
- wheel_current.append(((data[6] + ((data[7] & 0x3f) << 8)) - 8192) / 8192.0)
- elif can_id == 2:
- data = [int(d, 16) for d in data[3:]]
- trigger_request_current.append(((data[4] + (data[5] << 8)) - 32768) / 32768.0)
- trigger_request_time.append(len(trigger) * 0.001)
- elif can_id == 3:
- data = [int(d, 16) for d in data[3:]]
- wheel_request_current.append(((data[4] + (data[5] << 8)) - 32768) / 32768.0)
- wheel_request_time.append(len(wheel) * 0.001)
+ with open(filename, 'r') as fd:
+ for line in fd:
+ data = line.split()
+ can_id = int(data[1], 16)
+ if can_id == 0:
+ data = [int(d, 16) for d in data[3:]]
+ trigger.append(((data[0] + (data[1] << 8)) - 32768) / 32768.0)
+ trigger_velocity.append(
+ ((data[2] + (data[3] << 8)) - 32768) / 32768.0)
+ trigger_torque.append(
+ ((data[4] + (data[5] << 8)) - 32768) / 32768.0)
+ trigger_current.append(
+ ((data[6] + ((data[7] & 0x3f) << 8)) - 8192) / 8192.0)
+ elif can_id == 1:
+ data = [int(d, 16) for d in data[3:]]
+ wheel.append(((data[0] + (data[1] << 8)) - 32768) / 32768.0)
+ wheel_velocity.append(
+ ((data[2] + (data[3] << 8)) - 32768) / 32768.0)
+ wheel_torque.append(
+ ((data[4] + (data[5] << 8)) - 32768) / 32768.0)
+ wheel_current.append(
+ ((data[6] + ((data[7] & 0x3f) << 8)) - 8192) / 8192.0)
+ elif can_id == 2:
+ data = [int(d, 16) for d in data[3:]]
+ trigger_request_current.append(
+ ((data[4] + (data[5] << 8)) - 32768) / 32768.0)
+ trigger_request_time.append(len(trigger) * 0.001)
+ elif can_id == 3:
+ data = [int(d, 16) for d in data[3:]]
+ wheel_request_current.append(
+ ((data[4] + (data[5] << 8)) - 32768) / 32768.0)
+ wheel_request_time.append(len(wheel) * 0.001)
- trigger_data_time = numpy.arange(0, len(trigger)) * 0.001
- wheel_data_time = numpy.arange(0, len(wheel)) * 0.001
+ trigger_data_time = numpy.arange(0, len(trigger)) * 0.001
+ wheel_data_time = numpy.arange(0, len(wheel)) * 0.001
- # Extend out the data in the interpolation table.
- trigger_request_time.append(trigger_data_time[-1])
- trigger_request_current.append(trigger_request_current[-1])
- wheel_request_time.append(wheel_data_time[-1])
- wheel_request_current.append(wheel_request_current[-1])
+ # Extend out the data in the interpolation table.
+ trigger_request_time.append(trigger_data_time[-1])
+ trigger_request_current.append(trigger_request_current[-1])
+ wheel_request_time.append(wheel_data_time[-1])
+ wheel_request_current.append(wheel_request_current[-1])
- return (trigger_data_time, wheel_data_time, trigger, wheel, trigger_velocity,
- wheel_velocity, trigger_torque, wheel_torque, trigger_current,
- wheel_current, trigger_request_time, trigger_request_current,
- wheel_request_time, wheel_request_current)
+ return (trigger_data_time, wheel_data_time, trigger, wheel,
+ trigger_velocity, wheel_velocity, trigger_torque, wheel_torque,
+ trigger_current, wheel_current, trigger_request_time,
+ trigger_request_current, wheel_request_time, wheel_request_current)
-def rerun_and_plot_kf(data_time, data_radians, data_current, data_request_current,
- params, run_correct=True):
- kf_velocity = []
- dt_velocity = []
- kf_position = []
- adjusted_position = []
- last_angle = None
- haptic_observer = IntegralHapticInput(params=params)
- # Parameter sweep J.
- num_kf = 1
- min_J = max_J = params.J
+def rerun_and_plot_kf(data_time,
+ data_radians,
+ data_current,
+ data_request_current,
+ params,
+ run_correct=True):
+ kf_velocity = []
+ dt_velocity = []
+ kf_position = []
+ adjusted_position = []
+ last_angle = None
+ haptic_observer = IntegralHapticInput(params=params)
- # J = 0.0022
- #num_kf = 15
- #min_J = min_J / 2.0
- #max_J = max_J * 2.0
- initial_velocity = (data_radians[1] - data_radians[0]) * 1000.0
+ # Parameter sweep J.
+ num_kf = 1
+ min_J = max_J = params.J
- def DupParamsWithJ(params, J):
- p = copy.copy(params)
- p.J = J
- return p
- haptic_observers = [IntegralHapticInput(params=DupParamsWithJ(params, j)) for j in
- numpy.logspace(numpy.log10(min_J),
- numpy.log10(max_J), num=num_kf)]
- # Initialize all the KF's.
- haptic_observer.X_hat[1, 0] = initial_velocity
- haptic_observer.X_hat[0, 0] = data_radians[0]
- for observer in haptic_observers:
- observer.X_hat[1, 0] = initial_velocity
- observer.X_hat[0, 0] = data_radians[0]
+ # J = 0.0022
+ #num_kf = 15
+ #min_J = min_J / 2.0
+ #max_J = max_J * 2.0
+ initial_velocity = (data_radians[1] - data_radians[0]) * 1000.0
- last_request_current = data_request_current[0]
- kf_torques = [[] for i in xrange(num_kf)]
- for angle, current, request_current in zip(data_radians, data_current,
- data_request_current):
- # Predict and correct all the parameter swept observers.
- for i, observer in enumerate(haptic_observers):
- observer.Y = numpy.matrix([[angle]])
- if run_correct:
- observer.CorrectObserver(numpy.matrix([[current]]))
- kf_torques[i].append(-observer.X_hat[2, 0])
- observer.PredictObserver(numpy.matrix([[current]]))
- observer.PredictObserver(numpy.matrix([[current]]))
+ def DupParamsWithJ(params, J):
+ p = copy.copy(params)
+ p.J = J
+ return p
- # Predict and correct the main observer.
- haptic_observer.Y = numpy.matrix([[angle]])
- if run_correct:
- haptic_observer.CorrectObserver(numpy.matrix([[current]]))
- kf_position.append(haptic_observer.X_hat[0, 0])
- adjusted_position.append(kf_position[-1] - last_request_current / params.kP)
- last_request_current = last_request_current * params.kCompensationTimeconstant + request_current * (1.0 - params.kCompensationTimeconstant)
- kf_velocity.append(haptic_observer.X_hat[1, 0])
- if last_angle is None:
- last_angle = angle
- dt_velocity.append((angle - last_angle) / 0.001)
+ haptic_observers = [
+ IntegralHapticInput(params=DupParamsWithJ(params, j))
+ for j in numpy.logspace(
+ numpy.log10(min_J), numpy.log10(max_J), num=num_kf)
+ ]
+ # Initialize all the KF's.
+ haptic_observer.X_hat[1, 0] = initial_velocity
+ haptic_observer.X_hat[0, 0] = data_radians[0]
+ for observer in haptic_observers:
+ observer.X_hat[1, 0] = initial_velocity
+ observer.X_hat[0, 0] = data_radians[0]
- haptic_observer.PredictObserver(numpy.matrix([[current]]))
- last_angle = angle
+ last_request_current = data_request_current[0]
+ kf_torques = [[] for i in xrange(num_kf)]
+ for angle, current, request_current in zip(data_radians, data_current,
+ data_request_current):
+ # Predict and correct all the parameter swept observers.
+ for i, observer in enumerate(haptic_observers):
+ observer.Y = numpy.matrix([[angle]])
+ if run_correct:
+ observer.CorrectObserver(numpy.matrix([[current]]))
+ kf_torques[i].append(-observer.X_hat[2, 0])
+ observer.PredictObserver(numpy.matrix([[current]]))
+ observer.PredictObserver(numpy.matrix([[current]]))
- # Plot the wheel observers.
- fig, ax1 = pylab.subplots()
- ax1.plot(data_time, data_radians, '.', label='wheel')
- ax1.plot(data_time, dt_velocity, '.', label='dt_velocity')
- ax1.plot(data_time, kf_velocity, '.', label='kf_velocity')
- ax1.plot(data_time, kf_position, '.', label='kf_position')
- ax1.plot(data_time, adjusted_position, '.', label='adjusted_position')
+ # Predict and correct the main observer.
+ haptic_observer.Y = numpy.matrix([[angle]])
+ if run_correct:
+ haptic_observer.CorrectObserver(numpy.matrix([[current]]))
+ kf_position.append(haptic_observer.X_hat[0, 0])
+ adjusted_position.append(kf_position[-1] -
+ last_request_current / params.kP)
+ last_request_current = last_request_current * params.kCompensationTimeconstant + request_current * (
+ 1.0 - params.kCompensationTimeconstant)
+ kf_velocity.append(haptic_observer.X_hat[1, 0])
+ if last_angle is None:
+ last_angle = angle
+ dt_velocity.append((angle - last_angle) / 0.001)
- ax2 = ax1.twinx()
- ax2.plot(data_time, data_current, label='data_current')
- ax2.plot(data_time, data_request_current, label='request_current')
+ haptic_observer.PredictObserver(numpy.matrix([[current]]))
+ last_angle = angle
- for i, kf_torque in enumerate(kf_torques):
- ax2.plot(data_time, kf_torque,
- label='-kf_torque[%f]' % haptic_observers[i].J)
- fig.tight_layout()
- ax1.legend(loc=3)
- ax2.legend(loc=4)
+ # Plot the wheel observers.
+ fig, ax1 = pylab.subplots()
+ ax1.plot(data_time, data_radians, '.', label='wheel')
+ ax1.plot(data_time, dt_velocity, '.', label='dt_velocity')
+ ax1.plot(data_time, kf_velocity, '.', label='kf_velocity')
+ ax1.plot(data_time, kf_position, '.', label='kf_position')
+ ax1.plot(data_time, adjusted_position, '.', label='adjusted_position')
-def plot_input(data_time, data_radians, data_velocity, data_torque,
- data_current, params, run_correct=True):
- dt_velocity = []
- last_angle = None
- initial_velocity = (data_radians[1] - data_radians[0]) * 1000.0
+ ax2 = ax1.twinx()
+ ax2.plot(data_time, data_current, label='data_current')
+ ax2.plot(data_time, data_request_current, label='request_current')
- for angle in data_radians:
- if last_angle is None:
- last_angle = angle
- dt_velocity.append((angle - last_angle) / 0.001)
+ for i, kf_torque in enumerate(kf_torques):
+ ax2.plot(
+ data_time,
+ kf_torque,
+ label='-kf_torque[%f]' % haptic_observers[i].J)
+ fig.tight_layout()
+ ax1.legend(loc=3)
+ ax2.legend(loc=4)
- last_angle = angle
- # Plot the wheel observers.
- fig, ax1 = pylab.subplots()
- ax1.plot(data_time, data_radians, '.', label='angle')
- ax1.plot(data_time, data_velocity, '-', label='velocity')
- ax1.plot(data_time, dt_velocity, '.', label='dt_velocity')
+def plot_input(data_time,
+ data_radians,
+ data_velocity,
+ data_torque,
+ data_current,
+ params,
+ run_correct=True):
+ dt_velocity = []
+ last_angle = None
+ initial_velocity = (data_radians[1] - data_radians[0]) * 1000.0
- ax2 = ax1.twinx()
- ax2.plot(data_time, data_torque, label='data_torque')
- ax2.plot(data_time, data_current, label='data_current')
- fig.tight_layout()
- ax1.legend(loc=3)
- ax2.legend(loc=4)
+ for angle in data_radians:
+ if last_angle is None:
+ last_angle = angle
+ dt_velocity.append((angle - last_angle) / 0.001)
+
+ last_angle = angle
+
+ # Plot the wheel observers.
+ fig, ax1 = pylab.subplots()
+ ax1.plot(data_time, data_radians, '.', label='angle')
+ ax1.plot(data_time, data_velocity, '-', label='velocity')
+ ax1.plot(data_time, dt_velocity, '.', label='dt_velocity')
+
+ ax2 = ax1.twinx()
+ ax2.plot(data_time, data_torque, label='data_torque')
+ ax2.plot(data_time, data_current, label='data_current')
+ fig.tight_layout()
+ ax1.legend(loc=3)
+ ax2.legend(loc=4)
+
def main(argv):
- if FLAGS.plot:
- if FLAGS.data is None:
- haptic_wheel = HapticInput()
- haptic_wheel_controller = IntegralHapticInput()
- observer_haptic_wheel = IntegralHapticInput()
- observer_haptic_wheel.X_hat[2, 0] = 0.01
+ if FLAGS.plot:
+ if FLAGS.data is None:
+ haptic_wheel = HapticInput()
+ haptic_wheel_controller = IntegralHapticInput()
+ observer_haptic_wheel = IntegralHapticInput()
+ observer_haptic_wheel.X_hat[2, 0] = 0.01
- R = numpy.matrix([[0.0], [0.0], [0.0]])
+ R = numpy.matrix([[0.0], [0.0], [0.0]])
- control_loop.TestSingleIntegralAxisSquareWave(
- R, 1.0, haptic_wheel, haptic_wheel_controller, observer_haptic_wheel)
+ control_loop.TestSingleIntegralAxisSquareWave(
+ R, 1.0, haptic_wheel, haptic_wheel_controller,
+ observer_haptic_wheel)
+ else:
+ # Read the CAN trace in.
+ trigger_data_time, wheel_data_time, trigger, wheel, trigger_velocity, \
+ wheel_velocity, trigger_torque, wheel_torque, trigger_current, \
+ wheel_current, trigger_request_time, trigger_request_current, \
+ wheel_request_time, wheel_request_current = ReadCan(FLAGS.data)
+
+ wheel_radians = [w * numpy.pi * (338.16 / 360.0) for w in wheel]
+ wheel_velocity = [w * 50.0 for w in wheel_velocity]
+ wheel_torque = [w / 2.0 for w in wheel_torque]
+ wheel_current = [w * 10.0 for w in wheel_current]
+ wheel_request_current = [w * 2.0 for w in wheel_request_current]
+ resampled_wheel_request_current = scipy.interpolate.interp1d(
+ wheel_request_time, wheel_request_current,
+ kind="zero")(wheel_data_time)
+
+ trigger_radians = [t * numpy.pi * (45.0 / 360.0) for t in trigger]
+ trigger_velocity = [t * 50.0 for t in trigger_velocity]
+ trigger_torque = [t / 2.0 for t in trigger_torque]
+ trigger_current = [t * 10.0 for t in trigger_current]
+ trigger_request_current = [t * 2.0 for t in trigger_request_current]
+ resampled_trigger_request_current = scipy.interpolate.interp1d(
+ trigger_request_time, trigger_request_current,
+ kind="zero")(trigger_data_time)
+
+ if FLAGS.rerun_kf:
+ rerun_and_plot_kf(
+ trigger_data_time,
+ trigger_radians,
+ trigger_current,
+ resampled_trigger_request_current,
+ kTrigger,
+ run_correct=True)
+ rerun_and_plot_kf(
+ wheel_data_time,
+ wheel_radians,
+ wheel_current,
+ resampled_wheel_request_current,
+ kWheel,
+ run_correct=True)
+ else:
+ plot_input(trigger_data_time, trigger_radians, trigger_velocity,
+ trigger_torque, trigger_current, kTrigger)
+ plot_input(wheel_data_time, wheel_radians, wheel_velocity,
+ wheel_torque, wheel_current, kWheel)
+ pylab.show()
+
+ return
+
+ if len(argv) != 9:
+ glog.fatal('Expected .h file name and .cc file name')
else:
- # Read the CAN trace in.
- trigger_data_time, wheel_data_time, trigger, wheel, trigger_velocity, \
- wheel_velocity, trigger_torque, wheel_torque, trigger_current, \
- wheel_current, trigger_request_time, trigger_request_current, \
- wheel_request_time, wheel_request_current = ReadCan(FLAGS.data)
+ namespaces = ['frc971', 'control_loops', 'drivetrain']
+ for name, params, filenames in [('HapticWheel', kWheel, argv[1:5]),
+ ('HapticTrigger', kTrigger, argv[5:9])]:
+ haptic_input = HapticInput(params=params, name=name)
+ loop_writer = control_loop.ControlLoopWriter(
+ name, [haptic_input],
+ namespaces=namespaces,
+ scalar_type='float')
+ loop_writer.Write(filenames[0], filenames[1])
- wheel_radians = [w * numpy.pi * (338.16 / 360.0) for w in wheel]
- wheel_velocity = [w * 50.0 for w in wheel_velocity]
- wheel_torque = [w / 2.0 for w in wheel_torque]
- wheel_current = [w * 10.0 for w in wheel_current]
- wheel_request_current = [w * 2.0 for w in wheel_request_current]
- resampled_wheel_request_current = scipy.interpolate.interp1d(
- wheel_request_time, wheel_request_current, kind="zero")(wheel_data_time)
+ integral_haptic_input = IntegralHapticInput(
+ params=params, name='Integral' + name)
+ integral_loop_writer = control_loop.ControlLoopWriter(
+ 'Integral' + name, [integral_haptic_input],
+ namespaces=namespaces,
+ scalar_type='float')
- trigger_radians = [t * numpy.pi * (45.0 / 360.0) for t in trigger]
- trigger_velocity = [t * 50.0 for t in trigger_velocity]
- trigger_torque = [t / 2.0 for t in trigger_torque]
- trigger_current = [t * 10.0 for t in trigger_current]
- trigger_request_current = [t * 2.0 for t in trigger_request_current]
- resampled_trigger_request_current = scipy.interpolate.interp1d(
- trigger_request_time, trigger_request_current, kind="zero")(trigger_data_time)
+ integral_loop_writer.AddConstant(
+ control_loop.Constant("k" + name + "Dt", "%f",
+ integral_haptic_input.dt))
+ integral_loop_writer.AddConstant(
+ control_loop.Constant("k" + name + "FreeCurrent", "%f",
+ integral_haptic_input.motor.free_current))
+ integral_loop_writer.AddConstant(
+ control_loop.Constant("k" + name + "StallTorque", "%f",
+ integral_haptic_input.motor.stall_torque))
+ integral_loop_writer.AddConstant(
+ control_loop.Constant("k" + name + "J", "%f",
+ integral_haptic_input.J))
+ integral_loop_writer.AddConstant(
+ control_loop.Constant("k" + name + "R", "%f",
+ integral_haptic_input.motor.resistance))
+ integral_loop_writer.AddConstant(
+ control_loop.Constant("k" + name + "T", "%f",
+ integral_haptic_input.motor.Kt))
+ integral_loop_writer.AddConstant(
+ control_loop.Constant("k" + name + "V", "%f",
+ integral_haptic_input.motor.Kv))
+ integral_loop_writer.AddConstant(
+ control_loop.Constant("k" + name + "P", "%f", params.kP))
+ integral_loop_writer.AddConstant(
+ control_loop.Constant("k" + name + "D", "%f", params.kD))
+ integral_loop_writer.AddConstant(
+ control_loop.Constant("k" + name + "G", "%f", params.G))
+ integral_loop_writer.AddConstant(
+ control_loop.Constant("k" + name + "CurrentLimit", "%f",
+ params.current_limit))
- if FLAGS.rerun_kf:
- rerun_and_plot_kf(trigger_data_time, trigger_radians, trigger_current,
- resampled_trigger_request_current, kTrigger, run_correct=True)
- rerun_and_plot_kf(wheel_data_time, wheel_radians, wheel_current,
- resampled_wheel_request_current, kWheel, run_correct=True)
- else:
- plot_input(trigger_data_time, trigger_radians, trigger_velocity,
- trigger_torque, trigger_current, kTrigger)
- plot_input(wheel_data_time, wheel_radians, wheel_velocity, wheel_torque,
- wheel_current, kWheel)
- pylab.show()
-
- return
-
- if len(argv) != 9:
- glog.fatal('Expected .h file name and .cc file name')
- else:
- namespaces = ['frc971', 'control_loops', 'drivetrain']
- for name, params, filenames in [('HapticWheel', kWheel, argv[1:5]),
- ('HapticTrigger', kTrigger, argv[5:9])]:
- haptic_input = HapticInput(params=params, name=name)
- loop_writer = control_loop.ControlLoopWriter(name, [haptic_input],
- namespaces=namespaces,
- scalar_type='float')
- loop_writer.Write(filenames[0], filenames[1])
-
- integral_haptic_input = IntegralHapticInput(params=params,
- name='Integral' + name)
- integral_loop_writer = control_loop.ControlLoopWriter(
- 'Integral' + name, [integral_haptic_input], namespaces=namespaces,
- scalar_type='float')
-
- integral_loop_writer.AddConstant(
- control_loop.Constant("k" + name + "Dt", "%f",
- integral_haptic_input.dt))
- integral_loop_writer.AddConstant(
- control_loop.Constant("k" + name + "FreeCurrent", "%f",
- integral_haptic_input.motor.free_current))
- integral_loop_writer.AddConstant(
- control_loop.Constant("k" + name + "StallTorque", "%f",
- integral_haptic_input.motor.stall_torque))
- integral_loop_writer.AddConstant(
- control_loop.Constant("k" + name + "J", "%f",
- integral_haptic_input.J))
- integral_loop_writer.AddConstant(
- control_loop.Constant("k" + name + "R", "%f",
- integral_haptic_input.motor.resistance))
- integral_loop_writer.AddConstant(
- control_loop.Constant("k" + name + "T", "%f",
- integral_haptic_input.motor.Kt))
- integral_loop_writer.AddConstant(
- control_loop.Constant("k" + name + "V", "%f",
- integral_haptic_input.motor.Kv))
- integral_loop_writer.AddConstant(
- control_loop.Constant("k" + name + "P", "%f",
- params.kP))
- integral_loop_writer.AddConstant(
- control_loop.Constant("k" + name + "D", "%f",
- params.kD))
- integral_loop_writer.AddConstant(
- control_loop.Constant("k" + name + "G", "%f",
- params.G))
- integral_loop_writer.AddConstant(
- control_loop.Constant("k" + name + "CurrentLimit", "%f",
- params.current_limit))
-
- integral_loop_writer.Write(filenames[2], filenames[3])
+ integral_loop_writer.Write(filenames[2], filenames[3])
if __name__ == '__main__':
- argv = FLAGS(sys.argv)
- sys.exit(main(argv))
+ argv = FLAGS(sys.argv)
+ sys.exit(main(argv))
diff --git a/frc971/control_loops/python/linear_system.py b/frc971/control_loops/python/linear_system.py
index e1f55cc..175fffa 100755
--- a/frc971/control_loops/python/linear_system.py
+++ b/frc971/control_loops/python/linear_system.py
@@ -9,6 +9,7 @@
class LinearSystemParams(object):
+
def __init__(self,
name,
motor,
@@ -37,6 +38,7 @@
class LinearSystem(control_loop.ControlLoop):
+
def __init__(self, params, name='LinearSystem'):
super(LinearSystem, self).__init__(name)
self.params = params
@@ -57,10 +59,9 @@
# State is [position, velocity]
# Input is [Voltage]
C1 = self.motor.Kt / (self.G * self.G * self.radius * self.radius *
- self.motor.resistance * self.mass *
- self.motor.Kv)
- C2 = self.motor.Kt / (self.G * self.radius * self.motor.resistance *
- self.mass)
+ self.motor.resistance * self.mass * self.motor.Kv)
+ C2 = self.motor.Kt / (
+ self.G * self.radius * self.motor.resistance * self.mass)
self.A_continuous = numpy.matrix([[0, 1], [0, -C1]])
@@ -79,9 +80,10 @@
glog.debug('Controllability of %d',
numpy.linalg.matrix_rank(controllability))
glog.debug('Mass: %f', self.mass)
- glog.debug('Stall force: %f', self.motor.stall_torque / self.G / self.radius)
- glog.debug('Stall acceleration: %f', self.motor.stall_torque / self.G /
- self.radius / self.mass)
+ glog.debug('Stall force: %f',
+ self.motor.stall_torque / self.G / self.radius)
+ glog.debug('Stall acceleration: %f',
+ self.motor.stall_torque / self.G / self.radius / self.mass)
glog.debug('Free speed is %f',
-self.B_continuous[1, 0] / self.A_continuous[1, 1] * 12.0)
@@ -122,6 +124,7 @@
class IntegralLinearSystem(LinearSystem):
+
def __init__(self, params, name='IntegralLinearSystem'):
super(IntegralLinearSystem, self).__init__(params, name=name)
@@ -142,10 +145,9 @@
self.A, self.B = self.ContinuousToDiscrete(self.A_continuous,
self.B_continuous, self.dt)
- self.Q = numpy.matrix(
- [[(self.params.kalman_q_pos**2.0), 0.0, 0.0],
- [0.0, (self.params.kalman_q_vel**2.0), 0.0],
- [0.0, 0.0, (self.params.kalman_q_voltage**2.0)]])
+ self.Q = numpy.matrix([[(self.params.kalman_q_pos**2.0), 0.0, 0.0],
+ [0.0, (self.params.kalman_q_vel**2.0), 0.0],
+ [0.0, 0.0, (self.params.kalman_q_voltage**2.0)]])
self.R = numpy.matrix([[(self.params.kalman_r_position**2.0)]])
@@ -203,8 +205,8 @@
vbat = 12.0
- goal = numpy.concatenate(
- (plant.X, numpy.matrix(numpy.zeros((1, 1)))), axis=0)
+ goal = numpy.concatenate((plant.X, numpy.matrix(numpy.zeros((1, 1)))),
+ axis=0)
profile = TrapezoidProfile(plant.dt)
profile.set_maximum_acceleration(max_acceleration)
@@ -263,8 +265,7 @@
goal = controller.A * goal + controller.B * ff_U
if U[0, 0] != U_uncapped[0, 0]:
- profile.MoveCurrentState(
- numpy.matrix([[goal[0, 0]], [goal[1, 0]]]))
+ profile.MoveCurrentState(numpy.matrix([[goal[0, 0]], [goal[1, 0]]]))
glog.debug('Time: %f', t_plot[-1])
glog.debug('goal_error %s', repr(end_goal - goal))
@@ -378,11 +379,11 @@
loop_writer = control_loop.ControlLoopWriter(
linear_system.name, [linear_system], namespaces=year_namespaces)
loop_writer.AddConstant(
- control_loop.Constant('kFreeSpeed', '%f', linear_system.motor.
- free_speed))
+ control_loop.Constant('kFreeSpeed', '%f',
+ linear_system.motor.free_speed))
loop_writer.AddConstant(
- control_loop.Constant('kOutputRatio', '%f', linear_system.G *
- linear_system.radius))
+ control_loop.Constant('kOutputRatio', '%f',
+ linear_system.G * linear_system.radius))
loop_writer.AddConstant(
control_loop.Constant('kRadius', '%f', linear_system.radius))
loop_writer.Write(plant_files[0], plant_files[1])
diff --git a/frc971/control_loops/python/polydrivetrain.py b/frc971/control_loops/python/polydrivetrain.py
index 91dba3c..06a182e 100644
--- a/frc971/control_loops/python/polydrivetrain.py
+++ b/frc971/control_loops/python/polydrivetrain.py
@@ -10,516 +10,565 @@
import glog
+
def CoerceGoal(region, K, w, R):
- """Intersects a line with a region, and finds the closest point to R.
+ """Intersects a line with a region, and finds the closest point to R.
- Finds a point that is closest to R inside the region, and on the line
- defined by K X = w. If it is not possible to find a point on the line,
- finds a point that is inside the region and closest to the line. This
- function assumes that
+ Finds a point that is closest to R inside the region, and on the line
+ defined by K X = w. If it is not possible to find a point on the line,
+ finds a point that is inside the region and closest to the line. This
+ function assumes that
- Args:
- region: HPolytope, the valid goal region.
- K: numpy.matrix (2 x 1), the matrix for the equation [K1, K2] [x1; x2] = w
- w: float, the offset in the equation above.
- R: numpy.matrix (2 x 1), the point to be closest to.
+ Args:
+ region: HPolytope, the valid goal region.
+ K: numpy.matrix (2 x 1), the matrix for the equation [K1, K2] [x1; x2] = w
+ w: float, the offset in the equation above.
+ R: numpy.matrix (2 x 1), the point to be closest to.
- Returns:
- numpy.matrix (2 x 1), the point.
- """
- return DoCoerceGoal(region, K, w, R)[0]
+ Returns:
+ numpy.matrix (2 x 1), the point.
+ """
+ return DoCoerceGoal(region, K, w, R)[0]
+
def DoCoerceGoal(region, K, w, R):
- if region.IsInside(R):
- return (R, True)
+ if region.IsInside(R):
+ return (R, True)
- perpendicular_vector = K.T / numpy.linalg.norm(K)
- parallel_vector = numpy.matrix([[perpendicular_vector[1, 0]],
- [-perpendicular_vector[0, 0]]])
+ perpendicular_vector = K.T / numpy.linalg.norm(K)
+ parallel_vector = numpy.matrix([[perpendicular_vector[1, 0]],
+ [-perpendicular_vector[0, 0]]])
- # We want to impose the constraint K * X = w on the polytope H * X <= k.
- # We do this by breaking X up into parallel and perpendicular components to
- # the half plane. This gives us the following equation.
- #
- # parallel * (parallel.T \dot X) + perpendicular * (perpendicular \dot X)) = X
- #
- # Then, substitute this into the polytope.
- #
- # H * (parallel * (parallel.T \dot X) + perpendicular * (perpendicular \dot X)) <= k
- #
- # Substitute K * X = w
- #
- # H * parallel * (parallel.T \dot X) + H * perpendicular * w <= k
- #
- # Move all the knowns to the right side.
- #
- # H * parallel * ([parallel1 parallel2] * X) <= k - H * perpendicular * w
- #
- # Let t = parallel.T \dot X, the component parallel to the surface.
- #
- # H * parallel * t <= k - H * perpendicular * w
- #
- # This is a polytope which we can solve, and use to figure out the range of X
- # that we care about!
+ # We want to impose the constraint K * X = w on the polytope H * X <= k.
+ # We do this by breaking X up into parallel and perpendicular components to
+ # the half plane. This gives us the following equation.
+ #
+ # parallel * (parallel.T \dot X) + perpendicular * (perpendicular \dot X)) = X
+ #
+ # Then, substitute this into the polytope.
+ #
+ # H * (parallel * (parallel.T \dot X) + perpendicular * (perpendicular \dot X)) <= k
+ #
+ # Substitute K * X = w
+ #
+ # H * parallel * (parallel.T \dot X) + H * perpendicular * w <= k
+ #
+ # Move all the knowns to the right side.
+ #
+ # H * parallel * ([parallel1 parallel2] * X) <= k - H * perpendicular * w
+ #
+ # Let t = parallel.T \dot X, the component parallel to the surface.
+ #
+ # H * parallel * t <= k - H * perpendicular * w
+ #
+ # This is a polytope which we can solve, and use to figure out the range of X
+ # that we care about!
- t_poly = polytope.HPolytope(
- region.H * parallel_vector,
- region.k - region.H * perpendicular_vector * w)
+ t_poly = polytope.HPolytope(region.H * parallel_vector,
+ region.k - region.H * perpendicular_vector * w)
- vertices = t_poly.Vertices()
+ vertices = t_poly.Vertices()
- if vertices.shape[0]:
- # The region exists!
- # Find the closest vertex
- min_distance = numpy.infty
- closest_point = None
- for vertex in vertices:
- point = parallel_vector * vertex + perpendicular_vector * w
- length = numpy.linalg.norm(R - point)
- if length < min_distance:
- min_distance = length
- closest_point = point
+ if vertices.shape[0]:
+ # The region exists!
+ # Find the closest vertex
+ min_distance = numpy.infty
+ closest_point = None
+ for vertex in vertices:
+ point = parallel_vector * vertex + perpendicular_vector * w
+ length = numpy.linalg.norm(R - point)
+ if length < min_distance:
+ min_distance = length
+ closest_point = point
- return (closest_point, True)
- else:
- # Find the vertex of the space that is closest to the line.
- region_vertices = region.Vertices()
- min_distance = numpy.infty
- closest_point = None
- for vertex in region_vertices:
- point = vertex.T
- length = numpy.abs((perpendicular_vector.T * point)[0, 0])
- if length < min_distance:
- min_distance = length
- closest_point = point
+ return (closest_point, True)
+ else:
+ # Find the vertex of the space that is closest to the line.
+ region_vertices = region.Vertices()
+ min_distance = numpy.infty
+ closest_point = None
+ for vertex in region_vertices:
+ point = vertex.T
+ length = numpy.abs((perpendicular_vector.T * point)[0, 0])
+ if length < min_distance:
+ min_distance = length
+ closest_point = point
- return (closest_point, False)
+ return (closest_point, False)
+
class VelocityDrivetrainModel(control_loop.ControlLoop):
- def __init__(self, drivetrain_params, left_low=True, right_low=True,
- name="VelocityDrivetrainModel"):
- super(VelocityDrivetrainModel, self).__init__(name)
- self._drivetrain = frc971.control_loops.python.drivetrain.Drivetrain(
- left_low=left_low, right_low=right_low,
- drivetrain_params=drivetrain_params)
- self.dt = drivetrain_params.dt
- self.A_continuous = numpy.matrix(
- [[self._drivetrain.A_continuous[1, 1], self._drivetrain.A_continuous[1, 3]],
- [self._drivetrain.A_continuous[3, 1], self._drivetrain.A_continuous[3, 3]]])
- self.B_continuous = numpy.matrix(
- [[self._drivetrain.B_continuous[1, 0], self._drivetrain.B_continuous[1, 1]],
- [self._drivetrain.B_continuous[3, 0], self._drivetrain.B_continuous[3, 1]]])
- self.C = numpy.matrix(numpy.eye(2))
- self.D = numpy.matrix(numpy.zeros((2, 2)))
+ def __init__(self,
+ drivetrain_params,
+ left_low=True,
+ right_low=True,
+ name="VelocityDrivetrainModel"):
+ super(VelocityDrivetrainModel, self).__init__(name)
+ self._drivetrain = frc971.control_loops.python.drivetrain.Drivetrain(
+ left_low=left_low,
+ right_low=right_low,
+ drivetrain_params=drivetrain_params)
+ self.dt = drivetrain_params.dt
+ self.A_continuous = numpy.matrix(
+ [[
+ self._drivetrain.A_continuous[1, 1],
+ self._drivetrain.A_continuous[1, 3]
+ ],
+ [
+ self._drivetrain.A_continuous[3, 1],
+ self._drivetrain.A_continuous[3, 3]
+ ]])
- self.A, self.B = self.ContinuousToDiscrete(self.A_continuous,
- self.B_continuous, self.dt)
+ self.B_continuous = numpy.matrix(
+ [[
+ self._drivetrain.B_continuous[1, 0],
+ self._drivetrain.B_continuous[1, 1]
+ ],
+ [
+ self._drivetrain.B_continuous[3, 0],
+ self._drivetrain.B_continuous[3, 1]
+ ]])
+ self.C = numpy.matrix(numpy.eye(2))
+ self.D = numpy.matrix(numpy.zeros((2, 2)))
- # FF * X = U (steady state)
- self.FF = self.B.I * (numpy.eye(2) - self.A)
+ self.A, self.B = self.ContinuousToDiscrete(self.A_continuous,
+ self.B_continuous, self.dt)
- self.PlaceControllerPoles(drivetrain_params.controller_poles)
- # Build a kalman filter for the velocity. We don't care what the gains
- # are, but the hybrid kalman filter that we want to write to disk to get
- # access to A_continuous and B_continuous needs this for completeness.
- self.Q_continuous = numpy.matrix([[(0.5 ** 2.0), 0.0], [0.0, (0.5 ** 2.0)]])
- self.R_continuous = numpy.matrix([[(0.1 ** 2.0), 0.0], [0.0, (0.1 ** 2.0)]])
- self.PlaceObserverPoles(drivetrain_params.observer_poles)
- _, _, self.Q, self.R = controls.kalmd(
- A_continuous=self.A_continuous, B_continuous=self.B_continuous,
- Q_continuous=self.Q_continuous, R_continuous=self.R_continuous,
- dt=self.dt)
+ # FF * X = U (steady state)
+ self.FF = self.B.I * (numpy.eye(2) - self.A)
- self.KalmanGain, self.P_steady_state = controls.kalman(
- A=self.A, B=self.B, C=self.C, Q=self.Q, R=self.R)
+ self.PlaceControllerPoles(drivetrain_params.controller_poles)
+ # Build a kalman filter for the velocity. We don't care what the gains
+ # are, but the hybrid kalman filter that we want to write to disk to get
+ # access to A_continuous and B_continuous needs this for completeness.
+ self.Q_continuous = numpy.matrix([[(0.5**2.0), 0.0], [0.0, (0.5**2.0)]])
+ self.R_continuous = numpy.matrix([[(0.1**2.0), 0.0], [0.0, (0.1**2.0)]])
+ self.PlaceObserverPoles(drivetrain_params.observer_poles)
+ _, _, self.Q, self.R = controls.kalmd(
+ A_continuous=self.A_continuous,
+ B_continuous=self.B_continuous,
+ Q_continuous=self.Q_continuous,
+ R_continuous=self.R_continuous,
+ dt=self.dt)
- self.G_high = self._drivetrain.G_high
- self.G_low = self._drivetrain.G_low
- self.resistance = self._drivetrain.resistance
- self.r = self._drivetrain.r
- self.Kv = self._drivetrain.Kv
- self.Kt = self._drivetrain.Kt
+ self.KalmanGain, self.P_steady_state = controls.kalman(
+ A=self.A, B=self.B, C=self.C, Q=self.Q, R=self.R)
- self.U_max = self._drivetrain.U_max
- self.U_min = self._drivetrain.U_min
+ self.G_high = self._drivetrain.G_high
+ self.G_low = self._drivetrain.G_low
+ self.resistance = self._drivetrain.resistance
+ self.r = self._drivetrain.r
+ self.Kv = self._drivetrain.Kv
+ self.Kt = self._drivetrain.Kt
- @property
- def robot_radius_l(self):
- return self._drivetrain.robot_radius_l
- @property
- def robot_radius_r(self):
- return self._drivetrain.robot_radius_r
+ self.U_max = self._drivetrain.U_max
+ self.U_min = self._drivetrain.U_min
+
+ @property
+ def robot_radius_l(self):
+ return self._drivetrain.robot_radius_l
+
+ @property
+ def robot_radius_r(self):
+ return self._drivetrain.robot_radius_r
+
class VelocityDrivetrain(object):
- HIGH = 'high'
- LOW = 'low'
- SHIFTING_UP = 'up'
- SHIFTING_DOWN = 'down'
+ HIGH = 'high'
+ LOW = 'low'
+ SHIFTING_UP = 'up'
+ SHIFTING_DOWN = 'down'
- def __init__(self, drivetrain_params, name='VelocityDrivetrain'):
- self.drivetrain_low_low = VelocityDrivetrainModel(
- left_low=True, right_low=True, name=name + 'LowLow',
- drivetrain_params=drivetrain_params)
- self.drivetrain_low_high = VelocityDrivetrainModel(
- left_low=True, right_low=False, name=name + 'LowHigh',
- drivetrain_params=drivetrain_params)
- self.drivetrain_high_low = VelocityDrivetrainModel(
- left_low=False, right_low=True, name = name + 'HighLow',
- drivetrain_params=drivetrain_params)
- self.drivetrain_high_high = VelocityDrivetrainModel(
- left_low=False, right_low=False, name = name + 'HighHigh',
- drivetrain_params=drivetrain_params)
+ def __init__(self, drivetrain_params, name='VelocityDrivetrain'):
+ self.drivetrain_low_low = VelocityDrivetrainModel(
+ left_low=True,
+ right_low=True,
+ name=name + 'LowLow',
+ drivetrain_params=drivetrain_params)
+ self.drivetrain_low_high = VelocityDrivetrainModel(
+ left_low=True,
+ right_low=False,
+ name=name + 'LowHigh',
+ drivetrain_params=drivetrain_params)
+ self.drivetrain_high_low = VelocityDrivetrainModel(
+ left_low=False,
+ right_low=True,
+ name=name + 'HighLow',
+ drivetrain_params=drivetrain_params)
+ self.drivetrain_high_high = VelocityDrivetrainModel(
+ left_low=False,
+ right_low=False,
+ name=name + 'HighHigh',
+ drivetrain_params=drivetrain_params)
- # X is [lvel, rvel]
- self.X = numpy.matrix(
- [[0.0],
- [0.0]])
+ # X is [lvel, rvel]
+ self.X = numpy.matrix([[0.0], [0.0]])
- self.U_poly = polytope.HPolytope(
- numpy.matrix([[1, 0],
- [-1, 0],
- [0, 1],
- [0, -1]]),
- numpy.matrix([[12],
- [12],
- [12],
- [12]]))
+ self.U_poly = polytope.HPolytope(
+ numpy.matrix([[1, 0], [-1, 0], [0, 1], [0, -1]]),
+ numpy.matrix([[12], [12], [12], [12]]))
- self.U_max = numpy.matrix(
- [[12.0],
- [12.0]])
- self.U_min = numpy.matrix(
- [[-12.0000000000],
- [-12.0000000000]])
+ self.U_max = numpy.matrix([[12.0], [12.0]])
+ self.U_min = numpy.matrix([[-12.0000000000], [-12.0000000000]])
- self.dt = 0.00505
+ self.dt = 0.00505
- self.R = numpy.matrix(
- [[0.0],
- [0.0]])
+ self.R = numpy.matrix([[0.0], [0.0]])
- self.U_ideal = numpy.matrix(
- [[0.0],
- [0.0]])
+ self.U_ideal = numpy.matrix([[0.0], [0.0]])
- # ttrust is the comprimise between having full throttle negative inertia,
- # and having no throttle negative inertia. A value of 0 is full throttle
- # inertia. A value of 1 is no throttle negative inertia.
- self.ttrust = 1.0
+ # ttrust is the comprimise between having full throttle negative inertia,
+ # and having no throttle negative inertia. A value of 0 is full throttle
+ # inertia. A value of 1 is no throttle negative inertia.
+ self.ttrust = 1.0
- self.left_gear = VelocityDrivetrain.LOW
- self.right_gear = VelocityDrivetrain.LOW
- self.left_shifter_position = 0.0
- self.right_shifter_position = 0.0
- self.left_cim = CIM()
- self.right_cim = CIM()
+ self.left_gear = VelocityDrivetrain.LOW
+ self.right_gear = VelocityDrivetrain.LOW
+ self.left_shifter_position = 0.0
+ self.right_shifter_position = 0.0
+ self.left_cim = CIM()
+ self.right_cim = CIM()
- def IsInGear(self, gear):
- return gear is VelocityDrivetrain.HIGH or gear is VelocityDrivetrain.LOW
+ def IsInGear(self, gear):
+ return gear is VelocityDrivetrain.HIGH or gear is VelocityDrivetrain.LOW
- def MotorRPM(self, shifter_position, velocity):
- if shifter_position > 0.5:
- return (velocity / self.CurrentDrivetrain().G_high /
- self.CurrentDrivetrain().r)
- else:
- return (velocity / self.CurrentDrivetrain().G_low /
- self.CurrentDrivetrain().r)
-
- def CurrentDrivetrain(self):
- if self.left_shifter_position > 0.5:
- if self.right_shifter_position > 0.5:
- return self.drivetrain_high_high
- else:
- return self.drivetrain_high_low
- else:
- if self.right_shifter_position > 0.5:
- return self.drivetrain_low_high
- else:
- return self.drivetrain_low_low
-
- def SimShifter(self, gear, shifter_position):
- if gear is VelocityDrivetrain.HIGH or gear is VelocityDrivetrain.SHIFTING_UP:
- shifter_position = min(shifter_position + 0.5, 1.0)
- else:
- shifter_position = max(shifter_position - 0.5, 0.0)
-
- if shifter_position == 1.0:
- gear = VelocityDrivetrain.HIGH
- elif shifter_position == 0.0:
- gear = VelocityDrivetrain.LOW
-
- return gear, shifter_position
-
- def ComputeGear(self, wheel_velocity, should_print=False, current_gear=False, gear_name=None):
- high_omega = (wheel_velocity / self.CurrentDrivetrain().G_high /
- self.CurrentDrivetrain().r)
- low_omega = (wheel_velocity / self.CurrentDrivetrain().G_low /
- self.CurrentDrivetrain().r)
- #print gear_name, "Motor Energy Difference.", 0.5 * 0.000140032647 * (low_omega * low_omega - high_omega * high_omega), "joules"
- high_torque = ((12.0 - high_omega / self.CurrentDrivetrain().Kv) *
- self.CurrentDrivetrain().Kt / self.CurrentDrivetrain().resistance)
- low_torque = ((12.0 - low_omega / self.CurrentDrivetrain().Kv) *
- self.CurrentDrivetrain().Kt / self.CurrentDrivetrain().resistance)
- high_power = high_torque * high_omega
- low_power = low_torque * low_omega
- #if should_print:
- # print gear_name, "High omega", high_omega, "Low omega", low_omega
- # print gear_name, "High torque", high_torque, "Low torque", low_torque
- # print gear_name, "High power", high_power, "Low power", low_power
-
- # Shift algorithm improvements.
- # TODO(aschuh):
- # It takes time to shift. Shifting down for 1 cycle doesn't make sense
- # because you will end up slower than without shifting. Figure out how
- # to include that info.
- # If the driver is still in high gear, but isn't asking for the extra power
- # from low gear, don't shift until he asks for it.
- goal_gear_is_high = high_power > low_power
- #goal_gear_is_high = True
-
- if not self.IsInGear(current_gear):
- glog.debug('%s Not in gear.', gear_name)
- return current_gear
- else:
- is_high = current_gear is VelocityDrivetrain.HIGH
- if is_high != goal_gear_is_high:
- if goal_gear_is_high:
- glog.debug('%s Shifting up.', gear_name)
- return VelocityDrivetrain.SHIFTING_UP
+ def MotorRPM(self, shifter_position, velocity):
+ if shifter_position > 0.5:
+ return (velocity / self.CurrentDrivetrain().G_high /
+ self.CurrentDrivetrain().r)
else:
- glog.debug('%s Shifting down.', gear_name)
- return VelocityDrivetrain.SHIFTING_DOWN
- else:
- return current_gear
+ return (velocity / self.CurrentDrivetrain().G_low /
+ self.CurrentDrivetrain().r)
- def FilterVelocity(self, throttle):
- # Invert the plant to figure out how the velocity filter would have to work
- # out in order to filter out the forwards negative inertia.
- # This math assumes that the left and right power and velocity are equal.
+ def CurrentDrivetrain(self):
+ if self.left_shifter_position > 0.5:
+ if self.right_shifter_position > 0.5:
+ return self.drivetrain_high_high
+ else:
+ return self.drivetrain_high_low
+ else:
+ if self.right_shifter_position > 0.5:
+ return self.drivetrain_low_high
+ else:
+ return self.drivetrain_low_low
- # The throttle filter should filter such that the motor in the highest gear
- # should be controlling the time constant.
- # Do this by finding the index of FF that has the lowest value, and computing
- # the sums using that index.
- FF_sum = self.CurrentDrivetrain().FF.sum(axis=1)
- min_FF_sum_index = numpy.argmin(FF_sum)
- min_FF_sum = FF_sum[min_FF_sum_index, 0]
- min_K_sum = self.CurrentDrivetrain().K[min_FF_sum_index, :].sum()
- # Compute the FF sum for high gear.
- high_min_FF_sum = self.drivetrain_high_high.FF[0, :].sum()
+ def SimShifter(self, gear, shifter_position):
+ if gear is VelocityDrivetrain.HIGH or gear is VelocityDrivetrain.SHIFTING_UP:
+ shifter_position = min(shifter_position + 0.5, 1.0)
+ else:
+ shifter_position = max(shifter_position - 0.5, 0.0)
- # U = self.K[0, :].sum() * (R - x_avg) + self.FF[0, :].sum() * R
- # throttle * 12.0 = (self.K[0, :].sum() + self.FF[0, :].sum()) * R
- # - self.K[0, :].sum() * x_avg
+ if shifter_position == 1.0:
+ gear = VelocityDrivetrain.HIGH
+ elif shifter_position == 0.0:
+ gear = VelocityDrivetrain.LOW
- # R = (throttle * 12.0 + self.K[0, :].sum() * x_avg) /
- # (self.K[0, :].sum() + self.FF[0, :].sum())
+ return gear, shifter_position
- # U = (K + FF) * R - K * X
- # (K + FF) ^-1 * (U + K * X) = R
+ def ComputeGear(self,
+ wheel_velocity,
+ should_print=False,
+ current_gear=False,
+ gear_name=None):
+ high_omega = (wheel_velocity / self.CurrentDrivetrain().G_high /
+ self.CurrentDrivetrain().r)
+ low_omega = (wheel_velocity / self.CurrentDrivetrain().G_low /
+ self.CurrentDrivetrain().r)
+ #print gear_name, "Motor Energy Difference.", 0.5 * 0.000140032647 * (low_omega * low_omega - high_omega * high_omega), "joules"
+ high_torque = (
+ (12.0 - high_omega / self.CurrentDrivetrain().Kv) *
+ self.CurrentDrivetrain().Kt / self.CurrentDrivetrain().resistance)
+ low_torque = (
+ (12.0 - low_omega / self.CurrentDrivetrain().Kv) *
+ self.CurrentDrivetrain().Kt / self.CurrentDrivetrain().resistance)
+ high_power = high_torque * high_omega
+ low_power = low_torque * low_omega
+ #if should_print:
+ # print gear_name, "High omega", high_omega, "Low omega", low_omega
+ # print gear_name, "High torque", high_torque, "Low torque", low_torque
+ # print gear_name, "High power", high_power, "Low power", low_power
- # Scale throttle by min_FF_sum / high_min_FF_sum. This will make low gear
- # have the same velocity goal as high gear, and so that the robot will hold
- # the same speed for the same throttle for all gears.
- adjusted_ff_voltage = numpy.clip(throttle * 12.0 * min_FF_sum / high_min_FF_sum, -12.0, 12.0)
- return ((adjusted_ff_voltage + self.ttrust * min_K_sum * (self.X[0, 0] + self.X[1, 0]) / 2.0)
- / (self.ttrust * min_K_sum + min_FF_sum))
+ # Shift algorithm improvements.
+ # TODO(aschuh):
+ # It takes time to shift. Shifting down for 1 cycle doesn't make sense
+ # because you will end up slower than without shifting. Figure out how
+ # to include that info.
+ # If the driver is still in high gear, but isn't asking for the extra power
+ # from low gear, don't shift until he asks for it.
+ goal_gear_is_high = high_power > low_power
+ #goal_gear_is_high = True
- def Update(self, throttle, steering):
- # Shift into the gear which sends the most power to the floor.
- # This is the same as sending the most torque down to the floor at the
- # wheel.
+ if not self.IsInGear(current_gear):
+ glog.debug('%s Not in gear.', gear_name)
+ return current_gear
+ else:
+ is_high = current_gear is VelocityDrivetrain.HIGH
+ if is_high != goal_gear_is_high:
+ if goal_gear_is_high:
+ glog.debug('%s Shifting up.', gear_name)
+ return VelocityDrivetrain.SHIFTING_UP
+ else:
+ glog.debug('%s Shifting down.', gear_name)
+ return VelocityDrivetrain.SHIFTING_DOWN
+ else:
+ return current_gear
- self.left_gear = self.right_gear = True
- if True:
- self.left_gear = self.ComputeGear(self.X[0, 0], should_print=True,
- current_gear=self.left_gear,
- gear_name="left")
- self.right_gear = self.ComputeGear(self.X[1, 0], should_print=True,
- current_gear=self.right_gear,
- gear_name="right")
- if self.IsInGear(self.left_gear):
- self.left_cim.X[0, 0] = self.MotorRPM(self.left_shifter_position, self.X[0, 0])
+ def FilterVelocity(self, throttle):
+ # Invert the plant to figure out how the velocity filter would have to work
+ # out in order to filter out the forwards negative inertia.
+ # This math assumes that the left and right power and velocity are equal.
- if self.IsInGear(self.right_gear):
- self.right_cim.X[0, 0] = self.MotorRPM(self.right_shifter_position, self.X[0, 0])
+ # The throttle filter should filter such that the motor in the highest gear
+ # should be controlling the time constant.
+ # Do this by finding the index of FF that has the lowest value, and computing
+ # the sums using that index.
+ FF_sum = self.CurrentDrivetrain().FF.sum(axis=1)
+ min_FF_sum_index = numpy.argmin(FF_sum)
+ min_FF_sum = FF_sum[min_FF_sum_index, 0]
+ min_K_sum = self.CurrentDrivetrain().K[min_FF_sum_index, :].sum()
+ # Compute the FF sum for high gear.
+ high_min_FF_sum = self.drivetrain_high_high.FF[0, :].sum()
- if self.IsInGear(self.left_gear) and self.IsInGear(self.right_gear):
- # Filter the throttle to provide a nicer response.
- fvel = self.FilterVelocity(throttle)
+ # U = self.K[0, :].sum() * (R - x_avg) + self.FF[0, :].sum() * R
+ # throttle * 12.0 = (self.K[0, :].sum() + self.FF[0, :].sum()) * R
+ # - self.K[0, :].sum() * x_avg
- # Constant radius means that angualar_velocity / linear_velocity = constant.
- # Compute the left and right velocities.
- steering_velocity = numpy.abs(fvel) * steering
- left_velocity = fvel - steering_velocity
- right_velocity = fvel + steering_velocity
+ # R = (throttle * 12.0 + self.K[0, :].sum() * x_avg) /
+ # (self.K[0, :].sum() + self.FF[0, :].sum())
- # Write this constraint in the form of K * R = w
- # angular velocity / linear velocity = constant
- # (left - right) / (left + right) = constant
- # left - right = constant * left + constant * right
+ # U = (K + FF) * R - K * X
+ # (K + FF) ^-1 * (U + K * X) = R
- # (fvel - steering * numpy.abs(fvel) - fvel - steering * numpy.abs(fvel)) /
- # (fvel - steering * numpy.abs(fvel) + fvel + steering * numpy.abs(fvel)) =
- # constant
- # (- 2 * steering * numpy.abs(fvel)) / (2 * fvel) = constant
- # (-steering * sign(fvel)) = constant
- # (-steering * sign(fvel)) * (left + right) = left - right
- # (steering * sign(fvel) + 1) * left + (steering * sign(fvel) - 1) * right = 0
+ # Scale throttle by min_FF_sum / high_min_FF_sum. This will make low gear
+ # have the same velocity goal as high gear, and so that the robot will hold
+ # the same speed for the same throttle for all gears.
+ adjusted_ff_voltage = numpy.clip(
+ throttle * 12.0 * min_FF_sum / high_min_FF_sum, -12.0, 12.0)
+ return ((adjusted_ff_voltage + self.ttrust * min_K_sum *
+ (self.X[0, 0] + self.X[1, 0]) / 2.0) /
+ (self.ttrust * min_K_sum + min_FF_sum))
- equality_k = numpy.matrix(
- [[1 + steering * numpy.sign(fvel), -(1 - steering * numpy.sign(fvel))]])
- equality_w = 0.0
+ def Update(self, throttle, steering):
+ # Shift into the gear which sends the most power to the floor.
+ # This is the same as sending the most torque down to the floor at the
+ # wheel.
- self.R[0, 0] = left_velocity
- self.R[1, 0] = right_velocity
+ self.left_gear = self.right_gear = True
+ if True:
+ self.left_gear = self.ComputeGear(
+ self.X[0, 0],
+ should_print=True,
+ current_gear=self.left_gear,
+ gear_name="left")
+ self.right_gear = self.ComputeGear(
+ self.X[1, 0],
+ should_print=True,
+ current_gear=self.right_gear,
+ gear_name="right")
+ if self.IsInGear(self.left_gear):
+ self.left_cim.X[0, 0] = self.MotorRPM(
+ self.left_shifter_position, self.X[0, 0])
- # Construct a constraint on R by manipulating the constraint on U
- # Start out with H * U <= k
- # U = FF * R + K * (R - X)
- # H * (FF * R + K * R - K * X) <= k
- # H * (FF + K) * R <= k + H * K * X
- R_poly = polytope.HPolytope(
- self.U_poly.H * (self.CurrentDrivetrain().K + self.CurrentDrivetrain().FF),
- self.U_poly.k + self.U_poly.H * self.CurrentDrivetrain().K * self.X)
+ if self.IsInGear(self.right_gear):
+ self.right_cim.X[0, 0] = self.MotorRPM(
+ self.right_shifter_position, self.X[0, 0])
- # Limit R back inside the box.
- self.boxed_R = CoerceGoal(R_poly, equality_k, equality_w, self.R)
+ if self.IsInGear(self.left_gear) and self.IsInGear(self.right_gear):
+ # Filter the throttle to provide a nicer response.
+ fvel = self.FilterVelocity(throttle)
- FF_volts = self.CurrentDrivetrain().FF * self.boxed_R
- self.U_ideal = self.CurrentDrivetrain().K * (self.boxed_R - self.X) + FF_volts
- else:
- glog.debug('Not all in gear')
- if not self.IsInGear(self.left_gear) and not self.IsInGear(self.right_gear):
- # TODO(austin): Use battery volts here.
- R_left = self.MotorRPM(self.left_shifter_position, self.X[0, 0])
- self.U_ideal[0, 0] = numpy.clip(
- self.left_cim.K * (R_left - self.left_cim.X) + R_left / self.left_cim.Kv,
- self.left_cim.U_min, self.left_cim.U_max)
- self.left_cim.Update(self.U_ideal[0, 0])
+ # Constant radius means that angualar_velocity / linear_velocity = constant.
+ # Compute the left and right velocities.
+ steering_velocity = numpy.abs(fvel) * steering
+ left_velocity = fvel - steering_velocity
+ right_velocity = fvel + steering_velocity
- R_right = self.MotorRPM(self.right_shifter_position, self.X[1, 0])
- self.U_ideal[1, 0] = numpy.clip(
- self.right_cim.K * (R_right - self.right_cim.X) + R_right / self.right_cim.Kv,
- self.right_cim.U_min, self.right_cim.U_max)
- self.right_cim.Update(self.U_ideal[1, 0])
- else:
- assert False
+ # Write this constraint in the form of K * R = w
+ # angular velocity / linear velocity = constant
+ # (left - right) / (left + right) = constant
+ # left - right = constant * left + constant * right
- self.U = numpy.clip(self.U_ideal, self.U_min, self.U_max)
+ # (fvel - steering * numpy.abs(fvel) - fvel - steering * numpy.abs(fvel)) /
+ # (fvel - steering * numpy.abs(fvel) + fvel + steering * numpy.abs(fvel)) =
+ # constant
+ # (- 2 * steering * numpy.abs(fvel)) / (2 * fvel) = constant
+ # (-steering * sign(fvel)) = constant
+ # (-steering * sign(fvel)) * (left + right) = left - right
+ # (steering * sign(fvel) + 1) * left + (steering * sign(fvel) - 1) * right = 0
- # TODO(austin): Model the robot as not accelerating when you shift...
- # This hack only works when you shift at the same time.
- if self.IsInGear(self.left_gear) and self.IsInGear(self.right_gear):
- self.X = self.CurrentDrivetrain().A * self.X + self.CurrentDrivetrain().B * self.U
+ equality_k = numpy.matrix([[
+ 1 + steering * numpy.sign(fvel),
+ -(1 - steering * numpy.sign(fvel))
+ ]])
+ equality_w = 0.0
- self.left_gear, self.left_shifter_position = self.SimShifter(
- self.left_gear, self.left_shifter_position)
- self.right_gear, self.right_shifter_position = self.SimShifter(
- self.right_gear, self.right_shifter_position)
+ self.R[0, 0] = left_velocity
+ self.R[1, 0] = right_velocity
- glog.debug('U is %s %s', str(self.U[0, 0]), str(self.U[1, 0]))
- glog.debug('Left shifter %s %d Right shifter %s %d',
- self.left_gear, self.left_shifter_position,
- self.right_gear, self.right_shifter_position)
+ # Construct a constraint on R by manipulating the constraint on U
+ # Start out with H * U <= k
+ # U = FF * R + K * (R - X)
+ # H * (FF * R + K * R - K * X) <= k
+ # H * (FF + K) * R <= k + H * K * X
+ R_poly = polytope.HPolytope(
+ self.U_poly.H *
+ (self.CurrentDrivetrain().K + self.CurrentDrivetrain().FF),
+ self.U_poly.k +
+ self.U_poly.H * self.CurrentDrivetrain().K * self.X)
-def WritePolyDrivetrain(drivetrain_files, motor_files, hybrid_files,
- year_namespace, drivetrain_params,
+ # Limit R back inside the box.
+ self.boxed_R = CoerceGoal(R_poly, equality_k, equality_w, self.R)
+
+ FF_volts = self.CurrentDrivetrain().FF * self.boxed_R
+ self.U_ideal = self.CurrentDrivetrain().K * (
+ self.boxed_R - self.X) + FF_volts
+ else:
+ glog.debug('Not all in gear')
+ if not self.IsInGear(self.left_gear) and not self.IsInGear(
+ self.right_gear):
+ # TODO(austin): Use battery volts here.
+ R_left = self.MotorRPM(self.left_shifter_position, self.X[0, 0])
+ self.U_ideal[0, 0] = numpy.clip(
+ self.left_cim.K * (R_left - self.left_cim.X) +
+ R_left / self.left_cim.Kv, self.left_cim.U_min,
+ self.left_cim.U_max)
+ self.left_cim.Update(self.U_ideal[0, 0])
+
+ R_right = self.MotorRPM(self.right_shifter_position,
+ self.X[1, 0])
+ self.U_ideal[1, 0] = numpy.clip(
+ self.right_cim.K * (R_right - self.right_cim.X) +
+ R_right / self.right_cim.Kv, self.right_cim.U_min,
+ self.right_cim.U_max)
+ self.right_cim.Update(self.U_ideal[1, 0])
+ else:
+ assert False
+
+ self.U = numpy.clip(self.U_ideal, self.U_min, self.U_max)
+
+ # TODO(austin): Model the robot as not accelerating when you shift...
+ # This hack only works when you shift at the same time.
+ if self.IsInGear(self.left_gear) and self.IsInGear(self.right_gear):
+ self.X = self.CurrentDrivetrain(
+ ).A * self.X + self.CurrentDrivetrain().B * self.U
+
+ self.left_gear, self.left_shifter_position = self.SimShifter(
+ self.left_gear, self.left_shifter_position)
+ self.right_gear, self.right_shifter_position = self.SimShifter(
+ self.right_gear, self.right_shifter_position)
+
+ glog.debug('U is %s %s', str(self.U[0, 0]), str(self.U[1, 0]))
+ glog.debug('Left shifter %s %d Right shifter %s %d', self.left_gear,
+ self.left_shifter_position, self.right_gear,
+ self.right_shifter_position)
+
+
+def WritePolyDrivetrain(drivetrain_files,
+ motor_files,
+ hybrid_files,
+ year_namespace,
+ drivetrain_params,
scalar_type='double'):
- vdrivetrain = VelocityDrivetrain(drivetrain_params)
- hybrid_vdrivetrain = VelocityDrivetrain(drivetrain_params,
- name="HybridVelocityDrivetrain")
- if isinstance(year_namespace, list):
- namespaces = year_namespace
- else:
- namespaces = [year_namespace, 'control_loops', 'drivetrain']
- dog_loop_writer = control_loop.ControlLoopWriter(
- "VelocityDrivetrain", [vdrivetrain.drivetrain_low_low,
- vdrivetrain.drivetrain_low_high,
- vdrivetrain.drivetrain_high_low,
- vdrivetrain.drivetrain_high_high],
- namespaces=namespaces,
- scalar_type=scalar_type)
+ vdrivetrain = VelocityDrivetrain(drivetrain_params)
+ hybrid_vdrivetrain = VelocityDrivetrain(
+ drivetrain_params, name="HybridVelocityDrivetrain")
+ if isinstance(year_namespace, list):
+ namespaces = year_namespace
+ else:
+ namespaces = [year_namespace, 'control_loops', 'drivetrain']
+ dog_loop_writer = control_loop.ControlLoopWriter(
+ "VelocityDrivetrain", [
+ vdrivetrain.drivetrain_low_low, vdrivetrain.drivetrain_low_high,
+ vdrivetrain.drivetrain_high_low, vdrivetrain.drivetrain_high_high
+ ],
+ namespaces=namespaces,
+ scalar_type=scalar_type)
- dog_loop_writer.Write(drivetrain_files[0], drivetrain_files[1])
+ dog_loop_writer.Write(drivetrain_files[0], drivetrain_files[1])
- hybrid_loop_writer = control_loop.ControlLoopWriter(
- "HybridVelocityDrivetrain", [hybrid_vdrivetrain.drivetrain_low_low,
- hybrid_vdrivetrain.drivetrain_low_high,
- hybrid_vdrivetrain.drivetrain_high_low,
- hybrid_vdrivetrain.drivetrain_high_high],
- namespaces=namespaces,
- scalar_type=scalar_type,
- plant_type='StateFeedbackHybridPlant',
- observer_type='HybridKalman')
+ hybrid_loop_writer = control_loop.ControlLoopWriter(
+ "HybridVelocityDrivetrain", [
+ hybrid_vdrivetrain.drivetrain_low_low,
+ hybrid_vdrivetrain.drivetrain_low_high,
+ hybrid_vdrivetrain.drivetrain_high_low,
+ hybrid_vdrivetrain.drivetrain_high_high
+ ],
+ namespaces=namespaces,
+ scalar_type=scalar_type,
+ plant_type='StateFeedbackHybridPlant',
+ observer_type='HybridKalman')
- hybrid_loop_writer.Write(hybrid_files[0], hybrid_files[1])
+ hybrid_loop_writer.Write(hybrid_files[0], hybrid_files[1])
- cim_writer = control_loop.ControlLoopWriter("CIM", [CIM()], scalar_type=scalar_type)
+ cim_writer = control_loop.ControlLoopWriter(
+ "CIM", [CIM()], scalar_type=scalar_type)
- cim_writer.Write(motor_files[0], motor_files[1])
+ cim_writer.Write(motor_files[0], motor_files[1])
+
def PlotPolyDrivetrainMotions(drivetrain_params):
- vdrivetrain = VelocityDrivetrain(drivetrain_params)
- vl_plot = []
- vr_plot = []
- ul_plot = []
- ur_plot = []
- radius_plot = []
- t_plot = []
- left_gear_plot = []
- right_gear_plot = []
- vdrivetrain.left_shifter_position = 0.0
- vdrivetrain.right_shifter_position = 0.0
- vdrivetrain.left_gear = VelocityDrivetrain.LOW
- vdrivetrain.right_gear = VelocityDrivetrain.LOW
+ vdrivetrain = VelocityDrivetrain(drivetrain_params)
+ vl_plot = []
+ vr_plot = []
+ ul_plot = []
+ ur_plot = []
+ radius_plot = []
+ t_plot = []
+ left_gear_plot = []
+ right_gear_plot = []
+ vdrivetrain.left_shifter_position = 0.0
+ vdrivetrain.right_shifter_position = 0.0
+ vdrivetrain.left_gear = VelocityDrivetrain.LOW
+ vdrivetrain.right_gear = VelocityDrivetrain.LOW
- glog.debug('K is %s', str(vdrivetrain.CurrentDrivetrain().K))
+ glog.debug('K is %s', str(vdrivetrain.CurrentDrivetrain().K))
- if vdrivetrain.left_gear is VelocityDrivetrain.HIGH:
- glog.debug('Left is high')
- else:
- glog.debug('Left is low')
- if vdrivetrain.right_gear is VelocityDrivetrain.HIGH:
- glog.debug('Right is high')
- else:
- glog.debug('Right is low')
-
- for t in numpy.arange(0, 1.7, vdrivetrain.dt):
- if t < 0.5:
- vdrivetrain.Update(throttle=0.00, steering=1.0)
- elif t < 1.2:
- vdrivetrain.Update(throttle=0.5, steering=1.0)
+ if vdrivetrain.left_gear is VelocityDrivetrain.HIGH:
+ glog.debug('Left is high')
else:
- vdrivetrain.Update(throttle=0.00, steering=1.0)
- t_plot.append(t)
- vl_plot.append(vdrivetrain.X[0, 0])
- vr_plot.append(vdrivetrain.X[1, 0])
- ul_plot.append(vdrivetrain.U[0, 0])
- ur_plot.append(vdrivetrain.U[1, 0])
- left_gear_plot.append((vdrivetrain.left_gear is VelocityDrivetrain.HIGH) * 2.0 - 10.0)
- right_gear_plot.append((vdrivetrain.right_gear is VelocityDrivetrain.HIGH) * 2.0 - 10.0)
-
- fwd_velocity = (vdrivetrain.X[1, 0] + vdrivetrain.X[0, 0]) / 2
- turn_velocity = (vdrivetrain.X[1, 0] - vdrivetrain.X[0, 0])
- if abs(fwd_velocity) < 0.0000001:
- radius_plot.append(turn_velocity)
+ glog.debug('Left is low')
+ if vdrivetrain.right_gear is VelocityDrivetrain.HIGH:
+ glog.debug('Right is high')
else:
- radius_plot.append(turn_velocity / fwd_velocity)
+ glog.debug('Right is low')
- # TODO(austin):
- # Shifting compensation.
+ for t in numpy.arange(0, 1.7, vdrivetrain.dt):
+ if t < 0.5:
+ vdrivetrain.Update(throttle=0.00, steering=1.0)
+ elif t < 1.2:
+ vdrivetrain.Update(throttle=0.5, steering=1.0)
+ else:
+ vdrivetrain.Update(throttle=0.00, steering=1.0)
+ t_plot.append(t)
+ vl_plot.append(vdrivetrain.X[0, 0])
+ vr_plot.append(vdrivetrain.X[1, 0])
+ ul_plot.append(vdrivetrain.U[0, 0])
+ ur_plot.append(vdrivetrain.U[1, 0])
+ left_gear_plot.append(
+ (vdrivetrain.left_gear is VelocityDrivetrain.HIGH) * 2.0 - 10.0)
+ right_gear_plot.append(
+ (vdrivetrain.right_gear is VelocityDrivetrain.HIGH) * 2.0 - 10.0)
- # Tighten the turn.
- # Closed loop drive.
+ fwd_velocity = (vdrivetrain.X[1, 0] + vdrivetrain.X[0, 0]) / 2
+ turn_velocity = (vdrivetrain.X[1, 0] - vdrivetrain.X[0, 0])
+ if abs(fwd_velocity) < 0.0000001:
+ radius_plot.append(turn_velocity)
+ else:
+ radius_plot.append(turn_velocity / fwd_velocity)
- pylab.plot(t_plot, vl_plot, label='left velocity')
- pylab.plot(t_plot, vr_plot, label='right velocity')
- pylab.plot(t_plot, ul_plot, label='left voltage')
- pylab.plot(t_plot, ur_plot, label='right voltage')
- pylab.plot(t_plot, radius_plot, label='radius')
- pylab.plot(t_plot, left_gear_plot, label='left gear high')
- pylab.plot(t_plot, right_gear_plot, label='right gear high')
- pylab.legend()
- pylab.show()
+ # TODO(austin):
+ # Shifting compensation.
+
+ # Tighten the turn.
+ # Closed loop drive.
+
+ pylab.plot(t_plot, vl_plot, label='left velocity')
+ pylab.plot(t_plot, vr_plot, label='right velocity')
+ pylab.plot(t_plot, ul_plot, label='left voltage')
+ pylab.plot(t_plot, ur_plot, label='right voltage')
+ pylab.plot(t_plot, radius_plot, label='radius')
+ pylab.plot(t_plot, left_gear_plot, label='left gear high')
+ pylab.plot(t_plot, right_gear_plot, label='right gear high')
+ pylab.legend()
+ pylab.show()
diff --git a/frc971/control_loops/python/static_zeroing_single_dof_profiled_subsystem_test.py b/frc971/control_loops/python/static_zeroing_single_dof_profiled_subsystem_test.py
index caa3082..839677e 100644
--- a/frc971/control_loops/python/static_zeroing_single_dof_profiled_subsystem_test.py
+++ b/frc971/control_loops/python/static_zeroing_single_dof_profiled_subsystem_test.py
@@ -39,11 +39,9 @@
# Write the generated constants out to a file.
if len(argv) != 5:
- glog.fatal(
- 'Expected .h file name and .cc file name for the \
+ glog.fatal('Expected .h file name and .cc file name for the \
static_zeroing_single_dof_profiled_subsystem_test and integral \
- static_zeroing_single_dof_profiled_subsystem_test.'
- )
+ static_zeroing_single_dof_profiled_subsystem_test.')
else:
namespaces = ['frc971', 'control_loops']
linear_system.WriteLinearSystem(kIntake, argv[1:3], argv[3:5],
diff --git a/frc971/control_loops/state_feedback_loop.h b/frc971/control_loops/state_feedback_loop.h
index fc3ff34..0da8bd6 100644
--- a/frc971/control_loops/state_feedback_loop.h
+++ b/frc971/control_loops/state_feedback_loop.h
@@ -3,10 +3,10 @@
#include <assert.h>
+#include <chrono>
#include <memory>
#include <utility>
#include <vector>
-#include <chrono>
#include "Eigen/Dense"
#include "unsupported/Eigen/MatrixFunctions"
@@ -72,8 +72,7 @@
Reset();
}
- StateFeedbackPlant(StateFeedbackPlant &&other)
- : index_(other.index_) {
+ StateFeedbackPlant(StateFeedbackPlant &&other) : index_(other.index_) {
::std::swap(coefficients_, other.coefficients_);
X_.swap(other.X_);
Y_.swap(other.Y_);
@@ -279,8 +278,8 @@
const Eigen::Matrix<Scalar, number_of_outputs, number_of_outputs> R;
StateFeedbackObserverCoefficients(
- const Eigen::Matrix<Scalar, number_of_states, number_of_outputs> &
- KalmanGain,
+ const Eigen::Matrix<Scalar, number_of_states, number_of_outputs>
+ &KalmanGain,
const Eigen::Matrix<Scalar, number_of_states, number_of_states> &Q,
const Eigen::Matrix<Scalar, number_of_outputs, number_of_outputs> &R)
: KalmanGain(KalmanGain), Q(Q), R(R) {}
@@ -294,7 +293,8 @@
explicit StateFeedbackObserver(
::std::vector<::std::unique_ptr<StateFeedbackObserverCoefficients<
- number_of_states, number_of_inputs, number_of_outputs, Scalar>>> *observers)
+ number_of_states, number_of_inputs, number_of_outputs, Scalar>>>
+ *observers)
: coefficients_(::std::move(*observers)) {}
StateFeedbackObserver(StateFeedbackObserver &&other)
@@ -302,7 +302,8 @@
::std::swap(coefficients_, other.coefficients_);
}
- const Eigen::Matrix<Scalar, number_of_states, number_of_outputs> &KalmanGain() const {
+ const Eigen::Matrix<Scalar, number_of_states, number_of_outputs> &KalmanGain()
+ const {
return coefficients().KalmanGain;
}
Scalar KalmanGain(int i, int j) const { return KalmanGain()(i, j); }
@@ -328,8 +329,7 @@
number_of_outputs, Scalar> &plant,
const Eigen::Matrix<Scalar, number_of_inputs, 1> &U,
const Eigen::Matrix<Scalar, number_of_outputs, 1> &Y) {
- mutable_X_hat() +=
- KalmanGain() * (Y - plant.C() * X_hat() - plant.D() * U);
+ mutable_X_hat() += KalmanGain() * (Y - plant.C() * X_hat() - plant.D() * U);
}
// Sets the current controller to be index, clamped to be within range.
diff --git a/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem.h b/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem.h
index 7b7b370..635db59 100644
--- a/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem.h
+++ b/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem.h
@@ -110,7 +110,8 @@
};
template <typename ZeroingEstimator, typename ProfiledJointStatus>
-void StaticZeroingSingleDOFProfiledSubsystem<ZeroingEstimator, ProfiledJointStatus>::Reset() {
+void StaticZeroingSingleDOFProfiledSubsystem<ZeroingEstimator,
+ ProfiledJointStatus>::Reset() {
state_ = State::UNINITIALIZED;
clear_min_position();
clear_max_position();
@@ -118,10 +119,11 @@
}
template <typename ZeroingEstimator, typename ProfiledJointStatus>
-void StaticZeroingSingleDOFProfiledSubsystem<ZeroingEstimator, ProfiledJointStatus>::Iterate(
- const StaticZeroingSingleDOFProfiledSubsystemGoal *goal,
- const typename ZeroingEstimator::Position *position, double *output,
- ProfiledJointStatus *status) {
+void StaticZeroingSingleDOFProfiledSubsystem<ZeroingEstimator,
+ ProfiledJointStatus>::
+ Iterate(const StaticZeroingSingleDOFProfiledSubsystemGoal *goal,
+ const typename ZeroingEstimator::Position *position, double *output,
+ ProfiledJointStatus *status) {
bool disabled = output == nullptr;
profiled_subsystem_.Correct(*position);
diff --git a/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem_test.cc b/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem_test.cc
index 9a9c092..66b1ab9 100644
--- a/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem_test.cc
+++ b/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem_test.cc
@@ -53,13 +53,7 @@
template <>
const zeroing::AbsoluteEncoderZeroingEstimator::ZeroingConstants
TestIntakeSystemValues<zeroing::AbsoluteEncoderZeroingEstimator>::kZeroing{
- kZeroingSampleSize,
- kEncoderIndexDifference,
- 0.0,
- 0.2,
- 0.0005,
- 20,
- 1.9};
+ kZeroingSampleSize, kEncoderIndexDifference, 0.0, 0.2, 0.0005, 20, 1.9};
template <typename ZeroingEstimator>
const StaticZeroingSingleDOFProfiledSubsystemParams<ZeroingEstimator>
@@ -94,8 +88,7 @@
TestIntakeSystemSimulation()
: subsystem_plant_(new CappedTestPlant(
::frc971::control_loops::MakeTestIntakeSystemPlant())),
- subsystem_sensor_sim_(
- kEncoderIndexDifference) {
+ subsystem_sensor_sim_(kEncoderIndexDifference) {
// Start the subsystem out in the middle by default.
InitializeSubsystemPosition((kRange.lower + kRange.upper) / 2.0);
}
diff --git a/y2014/control_loops/python/claw.py b/y2014/control_loops/python/claw.py
index 3ae9fe9..cade03d 100755
--- a/y2014/control_loops/python/claw.py
+++ b/y2014/control_loops/python/claw.py
@@ -13,493 +13,500 @@
FLAGS = gflags.FLAGS
try:
- gflags.DEFINE_bool('plot', False, 'If true, plot the loop response.')
+ gflags.DEFINE_bool('plot', False, 'If true, plot the loop response.')
except gflags.DuplicateFlagError:
- pass
+ pass
+
class Claw(control_loop.ControlLoop):
- def __init__(self, name="RawClaw"):
- super(Claw, self).__init__(name)
- # Stall Torque in N m
- self.stall_torque = 2.42
- # Stall Current in Amps
- self.stall_current = 133
- # Free Speed in RPM
- self.free_speed = 5500.0
- # Free Current in Amps
- self.free_current = 2.7
- # Moment of inertia of the claw in kg m^2
- self.J_top = 2.8
- self.J_bottom = 3.0
- # Resistance of the motor
- self.R = 12.0 / self.stall_current
- # Motor velocity constant
- self.Kv = ((self.free_speed / 60.0 * 2.0 * numpy.pi) /
- (13.5 - self.R * self.free_current))
- # Torque constant
- self.Kt = self.stall_torque / self.stall_current
- # Gear ratio
- self.G = 14.0 / 48.0 * 18.0 / 32.0 * 18.0 / 66.0 * 12.0 / 60.0
- # Control loop time step
- self.dt = 0.005
+ def __init__(self, name="RawClaw"):
+ super(Claw, self).__init__(name)
+ # Stall Torque in N m
+ self.stall_torque = 2.42
+ # Stall Current in Amps
+ self.stall_current = 133
+ # Free Speed in RPM
+ self.free_speed = 5500.0
+ # Free Current in Amps
+ self.free_current = 2.7
+ # Moment of inertia of the claw in kg m^2
+ self.J_top = 2.8
+ self.J_bottom = 3.0
- # State is [bottom position, bottom velocity, top - bottom position,
- # top - bottom velocity]
- # Input is [bottom power, top power - bottom power * J_top / J_bottom]
- # Motor time constants. difference_bottom refers to the constant for how the
- # bottom velocity affects the difference of the top and bottom velocities.
- self.common_motor_constant = -self.Kt / self.Kv / (self.G * self.G * self.R)
- self.bottom_bottom = self.common_motor_constant / self.J_bottom
- self.difference_bottom = -self.common_motor_constant * (1 / self.J_bottom
- - 1 / self.J_top)
- self.difference_difference = self.common_motor_constant / self.J_top
- # State feedback matrices
+ # Resistance of the motor
+ self.R = 12.0 / self.stall_current
+ # Motor velocity constant
+ self.Kv = ((self.free_speed / 60.0 * 2.0 * numpy.pi) /
+ (13.5 - self.R * self.free_current))
+ # Torque constant
+ self.Kt = self.stall_torque / self.stall_current
+ # Gear ratio
+ self.G = 14.0 / 48.0 * 18.0 / 32.0 * 18.0 / 66.0 * 12.0 / 60.0
+ # Control loop time step
+ self.dt = 0.005
- self.A_continuous = numpy.matrix(
- [[0, 0, 1, 0],
- [0, 0, 0, 1],
- [0, 0, self.bottom_bottom, 0],
- [0, 0, self.difference_bottom, self.difference_difference]])
+ # State is [bottom position, bottom velocity, top - bottom position,
+ # top - bottom velocity]
+ # Input is [bottom power, top power - bottom power * J_top / J_bottom]
+ # Motor time constants. difference_bottom refers to the constant for how the
+ # bottom velocity affects the difference of the top and bottom velocities.
+ self.common_motor_constant = -self.Kt / self.Kv / (
+ self.G * self.G * self.R)
+ self.bottom_bottom = self.common_motor_constant / self.J_bottom
+ self.difference_bottom = -self.common_motor_constant * (
+ 1 / self.J_bottom - 1 / self.J_top)
+ self.difference_difference = self.common_motor_constant / self.J_top
+ # State feedback matrices
- self.A_bottom_cont = numpy.matrix(
- [[0, 1],
- [0, self.bottom_bottom]])
+ self.A_continuous = numpy.matrix(
+ [[0, 0, 1, 0], [0, 0, 0, 1], [0, 0, self.bottom_bottom, 0],
+ [0, 0, self.difference_bottom, self.difference_difference]])
- self.A_diff_cont = numpy.matrix(
- [[0, 1],
- [0, self.difference_difference]])
+ self.A_bottom_cont = numpy.matrix([[0, 1], [0, self.bottom_bottom]])
- self.motor_feedforward = self.Kt / (self.G * self.R)
- self.motor_feedforward_bottom = self.motor_feedforward / self.J_bottom
- self.motor_feedforward_difference = self.motor_feedforward / self.J_top
- self.motor_feedforward_difference_bottom = (
- self.motor_feedforward * (1 / self.J_bottom - 1 / self.J_top))
- self.B_continuous = numpy.matrix(
- [[0, 0],
- [0, 0],
- [self.motor_feedforward_bottom, 0],
- [-self.motor_feedforward_bottom, self.motor_feedforward_difference]])
+ self.A_diff_cont = numpy.matrix([[0, 1],
+ [0, self.difference_difference]])
- glog.debug('Cont X_ss %f', self.motor_feedforward / -self.common_motor_constant)
+ self.motor_feedforward = self.Kt / (self.G * self.R)
+ self.motor_feedforward_bottom = self.motor_feedforward / self.J_bottom
+ self.motor_feedforward_difference = self.motor_feedforward / self.J_top
+ self.motor_feedforward_difference_bottom = (
+ self.motor_feedforward * (1 / self.J_bottom - 1 / self.J_top))
+ self.B_continuous = numpy.matrix([[0, 0], [0, 0],
+ [self.motor_feedforward_bottom, 0],
+ [
+ -self.motor_feedforward_bottom,
+ self.motor_feedforward_difference
+ ]])
- self.B_bottom_cont = numpy.matrix(
- [[0],
- [self.motor_feedforward_bottom]])
+ glog.debug('Cont X_ss %f',
+ self.motor_feedforward / -self.common_motor_constant)
- self.B_diff_cont = numpy.matrix(
- [[0],
- [self.motor_feedforward_difference]])
+ self.B_bottom_cont = numpy.matrix([[0],
+ [self.motor_feedforward_bottom]])
- self.C = numpy.matrix([[1, 0, 0, 0],
- [1, 1, 0, 0]])
- self.D = numpy.matrix([[0, 0],
- [0, 0]])
+ self.B_diff_cont = numpy.matrix([[0],
+ [self.motor_feedforward_difference]])
- self.A, self.B = self.ContinuousToDiscrete(
- self.A_continuous, self.B_continuous, self.dt)
+ self.C = numpy.matrix([[1, 0, 0, 0], [1, 1, 0, 0]])
+ self.D = numpy.matrix([[0, 0], [0, 0]])
- self.A_bottom, self.B_bottom = controls.c2d(
- self.A_bottom_cont, self.B_bottom_cont, self.dt)
- self.A_diff, self.B_diff = controls.c2d(
- self.A_diff_cont, self.B_diff_cont, self.dt)
+ self.A, self.B = self.ContinuousToDiscrete(self.A_continuous,
+ self.B_continuous, self.dt)
- self.K_bottom = controls.dplace(self.A_bottom, self.B_bottom,
- [0.87 + 0.05j, 0.87 - 0.05j])
- self.K_diff = controls.dplace(self.A_diff, self.B_diff,
- [0.85 + 0.05j, 0.85 - 0.05j])
+ self.A_bottom, self.B_bottom = controls.c2d(self.A_bottom_cont,
+ self.B_bottom_cont, self.dt)
+ self.A_diff, self.B_diff = controls.c2d(self.A_diff_cont,
+ self.B_diff_cont, self.dt)
- glog.debug('K_diff %s', str(self.K_diff))
- glog.debug('K_bottom %s', str(self.K_bottom))
+ self.K_bottom = controls.dplace(self.A_bottom, self.B_bottom,
+ [0.87 + 0.05j, 0.87 - 0.05j])
+ self.K_diff = controls.dplace(self.A_diff, self.B_diff,
+ [0.85 + 0.05j, 0.85 - 0.05j])
- glog.debug('A')
- glog.debug(self.A)
- glog.debug('B')
- glog.debug(self.B)
+ glog.debug('K_diff %s', str(self.K_diff))
+ glog.debug('K_bottom %s', str(self.K_bottom))
-
- self.Q = numpy.matrix([[(1.0 / (0.10 ** 2.0)), 0.0, 0.0, 0.0],
- [0.0, (1.0 / (0.06 ** 2.0)), 0.0, 0.0],
- [0.0, 0.0, 0.10, 0.0],
- [0.0, 0.0, 0.0, 0.1]])
+ glog.debug('A')
+ glog.debug(self.A)
+ glog.debug('B')
+ glog.debug(self.B)
- self.R = numpy.matrix([[(1.0 / (40.0 ** 2.0)), 0.0],
- [0.0, (1.0 / (5.0 ** 2.0))]])
- #self.K = controls.dlqr(self.A, self.B, self.Q, self.R)
+ self.Q = numpy.matrix([[(1.0 / (0.10**2.0)), 0.0, 0.0, 0.0],
+ [0.0, (1.0 / (0.06**2.0)), 0.0, 0.0],
+ [0.0, 0.0, 0.10, 0.0], [0.0, 0.0, 0.0, 0.1]])
- self.K = numpy.matrix([[self.K_bottom[0, 0], 0.0, self.K_bottom[0, 1], 0.0],
- [0.0, self.K_diff[0, 0], 0.0, self.K_diff[0, 1]]])
+ self.R = numpy.matrix([[(1.0 / (40.0**2.0)), 0.0],
+ [0.0, (1.0 / (5.0**2.0))]])
+ #self.K = controls.dlqr(self.A, self.B, self.Q, self.R)
- # Compute the feed forwards aceleration term.
- self.K[1, 0] = -self.B[1, 0] * self.K[0, 0] / self.B[1, 1]
+ self.K = numpy.matrix(
+ [[self.K_bottom[0, 0], 0.0, self.K_bottom[0, 1], 0.0],
+ [0.0, self.K_diff[0, 0], 0.0, self.K_diff[0, 1]]])
- lstsq_A = numpy.identity(2)
- lstsq_A[0, :] = self.B[1, :]
- lstsq_A[1, :] = self.B[3, :]
- glog.debug('System of Equations coefficients:')
- glog.debug(str(lstsq_A))
- glog.debug('det %s', str(numpy.linalg.det(lstsq_A)))
+ # Compute the feed forwards aceleration term.
+ self.K[1, 0] = -self.B[1, 0] * self.K[0, 0] / self.B[1, 1]
- out_x = numpy.linalg.lstsq(
- lstsq_A,
- numpy.matrix([[self.A[1, 2]], [self.A[3, 2]]]))[0]
- self.K[1, 2] = -lstsq_A[0, 0] * (self.K[0, 2] - out_x[0]) / lstsq_A[0, 1] + out_x[1]
+ lstsq_A = numpy.identity(2)
+ lstsq_A[0, :] = self.B[1, :]
+ lstsq_A[1, :] = self.B[3, :]
+ glog.debug('System of Equations coefficients:')
+ glog.debug(str(lstsq_A))
+ glog.debug('det %s', str(numpy.linalg.det(lstsq_A)))
- glog.debug('K unaugmented')
- glog.debug(str(self.K))
- glog.debug('B * K unaugmented')
- glog.debug(str(self.B * self.K))
- F = self.A - self.B * self.K
- glog.debug('A - B * K unaugmented')
- glog.debug(str(F))
- glog.debug('eigenvalues')
- glog.debug(str(numpy.linalg.eig(F)[0]))
+ out_x = numpy.linalg.lstsq(
+ lstsq_A, numpy.matrix([[self.A[1, 2]], [self.A[3, 2]]]))[0]
+ self.K[1, 2] = -lstsq_A[0, 0] * (
+ self.K[0, 2] - out_x[0]) / lstsq_A[0, 1] + out_x[1]
- self.rpl = .09
- self.ipl = 0.030
- self.PlaceObserverPoles([self.rpl + 1j * self.ipl,
- self.rpl + 1j * self.ipl,
- self.rpl - 1j * self.ipl,
- self.rpl - 1j * self.ipl])
+ glog.debug('K unaugmented')
+ glog.debug(str(self.K))
+ glog.debug('B * K unaugmented')
+ glog.debug(str(self.B * self.K))
+ F = self.A - self.B * self.K
+ glog.debug('A - B * K unaugmented')
+ glog.debug(str(F))
+ glog.debug('eigenvalues')
+ glog.debug(str(numpy.linalg.eig(F)[0]))
- # The box formed by U_min and U_max must encompass all possible values,
- # or else Austin's code gets angry.
- self.U_max = numpy.matrix([[12.0], [12.0]])
- self.U_min = numpy.matrix([[-12.0], [-12.0]])
+ self.rpl = .09
+ self.ipl = 0.030
+ self.PlaceObserverPoles([
+ self.rpl + 1j * self.ipl, self.rpl + 1j * self.ipl,
+ self.rpl - 1j * self.ipl, self.rpl - 1j * self.ipl
+ ])
- # For the tests that check the limits, these are (upper, lower) for both
- # claws.
- self.hard_pos_limits = None
- self.pos_limits = None
+ # The box formed by U_min and U_max must encompass all possible values,
+ # or else Austin's code gets angry.
+ self.U_max = numpy.matrix([[12.0], [12.0]])
+ self.U_min = numpy.matrix([[-12.0], [-12.0]])
- # Compute the steady state velocities for a given applied voltage.
- # The top and bottom of the claw should spin at the same rate if the
- # physics is right.
- X_ss = numpy.matrix([[0], [0], [0.0], [0]])
-
- U = numpy.matrix([[1.0],[1.0]])
- A = self.A
- B = self.B
- #X_ss[2, 0] = X_ss[2, 0] * A[2, 2] + B[2, 0] * U[0, 0]
- X_ss[2, 0] = 1 / (1 - A[2, 2]) * B[2, 0] * U[0, 0]
- #X_ss[3, 0] = X_ss[3, 0] * A[3, 3] + X_ss[2, 0] * A[3, 2] + B[3, 0] * U[0, 0] + B[3, 1] * U[1, 0]
- #X_ss[3, 0] * (1 - A[3, 3]) = X_ss[2, 0] * A[3, 2] + B[3, 0] * U[0, 0] + B[3, 1] * U[1, 0]
- X_ss[3, 0] = 1 / (1 - A[3, 3]) * (X_ss[2, 0] * A[3, 2] + B[3, 1] * U[1, 0] + B[3, 0] * U[0, 0])
- #X_ss[3, 0] = 1 / (1 - A[3, 3]) / (1 - A[2, 2]) * B[2, 0] * U[0, 0] * A[3, 2] + B[3, 0] * U[0, 0] + B[3, 1] * U[1, 0]
- X_ss[0, 0] = A[0, 2] * X_ss[2, 0] + B[0, 0] * U[0, 0]
- X_ss[1, 0] = A[1, 2] * X_ss[2, 0] + A[1, 3] * X_ss[3, 0] + B[1, 0] * U[0, 0] + B[1, 1] * U[1, 0]
+ # For the tests that check the limits, these are (upper, lower) for both
+ # claws.
+ self.hard_pos_limits = None
+ self.pos_limits = None
- glog.debug('X_ss %s', str(X_ss))
+ # Compute the steady state velocities for a given applied voltage.
+ # The top and bottom of the claw should spin at the same rate if the
+ # physics is right.
+ X_ss = numpy.matrix([[0], [0], [0.0], [0]])
- self.InitializeState()
+ U = numpy.matrix([[1.0], [1.0]])
+ A = self.A
+ B = self.B
+ #X_ss[2, 0] = X_ss[2, 0] * A[2, 2] + B[2, 0] * U[0, 0]
+ X_ss[2, 0] = 1 / (1 - A[2, 2]) * B[2, 0] * U[0, 0]
+ #X_ss[3, 0] = X_ss[3, 0] * A[3, 3] + X_ss[2, 0] * A[3, 2] + B[3, 0] * U[0, 0] + B[3, 1] * U[1, 0]
+ #X_ss[3, 0] * (1 - A[3, 3]) = X_ss[2, 0] * A[3, 2] + B[3, 0] * U[0, 0] + B[3, 1] * U[1, 0]
+ X_ss[3, 0] = 1 / (1 - A[3, 3]) * (
+ X_ss[2, 0] * A[3, 2] + B[3, 1] * U[1, 0] + B[3, 0] * U[0, 0])
+ #X_ss[3, 0] = 1 / (1 - A[3, 3]) / (1 - A[2, 2]) * B[2, 0] * U[0, 0] * A[3, 2] + B[3, 0] * U[0, 0] + B[3, 1] * U[1, 0]
+ X_ss[0, 0] = A[0, 2] * X_ss[2, 0] + B[0, 0] * U[0, 0]
+ X_ss[1, 0] = (A[1, 2] * X_ss[2, 0]) + (A[1, 3] * X_ss[3, 0]) + (
+ B[1, 0] * U[0, 0]) + (B[1, 1] * U[1, 0])
+
+ glog.debug('X_ss %s', str(X_ss))
+
+ self.InitializeState()
class ClawDeltaU(Claw):
- def __init__(self, name="Claw"):
- super(ClawDeltaU, self).__init__(name)
- A_unaugmented = self.A
- B_unaugmented = self.B
- C_unaugmented = self.C
- self.A = numpy.matrix([[0.0, 0.0, 0.0, 0.0, 0.0],
- [0.0, 0.0, 0.0, 0.0, 0.0],
- [0.0, 0.0, 0.0, 0.0, 0.0],
- [0.0, 0.0, 0.0, 0.0, 0.0],
- [0.0, 0.0, 0.0, 0.0, 1.0]])
- self.A[0:4, 0:4] = A_unaugmented
- self.A[0:4, 4] = B_unaugmented[0:4, 0]
+ def __init__(self, name="Claw"):
+ super(ClawDeltaU, self).__init__(name)
+ A_unaugmented = self.A
+ B_unaugmented = self.B
+ C_unaugmented = self.C
- self.B = numpy.matrix([[0.0, 0.0],
- [0.0, 0.0],
- [0.0, 0.0],
- [0.0, 0.0],
- [1.0, 0.0]])
- self.B[0:4, 1] = B_unaugmented[0:4, 1]
+ self.A = numpy.matrix([[0.0, 0.0, 0.0, 0.0, 0.0],
+ [0.0, 0.0, 0.0, 0.0, 0.0],
+ [0.0, 0.0, 0.0, 0.0, 0.0],
+ [0.0, 0.0, 0.0, 0.0, 0.0],
+ [0.0, 0.0, 0.0, 0.0, 1.0]])
+ self.A[0:4, 0:4] = A_unaugmented
+ self.A[0:4, 4] = B_unaugmented[0:4, 0]
- self.C = numpy.concatenate((C_unaugmented, numpy.matrix([[0.0], [0.0]])),
- axis=1)
- self.D = numpy.matrix([[0.0, 0.0],
- [0.0, 0.0]])
+ self.B = numpy.matrix([[0.0, 0.0], [0.0, 0.0], [0.0, 0.0], [0.0, 0.0],
+ [1.0, 0.0]])
+ self.B[0:4, 1] = B_unaugmented[0:4, 1]
- #self.PlaceControllerPoles([0.55, 0.35, 0.55, 0.35, 0.80])
- self.Q = numpy.matrix([[(1.0 / (0.04 ** 2.0)), 0.0, 0.0, 0.0, 0.0],
- [0.0, (1.0 / (0.01 ** 2)), 0.0, 0.0, 0.0],
- [0.0, 0.0, 0.01, 0.0, 0.0],
- [0.0, 0.0, 0.0, 0.08, 0.0],
- [0.0, 0.0, 0.0, 0.0, (1.0 / (10.0 ** 2))]])
+ self.C = numpy.concatenate(
+ (C_unaugmented, numpy.matrix([[0.0], [0.0]])), axis=1)
+ self.D = numpy.matrix([[0.0, 0.0], [0.0, 0.0]])
- self.R = numpy.matrix([[0.000001, 0.0],
- [0.0, 1.0 / (10.0 ** 2.0)]])
- self.K = controls.dlqr(self.A, self.B, self.Q, self.R)
+ #self.PlaceControllerPoles([0.55, 0.35, 0.55, 0.35, 0.80])
+ self.Q = numpy.matrix([[(1.0 / (0.04**2.0)), 0.0, 0.0, 0.0, 0.0],
+ [0.0, (1.0 / (0.01**2)), 0.0, 0.0, 0.0],
+ [0.0, 0.0, 0.01, 0.0, 0.0],
+ [0.0, 0.0, 0.0, 0.08, 0.0],
+ [0.0, 0.0, 0.0, 0.0, (1.0 / (10.0**2))]])
- self.K = numpy.matrix([[50.0, 0.0, 10.0, 0.0, 1.0],
- [50.0, 0.0, 10.0, 0.0, 1.0]])
+ self.R = numpy.matrix([[0.000001, 0.0], [0.0, 1.0 / (10.0**2.0)]])
+ self.K = controls.dlqr(self.A, self.B, self.Q, self.R)
- controllability = controls.ctrb(self.A, self.B)
- glog.debug('Rank of augmented controllability matrix: %d',
- numpy.linalg.matrix_rank(controllability))
+ self.K = numpy.matrix([[50.0, 0.0, 10.0, 0.0, 1.0],
+ [50.0, 0.0, 10.0, 0.0, 1.0]])
- glog.debug('K')
- glog.debug(str(self.K))
- glog.debug('Placed controller poles are')
- glog.debug(str(numpy.linalg.eig(self.A - self.B * self.K)[0]))
- glog.debug(str([numpy.abs(x) for x in
- numpy.linalg.eig(self.A - self.B * self.K)[0]]))
+ controllability = controls.ctrb(self.A, self.B)
+ glog.debug('Rank of augmented controllability matrix: %d',
+ numpy.linalg.matrix_rank(controllability))
- self.rpl = .05
- self.ipl = 0.008
- self.PlaceObserverPoles([self.rpl + 1j * self.ipl, 0.10, 0.09,
- self.rpl - 1j * self.ipl, 0.90])
- #print "A is"
- #print self.A
- #print "L is"
- #print self.L
- #print "C is"
- #print self.C
- #print "A - LC is"
- #print self.A - self.L * self.C
+ glog.debug('K')
+ glog.debug(str(self.K))
+ glog.debug('Placed controller poles are')
+ glog.debug(str(numpy.linalg.eig(self.A - self.B * self.K)[0]))
+ glog.debug(
+ str([
+ numpy.abs(x)
+ for x in numpy.linalg.eig(self.A - self.B * self.K)[0]
+ ]))
- #print "Placed observer poles are"
- #print numpy.linalg.eig(self.A - self.L * self.C)[0]
+ self.rpl = .05
+ self.ipl = 0.008
+ self.PlaceObserverPoles([
+ self.rpl + 1j * self.ipl, 0.10, 0.09, self.rpl - 1j * self.ipl, 0.90
+ ])
+ #print "A is"
+ #print self.A
+ #print "L is"
+ #print self.L
+ #print "C is"
+ #print self.C
+ #print "A - LC is"
+ #print self.A - self.L * self.C
- self.U_max = numpy.matrix([[12.0], [12.0]])
- self.U_min = numpy.matrix([[-12.0], [-12.0]])
+ #print "Placed observer poles are"
+ #print numpy.linalg.eig(self.A - self.L * self.C)[0]
- self.InitializeState()
+ self.U_max = numpy.matrix([[12.0], [12.0]])
+ self.U_min = numpy.matrix([[-12.0], [-12.0]])
+
+ self.InitializeState()
+
def ScaleU(claw, U, K, error):
- """Clips U as necessary.
+ """Clips U as necessary.
Args:
- claw: claw object containing moments of inertia and U limits.
- U: Input matrix to clip as necessary.
- """
+ claw: claw object containing moments of inertia and U limits.
+ U: Input matrix to clip as necessary.
+ """
- bottom_u = U[0, 0]
- top_u = U[1, 0]
- position_error = error[0:2, 0]
- velocity_error = error[2:, 0]
+ bottom_u = U[0, 0]
+ top_u = U[1, 0]
+ position_error = error[0:2, 0]
+ velocity_error = error[2:, 0]
- U_poly = polytope.HPolytope(
- numpy.matrix([[1, 0],
- [-1, 0],
- [0, 1],
- [0, -1]]),
- numpy.matrix([[12],
- [12],
- [12],
- [12]]))
+ U_poly = polytope.HPolytope(
+ numpy.matrix([[1, 0], [-1, 0], [0, 1], [0, -1]]),
+ numpy.matrix([[12], [12], [12], [12]]))
- if (bottom_u > claw.U_max[0, 0] or bottom_u < claw.U_min[0, 0] or
- top_u > claw.U_max[0, 0] or top_u < claw.U_min[0, 0]):
+ if (bottom_u > claw.U_max[0, 0] or bottom_u < claw.U_min[0, 0] or
+ top_u > claw.U_max[0, 0] or top_u < claw.U_min[0, 0]):
- position_K = K[:, 0:2]
- velocity_K = K[:, 2:]
+ position_K = K[:, 0:2]
+ velocity_K = K[:, 2:]
- # H * U <= k
- # U = UPos + UVel
- # H * (UPos + UVel) <= k
- # H * UPos <= k - H * UVel
- #
- # Now, we can do a coordinate transformation and say the following.
- #
- # UPos = position_K * position_error
- # (H * position_K) * position_error <= k - H * UVel
- #
- # Add in the constraint that 0 <= t <= 1
- # Now, there are 2 ways this can go. Either we have a region, or we don't
- # have a region. If we have a region, then pick the largest t and go for it.
- # If we don't have a region, we need to pick a good comprimise.
+ # H * U <= k
+ # U = UPos + UVel
+ # H * (UPos + UVel) <= k
+ # H * UPos <= k - H * UVel
+ #
+ # Now, we can do a coordinate transformation and say the following.
+ #
+ # UPos = position_K * position_error
+ # (H * position_K) * position_error <= k - H * UVel
+ #
+ # Add in the constraint that 0 <= t <= 1
+ # Now, there are 2 ways this can go. Either we have a region, or we don't
+ # have a region. If we have a region, then pick the largest t and go for it.
+ # If we don't have a region, we need to pick a good comprimise.
- pos_poly = polytope.HPolytope(
- U_poly.H * position_K,
- U_poly.k - U_poly.H * velocity_K * velocity_error)
+ pos_poly = polytope.HPolytope(
+ U_poly.H * position_K,
+ U_poly.k - U_poly.H * velocity_K * velocity_error)
- # The actual angle for the line we call 45.
- angle_45 = numpy.matrix([[numpy.sqrt(3), 1]])
- if claw.pos_limits and claw.hard_pos_limits and claw.X[0, 0] + claw.X[1, 0] > claw.pos_limits[1]:
- angle_45 = numpy.matrix([[1, 1]])
+ # The actual angle for the line we call 45.
+ angle_45 = numpy.matrix([[numpy.sqrt(3), 1]])
+ if claw.pos_limits and claw.hard_pos_limits and (
+ claw.X[0, 0] + claw.X[1, 0]) > claw.pos_limits[1]:
+ angle_45 = numpy.matrix([[1, 1]])
- P = position_error
- L45 = numpy.multiply(numpy.matrix([[numpy.sign(P[1, 0]), -numpy.sign(P[0, 0])]]), angle_45)
- if L45[0, 1] == 0:
- L45[0, 1] = 1
- if L45[0, 0] == 0:
- L45[0, 0] = 1
- w45 = numpy.matrix([[0]])
+ P = position_error
+ L45 = numpy.multiply(
+ numpy.matrix([[numpy.sign(P[1, 0]), -numpy.sign(P[0, 0])]]),
+ angle_45)
+ if L45[0, 1] == 0:
+ L45[0, 1] = 1
+ if L45[0, 0] == 0:
+ L45[0, 0] = 1
+ w45 = numpy.matrix([[0]])
- if numpy.abs(P[0, 0]) > numpy.abs(P[1, 0]):
- LH = numpy.matrix([[0, 1]])
- else:
- LH = numpy.matrix([[1, 0]])
- wh = LH * P
- standard = numpy.concatenate((L45, LH))
- W = numpy.concatenate((w45, wh))
- intersection = numpy.linalg.inv(standard) * W
- adjusted_pos_error_h, is_inside_h = polydrivetrain.DoCoerceGoal(pos_poly,
- LH, wh, position_error)
- adjusted_pos_error_45, is_inside_45 = polydrivetrain.DoCoerceGoal(pos_poly,
- L45, w45, intersection)
- if pos_poly.IsInside(intersection):
- adjusted_pos_error = adjusted_pos_error_h
- else:
- if is_inside_h:
- if numpy.linalg.norm(adjusted_pos_error_h) > numpy.linalg.norm(adjusted_pos_error_45):
- adjusted_pos_error = adjusted_pos_error_h
+ if numpy.abs(P[0, 0]) > numpy.abs(P[1, 0]):
+ LH = numpy.matrix([[0, 1]])
else:
- adjusted_pos_error = adjusted_pos_error_45
- else:
- adjusted_pos_error = adjusted_pos_error_45
- #print adjusted_pos_error
+ LH = numpy.matrix([[1, 0]])
+ wh = LH * P
+ standard = numpy.concatenate((L45, LH))
+ W = numpy.concatenate((w45, wh))
+ intersection = numpy.linalg.inv(standard) * W
+ adjusted_pos_error_h, is_inside_h = polydrivetrain.DoCoerceGoal(
+ pos_poly, LH, wh, position_error)
+ adjusted_pos_error_45, is_inside_45 = polydrivetrain.DoCoerceGoal(
+ pos_poly, L45, w45, intersection)
+ if pos_poly.IsInside(intersection):
+ adjusted_pos_error = adjusted_pos_error_h
+ else:
+ if is_inside_h:
+ if numpy.linalg.norm(adjusted_pos_error_h) > numpy.linalg.norm(
+ adjusted_pos_error_45):
+ adjusted_pos_error = adjusted_pos_error_h
+ else:
+ adjusted_pos_error = adjusted_pos_error_45
+ else:
+ adjusted_pos_error = adjusted_pos_error_45
+ #print adjusted_pos_error
- #print "Actual power is ", velocity_K * velocity_error + position_K * adjusted_pos_error
- return velocity_K * velocity_error + position_K * adjusted_pos_error
+ #print "Actual power is ", velocity_K * velocity_error + position_K * adjusted_pos_error
+ return velocity_K * velocity_error + position_K * adjusted_pos_error
- #U = Kpos * poserror + Kvel * velerror
-
- #scalar = claw.U_max[0, 0] / max(numpy.abs(top_u), numpy.abs(bottom_u))
+ #U = Kpos * poserror + Kvel * velerror
- #top_u *= scalar
- #bottom_u *= scalar
+ #scalar = claw.U_max[0, 0] / max(numpy.abs(top_u), numpy.abs(bottom_u))
- return numpy.matrix([[bottom_u], [top_u]])
+ #top_u *= scalar
+ #bottom_u *= scalar
-def run_test(claw, initial_X, goal, max_separation_error=0.01, show_graph=True, iterations=200):
- """Runs the claw plant on a given claw (claw) with an initial condition (initial_X) and goal (goal).
+ return numpy.matrix([[bottom_u], [top_u]])
+
+
+def run_test(claw,
+ initial_X,
+ goal,
+ max_separation_error=0.01,
+ show_graph=True,
+ iterations=200):
+ """Runs the claw plant on a given claw (claw) with an initial condition (initial_X) and goal (goal).
The tests themselves are not terribly sophisticated; I just test for
whether the goal has been reached and whether the separation goes
outside of the initial and goal values by more than max_separation_error.
Prints out something for a failure of either condition and returns
False if tests fail.
+
Args:
- claw: claw object to use.
- initial_X: starting state.
- goal: goal state.
- show_graph: Whether or not to display a graph showing the changing
- states and voltages.
- iterations: Number of timesteps to run the model for."""
+ claw: claw object to use.
+ initial_X: starting state.
+ goal: goal state.
+ show_graph: Whether or not to display a graph showing the changing
+ states and voltages.
+ iterations: Number of timesteps to run the model for."""
- claw.X = initial_X
+ claw.X = initial_X
- # Various lists for graphing things.
- t = []
- x_bottom = []
- x_top = []
- u_bottom = []
- u_top = []
- x_separation = []
+ # Various lists for graphing things.
+ t = []
+ x_bottom = []
+ x_top = []
+ u_bottom = []
+ u_top = []
+ x_separation = []
- tests_passed = True
+ tests_passed = True
- # Bounds which separation should not exceed.
- lower_bound = (initial_X[1, 0] if initial_X[1, 0] < goal[1, 0]
- else goal[1, 0]) - max_separation_error
- upper_bound = (initial_X[1, 0] if initial_X[1, 0] > goal[1, 0]
- else goal[1, 0]) + max_separation_error
+ # Bounds which separation should not exceed.
+ lower_bound = (initial_X[1, 0] if initial_X[1, 0] < goal[1, 0] else
+ goal[1, 0]) - max_separation_error
+ upper_bound = (initial_X[1, 0] if initial_X[1, 0] > goal[1, 0] else
+ goal[1, 0]) + max_separation_error
- for i in xrange(iterations):
- U = claw.K * (goal - claw.X)
- U = ScaleU(claw, U, claw.K, goal - claw.X)
- claw.Update(U)
+ for i in xrange(iterations):
+ U = claw.K * (goal - claw.X)
+ U = ScaleU(claw, U, claw.K, goal - claw.X)
+ claw.Update(U)
- if claw.X[1, 0] > upper_bound or claw.X[1, 0] < lower_bound:
- tests_passed = False
- glog.info('Claw separation was %f', claw.X[1, 0])
- glog.info("Should have been between", lower_bound, "and", upper_bound)
+ if claw.X[1, 0] > upper_bound or claw.X[1, 0] < lower_bound:
+ tests_passed = False
+ glog.info('Claw separation was %f', claw.X[1, 0])
+ glog.info("Should have been between", lower_bound, "and",
+ upper_bound)
- if claw.hard_pos_limits and \
- (claw.X[0, 0] > claw.hard_pos_limits[1] or
- claw.X[0, 0] < claw.hard_pos_limits[0] or
- claw.X[0, 0] + claw.X[1, 0] > claw.hard_pos_limits[1] or
- claw.X[0, 0] + claw.X[1, 0] < claw.hard_pos_limits[0]):
- tests_passed = False
- glog.info('Claws at %f and %f', claw.X[0, 0], claw.X[0, 0] + claw.X[1, 0])
- glog.info("Both should be in %s, definitely %s",
- claw.pos_limits, claw.hard_pos_limits)
+ if claw.hard_pos_limits and \
+ (claw.X[0, 0] > claw.hard_pos_limits[1] or
+ claw.X[0, 0] < claw.hard_pos_limits[0] or
+ claw.X[0, 0] + claw.X[1, 0] > claw.hard_pos_limits[1] or
+ claw.X[0, 0] + claw.X[1, 0] < claw.hard_pos_limits[0]):
+ tests_passed = False
+ glog.info('Claws at %f and %f', claw.X[0, 0],
+ claw.X[0, 0] + claw.X[1, 0])
+ glog.info("Both should be in %s, definitely %s", claw.pos_limits,
+ claw.hard_pos_limits)
- t.append(i * claw.dt)
- x_bottom.append(claw.X[0, 0] * 10.0)
- x_top.append((claw.X[1, 0] + claw.X[0, 0]) * 10.0)
- u_bottom.append(U[0, 0])
- u_top.append(U[1, 0])
- x_separation.append(claw.X[1, 0] * 10.0)
+ t.append(i * claw.dt)
+ x_bottom.append(claw.X[0, 0] * 10.0)
+ x_top.append((claw.X[1, 0] + claw.X[0, 0]) * 10.0)
+ u_bottom.append(U[0, 0])
+ u_top.append(U[1, 0])
+ x_separation.append(claw.X[1, 0] * 10.0)
- if show_graph:
- pylab.plot(t, x_bottom, label='x bottom * 10')
- pylab.plot(t, x_top, label='x top * 10')
- pylab.plot(t, u_bottom, label='u bottom')
- pylab.plot(t, u_top, label='u top')
- pylab.plot(t, x_separation, label='separation * 10')
- pylab.legend()
- pylab.show()
+ if show_graph:
+ pylab.plot(t, x_bottom, label='x bottom * 10')
+ pylab.plot(t, x_top, label='x top * 10')
+ pylab.plot(t, u_bottom, label='u bottom')
+ pylab.plot(t, u_top, label='u top')
+ pylab.plot(t, x_separation, label='separation * 10')
+ pylab.legend()
+ pylab.show()
- # Test to make sure that we are near the goal.
- if numpy.max(abs(claw.X - goal)) > 1e-4:
- tests_passed = False
- glog.error('X was %s Expected %s', str(claw.X), str(goal))
+ # Test to make sure that we are near the goal.
+ if numpy.max(abs(claw.X - goal)) > 1e-4:
+ tests_passed = False
+ glog.error('X was %s Expected %s', str(claw.X), str(goal))
- return tests_passed
+ return tests_passed
+
def main(argv):
- argv = FLAGS(argv)
+ argv = FLAGS(argv)
- claw = Claw()
- if FLAGS.plot:
- # Test moving the claw with constant separation.
- initial_X = numpy.matrix([[-1.0], [0.0], [0.0], [0.0]])
- R = numpy.matrix([[1.0], [0.0], [0.0], [0.0]])
- run_test(claw, initial_X, R)
+ claw = Claw()
+ if FLAGS.plot:
+ # Test moving the claw with constant separation.
+ initial_X = numpy.matrix([[-1.0], [0.0], [0.0], [0.0]])
+ R = numpy.matrix([[1.0], [0.0], [0.0], [0.0]])
+ run_test(claw, initial_X, R)
- # Test just changing separation.
- initial_X = numpy.matrix([[0.0], [0.0], [0.0], [0.0]])
- R = numpy.matrix([[0.0], [1.0], [0.0], [0.0]])
- run_test(claw, initial_X, R)
+ # Test just changing separation.
+ initial_X = numpy.matrix([[0.0], [0.0], [0.0], [0.0]])
+ R = numpy.matrix([[0.0], [1.0], [0.0], [0.0]])
+ run_test(claw, initial_X, R)
- # Test changing both separation and position at once.
- initial_X = numpy.matrix([[0.0], [0.0], [0.0], [0.0]])
- R = numpy.matrix([[1.0], [1.0], [0.0], [0.0]])
- run_test(claw, initial_X, R)
+ # Test changing both separation and position at once.
+ initial_X = numpy.matrix([[0.0], [0.0], [0.0], [0.0]])
+ R = numpy.matrix([[1.0], [1.0], [0.0], [0.0]])
+ run_test(claw, initial_X, R)
- # Test a small separation error and a large position one.
- initial_X = numpy.matrix([[0.0], [0.0], [0.0], [0.0]])
- R = numpy.matrix([[2.0], [0.05], [0.0], [0.0]])
- run_test(claw, initial_X, R)
+ # Test a small separation error and a large position one.
+ initial_X = numpy.matrix([[0.0], [0.0], [0.0], [0.0]])
+ R = numpy.matrix([[2.0], [0.05], [0.0], [0.0]])
+ run_test(claw, initial_X, R)
- # Test a small separation error and a large position one.
- initial_X = numpy.matrix([[0.0], [0.0], [0.0], [0.0]])
- R = numpy.matrix([[-0.5], [1.0], [0.0], [0.0]])
- run_test(claw, initial_X, R)
+ # Test a small separation error and a large position one.
+ initial_X = numpy.matrix([[0.0], [0.0], [0.0], [0.0]])
+ R = numpy.matrix([[-0.5], [1.0], [0.0], [0.0]])
+ run_test(claw, initial_X, R)
- # Test opening with the top claw at the limit.
- initial_X = numpy.matrix([[0.0], [0.0], [0.0], [0.0]])
- R = numpy.matrix([[-1.5], [1.5], [0.0], [0.0]])
- claw.hard_pos_limits = (-1.6, 0.1)
- claw.pos_limits = (-1.5, 0.0)
- run_test(claw, initial_X, R)
- claw.pos_limits = None
+ # Test opening with the top claw at the limit.
+ initial_X = numpy.matrix([[0.0], [0.0], [0.0], [0.0]])
+ R = numpy.matrix([[-1.5], [1.5], [0.0], [0.0]])
+ claw.hard_pos_limits = (-1.6, 0.1)
+ claw.pos_limits = (-1.5, 0.0)
+ run_test(claw, initial_X, R)
+ claw.pos_limits = None
- # Test opening with the bottom claw at the limit.
- initial_X = numpy.matrix([[0.0], [0.0], [0.0], [0.0]])
- R = numpy.matrix([[0], [1.5], [0.0], [0.0]])
- claw.hard_pos_limits = (-0.1, 1.6)
- claw.pos_limits = (0.0, 1.6)
- run_test(claw, initial_X, R)
- claw.pos_limits = None
+ # Test opening with the bottom claw at the limit.
+ initial_X = numpy.matrix([[0.0], [0.0], [0.0], [0.0]])
+ R = numpy.matrix([[0], [1.5], [0.0], [0.0]])
+ claw.hard_pos_limits = (-0.1, 1.6)
+ claw.pos_limits = (0.0, 1.6)
+ run_test(claw, initial_X, R)
+ claw.pos_limits = None
- # Write the generated constants out to a file.
- if len(argv) != 3:
- glog.fatal('Expected .h file name and .cc file name for the claw.')
- else:
- namespaces = ['y2014', 'control_loops', 'claw']
- claw = Claw('Claw')
- loop_writer = control_loop.ControlLoopWriter('Claw', [claw],
- namespaces=namespaces)
- loop_writer.AddConstant(control_loop.Constant('kClawMomentOfInertiaRatio',
- '%f', claw.J_top / claw.J_bottom))
- loop_writer.AddConstant(control_loop.Constant('kDt', '%f',
- claw.dt))
- loop_writer.Write(argv[1], argv[2])
+ # Write the generated constants out to a file.
+ if len(argv) != 3:
+ glog.fatal('Expected .h file name and .cc file name for the claw.')
+ else:
+ namespaces = ['y2014', 'control_loops', 'claw']
+ claw = Claw('Claw')
+ loop_writer = control_loop.ControlLoopWriter(
+ 'Claw', [claw], namespaces=namespaces)
+ loop_writer.AddConstant(
+ control_loop.Constant('kClawMomentOfInertiaRatio', '%f',
+ claw.J_top / claw.J_bottom))
+ loop_writer.AddConstant(control_loop.Constant('kDt', '%f', claw.dt))
+ loop_writer.Write(argv[1], argv[2])
+
if __name__ == '__main__':
- sys.exit(main(sys.argv))
+ sys.exit(main(sys.argv))
diff --git a/y2014/control_loops/python/shooter.py b/y2014/control_loops/python/shooter.py
index 6a6bb3e..9287dae 100755
--- a/y2014/control_loops/python/shooter.py
+++ b/y2014/control_loops/python/shooter.py
@@ -13,272 +13,271 @@
gflags.DEFINE_bool('plot', False, 'If true, plot the loop response.')
+
class SprungShooter(control_loop.ControlLoop):
- def __init__(self, name="RawSprungShooter"):
- super(SprungShooter, self).__init__(name)
- # Stall Torque in N m
- self.stall_torque = .4982
- # Stall Current in Amps
- self.stall_current = 85
- # Free Speed in RPM
- self.free_speed = 19300.0
- # Free Current in Amps
- self.free_current = 1.2
- # Effective mass of the shooter in kg.
- # This rough estimate should about include the effect of the masses
- # of the gears. If this number is too low, the eigen values of self.A
- # will start to become extremely small.
- self.J = 200
- # Resistance of the motor, divided by the number of motors.
- self.R = 12.0 / self.stall_current / 2.0
- # Motor velocity constant
- self.Kv = ((self.free_speed / 60.0 * 2.0 * numpy.pi) /
- (12.0 - self.R * self.free_current))
- # Torque constant
- self.Kt = self.stall_torque / self.stall_current
- # Spring constant for the springs, N/m
- self.Ks = 2800.0
- # Maximum extension distance (Distance from the 0 force point on the
- # spring to the latch position.)
- self.max_extension = 0.32385
- # Gear ratio multiplied by radius of final sprocket.
- self.G = 10.0 / 40.0 * 20.0 / 54.0 * 24.0 / 54.0 * 20.0 / 84.0 * 16.0 * (3.0 / 8.0) / (2.0 * numpy.pi) * 0.0254
- # Control loop time step
- self.dt = 0.005
+ def __init__(self, name="RawSprungShooter"):
+ super(SprungShooter, self).__init__(name)
+ # Stall Torque in N m
+ self.stall_torque = .4982
+ # Stall Current in Amps
+ self.stall_current = 85
+ # Free Speed in RPM
+ self.free_speed = 19300.0
+ # Free Current in Amps
+ self.free_current = 1.2
+ # Effective mass of the shooter in kg.
+ # This rough estimate should about include the effect of the masses
+ # of the gears. If this number is too low, the eigen values of self.A
+ # will start to become extremely small.
+ self.J = 200
+ # Resistance of the motor, divided by the number of motors.
+ self.R = 12.0 / self.stall_current / 2.0
+ # Motor velocity constant
+ self.Kv = ((self.free_speed / 60.0 * 2.0 * numpy.pi) /
+ (12.0 - self.R * self.free_current))
+ # Torque constant
+ self.Kt = self.stall_torque / self.stall_current
+ # Spring constant for the springs, N/m
+ self.Ks = 2800.0
+ # Maximum extension distance (Distance from the 0 force point on the
+ # spring to the latch position.)
+ self.max_extension = 0.32385
+ # Gear ratio multiplied by radius of final sprocket.
+ self.G = 10.0 / 40.0 * 20.0 / 54.0 * 24.0 / 54.0 * 20.0 / 84.0 * 16.0 * (
+ 3.0 / 8.0) / (2.0 * numpy.pi) * 0.0254
- # State feedback matrices
- self.A_continuous = numpy.matrix(
- [[0, 1],
- [-self.Ks / self.J,
- -self.Kt / self.Kv / (self.J * self.G * self.G * self.R)]])
- self.B_continuous = numpy.matrix(
- [[0],
- [self.Kt / (self.J * self.G * self.R)]])
- self.C = numpy.matrix([[1, 0]])
- self.D = numpy.matrix([[0]])
+ # Control loop time step
+ self.dt = 0.005
- self.A, self.B = self.ContinuousToDiscrete(
- self.A_continuous, self.B_continuous, self.dt)
+ # State feedback matrices
+ self.A_continuous = numpy.matrix(
+ [[0, 1],
+ [
+ -self.Ks / self.J,
+ -self.Kt / self.Kv / (self.J * self.G * self.G * self.R)
+ ]])
+ self.B_continuous = numpy.matrix(
+ [[0], [self.Kt / (self.J * self.G * self.R)]])
+ self.C = numpy.matrix([[1, 0]])
+ self.D = numpy.matrix([[0]])
- self.PlaceControllerPoles([0.45, 0.45])
+ self.A, self.B = self.ContinuousToDiscrete(self.A_continuous,
+ self.B_continuous, self.dt)
- self.rpl = .05
- self.ipl = 0.008
- self.PlaceObserverPoles([self.rpl,
- self.rpl])
+ self.PlaceControllerPoles([0.45, 0.45])
- self.U_max = numpy.matrix([[12.0]])
- self.U_min = numpy.matrix([[-12.0]])
+ self.rpl = .05
+ self.ipl = 0.008
+ self.PlaceObserverPoles([self.rpl, self.rpl])
- self.InitializeState()
+ self.U_max = numpy.matrix([[12.0]])
+ self.U_min = numpy.matrix([[-12.0]])
+
+ self.InitializeState()
class Shooter(SprungShooter):
- def __init__(self, name="RawShooter"):
- super(Shooter, self).__init__(name)
- # State feedback matrices
- self.A_continuous = numpy.matrix(
- [[0, 1],
- [0, -self.Kt / self.Kv / (self.J * self.G * self.G * self.R)]])
- self.B_continuous = numpy.matrix(
- [[0],
- [self.Kt / (self.J * self.G * self.R)]])
+ def __init__(self, name="RawShooter"):
+ super(Shooter, self).__init__(name)
- self.A, self.B = self.ContinuousToDiscrete(
- self.A_continuous, self.B_continuous, self.dt)
+ # State feedback matrices
+ self.A_continuous = numpy.matrix(
+ [[0, 1],
+ [0, -self.Kt / self.Kv / (self.J * self.G * self.G * self.R)]])
+ self.B_continuous = numpy.matrix(
+ [[0], [self.Kt / (self.J * self.G * self.R)]])
- self.PlaceControllerPoles([0.45, 0.45])
+ self.A, self.B = self.ContinuousToDiscrete(self.A_continuous,
+ self.B_continuous, self.dt)
- self.rpl = .05
- self.ipl = 0.008
- self.PlaceObserverPoles([self.rpl,
- self.rpl])
+ self.PlaceControllerPoles([0.45, 0.45])
- self.U_max = numpy.matrix([[12.0]])
- self.U_min = numpy.matrix([[-12.0]])
+ self.rpl = .05
+ self.ipl = 0.008
+ self.PlaceObserverPoles([self.rpl, self.rpl])
- self.InitializeState()
+ self.U_max = numpy.matrix([[12.0]])
+ self.U_min = numpy.matrix([[-12.0]])
+
+ self.InitializeState()
class SprungShooterDeltaU(SprungShooter):
- def __init__(self, name="SprungShooter"):
- super(SprungShooterDeltaU, self).__init__(name)
- A_unaugmented = self.A
- B_unaugmented = self.B
- A_continuous_unaugmented = self.A_continuous
- B_continuous_unaugmented = self.B_continuous
+ def __init__(self, name="SprungShooter"):
+ super(SprungShooterDeltaU, self).__init__(name)
+ A_unaugmented = self.A
+ B_unaugmented = self.B
- self.A_continuous = numpy.matrix(numpy.zeros((3, 3)))
- self.A_continuous[0:2, 0:2] = A_continuous_unaugmented
- self.A_continuous[0:2, 2] = B_continuous_unaugmented
+ A_continuous_unaugmented = self.A_continuous
+ B_continuous_unaugmented = self.B_continuous
- self.B_continuous = numpy.matrix(numpy.zeros((3, 1)))
- self.B_continuous[2, 0] = 1.0 / self.dt
+ self.A_continuous = numpy.matrix(numpy.zeros((3, 3)))
+ self.A_continuous[0:2, 0:2] = A_continuous_unaugmented
+ self.A_continuous[0:2, 2] = B_continuous_unaugmented
- self.A = numpy.matrix([[0.0, 0.0, 0.0],
- [0.0, 0.0, 0.0],
- [0.0, 0.0, 1.0]])
- self.A[0:2, 0:2] = A_unaugmented
- self.A[0:2, 2] = B_unaugmented
+ self.B_continuous = numpy.matrix(numpy.zeros((3, 1)))
+ self.B_continuous[2, 0] = 1.0 / self.dt
- self.B = numpy.matrix([[0.0],
- [0.0],
- [1.0]])
+ self.A = numpy.matrix([[0.0, 0.0, 0.0], [0.0, 0.0, 0.0],
+ [0.0, 0.0, 1.0]])
+ self.A[0:2, 0:2] = A_unaugmented
+ self.A[0:2, 2] = B_unaugmented
- self.C = numpy.matrix([[1.0, 0.0, 0.0]])
- self.D = numpy.matrix([[0.0]])
+ self.B = numpy.matrix([[0.0], [0.0], [1.0]])
- self.PlaceControllerPoles([0.50, 0.35, 0.80])
+ self.C = numpy.matrix([[1.0, 0.0, 0.0]])
+ self.D = numpy.matrix([[0.0]])
- glog.debug('K')
- glog.debug(str(self.K))
- glog.debug('Placed controller poles are')
- glog.debug(str(numpy.linalg.eig(self.A - self.B * self.K)[0]))
+ self.PlaceControllerPoles([0.50, 0.35, 0.80])
- self.rpl = .05
- self.ipl = 0.008
- self.PlaceObserverPoles([self.rpl + 1j * self.ipl,
- self.rpl - 1j * self.ipl, 0.90])
- glog.debug('Placed observer poles are')
- glog.debug(str(numpy.linalg.eig(self.A - self.L * self.C)[0]))
+ glog.debug('K')
+ glog.debug(str(self.K))
+ glog.debug('Placed controller poles are')
+ glog.debug(str(numpy.linalg.eig(self.A - self.B * self.K)[0]))
- self.U_max = numpy.matrix([[12.0]])
- self.U_min = numpy.matrix([[-12.0]])
+ self.rpl = .05
+ self.ipl = 0.008
+ self.PlaceObserverPoles(
+ [self.rpl + 1j * self.ipl, self.rpl - 1j * self.ipl, 0.90])
+ glog.debug('Placed observer poles are')
+ glog.debug(str(numpy.linalg.eig(self.A - self.L * self.C)[0]))
- self.InitializeState()
+ self.U_max = numpy.matrix([[12.0]])
+ self.U_min = numpy.matrix([[-12.0]])
+
+ self.InitializeState()
class ShooterDeltaU(Shooter):
- def __init__(self, name="Shooter"):
- super(ShooterDeltaU, self).__init__(name)
- A_unaugmented = self.A
- B_unaugmented = self.B
- A_continuous_unaugmented = self.A_continuous
- B_continuous_unaugmented = self.B_continuous
+ def __init__(self, name="Shooter"):
+ super(ShooterDeltaU, self).__init__(name)
+ A_unaugmented = self.A
+ B_unaugmented = self.B
- self.A_continuous = numpy.matrix(numpy.zeros((3, 3)))
- self.A_continuous[0:2, 0:2] = A_continuous_unaugmented
- self.A_continuous[0:2, 2] = B_continuous_unaugmented
+ A_continuous_unaugmented = self.A_continuous
+ B_continuous_unaugmented = self.B_continuous
- self.B_continuous = numpy.matrix(numpy.zeros((3, 1)))
- self.B_continuous[2, 0] = 1.0 / self.dt
+ self.A_continuous = numpy.matrix(numpy.zeros((3, 3)))
+ self.A_continuous[0:2, 0:2] = A_continuous_unaugmented
+ self.A_continuous[0:2, 2] = B_continuous_unaugmented
- self.A = numpy.matrix([[0.0, 0.0, 0.0],
- [0.0, 0.0, 0.0],
- [0.0, 0.0, 1.0]])
- self.A[0:2, 0:2] = A_unaugmented
- self.A[0:2, 2] = B_unaugmented
+ self.B_continuous = numpy.matrix(numpy.zeros((3, 1)))
+ self.B_continuous[2, 0] = 1.0 / self.dt
- self.B = numpy.matrix([[0.0],
- [0.0],
- [1.0]])
+ self.A = numpy.matrix([[0.0, 0.0, 0.0], [0.0, 0.0, 0.0],
+ [0.0, 0.0, 1.0]])
+ self.A[0:2, 0:2] = A_unaugmented
+ self.A[0:2, 2] = B_unaugmented
- self.C = numpy.matrix([[1.0, 0.0, 0.0]])
- self.D = numpy.matrix([[0.0]])
+ self.B = numpy.matrix([[0.0], [0.0], [1.0]])
- self.PlaceControllerPoles([0.55, 0.45, 0.80])
+ self.C = numpy.matrix([[1.0, 0.0, 0.0]])
+ self.D = numpy.matrix([[0.0]])
- glog.debug('K')
- glog.debug(str(self.K))
- glog.debug('Placed controller poles are')
- glog.debug(str(numpy.linalg.eig(self.A - self.B * self.K)[0]))
+ self.PlaceControllerPoles([0.55, 0.45, 0.80])
- self.rpl = .05
- self.ipl = 0.008
- self.PlaceObserverPoles([self.rpl + 1j * self.ipl,
- self.rpl - 1j * self.ipl, 0.90])
- glog.debug('Placed observer poles are')
- glog.debug(str(numpy.linalg.eig(self.A - self.L * self.C)[0]))
+ glog.debug('K')
+ glog.debug(str(self.K))
+ glog.debug('Placed controller poles are')
+ glog.debug(str(numpy.linalg.eig(self.A - self.B * self.K)[0]))
- self.U_max = numpy.matrix([[12.0]])
- self.U_min = numpy.matrix([[-12.0]])
+ self.rpl = .05
+ self.ipl = 0.008
+ self.PlaceObserverPoles(
+ [self.rpl + 1j * self.ipl, self.rpl - 1j * self.ipl, 0.90])
+ glog.debug('Placed observer poles are')
+ glog.debug(str(numpy.linalg.eig(self.A - self.L * self.C)[0]))
- self.InitializeState()
+ self.U_max = numpy.matrix([[12.0]])
+ self.U_min = numpy.matrix([[-12.0]])
+
+ self.InitializeState()
def ClipDeltaU(shooter, old_voltage, delta_u):
- old_u = old_voltage
- new_u = numpy.clip(old_u + delta_u, shooter.U_min, shooter.U_max)
- return new_u - old_u
+ old_u = old_voltage
+ new_u = numpy.clip(old_u + delta_u, shooter.U_min, shooter.U_max)
+ return new_u - old_u
+
def main(argv):
- argv = FLAGS(argv)
+ argv = FLAGS(argv)
- # Simulate the response of the system to a goal.
- sprung_shooter = SprungShooterDeltaU()
- raw_sprung_shooter = SprungShooter()
- close_loop_x = []
- close_loop_u = []
- goal_position = -0.3
- R = numpy.matrix([[goal_position],
- [0.0],
- [-sprung_shooter.A[1, 0] / sprung_shooter.A[1, 2] *
- goal_position]])
- voltage = numpy.matrix([[0.0]])
- for _ in xrange(500):
- U = sprung_shooter.K * (R - sprung_shooter.X_hat)
- U = ClipDeltaU(sprung_shooter, voltage, U)
- sprung_shooter.Y = raw_sprung_shooter.Y + 0.01
- sprung_shooter.UpdateObserver(U)
- voltage += U
- raw_sprung_shooter.Update(voltage)
- close_loop_x.append(raw_sprung_shooter.X[0, 0] * 10)
- close_loop_u.append(voltage[0, 0])
+ # Simulate the response of the system to a goal.
+ sprung_shooter = SprungShooterDeltaU()
+ raw_sprung_shooter = SprungShooter()
+ close_loop_x = []
+ close_loop_u = []
+ goal_position = -0.3
+ R = numpy.matrix(
+ [[goal_position], [0.0],
+ [-sprung_shooter.A[1, 0] / sprung_shooter.A[1, 2] * goal_position]])
+ voltage = numpy.matrix([[0.0]])
+ for _ in xrange(500):
+ U = sprung_shooter.K * (R - sprung_shooter.X_hat)
+ U = ClipDeltaU(sprung_shooter, voltage, U)
+ sprung_shooter.Y = raw_sprung_shooter.Y + 0.01
+ sprung_shooter.UpdateObserver(U)
+ voltage += U
+ raw_sprung_shooter.Update(voltage)
+ close_loop_x.append(raw_sprung_shooter.X[0, 0] * 10)
+ close_loop_u.append(voltage[0, 0])
- if FLAGS.plot:
- pylab.plot(range(500), close_loop_x)
- pylab.plot(range(500), close_loop_u)
- pylab.show()
+ if FLAGS.plot:
+ pylab.plot(range(500), close_loop_x)
+ pylab.plot(range(500), close_loop_u)
+ pylab.show()
- shooter = ShooterDeltaU()
- raw_shooter = Shooter()
- close_loop_x = []
- close_loop_u = []
- goal_position = -0.3
- R = numpy.matrix([[goal_position], [0.0], [-shooter.A[1, 0] / shooter.A[1, 2] * goal_position]])
- voltage = numpy.matrix([[0.0]])
- for _ in xrange(500):
- U = shooter.K * (R - shooter.X_hat)
- U = ClipDeltaU(shooter, voltage, U)
- shooter.Y = raw_shooter.Y + 0.01
- shooter.UpdateObserver(U)
- voltage += U
- raw_shooter.Update(voltage)
- close_loop_x.append(raw_shooter.X[0, 0] * 10)
- close_loop_u.append(voltage[0, 0])
+ shooter = ShooterDeltaU()
+ raw_shooter = Shooter()
+ close_loop_x = []
+ close_loop_u = []
+ goal_position = -0.3
+ R = numpy.matrix([[goal_position], [0.0],
+ [-shooter.A[1, 0] / shooter.A[1, 2] * goal_position]])
+ voltage = numpy.matrix([[0.0]])
+ for _ in xrange(500):
+ U = shooter.K * (R - shooter.X_hat)
+ U = ClipDeltaU(shooter, voltage, U)
+ shooter.Y = raw_shooter.Y + 0.01
+ shooter.UpdateObserver(U)
+ voltage += U
+ raw_shooter.Update(voltage)
+ close_loop_x.append(raw_shooter.X[0, 0] * 10)
+ close_loop_u.append(voltage[0, 0])
- if FLAGS.plot:
- pylab.plot(range(500), close_loop_x)
- pylab.plot(range(500), close_loop_u)
- pylab.show()
+ if FLAGS.plot:
+ pylab.plot(range(500), close_loop_x)
+ pylab.plot(range(500), close_loop_u)
+ pylab.show()
- # Write the generated constants out to a file.
- unaug_sprung_shooter = SprungShooter("RawSprungShooter")
- unaug_shooter = Shooter("RawShooter")
- namespaces = ['y2014', 'control_loops', 'shooter']
- unaug_loop_writer = control_loop.ControlLoopWriter("RawShooter",
- [unaug_sprung_shooter,
- unaug_shooter],
- namespaces=namespaces)
- unaug_loop_writer.Write(argv[4], argv[3])
+ # Write the generated constants out to a file.
+ unaug_sprung_shooter = SprungShooter("RawSprungShooter")
+ unaug_shooter = Shooter("RawShooter")
+ namespaces = ['y2014', 'control_loops', 'shooter']
+ unaug_loop_writer = control_loop.ControlLoopWriter(
+ "RawShooter", [unaug_sprung_shooter, unaug_shooter],
+ namespaces=namespaces)
+ unaug_loop_writer.Write(argv[4], argv[3])
- sprung_shooter = SprungShooterDeltaU()
- shooter = ShooterDeltaU()
- loop_writer = control_loop.ControlLoopWriter("Shooter",
- [sprung_shooter, shooter],
- namespaces=namespaces)
+ sprung_shooter = SprungShooterDeltaU()
+ shooter = ShooterDeltaU()
+ loop_writer = control_loop.ControlLoopWriter(
+ "Shooter", [sprung_shooter, shooter], namespaces=namespaces)
- loop_writer.AddConstant(control_loop.Constant("kMaxExtension", "%f",
- sprung_shooter.max_extension))
- loop_writer.AddConstant(control_loop.Constant("kSpringConstant", "%f",
- sprung_shooter.Ks))
- loop_writer.AddConstant(control_loop.Constant("kDt", "%f",
- sprung_shooter.dt))
- loop_writer.Write(argv[2], argv[1])
+ loop_writer.AddConstant(
+ control_loop.Constant("kMaxExtension", "%f",
+ sprung_shooter.max_extension))
+ loop_writer.AddConstant(
+ control_loop.Constant("kSpringConstant", "%f", sprung_shooter.Ks))
+ loop_writer.AddConstant(
+ control_loop.Constant("kDt", "%f", sprung_shooter.dt))
+ loop_writer.Write(argv[2], argv[1])
+
if __name__ == '__main__':
- sys.exit(main(sys.argv))
+ sys.exit(main(sys.argv))
diff --git a/y2016/control_loops/python/arm.py b/y2016/control_loops/python/arm.py
index 5c5793b..96975bb 100755
--- a/y2016/control_loops/python/arm.py
+++ b/y2016/control_loops/python/arm.py
@@ -18,414 +18,439 @@
FLAGS = gflags.FLAGS
try:
- gflags.DEFINE_bool('plot', False, 'If true, plot the loop response.')
+ gflags.DEFINE_bool('plot', False, 'If true, plot the loop response.')
except gflags.DuplicateFlagError:
- pass
+ pass
class Arm(control_loop.ControlLoop):
- def __init__(self, name="Arm", J=None):
- super(Arm, self).__init__(name=name)
- self._shoulder = Shoulder(name=name, J=J)
- self._shooter = Wrist(name=name)
- self.shoulder_Kv = self._shoulder.Kv / self._shoulder.G
- # Do a coordinate transformation.
- # X_shooter_grounded = X_shooter + X_shoulder
- # dX_shooter_grounded/dt = A_shooter * X_shooter + A_shoulder * X_shoulder +
- # B_shoulder * U_shoulder + B_shooter * U_shooter
- # dX_shooter_grounded/dt = A_shooter * (X_shooter_grounded - X_shoulder) +
- # A_shoulder * X_shoulder + B_shooter * U_shooter + B_shoulder * U_shoulder
- # X = [X_shoulder; X_shooter + X_shoulder]
- # dX/dt = [A_shoulder 0] [X_shoulder ] + [B_shoulder 0] [U_shoulder]
- # [(A_shoulder - A_shooter) A_shooter] [X_shooter_grounded] + [B_shoulder B_shooter] [ U_shooter]
- # Y_shooter_grounded = Y_shooter + Y_shoulder
+ def __init__(self, name="Arm", J=None):
+ super(Arm, self).__init__(name=name)
+ self._shoulder = Shoulder(name=name, J=J)
+ self._shooter = Wrist(name=name)
+ self.shoulder_Kv = self._shoulder.Kv / self._shoulder.G
- self.A_continuous = numpy.matrix(numpy.zeros((4, 4)))
- self.A_continuous[0:2, 0:2] = self._shoulder.A_continuous
- self.A_continuous[2:4, 0:2] = (self._shoulder.A_continuous -
- self._shooter.A_continuous)
- self.A_continuous[2:4, 2:4] = self._shooter.A_continuous
+ # Do a coordinate transformation.
+ # X_shooter_grounded = X_shooter + X_shoulder
+ # dX_shooter_grounded/dt = A_shooter * X_shooter + A_shoulder * X_shoulder +
+ # B_shoulder * U_shoulder + B_shooter * U_shooter
+ # dX_shooter_grounded/dt = A_shooter * (X_shooter_grounded - X_shoulder) +
+ # A_shoulder * X_shoulder + B_shooter * U_shooter + B_shoulder * U_shoulder
+ # X = [X_shoulder; X_shooter + X_shoulder]
+ # dX/dt = [A_shoulder 0] [X_shoulder ] + [B_shoulder 0] [U_shoulder]
+ # [(A_shoulder - A_shooter) A_shooter] [X_shooter_grounded] + [B_shoulder B_shooter] [ U_shooter]
+ # Y_shooter_grounded = Y_shooter + Y_shoulder
- self.B_continuous = numpy.matrix(numpy.zeros((4, 2)))
- self.B_continuous[0:2, 0:1] = self._shoulder.B_continuous
- self.B_continuous[2:4, 1:2] = self._shooter.B_continuous
- self.B_continuous[2:4, 0:1] = self._shoulder.B_continuous
+ self.A_continuous = numpy.matrix(numpy.zeros((4, 4)))
+ self.A_continuous[0:2, 0:2] = self._shoulder.A_continuous
+ self.A_continuous[2:4, 0:2] = (
+ self._shoulder.A_continuous - self._shooter.A_continuous)
+ self.A_continuous[2:4, 2:4] = self._shooter.A_continuous
- self.C = numpy.matrix(numpy.zeros((2, 4)))
- self.C[0:1, 0:2] = self._shoulder.C
- self.C[1:2, 0:2] = -self._shoulder.C
- self.C[1:2, 2:4] = self._shooter.C
+ self.B_continuous = numpy.matrix(numpy.zeros((4, 2)))
+ self.B_continuous[0:2, 0:1] = self._shoulder.B_continuous
+ self.B_continuous[2:4, 1:2] = self._shooter.B_continuous
+ self.B_continuous[2:4, 0:1] = self._shoulder.B_continuous
- # D is 0 for all our loops.
- self.D = numpy.matrix(numpy.zeros((2, 2)))
+ self.C = numpy.matrix(numpy.zeros((2, 4)))
+ self.C[0:1, 0:2] = self._shoulder.C
+ self.C[1:2, 0:2] = -self._shoulder.C
+ self.C[1:2, 2:4] = self._shooter.C
- self.dt = 0.005
+ # D is 0 for all our loops.
+ self.D = numpy.matrix(numpy.zeros((2, 2)))
- self.A, self.B = self.ContinuousToDiscrete(
- self.A_continuous, self.B_continuous, self.dt)
+ self.dt = 0.005
- # Cost of error
- self.Q = numpy.matrix(numpy.zeros((4, 4)))
- q_pos_shoulder = 0.014
- q_vel_shoulder = 4.00
- q_pos_shooter = 0.014
- q_vel_shooter = 4.00
- self.Q[0, 0] = 1.0 / q_pos_shoulder ** 2.0
- self.Q[1, 1] = 1.0 / q_vel_shoulder ** 2.0
- self.Q[2, 2] = 1.0 / q_pos_shooter ** 2.0
- self.Q[3, 3] = 1.0 / q_vel_shooter ** 2.0
+ self.A, self.B = self.ContinuousToDiscrete(self.A_continuous,
+ self.B_continuous, self.dt)
- self.Qff = numpy.matrix(numpy.zeros((4, 4)))
- qff_pos_shoulder = 0.005
- qff_vel_shoulder = 1.00
- qff_pos_shooter = 0.005
- qff_vel_shooter = 1.00
- self.Qff[0, 0] = 1.0 / qff_pos_shoulder ** 2.0
- self.Qff[1, 1] = 1.0 / qff_vel_shoulder ** 2.0
- self.Qff[2, 2] = 1.0 / qff_pos_shooter ** 2.0
- self.Qff[3, 3] = 1.0 / qff_vel_shooter ** 2.0
+ # Cost of error
+ self.Q = numpy.matrix(numpy.zeros((4, 4)))
+ q_pos_shoulder = 0.014
+ q_vel_shoulder = 4.00
+ q_pos_shooter = 0.014
+ q_vel_shooter = 4.00
+ self.Q[0, 0] = 1.0 / q_pos_shoulder**2.0
+ self.Q[1, 1] = 1.0 / q_vel_shoulder**2.0
+ self.Q[2, 2] = 1.0 / q_pos_shooter**2.0
+ self.Q[3, 3] = 1.0 / q_vel_shooter**2.0
- # Cost of control effort
- self.R = numpy.matrix(numpy.zeros((2, 2)))
- r_voltage = 1.0 / 12.0
- self.R[0, 0] = r_voltage ** 2.0
- self.R[1, 1] = r_voltage ** 2.0
+ self.Qff = numpy.matrix(numpy.zeros((4, 4)))
+ qff_pos_shoulder = 0.005
+ qff_vel_shoulder = 1.00
+ qff_pos_shooter = 0.005
+ qff_vel_shooter = 1.00
+ self.Qff[0, 0] = 1.0 / qff_pos_shoulder**2.0
+ self.Qff[1, 1] = 1.0 / qff_vel_shoulder**2.0
+ self.Qff[2, 2] = 1.0 / qff_pos_shooter**2.0
+ self.Qff[3, 3] = 1.0 / qff_vel_shooter**2.0
- self.Kff = controls.TwoStateFeedForwards(self.B, self.Qff)
+ # Cost of control effort
+ self.R = numpy.matrix(numpy.zeros((2, 2)))
+ r_voltage = 1.0 / 12.0
+ self.R[0, 0] = r_voltage**2.0
+ self.R[1, 1] = r_voltage**2.0
- glog.debug('Shoulder K')
- glog.debug(repr(self._shoulder.K))
- glog.debug('Poles are %s',
- repr(numpy.linalg.eig(self._shoulder.A -
- self._shoulder.B * self._shoulder.K)[0]))
+ self.Kff = controls.TwoStateFeedForwards(self.B, self.Qff)
- # Compute controller gains.
- # self.K = controls.dlqr(self.A, self.B, self.Q, self.R)
- self.K = numpy.matrix(numpy.zeros((2, 4)))
- self.K[0:1, 0:2] = self._shoulder.K
- self.K[1:2, 0:2] = (
- -self.Kff[1:2, 2:4] * self.B[2:4, 0:1] * self._shoulder.K
- + self.Kff[1:2, 2:4] * self.A[2:4, 0:2])
- self.K[1:2, 2:4] = self._shooter.K
+ glog.debug('Shoulder K')
+ glog.debug(repr(self._shoulder.K))
+ glog.debug(
+ 'Poles are %s',
+ repr(
+ numpy.linalg.eig(self._shoulder.A -
+ self._shoulder.B * self._shoulder.K)[0]))
- glog.debug('Arm controller %s', repr(self.K))
+ # Compute controller gains.
+ # self.K = controls.dlqr(self.A, self.B, self.Q, self.R)
+ self.K = numpy.matrix(numpy.zeros((2, 4)))
+ self.K[0:1, 0:2] = self._shoulder.K
+ self.K[1:2, 0:2] = (
+ -self.Kff[1:2, 2:4] * self.B[2:4, 0:1] * self._shoulder.K +
+ self.Kff[1:2, 2:4] * self.A[2:4, 0:2])
+ self.K[1:2, 2:4] = self._shooter.K
- # Cost of error
- self.Q = numpy.matrix(numpy.zeros((4, 4)))
- q_pos_shoulder = 0.05
- q_vel_shoulder = 2.65
- q_pos_shooter = 0.05
- q_vel_shooter = 2.65
- self.Q[0, 0] = q_pos_shoulder ** 2.0
- self.Q[1, 1] = q_vel_shoulder ** 2.0
- self.Q[2, 2] = q_pos_shooter ** 2.0
- self.Q[3, 3] = q_vel_shooter ** 2.0
+ glog.debug('Arm controller %s', repr(self.K))
- # Cost of control effort
- self.R = numpy.matrix(numpy.zeros((2, 2)))
- r_voltage = 0.025
- self.R[0, 0] = r_voltage ** 2.0
- self.R[1, 1] = r_voltage ** 2.0
+ # Cost of error
+ self.Q = numpy.matrix(numpy.zeros((4, 4)))
+ q_pos_shoulder = 0.05
+ q_vel_shoulder = 2.65
+ q_pos_shooter = 0.05
+ q_vel_shooter = 2.65
+ self.Q[0, 0] = q_pos_shoulder**2.0
+ self.Q[1, 1] = q_vel_shoulder**2.0
+ self.Q[2, 2] = q_pos_shooter**2.0
+ self.Q[3, 3] = q_vel_shooter**2.0
- self.KalmanGain, self.Q_steady = controls.kalman(
- A=self.A, B=self.B, C=self.C, Q=self.Q, R=self.R)
- self.L = self.A * self.KalmanGain
+ # Cost of control effort
+ self.R = numpy.matrix(numpy.zeros((2, 2)))
+ r_voltage = 0.025
+ self.R[0, 0] = r_voltage**2.0
+ self.R[1, 1] = r_voltage**2.0
- self.U_max = numpy.matrix([[12.0], [12.0]])
- self.U_min = numpy.matrix([[-12.0], [-12.0]])
+ self.KalmanGain, self.Q_steady = controls.kalman(
+ A=self.A, B=self.B, C=self.C, Q=self.Q, R=self.R)
+ self.L = self.A * self.KalmanGain
- self.InitializeState()
+ self.U_max = numpy.matrix([[12.0], [12.0]])
+ self.U_min = numpy.matrix([[-12.0], [-12.0]])
+
+ self.InitializeState()
class IntegralArm(Arm):
- def __init__(self, name="IntegralArm", J=None):
- super(IntegralArm, self).__init__(name=name, J=J)
- self.A_continuous_unaugmented = self.A_continuous
- self.B_continuous_unaugmented = self.B_continuous
+ def __init__(self, name="IntegralArm", J=None):
+ super(IntegralArm, self).__init__(name=name, J=J)
- self.A_continuous = numpy.matrix(numpy.zeros((6, 6)))
- self.A_continuous[0:4, 0:4] = self.A_continuous_unaugmented
- self.A_continuous[0:4, 4:6] = self.B_continuous_unaugmented
+ self.A_continuous_unaugmented = self.A_continuous
+ self.B_continuous_unaugmented = self.B_continuous
- self.B_continuous = numpy.matrix(numpy.zeros((6, 2)))
- self.B_continuous[0:4, 0:2] = self.B_continuous_unaugmented
+ self.A_continuous = numpy.matrix(numpy.zeros((6, 6)))
+ self.A_continuous[0:4, 0:4] = self.A_continuous_unaugmented
+ self.A_continuous[0:4, 4:6] = self.B_continuous_unaugmented
- self.C_unaugmented = self.C
- self.C = numpy.matrix(numpy.zeros((2, 6)))
- self.C[0:2, 0:4] = self.C_unaugmented
+ self.B_continuous = numpy.matrix(numpy.zeros((6, 2)))
+ self.B_continuous[0:4, 0:2] = self.B_continuous_unaugmented
- self.A, self.B = self.ContinuousToDiscrete(self.A_continuous, self.B_continuous, self.dt)
+ self.C_unaugmented = self.C
+ self.C = numpy.matrix(numpy.zeros((2, 6)))
+ self.C[0:2, 0:4] = self.C_unaugmented
- q_pos_shoulder = 0.10
- q_vel_shoulder = 0.005
- q_voltage_shoulder = 3.5
- q_pos_shooter = 0.08
- q_vel_shooter = 2.00
- q_voltage_shooter = 1.0
- self.Q = numpy.matrix(numpy.zeros((6, 6)))
- self.Q[0, 0] = q_pos_shoulder ** 2.0
- self.Q[1, 1] = q_vel_shoulder ** 2.0
- self.Q[2, 2] = q_pos_shooter ** 2.0
- self.Q[3, 3] = q_vel_shooter ** 2.0
- self.Q[4, 4] = q_voltage_shoulder ** 2.0
- self.Q[5, 5] = q_voltage_shooter ** 2.0
+ self.A, self.B = self.ContinuousToDiscrete(self.A_continuous,
+ self.B_continuous, self.dt)
- self.R = numpy.matrix(numpy.zeros((2, 2)))
- r_pos = 0.05
- self.R[0, 0] = r_pos ** 2.0
- self.R[1, 1] = r_pos ** 2.0
+ q_pos_shoulder = 0.10
+ q_vel_shoulder = 0.005
+ q_voltage_shoulder = 3.5
+ q_pos_shooter = 0.08
+ q_vel_shooter = 2.00
+ q_voltage_shooter = 1.0
+ self.Q = numpy.matrix(numpy.zeros((6, 6)))
+ self.Q[0, 0] = q_pos_shoulder**2.0
+ self.Q[1, 1] = q_vel_shoulder**2.0
+ self.Q[2, 2] = q_pos_shooter**2.0
+ self.Q[3, 3] = q_vel_shooter**2.0
+ self.Q[4, 4] = q_voltage_shoulder**2.0
+ self.Q[5, 5] = q_voltage_shooter**2.0
- self.KalmanGain, self.Q_steady = controls.kalman(
- A=self.A, B=self.B, C=self.C, Q=self.Q, R=self.R)
- self.L = self.A * self.KalmanGain
+ self.R = numpy.matrix(numpy.zeros((2, 2)))
+ r_pos = 0.05
+ self.R[0, 0] = r_pos**2.0
+ self.R[1, 1] = r_pos**2.0
- self.K_unaugmented = self.K
- self.K = numpy.matrix(numpy.zeros((2, 6)))
- self.K[0:2, 0:4] = self.K_unaugmented
- self.K[0, 4] = 1
- self.K[1, 5] = 1
+ self.KalmanGain, self.Q_steady = controls.kalman(
+ A=self.A, B=self.B, C=self.C, Q=self.Q, R=self.R)
+ self.L = self.A * self.KalmanGain
- self.Kff = numpy.concatenate((self.Kff, numpy.matrix(numpy.zeros((2, 2)))), axis=1)
+ self.K_unaugmented = self.K
+ self.K = numpy.matrix(numpy.zeros((2, 6)))
+ self.K[0:2, 0:4] = self.K_unaugmented
+ self.K[0, 4] = 1
+ self.K[1, 5] = 1
- self.InitializeState()
+ self.Kff = numpy.concatenate(
+ (self.Kff, numpy.matrix(numpy.zeros((2, 2)))), axis=1)
+
+ self.InitializeState()
class ScenarioPlotter(object):
- def __init__(self):
- # Various lists for graphing things.
- self.t = []
- self.x_shoulder = []
- self.v_shoulder = []
- self.a_shoulder = []
- self.x_hat_shoulder = []
- self.u_shoulder = []
- self.offset_shoulder = []
- self.x_shooter = []
- self.v_shooter = []
- self.a_shooter = []
- self.x_hat_shooter = []
- self.u_shooter = []
- self.offset_shooter = []
- self.goal_x_shoulder = []
- self.goal_v_shoulder = []
- self.goal_x_shooter = []
- self.goal_v_shooter = []
- def run_test(self, arm, end_goal,
- iterations=200, controller=None, observer=None):
- """Runs the plant with an initial condition and goal.
+ def __init__(self):
+ # Various lists for graphing things.
+ self.t = []
+ self.x_shoulder = []
+ self.v_shoulder = []
+ self.a_shoulder = []
+ self.x_hat_shoulder = []
+ self.u_shoulder = []
+ self.offset_shoulder = []
+ self.x_shooter = []
+ self.v_shooter = []
+ self.a_shooter = []
+ self.x_hat_shooter = []
+ self.u_shooter = []
+ self.offset_shooter = []
+ self.goal_x_shoulder = []
+ self.goal_v_shoulder = []
+ self.goal_x_shooter = []
+ self.goal_v_shooter = []
- Args:
- arm: Arm object to use.
- end_goal: numpy.Matrix[6, 1], end goal state.
- iterations: Number of timesteps to run the model for.
- controller: Arm object to get K from, or None if we should
- use arm.
- observer: Arm object to use for the observer, or None if we should
- use the actual state.
- """
+ def run_test(self,
+ arm,
+ end_goal,
+ iterations=200,
+ controller=None,
+ observer=None):
+ """Runs the plant with an initial condition and goal.
- if controller is None:
- controller = arm
+ Args:
+ arm: Arm object to use.
+ end_goal: numpy.Matrix[6, 1], end goal state.
+ iterations: Number of timesteps to run the model for.
+ controller: Arm object to get K from, or None if we should
+ use arm.
+ observer: Arm object to use for the observer, or None if we should
+ use the actual state.
+ """
- vbat = 12.0
+ if controller is None:
+ controller = arm
- if self.t:
- initial_t = self.t[-1] + arm.dt
- else:
- initial_t = 0
+ vbat = 12.0
- goal = numpy.concatenate((arm.X, numpy.matrix(numpy.zeros((2, 1)))), axis=0)
+ if self.t:
+ initial_t = self.t[-1] + arm.dt
+ else:
+ initial_t = 0
- shoulder_profile = TrapezoidProfile(arm.dt)
- shoulder_profile.set_maximum_acceleration(12.0)
- shoulder_profile.set_maximum_velocity(10.0)
- shoulder_profile.SetGoal(goal[0, 0])
- shooter_profile = TrapezoidProfile(arm.dt)
- shooter_profile.set_maximum_acceleration(50.0)
- shooter_profile.set_maximum_velocity(10.0)
- shooter_profile.SetGoal(goal[2, 0])
+ goal = numpy.concatenate((arm.X, numpy.matrix(numpy.zeros((2, 1)))),
+ axis=0)
- U_last = numpy.matrix(numpy.zeros((2, 1)))
- for i in xrange(iterations):
- X_hat = arm.X
+ shoulder_profile = TrapezoidProfile(arm.dt)
+ shoulder_profile.set_maximum_acceleration(12.0)
+ shoulder_profile.set_maximum_velocity(10.0)
+ shoulder_profile.SetGoal(goal[0, 0])
+ shooter_profile = TrapezoidProfile(arm.dt)
+ shooter_profile.set_maximum_acceleration(50.0)
+ shooter_profile.set_maximum_velocity(10.0)
+ shooter_profile.SetGoal(goal[2, 0])
- if observer is not None:
- observer.Y = arm.Y
- observer.CorrectObserver(U_last)
- self.offset_shoulder.append(observer.X_hat[4, 0])
- self.offset_shooter.append(observer.X_hat[5, 0])
+ U_last = numpy.matrix(numpy.zeros((2, 1)))
+ for i in xrange(iterations):
+ X_hat = arm.X
- X_hat = observer.X_hat
- self.x_hat_shoulder.append(observer.X_hat[0, 0])
- self.x_hat_shooter.append(observer.X_hat[2, 0])
+ if observer is not None:
+ observer.Y = arm.Y
+ observer.CorrectObserver(U_last)
+ self.offset_shoulder.append(observer.X_hat[4, 0])
+ self.offset_shooter.append(observer.X_hat[5, 0])
- next_shoulder_goal = shoulder_profile.Update(end_goal[0, 0], end_goal[1, 0])
- next_shooter_goal = shooter_profile.Update(end_goal[2, 0], end_goal[3, 0])
+ X_hat = observer.X_hat
+ self.x_hat_shoulder.append(observer.X_hat[0, 0])
+ self.x_hat_shooter.append(observer.X_hat[2, 0])
- next_goal = numpy.concatenate(
- (next_shoulder_goal,
- next_shooter_goal,
- numpy.matrix(numpy.zeros((2, 1)))),
- axis=0)
- self.goal_x_shoulder.append(goal[0, 0])
- self.goal_v_shoulder.append(goal[1, 0])
- self.goal_x_shooter.append(goal[2, 0])
- self.goal_v_shooter.append(goal[3, 0])
+ next_shoulder_goal = shoulder_profile.Update(
+ end_goal[0, 0], end_goal[1, 0])
+ next_shooter_goal = shooter_profile.Update(end_goal[2, 0],
+ end_goal[3, 0])
- ff_U = controller.Kff * (next_goal - observer.A * goal)
+ next_goal = numpy.concatenate(
+ (next_shoulder_goal, next_shooter_goal,
+ numpy.matrix(numpy.zeros((2, 1)))),
+ axis=0)
+ self.goal_x_shoulder.append(goal[0, 0])
+ self.goal_v_shoulder.append(goal[1, 0])
+ self.goal_x_shooter.append(goal[2, 0])
+ self.goal_v_shooter.append(goal[3, 0])
- U_uncapped = controller.K * (goal - X_hat) + ff_U
- U = U_uncapped.copy()
+ ff_U = controller.Kff * (next_goal - observer.A * goal)
- U[0, 0] = numpy.clip(U[0, 0], -vbat, vbat)
- U[1, 0] = numpy.clip(U[1, 0], -vbat, vbat)
- self.x_shoulder.append(arm.X[0, 0])
- self.x_shooter.append(arm.X[2, 0])
+ U_uncapped = controller.K * (goal - X_hat) + ff_U
+ U = U_uncapped.copy()
- if self.v_shoulder:
- last_v_shoulder = self.v_shoulder[-1]
- else:
- last_v_shoulder = 0
- self.v_shoulder.append(arm.X[1, 0])
- self.a_shoulder.append(
- (self.v_shoulder[-1] - last_v_shoulder) / arm.dt)
+ U[0, 0] = numpy.clip(U[0, 0], -vbat, vbat)
+ U[1, 0] = numpy.clip(U[1, 0], -vbat, vbat)
+ self.x_shoulder.append(arm.X[0, 0])
+ self.x_shooter.append(arm.X[2, 0])
- if self.v_shooter:
- last_v_shooter = self.v_shooter[-1]
- else:
- last_v_shooter = 0
- self.v_shooter.append(arm.X[3, 0])
- self.a_shooter.append(
- (self.v_shooter[-1] - last_v_shooter) / arm.dt)
+ if self.v_shoulder:
+ last_v_shoulder = self.v_shoulder[-1]
+ else:
+ last_v_shoulder = 0
+ self.v_shoulder.append(arm.X[1, 0])
+ self.a_shoulder.append(
+ (self.v_shoulder[-1] - last_v_shoulder) / arm.dt)
- if i % 40 == 0:
- # Test that if we move the shoulder, the shooter stays perfect.
- #observer.X_hat[0, 0] += 0.20
- #arm.X[0, 0] += 0.20
- pass
- U_error = numpy.matrix([[2.0], [2.0]])
- # Kick it and see what happens.
- #if (initial_t + i * arm.dt) % 0.4 > 0.2:
- #U_error = numpy.matrix([[4.0], [0.0]])
- #else:
- #U_error = numpy.matrix([[-4.0], [0.0]])
+ if self.v_shooter:
+ last_v_shooter = self.v_shooter[-1]
+ else:
+ last_v_shooter = 0
+ self.v_shooter.append(arm.X[3, 0])
+ self.a_shooter.append(
+ (self.v_shooter[-1] - last_v_shooter) / arm.dt)
- arm.Update(U + U_error)
+ if i % 40 == 0:
+ # Test that if we move the shoulder, the shooter stays perfect.
+ #observer.X_hat[0, 0] += 0.20
+ #arm.X[0, 0] += 0.20
+ pass
+ U_error = numpy.matrix([[2.0], [2.0]])
+ # Kick it and see what happens.
+ #if (initial_t + i * arm.dt) % 0.4 > 0.2:
+ #U_error = numpy.matrix([[4.0], [0.0]])
+ #else:
+ #U_error = numpy.matrix([[-4.0], [0.0]])
- if observer is not None:
- observer.PredictObserver(U)
+ arm.Update(U + U_error)
- self.t.append(initial_t + i * arm.dt)
- self.u_shoulder.append(U[0, 0])
- self.u_shooter.append(U[1, 0])
+ if observer is not None:
+ observer.PredictObserver(U)
- ff_U -= U_uncapped - U
- goal = controller.A * goal + controller.B * ff_U
+ self.t.append(initial_t + i * arm.dt)
+ self.u_shoulder.append(U[0, 0])
+ self.u_shooter.append(U[1, 0])
- if U[0, 0] != U_uncapped[0, 0]:
- glog.debug('Moving shoulder %s', repr(initial_t + i * arm.dt))
- glog.debug('U error %s', repr(U_uncapped - U))
- glog.debug('goal change is %s',
- repr(next_shoulder_goal -
- numpy.matrix([[goal[0, 0]], [goal[1, 0]]])))
- shoulder_profile.MoveCurrentState(
- numpy.matrix([[goal[0, 0]], [goal[1, 0]]]))
- if U[1, 0] != U_uncapped[1, 0]:
- glog.debug('Moving shooter %s', repr(initial_t + i * arm.dt))
- glog.debug('U error %s', repr(U_uncapped - U))
- shooter_profile.MoveCurrentState(
- numpy.matrix([[goal[2, 0]], [goal[3, 0]]]))
- U_last = U
- glog.debug('goal_error %s', repr(end_goal - goal))
- glog.debug('error %s', repr(observer.X_hat - end_goal))
+ ff_U -= U_uncapped - U
+ goal = controller.A * goal + controller.B * ff_U
+ if U[0, 0] != U_uncapped[0, 0]:
+ glog.debug('Moving shoulder %s', repr(initial_t + i * arm.dt))
+ glog.debug('U error %s', repr(U_uncapped - U))
+ glog.debug(
+ 'goal change is %s',
+ repr(next_shoulder_goal -
+ numpy.matrix([[goal[0, 0]], [goal[1, 0]]])))
+ shoulder_profile.MoveCurrentState(
+ numpy.matrix([[goal[0, 0]], [goal[1, 0]]]))
+ if U[1, 0] != U_uncapped[1, 0]:
+ glog.debug('Moving shooter %s', repr(initial_t + i * arm.dt))
+ glog.debug('U error %s', repr(U_uncapped - U))
+ shooter_profile.MoveCurrentState(
+ numpy.matrix([[goal[2, 0]], [goal[3, 0]]]))
+ U_last = U
+ glog.debug('goal_error %s', repr(end_goal - goal))
+ glog.debug('error %s', repr(observer.X_hat - end_goal))
- def Plot(self):
- pylab.subplot(3, 1, 1)
- pylab.plot(self.t, self.x_shoulder, label='x shoulder')
- pylab.plot(self.t, self.goal_x_shoulder, label='goal x shoulder')
- pylab.plot(self.t, self.x_hat_shoulder, label='x_hat shoulder')
+ def Plot(self):
+ pylab.subplot(3, 1, 1)
+ pylab.plot(self.t, self.x_shoulder, label='x shoulder')
+ pylab.plot(self.t, self.goal_x_shoulder, label='goal x shoulder')
+ pylab.plot(self.t, self.x_hat_shoulder, label='x_hat shoulder')
- pylab.plot(self.t, self.x_shooter, label='x shooter')
- pylab.plot(self.t, self.x_hat_shooter, label='x_hat shooter')
- pylab.plot(self.t, self.goal_x_shooter, label='goal x shooter')
- pylab.plot(self.t, map(operator.add, self.x_shooter, self.x_shoulder),
- label='x shooter ground')
- pylab.plot(self.t, map(operator.add, self.x_hat_shooter, self.x_hat_shoulder),
- label='x_hat shooter ground')
- pylab.legend()
+ pylab.plot(self.t, self.x_shooter, label='x shooter')
+ pylab.plot(self.t, self.x_hat_shooter, label='x_hat shooter')
+ pylab.plot(self.t, self.goal_x_shooter, label='goal x shooter')
+ pylab.plot(
+ self.t,
+ map(operator.add, self.x_shooter, self.x_shoulder),
+ label='x shooter ground')
+ pylab.plot(
+ self.t,
+ map(operator.add, self.x_hat_shooter, self.x_hat_shoulder),
+ label='x_hat shooter ground')
+ pylab.legend()
- pylab.subplot(3, 1, 2)
- pylab.plot(self.t, self.u_shoulder, label='u shoulder')
- pylab.plot(self.t, self.offset_shoulder, label='voltage_offset shoulder')
- pylab.plot(self.t, self.u_shooter, label='u shooter')
- pylab.plot(self.t, self.offset_shooter, label='voltage_offset shooter')
- pylab.legend()
+ pylab.subplot(3, 1, 2)
+ pylab.plot(self.t, self.u_shoulder, label='u shoulder')
+ pylab.plot(
+ self.t, self.offset_shoulder, label='voltage_offset shoulder')
+ pylab.plot(self.t, self.u_shooter, label='u shooter')
+ pylab.plot(self.t, self.offset_shooter, label='voltage_offset shooter')
+ pylab.legend()
- pylab.subplot(3, 1, 3)
- pylab.plot(self.t, self.a_shoulder, label='a_shoulder')
- pylab.plot(self.t, self.a_shooter, label='a_shooter')
- pylab.legend()
+ pylab.subplot(3, 1, 3)
+ pylab.plot(self.t, self.a_shoulder, label='a_shoulder')
+ pylab.plot(self.t, self.a_shooter, label='a_shooter')
+ pylab.legend()
- pylab.show()
+ pylab.show()
def main(argv):
- argv = FLAGS(argv)
- glog.init()
+ argv = FLAGS(argv)
+ glog.init()
- scenario_plotter = ScenarioPlotter()
+ scenario_plotter = ScenarioPlotter()
- J_accelerating = 18
- J_decelerating = 7
+ J_accelerating = 18
+ J_decelerating = 7
- arm = Arm(name='AcceleratingArm', J=J_accelerating)
- arm_integral_controller = IntegralArm(
- name='AcceleratingIntegralArm', J=J_accelerating)
- arm_observer = IntegralArm()
+ arm = Arm(name='AcceleratingArm', J=J_accelerating)
+ arm_integral_controller = IntegralArm(
+ name='AcceleratingIntegralArm', J=J_accelerating)
+ arm_observer = IntegralArm()
- # Test moving the shoulder with constant separation.
- initial_X = numpy.matrix([[0.0], [0.0], [0.0], [0.0], [0.0], [0.0]])
- R = numpy.matrix([[numpy.pi / 2.0],
- [0.0],
- [0.0], #[numpy.pi / 2.0],
- [0.0],
- [0.0],
- [0.0]])
- arm.X = initial_X[0:4, 0]
- arm_observer.X = initial_X
+ # Test moving the shoulder with constant separation.
+ initial_X = numpy.matrix([[0.0], [0.0], [0.0], [0.0], [0.0], [0.0]])
+ R = numpy.matrix([
+ [numpy.pi / 2.0],
+ [0.0],
+ [0.0], #[numpy.pi / 2.0],
+ [0.0],
+ [0.0],
+ [0.0]
+ ])
+ arm.X = initial_X[0:4, 0]
+ arm_observer.X = initial_X
- scenario_plotter.run_test(arm=arm,
- end_goal=R,
- iterations=300,
- controller=arm_integral_controller,
- observer=arm_observer)
+ scenario_plotter.run_test(
+ arm=arm,
+ end_goal=R,
+ iterations=300,
+ controller=arm_integral_controller,
+ observer=arm_observer)
- if len(argv) != 5:
- glog.fatal('Expected .h file name and .cc file name for the wrist and integral wrist.')
- else:
- namespaces = ['y2016', 'control_loops', 'superstructure']
- decelerating_arm = Arm(name='DeceleratingArm', J=J_decelerating)
- loop_writer = control_loop.ControlLoopWriter(
- 'Arm', [arm, decelerating_arm], namespaces=namespaces)
- loop_writer.Write(argv[1], argv[2])
+ if len(argv) != 5:
+ glog.fatal(
+ 'Expected .h file name and .cc file name for the wrist and integral wrist.'
+ )
+ else:
+ namespaces = ['y2016', 'control_loops', 'superstructure']
+ decelerating_arm = Arm(name='DeceleratingArm', J=J_decelerating)
+ loop_writer = control_loop.ControlLoopWriter(
+ 'Arm', [arm, decelerating_arm], namespaces=namespaces)
+ loop_writer.Write(argv[1], argv[2])
- decelerating_integral_arm_controller = IntegralArm(
- name='DeceleratingIntegralArm', J=J_decelerating)
+ decelerating_integral_arm_controller = IntegralArm(
+ name='DeceleratingIntegralArm', J=J_decelerating)
- integral_loop_writer = control_loop.ControlLoopWriter(
- 'IntegralArm',
- [arm_integral_controller, decelerating_integral_arm_controller],
- namespaces=namespaces)
- integral_loop_writer.AddConstant(control_loop.Constant("kV_shoulder", "%f",
- arm_integral_controller.shoulder_Kv))
- integral_loop_writer.Write(argv[3], argv[4])
+ integral_loop_writer = control_loop.ControlLoopWriter(
+ 'IntegralArm',
+ [arm_integral_controller, decelerating_integral_arm_controller],
+ namespaces=namespaces)
+ integral_loop_writer.AddConstant(
+ control_loop.Constant("kV_shoulder", "%f",
+ arm_integral_controller.shoulder_Kv))
+ integral_loop_writer.Write(argv[3], argv[4])
- if FLAGS.plot:
- scenario_plotter.Plot()
+ if FLAGS.plot:
+ scenario_plotter.Plot()
+
if __name__ == '__main__':
- sys.exit(main(sys.argv))
+ sys.exit(main(sys.argv))
diff --git a/y2016/control_loops/python/shooter.py b/y2016/control_loops/python/shooter.py
index 53793d0..b3c988c 100755
--- a/y2016/control_loops/python/shooter.py
+++ b/y2016/control_loops/python/shooter.py
@@ -13,262 +13,274 @@
gflags.DEFINE_bool('plot', False, 'If true, plot the loop response.')
+
class VelocityShooter(control_loop.ControlLoop):
- def __init__(self, name='VelocityShooter'):
- super(VelocityShooter, self).__init__(name)
- # Stall Torque in N m
- self.stall_torque = 0.71
- # Stall Current in Amps
- self.stall_current = 134
- # Free Speed in RPM
- self.free_speed = 18730.0
- # Free Current in Amps
- self.free_current = 0.7
- # Moment of inertia of the shooter wheel in kg m^2
- self.J = 0.00032
- # Resistance of the motor, divided by 2 to account for the 2 motors
- self.R = 12.0 / self.stall_current
- # Motor velocity constant
- self.Kv = ((self.free_speed / 60.0 * 2.0 * numpy.pi) /
- (12.0 - self.R * self.free_current))
- # Torque constant
- self.Kt = self.stall_torque / self.stall_current
- # Gear ratio
- self.G = 12.0 / 18.0
- # Control loop time step
- self.dt = 0.005
- # State feedback matrices
- # [angular velocity]
- self.A_continuous = numpy.matrix(
- [[-self.Kt / self.Kv / (self.J * self.G * self.G * self.R)]])
- self.B_continuous = numpy.matrix(
- [[self.Kt / (self.J * self.G * self.R)]])
- self.C = numpy.matrix([[1]])
- self.D = numpy.matrix([[0]])
+ def __init__(self, name='VelocityShooter'):
+ super(VelocityShooter, self).__init__(name)
+ # Stall Torque in N m
+ self.stall_torque = 0.71
+ # Stall Current in Amps
+ self.stall_current = 134
+ # Free Speed in RPM
+ self.free_speed = 18730.0
+ # Free Current in Amps
+ self.free_current = 0.7
+ # Moment of inertia of the shooter wheel in kg m^2
+ self.J = 0.00032
+ # Resistance of the motor, divided by 2 to account for the 2 motors
+ self.R = 12.0 / self.stall_current
+ # Motor velocity constant
+ self.Kv = ((self.free_speed / 60.0 * 2.0 * numpy.pi) /
+ (12.0 - self.R * self.free_current))
+ # Torque constant
+ self.Kt = self.stall_torque / self.stall_current
+ # Gear ratio
+ self.G = 12.0 / 18.0
+ # Control loop time step
+ self.dt = 0.005
- self.A, self.B = self.ContinuousToDiscrete(
- self.A_continuous, self.B_continuous, self.dt)
+ # State feedback matrices
+ # [angular velocity]
+ self.A_continuous = numpy.matrix(
+ [[-self.Kt / self.Kv / (self.J * self.G * self.G * self.R)]])
+ self.B_continuous = numpy.matrix(
+ [[self.Kt / (self.J * self.G * self.R)]])
+ self.C = numpy.matrix([[1]])
+ self.D = numpy.matrix([[0]])
- self.PlaceControllerPoles([.87])
+ self.A, self.B = self.ContinuousToDiscrete(self.A_continuous,
+ self.B_continuous, self.dt)
- self.PlaceObserverPoles([0.3])
+ self.PlaceControllerPoles([.87])
- self.U_max = numpy.matrix([[12.0]])
- self.U_min = numpy.matrix([[-12.0]])
+ self.PlaceObserverPoles([0.3])
- qff_vel = 8.0
- self.Qff = numpy.matrix([[1.0 / (qff_vel ** 2.0)]])
+ self.U_max = numpy.matrix([[12.0]])
+ self.U_min = numpy.matrix([[-12.0]])
- self.Kff = controls.TwoStateFeedForwards(self.B, self.Qff)
+ qff_vel = 8.0
+ self.Qff = numpy.matrix([[1.0 / (qff_vel**2.0)]])
+
+ self.Kff = controls.TwoStateFeedForwards(self.B, self.Qff)
class Shooter(VelocityShooter):
- def __init__(self, name='Shooter'):
- super(Shooter, self).__init__(name)
- self.A_continuous_unaugmented = self.A_continuous
- self.B_continuous_unaugmented = self.B_continuous
+ def __init__(self, name='Shooter'):
+ super(Shooter, self).__init__(name)
- self.A_continuous = numpy.matrix(numpy.zeros((2, 2)))
- self.A_continuous[1:2, 1:2] = self.A_continuous_unaugmented
- self.A_continuous[0, 1] = 1
+ self.A_continuous_unaugmented = self.A_continuous
+ self.B_continuous_unaugmented = self.B_continuous
- self.B_continuous = numpy.matrix(numpy.zeros((2, 1)))
- self.B_continuous[1:2, 0] = self.B_continuous_unaugmented
+ self.A_continuous = numpy.matrix(numpy.zeros((2, 2)))
+ self.A_continuous[1:2, 1:2] = self.A_continuous_unaugmented
+ self.A_continuous[0, 1] = 1
- # State feedback matrices
- # [position, angular velocity]
- self.C = numpy.matrix([[1, 0]])
- self.D = numpy.matrix([[0]])
+ self.B_continuous = numpy.matrix(numpy.zeros((2, 1)))
+ self.B_continuous[1:2, 0] = self.B_continuous_unaugmented
- self.A, self.B = self.ContinuousToDiscrete(
- self.A_continuous, self.B_continuous, self.dt)
+ # State feedback matrices
+ # [position, angular velocity]
+ self.C = numpy.matrix([[1, 0]])
+ self.D = numpy.matrix([[0]])
- self.rpl = .45
- self.ipl = 0.07
- self.PlaceObserverPoles([self.rpl + 1j * self.ipl,
- self.rpl - 1j * self.ipl])
+ self.A, self.B = self.ContinuousToDiscrete(self.A_continuous,
+ self.B_continuous, self.dt)
- self.K_unaugmented = self.K
- self.K = numpy.matrix(numpy.zeros((1, 2)))
- self.K[0, 1:2] = self.K_unaugmented
- self.Kff_unaugmented = self.Kff
- self.Kff = numpy.matrix(numpy.zeros((1, 2)))
- self.Kff[0, 1:2] = self.Kff_unaugmented
+ self.rpl = .45
+ self.ipl = 0.07
+ self.PlaceObserverPoles(
+ [self.rpl + 1j * self.ipl, self.rpl - 1j * self.ipl])
- self.InitializeState()
+ self.K_unaugmented = self.K
+ self.K = numpy.matrix(numpy.zeros((1, 2)))
+ self.K[0, 1:2] = self.K_unaugmented
+ self.Kff_unaugmented = self.Kff
+ self.Kff = numpy.matrix(numpy.zeros((1, 2)))
+ self.Kff[0, 1:2] = self.Kff_unaugmented
+
+ self.InitializeState()
class IntegralShooter(Shooter):
- def __init__(self, name="IntegralShooter"):
- super(IntegralShooter, self).__init__(name=name)
- self.A_continuous_unaugmented = self.A_continuous
- self.B_continuous_unaugmented = self.B_continuous
+ def __init__(self, name="IntegralShooter"):
+ super(IntegralShooter, self).__init__(name=name)
- self.A_continuous = numpy.matrix(numpy.zeros((3, 3)))
- self.A_continuous[0:2, 0:2] = self.A_continuous_unaugmented
- self.A_continuous[0:2, 2] = self.B_continuous_unaugmented
+ self.A_continuous_unaugmented = self.A_continuous
+ self.B_continuous_unaugmented = self.B_continuous
- self.B_continuous = numpy.matrix(numpy.zeros((3, 1)))
- self.B_continuous[0:2, 0] = self.B_continuous_unaugmented
+ self.A_continuous = numpy.matrix(numpy.zeros((3, 3)))
+ self.A_continuous[0:2, 0:2] = self.A_continuous_unaugmented
+ self.A_continuous[0:2, 2] = self.B_continuous_unaugmented
- self.C_unaugmented = self.C
- self.C = numpy.matrix(numpy.zeros((1, 3)))
- self.C[0:1, 0:2] = self.C_unaugmented
+ self.B_continuous = numpy.matrix(numpy.zeros((3, 1)))
+ self.B_continuous[0:2, 0] = self.B_continuous_unaugmented
- self.A, self.B = self.ContinuousToDiscrete(
- self.A_continuous, self.B_continuous, self.dt)
+ self.C_unaugmented = self.C
+ self.C = numpy.matrix(numpy.zeros((1, 3)))
+ self.C[0:1, 0:2] = self.C_unaugmented
- q_pos = 0.08
- q_vel = 4.00
- q_voltage = 0.3
- self.Q = numpy.matrix([[(q_pos ** 2.0), 0.0, 0.0],
- [0.0, (q_vel ** 2.0), 0.0],
- [0.0, 0.0, (q_voltage ** 2.0)]])
+ self.A, self.B = self.ContinuousToDiscrete(self.A_continuous,
+ self.B_continuous, self.dt)
- r_pos = 0.05
- self.R = numpy.matrix([[(r_pos ** 2.0)]])
+ q_pos = 0.08
+ q_vel = 4.00
+ q_voltage = 0.3
+ self.Q = numpy.matrix([[(q_pos**2.0), 0.0, 0.0],
+ [0.0, (q_vel**2.0), 0.0],
+ [0.0, 0.0, (q_voltage**2.0)]])
- self.KalmanGain, self.Q_steady = controls.kalman(
- A=self.A, B=self.B, C=self.C, Q=self.Q, R=self.R)
- self.L = self.A * self.KalmanGain
+ r_pos = 0.05
+ self.R = numpy.matrix([[(r_pos**2.0)]])
- self.K_unaugmented = self.K
- self.K = numpy.matrix(numpy.zeros((1, 3)))
- self.K[0, 0:2] = self.K_unaugmented
- self.K[0, 2] = 1
- self.Kff_unaugmented = self.Kff
- self.Kff = numpy.matrix(numpy.zeros((1, 3)))
- self.Kff[0, 0:2] = self.Kff_unaugmented
+ self.KalmanGain, self.Q_steady = controls.kalman(
+ A=self.A, B=self.B, C=self.C, Q=self.Q, R=self.R)
+ self.L = self.A * self.KalmanGain
- self.InitializeState()
+ self.K_unaugmented = self.K
+ self.K = numpy.matrix(numpy.zeros((1, 3)))
+ self.K[0, 0:2] = self.K_unaugmented
+ self.K[0, 2] = 1
+ self.Kff_unaugmented = self.Kff
+ self.Kff = numpy.matrix(numpy.zeros((1, 3)))
+ self.Kff[0, 0:2] = self.Kff_unaugmented
+
+ self.InitializeState()
class ScenarioPlotter(object):
- def __init__(self):
- # Various lists for graphing things.
- self.t = []
- self.x = []
- self.v = []
- self.a = []
- self.x_hat = []
- self.u = []
- self.offset = []
- def run_test(self, shooter, goal, iterations=200, controller_shooter=None,
- observer_shooter=None):
- """Runs the shooter plant with an initial condition and goal.
+ def __init__(self):
+ # Various lists for graphing things.
+ self.t = []
+ self.x = []
+ self.v = []
+ self.a = []
+ self.x_hat = []
+ self.u = []
+ self.offset = []
- Args:
- shooter: Shooter object to use.
- goal: goal state.
- iterations: Number of timesteps to run the model for.
- controller_shooter: Shooter object to get K from, or None if we should
- use shooter.
- observer_shooter: Shooter object to use for the observer, or None if we
- should use the actual state.
- """
+ def run_test(self,
+ shooter,
+ goal,
+ iterations=200,
+ controller_shooter=None,
+ observer_shooter=None):
+ """Runs the shooter plant with an initial condition and goal.
- if controller_shooter is None:
- controller_shooter = shooter
+ Args:
+ shooter: Shooter object to use.
+ goal: goal state.
+ iterations: Number of timesteps to run the model for.
+ controller_shooter: Shooter object to get K from, or None if we should
+ use shooter.
+ observer_shooter: Shooter object to use for the observer, or None if we
+ should use the actual state.
+ """
- vbat = 12.0
+ if controller_shooter is None:
+ controller_shooter = shooter
- if self.t:
- initial_t = self.t[-1] + shooter.dt
- else:
- initial_t = 0
+ vbat = 12.0
- for i in xrange(iterations):
- X_hat = shooter.X
+ if self.t:
+ initial_t = self.t[-1] + shooter.dt
+ else:
+ initial_t = 0
- if observer_shooter is not None:
- X_hat = observer_shooter.X_hat
- self.x_hat.append(observer_shooter.X_hat[1, 0])
+ for i in xrange(iterations):
+ X_hat = shooter.X
- ff_U = controller_shooter.Kff * (goal - observer_shooter.A * goal)
+ if observer_shooter is not None:
+ X_hat = observer_shooter.X_hat
+ self.x_hat.append(observer_shooter.X_hat[1, 0])
- U = controller_shooter.K * (goal - X_hat) + ff_U
- U[0, 0] = numpy.clip(U[0, 0], -vbat, vbat)
- self.x.append(shooter.X[0, 0])
+ ff_U = controller_shooter.Kff * (goal - observer_shooter.A * goal)
+ U = controller_shooter.K * (goal - X_hat) + ff_U
+ U[0, 0] = numpy.clip(U[0, 0], -vbat, vbat)
+ self.x.append(shooter.X[0, 0])
- if self.v:
- last_v = self.v[-1]
- else:
- last_v = 0
+ if self.v:
+ last_v = self.v[-1]
+ else:
+ last_v = 0
- self.v.append(shooter.X[1, 0])
- self.a.append((self.v[-1] - last_v) / shooter.dt)
+ self.v.append(shooter.X[1, 0])
+ self.a.append((self.v[-1] - last_v) / shooter.dt)
- if observer_shooter is not None:
- observer_shooter.Y = shooter.Y
- observer_shooter.CorrectObserver(U)
- self.offset.append(observer_shooter.X_hat[2, 0])
+ if observer_shooter is not None:
+ observer_shooter.Y = shooter.Y
+ observer_shooter.CorrectObserver(U)
+ self.offset.append(observer_shooter.X_hat[2, 0])
- applied_U = U.copy()
- if i > 30:
- applied_U += 2
- shooter.Update(applied_U)
+ applied_U = U.copy()
+ if i > 30:
+ applied_U += 2
+ shooter.Update(applied_U)
- if observer_shooter is not None:
- observer_shooter.PredictObserver(U)
+ if observer_shooter is not None:
+ observer_shooter.PredictObserver(U)
- self.t.append(initial_t + i * shooter.dt)
- self.u.append(U[0, 0])
+ self.t.append(initial_t + i * shooter.dt)
+ self.u.append(U[0, 0])
- glog.debug('Time: %f', self.t[-1])
+ glog.debug('Time: %f', self.t[-1])
- def Plot(self):
- pylab.subplot(3, 1, 1)
- pylab.plot(self.t, self.v, label='x')
- pylab.plot(self.t, self.x_hat, label='x_hat')
- pylab.legend()
+ def Plot(self):
+ pylab.subplot(3, 1, 1)
+ pylab.plot(self.t, self.v, label='x')
+ pylab.plot(self.t, self.x_hat, label='x_hat')
+ pylab.legend()
- pylab.subplot(3, 1, 2)
- pylab.plot(self.t, self.u, label='u')
- pylab.plot(self.t, self.offset, label='voltage_offset')
- pylab.legend()
+ pylab.subplot(3, 1, 2)
+ pylab.plot(self.t, self.u, label='u')
+ pylab.plot(self.t, self.offset, label='voltage_offset')
+ pylab.legend()
- pylab.subplot(3, 1, 3)
- pylab.plot(self.t, self.a, label='a')
- pylab.legend()
+ pylab.subplot(3, 1, 3)
+ pylab.plot(self.t, self.a, label='a')
+ pylab.legend()
- pylab.show()
+ pylab.show()
def main(argv):
- scenario_plotter = ScenarioPlotter()
+ scenario_plotter = ScenarioPlotter()
- shooter = Shooter()
- shooter_controller = IntegralShooter()
- observer_shooter = IntegralShooter()
+ shooter = Shooter()
+ shooter_controller = IntegralShooter()
+ observer_shooter = IntegralShooter()
- initial_X = numpy.matrix([[0.0], [0.0]])
- R = numpy.matrix([[0.0], [100.0], [0.0]])
- scenario_plotter.run_test(shooter, goal=R, controller_shooter=shooter_controller,
- observer_shooter=observer_shooter, iterations=200)
+ initial_X = numpy.matrix([[0.0], [0.0]])
+ R = numpy.matrix([[0.0], [100.0], [0.0]])
+ scenario_plotter.run_test(
+ shooter,
+ goal=R,
+ controller_shooter=shooter_controller,
+ observer_shooter=observer_shooter,
+ iterations=200)
- if FLAGS.plot:
- scenario_plotter.Plot()
+ if FLAGS.plot:
+ scenario_plotter.Plot()
- if len(argv) != 5:
- glog.fatal('Expected .h file name and .cc file name')
- else:
- namespaces = ['y2016', 'control_loops', 'shooter']
- shooter = Shooter('Shooter')
- loop_writer = control_loop.ControlLoopWriter('Shooter', [shooter],
- namespaces=namespaces)
- loop_writer.Write(argv[1], argv[2])
+ if len(argv) != 5:
+ glog.fatal('Expected .h file name and .cc file name')
+ else:
+ namespaces = ['y2016', 'control_loops', 'shooter']
+ shooter = Shooter('Shooter')
+ loop_writer = control_loop.ControlLoopWriter(
+ 'Shooter', [shooter], namespaces=namespaces)
+ loop_writer.Write(argv[1], argv[2])
- integral_shooter = IntegralShooter('IntegralShooter')
- integral_loop_writer = control_loop.ControlLoopWriter(
- 'IntegralShooter', [integral_shooter], namespaces=namespaces)
- integral_loop_writer.Write(argv[3], argv[4])
+ integral_shooter = IntegralShooter('IntegralShooter')
+ integral_loop_writer = control_loop.ControlLoopWriter(
+ 'IntegralShooter', [integral_shooter], namespaces=namespaces)
+ integral_loop_writer.Write(argv[3], argv[4])
if __name__ == '__main__':
- argv = FLAGS(sys.argv)
- sys.exit(main(argv))
+ argv = FLAGS(sys.argv)
+ sys.exit(main(argv))
diff --git a/y2017/control_loops/python/column.py b/y2017/control_loops/python/column.py
index 1f8bd76..70cd649 100755
--- a/y2017/control_loops/python/column.py
+++ b/y2017/control_loops/python/column.py
@@ -14,383 +14,413 @@
FLAGS = gflags.FLAGS
try:
- gflags.DEFINE_bool('plot', False, 'If true, plot the loop response.')
+ gflags.DEFINE_bool('plot', False, 'If true, plot the loop response.')
except gflags.DuplicateFlagError:
- pass
+ pass
class ColumnController(control_loop.ControlLoop):
- def __init__(self, name='Column'):
- super(ColumnController, self).__init__(name)
- self.turret = turret.Turret(name + 'Turret')
- self.indexer = indexer.Indexer(name + 'Indexer')
- # Control loop time step
- self.dt = 0.005
+ def __init__(self, name='Column'):
+ super(ColumnController, self).__init__(name)
+ self.turret = turret.Turret(name + 'Turret')
+ self.indexer = indexer.Indexer(name + 'Indexer')
- # State is [position_indexer,
- # velocity_indexer,
- # position_shooter,
- # velocity_shooter]
- # Input is [volts_indexer, volts_shooter]
- self.A_continuous = numpy.matrix(numpy.zeros((3, 3)))
- self.B_continuous = numpy.matrix(numpy.zeros((3, 2)))
+ # Control loop time step
+ self.dt = 0.005
- self.A_continuous[0, 0] = -(self.indexer.Kt / self.indexer.Kv / (self.indexer.J * self.indexer.resistance * self.indexer.G * self.indexer.G) +
- self.turret.Kt / self.turret.Kv / (self.indexer.J * self.turret.resistance * self.turret.G * self.turret.G))
- self.A_continuous[0, 2] = self.turret.Kt / self.turret.Kv / (self.indexer.J * self.turret.resistance * self.turret.G * self.turret.G)
- self.B_continuous[0, 0] = self.indexer.Kt / (self.indexer.J * self.indexer.resistance * self.indexer.G)
- self.B_continuous[0, 1] = -self.turret.Kt / (self.indexer.J * self.turret.resistance * self.turret.G)
+ # State is [position_indexer,
+ # velocity_indexer,
+ # position_shooter,
+ # velocity_shooter]
+ # Input is [volts_indexer, volts_shooter]
+ self.A_continuous = numpy.matrix(numpy.zeros((3, 3)))
+ self.B_continuous = numpy.matrix(numpy.zeros((3, 2)))
- self.A_continuous[1, 2] = 1
+ self.A_continuous[0, 0] = -(
+ self.indexer.Kt / self.indexer.Kv /
+ (self.indexer.J * self.indexer.resistance * self.indexer.G *
+ self.indexer.G) + self.turret.Kt / self.turret.Kv /
+ (self.indexer.J * self.turret.resistance * self.turret.G *
+ self.turret.G))
+ self.A_continuous[0, 2] = self.turret.Kt / self.turret.Kv / (
+ self.indexer.J * self.turret.resistance * self.turret.G *
+ self.turret.G)
+ self.B_continuous[0, 0] = self.indexer.Kt / (
+ self.indexer.J * self.indexer.resistance * self.indexer.G)
+ self.B_continuous[0, 1] = -self.turret.Kt / (
+ self.indexer.J * self.turret.resistance * self.turret.G)
- self.A_continuous[2, 0] = self.turret.Kt / self.turret.Kv / (self.turret.J * self.turret.resistance * self.turret.G * self.turret.G)
- self.A_continuous[2, 2] = -self.turret.Kt / self.turret.Kv / (self.turret.J * self.turret.resistance * self.turret.G * self.turret.G)
+ self.A_continuous[1, 2] = 1
- self.B_continuous[2, 1] = self.turret.Kt / (self.turret.J * self.turret.resistance * self.turret.G)
+ self.A_continuous[2, 0] = self.turret.Kt / self.turret.Kv / (
+ self.turret.J * self.turret.resistance * self.turret.G *
+ self.turret.G)
+ self.A_continuous[2, 2] = -self.turret.Kt / self.turret.Kv / (
+ self.turret.J * self.turret.resistance * self.turret.G *
+ self.turret.G)
- self.C = numpy.matrix([[1, 0, 0], [0, 1, 0]])
- self.D = numpy.matrix([[0, 0], [0, 0]])
+ self.B_continuous[2, 1] = self.turret.Kt / (
+ self.turret.J * self.turret.resistance * self.turret.G)
- self.A, self.B = self.ContinuousToDiscrete(
- self.A_continuous, self.B_continuous, self.dt)
+ self.C = numpy.matrix([[1, 0, 0], [0, 1, 0]])
+ self.D = numpy.matrix([[0, 0], [0, 0]])
- q_indexer_vel = 13.0
- q_pos = 0.05
- q_vel = 0.8
- self.Q = numpy.matrix([[(1.0 / (q_indexer_vel ** 2.0)), 0.0, 0.0],
- [0.0, (1.0 / (q_pos ** 2.0)), 0.0],
- [0.0, 0.0, (1.0 / (q_vel ** 2.0))]])
+ self.A, self.B = self.ContinuousToDiscrete(self.A_continuous,
+ self.B_continuous, self.dt)
- self.R = numpy.matrix([[(1.0 / (12.0 ** 2.0)), 0.0],
- [0.0, (1.0 / (12.0 ** 2.0))]])
- self.K = controls.dlqr(self.A, self.B, self.Q, self.R)
+ q_indexer_vel = 13.0
+ q_pos = 0.05
+ q_vel = 0.8
+ self.Q = numpy.matrix([[(1.0 / (q_indexer_vel**2.0)), 0.0, 0.0],
+ [0.0, (1.0 / (q_pos**2.0)), 0.0],
+ [0.0, 0.0, (1.0 / (q_vel**2.0))]])
- glog.debug('Controller poles are ' + repr(numpy.linalg.eig(self.A - self.B * self.K)[0]))
+ self.R = numpy.matrix([[(1.0 / (12.0**2.0)), 0.0],
+ [0.0, (1.0 / (12.0**2.0))]])
+ self.K = controls.dlqr(self.A, self.B, self.Q, self.R)
- q_vel_indexer_ff = 0.000005
- q_pos_ff = 0.0000005
- q_vel_ff = 0.00008
- self.Qff = numpy.matrix([[(1.0 / (q_vel_indexer_ff ** 2.0)), 0.0, 0.0],
- [0.0, (1.0 / (q_pos_ff ** 2.0)), 0.0],
- [0.0, 0.0, (1.0 / (q_vel_ff ** 2.0))]])
+ glog.debug('Controller poles are ' +
+ repr(numpy.linalg.eig(self.A - self.B * self.K)[0]))
- self.Kff = controls.TwoStateFeedForwards(self.B, self.Qff)
+ q_vel_indexer_ff = 0.000005
+ q_pos_ff = 0.0000005
+ q_vel_ff = 0.00008
+ self.Qff = numpy.matrix([[(1.0 / (q_vel_indexer_ff**2.0)), 0.0, 0.0],
+ [0.0, (1.0 / (q_pos_ff**2.0)), 0.0],
+ [0.0, 0.0, (1.0 / (q_vel_ff**2.0))]])
- self.U_max = numpy.matrix([[12.0], [12.0]])
- self.U_min = numpy.matrix([[-12.0], [-12.0]])
+ self.Kff = controls.TwoStateFeedForwards(self.B, self.Qff)
- self.InitializeState()
+ self.U_max = numpy.matrix([[12.0], [12.0]])
+ self.U_min = numpy.matrix([[-12.0], [-12.0]])
+
+ self.InitializeState()
class Column(ColumnController):
- def __init__(self, name='Column', disable_indexer=False):
- super(Column, self).__init__(name)
- A_continuous = numpy.matrix(numpy.zeros((4, 4)))
- B_continuous = numpy.matrix(numpy.zeros((4, 2)))
- A_continuous[0, 1] = 1
- A_continuous[1:, 1:] = self.A_continuous
- B_continuous[1:, :] = self.B_continuous
+ def __init__(self, name='Column', disable_indexer=False):
+ super(Column, self).__init__(name)
+ A_continuous = numpy.matrix(numpy.zeros((4, 4)))
+ B_continuous = numpy.matrix(numpy.zeros((4, 2)))
- self.A_continuous = A_continuous
- self.B_continuous = B_continuous
+ A_continuous[0, 1] = 1
+ A_continuous[1:, 1:] = self.A_continuous
+ B_continuous[1:, :] = self.B_continuous
- self.A, self.B = self.ContinuousToDiscrete(
- self.A_continuous, self.B_continuous, self.dt)
+ self.A_continuous = A_continuous
+ self.B_continuous = B_continuous
- self.C = numpy.matrix([[1, 0, 0, 0], [-1, 0, 1, 0]])
- self.D = numpy.matrix([[0, 0], [0, 0]])
+ self.A, self.B = self.ContinuousToDiscrete(self.A_continuous,
+ self.B_continuous, self.dt)
- orig_K = self.K
- self.K = numpy.matrix(numpy.zeros((2, 4)))
- self.K[:, 1:] = orig_K
+ self.C = numpy.matrix([[1, 0, 0, 0], [-1, 0, 1, 0]])
+ self.D = numpy.matrix([[0, 0], [0, 0]])
- glog.debug('K is ' + repr(self.K))
- # TODO(austin): Do we want to damp velocity out or not when disabled?
- #if disable_indexer:
- # self.K[0, 1] = 0.0
- # self.K[1, 1] = 0.0
+ orig_K = self.K
+ self.K = numpy.matrix(numpy.zeros((2, 4)))
+ self.K[:, 1:] = orig_K
- orig_Kff = self.Kff
- self.Kff = numpy.matrix(numpy.zeros((2, 4)))
- self.Kff[:, 1:] = orig_Kff
+ glog.debug('K is ' + repr(self.K))
+ # TODO(austin): Do we want to damp velocity out or not when disabled?
+ #if disable_indexer:
+ # self.K[0, 1] = 0.0
+ # self.K[1, 1] = 0.0
- q_pos = 0.12
- q_vel = 2.00
- self.Q = numpy.matrix([[(q_pos ** 2.0), 0.0, 0.0, 0.0],
- [0.0, (q_vel ** 2.0), 0.0, 0.0],
- [0.0, 0.0, (q_pos ** 2.0), 0.0],
- [0.0, 0.0, 0.0, (q_vel ** 2.0)]])
+ orig_Kff = self.Kff
+ self.Kff = numpy.matrix(numpy.zeros((2, 4)))
+ self.Kff[:, 1:] = orig_Kff
- r_pos = 0.05
- self.R = numpy.matrix([[(r_pos ** 2.0), 0.0],
- [0.0, (r_pos ** 2.0)]])
+ q_pos = 0.12
+ q_vel = 2.00
+ self.Q = numpy.matrix([[(q_pos**2.0), 0.0, 0.0, 0.0],
+ [0.0, (q_vel**2.0), 0.0, 0.0],
+ [0.0, 0.0, (q_pos**2.0), 0.0],
+ [0.0, 0.0, 0.0, (q_vel**2.0)]])
- self.KalmanGain, self.Q_steady = controls.kalman(
- A=self.A, B=self.B, C=self.C, Q=self.Q, R=self.R)
- self.L = self.A * self.KalmanGain
+ r_pos = 0.05
+ self.R = numpy.matrix([[(r_pos**2.0), 0.0], [0.0, (r_pos**2.0)]])
- self.InitializeState()
+ self.KalmanGain, self.Q_steady = controls.kalman(
+ A=self.A, B=self.B, C=self.C, Q=self.Q, R=self.R)
+ self.L = self.A * self.KalmanGain
+
+ self.InitializeState()
class IntegralColumn(Column):
- def __init__(self, name='IntegralColumn', voltage_error_noise=None,
- disable_indexer=False):
- super(IntegralColumn, self).__init__(name)
- A_continuous = numpy.matrix(numpy.zeros((6, 6)))
- A_continuous[0:4, 0:4] = self.A_continuous
- A_continuous[0:4:, 4:6] = self.B_continuous
+ def __init__(self,
+ name='IntegralColumn',
+ voltage_error_noise=None,
+ disable_indexer=False):
+ super(IntegralColumn, self).__init__(name)
- B_continuous = numpy.matrix(numpy.zeros((6, 2)))
- B_continuous[0:4, :] = self.B_continuous
+ A_continuous = numpy.matrix(numpy.zeros((6, 6)))
+ A_continuous[0:4, 0:4] = self.A_continuous
+ A_continuous[0:4:, 4:6] = self.B_continuous
- self.A_continuous = A_continuous
- self.B_continuous = B_continuous
+ B_continuous = numpy.matrix(numpy.zeros((6, 2)))
+ B_continuous[0:4, :] = self.B_continuous
- self.A, self.B = self.ContinuousToDiscrete(
- self.A_continuous, self.B_continuous, self.dt)
+ self.A_continuous = A_continuous
+ self.B_continuous = B_continuous
- C = numpy.matrix(numpy.zeros((2, 6)))
- C[0:2, 0:4] = self.C
- self.C = C
+ self.A, self.B = self.ContinuousToDiscrete(self.A_continuous,
+ self.B_continuous, self.dt)
- self.D = numpy.matrix([[0, 0], [0, 0]])
+ C = numpy.matrix(numpy.zeros((2, 6)))
+ C[0:2, 0:4] = self.C
+ self.C = C
- orig_K = self.K
- self.K = numpy.matrix(numpy.zeros((2, 6)))
- self.K[:, 0:4] = orig_K
+ self.D = numpy.matrix([[0, 0], [0, 0]])
- # TODO(austin): I'm not certain this is ideal. If someone spins the bottom
- # at a constant rate, we'll learn a voltage offset. That should translate
- # directly to a voltage on the turret to hold it steady. I'm also not
- # convinced we care that much. If the indexer is off, it'll stop rather
- # quickly anyways, so this is mostly a moot point.
- if not disable_indexer:
- self.K[0, 4] = 1
- self.K[1, 5] = 1
+ orig_K = self.K
+ self.K = numpy.matrix(numpy.zeros((2, 6)))
+ self.K[:, 0:4] = orig_K
- orig_Kff = self.Kff
- self.Kff = numpy.matrix(numpy.zeros((2, 6)))
- self.Kff[:, 0:4] = orig_Kff
+ # TODO(austin): I'm not certain this is ideal. If someone spins the bottom
+ # at a constant rate, we'll learn a voltage offset. That should translate
+ # directly to a voltage on the turret to hold it steady. I'm also not
+ # convinced we care that much. If the indexer is off, it'll stop rather
+ # quickly anyways, so this is mostly a moot point.
+ if not disable_indexer:
+ self.K[0, 4] = 1
+ self.K[1, 5] = 1
- q_pos = 0.40
- q_vel = 2.00
- q_voltage = 8.0
- if voltage_error_noise is not None:
- q_voltage = voltage_error_noise
+ orig_Kff = self.Kff
+ self.Kff = numpy.matrix(numpy.zeros((2, 6)))
+ self.Kff[:, 0:4] = orig_Kff
- self.Q = numpy.matrix([[(q_pos ** 2.0), 0.0, 0.0, 0.0, 0.0, 0.0],
- [0.0, (q_vel ** 2.0), 0.0, 0.0, 0.0, 0.0],
- [0.0, 0.0, (q_pos ** 2.0), 0.0, 0.0, 0.0],
- [0.0, 0.0, 0.0, (q_vel ** 2.0), 0.0, 0.0],
- [0.0, 0.0, 0.0, 0.0, (q_voltage ** 2.0), 0.0],
- [0.0, 0.0, 0.0, 0.0, 0.0, (q_voltage ** 2.0)]])
+ q_pos = 0.40
+ q_vel = 2.00
+ q_voltage = 8.0
+ if voltage_error_noise is not None:
+ q_voltage = voltage_error_noise
- r_pos = 0.05
- self.R = numpy.matrix([[(r_pos ** 2.0), 0.0],
- [0.0, (r_pos ** 2.0)]])
+ self.Q = numpy.matrix([[(q_pos**2.0), 0.0, 0.0, 0.0, 0.0, 0.0],
+ [0.0, (q_vel**2.0), 0.0, 0.0, 0.0, 0.0],
+ [0.0, 0.0, (q_pos**2.0), 0.0, 0.0, 0.0],
+ [0.0, 0.0, 0.0, (q_vel**2.0), 0.0, 0.0],
+ [0.0, 0.0, 0.0, 0.0, (q_voltage**2.0), 0.0],
+ [0.0, 0.0, 0.0, 0.0, 0.0, (q_voltage**2.0)]])
- self.KalmanGain, self.Q_steady = controls.kalman(
- A=self.A, B=self.B, C=self.C, Q=self.Q, R=self.R)
- self.L = self.A * self.KalmanGain
+ r_pos = 0.05
+ self.R = numpy.matrix([[(r_pos**2.0), 0.0], [0.0, (r_pos**2.0)]])
- self.InitializeState()
+ self.KalmanGain, self.Q_steady = controls.kalman(
+ A=self.A, B=self.B, C=self.C, Q=self.Q, R=self.R)
+ self.L = self.A * self.KalmanGain
+
+ self.InitializeState()
class ScenarioPlotter(object):
- def __init__(self):
- # Various lists for graphing things.
- self.t = []
- self.xi = []
- self.xt = []
- self.vi = []
- self.vt = []
- self.ai = []
- self.at = []
- self.x_hat = []
- self.ui = []
- self.ut = []
- self.ui_fb = []
- self.ut_fb = []
- self.offseti = []
- self.offsett = []
- self.turret_error = []
- def run_test(self, column, end_goal,
- controller_column,
- observer_column=None,
- iterations=200):
- """Runs the column plant with an initial condition and goal.
+ def __init__(self):
+ # Various lists for graphing things.
+ self.t = []
+ self.xi = []
+ self.xt = []
+ self.vi = []
+ self.vt = []
+ self.ai = []
+ self.at = []
+ self.x_hat = []
+ self.ui = []
+ self.ut = []
+ self.ui_fb = []
+ self.ut_fb = []
+ self.offseti = []
+ self.offsett = []
+ self.turret_error = []
- Args:
- column: column object to use.
- end_goal: end_goal state.
- controller_column: Intake object to get K from, or None if we should
- use column.
- observer_column: Intake object to use for the observer, or None if we should
- use the actual state.
- iterations: Number of timesteps to run the model for.
- """
+ def run_test(self,
+ column,
+ end_goal,
+ controller_column,
+ observer_column=None,
+ iterations=200):
+ """Runs the column plant with an initial condition and goal.
- if controller_column is None:
- controller_column = column
+ Args:
+ column: column object to use.
+ end_goal: end_goal state.
+ controller_column: Intake object to get K from, or None if we should
+ use column.
+ observer_column: Intake object to use for the observer, or None if we should
+ use the actual state.
+ iterations: Number of timesteps to run the model for.
+ """
- vbat = 12.0
+ if controller_column is None:
+ controller_column = column
- if self.t:
- initial_t = self.t[-1] + column.dt
- else:
- initial_t = 0
+ vbat = 12.0
- goal = numpy.concatenate((column.X, numpy.matrix(numpy.zeros((2, 1)))), axis=0)
+ if self.t:
+ initial_t = self.t[-1] + column.dt
+ else:
+ initial_t = 0
- profile = TrapezoidProfile(column.dt)
- profile.set_maximum_acceleration(10.0)
- profile.set_maximum_velocity(3.0)
- profile.SetGoal(goal[2, 0])
+ goal = numpy.concatenate((column.X, numpy.matrix(numpy.zeros((2, 1)))),
+ axis=0)
- U_last = numpy.matrix(numpy.zeros((2, 1)))
- for i in xrange(iterations):
- observer_column.Y = column.Y
- observer_column.CorrectObserver(U_last)
+ profile = TrapezoidProfile(column.dt)
+ profile.set_maximum_acceleration(10.0)
+ profile.set_maximum_velocity(3.0)
+ profile.SetGoal(goal[2, 0])
- self.offseti.append(observer_column.X_hat[4, 0])
- self.offsett.append(observer_column.X_hat[5, 0])
- self.x_hat.append(observer_column.X_hat[0, 0])
+ U_last = numpy.matrix(numpy.zeros((2, 1)))
+ for i in xrange(iterations):
+ observer_column.Y = column.Y
+ observer_column.CorrectObserver(U_last)
- next_goal = numpy.concatenate(
- (end_goal[0:2, :],
- profile.Update(end_goal[2, 0], end_goal[3, 0]),
- end_goal[4:6, :]),
- axis=0)
+ self.offseti.append(observer_column.X_hat[4, 0])
+ self.offsett.append(observer_column.X_hat[5, 0])
+ self.x_hat.append(observer_column.X_hat[0, 0])
- ff_U = controller_column.Kff * (next_goal - observer_column.A * goal)
- fb_U = controller_column.K * (goal - observer_column.X_hat)
- self.turret_error.append((goal[2, 0] - column.X[2, 0]) * 100.0)
- self.ui_fb.append(fb_U[0, 0])
- self.ut_fb.append(fb_U[1, 0])
+ next_goal = numpy.concatenate(
+ (end_goal[0:2, :], profile.Update(
+ end_goal[2, 0], end_goal[3, 0]), end_goal[4:6, :]),
+ axis=0)
- U_uncapped = ff_U + fb_U
+ ff_U = controller_column.Kff * (
+ next_goal - observer_column.A * goal)
+ fb_U = controller_column.K * (goal - observer_column.X_hat)
+ self.turret_error.append((goal[2, 0] - column.X[2, 0]) * 100.0)
+ self.ui_fb.append(fb_U[0, 0])
+ self.ut_fb.append(fb_U[1, 0])
- U = U_uncapped.copy()
- U[0, 0] = numpy.clip(U[0, 0], -vbat, vbat)
- U[1, 0] = numpy.clip(U[1, 0], -vbat, vbat)
- self.xi.append(column.X[0, 0])
- self.xt.append(column.X[2, 0])
+ U_uncapped = ff_U + fb_U
- if self.vi:
- last_vi = self.vi[-1]
- else:
- last_vi = 0
- if self.vt:
- last_vt = self.vt[-1]
- else:
- last_vt = 0
+ U = U_uncapped.copy()
+ U[0, 0] = numpy.clip(U[0, 0], -vbat, vbat)
+ U[1, 0] = numpy.clip(U[1, 0], -vbat, vbat)
+ self.xi.append(column.X[0, 0])
+ self.xt.append(column.X[2, 0])
- self.vi.append(column.X[1, 0])
- self.vt.append(column.X[3, 0])
- self.ai.append((self.vi[-1] - last_vi) / column.dt)
- self.at.append((self.vt[-1] - last_vt) / column.dt)
+ if self.vi:
+ last_vi = self.vi[-1]
+ else:
+ last_vi = 0
+ if self.vt:
+ last_vt = self.vt[-1]
+ else:
+ last_vt = 0
- offset = 0.0
- if i > 100:
- offset = 1.0
- column.Update(U + numpy.matrix([[0.0], [offset]]))
+ self.vi.append(column.X[1, 0])
+ self.vt.append(column.X[3, 0])
+ self.ai.append((self.vi[-1] - last_vi) / column.dt)
+ self.at.append((self.vt[-1] - last_vt) / column.dt)
- observer_column.PredictObserver(U)
+ offset = 0.0
+ if i > 100:
+ offset = 1.0
+ column.Update(U + numpy.matrix([[0.0], [offset]]))
- self.t.append(initial_t + i * column.dt)
- self.ui.append(U[0, 0])
- self.ut.append(U[1, 0])
+ observer_column.PredictObserver(U)
- ff_U -= U_uncapped - U
- goal = controller_column.A * goal + controller_column.B * ff_U
+ self.t.append(initial_t + i * column.dt)
+ self.ui.append(U[0, 0])
+ self.ut.append(U[1, 0])
- if U[1, 0] != U_uncapped[1, 0]:
- profile.MoveCurrentState(
- numpy.matrix([[goal[2, 0]], [goal[3, 0]]]))
+ ff_U -= U_uncapped - U
+ goal = controller_column.A * goal + controller_column.B * ff_U
- glog.debug('Time: %f', self.t[-1])
- glog.debug('goal_error %s', repr((end_goal - goal).T))
- glog.debug('error %s', repr((observer_column.X_hat - end_goal).T))
+ if U[1, 0] != U_uncapped[1, 0]:
+ profile.MoveCurrentState(
+ numpy.matrix([[goal[2, 0]], [goal[3, 0]]]))
- def Plot(self):
- pylab.subplot(3, 1, 1)
- pylab.plot(self.t, self.xi, label='x_indexer')
- pylab.plot(self.t, self.xt, label='x_turret')
- pylab.plot(self.t, self.x_hat, label='x_hat')
- pylab.plot(self.t, self.turret_error, label='turret_error * 100')
- pylab.legend()
+ glog.debug('Time: %f', self.t[-1])
+ glog.debug('goal_error %s', repr((end_goal - goal).T))
+ glog.debug('error %s', repr((observer_column.X_hat - end_goal).T))
- pylab.subplot(3, 1, 2)
- pylab.plot(self.t, self.ui, label='u_indexer')
- pylab.plot(self.t, self.ui_fb, label='u_indexer_fb')
- pylab.plot(self.t, self.ut, label='u_turret')
- pylab.plot(self.t, self.ut_fb, label='u_turret_fb')
- pylab.plot(self.t, self.offseti, label='voltage_offset_indexer')
- pylab.plot(self.t, self.offsett, label='voltage_offset_turret')
- pylab.legend()
+ def Plot(self):
+ pylab.subplot(3, 1, 1)
+ pylab.plot(self.t, self.xi, label='x_indexer')
+ pylab.plot(self.t, self.xt, label='x_turret')
+ pylab.plot(self.t, self.x_hat, label='x_hat')
+ pylab.plot(self.t, self.turret_error, label='turret_error * 100')
+ pylab.legend()
- pylab.subplot(3, 1, 3)
- pylab.plot(self.t, self.ai, label='a_indexer')
- pylab.plot(self.t, self.at, label='a_turret')
- pylab.plot(self.t, self.vi, label='v_indexer')
- pylab.plot(self.t, self.vt, label='v_turret')
- pylab.legend()
+ pylab.subplot(3, 1, 2)
+ pylab.plot(self.t, self.ui, label='u_indexer')
+ pylab.plot(self.t, self.ui_fb, label='u_indexer_fb')
+ pylab.plot(self.t, self.ut, label='u_turret')
+ pylab.plot(self.t, self.ut_fb, label='u_turret_fb')
+ pylab.plot(self.t, self.offseti, label='voltage_offset_indexer')
+ pylab.plot(self.t, self.offsett, label='voltage_offset_turret')
+ pylab.legend()
- pylab.show()
+ pylab.subplot(3, 1, 3)
+ pylab.plot(self.t, self.ai, label='a_indexer')
+ pylab.plot(self.t, self.at, label='a_turret')
+ pylab.plot(self.t, self.vi, label='v_indexer')
+ pylab.plot(self.t, self.vt, label='v_turret')
+ pylab.legend()
+
+ pylab.show()
def main(argv):
- scenario_plotter = ScenarioPlotter()
+ scenario_plotter = ScenarioPlotter()
- column = Column()
- column_controller = IntegralColumn()
- observer_column = IntegralColumn()
+ column = Column()
+ column_controller = IntegralColumn()
+ observer_column = IntegralColumn()
- initial_X = numpy.matrix([[0.0], [0.0], [0.0], [0.0]])
- R = numpy.matrix([[0.0], [10.0], [5.0], [0.0], [0.0], [0.0]])
- scenario_plotter.run_test(column, end_goal=R, controller_column=column_controller,
- observer_column=observer_column, iterations=400)
+ initial_X = numpy.matrix([[0.0], [0.0], [0.0], [0.0]])
+ R = numpy.matrix([[0.0], [10.0], [5.0], [0.0], [0.0], [0.0]])
+ scenario_plotter.run_test(
+ column,
+ end_goal=R,
+ controller_column=column_controller,
+ observer_column=observer_column,
+ iterations=400)
- if FLAGS.plot:
- scenario_plotter.Plot()
+ if FLAGS.plot:
+ scenario_plotter.Plot()
- if len(argv) != 7:
- glog.fatal('Expected .h file name and .cc file names')
- else:
- namespaces = ['y2017', 'control_loops', 'superstructure', 'column']
- column = Column('Column')
- loop_writer = control_loop.ControlLoopWriter('Column', [column],
- namespaces=namespaces)
- loop_writer.AddConstant(control_loop.Constant(
- 'kIndexerFreeSpeed', '%f', column.indexer.free_speed))
- loop_writer.AddConstant(control_loop.Constant(
- 'kIndexerOutputRatio', '%f', column.indexer.G))
- loop_writer.AddConstant(control_loop.Constant(
- 'kTurretFreeSpeed', '%f', column.turret.free_speed))
- loop_writer.AddConstant(control_loop.Constant(
- 'kTurretOutputRatio', '%f', column.turret.G))
- loop_writer.Write(argv[1], argv[2])
+ if len(argv) != 7:
+ glog.fatal('Expected .h file name and .cc file names')
+ else:
+ namespaces = ['y2017', 'control_loops', 'superstructure', 'column']
+ column = Column('Column')
+ loop_writer = control_loop.ControlLoopWriter(
+ 'Column', [column], namespaces=namespaces)
+ loop_writer.AddConstant(
+ control_loop.Constant('kIndexerFreeSpeed', '%f',
+ column.indexer.free_speed))
+ loop_writer.AddConstant(
+ control_loop.Constant('kIndexerOutputRatio', '%f',
+ column.indexer.G))
+ loop_writer.AddConstant(
+ control_loop.Constant('kTurretFreeSpeed', '%f',
+ column.turret.free_speed))
+ loop_writer.AddConstant(
+ control_loop.Constant('kTurretOutputRatio', '%f', column.turret.G))
+ loop_writer.Write(argv[1], argv[2])
- # IntegralColumn controller 1 will disable the indexer.
- integral_column = IntegralColumn('IntegralColumn')
- disabled_integral_column = IntegralColumn('DisabledIntegralColumn',
- disable_indexer=True)
- integral_loop_writer = control_loop.ControlLoopWriter(
- 'IntegralColumn', [integral_column, disabled_integral_column],
- namespaces=namespaces)
- integral_loop_writer.Write(argv[3], argv[4])
+ # IntegralColumn controller 1 will disable the indexer.
+ integral_column = IntegralColumn('IntegralColumn')
+ disabled_integral_column = IntegralColumn(
+ 'DisabledIntegralColumn', disable_indexer=True)
+ integral_loop_writer = control_loop.ControlLoopWriter(
+ 'IntegralColumn', [integral_column, disabled_integral_column],
+ namespaces=namespaces)
+ integral_loop_writer.Write(argv[3], argv[4])
- stuck_integral_column = IntegralColumn('StuckIntegralColumn', voltage_error_noise=8.0)
- stuck_integral_loop_writer = control_loop.ControlLoopWriter(
- 'StuckIntegralColumn', [stuck_integral_column], namespaces=namespaces)
- stuck_integral_loop_writer.Write(argv[5], argv[6])
+ stuck_integral_column = IntegralColumn(
+ 'StuckIntegralColumn', voltage_error_noise=8.0)
+ stuck_integral_loop_writer = control_loop.ControlLoopWriter(
+ 'StuckIntegralColumn', [stuck_integral_column],
+ namespaces=namespaces)
+ stuck_integral_loop_writer.Write(argv[5], argv[6])
if __name__ == '__main__':
- argv = FLAGS(sys.argv)
- glog.init()
- sys.exit(main(argv))
+ argv = FLAGS(sys.argv)
+ glog.init()
+ sys.exit(main(argv))
diff --git a/y2017/control_loops/python/hood.py b/y2017/control_loops/python/hood.py
index fb5aa4e..58bd15e 100755
--- a/y2017/control_loops/python/hood.py
+++ b/y2017/control_loops/python/hood.py
@@ -12,329 +12,339 @@
FLAGS = gflags.FLAGS
try:
- gflags.DEFINE_bool('plot', False, 'If true, plot the loop response.')
+ gflags.DEFINE_bool('plot', False, 'If true, plot the loop response.')
except gflags.DuplicateFlagError:
- pass
+ pass
+
class Hood(control_loop.ControlLoop):
- def __init__(self, name='Hood'):
- super(Hood, self).__init__(name)
- # Stall Torque in N m
- self.stall_torque = 0.43
- # Stall Current in Amps
- self.stall_current = 53.0
- self.free_speed_rpm = 13180.0
- # Free Speed in rotations/second.
- self.free_speed = self.free_speed_rpm / 60
- # Free Current in Amps
- self.free_current = 1.8
- # Resistance of the motor
- self.R = 12.0 / self.stall_current
- # Motor velocity constant
- self.Kv = ((self.free_speed * 2.0 * numpy.pi) /
- (12.0 - self.R * self.free_current))
- # Torque constant
- self.Kt = self.stall_torque / self.stall_current
- # First axle gear ratio off the motor
- self.G1 = (12.0 / 60.0)
- # Second axle gear ratio off the motor
- self.G2 = self.G1 * (14.0 / 36.0)
- # Third axle gear ratio off the motor
- self.G3 = self.G2 * (14.0 / 36.0)
- # The last gear reduction (encoder -> hood angle)
- self.last_G = (20.0 / 345.0)
- # Gear ratio
- self.G = (12.0 / 60.0) * (14.0 / 36.0) * (14.0 / 36.0) * self.last_G
+ def __init__(self, name='Hood'):
+ super(Hood, self).__init__(name)
+ # Stall Torque in N m
+ self.stall_torque = 0.43
+ # Stall Current in Amps
+ self.stall_current = 53.0
+ self.free_speed_rpm = 13180.0
+ # Free Speed in rotations/second.
+ self.free_speed = self.free_speed_rpm / 60
+ # Free Current in Amps
+ self.free_current = 1.8
- # 36 tooth gear inertia in kg * m^2
- self.big_gear_inertia = 0.5 * 0.039 * ((36.0 / 40.0 * 0.025) ** 2)
+ # Resistance of the motor
+ self.R = 12.0 / self.stall_current
+ # Motor velocity constant
+ self.Kv = ((self.free_speed * 2.0 * numpy.pi) /
+ (12.0 - self.R * self.free_current))
+ # Torque constant
+ self.Kt = self.stall_torque / self.stall_current
+ # First axle gear ratio off the motor
+ self.G1 = (12.0 / 60.0)
+ # Second axle gear ratio off the motor
+ self.G2 = self.G1 * (14.0 / 36.0)
+ # Third axle gear ratio off the motor
+ self.G3 = self.G2 * (14.0 / 36.0)
+ # The last gear reduction (encoder -> hood angle)
+ self.last_G = (20.0 / 345.0)
+ # Gear ratio
+ self.G = (12.0 / 60.0) * (14.0 / 36.0) * (14.0 / 36.0) * self.last_G
- # Motor inertia in kg * m^2
- self.motor_inertia = 0.000006
- glog.debug(self.big_gear_inertia)
+ # 36 tooth gear inertia in kg * m^2
+ self.big_gear_inertia = 0.5 * 0.039 * ((36.0 / 40.0 * 0.025)**2)
- # Moment of inertia, measured in CAD.
- # Extra mass to compensate for friction is added on.
- self.J = 0.08 + 2.3 + \
- self.big_gear_inertia * ((self.G1 / self.G) ** 2) + \
- self.big_gear_inertia * ((self.G2 / self.G) ** 2) + \
- self.motor_inertia * ((1.0 / self.G) ** 2.0)
- glog.debug('J effective %f', self.J)
+ # Motor inertia in kg * m^2
+ self.motor_inertia = 0.000006
+ glog.debug(self.big_gear_inertia)
- # Control loop time step
- self.dt = 0.005
+ # Moment of inertia, measured in CAD.
+ # Extra mass to compensate for friction is added on.
+ self.J = 0.08 + 2.3 + \
+ self.big_gear_inertia * ((self.G1 / self.G) ** 2) + \
+ self.big_gear_inertia * ((self.G2 / self.G) ** 2) + \
+ self.motor_inertia * ((1.0 / self.G) ** 2.0)
+ glog.debug('J effective %f', self.J)
- # State is [position, velocity]
- # Input is [Voltage]
+ # Control loop time step
+ self.dt = 0.005
- C1 = self.Kt / (self.R * self.J * self.Kv * self.G * self.G)
- C2 = self.Kt / (self.J * self.R * self.G)
+ # State is [position, velocity]
+ # Input is [Voltage]
- self.A_continuous = numpy.matrix(
- [[0, 1],
- [0, -C1]])
+ C1 = self.Kt / (self.R * self.J * self.Kv * self.G * self.G)
+ C2 = self.Kt / (self.J * self.R * self.G)
- # Start with the unmodified input
- self.B_continuous = numpy.matrix(
- [[0],
- [C2]])
+ self.A_continuous = numpy.matrix([[0, 1], [0, -C1]])
- self.C = numpy.matrix([[1, 0]])
- self.D = numpy.matrix([[0]])
+ # Start with the unmodified input
+ self.B_continuous = numpy.matrix([[0], [C2]])
- self.A, self.B = self.ContinuousToDiscrete(
- self.A_continuous, self.B_continuous, self.dt)
+ self.C = numpy.matrix([[1, 0]])
+ self.D = numpy.matrix([[0]])
- controllability = controls.ctrb(self.A, self.B)
+ self.A, self.B = self.ContinuousToDiscrete(self.A_continuous,
+ self.B_continuous, self.dt)
- glog.debug('Free speed is %f',
- -self.B_continuous[1, 0] / self.A_continuous[1, 1] * 12.0)
- glog.debug(repr(self.A_continuous))
+ controllability = controls.ctrb(self.A, self.B)
- # Calculate the LQR controller gain
- q_pos = 0.015
- q_vel = 0.40
- self.Q = numpy.matrix([[(1.0 / (q_pos ** 2.0)), 0.0],
- [0.0, (1.0 / (q_vel ** 2.0))]])
+ glog.debug('Free speed is %f',
+ -self.B_continuous[1, 0] / self.A_continuous[1, 1] * 12.0)
+ glog.debug(repr(self.A_continuous))
- self.R = numpy.matrix([[(5.0 / (12.0 ** 2.0))]])
- self.K = controls.dlqr(self.A, self.B, self.Q, self.R)
+ # Calculate the LQR controller gain
+ q_pos = 0.015
+ q_vel = 0.40
+ self.Q = numpy.matrix([[(1.0 / (q_pos**2.0)), 0.0],
+ [0.0, (1.0 / (q_vel**2.0))]])
- # Calculate the feed forwards gain.
- q_pos_ff = 0.005
- q_vel_ff = 1.0
- self.Qff = numpy.matrix([[(1.0 / (q_pos_ff ** 2.0)), 0.0],
- [0.0, (1.0 / (q_vel_ff ** 2.0))]])
+ self.R = numpy.matrix([[(5.0 / (12.0**2.0))]])
+ self.K = controls.dlqr(self.A, self.B, self.Q, self.R)
- self.Kff = controls.TwoStateFeedForwards(self.B, self.Qff)
+ # Calculate the feed forwards gain.
+ q_pos_ff = 0.005
+ q_vel_ff = 1.0
+ self.Qff = numpy.matrix([[(1.0 / (q_pos_ff**2.0)), 0.0],
+ [0.0, (1.0 / (q_vel_ff**2.0))]])
- glog.debug('K %s', repr(self.K))
- glog.debug('Poles are %s',
- repr(numpy.linalg.eig(self.A - self.B * self.K)[0]))
+ self.Kff = controls.TwoStateFeedForwards(self.B, self.Qff)
- q_pos = 0.10
- q_vel = 1.65
- self.Q = numpy.matrix([[(q_pos ** 2.0), 0.0],
- [0.0, (q_vel ** 2.0)]])
+ glog.debug('K %s', repr(self.K))
+ glog.debug('Poles are %s',
+ repr(numpy.linalg.eig(self.A - self.B * self.K)[0]))
- r_volts = 0.025
- self.R = numpy.matrix([[(r_volts ** 2.0)]])
+ q_pos = 0.10
+ q_vel = 1.65
+ self.Q = numpy.matrix([[(q_pos**2.0), 0.0], [0.0, (q_vel**2.0)]])
- self.KalmanGain, self.Q_steady = controls.kalman(
- A=self.A, B=self.B, C=self.C, Q=self.Q, R=self.R)
+ r_volts = 0.025
+ self.R = numpy.matrix([[(r_volts**2.0)]])
- glog.debug('Kal %s', repr(self.KalmanGain))
- self.L = self.A * self.KalmanGain
- glog.debug('KalL is %s', repr(self.L))
+ self.KalmanGain, self.Q_steady = controls.kalman(
+ A=self.A, B=self.B, C=self.C, Q=self.Q, R=self.R)
- # The box formed by U_min and U_max must encompass all possible values,
- # or else Austin's code gets angry.
- self.U_max = numpy.matrix([[12.0]])
- self.U_min = numpy.matrix([[-12.0]])
+ glog.debug('Kal %s', repr(self.KalmanGain))
+ self.L = self.A * self.KalmanGain
+ glog.debug('KalL is %s', repr(self.L))
- self.InitializeState()
+ # The box formed by U_min and U_max must encompass all possible values,
+ # or else Austin's code gets angry.
+ self.U_max = numpy.matrix([[12.0]])
+ self.U_min = numpy.matrix([[-12.0]])
+
+ self.InitializeState()
+
class IntegralHood(Hood):
- def __init__(self, name='IntegralHood'):
- super(IntegralHood, self).__init__(name=name)
- self.A_continuous_unaugmented = self.A_continuous
- self.B_continuous_unaugmented = self.B_continuous
+ def __init__(self, name='IntegralHood'):
+ super(IntegralHood, self).__init__(name=name)
- self.A_continuous = numpy.matrix(numpy.zeros((3, 3)))
- self.A_continuous[0:2, 0:2] = self.A_continuous_unaugmented
- self.A_continuous[0:2, 2] = self.B_continuous_unaugmented
+ self.A_continuous_unaugmented = self.A_continuous
+ self.B_continuous_unaugmented = self.B_continuous
- self.B_continuous = numpy.matrix(numpy.zeros((3, 1)))
- self.B_continuous[0:2, 0] = self.B_continuous_unaugmented
+ self.A_continuous = numpy.matrix(numpy.zeros((3, 3)))
+ self.A_continuous[0:2, 0:2] = self.A_continuous_unaugmented
+ self.A_continuous[0:2, 2] = self.B_continuous_unaugmented
- self.C_unaugmented = self.C
- self.C = numpy.matrix(numpy.zeros((1, 3)))
- self.C[0:1, 0:2] = self.C_unaugmented
+ self.B_continuous = numpy.matrix(numpy.zeros((3, 1)))
+ self.B_continuous[0:2, 0] = self.B_continuous_unaugmented
- self.A, self.B = self.ContinuousToDiscrete(
- self.A_continuous, self.B_continuous, self.dt)
+ self.C_unaugmented = self.C
+ self.C = numpy.matrix(numpy.zeros((1, 3)))
+ self.C[0:1, 0:2] = self.C_unaugmented
- q_pos = 0.01
- q_vel = 4.0
- q_voltage = 4.0
- self.Q = numpy.matrix([[(q_pos ** 2.0), 0.0, 0.0],
- [0.0, (q_vel ** 2.0), 0.0],
- [0.0, 0.0, (q_voltage ** 2.0)]])
+ self.A, self.B = self.ContinuousToDiscrete(self.A_continuous,
+ self.B_continuous, self.dt)
- r_pos = 0.001
- self.R = numpy.matrix([[(r_pos ** 2.0)]])
+ q_pos = 0.01
+ q_vel = 4.0
+ q_voltage = 4.0
+ self.Q = numpy.matrix([[(q_pos**2.0), 0.0, 0.0],
+ [0.0, (q_vel**2.0), 0.0],
+ [0.0, 0.0, (q_voltage**2.0)]])
- self.KalmanGain, self.Q_steady = controls.kalman(
- A=self.A, B=self.B, C=self.C, Q=self.Q, R=self.R)
- self.L = self.A * self.KalmanGain
+ r_pos = 0.001
+ self.R = numpy.matrix([[(r_pos**2.0)]])
- self.K_unaugmented = self.K
- self.K = numpy.matrix(numpy.zeros((1, 3)))
- self.K[0, 0:2] = self.K_unaugmented
- self.K[0, 2] = 1
+ self.KalmanGain, self.Q_steady = controls.kalman(
+ A=self.A, B=self.B, C=self.C, Q=self.Q, R=self.R)
+ self.L = self.A * self.KalmanGain
- self.Kff = numpy.concatenate((self.Kff, numpy.matrix(numpy.zeros((1, 1)))), axis=1)
+ self.K_unaugmented = self.K
+ self.K = numpy.matrix(numpy.zeros((1, 3)))
+ self.K[0, 0:2] = self.K_unaugmented
+ self.K[0, 2] = 1
- self.InitializeState()
+ self.Kff = numpy.concatenate(
+ (self.Kff, numpy.matrix(numpy.zeros((1, 1)))), axis=1)
+
+ self.InitializeState()
+
class ScenarioPlotter(object):
- def __init__(self):
- # Various lists for graphing things.
- self.t = []
- self.x = []
- self.v = []
- self.v_hat = []
- self.a = []
- self.x_hat = []
- self.u = []
- self.offset = []
- def run_test(self, hood, end_goal,
- controller_hood,
- observer_hood=None,
- iterations=200):
- """Runs the hood plant with an initial condition and goal.
+ def __init__(self):
+ # Various lists for graphing things.
+ self.t = []
+ self.x = []
+ self.v = []
+ self.v_hat = []
+ self.a = []
+ self.x_hat = []
+ self.u = []
+ self.offset = []
- Args:
- hood: hood object to use.
- end_goal: end_goal state.
- controller_hood: Hood object to get K from, or None if we should
- use hood.
- observer_hood: Hood object to use for the observer, or None if we should
- use the actual state.
- iterations: Number of timesteps to run the model for.
- """
+ def run_test(self,
+ hood,
+ end_goal,
+ controller_hood,
+ observer_hood=None,
+ iterations=200):
+ """Runs the hood plant with an initial condition and goal.
- if controller_hood is None:
- controller_hood = hood
+ Args:
+ hood: hood object to use.
+ end_goal: end_goal state.
+ controller_hood: Hood object to get K from, or None if we should
+ use hood.
+ observer_hood: Hood object to use for the observer, or None if we should
+ use the actual state.
+ iterations: Number of timesteps to run the model for.
+ """
- vbat = 12.0
+ if controller_hood is None:
+ controller_hood = hood
- if self.t:
- initial_t = self.t[-1] + hood.dt
- else:
- initial_t = 0
+ vbat = 12.0
- goal = numpy.concatenate((hood.X, numpy.matrix(numpy.zeros((1, 1)))), axis=0)
+ if self.t:
+ initial_t = self.t[-1] + hood.dt
+ else:
+ initial_t = 0
- profile = TrapezoidProfile(hood.dt)
- profile.set_maximum_acceleration(10.0)
- profile.set_maximum_velocity(1.0)
- profile.SetGoal(goal[0, 0])
+ goal = numpy.concatenate((hood.X, numpy.matrix(numpy.zeros((1, 1)))),
+ axis=0)
- U_last = numpy.matrix(numpy.zeros((1, 1)))
- for i in xrange(iterations):
- observer_hood.Y = hood.Y
- observer_hood.CorrectObserver(U_last)
+ profile = TrapezoidProfile(hood.dt)
+ profile.set_maximum_acceleration(10.0)
+ profile.set_maximum_velocity(1.0)
+ profile.SetGoal(goal[0, 0])
- self.offset.append(observer_hood.X_hat[2, 0])
- self.x_hat.append(observer_hood.X_hat[0, 0])
+ U_last = numpy.matrix(numpy.zeros((1, 1)))
+ for i in xrange(iterations):
+ observer_hood.Y = hood.Y
+ observer_hood.CorrectObserver(U_last)
- next_goal = numpy.concatenate(
- (profile.Update(end_goal[0, 0], end_goal[1, 0]),
- numpy.matrix(numpy.zeros((1, 1)))),
- axis=0)
+ self.offset.append(observer_hood.X_hat[2, 0])
+ self.x_hat.append(observer_hood.X_hat[0, 0])
- ff_U = controller_hood.Kff * (next_goal - observer_hood.A * goal)
+ next_goal = numpy.concatenate(
+ (profile.Update(end_goal[0, 0], end_goal[1, 0]),
+ numpy.matrix(numpy.zeros((1, 1)))),
+ axis=0)
- U_uncapped = controller_hood.K * (goal - observer_hood.X_hat) + ff_U
- U = U_uncapped.copy()
- U[0, 0] = numpy.clip(U[0, 0], -vbat, vbat)
- self.x.append(hood.X[0, 0])
+ ff_U = controller_hood.Kff * (next_goal - observer_hood.A * goal)
- if self.v:
- last_v = self.v[-1]
- else:
- last_v = 0
+ U_uncapped = controller_hood.K * (goal - observer_hood.X_hat) + ff_U
+ U = U_uncapped.copy()
+ U[0, 0] = numpy.clip(U[0, 0], -vbat, vbat)
+ self.x.append(hood.X[0, 0])
- self.v.append(hood.X[1, 0])
- self.a.append((self.v[-1] - last_v) / hood.dt)
- self.v_hat.append(observer_hood.X_hat[1, 0])
+ if self.v:
+ last_v = self.v[-1]
+ else:
+ last_v = 0
- offset = 0.0
- if i > 100:
- offset = 2.0
- hood.Update(U + offset)
+ self.v.append(hood.X[1, 0])
+ self.a.append((self.v[-1] - last_v) / hood.dt)
+ self.v_hat.append(observer_hood.X_hat[1, 0])
- observer_hood.PredictObserver(U)
+ offset = 0.0
+ if i > 100:
+ offset = 2.0
+ hood.Update(U + offset)
- self.t.append(initial_t + i * hood.dt)
- self.u.append(U[0, 0])
+ observer_hood.PredictObserver(U)
- ff_U -= U_uncapped - U
- goal = controller_hood.A * goal + controller_hood.B * ff_U
+ self.t.append(initial_t + i * hood.dt)
+ self.u.append(U[0, 0])
- if U[0, 0] != U_uncapped[0, 0]:
- profile.MoveCurrentState(
- numpy.matrix([[goal[0, 0]], [goal[1, 0]]]))
+ ff_U -= U_uncapped - U
+ goal = controller_hood.A * goal + controller_hood.B * ff_U
- glog.debug('Time: %f', self.t[-1])
- glog.debug('goal_error %s', repr(end_goal - goal))
- glog.debug('error %s', repr(observer_hood.X_hat - end_goal))
+ if U[0, 0] != U_uncapped[0, 0]:
+ profile.MoveCurrentState(
+ numpy.matrix([[goal[0, 0]], [goal[1, 0]]]))
- def Plot(self):
- pylab.subplot(3, 1, 1)
- pylab.plot(self.t, self.x, label='x')
- pylab.plot(self.t, self.x_hat, label='x_hat')
- pylab.plot(self.t, self.v, label='v')
- pylab.plot(self.t, self.v_hat, label='v_hat')
- pylab.legend()
+ glog.debug('Time: %f', self.t[-1])
+ glog.debug('goal_error %s', repr(end_goal - goal))
+ glog.debug('error %s', repr(observer_hood.X_hat - end_goal))
- pylab.subplot(3, 1, 2)
- pylab.plot(self.t, self.u, label='u')
- pylab.plot(self.t, self.offset, label='voltage_offset')
- pylab.legend()
+ def Plot(self):
+ pylab.subplot(3, 1, 1)
+ pylab.plot(self.t, self.x, label='x')
+ pylab.plot(self.t, self.x_hat, label='x_hat')
+ pylab.plot(self.t, self.v, label='v')
+ pylab.plot(self.t, self.v_hat, label='v_hat')
+ pylab.legend()
- pylab.subplot(3, 1, 3)
- pylab.plot(self.t, self.a, label='a')
- pylab.legend()
+ pylab.subplot(3, 1, 2)
+ pylab.plot(self.t, self.u, label='u')
+ pylab.plot(self.t, self.offset, label='voltage_offset')
+ pylab.legend()
- pylab.show()
+ pylab.subplot(3, 1, 3)
+ pylab.plot(self.t, self.a, label='a')
+ pylab.legend()
+
+ pylab.show()
def main(argv):
- scenario_plotter = ScenarioPlotter()
+ scenario_plotter = ScenarioPlotter()
- hood = Hood()
- hood_controller = IntegralHood()
- observer_hood = IntegralHood()
+ hood = Hood()
+ hood_controller = IntegralHood()
+ observer_hood = IntegralHood()
- # Test moving the hood with constant separation.
- initial_X = numpy.matrix([[0.0], [0.0]])
- R = numpy.matrix([[numpy.pi/4.0], [0.0], [0.0]])
- scenario_plotter.run_test(hood, end_goal=R,
- controller_hood=hood_controller,
- observer_hood=observer_hood, iterations=200)
+ # Test moving the hood with constant separation.
+ initial_X = numpy.matrix([[0.0], [0.0]])
+ R = numpy.matrix([[numpy.pi / 4.0], [0.0], [0.0]])
+ scenario_plotter.run_test(
+ hood,
+ end_goal=R,
+ controller_hood=hood_controller,
+ observer_hood=observer_hood,
+ iterations=200)
- if FLAGS.plot:
- scenario_plotter.Plot()
+ if FLAGS.plot:
+ scenario_plotter.Plot()
- # Write the generated constants out to a file.
- if len(argv) != 5:
- glog.fatal('Expected .h file name and .cc file name for the hood and integral hood.')
- else:
- namespaces = ['y2017', 'control_loops', 'superstructure', 'hood']
- hood = Hood('Hood')
- loop_writer = control_loop.ControlLoopWriter('Hood', [hood],
- namespaces=namespaces)
- loop_writer.AddConstant(control_loop.Constant(
- 'kFreeSpeed', '%f', hood.free_speed))
- loop_writer.AddConstant(control_loop.Constant(
- 'kOutputRatio', '%f', hood.G))
- loop_writer.Write(argv[1], argv[2])
+ # Write the generated constants out to a file.
+ if len(argv) != 5:
+ glog.fatal(
+ 'Expected .h file name and .cc file name for the hood and integral hood.'
+ )
+ else:
+ namespaces = ['y2017', 'control_loops', 'superstructure', 'hood']
+ hood = Hood('Hood')
+ loop_writer = control_loop.ControlLoopWriter(
+ 'Hood', [hood], namespaces=namespaces)
+ loop_writer.AddConstant(
+ control_loop.Constant('kFreeSpeed', '%f', hood.free_speed))
+ loop_writer.AddConstant(
+ control_loop.Constant('kOutputRatio', '%f', hood.G))
+ loop_writer.Write(argv[1], argv[2])
- integral_hood = IntegralHood('IntegralHood')
- integral_loop_writer = control_loop.ControlLoopWriter('IntegralHood', [integral_hood],
- namespaces=namespaces)
- integral_loop_writer.AddConstant(control_loop.Constant('kLastReduction', '%f',
- integral_hood.last_G))
- integral_loop_writer.Write(argv[3], argv[4])
+ integral_hood = IntegralHood('IntegralHood')
+ integral_loop_writer = control_loop.ControlLoopWriter(
+ 'IntegralHood', [integral_hood], namespaces=namespaces)
+ integral_loop_writer.AddConstant(
+ control_loop.Constant('kLastReduction', '%f', integral_hood.last_G))
+ integral_loop_writer.Write(argv[3], argv[4])
if __name__ == '__main__':
- argv = FLAGS(sys.argv)
- glog.init()
- sys.exit(main(argv))
+ argv = FLAGS(sys.argv)
+ glog.init()
+ sys.exit(main(argv))
diff --git a/y2017/control_loops/python/shooter.py b/y2017/control_loops/python/shooter.py
index 1b0ff13..a825ff0 100755
--- a/y2017/control_loops/python/shooter.py
+++ b/y2017/control_loops/python/shooter.py
@@ -16,370 +16,388 @@
def PlotDiff(list1, list2, time):
- pylab.subplot(1, 1, 1)
- pylab.plot(time, numpy.subtract(list1, list2), label='diff')
- pylab.legend()
+ pylab.subplot(1, 1, 1)
+ pylab.plot(time, numpy.subtract(list1, list2), label='diff')
+ pylab.legend()
+
class VelocityShooter(control_loop.HybridControlLoop):
- def __init__(self, name='VelocityShooter'):
- super(VelocityShooter, self).__init__(name)
- # Number of motors
- self.num_motors = 2.0
- # Stall Torque in N m
- self.stall_torque = 0.71 * self.num_motors
- # Stall Current in Amps
- self.stall_current = 134.0 * self.num_motors
- # Free Speed in RPM
- self.free_speed_rpm = 18730.0
- # Free Speed in rotations/second.
- self.free_speed = self.free_speed_rpm / 60.0
- # Free Current in Amps
- self.free_current = 0.7 * self.num_motors
- # Moment of inertia of the shooter wheel in kg m^2
- # 1400.6 grams/cm^2
- # 1.407 *1e-4 kg m^2
- self.J = 0.00120
- # Resistance of the motor, divided by 2 to account for the 2 motors
- self.R = 12.0 / self.stall_current
- # Motor velocity constant
- self.Kv = ((self.free_speed * 2.0 * numpy.pi) /
- (12.0 - self.R * self.free_current))
- # Torque constant
- self.Kt = self.stall_torque / self.stall_current
- # Gear ratio
- self.G = 12.0 / 36.0
- # Control loop time step
- self.dt = 0.00505
- # State feedback matrices
- # [angular velocity]
- self.A_continuous = numpy.matrix(
- [[-self.Kt / (self.Kv * self.J * self.G * self.G * self.R)]])
- self.B_continuous = numpy.matrix(
- [[self.Kt / (self.J * self.G * self.R)]])
- self.C = numpy.matrix([[1]])
- self.D = numpy.matrix([[0]])
+ def __init__(self, name='VelocityShooter'):
+ super(VelocityShooter, self).__init__(name)
+ # Number of motors
+ self.num_motors = 2.0
+ # Stall Torque in N m
+ self.stall_torque = 0.71 * self.num_motors
+ # Stall Current in Amps
+ self.stall_current = 134.0 * self.num_motors
+ # Free Speed in RPM
+ self.free_speed_rpm = 18730.0
+ # Free Speed in rotations/second.
+ self.free_speed = self.free_speed_rpm / 60.0
+ # Free Current in Amps
+ self.free_current = 0.7 * self.num_motors
+ # Moment of inertia of the shooter wheel in kg m^2
+ # 1400.6 grams/cm^2
+ # 1.407 *1e-4 kg m^2
+ self.J = 0.00120
+ # Resistance of the motor, divided by 2 to account for the 2 motors
+ self.R = 12.0 / self.stall_current
+ # Motor velocity constant
+ self.Kv = ((self.free_speed * 2.0 * numpy.pi) /
+ (12.0 - self.R * self.free_current))
+ # Torque constant
+ self.Kt = self.stall_torque / self.stall_current
+ # Gear ratio
+ self.G = 12.0 / 36.0
+ # Control loop time step
+ self.dt = 0.00505
- # The states are [unfiltered_velocity]
- self.A, self.B = self.ContinuousToDiscrete(
- self.A_continuous, self.B_continuous, self.dt)
+ # State feedback matrices
+ # [angular velocity]
+ self.A_continuous = numpy.matrix(
+ [[-self.Kt / (self.Kv * self.J * self.G * self.G * self.R)]])
+ self.B_continuous = numpy.matrix(
+ [[self.Kt / (self.J * self.G * self.R)]])
+ self.C = numpy.matrix([[1]])
+ self.D = numpy.matrix([[0]])
- self.PlaceControllerPoles([.75])
+ # The states are [unfiltered_velocity]
+ self.A, self.B = self.ContinuousToDiscrete(self.A_continuous,
+ self.B_continuous, self.dt)
- glog.debug('K %s', repr(self.K))
- glog.debug('System poles are %s',
- repr(numpy.linalg.eig(self.A_continuous)[0]))
- glog.debug('Poles are %s',
- repr(numpy.linalg.eig(self.A - self.B * self.K)[0]))
+ self.PlaceControllerPoles([.75])
- self.PlaceObserverPoles([0.3])
+ glog.debug('K %s', repr(self.K))
+ glog.debug('System poles are %s',
+ repr(numpy.linalg.eig(self.A_continuous)[0]))
+ glog.debug('Poles are %s',
+ repr(numpy.linalg.eig(self.A - self.B * self.K)[0]))
- self.U_max = numpy.matrix([[12.0]])
- self.U_min = numpy.matrix([[-12.0]])
+ self.PlaceObserverPoles([0.3])
- qff_vel = 8.0
- self.Qff = numpy.matrix([[1.0 / (qff_vel ** 2.0)]])
+ self.U_max = numpy.matrix([[12.0]])
+ self.U_min = numpy.matrix([[-12.0]])
- self.Kff = controls.TwoStateFeedForwards(self.B, self.Qff)
- self.InitializeState()
+ qff_vel = 8.0
+ self.Qff = numpy.matrix([[1.0 / (qff_vel**2.0)]])
+
+ self.Kff = controls.TwoStateFeedForwards(self.B, self.Qff)
+ self.InitializeState()
+
class SecondOrderVelocityShooter(VelocityShooter):
- def __init__(self, name='SecondOrderVelocityShooter'):
- super(SecondOrderVelocityShooter, self).__init__(name)
- self.A_continuous_unaugmented = self.A_continuous
- self.B_continuous_unaugmented = self.B_continuous
+ def __init__(self, name='SecondOrderVelocityShooter'):
+ super(SecondOrderVelocityShooter, self).__init__(name)
- self.A_continuous = numpy.matrix(numpy.zeros((2, 2)))
- self.A_continuous[0:1, 0:1] = self.A_continuous_unaugmented
- self.A_continuous[1, 0] = 175.0
- self.A_continuous[1, 1] = -self.A_continuous[1, 0]
+ self.A_continuous_unaugmented = self.A_continuous
+ self.B_continuous_unaugmented = self.B_continuous
- self.B_continuous = numpy.matrix(numpy.zeros((2, 1)))
- self.B_continuous[0:1, 0] = self.B_continuous_unaugmented
+ self.A_continuous = numpy.matrix(numpy.zeros((2, 2)))
+ self.A_continuous[0:1, 0:1] = self.A_continuous_unaugmented
+ self.A_continuous[1, 0] = 175.0
+ self.A_continuous[1, 1] = -self.A_continuous[1, 0]
- self.C = numpy.matrix([[0, 1]])
- self.D = numpy.matrix([[0]])
+ self.B_continuous = numpy.matrix(numpy.zeros((2, 1)))
+ self.B_continuous[0:1, 0] = self.B_continuous_unaugmented
- # The states are [unfiltered_velocity, velocity]
- self.A, self.B = self.ContinuousToDiscrete(
- self.A_continuous, self.B_continuous, self.dt)
+ self.C = numpy.matrix([[0, 1]])
+ self.D = numpy.matrix([[0]])
- self.PlaceControllerPoles([.70, 0.60])
+ # The states are [unfiltered_velocity, velocity]
+ self.A, self.B = self.ContinuousToDiscrete(self.A_continuous,
+ self.B_continuous, self.dt)
- q_vel = 40.0
- q_filteredvel = 30.0
- self.Q = numpy.matrix([[(1.0 / (q_vel ** 2.0)), 0.0],
- [0.0, (1.0 / (q_filteredvel ** 2.0))]])
+ self.PlaceControllerPoles([.70, 0.60])
- self.R = numpy.matrix([[(1.0 / (3.0 ** 2.0))]])
- self.K = controls.dlqr(self.A, self.B, self.Q, self.R)
+ q_vel = 40.0
+ q_filteredvel = 30.0
+ self.Q = numpy.matrix([[(1.0 / (q_vel**2.0)), 0.0],
+ [0.0, (1.0 / (q_filteredvel**2.0))]])
- glog.debug('K %s', repr(self.K))
- glog.debug('System poles are %s',
- repr(numpy.linalg.eig(self.A_continuous)[0]))
- glog.debug('Poles are %s',
- repr(numpy.linalg.eig(self.A - self.B * self.K)[0]))
+ self.R = numpy.matrix([[(1.0 / (3.0**2.0))]])
+ self.K = controls.dlqr(self.A, self.B, self.Q, self.R)
- self.PlaceObserverPoles([0.3, 0.3])
+ glog.debug('K %s', repr(self.K))
+ glog.debug('System poles are %s',
+ repr(numpy.linalg.eig(self.A_continuous)[0]))
+ glog.debug('Poles are %s',
+ repr(numpy.linalg.eig(self.A - self.B * self.K)[0]))
- self.U_max = numpy.matrix([[12.0]])
- self.U_min = numpy.matrix([[-12.0]])
+ self.PlaceObserverPoles([0.3, 0.3])
- qff_vel = 8.0
- self.Qff = numpy.matrix([[1.0 / (qff_vel ** 2.0), 0.0],
- [0.0, 1.0 / (qff_vel ** 2.0)]])
+ self.U_max = numpy.matrix([[12.0]])
+ self.U_min = numpy.matrix([[-12.0]])
- self.Kff = controls.TwoStateFeedForwards(self.B, self.Qff)
- self.InitializeState()
+ qff_vel = 8.0
+ self.Qff = numpy.matrix([[1.0 / (qff_vel**2.0), 0.0],
+ [0.0, 1.0 / (qff_vel**2.0)]])
+
+ self.Kff = controls.TwoStateFeedForwards(self.B, self.Qff)
+ self.InitializeState()
class Shooter(SecondOrderVelocityShooter):
- def __init__(self, name='Shooter'):
- super(Shooter, self).__init__(name)
- self.A_continuous_unaugmented = self.A_continuous
- self.B_continuous_unaugmented = self.B_continuous
+ def __init__(self, name='Shooter'):
+ super(Shooter, self).__init__(name)
- self.A_continuous = numpy.matrix(numpy.zeros((3, 3)))
- self.A_continuous[1:3, 1:3] = self.A_continuous_unaugmented
- self.A_continuous[0, 2] = 1
+ self.A_continuous_unaugmented = self.A_continuous
+ self.B_continuous_unaugmented = self.B_continuous
- self.B_continuous = numpy.matrix(numpy.zeros((3, 1)))
- self.B_continuous[1:3, 0] = self.B_continuous_unaugmented
+ self.A_continuous = numpy.matrix(numpy.zeros((3, 3)))
+ self.A_continuous[1:3, 1:3] = self.A_continuous_unaugmented
+ self.A_continuous[0, 2] = 1
- # State feedback matrices
- # [position, unfiltered_velocity, angular velocity]
- self.C = numpy.matrix([[1, 0, 0]])
- self.D = numpy.matrix([[0]])
+ self.B_continuous = numpy.matrix(numpy.zeros((3, 1)))
+ self.B_continuous[1:3, 0] = self.B_continuous_unaugmented
- self.A, self.B = self.ContinuousToDiscrete(
- self.A_continuous, self.B_continuous, self.dt)
- glog.debug(repr(self.A_continuous))
- glog.debug(repr(self.B_continuous))
+ # State feedback matrices
+ # [position, unfiltered_velocity, angular velocity]
+ self.C = numpy.matrix([[1, 0, 0]])
+ self.D = numpy.matrix([[0]])
- observeability = controls.ctrb(self.A.T, self.C.T)
- glog.debug('Rank of augmented observability matrix. %d', numpy.linalg.matrix_rank(
- observeability))
+ self.A, self.B = self.ContinuousToDiscrete(self.A_continuous,
+ self.B_continuous, self.dt)
+ glog.debug(repr(self.A_continuous))
+ glog.debug(repr(self.B_continuous))
+ observeability = controls.ctrb(self.A.T, self.C.T)
+ glog.debug('Rank of augmented observability matrix. %d',
+ numpy.linalg.matrix_rank(observeability))
- self.PlaceObserverPoles([0.9, 0.8, 0.7])
+ self.PlaceObserverPoles([0.9, 0.8, 0.7])
- self.K_unaugmented = self.K
- self.K = numpy.matrix(numpy.zeros((1, 3)))
- self.K[0, 1:3] = self.K_unaugmented
- self.Kff_unaugmented = self.Kff
- self.Kff = numpy.matrix(numpy.zeros((1, 3)))
- self.Kff[0, 1:3] = self.Kff_unaugmented
+ self.K_unaugmented = self.K
+ self.K = numpy.matrix(numpy.zeros((1, 3)))
+ self.K[0, 1:3] = self.K_unaugmented
+ self.Kff_unaugmented = self.Kff
+ self.Kff = numpy.matrix(numpy.zeros((1, 3)))
+ self.Kff[0, 1:3] = self.Kff_unaugmented
- self.InitializeState()
+ self.InitializeState()
class IntegralShooter(Shooter):
- def __init__(self, name='IntegralShooter'):
- super(IntegralShooter, self).__init__(name=name)
- self.A_continuous_unaugmented = self.A_continuous
- self.B_continuous_unaugmented = self.B_continuous
+ def __init__(self, name='IntegralShooter'):
+ super(IntegralShooter, self).__init__(name=name)
- self.A_continuous = numpy.matrix(numpy.zeros((4, 4)))
- self.A_continuous[0:3, 0:3] = self.A_continuous_unaugmented
- self.A_continuous[0:3, 3] = self.B_continuous_unaugmented
+ self.A_continuous_unaugmented = self.A_continuous
+ self.B_continuous_unaugmented = self.B_continuous
- self.B_continuous = numpy.matrix(numpy.zeros((4, 1)))
- self.B_continuous[0:3, 0] = self.B_continuous_unaugmented
+ self.A_continuous = numpy.matrix(numpy.zeros((4, 4)))
+ self.A_continuous[0:3, 0:3] = self.A_continuous_unaugmented
+ self.A_continuous[0:3, 3] = self.B_continuous_unaugmented
- self.C_unaugmented = self.C
- self.C = numpy.matrix(numpy.zeros((1, 4)))
- self.C[0:1, 0:3] = self.C_unaugmented
+ self.B_continuous = numpy.matrix(numpy.zeros((4, 1)))
+ self.B_continuous[0:3, 0] = self.B_continuous_unaugmented
- # The states are [position, unfiltered_velocity, velocity, torque_error]
- self.A, self.B = self.ContinuousToDiscrete(
- self.A_continuous, self.B_continuous, self.dt)
+ self.C_unaugmented = self.C
+ self.C = numpy.matrix(numpy.zeros((1, 4)))
+ self.C[0:1, 0:3] = self.C_unaugmented
- glog.debug('A: \n%s', repr(self.A_continuous))
- glog.debug('eig(A): \n%s', repr(scipy.linalg.eig(self.A_continuous)))
- glog.debug('schur(A): \n%s', repr(scipy.linalg.schur(self.A_continuous)))
- glog.debug('A_dt(A): \n%s', repr(self.A))
+ # The states are [position, unfiltered_velocity, velocity, torque_error]
+ self.A, self.B = self.ContinuousToDiscrete(self.A_continuous,
+ self.B_continuous, self.dt)
- q_pos = 0.01
- q_vel = 5.0
- q_velfilt = 1.5
- q_voltage = 2.0
- self.Q_continuous = numpy.matrix([[(q_pos ** 2.0), 0.0, 0.0, 0.0],
- [0.0, (q_vel ** 2.0), 0.0, 0.0],
- [0.0, 0.0, (q_velfilt ** 2.0), 0.0],
- [0.0, 0.0, 0.0, (q_voltage ** 2.0)]])
+ glog.debug('A: \n%s', repr(self.A_continuous))
+ glog.debug('eig(A): \n%s', repr(scipy.linalg.eig(self.A_continuous)))
+ glog.debug('schur(A): \n%s', repr(
+ scipy.linalg.schur(self.A_continuous)))
+ glog.debug('A_dt(A): \n%s', repr(self.A))
- r_pos = 0.0003
- self.R_continuous = numpy.matrix([[(r_pos ** 2.0)]])
+ q_pos = 0.01
+ q_vel = 5.0
+ q_velfilt = 1.5
+ q_voltage = 2.0
+ self.Q_continuous = numpy.matrix([[(q_pos**2.0), 0.0, 0.0, 0.0],
+ [0.0, (q_vel**2.0), 0.0, 0.0],
+ [0.0, 0.0, (q_velfilt**2.0), 0.0],
+ [0.0, 0.0, 0.0, (q_voltage**2.0)]])
- _, _, self.Q, self.R = controls.kalmd(
- A_continuous=self.A_continuous, B_continuous=self.B_continuous,
- Q_continuous=self.Q_continuous, R_continuous=self.R_continuous,
- dt=self.dt)
+ r_pos = 0.0003
+ self.R_continuous = numpy.matrix([[(r_pos**2.0)]])
- self.KalmanGain, self.P_steady_state = controls.kalman(
- A=self.A, B=self.B, C=self.C, Q=self.Q, R=self.R)
- self.L = self.A * self.KalmanGain
+ _, _, self.Q, self.R = controls.kalmd(
+ A_continuous=self.A_continuous,
+ B_continuous=self.B_continuous,
+ Q_continuous=self.Q_continuous,
+ R_continuous=self.R_continuous,
+ dt=self.dt)
- self.K_unaugmented = self.K
- self.K = numpy.matrix(numpy.zeros((1, 4)))
- self.K[0, 0:3] = self.K_unaugmented
- self.K[0, 3] = 1
- self.Kff_unaugmented = self.Kff
- self.Kff = numpy.matrix(numpy.zeros((1, 4)))
- self.Kff[0, 0:3] = self.Kff_unaugmented
+ self.KalmanGain, self.P_steady_state = controls.kalman(
+ A=self.A, B=self.B, C=self.C, Q=self.Q, R=self.R)
+ self.L = self.A * self.KalmanGain
- self.InitializeState()
+ self.K_unaugmented = self.K
+ self.K = numpy.matrix(numpy.zeros((1, 4)))
+ self.K[0, 0:3] = self.K_unaugmented
+ self.K[0, 3] = 1
+ self.Kff_unaugmented = self.Kff
+ self.Kff = numpy.matrix(numpy.zeros((1, 4)))
+ self.Kff[0, 0:3] = self.Kff_unaugmented
+
+ self.InitializeState()
class ScenarioPlotter(object):
- def __init__(self):
- # Various lists for graphing things.
- self.t = []
- self.x = []
- self.v = []
- self.a = []
- self.x_hat = []
- self.u = []
- self.offset = []
- self.diff = []
- def run_test(self, shooter, goal, iterations=200, controller_shooter=None,
- observer_shooter=None, hybrid_obs = False):
- """Runs the shooter plant with an initial condition and goal.
+ def __init__(self):
+ # Various lists for graphing things.
+ self.t = []
+ self.x = []
+ self.v = []
+ self.a = []
+ self.x_hat = []
+ self.u = []
+ self.offset = []
+ self.diff = []
- Args:
- shooter: Shooter object to use.
- goal: goal state.
- iterations: Number of timesteps to run the model for.
- controller_shooter: Shooter object to get K from, or None if we should
- use shooter.
- observer_shooter: Shooter object to use for the observer, or None if we
- should use the actual state.
- """
+ def run_test(self,
+ shooter,
+ goal,
+ iterations=200,
+ controller_shooter=None,
+ observer_shooter=None,
+ hybrid_obs=False):
+ """Runs the shooter plant with an initial condition and goal.
- if controller_shooter is None:
- controller_shooter = shooter
+ Args:
+ shooter: Shooter object to use.
+ goal: goal state.
+ iterations: Number of timesteps to run the model for.
+ controller_shooter: Shooter object to get K from, or None if we should
+ use shooter.
+ observer_shooter: Shooter object to use for the observer, or None if we
+ should use the actual state.
+ """
- vbat = 12.0
+ if controller_shooter is None:
+ controller_shooter = shooter
- if self.t:
- initial_t = self.t[-1] + shooter.dt
- else:
- initial_t = 0
+ vbat = 12.0
- last_U = numpy.matrix([[0.0]])
- for i in xrange(iterations):
- X_hat = shooter.X
-
- if observer_shooter is not None:
- X_hat = observer_shooter.X_hat
- self.x_hat.append(observer_shooter.X_hat[2, 0])
-
- ff_U = controller_shooter.Kff * (goal - observer_shooter.A * goal)
-
- U = controller_shooter.K * (goal - X_hat) + ff_U
- U[0, 0] = numpy.clip(U[0, 0], -vbat, vbat)
- self.x.append(shooter.X[0, 0])
-
- self.diff.append(shooter.X[2, 0] - observer_shooter.X_hat[2, 0])
-
- if self.v:
- last_v = self.v[-1]
- else:
- last_v = 0
-
- self.v.append(shooter.X[2, 0])
- self.a.append((self.v[-1] - last_v) / shooter.dt)
-
- if observer_shooter is not None:
- if i != 0:
- observer_shooter.Y = shooter.Y
- observer_shooter.CorrectObserver(U)
- self.offset.append(observer_shooter.X_hat[3, 0])
-
- applied_U = last_U.copy()
- if i > 60:
- applied_U += 2
- shooter.Update(applied_U)
-
- if observer_shooter is not None:
- if hybrid_obs:
- observer_shooter.PredictHybridObserver(last_U, shooter.dt)
+ if self.t:
+ initial_t = self.t[-1] + shooter.dt
else:
- observer_shooter.PredictObserver(last_U)
- last_U = U.copy()
+ initial_t = 0
+ last_U = numpy.matrix([[0.0]])
+ for i in xrange(iterations):
+ X_hat = shooter.X
- self.t.append(initial_t + i * shooter.dt)
- self.u.append(U[0, 0])
+ if observer_shooter is not None:
+ X_hat = observer_shooter.X_hat
+ self.x_hat.append(observer_shooter.X_hat[2, 0])
- def Plot(self):
- pylab.figure()
- pylab.subplot(3, 1, 1)
- pylab.plot(self.t, self.v, label='x')
- pylab.plot(self.t, self.x_hat, label='x_hat')
- pylab.legend()
+ ff_U = controller_shooter.Kff * (goal - observer_shooter.A * goal)
- pylab.subplot(3, 1, 2)
- pylab.plot(self.t, self.u, label='u')
- pylab.plot(self.t, self.offset, label='voltage_offset')
- pylab.legend()
+ U = controller_shooter.K * (goal - X_hat) + ff_U
+ U[0, 0] = numpy.clip(U[0, 0], -vbat, vbat)
+ self.x.append(shooter.X[0, 0])
- pylab.subplot(3, 1, 3)
- pylab.plot(self.t, self.a, label='a')
- pylab.legend()
+ self.diff.append(shooter.X[2, 0] - observer_shooter.X_hat[2, 0])
- pylab.draw()
+ if self.v:
+ last_v = self.v[-1]
+ else:
+ last_v = 0
+
+ self.v.append(shooter.X[2, 0])
+ self.a.append((self.v[-1] - last_v) / shooter.dt)
+
+ if observer_shooter is not None:
+ if i != 0:
+ observer_shooter.Y = shooter.Y
+ observer_shooter.CorrectObserver(U)
+ self.offset.append(observer_shooter.X_hat[3, 0])
+
+ applied_U = last_U.copy()
+ if i > 60:
+ applied_U += 2
+ shooter.Update(applied_U)
+
+ if observer_shooter is not None:
+ if hybrid_obs:
+ observer_shooter.PredictHybridObserver(last_U, shooter.dt)
+ else:
+ observer_shooter.PredictObserver(last_U)
+ last_U = U.copy()
+
+ self.t.append(initial_t + i * shooter.dt)
+ self.u.append(U[0, 0])
+
+ def Plot(self):
+ pylab.figure()
+ pylab.subplot(3, 1, 1)
+ pylab.plot(self.t, self.v, label='x')
+ pylab.plot(self.t, self.x_hat, label='x_hat')
+ pylab.legend()
+
+ pylab.subplot(3, 1, 2)
+ pylab.plot(self.t, self.u, label='u')
+ pylab.plot(self.t, self.offset, label='voltage_offset')
+ pylab.legend()
+
+ pylab.subplot(3, 1, 3)
+ pylab.plot(self.t, self.a, label='a')
+ pylab.legend()
+
+ pylab.draw()
def main(argv):
- scenario_plotter = ScenarioPlotter()
+ scenario_plotter = ScenarioPlotter()
- if FLAGS.plot:
- iterations = 200
+ if FLAGS.plot:
+ iterations = 200
- initial_X = numpy.matrix([[0.0], [0.0], [0.0]])
- R = numpy.matrix([[0.0], [100.0], [100.0], [0.0]])
+ initial_X = numpy.matrix([[0.0], [0.0], [0.0]])
+ R = numpy.matrix([[0.0], [100.0], [100.0], [0.0]])
- scenario_plotter_int = ScenarioPlotter()
+ scenario_plotter_int = ScenarioPlotter()
- shooter = Shooter()
- shooter_controller = IntegralShooter()
- observer_shooter_hybrid = IntegralShooter()
+ shooter = Shooter()
+ shooter_controller = IntegralShooter()
+ observer_shooter_hybrid = IntegralShooter()
- scenario_plotter_int.run_test(shooter, goal=R, controller_shooter=shooter_controller,
- observer_shooter=observer_shooter_hybrid, iterations=iterations,
- hybrid_obs = True)
+ scenario_plotter_int.run_test(
+ shooter,
+ goal=R,
+ controller_shooter=shooter_controller,
+ observer_shooter=observer_shooter_hybrid,
+ iterations=iterations,
+ hybrid_obs=True)
- scenario_plotter_int.Plot()
+ scenario_plotter_int.Plot()
- pylab.show()
+ pylab.show()
- if len(argv) != 5:
- glog.fatal('Expected .h file name and .cc file name')
- else:
- namespaces = ['y2017', 'control_loops', 'superstructure', 'shooter']
- shooter = Shooter('Shooter')
- loop_writer = control_loop.ControlLoopWriter('Shooter', [shooter],
- namespaces=namespaces)
- loop_writer.AddConstant(control_loop.Constant(
- 'kFreeSpeed', '%f', shooter.free_speed))
- loop_writer.AddConstant(control_loop.Constant(
- 'kOutputRatio', '%f', shooter.G))
- loop_writer.Write(argv[1], argv[2])
+ if len(argv) != 5:
+ glog.fatal('Expected .h file name and .cc file name')
+ else:
+ namespaces = ['y2017', 'control_loops', 'superstructure', 'shooter']
+ shooter = Shooter('Shooter')
+ loop_writer = control_loop.ControlLoopWriter(
+ 'Shooter', [shooter], namespaces=namespaces)
+ loop_writer.AddConstant(
+ control_loop.Constant('kFreeSpeed', '%f', shooter.free_speed))
+ loop_writer.AddConstant(
+ control_loop.Constant('kOutputRatio', '%f', shooter.G))
+ loop_writer.Write(argv[1], argv[2])
- integral_shooter = IntegralShooter('IntegralShooter')
- integral_loop_writer = control_loop.ControlLoopWriter(
- 'IntegralShooter', [integral_shooter], namespaces=namespaces,
- plant_type='StateFeedbackHybridPlant',
- observer_type='HybridKalman')
- integral_loop_writer.Write(argv[3], argv[4])
+ integral_shooter = IntegralShooter('IntegralShooter')
+ integral_loop_writer = control_loop.ControlLoopWriter(
+ 'IntegralShooter', [integral_shooter],
+ namespaces=namespaces,
+ plant_type='StateFeedbackHybridPlant',
+ observer_type='HybridKalman')
+ integral_loop_writer.Write(argv[3], argv[4])
if __name__ == '__main__':
- argv = FLAGS(sys.argv)
- glog.init()
- sys.exit(main(argv))
+ argv = FLAGS(sys.argv)
+ glog.init()
+ sys.exit(main(argv))
diff --git a/y2018/control_loops/python/intake.py b/y2018/control_loops/python/intake.py
index 471f246..b76016a 100755
--- a/y2018/control_loops/python/intake.py
+++ b/y2018/control_loops/python/intake.py
@@ -17,6 +17,7 @@
class Intake(control_loop.ControlLoop):
+
def __init__(self, name="Intake"):
super(Intake, self).__init__(name)
self.motor = control_loop.BAG()
@@ -67,12 +68,11 @@
self.A_continuous = numpy.matrix(
[[0.0, 1.0, 0.0, 0.0],
- [(-self.Ks / self.Jo), (-self.b/self.Jo),
- (self.Ks / self.Jo), ( self.b/self.Jo)],
- [0.0, 0.0, 0.0, 1.0],
- [( self.Ks / self.Je), ( self.b/self.Je),
- (-self.Ks / self.Je), (-self.b/self.Je)
- -self.Kt / (self.Je * self.resistance * self.Kv * self.G * self.G)]])
+ [(-self.Ks / self.Jo), (-self.b / self.Jo), (self.Ks / self.Jo),
+ (self.b / self.Jo)], [0.0, 0.0, 0.0, 1.0],
+ [(self.Ks / self.Je), (self.b / self.Je), (-self.Ks / self.Je),
+ (-self.b / self.Je) - self.Kt /
+ (self.Je * self.resistance * self.Kv * self.G * self.G)]])
# Start with the unmodified input
self.B_continuous = numpy.matrix(
@@ -99,8 +99,8 @@
q_pos = 0.05
q_vel = 2.65
self.Q = numpy.matrix(
- numpy.diag([(q_pos**2.0), (q_vel**2.0), (q_pos**2.0), (q_vel
- **2.0)]))
+ numpy.diag([(q_pos**2.0), (q_vel**2.0), (q_pos**2.0),
+ (q_vel**2.0)]))
r_nm = 0.025
self.R = numpy.matrix(numpy.diag([(r_nm**2.0), (r_nm**2.0)]))
@@ -117,6 +117,7 @@
class DelayedIntake(Intake):
+
def __init__(self, name="DelayedIntake"):
super(DelayedIntake, self).__init__(name=name)
@@ -137,12 +138,11 @@
# Coordinate transform fom absolute angles to relative angles.
# [output_position, output_velocity, spring_angle, spring_velocity, voltage]
- abs_to_rel = numpy.matrix(
- [[1.0, 0.0, 0.0, 0.0, 0.0],
- [0.0, 1.0, 0.0, 0.0, 0.0],
- [1.0, 0.0, -1.0, 0.0, 0.0],
- [0.0, 1.0, 0.0, -1.0, 0.0],
- [0.0, 0.0, 0.0, 0.0, 1.0]])
+ abs_to_rel = numpy.matrix([[1.0, 0.0, 0.0, 0.0, 0.0],
+ [0.0, 1.0, 0.0, 0.0, 0.0],
+ [1.0, 0.0, -1.0, 0.0, 0.0],
+ [0.0, 1.0, 0.0, -1.0, 0.0],
+ [0.0, 0.0, 0.0, 0.0, 1.0]])
# and back again.
rel_to_abs = numpy.matrix(numpy.linalg.inv(abs_to_rel))
@@ -185,14 +185,14 @@
self.A_transformed, self.B_transformed, self.Q_lqr, self.R)
# Write the controller back out in the absolute coordinate system.
- self.K = numpy.hstack((numpy.matrix([[0.0]]),
- self.K_transformed)) * abs_to_rel
+ self.K = numpy.hstack(
+ (numpy.matrix([[0.0]]), self.K_transformed)) * abs_to_rel
controllability = controls.ctrb(self.A_transformed, self.B_transformed)
glog.debug('ctrb: ' + repr(numpy.linalg.matrix_rank(controllability)))
- w, v = numpy.linalg.eig(
- self.A_transformed - self.B_transformed * self.K_transformed)
+ w, v = numpy.linalg.eig(self.A_transformed -
+ self.B_transformed * self.K_transformed)
glog.debug('Poles are %s, for %s', repr(w), self._name)
for i in range(len(w)):
@@ -222,6 +222,7 @@
class ScenarioPlotter(object):
+
def __init__(self):
# Various lists for graphing things.
self.t = []
@@ -295,7 +296,7 @@
[goal_velocity / (intake.G * intake.Kv)]])
U = controller_intake.K * (R - X_hat) + R[4, 0]
- U[0, 0] = numpy.clip(U[0, 0], -vbat, vbat) # * 0.0
+ U[0, 0] = numpy.clip(U[0, 0], -vbat, vbat) # * 0.0
self.x_output.append(intake.X[0, 0])
self.x_motor.append(intake.X[2, 0])