Switch to python3 and scipy from slycot
Turns out we need python3 matplotlib to make scipy work well enough to
place the poles correctly for our systems. Rather than do it piecemeal,
do it all at once.
This includes a python opencv upgrade too to support the new python, and
a matplotlib upgrade.
Change-Id: Ic7517b5ebbfdca9cc90ae6a61d86b474f2f21b29
diff --git a/frc971/analysis/BUILD b/frc971/analysis/BUILD
index 45e947b..2901463 100644
--- a/frc971/analysis/BUILD
+++ b/frc971/analysis/BUILD
@@ -6,22 +6,6 @@
load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library", "flatbuffer_ts_library")
load("//aos:config.bzl", "aos_config")
-py_binary(
- name = "plot_action",
- srcs = [
- "logentry.py",
- "logreader.py",
- "plot_action.py",
- "plotter.py",
- ],
- legacy_create_init = False,
- target_compatible_with = ["@platforms//os:linux"],
- deps = [
- ":python_init",
- "@matplotlib_repo//:matplotlib2.7",
- ],
-)
-
py_library(
name = "python_init",
srcs = ["__init__.py"],
@@ -41,7 +25,7 @@
"//aos/events:simulated_event_loop",
"//aos/events/logging:logger",
"@com_github_google_glog//:glog",
- "@python_repo//:python3.5_lib",
+ "@python_repo//:python3.7_lib",
],
)
diff --git a/frc971/analysis/logentry.py b/frc971/analysis/logentry.py
deleted file mode 100644
index 08aa469..0000000
--- a/frc971/analysis/logentry.py
+++ /dev/null
@@ -1,275 +0,0 @@
-#!/usr/bin/python
-
-import re
-
-"""
-A regular expression to match the envelope part of the log entry.
-Parsing of the JSON msg is handled elsewhere.
-"""
-LOG_RE = re.compile("""
- (.*?) # 1 name
- \((\d+)\) # 2 pid
- \((\d+)\) # 3 message_index
- :\s
- (\w+?) # 4 level
- \s+at\s+
- (\d+\.\d+)s # 5 time
- :\s
- ([A-Za-z0-9_./-]+) # 6 filename
- :\s
- (\d+) # 7 linenumber
- :\s
- (.*) # 8 msg
- """, re.VERBOSE)
-
-class LogEntry:
- """
- This class provides a way to parse log entries.
- The header portion of the log entry is parsed eagerly.
- The structured portion of a log entry is parsed on demand.
- """
-
- def __init__(self, line):
- """Populates a LogEntry from a line."""
- self.line = line
- m = LOG_RE.match(line)
- if m is None:
- print("LOG_RE failed on", line)
- return
- self.name = m.group(1)
- self.pid_index = int(m.group(2))
- self.msg_index = int(m.group(3))
- self.level = m.group(4)
- self.time = float(m.group(5))
- self.filename = m.group(6)
- self.linenumber = m.group(7)
- self.msg = m.group(8)
- self.struct_name = None
-
- def __str__(self):
- """Formats the data cleanly."""
- return '%s(%d)(%d): %s at %fs: %s: %d: %s' % (
- self.name, self.pid, self.msg_index, self.level, self.time, self.filename, self.linenumber, self.msg)
-
- def ParseStruct(self):
- """Parses the message as a structure.
-
- Returns:
- struct_name, struct_type, json dict.
- """
- if self.struct_name:
- # We've already parsed the structural part. Return the cached result
- return (self.struct_name, self.struct_type, self.struct_json)
-
- struct_name_index = self.msg.find(':')
- struct_name = self.msg[0:struct_name_index]
-
- struct_body = self.msg[struct_name_index+2:]
- tokens = []
- this_token = ''
- # For the various deliminators, append what we have found so far to the
- # list and the token.
- for char in struct_body:
- if char == '{':
- if this_token:
- tokens.append(this_token)
- this_token = ''
- tokens.append('{')
- elif char == '}':
- if this_token:
- tokens.append(this_token)
- this_token = ''
- tokens.append('}')
- elif char == '[':
- if this_token:
- tokens.append(this_token)
- this_token = ''
- tokens.append('[')
- elif char == ']':
- if this_token:
- tokens.append(this_token)
- this_token = ''
- tokens.append(']')
- elif char == ':':
- if this_token:
- tokens.append(this_token)
- this_token = ''
- tokens.append(':')
- elif char == ',':
- if this_token:
- tokens.append(this_token)
- this_token = ''
- tokens.append(',')
- elif char == ' ':
- if this_token:
- tokens.append(this_token)
- this_token = ''
- else:
- this_token += char
- if this_token:
- tokens.append(this_token)
-
- struct_type = tokens[0]
- json = dict()
- # Now that we have tokens, parse them.
- self.JsonizeTokens(json, tokens, 1)
-
- # Cache the result to avoid having to reparse.
- self.struct_name = struct_name
- self.struct_type = struct_type
- self.struct_json = json
-
- return (struct_name, struct_type, json)
-
- def JsonizeTokens(self, json, tokens, token_index):
- """Creates a json-like dictionary from the provided tokens.
-
- Args:
- json: dict, The dict to stick the elements in.
- tokens: list of strings, The list with all the tokens in it.
- token_index: int, Where to start in the token list.
-
- Returns:
- int, The last token used.
- """
- # Check that the message starts with a {
- if tokens[token_index] != '{':
- print(tokens)
- print('Expected { at beginning, found', tokens[token_index])
- return None
-
- # Eat the {
- token_index += 1
-
- # States and state variable for parsing elements.
- STATE_INIT = 'init'
- STATE_HAS_NAME = 'name'
- STATE_HAS_COLON = 'colon'
- STATE_EXPECTING_SUBMSG = 'submsg'
- STATE_EXPECTING_COMMA = 'comma'
- parser_state = STATE_INIT
-
- while token_index < len(tokens):
- if tokens[token_index] == '}':
- # Finish if there is a }
- return token_index + 1
- elif tokens[token_index] == '{':
- if parser_state != STATE_EXPECTING_SUBMSG:
- print(tokens)
- print(parser_state)
- print('Bad input, was not expecting {')
- return None
- # Found a submessage, parse it.
- sub_json = dict()
- token_index = self.JsonizeTokens(sub_json, tokens, token_index)
- json[token_name] = sub_json
- parser_state = STATE_EXPECTING_COMMA
- else:
- if parser_state == STATE_INIT:
- # This token is the name.
- token_name = tokens[token_index]
- parser_state = STATE_HAS_NAME
- elif parser_state == STATE_HAS_NAME:
- if tokens[token_index] != ':':
- print(tokens)
- print(parser_state)
- print('Bad input, found', tokens[token_index], 'expected :')
- return None
- # After a name, comes a :
- parser_state = STATE_HAS_COLON
- elif parser_state == STATE_HAS_COLON:
- # After the colon, figure out what is next.
- if tokens[token_index] == '[':
- # Found a sub-array!
- sub_array = []
- token_index = self.__JsonizeTokenArray(sub_array, tokens, token_index)
- json[token_name] = sub_array
- parser_state = STATE_EXPECTING_COMMA
- elif tokens[token_index + 1] == '{':
- # Found a sub-message, trigger parsing it.
- parser_state = STATE_EXPECTING_SUBMSG
- else:
- # This is just an element, move on.
- json[token_name] = tokens[token_index]
- parser_state = STATE_EXPECTING_COMMA
- elif parser_state == STATE_EXPECTING_COMMA:
- # Complain if there isn't a comma here.
- if tokens[token_index] != ',':
- print(tokens)
- print(parser_state)
- print('Bad input, found', tokens[token_index], 'expected ,')
- return None
- parser_state = STATE_INIT
- else:
- print('Bad parser state')
- return None
- token_index += 1
-
- print('Unexpected end')
- return None
-
- def __JsonizeTokenArray(self, sub_array, tokens, token_index):
- """Parses an array from the provided tokens.
-
- Args:
- sub_array: list, The list to stick the elements in.
- tokens: list of strings, The list with all the tokens in it.
- token_index: int, Where to start in the token list.
-
- Returns:
- int, The last token used.
- """
- # Make sure the data starts with a '['
- if tokens[token_index] != '[':
- print(tokens)
- print('Expected [ at beginning, found', tokens[token_index + 1])
- return None
-
- # Eat the '['
- token_index += 1
-
- # Loop through the tokens.
- while token_index < len(tokens):
- if tokens[token_index + 1] == ',':
- # Next item is a comma, so we should just add the element.
- sub_array.append(tokens[token_index])
- token_index += 2
- elif tokens[token_index + 1] == ']':
- # Next item is a ']', so we should just add the element and finish.
- sub_array.append(tokens[token_index])
- token_index += 1
- return token_index
- else:
- # Otherwise, it must be a sub-message.
- sub_json = dict()
- token_index = self.JsonizeTokens(sub_json, tokens, token_index + 1)
- sub_array.append(sub_json)
- if tokens[token_index] == ',':
- # Handle there either being another data element.
- token_index += 1
- elif tokens[token_index] == ']':
- # Handle the end of the array.
- return token_index
- else:
- print('Unexpected ', tokens[token_index])
- return None
-
- print('Unexpected end')
- return None
-
-
-if __name__ == '__main__':
- def ParseLine(line):
- return LogEntry(line)
-
- print('motor_writer(2240)(07421): DEBUG at 0000000819.99620s: ../../frc971/output/motor_writer.cc: 105: sending: .aos.controls.OutputCheck{pwm_value:221, pulse_length:2.233333}')
- line = ParseLine('motor_writer(2240)(07421): DEBUG at 0000000819.99620s: ../../frc971/output/motor_writer.cc: 105: sending: .aos.controls.OutputCheck{pwm_value:221, pulse_length:2.233333}')
- if '.aos.controls.OutputCheck' in line.msg:
- print(line)
- print(line.ParseStruct())
-
- line = ParseLine('claw(2263)(19404): DEBUG at 0000000820.00000s: ../../aos/controls/control_loop-tmpl.h: 104: position: .frc971.control_loops.ClawGroup.Position{top:.frc971.control_loops.HalfClawPosition{position:1.672153, front:.frc971.HallEffectStruct{current:f, posedge_count:0, negedge_count:52}, calibration:.frc971.HallEffectStruct{current:f, posedge_count:6, negedge_count:13}, back:.frc971.HallEffectStruct{current:f, posedge_count:0, negedge_count:62}, posedge_value:0.642681, negedge_value:0.922207}, bottom:.frc971.control_loops.HalfClawPosition{position:1.353539, front:.frc971.HallEffectStruct{current:f, posedge_count:2, negedge_count:150}, calibration:.frc971.HallEffectStruct{current:f, posedge_count:8, negedge_count:18}, back:.frc971.HallEffectStruct{current:f, posedge_count:0, negedge_count:6}, posedge_value:0.434514, negedge_value:0.759491}}')
- print(line.ParseStruct())
-
- line = ParseLine('joystick_proxy(2255)(39560): DEBUG at 0000000820.00730s: ../../aos/prime/input/joystick_input.cc: 61: sending: .aos.RobotState{joysticks:[.aos.Joystick{buttons:0, axis:[0.000000, 1.000000, 1.000000, 0.000000]}, .aos.Joystick{buttons:0, axis:[-0.401575, 1.000000, -1.007874, 0.000000]}, .aos.Joystick{buttons:0, axis:[0.007874, 0.000000, 1.000000, -1.007874]}, .aos.Joystick{buttons:0, axis:[0.000000, 0.000000, 0.000000, 0.000000]}], test_mode:f, fms_attached:f, enabled:T, autonomous:f, team_id:971, fake:f}')
- print(line.ParseStruct())
diff --git a/frc971/analysis/logreader.py b/frc971/analysis/logreader.py
deleted file mode 100644
index 7449569..0000000
--- a/frc971/analysis/logreader.py
+++ /dev/null
@@ -1,103 +0,0 @@
-#!/usr/bin/python
-
-import collections
-from frc971.analysis.logentry import LogEntry
-
-class Dataset(object):
- def __init__(self):
- self.time = []
- self.data = []
-
- def Add(self, time, data):
- self.time.append(time)
- self.data.append(data)
-
-class CollectingLogReader(object):
- """
- Reads log files and collected requested data.
- """
- def __init__(self):
- self.signal = collections.OrderedDict()
-
- def Add(self, binary, struct_instance_name, *data_search_path):
- """
- Specifies a specific piece of data to collect
-
- Args:
- binary: str, The name of the executable that generated the log.
- struct_instance_name: str, The name of the struct instance whose data
- contents should be collected.
- data_search_path: [str], The path into the struct of the exact piece of
- data to collect.
-
- Returns:
- None
- """
- self.signal[(binary, struct_instance_name, data_search_path)] = Dataset()
-
- def HandleFile(self, f):
- """
- Parses the specified log file.
-
- Args:
- f: str, The filename of the log whose data to parse.
-
- Returns:
- None
- """
- with open(f, 'r') as fd:
- for line in fd:
- try:
- self.HandleLine(line)
- except Exception as ex:
- # It's common for the last line of the file to be malformed.
- print("Ignoring malformed log entry: ", line, ex)
-
- def HandleLine(self, line):
- """
- Parses a line from a log file and adds the data to the plot data.
-
- Args:
- line: str, The line from the log file to parse
-
- Returns:
- None
- """
- pline = LogEntry(line)
-
- for key in self.signal:
- value = self.signal[key]
- binary = key[0]
- struct_instance_name = key[1]
- data_search_path = key[2]
- boolean_multiplier = False
- multiplier = 1.0
-
- # If the plot definition line ends with a "-b X" where X is a number then
- # that number gets drawn when the value is True. Zero gets drawn when the
- # value is False.
- if len(data_search_path) >= 2 and data_search_path[-2] == '-b':
- multiplier = float(data_search_path[-1])
- boolean_multiplier = True
- data_search_path = data_search_path[:-2]
-
- if len(data_search_path) >= 2 and data_search_path[-2] == '-m':
- multiplier = float(data_search_path[-1])
- data_search_path = data_search_path[:-2]
-
- # Make sure that we're looking at the right binary structure instance.
- if binary == pline.name:
- if pline.msg.startswith(struct_instance_name + ': '):
- # Traverse the structure as specified in `data_search_path`.
- # This lets the user access very deeply nested structures.
- _, _, data = pline.ParseStruct()
- for path in data_search_path:
- data = data[path]
-
- if boolean_multiplier:
- if data == 'T':
- value.Add(pline.time, multiplier)
- else:
- value.Add(pline.time, 0)
- else:
- value.Add(pline.time, float(data) * multiplier)
diff --git a/frc971/analysis/plot_action.py b/frc971/analysis/plot_action.py
deleted file mode 100755
index d157065..0000000
--- a/frc971/analysis/plot_action.py
+++ /dev/null
@@ -1,97 +0,0 @@
-#!/usr/bin/python
-
-import sys
-import numpy
-from frc971.analysis.plotter import Plotter
-import argparse
-
-def ReadPlotDefinitions(filename):
- """
- Read a file with plotting definitions.
-
- A plotting definition is a single line that defines what data to search for
- in order to plot it. The following in a file would duplicate the default
- behaviour:
-
- fridge goal height
- fridge goal angle
- fridge goal velocity
- fridge goal angular_velocity
- fridge output left_arm
- fridge output right_arm
- fridge output left_elevator
- fridge output right_elevator
-
- Lines are ignored if they start with a hash mark (i.e. '#').
-
- Lines that end with a "-b X" where X is a number then it designates that line
- as plotting a boolean value. X is the value plotted when the boolean is true.
- When the boolean is false then the values is plotted as zero. For example,
- the following boolean value is drawn to toggle between 2.0 and 0 when the
- boolean is True and False, respectively:
-
- fridge status zeroed -b 2.0
-
- Args:
- filename: The name of the file to read the definitions from.
-
- Returns:
- [[str]]: The definitions in the specified file.
- """
- defs = []
- with open(filename) as fd:
- for line in fd:
- raw_defs = line.split()
-
- # Only add to the list of definitions if the line's not empty and it
- # doesn't start with a hash.
- if raw_defs and not raw_defs[0].startswith('#'):
- defs.append(raw_defs)
-
- return defs
-
-
-def maybeint(x):
- try:
- return int(x)
- except ValueError:
- return x
-
-
-def main():
- # Parse all command line arguments.
- arg_parser = argparse.ArgumentParser(description='Log Plotter')
- arg_parser.add_argument('log_file', metavar='LOG_FILE', type=str, \
- help='The file from which to read logs and plot.')
- arg_parser.add_argument('--plot-defs', '-p', action='store', type=str, \
- help='Read the items to plot from this file.')
- arg_parser.add_argument('--no-binary', '-n', action='store_true', \
- help='Don\'t print the binary name in the legend.')
-
- args = arg_parser.parse_args(sys.argv[1:])
-
- p = Plotter()
-
- # If the user defines the list of data to plot in a file, read it from there.
- if args.plot_defs:
- defs = ReadPlotDefinitions(args.plot_defs)
- for definition in defs:
- mapped_definitions = map(maybeint, definition[2:])
- p.Add(definition[0], definition[1], *mapped_definitions)
-
- # Otherwise use a pre-defined set of data to plot.
- else:
- p.Add('fridge', 'goal', 'height')
- p.Add('fridge', 'goal', 'angle')
- p.Add('fridge', 'goal', 'velocity')
- p.Add('fridge', 'goal', 'angular_velocity')
-
- p.Add('fridge', 'output', 'left_arm')
- p.Add('fridge', 'output', 'right_arm')
- p.Add('fridge', 'output', 'left_elevator')
- p.Add('fridge', 'output', 'right_elevator')
-
- p.PlotFile(args.log_file, args.no_binary)
-
-if __name__ == '__main__':
- main()
diff --git a/frc971/analysis/plotter.py b/frc971/analysis/plotter.py
deleted file mode 100755
index 6d23587..0000000
--- a/frc971/analysis/plotter.py
+++ /dev/null
@@ -1,50 +0,0 @@
-#!/usr/bin/python
-
-from frc971.analysis.logreader import CollectingLogReader
-import matplotlib
-from matplotlib import pylab
-from matplotlib.font_manager import FontProperties
-
-class Plotter(CollectingLogReader):
- """
- A CollectingLogReader that plots collected data.
- """
-
- def PlotFile(self, f, no_binary_in_legend=False):
- """
- Parses and plots all the data.
-
- Args:
- f: str, The filename of the log whose data to parse and plot.
-
- Returns:
- None
- """
- self.HandleFile(f)
- self.Plot(no_binary_in_legend)
-
- def Plot(self, no_binary_in_legend):
- """
- Plots all the data after it's parsed.
-
- This should only be called after `HandleFile` has been called so that there
- is actual data to plot.
- """
- for key in self.signal:
- value = self.signal[key]
-
- # Create a legend label using the binary name (optional), the structure
- # name and the data search path.
- label = key[1] + '.' + '.'.join(str(x) for x in key[2])
- if not no_binary_in_legend:
- label = key[0] + ' ' + label
-
- pylab.plot(value.time, value.data, label=label)
-
- # Set legend font size to small and move it to the top center.
- fontP = FontProperties()
- fontP.set_size('small')
- pylab.legend(bbox_to_anchor=(0.2, 1.10), prop=fontP)
-
- pylab.show()
-
diff --git a/frc971/control_loops/drivetrain/wheel_nonlinearity_plot.py b/frc971/control_loops/drivetrain/wheel_nonlinearity_plot.py
index 57831c5..9caa9dc 100755
--- a/frc971/control_loops/drivetrain/wheel_nonlinearity_plot.py
+++ b/frc971/control_loops/drivetrain/wheel_nonlinearity_plot.py
@@ -1,4 +1,4 @@
-#!/usr/bin/python
+#!/usr/bin/python3
#
# This is a quick script to show the effect of the wheel nonlinearity term on
# turning rate
diff --git a/frc971/control_loops/python/BUILD b/frc971/control_loops/python/BUILD
index 7851397..880624e 100644
--- a/frc971/control_loops/python/BUILD
+++ b/frc971/control_loops/python/BUILD
@@ -11,7 +11,7 @@
"//external:python-gflags",
"//external:python-glog",
"//frc971/control_loops/python:controls",
- "@matplotlib_repo//:matplotlib2.7",
+ "@matplotlib_repo//:matplotlib3",
],
)
@@ -32,7 +32,6 @@
deps = [
":python_init",
"//external:python-glog",
- "@slycot_repo//:slycot",
],
)
@@ -59,7 +58,7 @@
deps = [
":controls",
":python_init",
- "@matplotlib_repo//:matplotlib2.7",
+ "@matplotlib_repo//:matplotlib3",
],
)
@@ -72,7 +71,7 @@
deps = [
":controls",
":python_init",
- "@matplotlib_repo//:matplotlib2.7",
+ "@matplotlib_repo//:matplotlib3",
],
)
@@ -136,7 +135,7 @@
"//external:python-glog",
"//frc971/control_loops/python:controls",
"//y2016/control_loops/python:polydrivetrain_lib",
- "@matplotlib_repo//:matplotlib2.7",
+ "@matplotlib_repo//:matplotlib3",
],
)
@@ -149,7 +148,7 @@
":controls",
"//aos/util:py_trapezoid_profile",
"//frc971/control_loops:python_init",
- "@matplotlib_repo//:matplotlib2.7",
+ "@matplotlib_repo//:matplotlib3",
],
)
@@ -162,7 +161,7 @@
":controls",
"//aos/util:py_trapezoid_profile",
"//frc971/control_loops:python_init",
- "@matplotlib_repo//:matplotlib2.7",
+ "@matplotlib_repo//:matplotlib3",
],
)
@@ -184,7 +183,7 @@
":basic_window",
":libspline",
":python_init",
- "@matplotlib_repo//:matplotlib2.7",
+ "@matplotlib_repo//:matplotlib3",
"@python_gtk",
],
)
diff --git a/frc971/control_loops/python/angular_system.py b/frc971/control_loops/python/angular_system.py
index 397e6ee..34f4307 100755
--- a/frc971/control_loops/python/angular_system.py
+++ b/frc971/control_loops/python/angular_system.py
@@ -1,4 +1,4 @@
-#!/usr/bin/python
+#!/usr/bin/python3
from aos.util.trapezoid_profile import TrapezoidProfile
from frc971.control_loops.python import control_loop
diff --git a/frc971/control_loops/python/cim.py b/frc971/control_loops/python/cim.py
index 8c13eb0..f8a6e9a 100644
--- a/frc971/control_loops/python/cim.py
+++ b/frc971/control_loops/python/cim.py
@@ -1,4 +1,4 @@
-#!/usr/bin/python
+#!/usr/bin/python3
from frc971.control_loops.python import control_loop
import numpy
diff --git a/frc971/control_loops/python/control_loop.py b/frc971/control_loops/python/control_loop.py
index c3dc1a2..3270f78 100644
--- a/frc971/control_loops/python/control_loop.py
+++ b/frc971/control_loops/python/control_loop.py
@@ -357,7 +357,7 @@
for y in range(matrix.shape[1]):
write_type = repr(matrix[x, y])
if scalar_type == 'float':
- if '.' not in write_type:
+ if '.' not in write_type and 'e' not in write_type:
write_type += '.0'
write_type += 'f'
ans.append(
diff --git a/frc971/control_loops/python/controls.py b/frc971/control_loops/python/controls.py
index defdbd2..4fdf1e9 100644
--- a/frc971/control_loops/python/controls.py
+++ b/frc971/control_loops/python/controls.py
@@ -1,4 +1,4 @@
-#!/usr/bin/python
+#!/usr/bin/python3
"""
Control loop pole placement library.
@@ -10,22 +10,18 @@
__author__ = 'Austin Schuh (austin.linux@gmail.com)'
import numpy
-import slycot
import scipy.linalg
+import scipy.signal
import glog
class Error (Exception):
"""Base class for all control loop exceptions."""
-class PolePlacementError(Error):
- """Exception raised when pole placement fails."""
-
-
# TODO(aschuh): dplace should take a control system object.
# There should also exist a function to manipulate laplace expressions, and
# something to plot bode plots and all that.
-def dplace(A, B, poles, alpha=1e-6):
+def dplace(A, B, poles):
"""Set the poles of (A - BF) to poles.
Args:
@@ -34,55 +30,10 @@
poles: array(imaginary numbers), The poles to use. Complex conjugates poles
must be in pairs.
- Raises:
- ValueError: Arguments were the wrong shape or there were too many poles.
- PolePlacementError: Pole placement failed.
-
Returns:
numpy.matrix(m x n), K
"""
- # See http://www.icm.tu-bs.de/NICONET/doc/SB01BD.html for a description of the
- # fortran code that this is cleaning up the interface to.
- n = A.shape[0]
- if A.shape[1] != n:
- raise ValueError("A must be square")
- if B.shape[0] != n:
- raise ValueError("B must have the same number of states as A.")
- m = B.shape[1]
-
- num_poles = len(poles)
- if num_poles > n:
- raise ValueError("Trying to place more poles than states.")
-
- out = slycot.sb01bd(n=n,
- m=m,
- np=num_poles,
- alpha=alpha,
- A=A,
- B=B,
- w=numpy.array(poles),
- dico='D')
-
- A_z = numpy.matrix(out[0])
- num_too_small_eigenvalues = out[2]
- num_assigned_eigenvalues = out[3]
- num_uncontrollable_eigenvalues = out[4]
- K = numpy.matrix(-out[5])
- Z = numpy.matrix(out[6])
-
- if num_too_small_eigenvalues != 0:
- raise PolePlacementError("Number of eigenvalues that are too small "
- "and are therefore unmodified is %d." %
- num_too_small_eigenvalues)
- if num_assigned_eigenvalues != num_poles:
- raise PolePlacementError("Did not place all the eigenvalues that were "
- "requested. Only placed %d eigenvalues." %
- num_assigned_eigenvalues)
- if num_uncontrollable_eigenvalues != 0:
- raise PolePlacementError("Found %d uncontrollable eigenvlaues." %
- num_uncontrollable_eigenvalues)
-
- return K
+ return scipy.signal.place_poles(A=A, B=B, poles=numpy.array(poles)).gain_matrix
def c2d(A, B, dt):
"""Converts from continuous time state space representation to discrete time.
@@ -133,9 +84,7 @@
# P = (A.T * P * A) - (A.T * P * B * numpy.linalg.inv(R + B.T * P *B) * (A.T * P.T * B).T + Q
# 0.5 * X.T * P * X -> optimal cost to infinity
- P, rcond, w, S, T = slycot.sb02od(
- n=A.shape[0], m=B.shape[1], A=A, B=B, Q=Q, R=R, dico='D')
-
+ P = scipy.linalg.solve_discrete_are(a=A, b=B, q=Q, r=R)
F = numpy.linalg.inv(R + B.T * P * B) * B.T * P * A
if optimal_cost_function:
return F, P
@@ -164,9 +113,9 @@
controllability_rank, n)
# Compute the steady state covariance matrix.
- P_prior, rcond, w, S, T = slycot.sb02od(n=n, m=m, A=A.T, B=C.T, Q=Q, R=R, dico='D')
+ P_prior = scipy.linalg.solve_discrete_are(a=A.T, b=C.T, q=Q, r=R)
S = C * P_prior * C.T + R
- K = numpy.linalg.lstsq(S.T, (P_prior * C.T).T)[0].T
+ K = numpy.linalg.lstsq(S.T, (P_prior * C.T).T, rcond=None)[0].T
P = (I - K * C) * P_prior
return K, P
diff --git a/frc971/control_loops/python/down_estimator.py b/frc971/control_loops/python/down_estimator.py
index c4b8e7e..224fe03 100644
--- a/frc971/control_loops/python/down_estimator.py
+++ b/frc971/control_loops/python/down_estimator.py
@@ -1,4 +1,4 @@
-#!/usr/bin/python
+#!/usr/bin/python3
import math
import sys
diff --git a/frc971/control_loops/python/drivetrain.py b/frc971/control_loops/python/drivetrain.py
index a020276..b29e1e6 100644
--- a/frc971/control_loops/python/drivetrain.py
+++ b/frc971/control_loops/python/drivetrain.py
@@ -1,4 +1,4 @@
-#!/usr/bin/python
+#!/usr/bin/python3
from frc971.control_loops.python import control_loop
from frc971.control_loops.python import controls
diff --git a/frc971/control_loops/python/haptic_wheel.py b/frc971/control_loops/python/haptic_wheel.py
index 79c9c35..5e63df3 100755
--- a/frc971/control_loops/python/haptic_wheel.py
+++ b/frc971/control_loops/python/haptic_wheel.py
@@ -1,4 +1,4 @@
-#!/usr/bin/python
+#!/usr/bin/python3
from frc971.control_loops.python import control_loop
from frc971.control_loops.python import controls
diff --git a/frc971/control_loops/python/lib_spline_test.py b/frc971/control_loops/python/lib_spline_test.py
index b4efe72..a1e298a 100644
--- a/frc971/control_loops/python/lib_spline_test.py
+++ b/frc971/control_loops/python/lib_spline_test.py
@@ -1,4 +1,4 @@
-#!/usr/bin/python
+#!/usr/bin/python3
import math
import numpy as np
diff --git a/frc971/control_loops/python/libcdd.py b/frc971/control_loops/python/libcdd.py
index b46d908..72d5833 100644
--- a/frc971/control_loops/python/libcdd.py
+++ b/frc971/control_loops/python/libcdd.py
@@ -1,4 +1,4 @@
-#!/usr/bin/python
+#!/usr/bin/python3
"""Wrapper around libcdd, a polytope manipulation library."""
@@ -8,24 +8,16 @@
import os
import sys
-# Wrapper around PyFile_AsFile so that we can print out the error messages.
-# Set the arg type and return types of the function call.
-class FILE(ctypes.Structure):
- pass
-
-ctypes.pythonapi.PyFile_AsFile.argtypes = [ctypes.py_object]
-ctypes.pythonapi.PyFile_AsFile.restype = ctypes.POINTER(FILE)
-
# Load and init libcdd. libcdd is a C library that implements algorithm to
# manipulate half space and vertex representations of polytopes.
# Unfortunately, the library was compiled with C++ even though it has a lot of C
# code in it, so all the symbol names are mangled. Ug.
libcdd = None
for path in os.environ.get('PYTHONPATH').split(':'):
- try:
- libcdd = ctypes.cdll.LoadLibrary(os.path.join(path, 'third_party/cddlib/_cddlib.so'))
- except OSError, e:
- pass
+ try:
+ libcdd = ctypes.cdll.LoadLibrary(os.path.join(path, 'third_party/cddlib/_cddlib.so'))
+ except OSError:
+ pass
assert libcdd is not None, 'Failed to find _cddlib.so'
@@ -41,21 +33,21 @@
# Forward declaration for the polyhedra data structure.
class dd_polyhedradata(ctypes.Structure):
- pass
+ pass
# Definition of dd_matrixdata
class dd_matrixdata(ctypes.Structure):
- _fields_ = [
- ("rowsize", ctypes.c_long),
- ("linset", ctypes.POINTER(ctypes.c_ulong)),
- ("colsize", ctypes.c_long),
- ("representation", ctypes.c_int),
- ("numbtype", ctypes.c_int),
- ("matrix", ctypes.POINTER(ctypes.POINTER(mytype))),
- ("objective", ctypes.c_int),
- ("rowvec", ctypes.POINTER(mytype)),
- ]
+ _fields_ = [
+ ("rowsize", ctypes.c_long),
+ ("linset", ctypes.POINTER(ctypes.c_ulong)),
+ ("colsize", ctypes.c_long),
+ ("representation", ctypes.c_int),
+ ("numbtype", ctypes.c_int),
+ ("matrix", ctypes.POINTER(ctypes.POINTER(mytype))),
+ ("objective", ctypes.c_int),
+ ("rowvec", ctypes.POINTER(mytype)),
+ ]
# Define the input and output types for a bunch of libcdd functions.
libcdd.dd_CreateMatrix.restype = ctypes.POINTER(dd_matrixdata)
@@ -95,40 +87,40 @@
def dd_CreateMatrix(rows, cols):
- return libcdd.dd_CreateMatrix(ctypes.c_long(rows), ctypes.c_long(cols))
+ return libcdd.dd_CreateMatrix(ctypes.c_long(rows), ctypes.c_long(cols))
def dd_set_d(mytype_address, double_value):
- libcdd.ddd_set_d(mytype_address, ctypes.c_double(double_value))
+ libcdd.ddd_set_d(mytype_address, ctypes.c_double(double_value))
def dd_CopyGenerators(polyhedraptr):
- return libcdd.dd_CopyGenerators(polyhedraptr)
+ return libcdd.dd_CopyGenerators(polyhedraptr)
def dd_get_d(mytype_address):
- return libcdd.ddd_get_d(mytype_address)
+ return libcdd.ddd_get_d(mytype_address)
def dd_FreeMatrix(matrixptr):
- libcdd.dd_FreeMatrix(matrixptr)
+ libcdd.dd_FreeMatrix(matrixptr)
def dd_FreePolyhedra(polyhedraptr):
- libcdd.dd_FreePolyhedra(polyhedraptr)
+ libcdd.dd_FreePolyhedra(polyhedraptr)
def dd_DDMatrix2Poly(matrixptr):
- error = ctypes.c_int()
- polyhedraptr = libcdd.dd_DDMatrix2Poly(matrixptr, ctypes.byref(error))
+ error = ctypes.c_int()
+ polyhedraptr = libcdd.dd_DDMatrix2Poly(matrixptr, ctypes.byref(error))
- # Return None on error.
- # The error values are enums, so they aren't exposed.
- if error.value != DD_NO_ERRORS:
- # Dump out the errors to stderr
- libcdd.dd_WriteErrorMessages(
- ctypes.pythonapi.PyFile_AsFile(ctypes.py_object(sys.stdout)),
- error)
- dd_FreePolyhedra(polyhedraptr)
- return None
- return polyhedraptr
+ # Return None on error.
+ # The error values are enums, so they aren't exposed.
+ if error.value != DD_NO_ERRORS:
+ # TODO(austin): Dump out the errors to stderr
+ #libcdd.dd_WriteErrorMessages(
+ # ctypes.pythonapi.PyFile_AsFile(ctypes.py_object(sys.stdout)),
+ # error)
+ dd_FreePolyhedra(polyhedraptr)
+ return None
+ return polyhedraptr
diff --git a/frc971/control_loops/python/libspline.py b/frc971/control_loops/python/libspline.py
index 9799caa..24f1f51 100755
--- a/frc971/control_loops/python/libspline.py
+++ b/frc971/control_loops/python/libspline.py
@@ -1,4 +1,4 @@
-#!/usr/bin/python
+#!/usr/bin/python3
"""Wrapper around spline.h/cc through spline_array.cc."""
__author__ = 'Alex Perry (alex.perry96@gmail.com)'
diff --git a/frc971/control_loops/python/linear_system.py b/frc971/control_loops/python/linear_system.py
index a2e37f0..9cf49c2 100755
--- a/frc971/control_loops/python/linear_system.py
+++ b/frc971/control_loops/python/linear_system.py
@@ -1,4 +1,4 @@
-#!/usr/bin/python
+#!/usr/bin/python3
from aos.util.trapezoid_profile import TrapezoidProfile
from frc971.control_loops.python import control_loop
diff --git a/frc971/control_loops/python/polydrivetrain.py b/frc971/control_loops/python/polydrivetrain.py
index 06a182e..2623dda 100644
--- a/frc971/control_loops/python/polydrivetrain.py
+++ b/frc971/control_loops/python/polydrivetrain.py
@@ -1,4 +1,4 @@
-#!/usr/bin/python
+#!/usr/bin/python3
import numpy
from frc971.control_loops.python import polytope
diff --git a/frc971/control_loops/python/polytope.py b/frc971/control_loops/python/polytope.py
index a1cba57..5aa7ba3 100644
--- a/frc971/control_loops/python/polytope.py
+++ b/frc971/control_loops/python/polytope.py
@@ -1,4 +1,4 @@
-#!/usr/bin/python
+#!/usr/bin/python3
"""
Polyhedral set library.
@@ -29,7 +29,7 @@
split_string = s.split('\n')
width = max(len(stringpiece) for stringpiece in split_string) + 1
- padded_strings = [string.ljust(stringpiece, width, ' ')
+ padded_strings = [stringpiece.ljust(width, ' ')
for stringpiece in split_string]
return padded_strings
diff --git a/frc971/control_loops/python/polytope_test.py b/frc971/control_loops/python/polytope_test.py
index f5e4783..68401c2 100755
--- a/frc971/control_loops/python/polytope_test.py
+++ b/frc971/control_loops/python/polytope_test.py
@@ -1,4 +1,4 @@
-#!/usr/bin/python
+#!/usr/bin/python3
import numpy
from numpy.testing import *
@@ -153,39 +153,39 @@
def test_few_constraints_odd_constraint_even_dims_str(self):
"""Tests printing out the set with odd constraints and even dimensions."""
self.MakePWithDims(num_constraints=5, num_dims=2)
- self.assertEqual('[[ 0. 0.] [[ 0.] \n'
- ' [ 0. 0.] [[x0] [ 0.] \n'
- ' [ 0. 0.] [x1]] <= [ 0.] \n'
- ' [ 0. 0.] [ 0.] \n'
- ' [ 0. 0.]] [ 0.]] ',
+ self.assertEqual('[[0. 0.] [[0.] \n'
+ ' [0. 0.] [[x0] [0.] \n'
+ ' [0. 0.] [x1]] <= [0.] \n'
+ ' [0. 0.] [0.] \n'
+ ' [0. 0.]] [0.]] ',
str(self.p))
def test_few_constraints_odd_constraint_small_dims_str(self):
"""Tests printing out the set with odd constraints and odd dimensions."""
self.MakePWithDims(num_constraints=5, num_dims=1)
- self.assertEqual('[[ 0.] [[ 0.] \n'
- ' [ 0.] [ 0.] \n'
- ' [ 0.] [[x0]] <= [ 0.] \n'
- ' [ 0.] [ 0.] \n'
- ' [ 0.]] [ 0.]] ',
+ self.assertEqual('[[0.] [[0.] \n'
+ ' [0.] [0.] \n'
+ ' [0.] [[x0]] <= [0.] \n'
+ ' [0.] [0.] \n'
+ ' [0.]] [0.]] ',
str(self.p))
def test_few_constraints_odd_constraint_odd_dims_str(self):
"""Tests printing out the set with odd constraints and odd dimensions."""
self.MakePWithDims(num_constraints=5, num_dims=3)
- self.assertEqual('[[ 0. 0. 0.] [[ 0.] \n'
- ' [ 0. 0. 0.] [[x0] [ 0.] \n'
- ' [ 0. 0. 0.] [x1] <= [ 0.] \n'
- ' [ 0. 0. 0.] [x2]] [ 0.] \n'
- ' [ 0. 0. 0.]] [ 0.]] ',
+ self.assertEqual('[[0. 0. 0.] [[0.] \n'
+ ' [0. 0. 0.] [[x0] [0.] \n'
+ ' [0. 0. 0.] [x1] <= [0.] \n'
+ ' [0. 0. 0.] [x2]] [0.] \n'
+ ' [0. 0. 0.]] [0.]] ',
str(self.p))
def test_many_constraints_even_constraint_odd_dims_str(self):
"""Tests printing out the set with even constraints and odd dimensions."""
self.MakePWithDims(num_constraints=2, num_dims=3)
- self.assertEqual('[[ 0. 0. 0.] [[x0] [[ 0.] \n'
- ' [ 0. 0. 0.]] [x1] <= [ 0.]] \n'
- ' [x2]] ',
+ self.assertEqual('[[0. 0. 0.] [[x0] [[0.] \n'
+ ' [0. 0. 0.]] [x1] <= [0.]] \n'
+ ' [x2]] ',
str(self.p))
diff --git a/frc971/control_loops/python/spline.py b/frc971/control_loops/python/spline.py
index 890ef9f..8a0ac04 100644
--- a/frc971/control_loops/python/spline.py
+++ b/frc971/control_loops/python/spline.py
@@ -1,4 +1,4 @@
-#!/usr/bin/python
+#!/usr/bin/python3
from __future__ import print_function
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 839677e..8080aff 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
@@ -1,4 +1,4 @@
-#!/usr/bin/python
+#!/usr/bin/python3
# Generates profiled subsystem for use in
# static_zeroing_single_dof_profiled_subsystem_test