Run yapf on Spline UI
Change-Id: Id09023a4e651fd041ea5acc9814e441780307336
diff --git a/frc971/control_loops/python/angular_system.py b/frc971/control_loops/python/angular_system.py
index 34f4307..7ccb4af 100755
--- a/frc971/control_loops/python/angular_system.py
+++ b/frc971/control_loops/python/angular_system.py
@@ -9,7 +9,6 @@
class AngularSystemParams(object):
-
def __init__(self,
name,
motor,
@@ -43,7 +42,6 @@
class AngularSystem(control_loop.ControlLoop):
-
def __init__(self, params, name="AngularSystem"):
super(AngularSystem, self).__init__(name)
self.params = params
@@ -125,7 +123,6 @@
class IntegralAngularSystem(AngularSystem):
-
def __init__(self, params, name="IntegralAngularSystem"):
super(IntegralAngularSystem, self).__init__(params, name=name)
@@ -148,7 +145,8 @@
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)]])
+ [0.0, 0.0, (self.params.kalman_q_voltage
+ **2.0)]])
self.R = numpy.matrix([[(self.params.kalman_r_position**2.0)]])
@@ -266,7 +264,8 @@
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))
@@ -399,8 +398,8 @@
angular_systems.append(
AngularSystem(param, param.name + str(index)))
integral_angular_systems.append(
- IntegralAngularSystem(param, 'Integral' + param.name + str(
- index)))
+ IntegralAngularSystem(param,
+ 'Integral' + param.name + str(index)))
else:
name = params.name
angular_systems.append(AngularSystem(params, params.name))
@@ -412,8 +411,8 @@
loop_writer.AddConstant(
control_loop.Constant('kOutputRatio', '%f', angular_systems[0].G))
loop_writer.AddConstant(
- control_loop.Constant('kFreeSpeed', '%f', angular_systems[0]
- .motor.free_speed))
+ control_loop.Constant('kFreeSpeed', '%f',
+ angular_systems[0].motor.free_speed))
loop_writer.Write(plant_files[0], plant_files[1])
integral_loop_writer = control_loop.ControlLoopWriter(
diff --git a/frc971/control_loops/python/basic_window.py b/frc971/control_loops/python/basic_window.py
index 42ba84c..b8d49a9 100755
--- a/frc971/control_loops/python/basic_window.py
+++ b/frc971/control_loops/python/basic_window.py
@@ -10,6 +10,7 @@
identity = cairo.Matrix()
+
# Override the matrix of a cairo context.
class OverrideMatrix(object):
def __init__(self, cr, matrix):
@@ -26,15 +27,18 @@
mainloop = GLib.MainLoop()
+
def set_color(cr, color):
if color.a == 1.0:
cr.set_source_rgb(color.r, color.g, color.b)
else:
cr.set_source_rgba(color.r, color.g, color.b, color.a)
+
def quit_main_loop(*args):
mainloop.quit()
+
def RunApp():
try:
mainloop.run()
@@ -50,9 +54,9 @@
def __init__(self):
super(BaseWindow, self).__init__()
- self.set_size_request(2*SCREEN_SIZE, SCREEN_SIZE)
+ self.set_size_request(2 * SCREEN_SIZE, SCREEN_SIZE)
self.center = (0, 0)
- self.shape = (2*SCREEN_SIZE, SCREEN_SIZE)
+ self.shape = (2 * SCREEN_SIZE, SCREEN_SIZE)
self.needs_redraw = False
def get_current_scale(self):
@@ -81,4 +85,3 @@
# Handle the expose-event by drawing
def handle_draw(self, cr):
pass
-
diff --git a/frc971/control_loops/python/cim.py b/frc971/control_loops/python/cim.py
index f8a6e9a..ac527de 100644
--- a/frc971/control_loops/python/cim.py
+++ b/frc971/control_loops/python/cim.py
@@ -5,7 +5,6 @@
class CIM(control_loop.ControlLoop):
-
def __init__(self):
super(CIM, self).__init__("CIM")
# Stall Torque in N m
diff --git a/frc971/control_loops/python/control_loop.py b/frc971/control_loops/python/control_loop.py
index 3270f78..74141c1 100644
--- a/frc971/control_loops/python/control_loop.py
+++ b/frc971/control_loops/python/control_loop.py
@@ -4,7 +4,6 @@
class Constant(object):
-
def __init__(self, name, formatt, value, comment=None):
self.name = name
self.formatt = formatt
@@ -26,7 +25,6 @@
class ControlLoopWriter(object):
-
def __init__(self,
gain_schedule_name,
loops,
@@ -143,8 +141,8 @@
'#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'):
+ if (self._plant_type == 'StateFeedbackHybridPlant'
+ or self._observer_type == 'HybridKalman'):
fd.write(
'#include \"frc971/control_loops/hybrid_state_feedback_loop.h\"\n'
)
@@ -215,9 +213,9 @@
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(' 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')
@@ -228,9 +226,9 @@
(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()))
+ ' 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')
@@ -259,7 +257,6 @@
class ControlLoop(object):
-
def __init__(self, name):
"""Constructs a control loop object.
@@ -350,8 +347,8 @@
with the contents of matrix.
"""
ans = [
- ' Eigen::Matrix<%s, %d, %d> %s;\n' % (scalar_type, matrix.shape[0],
- matrix.shape[1], matrix_name)
+ ' Eigen::Matrix<%s, %d, %d> %s;\n' %
+ (scalar_type, matrix.shape[0], matrix.shape[1], matrix_name)
]
for x in range(matrix.shape[0]):
for y in range(matrix.shape[1]):
@@ -472,7 +469,8 @@
Returns:
string, The header declaration for the function.
"""
- return '%s %s;\n' % (observer_coefficient_type, self.ObserverFunction())
+ 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.
@@ -497,7 +495,7 @@
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,))
+ (observer_coefficient_type, ))
elif observer_coefficient_type.startswith('HybridKalman'):
ans.append(
@@ -511,7 +509,7 @@
scalar_type))
ans.append(
' return %s(Q_continuous, R_continuous, P_steady_state);\n' %
- (observer_coefficient_type,))
+ (observer_coefficient_type, ))
else:
glog.fatal('Unsupported observer type %s',
observer_coefficient_type)
@@ -521,7 +519,6 @@
class HybridControlLoop(ControlLoop):
-
def __init__(self, name):
super(HybridControlLoop, self).__init__(name=name)
@@ -553,7 +550,6 @@
class CIM(object):
-
def __init__(self):
# Stall Torque in N m
self.stall_torque = 2.42
@@ -573,7 +569,6 @@
class MiniCIM(object):
-
def __init__(self):
# Stall Torque in N m
self.stall_torque = 1.41
@@ -593,7 +588,6 @@
class NMotor(object):
-
def __init__(self, motor, n):
"""Gangs together n motors."""
self.motor = motor
@@ -609,7 +603,6 @@
class Vex775Pro(object):
-
def __init__(self):
# Stall Torque in N m
self.stall_torque = 0.71
@@ -653,7 +646,6 @@
class MN3510(object):
-
def __init__(self):
# http://www.robotshop.com/en/t-motor-navigator-mn3510-360kv-brushless-motor.html#Specifications
# Free Current in Amps
@@ -688,11 +680,11 @@
# Resistance of the motor, divided by 2 to account for the 2 motors
self.resistance = 12.0 / self.stall_current
# Motor velocity constant
- self.Kv = (self.free_speed /
- (12.0 - self.resistance * self.free_current))
+ self.Kv = (
+ self.free_speed / (12.0 - self.resistance * self.free_current))
# Torque constant
self.Kt = self.stall_torque / self.stall_current
# Motor inertia in kg m^2
# Diameter of 1.9", weight of: 100 grams
# TODO(austin): Get a number from Scott Westbrook for the mass
- self.motor_inertia = 0.1 * ((0.95 * 0.0254) ** 2.0)
+ self.motor_inertia = 0.1 * ((0.95 * 0.0254)**2.0)
diff --git a/frc971/control_loops/python/controls.py b/frc971/control_loops/python/controls.py
index 4fdf1e9..1dfd061 100644
--- a/frc971/control_loops/python/controls.py
+++ b/frc971/control_loops/python/controls.py
@@ -1,5 +1,4 @@
#!/usr/bin/python3
-
"""
Control loop pole placement library.
@@ -14,15 +13,16 @@
import scipy.signal
import glog
-class Error (Exception):
- """Base class for all control loop exceptions."""
+
+class Error(Exception):
+ """Base class for all control loop exceptions."""
# 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):
- """Set the poles of (A - BF) to poles.
+ """Set the poles of (A - BF) to poles.
Args:
A: numpy.matrix(n x n), The A matrix.
@@ -33,66 +33,71 @@
Returns:
numpy.matrix(m x n), K
"""
- return scipy.signal.place_poles(A=A, B=B, poles=numpy.array(poles)).gain_matrix
+ 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.
+ """Converts from continuous time state space representation to discrete time.
Returns (A, B). C and D are unchanged.
This code is copied from: scipy.signal.cont2discrete method zoh
"""
- a, b = numpy.array(A), numpy.array(B)
- # Build an exponential matrix
- em_upper = numpy.hstack((a, b))
+ a, b = numpy.array(A), numpy.array(B)
+ # Build an exponential matrix
+ em_upper = numpy.hstack((a, b))
- # Need to stack zeros under the a and b matrices
- em_lower = numpy.hstack((numpy.zeros((b.shape[1], a.shape[0])),
- numpy.zeros((b.shape[1], b.shape[1]))))
+ # Need to stack zeros under the a and b matrices
+ em_lower = numpy.hstack((numpy.zeros((b.shape[1], a.shape[0])),
+ numpy.zeros((b.shape[1], b.shape[1]))))
- em = numpy.vstack((em_upper, em_lower))
- ms = scipy.linalg.expm(dt * em)
+ em = numpy.vstack((em_upper, em_lower))
+ ms = scipy.linalg.expm(dt * em)
- # Dispose of the lower rows
- ms = ms[:a.shape[0], :]
+ # Dispose of the lower rows
+ ms = ms[:a.shape[0], :]
- ad = ms[:, 0:a.shape[1]]
- bd = ms[:, a.shape[1]:]
+ ad = ms[:, 0:a.shape[1]]
+ bd = ms[:, a.shape[1]:]
- return numpy.matrix(ad), numpy.matrix(bd)
+ return numpy.matrix(ad), numpy.matrix(bd)
+
def ctrb(A, B):
- """Returns the controllability matrix.
+ """Returns the controllability matrix.
This matrix must have full rank for all the states to be controllable.
"""
- n = A.shape[0]
- output = B
- intermediate = B
- for i in range(0, n):
- intermediate = A * intermediate
- output = numpy.concatenate((output, intermediate), axis=1)
+ n = A.shape[0]
+ output = B
+ intermediate = B
+ for i in range(0, n):
+ intermediate = A * intermediate
+ output = numpy.concatenate((output, intermediate), axis=1)
- return output
+ return output
+
def dlqr(A, B, Q, R, optimal_cost_function=False):
- """Solves for the optimal lqr controller.
+ """Solves for the optimal lqr controller.
x(n+1) = A * x(n) + B * u(n)
J = sum(0, inf, x.T * Q * x + u.T * R * u)
"""
- # 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 = (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 = 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
- else:
- return F
+ 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
+ else:
+ return F
+
def kalman(A, B, C, Q, R):
- """Solves for the steady state kalman gain and covariance matricies.
+ """Solves for the steady state kalman gain and covariance matricies.
Args:
A, B, C: SS matricies.
@@ -102,27 +107,27 @@
Returns:
KalmanGain, Covariance.
"""
- I = numpy.matrix(numpy.eye(Q.shape[0]))
- Z = numpy.matrix(numpy.zeros(Q.shape[0]))
- n = A.shape[0]
- m = C.shape[0]
+ I = numpy.matrix(numpy.eye(Q.shape[0]))
+ Z = numpy.matrix(numpy.zeros(Q.shape[0]))
+ n = A.shape[0]
+ m = C.shape[0]
- controllability_rank = numpy.linalg.matrix_rank(ctrb(A.T, C.T))
- if controllability_rank != n:
- glog.warning('Observability of %d != %d, unobservable state',
- controllability_rank, n)
+ controllability_rank = numpy.linalg.matrix_rank(ctrb(A.T, C.T))
+ if controllability_rank != n:
+ glog.warning('Observability of %d != %d, unobservable state',
+ controllability_rank, n)
- # Compute the steady state covariance matrix.
- 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, rcond=None)[0].T
- P = (I - K * C) * P_prior
+ # Compute the steady state covariance matrix.
+ 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, rcond=None)[0].T
+ P = (I - K * C) * P_prior
- return K, P
+ return K, P
def kalmd(A_continuous, B_continuous, Q_continuous, R_continuous, dt):
- """Converts a continuous time kalman filter to discrete time.
+ """Converts a continuous time kalman filter to discrete time.
Args:
A_continuous: The A continuous matrix
@@ -137,34 +142,38 @@
Returns:
The discrete matrices of A, B, Q, and R.
"""
- # TODO(austin): Verify that the dimensions make sense.
- number_of_states = A_continuous.shape[0]
- number_of_inputs = B_continuous.shape[1]
- M = numpy.zeros((len(A_continuous) + number_of_inputs,
- len(A_continuous) + number_of_inputs))
- M[0:number_of_states, 0:number_of_states] = A_continuous
- M[0:number_of_states, number_of_states:] = B_continuous
- M_exp = scipy.linalg.expm(M * dt)
- A_discrete = M_exp[0:number_of_states, 0:number_of_states]
- B_discrete = numpy.matrix(M_exp[0:number_of_states, number_of_states:])
- Q_continuous = (Q_continuous + Q_continuous.T) / 2.0
- R_continuous = (R_continuous + R_continuous.T) / 2.0
- M = numpy.concatenate((-A_continuous, Q_continuous), axis=1)
- M = numpy.concatenate(
- (M, numpy.concatenate((numpy.matrix(
- numpy.zeros((number_of_states, number_of_states))),
- numpy.transpose(A_continuous)), axis = 1)), axis = 0)
- phi = numpy.matrix(scipy.linalg.expm(M*dt))
- phi12 = phi[0:number_of_states, number_of_states:(2*number_of_states)]
- phi22 = phi[number_of_states:2*number_of_states, number_of_states:2*number_of_states]
- Q_discrete = phi22.T * phi12
- Q_discrete = (Q_discrete + Q_discrete.T) / 2.0
- R_discrete = R_continuous / dt
- return (A_discrete, B_discrete, Q_discrete, R_discrete)
+ # TODO(austin): Verify that the dimensions make sense.
+ number_of_states = A_continuous.shape[0]
+ number_of_inputs = B_continuous.shape[1]
+ M = numpy.zeros((len(A_continuous) + number_of_inputs,
+ len(A_continuous) + number_of_inputs))
+ M[0:number_of_states, 0:number_of_states] = A_continuous
+ M[0:number_of_states, number_of_states:] = B_continuous
+ M_exp = scipy.linalg.expm(M * dt)
+ A_discrete = M_exp[0:number_of_states, 0:number_of_states]
+ B_discrete = numpy.matrix(M_exp[0:number_of_states, number_of_states:])
+ Q_continuous = (Q_continuous + Q_continuous.T) / 2.0
+ R_continuous = (R_continuous + R_continuous.T) / 2.0
+ M = numpy.concatenate((-A_continuous, Q_continuous), axis=1)
+ M = numpy.concatenate(
+ (M,
+ numpy.concatenate(
+ (numpy.matrix(numpy.zeros((number_of_states, number_of_states))),
+ numpy.transpose(A_continuous)),
+ axis=1)),
+ axis=0)
+ phi = numpy.matrix(scipy.linalg.expm(M * dt))
+ phi12 = phi[0:number_of_states, number_of_states:(2 * number_of_states)]
+ phi22 = phi[number_of_states:2 * number_of_states, number_of_states:2 *
+ number_of_states]
+ Q_discrete = phi22.T * phi12
+ Q_discrete = (Q_discrete + Q_discrete.T) / 2.0
+ R_discrete = R_continuous / dt
+ return (A_discrete, B_discrete, Q_discrete, R_discrete)
def TwoStateFeedForwards(B, Q):
- """Computes the feed forwards constant for a 2 state controller.
+ """Computes the feed forwards constant for a 2 state controller.
This will take the form U = Kff * (R(n + 1) - A * R(n)), where Kff is the
feed-forwards constant. It is important that Kff is *only* computed off
@@ -178,9 +187,9 @@
numpy.Matrix[num_inputs, num_states]
"""
- # We want to find the optimal U such that we minimize the tracking cost.
- # This means that we want to minimize
- # (B * U - (R(n+1) - A R(n)))^T * Q * (B * U - (R(n+1) - A R(n)))
- # TODO(austin): This doesn't take into account the cost of U
+ # We want to find the optimal U such that we minimize the tracking cost.
+ # This means that we want to minimize
+ # (B * U - (R(n+1) - A R(n)))^T * Q * (B * U - (R(n+1) - A R(n)))
+ # TODO(austin): This doesn't take into account the cost of U
- return numpy.linalg.inv(B.T * Q * B) * B.T * Q.T
+ return numpy.linalg.inv(B.T * Q * B) * B.T * Q.T
diff --git a/frc971/control_loops/python/down_estimator.py b/frc971/control_loops/python/down_estimator.py
index 224fe03..84cdfa9 100644
--- a/frc971/control_loops/python/down_estimator.py
+++ b/frc971/control_loops/python/down_estimator.py
@@ -18,7 +18,6 @@
class DownEstimator(control_loop.ControlLoop):
-
def __init__(self, name='DownEstimator'):
super(DownEstimator, self).__init__(name)
self.dt = 0.005
@@ -55,7 +54,8 @@
self.InitializeState()
- def Update(self, accelerometer_x, accelerometer_y, accelerometer_z, gyro_x):
+ 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:
@@ -93,7 +93,8 @@
angle = math.pi / 2
velocity = 1
for i in range(100):
- measured_velocity = velocity + (random.random() - 0.5) * 0.01 + 0.05
+ 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,
diff --git a/frc971/control_loops/python/drawing_constants.py b/frc971/control_loops/python/drawing_constants.py
index 332f37e6..a364e40 100644
--- a/frc971/control_loops/python/drawing_constants.py
+++ b/frc971/control_loops/python/drawing_constants.py
@@ -137,10 +137,10 @@
for sign in [1.0, -1.0]:
set_color(cr, palette["GREEN"])
- cr.rectangle(mToPx(sign * trench_start_x),
- mToPx(sign * edge_of_field_y),
- mToPx(sign * trench_length_x),
- mToPx(sign * (edge_of_trench_y - edge_of_field_y)))
+ cr.rectangle(
+ mToPx(sign * trench_start_x), mToPx(sign * edge_of_field_y),
+ mToPx(sign * trench_length_x),
+ mToPx(sign * (edge_of_trench_y - edge_of_field_y)))
cr.stroke()
draw_circle(cr, mToPx(sign * ball_one_x), mToPx(sign * ball_line_y),
mToPx(0.1), palette["YELLOW"])
@@ -437,5 +437,5 @@
def draw_points(cr, p, size):
for i in range(0, len(p)):
- draw_px_cross(cr, p[i][0], p[i][1], size,
- Color(0, np.sqrt(0.2 * i), 0))
+ draw_px_cross(cr, p[i][0], p[i][1], size, Color(
+ 0, np.sqrt(0.2 * i), 0))
diff --git a/frc971/control_loops/python/drivetrain.py b/frc971/control_loops/python/drivetrain.py
index b29e1e6..4069036 100644
--- a/frc971/control_loops/python/drivetrain.py
+++ b/frc971/control_loops/python/drivetrain.py
@@ -9,7 +9,6 @@
class DrivetrainParams(object):
-
def __init__(self,
J,
mass,
@@ -110,7 +109,6 @@
class Drivetrain(control_loop.ControlLoop):
-
def __init__(self,
drivetrain_params,
name="Drivetrain",
@@ -219,9 +217,10 @@
# 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],
[self.msnl * self.mpl, self.mspr * self.mpr]])
@@ -250,12 +249,14 @@
self.K = controls.dlqr(self.A, self.B, self.Q, self.R)
glog.debug('DT q_pos %f q_vel %s %s', q_pos, q_vel, self._name)
- glog.debug('Poles: %s', str(numpy.linalg.eig(self.A - self.B * self.K)[0]))
- glog.debug('Time constants: %s hz',
- str([
- numpy.log(x) / -self.dt
- for x in numpy.linalg.eig(self.A - self.B * self.K)[0]
- ]))
+ glog.debug('Poles: %s',
+ str(numpy.linalg.eig(self.A - self.B * self.K)[0]))
+ glog.debug(
+ 'Time constants: %s hz',
+ str([
+ numpy.log(x) / -self.dt
+ for x in numpy.linalg.eig(self.A - self.B * self.K)[0]
+ ]))
glog.debug('K %s', repr(self.K))
self.hlp = 0.3
@@ -267,7 +268,6 @@
class KFDrivetrain(Drivetrain):
-
def __init__(self,
drivetrain_params,
name="KFDrivetrain",
@@ -308,7 +308,8 @@
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.msnr,
+ self.mspr]])
q_voltage = drivetrain_params.kf_q_voltage * self.mpl
else:
self.A_continuous[0:4, 4:6] = self.unaugmented_B_continuous
@@ -322,31 +323,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.C[3, 0:6] = 0.5 * (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
+ 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]])
@@ -619,7 +620,8 @@
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/graph.py b/frc971/control_loops/python/graph.py
index 60d1e65..9c69f9c 100644
--- a/frc971/control_loops/python/graph.py
+++ b/frc971/control_loops/python/graph.py
@@ -42,14 +42,15 @@
traj.Plan()
XVA = traj.GetPlanXVA(dT)
if len(XVA[0]) > 0:
- self.draw_x_axis(cr, start, height, zero, XVA, end)
- self.drawVelocity(cr, XVA, start, height, skip, zero, end)
- self.drawAcceleration(cr, XVA, start, height, skip, zero,
- AXIS_MARGIN_SPACE, end)
- self.drawVoltage(cr, XVA, start, height, skip, traj, zero, end)
- cr.set_source_rgb(0, 0, 0)
- cr.move_to(-1.0 * AXIS_MARGIN_SPACE, zero + height / 2.0)
- cr.line_to(AXIS_MARGIN_SPACE - SCREEN_SIZE, zero + height / 2.0)
+ self.draw_x_axis(cr, start, height, zero, XVA, end)
+ self.drawVelocity(cr, XVA, start, height, skip, zero, end)
+ self.drawAcceleration(cr, XVA, start, height, skip, zero,
+ AXIS_MARGIN_SPACE, end)
+ self.drawVoltage(cr, XVA, start, height, skip, traj, zero, end)
+ cr.set_source_rgb(0, 0, 0)
+ cr.move_to(-1.0 * AXIS_MARGIN_SPACE, zero + height / 2.0)
+ cr.line_to(AXIS_MARGIN_SPACE - SCREEN_SIZE,
+ zero + height / 2.0)
cr.stroke()
def connectLines(self, cr, points, color):
diff --git a/frc971/control_loops/python/haptic_wheel.py b/frc971/control_loops/python/haptic_wheel.py
index 5e63df3..7221870 100755
--- a/frc971/control_loops/python/haptic_wheel.py
+++ b/frc971/control_loops/python/haptic_wheel.py
@@ -22,7 +22,6 @@
class SystemParams(object):
-
def __init__(self, J, G, kP, kD, kCompensationTimeconstant, q_pos, q_vel,
q_torque, current_limit):
self.J = J
@@ -69,7 +68,6 @@
class HapticInput(control_loop.ControlLoop):
-
def __init__(self, params=None, name='HapticInput'):
# The defaults are for the steering wheel.
super(HapticInput, self).__init__(name)
@@ -109,7 +107,6 @@
class IntegralHapticInput(HapticInput):
-
def __init__(self, params=None, name="IntegralHapticInput"):
super(IntegralHapticInput, self).__init__(name=name, params=params)
@@ -369,7 +366,9 @@
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]
+ 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)
@@ -390,8 +389,9 @@
kWheel,
run_correct=True)
else:
- plot_input(trigger_data_time, trigger_radians, trigger_velocity,
- trigger_torque, trigger_current, kTrigger)
+ 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()
@@ -403,7 +403,8 @@
else:
namespaces = ['frc971', 'control_loops', 'drivetrain']
for name, params, filenames in [('HapticWheel', kWheel, argv[1:5]),
- ('HapticTrigger', kTrigger, argv[5:9])]:
+ ('HapticTrigger', kTrigger,
+ argv[5:9])]:
haptic_input = HapticInput(params=params, name=name)
loop_writer = control_loop.ControlLoopWriter(
name, [haptic_input],
@@ -422,11 +423,13 @@
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))
+ 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))
+ 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))
diff --git a/frc971/control_loops/python/lib_spline_test.py b/frc971/control_loops/python/lib_spline_test.py
index a1e298a..cf69a5c 100644
--- a/frc971/control_loops/python/lib_spline_test.py
+++ b/frc971/control_loops/python/lib_spline_test.py
@@ -7,6 +7,7 @@
from libspline import Spline, DistanceSpline, Trajectory
+
class TestSpline(unittest.TestCase):
def testSimpleSpline(self):
points = np.array([[1.0, 2.0, 3.0, 4.0, 5.0, 6.0],
@@ -15,6 +16,7 @@
assert_allclose(spline.Point(.5), [3.5, 4.5])
assert_almost_equal(spline.Theta(.5), math.atan2(1, 1))
+
class TestDistanceSpline(unittest.TestCase):
def testDistanceSpline(self):
points = np.array([[1.0, 2.0, 3.0, 4.0, 5.0, 6.0],
@@ -23,6 +25,7 @@
dSpline = DistanceSpline([spline])
assert_almost_equal(dSpline.Length(), 5 * math.sqrt(2))
+
class TestTrajectory(unittest.TestCase):
def testTrajectory(self):
points = np.array([[1.0, 2.0, 3.0, 4.0, 5.0, 5.0],
@@ -31,7 +34,7 @@
dSpline = DistanceSpline([spline])
trajectory = Trajectory(dSpline)
trajectory.Plan()
- plan = trajectory.GetPlanXVA(5.05*1e-3)
+ plan = trajectory.GetPlanXVA(5.05 * 1e-3)
self.assertEqual(plan.shape, (3, 624))
def testLimits(self):
@@ -43,7 +46,7 @@
trajectory = Trajectory(dSpline)
trajectory.LimitVelocity(0, trajectory.Length(), 3)
trajectory.Plan()
- plan = trajectory.GetPlanXVA(5.05*1e-3)
+ plan = trajectory.GetPlanXVA(5.05 * 1e-3)
self.assertEqual(plan.shape, (3, 656))
diff --git a/frc971/control_loops/python/libcdd.py b/frc971/control_loops/python/libcdd.py
index 72d5833..e22643f 100644
--- a/frc971/control_loops/python/libcdd.py
+++ b/frc971/control_loops/python/libcdd.py
@@ -1,5 +1,4 @@
#!/usr/bin/python3
-
"""Wrapper around libcdd, a polytope manipulation library."""
__author__ = 'Austin Schuh (austin.linux@gmail.com)'
@@ -15,7 +14,8 @@
libcdd = None
for path in os.environ.get('PYTHONPATH').split(':'):
try:
- libcdd = ctypes.cdll.LoadLibrary(os.path.join(path, 'third_party/cddlib/_cddlib.so'))
+ libcdd = ctypes.cdll.LoadLibrary(
+ os.path.join(path, 'third_party/cddlib/_cddlib.so'))
except OSError:
pass
@@ -49,36 +49,26 @@
("rowvec", ctypes.POINTER(mytype)),
]
+
# Define the input and output types for a bunch of libcdd functions.
libcdd.dd_CreateMatrix.restype = ctypes.POINTER(dd_matrixdata)
libcdd.ddd_get_d.argtypes = [mytype]
libcdd.ddd_get_d.restype = ctypes.c_double
-libcdd.dd_CopyGenerators.argtypes = [
- ctypes.POINTER(dd_polyhedradata)
-]
+libcdd.dd_CopyGenerators.argtypes = [ctypes.POINTER(dd_polyhedradata)]
libcdd.dd_CopyGenerators.restype = ctypes.POINTER(dd_matrixdata)
libcdd.dd_DDMatrix2Poly.argtypes = [
ctypes.POINTER(dd_matrixdata),
ctypes.POINTER(ctypes.c_int)
]
-libcdd.dd_DDMatrix2Poly.restype = (
- ctypes.POINTER(dd_polyhedradata))
+libcdd.dd_DDMatrix2Poly.restype = (ctypes.POINTER(dd_polyhedradata))
-libcdd.dd_FreeMatrix.argtypes = [
- ctypes.POINTER(dd_matrixdata)
-]
+libcdd.dd_FreeMatrix.argtypes = [ctypes.POINTER(dd_matrixdata)]
-libcdd.dd_FreePolyhedra.argtypes = [
- ctypes.POINTER(dd_polyhedradata)
-]
+libcdd.dd_FreePolyhedra.argtypes = [ctypes.POINTER(dd_polyhedradata)]
-libcdd.ddd_set_d.argtypes = [
- mytype,
- ctypes.c_double
-]
-
+libcdd.ddd_set_d.argtypes = [mytype, ctypes.c_double]
# Various enums.
DD_INEQUALITY = 1
diff --git a/frc971/control_loops/python/libspline.py b/frc971/control_loops/python/libspline.py
index 24f1f51..8facbb5 100755
--- a/frc971/control_loops/python/libspline.py
+++ b/frc971/control_loops/python/libspline.py
@@ -34,6 +34,7 @@
# Required for trajectory
libSpline.SetUpLogging()
+
class Spline:
"""
A wrapper around spline.h/cc through libspline.cc.
@@ -44,9 +45,10 @@
def __init__(self, points):
assert points.shape == (2, 6)
self.__points = points
- self.__spline = ct.c_void_p(libSpline.NewSpline(
- np.ctypeslib.as_ctypes(self.__points[0]),
- np.ctypeslib.as_ctypes(self.__points[1])))
+ self.__spline = ct.c_void_p(
+ libSpline.NewSpline(
+ np.ctypeslib.as_ctypes(self.__points[0]),
+ np.ctypeslib.as_ctypes(self.__points[1])))
def __del__(self):
libSpline.deleteSpline(self.__spline)
@@ -55,9 +57,10 @@
self.__points[0, index] = x
self.__points[1, index] = y
libSpline.deleteSpline(self.__spline)
- self.__spline = ct.c_void_p(libSpline.newSpline(
- np.ctypeslib.as_ctypes(self.__points[0]),
- np.ctypeslib.as_ctypes(self.__points[1])))
+ self.__spline = ct.c_void_p(
+ libSpline.newSpline(
+ np.ctypeslib.as_ctypes(self.__points[0]),
+ np.ctypeslib.as_ctypes(self.__points[1])))
def Point(self, alpha):
result = np.zeros(2)
@@ -114,8 +117,8 @@
spline_ptrs = np.array(spline_ptrs)
spline_array = np.ctypeslib.as_ctypes(spline_ptrs)
- self.__spline = ct.c_void_p(libSpline.NewDistanceSpline(
- ct.byref(spline_array), len(splines)))
+ self.__spline = ct.c_void_p(
+ libSpline.NewDistanceSpline(ct.byref(spline_array), len(splines)))
def __del__(self):
libSpline.deleteDistanceSpline(self.__spline)
@@ -166,8 +169,9 @@
"""A wrapper around trajectory.h/cc through libspline.cc."""
def __init__(self, distance_spline, vmax=10, num_distance=0):
- self.__trajectory = ct.c_void_p(libSpline.NewTrajectory(
- distance_spline.GetSplinePtr(), ct.c_double(vmax), num_distance))
+ self.__trajectory = ct.c_void_p(
+ libSpline.NewTrajectory(distance_spline.GetSplinePtr(),
+ ct.c_double(vmax), num_distance))
def __del__(self):
libSpline.deleteTrajectory(self.__trajectory)
@@ -243,7 +247,9 @@
long it takes to run the path.
This is slow so don't call more than once with the same data.
"""
- XVAPtr = ct.c_void_p(libSpline.TrajectoryGetPlanXVAPtr(self.__trajectory, int(dt*1e9)))
+ XVAPtr = ct.c_void_p(
+ libSpline.TrajectoryGetPlanXVAPtr(self.__trajectory,
+ int(dt * 1e9)))
XVALength = libSpline.TrajectoryGetVectorLength(XVAPtr)
X = np.zeros(XVALength)
V = np.zeros(XVALength)
diff --git a/frc971/control_loops/python/linear_system.py b/frc971/control_loops/python/linear_system.py
index 9cf49c2..0391d93 100755
--- a/frc971/control_loops/python/linear_system.py
+++ b/frc971/control_loops/python/linear_system.py
@@ -9,7 +9,6 @@
class LinearSystemParams(object):
-
def __init__(self,
name,
motor,
@@ -38,7 +37,6 @@
class LinearSystem(control_loop.ControlLoop):
-
def __init__(self, params, name='LinearSystem'):
super(LinearSystem, self).__init__(name)
self.params = params
@@ -58,8 +56,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)
+ 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)
@@ -124,7 +123,6 @@
class IntegralLinearSystem(LinearSystem):
-
def __init__(self, params, name='IntegralLinearSystem'):
super(IntegralLinearSystem, self).__init__(params, name=name)
@@ -147,7 +145,8 @@
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)]])
+ [0.0, 0.0, (self.params.kalman_q_voltage
+ **2.0)]])
self.R = numpy.matrix([[(self.params.kalman_r_position**2.0)]])
@@ -265,7 +264,8 @@
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))
@@ -395,11 +395,10 @@
if type(params) is list:
name = params[0].name
for index, param in enumerate(params):
- linear_systems.append(
- LinearSystem(param, param.name + str(index)))
+ linear_systems.append(LinearSystem(param, param.name + str(index)))
integral_linear_systems.append(
- IntegralLinearSystem(param, 'Integral' + param.name + str(
- index)))
+ IntegralLinearSystem(param,
+ 'Integral' + param.name + str(index)))
else:
name = params.name
linear_systems.append(LinearSystem(params, params.name))
@@ -409,17 +408,15 @@
loop_writer = control_loop.ControlLoopWriter(
name, linear_systems, namespaces=year_namespaces)
loop_writer.AddConstant(
- control_loop.Constant('kFreeSpeed', '%f', linear_systems[0]
- .motor.free_speed))
+ control_loop.Constant('kFreeSpeed', '%f',
+ linear_systems[0].motor.free_speed))
loop_writer.AddConstant(
- control_loop.Constant('kOutputRatio', '%f', linear_systems[0].G *
- linear_systems[0].radius))
+ control_loop.Constant('kOutputRatio', '%f',
+ linear_systems[0].G * linear_systems[0].radius))
loop_writer.AddConstant(
control_loop.Constant('kRadius', '%f', linear_systems[0].radius))
loop_writer.Write(plant_files[0], plant_files[1])
integral_loop_writer = control_loop.ControlLoopWriter(
- 'Integral' + name,
- integral_linear_systems,
- namespaces=year_namespaces)
+ 'Integral' + name, integral_linear_systems, namespaces=year_namespaces)
integral_loop_writer.Write(controller_files[0], controller_files[1])
diff --git a/frc971/control_loops/python/path_edit.py b/frc971/control_loops/python/path_edit.py
index 31d7a37..2e9ed95 100755
--- a/frc971/control_loops/python/path_edit.py
+++ b/frc971/control_loops/python/path_edit.py
@@ -32,6 +32,7 @@
class GTK_Widget(BaseWindow):
"""Create a GTK+ widget on which we will draw using Cairo"""
+
def __init__(self):
super(GTK_Widget, self).__init__()
@@ -75,6 +76,7 @@
self.module_path = os.path.dirname(os.path.realpath(sys.argv[0]))
"""set extents on images"""
+
def reinit_extents(self):
self.extents_x_min = -1.0 * SCREEN_SIZE
self.extents_x_max = SCREEN_SIZE
@@ -298,10 +300,11 @@
array[j, 1] = point[1]
spline = Spline(np.ascontiguousarray(np.transpose(array)))
for k in np.linspace(0.01, 1, 100):
- cr.move_to(mToPx(spline.Point(k - 0.01)[0]),
- mToPx(spline.Point(k - 0.01)[1]))
- cr.line_to(mToPx(spline.Point(k)[0]),
- mToPx(spline.Point(k)[1]))
+ cr.move_to(
+ mToPx(spline.Point(k - 0.01)[0]),
+ mToPx(spline.Point(k - 0.01)[1]))
+ cr.line_to(
+ mToPx(spline.Point(k)[0]), mToPx(spline.Point(k)[1]))
cr.stroke()
holding = [
spline.Point(k - 0.01)[0],
diff --git a/frc971/control_loops/python/polydrivetrain.py b/frc971/control_loops/python/polydrivetrain.py
index 2623dda..39fa33d 100644
--- a/frc971/control_loops/python/polydrivetrain.py
+++ b/frc971/control_loops/python/polydrivetrain.py
@@ -98,7 +98,6 @@
class VelocityDrivetrainModel(control_loop.ControlLoop):
-
def __init__(self,
drivetrain_params,
left_low=True,
@@ -142,8 +141,10 @@
# 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.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,
@@ -428,7 +429,8 @@
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])
+ 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,
diff --git a/frc971/control_loops/python/polytope.py b/frc971/control_loops/python/polytope.py
index 5aa7ba3..4c49bb8 100644
--- a/frc971/control_loops/python/polytope.py
+++ b/frc971/control_loops/python/polytope.py
@@ -1,5 +1,4 @@
#!/usr/bin/python3
-
"""
Polyhedral set library.
@@ -9,7 +8,6 @@
__author__ = 'Austin Schuh (austin.linux@gmail.com)'
-
from frc971.control_loops.python import libcdd
import numpy
import string
@@ -17,166 +15,166 @@
def _PiecewiseConcat(*args):
- """Concatenates strings inside lists, elementwise.
+ """Concatenates strings inside lists, elementwise.
Given ['a', 's'] and ['d', 'f'], returns ['ad', 'sf']
"""
- return map(''.join, zip(*args))
+ return map(''.join, zip(*args))
def _SplitAndPad(s):
- """Splits a string on newlines, and pads the lines to be the same width."""
- split_string = s.split('\n')
- width = max(len(stringpiece) for stringpiece in split_string) + 1
+ """Splits a string on newlines, and pads the lines to be the same width."""
+ split_string = s.split('\n')
+ width = max(len(stringpiece) for stringpiece in split_string) + 1
- padded_strings = [stringpiece.ljust(width, ' ')
- for stringpiece in split_string]
- return padded_strings
+ padded_strings = [
+ stringpiece.ljust(width, ' ') for stringpiece in split_string
+ ]
+ return padded_strings
def _PadHeight(padded_array, min_height):
- """Adds lines of spaces to the top and bottom of an array symmetrically."""
- height = len(padded_array)
- if height < min_height:
- pad_array = [' ' * len(padded_array[0])]
- height_error = min_height - height
- return (pad_array * int((height_error) / 2) +
- padded_array +
- pad_array * int((height_error + 1) / 2))
- return padded_array
+ """Adds lines of spaces to the top and bottom of an array symmetrically."""
+ height = len(padded_array)
+ if height < min_height:
+ pad_array = [' ' * len(padded_array[0])]
+ height_error = min_height - height
+ return (pad_array * int(
+ (height_error) / 2) + padded_array + pad_array * int(
+ (height_error + 1) / 2))
+ return padded_array
class HPolytope(object):
- """This object represents a H-polytope.
+ """This object represents a H-polytope.
Polytopes are convex regions in n-dimensional space.
For H-polytopes, this is represented as the intersection of a set of half
planes. The mathematic equation that represents this is H x <= k.
"""
- def __init__(self, H, k):
- """Constructs a H-polytope from the H and k matricies.
+ def __init__(self, H, k):
+ """Constructs a H-polytope from the H and k matricies.
Args:
H: numpy.Matrix (n by k), where n is the number of constraints, and k is
the number of dimensions in the space. Does not copy the matrix.
k: numpy.Matrix (n by 1). Does not copy the matrix.
"""
- self._H = H
- self._k = k
+ self._H = H
+ self._k = k
- @property
- def k(self):
- """Returns the k in H x <= k."""
- return self._k
+ @property
+ def k(self):
+ """Returns the k in H x <= k."""
+ return self._k
- @property
- def H(self):
- """Returns the H in H x <= k."""
- return self._H
+ @property
+ def H(self):
+ """Returns the H in H x <= k."""
+ return self._H
- @property
- def ndim(self):
- """Returns the dimension of the set."""
- return self._H.shape[1]
+ @property
+ def ndim(self):
+ """Returns the dimension of the set."""
+ return self._H.shape[1]
- @property
- def num_constraints(self):
- """Returns the number of constraints defining the set."""
- return self._k.shape[0]
+ @property
+ def num_constraints(self):
+ """Returns the number of constraints defining the set."""
+ return self._k.shape[0]
- def IsInside(self, point):
- """Returns true if the point is inside the polytope, edges included."""
- return (self._H * point <= self._k).all()
+ def IsInside(self, point):
+ """Returns true if the point is inside the polytope, edges included."""
+ return (self._H * point <= self._k).all()
- def Vertices(self):
- """Returns a matrix with the vertices of the set in its rows."""
- # TODO(aschuh): It would be better to write some small C/C++ function that
- # does all of this and takes in a numpy array.
- # The copy is expensive in Python and cheaper in C.
+ def Vertices(self):
+ """Returns a matrix with the vertices of the set in its rows."""
+ # TODO(aschuh): It would be better to write some small C/C++ function that
+ # does all of this and takes in a numpy array.
+ # The copy is expensive in Python and cheaper in C.
- # Create an empty matrix with the correct size.
- matrixptr = libcdd.dd_CreateMatrix(self.num_constraints, self.ndim + 1)
- matrix = matrixptr.contents
-
- try:
- # Copy the data into the matrix.
- for i in range(self.num_constraints):
- libcdd.dd_set_d(matrix.matrix[i][0], self._k[i, 0])
- for j in range(self.ndim):
- libcdd.dd_set_d(matrix.matrix[i][j + 1], -self._H[i, j])
-
- # Set enums to the correct values.
- matrix.representation = libcdd.DD_INEQUALITY
- matrix.numbtype = libcdd.DD_REAL
-
- # TODO(aschuh): Set linearity if it is useful.
- # This would be useful if we had any constraints saying B - A x = 0
-
- # Build a Polyhedra
- polyhedraptr = libcdd.dd_DDMatrix2Poly(matrixptr)
-
- if not polyhedraptr:
- return None
-
- try:
- # Return None on error.
- # The error values are enums, so they aren't exposed.
-
- # Magic happens here. Computes the vertices
- vertex_matrixptr = libcdd.dd_CopyGenerators(
- polyhedraptr)
- vertex_matrix = vertex_matrixptr.contents
+ # Create an empty matrix with the correct size.
+ matrixptr = libcdd.dd_CreateMatrix(self.num_constraints, self.ndim + 1)
+ matrix = matrixptr.contents
try:
- # Count the number of vertices and rays in the result.
- num_vertices = 0
- num_rays = 0
- for i in range(vertex_matrix.rowsize):
- if libcdd.dd_get_d(vertex_matrix.matrix[i][0]) == 0:
- num_rays += 1
- else:
- num_vertices += 1
+ # Copy the data into the matrix.
+ for i in range(self.num_constraints):
+ libcdd.dd_set_d(matrix.matrix[i][0], self._k[i, 0])
+ for j in range(self.ndim):
+ libcdd.dd_set_d(matrix.matrix[i][j + 1], -self._H[i, j])
- # Build zeroed matricies for the new vertices and rays.
- vertices = numpy.matrix(numpy.zeros((num_vertices,
- vertex_matrix.colsize - 1)))
- rays = numpy.matrix(numpy.zeros((num_rays,
- vertex_matrix.colsize - 1)))
+ # Set enums to the correct values.
+ matrix.representation = libcdd.DD_INEQUALITY
+ matrix.numbtype = libcdd.DD_REAL
- ray_index = 0
- vertex_index = 0
+ # TODO(aschuh): Set linearity if it is useful.
+ # This would be useful if we had any constraints saying B - A x = 0
- # Copy the data out of the matrix.
- for index in range(vertex_matrix.rowsize):
- if libcdd.dd_get_d(vertex_matrix.matrix[index][0]) == 0.0:
- for j in range(vertex_matrix.colsize - 1):
- rays[ray_index, j] = libcdd.dd_get_d(
- vertex_matrix.matrix[index][j + 1])
- ray_index += 1
- else:
- for j in range(vertex_matrix.colsize - 1):
- vertices[vertex_index, j] = libcdd.dd_get_d(
- vertex_matrix.matrix[index][j + 1])
- vertex_index += 1
+ # Build a Polyhedra
+ polyhedraptr = libcdd.dd_DDMatrix2Poly(matrixptr)
+
+ if not polyhedraptr:
+ return None
+
+ try:
+ # Return None on error.
+ # The error values are enums, so they aren't exposed.
+
+ # Magic happens here. Computes the vertices
+ vertex_matrixptr = libcdd.dd_CopyGenerators(polyhedraptr)
+ vertex_matrix = vertex_matrixptr.contents
+
+ try:
+ # Count the number of vertices and rays in the result.
+ num_vertices = 0
+ num_rays = 0
+ for i in range(vertex_matrix.rowsize):
+ if libcdd.dd_get_d(vertex_matrix.matrix[i][0]) == 0:
+ num_rays += 1
+ else:
+ num_vertices += 1
+
+ # Build zeroed matricies for the new vertices and rays.
+ vertices = numpy.matrix(
+ numpy.zeros((num_vertices, vertex_matrix.colsize - 1)))
+ rays = numpy.matrix(
+ numpy.zeros((num_rays, vertex_matrix.colsize - 1)))
+
+ ray_index = 0
+ vertex_index = 0
+
+ # Copy the data out of the matrix.
+ for index in range(vertex_matrix.rowsize):
+ if libcdd.dd_get_d(
+ vertex_matrix.matrix[index][0]) == 0.0:
+ for j in range(vertex_matrix.colsize - 1):
+ rays[ray_index, j] = libcdd.dd_get_d(
+ vertex_matrix.matrix[index][j + 1])
+ ray_index += 1
+ else:
+ for j in range(vertex_matrix.colsize - 1):
+ vertices[vertex_index, j] = libcdd.dd_get_d(
+ vertex_matrix.matrix[index][j + 1])
+ vertex_index += 1
+ finally:
+ # Free everything.
+ libcdd.dd_FreeMatrix(vertex_matrixptr)
+
+ finally:
+ libcdd.dd_FreePolyhedra(polyhedraptr)
+
finally:
- # Free everything.
- libcdd.dd_FreeMatrix(vertex_matrixptr)
+ libcdd.dd_FreeMatrix(matrixptr)
- finally:
- libcdd.dd_FreePolyhedra(polyhedraptr)
+ # Rays are unsupported right now. This may change in the future.
+ assert (rays.shape[0] == 0)
- finally:
- libcdd.dd_FreeMatrix(matrixptr)
+ return vertices
- # Rays are unsupported right now. This may change in the future.
- assert(rays.shape[0] == 0)
-
- return vertices
-
-
- def __str__(self):
- """Returns a formatted version of the polytope.
+ def __str__(self):
+ """Returns a formatted version of the polytope.
The dump will look something like the following, which prints out the matrix
comparison.
@@ -186,39 +184,39 @@
[ 0 1] [x1]] [12]
[ 0 -1]] [12]]
"""
- height = max(self.ndim, self.num_constraints)
+ height = max(self.ndim, self.num_constraints)
- # Split the print up into 4 parts and concatenate them all.
- H_strings = _PadHeight(_SplitAndPad(str(self.H)), height)
- v_strings = _PadHeight(_SplitAndPad(str(self.k)), height)
- x_strings = _PadHeight(self._MakeXStrings(), height)
- cmp_strings = self._MakeCmpStrings(height)
+ # Split the print up into 4 parts and concatenate them all.
+ H_strings = _PadHeight(_SplitAndPad(str(self.H)), height)
+ v_strings = _PadHeight(_SplitAndPad(str(self.k)), height)
+ x_strings = _PadHeight(self._MakeXStrings(), height)
+ cmp_strings = self._MakeCmpStrings(height)
- return '\n'.join(_PiecewiseConcat(H_strings, x_strings,
- cmp_strings, v_strings))
+ return '\n'.join(
+ _PiecewiseConcat(H_strings, x_strings, cmp_strings, v_strings))
- def _MakeXStrings(self):
- """Builds an array of strings with constraint names in it for printing."""
- x_strings = []
- if self.ndim == 1:
- x_strings = ["[[x0]] "]
- else:
- for index in range(self.ndim):
- if index == 0:
- x = "[[x%d] " % index
- elif index == self.ndim - 1:
- x = " [x%d]] " % index
+ def _MakeXStrings(self):
+ """Builds an array of strings with constraint names in it for printing."""
+ x_strings = []
+ if self.ndim == 1:
+ x_strings = ["[[x0]] "]
else:
- x = " [x%d] " % index
- x_strings.append(x)
- return x_strings
+ for index in range(self.ndim):
+ if index == 0:
+ x = "[[x%d] " % index
+ elif index == self.ndim - 1:
+ x = " [x%d]] " % index
+ else:
+ x = " [x%d] " % index
+ x_strings.append(x)
+ return x_strings
- def _MakeCmpStrings(self, height):
- """Builds an array of strings with the comparison in it for printing."""
- cmp_strings = []
- for index in range(height):
- if index == int((height - 1) / 2):
- cmp_strings.append("<= ")
- else:
- cmp_strings.append(" ")
- return cmp_strings
+ def _MakeCmpStrings(self, height):
+ """Builds an array of strings with the comparison in it for printing."""
+ cmp_strings = []
+ for index in range(height):
+ if index == int((height - 1) / 2):
+ cmp_strings.append("<= ")
+ else:
+ cmp_strings.append(" ")
+ return cmp_strings
diff --git a/frc971/control_loops/python/polytope_test.py b/frc971/control_loops/python/polytope_test.py
index 68401c2..e5b8d5f 100755
--- a/frc971/control_loops/python/polytope_test.py
+++ b/frc971/control_loops/python/polytope_test.py
@@ -8,21 +8,17 @@
__author__ = 'Austin Schuh (austin.linux@gmail.com)'
+
def MakePoint(*args):
"""Makes a point from a set of arguments."""
return numpy.matrix([[arg] for arg in args])
+
class TestHPolytope(unittest.TestCase):
def setUp(self):
"""Builds a simple box polytope."""
- self.H = numpy.matrix([[1, 0],
- [-1, 0],
- [0, 1],
- [0, -1]])
- self.k = numpy.matrix([[12],
- [12],
- [12],
- [12]])
+ self.H = numpy.matrix([[1, 0], [-1, 0], [0, 1], [0, -1]])
+ self.k = numpy.matrix([[12], [12], [12], [12]])
self.p = polytope.HPolytope(self.H, self.k)
def test_Hk(self):
@@ -33,31 +29,35 @@
def test_IsInside(self):
"""Tests IsInside for various points."""
inside_points = [
- MakePoint(0, 0),
- MakePoint(6, 6),
- MakePoint(12, 6),
- MakePoint(-6, 10)]
+ MakePoint(0, 0),
+ MakePoint(6, 6),
+ MakePoint(12, 6),
+ MakePoint(-6, 10)
+ ]
outside_points = [
- MakePoint(14, 0),
- MakePoint(-14, 0),
- MakePoint(0, 14),
- MakePoint(0, -14),
- MakePoint(14, -14)]
+ MakePoint(14, 0),
+ MakePoint(-14, 0),
+ MakePoint(0, 14),
+ MakePoint(0, -14),
+ MakePoint(14, -14)
+ ]
for inside_point in inside_points:
- self.assertTrue(self.p.IsInside(inside_point),
- msg='Point is' + str(inside_point))
+ self.assertTrue(
+ self.p.IsInside(inside_point),
+ msg='Point is' + str(inside_point))
for outside_point in outside_points:
- self.assertFalse(self.p.IsInside(outside_point),
- msg='Point is' + str(outside_point))
+ self.assertFalse(
+ self.p.IsInside(outside_point),
+ msg='Point is' + str(outside_point))
def AreVertices(self, p, vertices):
"""Checks that all the vertices are on corners of the set."""
for i in range(vertices.shape[0]):
# Check that all the vertices have the correct number of active
# constraints.
- lmda = p.H * vertices[i,:].T - p.k
+ lmda = p.H * vertices[i, :].T - p.k
num_active_constraints = 0
for j in range(lmda.shape[0]):
# Verify that the constraints are either active, or not violated.
@@ -80,54 +80,39 @@
found_points.add(actual_index)
break
- self.assertEqual(len(found_points), actual.shape[0],
+ self.assertEqual(
+ len(found_points),
+ actual.shape[0],
msg="Expected:\n" + str(expected) + "\nActual:\n" + str(actual))
def test_Skewed_Nonsym_Vertices(self):
"""Tests the vertices of a severely skewed space."""
- self.H = numpy.matrix([[10, -1],
- [-1, -1],
- [-1, 10],
- [10, 10]])
- self.k = numpy.matrix([[2],
- [2],
- [2],
- [2]])
+ self.H = numpy.matrix([[10, -1], [-1, -1], [-1, 10], [10, 10]])
+ self.k = numpy.matrix([[2], [2], [2], [2]])
self.p = polytope.HPolytope(self.H, self.k)
vertices = self.p.Vertices()
self.AreVertices(self.p, vertices)
self.HasSamePoints(
- numpy.matrix([[0., 0.2],
- [0.2, 0.],
- [-2., 0.],
- [0., -2.]]),
+ numpy.matrix([[0., 0.2], [0.2, 0.], [-2., 0.], [0., -2.]]),
vertices)
def test_Vertices_Nonsym(self):
"""Tests the vertices of a nonsymetric space."""
- self.k = numpy.matrix([[6],
- [12],
- [2],
- [10]])
+ self.k = numpy.matrix([[6], [12], [2], [10]])
self.p = polytope.HPolytope(self.H, self.k)
vertices = self.p.Vertices()
self.AreVertices(self.p, vertices)
self.HasSamePoints(
- numpy.matrix([[6., 2.],
- [6., -10.],
- [-12., -10.],
- [-12., 2.]]),
+ numpy.matrix([[6., 2.], [6., -10.], [-12., -10.], [-12., 2.]]),
vertices)
def test_Vertices(self):
"""Tests the vertices of a nonsymetric space."""
- self.HasSamePoints(self.p.Vertices(),
- numpy.matrix([[12., 12.],
- [12., -12.],
- [-12., -12.],
- [-12., 12.]]))
+ self.HasSamePoints(
+ self.p.Vertices(),
+ numpy.matrix([[12., 12.], [12., -12.], [-12., -12.], [-12., 12.]]))
def test_concat(self):
"""Tests that the concat function works for simple inputs."""
@@ -138,11 +123,11 @@
def test_str(self):
"""Verifies that the str method works for the provided p."""
- self.assertEqual('[[ 1 0] [[12] \n'
- ' [-1 0] [[x0] <= [12] \n'
- ' [ 0 1] [x1]] [12] \n'
- ' [ 0 -1]] [12]] ',
- str(self.p))
+ self.assertEqual(
+ '[[ 1 0] [[12] \n'
+ ' [-1 0] [[x0] <= [12] \n'
+ ' [ 0 1] [x1]] [12] \n'
+ ' [ 0 -1]] [12]] ', str(self.p))
def MakePWithDims(self, num_constraints, num_dims):
"""Makes a zeroed out polytope with the correct size."""
@@ -153,40 +138,40 @@
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.]] ',
- str(self.p))
+ 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.]] ',
- str(self.p))
+ 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.]] ',
- str(self.p))
+ 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]] ',
- str(self.p))
+ self.assertEqual(
+ '[[0. 0. 0.] [[x0] [[0.] \n'
+ ' [0. 0. 0.]] [x1] <= [0.]] \n'
+ ' [x2]] ', str(self.p))
if __name__ == '__main__':
diff --git a/frc971/control_loops/python/spline.py b/frc971/control_loops/python/spline.py
index 8a0ac04..44f9be6 100644
--- a/frc971/control_loops/python/spline.py
+++ b/frc971/control_loops/python/spline.py
@@ -217,19 +217,20 @@
dddy = numpy.array(dddspline_points)[1, :]
return -1.0 / ((dx**2.0 + dy**2.0)**2.0) * (dx * ddy - dy * ddx) * 2.0 * (
- dy * ddy + dx * ddx) + 1.0 / (dx**2.0 + dy**2.0) * (dx * dddy -
- dy * dddx)
+ dy * ddy + dx * ddx) + 1.0 / (dx**2.0 + dy**2.0) * (
+ dx * dddy - dy * dddx)
class Path(object):
"""Represents a path to follow."""
+
def __init__(self, control_points):
"""Constructs a path given the control points."""
self._control_points = control_points
def spline_velocity(alpha):
- return numpy.linalg.norm(dspline(alpha, self._control_points),
- axis=0)
+ return numpy.linalg.norm(
+ dspline(alpha, self._control_points), axis=0)
self._point_distances = [0.0]
num_alpha = 100
@@ -264,18 +265,16 @@
return 0.0
elif distance >= self.length():
return 1.0
- after_index = numpy.searchsorted(self._point_distances,
- distance,
- side='right')
+ after_index = numpy.searchsorted(
+ self._point_distances, distance, side='right')
before_index = after_index - 1
# Linearly interpolate alpha from our (sorted) distance table.
return (distance - self._point_distances[before_index]) / (
self._point_distances[after_index] -
self._point_distances[before_index]) * (
- 1.0 /
- (len(self._point_distances) - 1.0)) + float(before_index) / (
- len(self._point_distances) - 1.0)
+ 1.0 / (len(self._point_distances) - 1.0)
+ ) + float(before_index) / (len(self._point_distances) - 1.0)
def length(self):
"""Returns the length of the spline (in meters)"""
@@ -289,8 +288,8 @@
# TODO(austin): need a better name...
def dxy(self, distance):
"""Returns the xy velocity as a function of distance."""
- dspline_point = dspline(self.distance_to_alpha(distance),
- self._control_points)
+ dspline_point = dspline(
+ self.distance_to_alpha(distance), self._control_points)
return dspline_point / numpy.linalg.norm(dspline_point, axis=0)
# TODO(austin): need a better name...
@@ -303,16 +302,16 @@
norm = numpy.linalg.norm(dspline_points, axis=0)**2.0
return ddspline_points / norm - numpy.multiply(
- dspline_points, (numpy.array(dspline_points)[0, :] *
- numpy.array(ddspline_points)[0, :] +
- numpy.array(dspline_points)[1, :] *
+ dspline_points, (numpy.array(dspline_points)[0, :] * numpy.array(
+ ddspline_points)[0, :] + numpy.array(dspline_points)[1, :] *
numpy.array(ddspline_points)[1, :]) / (norm**2.0))
def theta(self, distance, dspline_points=None):
"""Returns the heading as a function of distance."""
- return spline_theta(self.distance_to_alpha(distance),
- self._control_points,
- dspline_points=dspline_points)
+ return spline_theta(
+ self.distance_to_alpha(distance),
+ self._control_points,
+ dspline_points=dspline_points)
def dtheta(self, distance, dspline_points=None, ddspline_points=None):
"""Returns the angular velocity as a function of distance."""
@@ -359,12 +358,12 @@
# TODO(austin): Factor out the d^alpha/dd^2.
return ddtheta_points / numpy.linalg.norm(
dspline_points, axis=0)**2.0 - numpy.multiply(
- dtheta_points, (numpy.array(dspline_points)[0, :] *
- numpy.array(ddspline_points)[0, :] +
- numpy.array(dspline_points)[1, :] *
- numpy.array(ddspline_points)[1, :]) /
- ((numpy.array(dspline_points)[0, :]**2.0 +
- numpy.array(dspline_points)[1, :]**2.0)**2.0))
+ dtheta_points,
+ (numpy.array(dspline_points)[0, :] * numpy.array(
+ ddspline_points)[0, :] + numpy.array(dspline_points)[1, :]
+ * numpy.array(ddspline_points)[1, :]) / (
+ (numpy.array(dspline_points)[0, :]**2.0 +
+ numpy.array(dspline_points)[1, :]**2.0)**2.0))
def integrate_accel_for_distance(f, v, x, dx):
@@ -428,8 +427,8 @@
return plan
def lateral_velocity_curvature(self, distance):
- return numpy.sqrt(self._lateral_accel /
- numpy.linalg.norm(self._path.ddxy(distance)))
+ return numpy.sqrt(
+ self._lateral_accel / numpy.linalg.norm(self._path.ddxy(distance)))
def lateral_accel_pass(self, plan):
plan = plan.copy()
@@ -553,9 +552,8 @@
if distance > self.distances[-1]:
distance = self.distances[-1]
else:
- after_index = numpy.searchsorted(self.distances,
- distance,
- side='right')
+ after_index = numpy.searchsorted(
+ self.distances, distance, side='right')
before_index = after_index - 1
vforwards = integrate_accel_for_distance(
@@ -594,8 +592,8 @@
return U
def goal_state(self, distance, velocity):
- width = (self._drivetrain.robot_radius_l +
- self._drivetrain.robot_radius_r)
+ width = (
+ self._drivetrain.robot_radius_l + self._drivetrain.robot_radius_r)
goal = numpy.matrix(numpy.zeros((5, 1)))
goal[0:2, :] = self._path.xy(distance)
@@ -627,23 +625,25 @@
# Compute theta and the two derivatives
theta = spline_theta(alphas, control_points, dspline_points=dspline_points)
- dtheta = dspline_theta(alphas,
- control_points,
- dspline_points=dspline_points)
- ddtheta = ddspline_theta(alphas,
- control_points,
- dspline_points=dspline_points,
- dddspline_points=dddspline_points)
+ dtheta = dspline_theta(
+ alphas, control_points, dspline_points=dspline_points)
+ ddtheta = ddspline_theta(
+ alphas,
+ control_points,
+ dspline_points=dspline_points,
+ dddspline_points=dddspline_points)
# Plot the control points and the spline.
pylab.figure()
- pylab.plot(numpy.array(control_points)[0, :],
- numpy.array(control_points)[1, :],
- '-o',
- label='control')
- pylab.plot(numpy.array(spline_points)[0, :],
- numpy.array(spline_points)[1, :],
- label='spline')
+ pylab.plot(
+ numpy.array(control_points)[0, :],
+ numpy.array(control_points)[1, :],
+ '-o',
+ label='control')
+ pylab.plot(
+ numpy.array(spline_points)[0, :],
+ numpy.array(spline_points)[1, :],
+ label='spline')
pylab.legend()
# For grins, confirm that the double integral of the acceleration (with
@@ -715,9 +715,10 @@
pylab.plot(alphas, ddtheta, label='ddtheta')
pylab.plot(alphas, thetaint_plot, label='thetai')
pylab.plot(alphas, dthetaint_plot, label='dthetai')
- pylab.plot(alphas,
- numpy.linalg.norm(numpy.array(dspline_points), axis=0),
- label='velocity')
+ pylab.plot(
+ alphas,
+ numpy.linalg.norm(numpy.array(dspline_points), axis=0),
+ label='velocity')
# Now, repeat as a function of path length as opposed to alpha
path = Path(control_points)
@@ -784,11 +785,12 @@
longitudal_accel = 3.0
lateral_accel = 2.0
- trajectory = Trajectory(path,
- drivetrain=velocity_drivetrain,
- longitudal_accel=longitudal_accel,
- lateral_accel=lateral_accel,
- distance_count=500)
+ trajectory = Trajectory(
+ path,
+ drivetrain=velocity_drivetrain,
+ longitudal_accel=longitudal_accel,
+ lateral_accel=lateral_accel,
+ distance_count=500)
vmax = numpy.inf
vmax = 10.0
@@ -884,8 +886,8 @@
state_plan[:, i] = numpy.matrix([[x_r], [y_r], [theta_r],
[vel_lr[0, 0]], [vel_lr[1, 0]]])
- u_plan[:, i] = trajectory.ff_voltage(backward_accel_plan, xva_plan[0,
- i])
+ u_plan[:, i] = trajectory.ff_voltage(backward_accel_plan,
+ xva_plan[0, i])
Q = numpy.matrix(
numpy.diag([
@@ -937,8 +939,8 @@
goal_state = trajectory.goal_state(xva_plan[0, i], xva_plan[1, i])
state_error = goal_state - state
- U = (trajectory.ff_voltage(backward_accel_plan, xva_plan[0, i]) + K *
- (state_error))
+ U = (trajectory.ff_voltage(backward_accel_plan, xva_plan[0, i]) +
+ K * (state_error))
def spline_diffeq(U, t, x):
velocity = x[3:5, :]
@@ -970,12 +972,12 @@
pylab.legend()
pylab.figure()
- pylab.plot(numpy.array(states)[0, :],
- numpy.array(states)[1, :],
- label='robot')
- pylab.plot(numpy.array(spline_points)[0, :],
- numpy.array(spline_points)[1, :],
- label='spline')
+ pylab.plot(
+ numpy.array(states)[0, :], numpy.array(states)[1, :], label='robot')
+ pylab.plot(
+ numpy.array(spline_points)[0, :],
+ numpy.array(spline_points)[1, :],
+ label='spline')
pylab.legend()
def a(_, x):
diff --git a/frc971/control_loops/python/spline_graph.py b/frc971/control_loops/python/spline_graph.py
index f6e3a0c..6303769 100755
--- a/frc971/control_loops/python/spline_graph.py
+++ b/frc971/control_loops/python/spline_graph.py
@@ -7,6 +7,7 @@
import cairo
import basic_window
+
class GridWindow(Gtk.Window):
def method_connect(self, event, cb):
def handler(obj, *args):