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/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