Run yapf on all python files in the repo
Signed-off-by: Ravago Jones <ravagojones@gmail.com>
Change-Id: I221e04c3f517fab8535b22551553799e0fee7a80
diff --git a/y2014/control_loops/python/claw.py b/y2014/control_loops/python/claw.py
index 77ccf14..488d792 100755
--- a/y2014/control_loops/python/claw.py
+++ b/y2014/control_loops/python/claw.py
@@ -51,8 +51,8 @@
# Input is [bottom power, top power - bottom power * J_top / J_bottom]
# Motor time constants. difference_bottom refers to the constant for how the
# bottom velocity affects the difference of the top and bottom velocities.
- self.common_motor_constant = -self.Kt / self.Kv / (
- self.G * self.G * self.R)
+ self.common_motor_constant = -self.Kt / self.Kv / (self.G * self.G *
+ self.R)
self.bottom_bottom = self.common_motor_constant / self.J_bottom
self.difference_bottom = -self.common_motor_constant * (
1 / self.J_bottom - 1 / self.J_top)
@@ -96,7 +96,8 @@
self.B_continuous, self.dt)
self.A_bottom, self.B_bottom = controls.c2d(self.A_bottom_cont,
- self.B_bottom_cont, self.dt)
+ self.B_bottom_cont,
+ self.dt)
self.A_diff, self.B_diff = controls.c2d(self.A_diff_cont,
self.B_diff_cont, self.dt)
@@ -135,12 +136,12 @@
glog.debug(str(lstsq_A))
glog.debug('det %s', str(numpy.linalg.det(lstsq_A)))
- out_x = numpy.linalg.lstsq(
- lstsq_A,
- numpy.matrix([[self.A[1, 2]], [self.A[3, 2]]]),
- rcond=None)[0]
- self.K[1, 2] = -lstsq_A[0, 0] * (
- self.K[0, 2] - out_x[0]) / lstsq_A[0, 1] + out_x[1]
+ out_x = numpy.linalg.lstsq(lstsq_A,
+ numpy.matrix([[self.A[1, 2]],
+ [self.A[3, 2]]]),
+ rcond=None)[0]
+ self.K[1, 2] = -lstsq_A[0, 0] * (self.K[0, 2] -
+ out_x[0]) / lstsq_A[0, 1] + out_x[1]
glog.debug('K unaugmented')
glog.debug(str(self.K))
@@ -181,8 +182,9 @@
X_ss[2, 0] = 1 / (1 - A[2, 2]) * B[2, 0] * U[0, 0]
#X_ss[3, 0] = X_ss[3, 0] * A[3, 3] + X_ss[2, 0] * A[3, 2] + B[3, 0] * U[0, 0] + B[3, 1] * U[1, 0]
#X_ss[3, 0] * (1 - A[3, 3]) = X_ss[2, 0] * A[3, 2] + B[3, 0] * U[0, 0] + B[3, 1] * U[1, 0]
- X_ss[3, 0] = 1 / (1 - A[3, 3]) * (
- X_ss[2, 0] * A[3, 2] + B[3, 1] * U[1, 0] + B[3, 0] * U[0, 0])
+ X_ss[3,
+ 0] = 1 / (1 - A[3, 3]) * (X_ss[2, 0] * A[3, 2] +
+ B[3, 1] * U[1, 0] + B[3, 0] * U[0, 0])
#X_ss[3, 0] = 1 / (1 - A[3, 3]) / (1 - A[2, 2]) * B[2, 0] * U[0, 0] * A[3, 2] + B[3, 0] * U[0, 0] + B[3, 1] * U[1, 0]
X_ss[0, 0] = A[0, 2] * X_ss[2, 0] + B[0, 0] * U[0, 0]
X_ss[1, 0] = (A[1, 2] * X_ss[2, 0]) + (A[1, 3] * X_ss[3, 0]) + (
@@ -247,7 +249,8 @@
self.rpl = .05
self.ipl = 0.008
self.PlaceObserverPoles([
- self.rpl + 1j * self.ipl, 0.10, 0.09, self.rpl - 1j * self.ipl, 0.90
+ self.rpl + 1j * self.ipl, 0.10, 0.09, self.rpl - 1j * self.ipl,
+ 0.90
])
#print "A is"
#print self.A
@@ -284,8 +287,8 @@
numpy.matrix([[1, 0], [-1, 0], [0, 1], [0, -1]]),
numpy.matrix([[12], [12], [12], [12]]))
- if (bottom_u > claw.U_max[0, 0] or bottom_u < claw.U_min[0, 0] or
- top_u > claw.U_max[0, 0] or top_u < claw.U_min[0, 0]):
+ if (bottom_u > claw.U_max[0, 0] or bottom_u < claw.U_min[0, 0]
+ or top_u > claw.U_max[0, 0] or top_u < claw.U_min[0, 0]):
position_K = K[:, 0:2]
velocity_K = K[:, 2:]
@@ -501,8 +504,8 @@
else:
namespaces = ['y2014', 'control_loops', 'claw']
claw = Claw('Claw')
- loop_writer = control_loop.ControlLoopWriter(
- 'Claw', [claw], namespaces=namespaces)
+ loop_writer = control_loop.ControlLoopWriter('Claw', [claw],
+ namespaces=namespaces)
loop_writer.AddConstant(
control_loop.Constant('kClawMomentOfInertiaRatio', '%f',
claw.J_top / claw.J_bottom))
diff --git a/y2014/control_loops/python/drivetrain.py b/y2014/control_loops/python/drivetrain.py
index cac236f..d72c222 100755
--- a/y2014/control_loops/python/drivetrain.py
+++ b/y2014/control_loops/python/drivetrain.py
@@ -11,30 +11,32 @@
gflags.DEFINE_bool('plot', False, 'If true, plot the loop response.')
-kDrivetrain = drivetrain.DrivetrainParams(J = 2.8,
- mass = 68,
- robot_radius = 0.647998644 / 2.0,
- wheel_radius = .04445,
- G_high = 28.0 / 50.0 * 18.0 / 50.0,
- G_low = 18.0 / 60.0 * 18.0 / 50.0,
- q_pos_low = 0.12,
- q_pos_high = 0.12,
- q_vel_low = 1.0,
- q_vel_high = 1.0,
- has_imu = False,
- dt = 0.005,
- controller_poles = [0.7, 0.7])
+kDrivetrain = drivetrain.DrivetrainParams(J=2.8,
+ mass=68,
+ robot_radius=0.647998644 / 2.0,
+ wheel_radius=.04445,
+ G_high=28.0 / 50.0 * 18.0 / 50.0,
+ G_low=18.0 / 60.0 * 18.0 / 50.0,
+ q_pos_low=0.12,
+ q_pos_high=0.12,
+ q_vel_low=1.0,
+ q_vel_high=1.0,
+ has_imu=False,
+ dt=0.005,
+ controller_poles=[0.7, 0.7])
+
def main(argv):
- argv = FLAGS(argv)
+ argv = FLAGS(argv)
- if FLAGS.plot:
- drivetrain.PlotDrivetrainMotions(kDrivetrain)
- elif len(argv) != 5:
- print("Expected .h file name and .cc file name")
- else:
- # Write the generated constants out to a file.
- drivetrain.WriteDrivetrain(argv[1:3], argv[3:5], 'y2014', kDrivetrain)
+ if FLAGS.plot:
+ drivetrain.PlotDrivetrainMotions(kDrivetrain)
+ elif len(argv) != 5:
+ print("Expected .h file name and .cc file name")
+ else:
+ # Write the generated constants out to a file.
+ drivetrain.WriteDrivetrain(argv[1:3], argv[3:5], 'y2014', kDrivetrain)
+
if __name__ == '__main__':
- sys.exit(main(sys.argv))
+ sys.exit(main(sys.argv))
diff --git a/y2014/control_loops/python/dt_mpc.py b/y2014/control_loops/python/dt_mpc.py
index 0c229c1..2f13807 100755
--- a/y2014/control_loops/python/dt_mpc.py
+++ b/y2014/control_loops/python/dt_mpc.py
@@ -13,8 +13,9 @@
#
# Inital algorithm from http://www.ece.ufrgs.br/~fetter/icma05_608.pdf
+
def cartesian_to_polar(X_cartesian):
- """Converts a cartesian coordinate to polar.
+ """Converts a cartesian coordinate to polar.
Args:
X_cartesian, numpy.matrix[3, 1] with x, y, theta as rows.
@@ -22,13 +23,13 @@
Returns:
numpy.matrix[3, 1] with e, phi, alpha as rows.
"""
- phi = numpy.arctan2(X_cartesian[1, 0], X_cartesian[0, 0])
- return numpy.matrix([[numpy.hypot(X_cartesian[0, 0], X_cartesian[1, 0])],
- [phi],
- [X_cartesian[2, 0] - phi]])
+ phi = numpy.arctan2(X_cartesian[1, 0], X_cartesian[0, 0])
+ return numpy.matrix([[numpy.hypot(X_cartesian[0, 0], X_cartesian[1, 0])],
+ [phi], [X_cartesian[2, 0] - phi]])
+
def polar_to_cartesian(X_polar):
- """Converts a polar coordinate to cartesian.
+ """Converts a polar coordinate to cartesian.
Args:
X_polar, numpy.matrix[3, 1] with e, phi, alpha as rows.
@@ -36,12 +37,13 @@
Returns:
numpy.matrix[3, 1] with x, y, theta as rows.
"""
- return numpy.matrix([[X_polar[0, 0] * numpy.cos(X_polar[1, 0])],
- [X_polar[0, 0] * numpy.sin(X_polar[1, 0])],
- [X_polar[1, 0] + X_polar[2, 0]]])
+ return numpy.matrix([[X_polar[0, 0] * numpy.cos(X_polar[1, 0])],
+ [X_polar[0, 0] * numpy.sin(X_polar[1, 0])],
+ [X_polar[1, 0] + X_polar[2, 0]]])
+
def simulate_dynamics(X_cartesian, U):
- """Calculates the robot location after 1 timestep.
+ """Calculates the robot location after 1 timestep.
Args:
X_cartesian, numpy.matrix[3, 1] with the starting location in
@@ -51,15 +53,15 @@
Returns:
numpy.matrix[3, 1] with the cartesian coordinate.
"""
- X_cartesian += numpy.matrix(
- [[U[0, 0] * numpy.cos(X_cartesian[2, 0]) * dt],
- [U[0, 0] * numpy.sin(X_cartesian[2, 0]) * dt],
- [U[1, 0] * dt]])
+ X_cartesian += numpy.matrix([[U[0, 0] * numpy.cos(X_cartesian[2, 0]) * dt],
+ [U[0, 0] * numpy.sin(X_cartesian[2, 0]) * dt],
+ [U[1, 0] * dt]])
- return X_cartesian
+ return X_cartesian
+
def U_from_array(U_array):
- """Converts the U array from the optimizer to a bunch of column vectors.
+ """Converts the U array from the optimizer to a bunch of column vectors.
Args:
U_array, numpy.array[N] The U coordinates in v, av, v, av, ...
@@ -67,10 +69,11 @@
Returns:
numpy.matrix[2, N/2] with [[v, v, v, ...], [av, av, av, ...]]
"""
- return numpy.matrix(U_array).reshape((2, -1), order='F')
+ return numpy.matrix(U_array).reshape((2, -1), order='F')
+
def U_to_array(U_matrix):
- """Converts the U matrix to the U array for the optimizer.
+ """Converts the U matrix to the U array for the optimizer.
Args:
U_matrix, numpy.matrix[2, N/2] with [[v, v, v, ...], [av, av, av, ...]]
@@ -78,10 +81,11 @@
Returns:
numpy.array[N] The U coordinates in v, av, v, av, ...
"""
- return numpy.array(U_matrix.reshape((1, -1), order='F'))
+ return numpy.array(U_matrix.reshape((1, -1), order='F'))
+
def cost(U_array, X_cartesian):
- """Computes the cost given the inital position and the U array.
+ """Computes the cost given the inital position and the U array.
Args:
U_array: numpy.array[N] The U coordinates.
@@ -91,91 +95,93 @@
Returns:
double, The quadratic cost of evaluating U.
"""
- X_cartesian_mod = X_cartesian.copy()
- U_matrix = U_from_array(U_array)
- my_cost = 0
- Q = numpy.matrix([[0.01, 0, 0],
- [0, 0.01, 0],
- [0, 0, 0.01]]) / dt / dt
- R = numpy.matrix([[0.001, 0],
- [0, 0.001]]) / dt / dt
- for U in U_matrix.T:
- # TODO(austin): Let it go to the point from either side.
- U = U.T
- X_cartesian_mod = simulate_dynamics(X_cartesian_mod, U)
- X_current_polar = cartesian_to_polar(X_cartesian_mod)
- my_cost += U.T * R * U + X_current_polar.T * Q * X_current_polar
+ X_cartesian_mod = X_cartesian.copy()
+ U_matrix = U_from_array(U_array)
+ my_cost = 0
+ Q = numpy.matrix([[0.01, 0, 0], [0, 0.01, 0], [0, 0, 0.01]]) / dt / dt
+ R = numpy.matrix([[0.001, 0], [0, 0.001]]) / dt / dt
+ for U in U_matrix.T:
+ # TODO(austin): Let it go to the point from either side.
+ U = U.T
+ X_cartesian_mod = simulate_dynamics(X_cartesian_mod, U)
+ X_current_polar = cartesian_to_polar(X_cartesian_mod)
+ my_cost += U.T * R * U + X_current_polar.T * Q * X_current_polar
- return my_cost
+ return my_cost
+
if __name__ == '__main__':
- X_cartesian = numpy.matrix([[-1.0],
- [-1.0],
- [0.0]])
- x_array = []
- y_array = []
- theta_array = []
+ X_cartesian = numpy.matrix([[-1.0], [-1.0], [0.0]])
+ x_array = []
+ y_array = []
+ theta_array = []
- e_array = []
- phi_array = []
- alpha_array = []
+ e_array = []
+ phi_array = []
+ alpha_array = []
- cost_array = []
+ cost_array = []
- time_array = []
- u0_array = []
- u1_array = []
+ time_array = []
+ u0_array = []
+ u1_array = []
- num_steps = 20
+ num_steps = 20
- U_array = numpy.zeros((num_steps * 2))
- for i in range(400):
- print('Iteration', i)
- # Solve the NMPC
- U_array, fx, _, _, _ = scipy.optimize.fmin_slsqp(
- cost, U_array.copy(), bounds = [(-1, 1), (-7, 7)] * num_steps,
- args=(X_cartesian,), iter=500, full_output=True)
- U_matrix = U_from_array(U_array)
+ U_array = numpy.zeros((num_steps * 2))
+ for i in range(400):
+ print('Iteration', i)
+ # Solve the NMPC
+ U_array, fx, _, _, _ = scipy.optimize.fmin_slsqp(cost,
+ U_array.copy(),
+ bounds=[(-1, 1),
+ (-7, 7)] *
+ num_steps,
+ args=(X_cartesian, ),
+ iter=500,
+ full_output=True)
+ U_matrix = U_from_array(U_array)
- # Simulate the dynamics
- X_cartesian = simulate_dynamics(X_cartesian, U_matrix[:, 0])
+ # Simulate the dynamics
+ X_cartesian = simulate_dynamics(X_cartesian, U_matrix[:, 0])
- # Save variables for plotting.
- cost_array.append(fx[0, 0])
- u0_array.append(U_matrix[0, 0])
- u1_array.append(U_matrix[1, 0])
- x_array.append(X_cartesian[0, 0])
- y_array.append(X_cartesian[1, 0])
- theta_array.append(X_cartesian[2, 0])
- time_array.append(i * dt)
- X_polar = cartesian_to_polar(X_cartesian)
- e_array.append(X_polar[0, 0])
- phi_array.append(X_polar[1, 0])
- alpha_array.append(X_polar[2, 0])
+ # Save variables for plotting.
+ cost_array.append(fx[0, 0])
+ u0_array.append(U_matrix[0, 0])
+ u1_array.append(U_matrix[1, 0])
+ x_array.append(X_cartesian[0, 0])
+ y_array.append(X_cartesian[1, 0])
+ theta_array.append(X_cartesian[2, 0])
+ time_array.append(i * dt)
+ X_polar = cartesian_to_polar(X_cartesian)
+ e_array.append(X_polar[0, 0])
+ phi_array.append(X_polar[1, 0])
+ alpha_array.append(X_polar[2, 0])
- U_array = U_to_array(numpy.hstack((U_matrix[:, 1:], numpy.matrix([[0], [0]]))))
+ U_array = U_to_array(
+ numpy.hstack((U_matrix[:, 1:], numpy.matrix([[0], [0]]))))
- if fx < 1e-05:
- print('Cost is', fx, 'after', i, 'cycles, finishing early')
- break
+ if fx < 1e-05:
+ print('Cost is', fx, 'after', i, 'cycles, finishing early')
+ break
- # Plot
- pylab.figure('xy')
- pylab.plot(x_array, y_array, label = 'trajectory')
+ # Plot
+ pylab.figure('xy')
+ pylab.plot(x_array, y_array, label='trajectory')
- pylab.figure('time')
- pylab.plot(time_array, x_array, label='x')
- pylab.plot(time_array, y_array, label='y')
- pylab.plot(time_array, theta_array, label = 'theta')
- pylab.plot(time_array, e_array, label='e')
- pylab.plot(time_array, phi_array, label='phi')
- pylab.plot(time_array, alpha_array, label='alpha')
- pylab.plot(time_array, cost_array, label='cost')
- pylab.legend()
+ pylab.figure('time')
+ pylab.plot(time_array, x_array, label='x')
+ pylab.plot(time_array, y_array, label='y')
+ pylab.plot(time_array, theta_array, label='theta')
+ pylab.plot(time_array, e_array, label='e')
+ pylab.plot(time_array, phi_array, label='phi')
+ pylab.plot(time_array, alpha_array, label='alpha')
+ pylab.plot(time_array, cost_array, label='cost')
+ pylab.legend()
- pylab.figure('u')
- pylab.plot(time_array, u0_array, label='u0')
- pylab.plot(time_array, u1_array, label='u1')
- pylab.legend()
+ pylab.figure('u')
+ pylab.plot(time_array, u0_array, label='u0')
+ pylab.plot(time_array, u1_array, label='u1')
+ pylab.legend()
- pylab.show()
+ pylab.show()
diff --git a/y2014/control_loops/python/extended_lqr.py b/y2014/control_loops/python/extended_lqr.py
index b3f2372..699dd30 100755
--- a/y2014/control_loops/python/extended_lqr.py
+++ b/y2014/control_loops/python/extended_lqr.py
@@ -17,8 +17,9 @@
num_inputs = 2
x_hat_initial = numpy.matrix([[0.10], [1.0], [0.0]])
+
def dynamics(X, U):
- """Calculates the dynamics for a 2 wheeled robot.
+ """Calculates the dynamics for a 2 wheeled robot.
Args:
X, numpy.matrix(3, 1), The state. [x, y, theta]
@@ -27,29 +28,33 @@
Returns:
numpy.matrix(3, 1), The derivative of the dynamics.
"""
- #return numpy.matrix([[X[1, 0]],
- # [X[2, 0]],
- # [U[0, 0]]])
- return numpy.matrix([[(U[0, 0] + U[1, 0]) * numpy.cos(X[2, 0]) / 2.0],
- [(U[0, 0] + U[1, 0]) * numpy.sin(X[2, 0]) / 2.0],
- [(U[1, 0] - U[0, 0]) / width]])
+ #return numpy.matrix([[X[1, 0]],
+ # [X[2, 0]],
+ # [U[0, 0]]])
+ return numpy.matrix([[(U[0, 0] + U[1, 0]) * numpy.cos(X[2, 0]) / 2.0],
+ [(U[0, 0] + U[1, 0]) * numpy.sin(X[2, 0]) / 2.0],
+ [(U[1, 0] - U[0, 0]) / width]])
+
def RungeKutta(f, x, dt):
- """4th order RungeKutta integration of F starting at X."""
- a = f(x)
- b = f(x + dt / 2.0 * a)
- c = f(x + dt / 2.0 * b)
- d = f(x + dt * c)
- return x + dt * (a + 2.0 * b + 2.0 * c + d) / 6.0
+ """4th order RungeKutta integration of F starting at X."""
+ a = f(x)
+ b = f(x + dt / 2.0 * a)
+ c = f(x + dt / 2.0 * b)
+ d = f(x + dt * c)
+ return x + dt * (a + 2.0 * b + 2.0 * c + d) / 6.0
+
def discrete_dynamics(X, U):
- return RungeKutta(lambda startingX: dynamics(startingX, U), X, dt)
+ return RungeKutta(lambda startingX: dynamics(startingX, U), X, dt)
+
def inverse_discrete_dynamics(X, U):
- return RungeKutta(lambda startingX: -dynamics(startingX, U), X, dt)
+ return RungeKutta(lambda startingX: -dynamics(startingX, U), X, dt)
+
def numerical_jacobian_x(fn, X, U, epsilon=1e-4):
- """Numerically estimates the jacobian around X, U in X.
+ """Numerically estimates the jacobian around X, U in X.
Args:
fn: A function of X, U.
@@ -62,20 +67,21 @@
numpy.matrix(num_states, num_states), The jacobian of fn with X as the
variable.
"""
- num_states = X.shape[0]
- nominal = fn(X, U)
- answer = numpy.matrix(numpy.zeros((nominal.shape[0], num_states)))
- # It's more expensive, but +- epsilon will be more reliable
- for i in range(0, num_states):
- dX_plus = X.copy()
- dX_plus[i] += epsilon
- dX_minus = X.copy()
- dX_minus[i] -= epsilon
- answer[:, i] = (fn(dX_plus, U) - fn(dX_minus, U)) / epsilon / 2.0
- return answer
+ num_states = X.shape[0]
+ nominal = fn(X, U)
+ answer = numpy.matrix(numpy.zeros((nominal.shape[0], num_states)))
+ # It's more expensive, but +- epsilon will be more reliable
+ for i in range(0, num_states):
+ dX_plus = X.copy()
+ dX_plus[i] += epsilon
+ dX_minus = X.copy()
+ dX_minus[i] -= epsilon
+ answer[:, i] = (fn(dX_plus, U) - fn(dX_minus, U)) / epsilon / 2.0
+ return answer
+
def numerical_jacobian_u(fn, X, U, epsilon=1e-4):
- """Numerically estimates the jacobian around X, U in U.
+ """Numerically estimates the jacobian around X, U in U.
Args:
fn: A function of X, U.
@@ -88,48 +94,56 @@
numpy.matrix(num_states, num_inputs), The jacobian of fn with U as the
variable.
"""
- num_states = X.shape[0]
- num_inputs = U.shape[0]
- nominal = fn(X, U)
- answer = numpy.matrix(numpy.zeros((nominal.shape[0], num_inputs)))
- for i in range(0, num_inputs):
- dU_plus = U.copy()
- dU_plus[i] += epsilon
- dU_minus = U.copy()
- dU_minus[i] -= epsilon
- answer[:, i] = (fn(X, dU_plus) - fn(X, dU_minus)) / epsilon / 2.0
- return answer
+ num_states = X.shape[0]
+ num_inputs = U.shape[0]
+ nominal = fn(X, U)
+ answer = numpy.matrix(numpy.zeros((nominal.shape[0], num_inputs)))
+ for i in range(0, num_inputs):
+ dU_plus = U.copy()
+ dU_plus[i] += epsilon
+ dU_minus = U.copy()
+ dU_minus[i] -= epsilon
+ answer[:, i] = (fn(X, dU_plus) - fn(X, dU_minus)) / epsilon / 2.0
+ return answer
+
def numerical_jacobian_x_x(fn, X, U):
- return numerical_jacobian_x(
- lambda X_inner, U_inner: numerical_jacobian_x(fn, X_inner, U_inner).T, X, U)
+ return numerical_jacobian_x(
+ lambda X_inner, U_inner: numerical_jacobian_x(fn, X_inner, U_inner).T,
+ X, U)
+
def numerical_jacobian_x_u(fn, X, U):
- return numerical_jacobian_x(
- lambda X_inner, U_inner: numerical_jacobian_u(fn, X_inner, U_inner).T, X, U)
+ return numerical_jacobian_x(
+ lambda X_inner, U_inner: numerical_jacobian_u(fn, X_inner, U_inner).T,
+ X, U)
+
def numerical_jacobian_u_x(fn, X, U):
- return numerical_jacobian_u(
- lambda X_inner, U_inner: numerical_jacobian_x(fn, X_inner, U_inner).T, X, U)
+ return numerical_jacobian_u(
+ lambda X_inner, U_inner: numerical_jacobian_x(fn, X_inner, U_inner).T,
+ X, U)
+
def numerical_jacobian_u_u(fn, X, U):
- return numerical_jacobian_u(
- lambda X_inner, U_inner: numerical_jacobian_u(fn, X_inner, U_inner).T, X, U)
+ return numerical_jacobian_u(
+ lambda X_inner, U_inner: numerical_jacobian_u(fn, X_inner, U_inner).T,
+ X, U)
+
# Simple implementation for a quadratic cost function.
class CostFunction:
- def __init__(self):
- self.num_states = num_states
- self.num_inputs = num_inputs
- self.dt = dt
- self.Q = numpy.matrix([[0.1, 0, 0],
- [0, 0.6, 0],
- [0, 0, 0.1]]) / dt / dt
- self.R = numpy.matrix([[0.40, 0],
- [0, 0.40]]) / dt / dt
- def estimate_Q_final(self, X_hat):
- """Returns the quadraticized final Q around X_hat.
+ def __init__(self):
+ self.num_states = num_states
+ self.num_inputs = num_inputs
+ self.dt = dt
+ self.Q = numpy.matrix([[0.1, 0, 0], [0, 0.6, 0], [0, 0, 0.1]
+ ]) / dt / dt
+ self.R = numpy.matrix([[0.40, 0], [0, 0.40]]) / dt / dt
+
+ def estimate_Q_final(self, X_hat):
+ """Returns the quadraticized final Q around X_hat.
This is calculated by evaluating partial^2 cost(X_hat) / (partial X * partial X)
@@ -139,11 +153,11 @@
Result:
numpy.matrix(self.num_states, self.num_states)
"""
- zero_U = numpy.matrix(numpy.zeros((self.num_inputs, 1)))
- return numerical_jacobian_x_x(self.final_cost, X_hat, zero_U)
+ zero_U = numpy.matrix(numpy.zeros((self.num_inputs, 1)))
+ return numerical_jacobian_x_x(self.final_cost, X_hat, zero_U)
- def estimate_partial_cost_partial_x_final(self, X_hat):
- """Returns \frac{\partial cost}{\partial X}(X_hat) for the final cost.
+ def estimate_partial_cost_partial_x_final(self, X_hat):
+ """Returns \frac{\partial cost}{\partial X}(X_hat) for the final cost.
Args:
X_hat: numpy.matrix(self.num_states, 1), The state to quadraticize around.
@@ -151,14 +165,17 @@
Result:
numpy.matrix(self.num_states, 1)
"""
- return numerical_jacobian_x(self.final_cost, X_hat, numpy.matrix(numpy.zeros((self.num_inputs, 1)))).T
+ return numerical_jacobian_x(
+ self.final_cost, X_hat,
+ numpy.matrix(numpy.zeros((self.num_inputs, 1)))).T
- def estimate_q_final(self, X_hat):
- """Returns q evaluated at X_hat for the final cost function."""
- return self.estimate_partial_cost_partial_x_final(X_hat) - self.estimate_Q_final(X_hat) * X_hat
+ def estimate_q_final(self, X_hat):
+ """Returns q evaluated at X_hat for the final cost function."""
+ return self.estimate_partial_cost_partial_x_final(
+ X_hat) - self.estimate_Q_final(X_hat) * X_hat
- def final_cost(self, X, U):
- """Computes the final cost of being at X
+ def final_cost(self, X, U):
+ """Computes the final cost of being at X
Args:
X: numpy.matrix(self.num_states, 1)
@@ -167,10 +184,10 @@
Returns:
numpy.matrix(1, 1), The quadratic cost of being at X
"""
- return X.T * self.Q * X * 1000
+ return X.T * self.Q * X * 1000
- def cost(self, X, U):
- """Computes the incremental cost given a position and U.
+ def cost(self, X, U):
+ """Computes the incremental cost given a position and U.
Args:
X: numpy.matrix(self.num_states, 1)
@@ -179,250 +196,334 @@
Returns:
numpy.matrix(1, 1), The quadratic cost of evaluating U.
"""
- return U.T * self.R * U + X.T * self.Q * X
+ return U.T * self.R * U + X.T * self.Q * X
+
cost_fn_obj = CostFunction()
-S_bar_t = [numpy.matrix(numpy.zeros((num_states, num_states))) for _ in range(l + 1)]
+S_bar_t = [
+ numpy.matrix(numpy.zeros((num_states, num_states))) for _ in range(l + 1)
+]
s_bar_t = [numpy.matrix(numpy.zeros((num_states, 1))) for _ in range(l + 1)]
s_scalar_bar_t = [numpy.matrix(numpy.zeros((1, 1))) for _ in range(l + 1)]
-L_t = [numpy.matrix(numpy.zeros((num_inputs, num_states))) for _ in range(l + 1)]
+L_t = [
+ numpy.matrix(numpy.zeros((num_inputs, num_states))) for _ in range(l + 1)
+]
l_t = [numpy.matrix(numpy.zeros((num_inputs, 1))) for _ in range(l + 1)]
-L_bar_t = [numpy.matrix(numpy.zeros((num_inputs, num_states))) for _ in range(l + 1)]
+L_bar_t = [
+ numpy.matrix(numpy.zeros((num_inputs, num_states))) for _ in range(l + 1)
+]
l_bar_t = [numpy.matrix(numpy.zeros((num_inputs, 1))) for _ in range(l + 1)]
-S_t = [numpy.matrix(numpy.zeros((num_states, num_states))) for _ in range(l + 1)]
+S_t = [
+ numpy.matrix(numpy.zeros((num_states, num_states))) for _ in range(l + 1)
+]
s_t = [numpy.matrix(numpy.zeros((num_states, 1))) for _ in range(l + 1)]
s_scalar_t = [numpy.matrix(numpy.zeros((1, 1))) for _ in range(l + 1)]
-
-last_x_hat_t = [numpy.matrix(numpy.zeros((num_states, 1))) for _ in range(l + 1)]
+last_x_hat_t = [
+ numpy.matrix(numpy.zeros((num_states, 1))) for _ in range(l + 1)
+]
for a in range(15):
- x_hat = x_hat_initial
- u_t = L_t[0] * x_hat + l_t[0]
- S_bar_t[0] = numpy.matrix(numpy.zeros((num_states, num_states)))
- s_bar_t[0] = numpy.matrix(numpy.zeros((num_states, 1)))
- s_scalar_bar_t[0] = numpy.matrix([[0]])
+ x_hat = x_hat_initial
+ u_t = L_t[0] * x_hat + l_t[0]
+ S_bar_t[0] = numpy.matrix(numpy.zeros((num_states, num_states)))
+ s_bar_t[0] = numpy.matrix(numpy.zeros((num_states, 1)))
+ s_scalar_bar_t[0] = numpy.matrix([[0]])
- last_x_hat_t[0] = x_hat_initial
+ last_x_hat_t[0] = x_hat_initial
- Q_t = numerical_jacobian_x_x(cost_fn_obj.cost, x_hat_initial, u_t)
- P_t = numerical_jacobian_x_u(cost_fn_obj.cost, x_hat_initial, u_t)
- R_t = numerical_jacobian_u_u(cost_fn_obj.cost, x_hat_initial, u_t)
+ Q_t = numerical_jacobian_x_x(cost_fn_obj.cost, x_hat_initial, u_t)
+ P_t = numerical_jacobian_x_u(cost_fn_obj.cost, x_hat_initial, u_t)
+ R_t = numerical_jacobian_u_u(cost_fn_obj.cost, x_hat_initial, u_t)
- q_t = numerical_jacobian_x(cost_fn_obj.cost, x_hat_initial, u_t).T - Q_t * x_hat_initial - P_t.T * u_t
- r_t = numerical_jacobian_u(cost_fn_obj.cost, x_hat_initial, u_t).T - P_t * x_hat_initial - R_t * u_t
+ q_t = numerical_jacobian_x(cost_fn_obj.cost, x_hat_initial,
+ u_t).T - Q_t * x_hat_initial - P_t.T * u_t
+ r_t = numerical_jacobian_u(cost_fn_obj.cost, x_hat_initial,
+ u_t).T - P_t * x_hat_initial - R_t * u_t
- q_scalar_t = cost_fn_obj.cost(x_hat_initial, u_t) - 0.5 * (x_hat_initial.T * (Q_t * x_hat_initial + P_t.T * u_t) + u_t.T * (P_t * x_hat_initial + R_t * u_t)) - x_hat_initial.T * q_t - u_t.T * r_t
+ q_scalar_t = cost_fn_obj.cost(
+ x_hat_initial,
+ u_t) - 0.5 * (x_hat_initial.T *
+ (Q_t * x_hat_initial + P_t.T * u_t) + u_t.T *
+ (P_t * x_hat_initial + R_t * u_t)
+ ) - x_hat_initial.T * q_t - u_t.T * r_t
- start_A_t = numerical_jacobian_x(discrete_dynamics, x_hat_initial, u_t)
- start_B_t = numerical_jacobian_u(discrete_dynamics, x_hat_initial, u_t)
- x_hat_next = discrete_dynamics(x_hat_initial, u_t)
- start_c_t = x_hat_next - start_A_t * x_hat_initial - start_B_t * u_t
+ start_A_t = numerical_jacobian_x(discrete_dynamics, x_hat_initial, u_t)
+ start_B_t = numerical_jacobian_u(discrete_dynamics, x_hat_initial, u_t)
+ x_hat_next = discrete_dynamics(x_hat_initial, u_t)
+ start_c_t = x_hat_next - start_A_t * x_hat_initial - start_B_t * u_t
- B_svd_u, B_svd_sigma_diag, B_svd_v = numpy.linalg.svd(start_B_t)
- B_svd_sigma = numpy.matrix(numpy.zeros(start_B_t.shape))
- B_svd_sigma[0:B_svd_sigma_diag.shape[0], 0:B_svd_sigma_diag.shape[0]] = numpy.diag(B_svd_sigma_diag)
+ B_svd_u, B_svd_sigma_diag, B_svd_v = numpy.linalg.svd(start_B_t)
+ B_svd_sigma = numpy.matrix(numpy.zeros(start_B_t.shape))
+ B_svd_sigma[0:B_svd_sigma_diag.shape[0],
+ 0:B_svd_sigma_diag.shape[0]] = numpy.diag(B_svd_sigma_diag)
- B_svd_sigma_inv = numpy.matrix(numpy.zeros(start_B_t.shape)).T
- B_svd_sigma_inv[0:B_svd_sigma_diag.shape[0], 0:B_svd_sigma_diag.shape[0]] = numpy.linalg.inv(numpy.diag(B_svd_sigma_diag))
- B_svd_inv = B_svd_v.T * B_svd_sigma_inv * B_svd_u.T
+ B_svd_sigma_inv = numpy.matrix(numpy.zeros(start_B_t.shape)).T
+ B_svd_sigma_inv[0:B_svd_sigma_diag.shape[0],
+ 0:B_svd_sigma_diag.shape[0]] = numpy.linalg.inv(
+ numpy.diag(B_svd_sigma_diag))
+ B_svd_inv = B_svd_v.T * B_svd_sigma_inv * B_svd_u.T
- L_bar_t[1] = B_svd_inv
- l_bar_t[1] = -B_svd_inv * (start_A_t * x_hat_initial + start_c_t)
+ L_bar_t[1] = B_svd_inv
+ l_bar_t[1] = -B_svd_inv * (start_A_t * x_hat_initial + start_c_t)
- S_bar_t[1] = L_bar_t[1].T * R_t * L_bar_t[1]
+ S_bar_t[1] = L_bar_t[1].T * R_t * L_bar_t[1]
- TotalS_1 = start_B_t.T * S_t[1] * start_B_t + R_t
- Totals_1 = start_B_t.T * S_t[1] * (start_c_t + start_A_t * x_hat_initial) + start_B_t.T * s_t[1] + P_t * x_hat_initial + r_t
- Totals_scalar_1 = 0.5 * (start_c_t.T + x_hat_initial.T * start_A_t.T) * S_t[1] * (start_c_t + start_A_t * x_hat_initial) + s_scalar_t[1] + x_hat_initial.T * q_t + q_scalar_t + 0.5 * x_hat_initial.T * Q_t * x_hat_initial + (start_c_t.T + x_hat_initial.T * start_A_t.T) * s_t[1]
+ TotalS_1 = start_B_t.T * S_t[1] * start_B_t + R_t
+ Totals_1 = start_B_t.T * S_t[1] * (
+ start_c_t + start_A_t *
+ x_hat_initial) + start_B_t.T * s_t[1] + P_t * x_hat_initial + r_t
+ Totals_scalar_1 = 0.5 * (
+ start_c_t.T + x_hat_initial.T * start_A_t.T
+ ) * S_t[1] * (start_c_t + start_A_t * x_hat_initial) + s_scalar_t[
+ 1] + x_hat_initial.T * q_t + q_scalar_t + 0.5 * x_hat_initial.T * Q_t * x_hat_initial + (
+ start_c_t.T + x_hat_initial.T * start_A_t.T) * s_t[1]
- optimal_u_1 = -numpy.linalg.solve(TotalS_1, Totals_1)
- optimal_x_1 = start_A_t * x_hat_initial + start_B_t * optimal_u_1 + start_c_t
+ optimal_u_1 = -numpy.linalg.solve(TotalS_1, Totals_1)
+ optimal_x_1 = start_A_t * x_hat_initial + start_B_t * optimal_u_1 + start_c_t
- S_bar_1_eigh_eigenvalues, S_bar_1_eigh_eigenvectors = numpy.linalg.eigh(S_bar_t[1])
- S_bar_1_eigh = numpy.matrix(numpy.diag(S_bar_1_eigh_eigenvalues))
- S_bar_1_eigh_eigenvalues_stiff = S_bar_1_eigh_eigenvalues.copy()
- for i in range(S_bar_1_eigh_eigenvalues_stiff.shape[0]):
- if abs(S_bar_1_eigh_eigenvalues_stiff[i]) < 1e-8:
- S_bar_1_eigh_eigenvalues_stiff[i] = max(S_bar_1_eigh_eigenvalues_stiff) * 1.0
+ S_bar_1_eigh_eigenvalues, S_bar_1_eigh_eigenvectors = numpy.linalg.eigh(
+ S_bar_t[1])
+ S_bar_1_eigh = numpy.matrix(numpy.diag(S_bar_1_eigh_eigenvalues))
+ S_bar_1_eigh_eigenvalues_stiff = S_bar_1_eigh_eigenvalues.copy()
+ for i in range(S_bar_1_eigh_eigenvalues_stiff.shape[0]):
+ if abs(S_bar_1_eigh_eigenvalues_stiff[i]) < 1e-8:
+ S_bar_1_eigh_eigenvalues_stiff[i] = max(
+ S_bar_1_eigh_eigenvalues_stiff) * 1.0
- #print 'eigh eigenvalues of S bar', S_bar_1_eigh_eigenvalues
- #print 'eigh eigenvectors of S bar', S_bar_1_eigh_eigenvectors.T
+ #print 'eigh eigenvalues of S bar', S_bar_1_eigh_eigenvalues
+ #print 'eigh eigenvectors of S bar', S_bar_1_eigh_eigenvectors.T
- #print 'S bar eig recreate', S_bar_1_eigh_eigenvectors * numpy.matrix(numpy.diag(S_bar_1_eigh_eigenvalues)) * S_bar_1_eigh_eigenvectors.T
- #print 'S bar eig recreate error', (S_bar_1_eigh_eigenvectors * numpy.matrix(numpy.diag(S_bar_1_eigh_eigenvalues)) * S_bar_1_eigh_eigenvectors.T - S_bar_t[1])
+ #print 'S bar eig recreate', S_bar_1_eigh_eigenvectors * numpy.matrix(numpy.diag(S_bar_1_eigh_eigenvalues)) * S_bar_1_eigh_eigenvectors.T
+ #print 'S bar eig recreate error', (S_bar_1_eigh_eigenvectors * numpy.matrix(numpy.diag(S_bar_1_eigh_eigenvalues)) * S_bar_1_eigh_eigenvectors.T - S_bar_t[1])
- S_bar_stiff = S_bar_1_eigh_eigenvectors * numpy.matrix(numpy.diag(S_bar_1_eigh_eigenvalues_stiff)) * S_bar_1_eigh_eigenvectors.T
+ S_bar_stiff = S_bar_1_eigh_eigenvectors * numpy.matrix(
+ numpy.diag(
+ S_bar_1_eigh_eigenvalues_stiff)) * S_bar_1_eigh_eigenvectors.T
- print 'Min u', -numpy.linalg.solve(TotalS_1, Totals_1)
- print 'Min x_hat', optimal_x_1
- s_bar_t[1] = -s_t[1] - (S_bar_stiff + S_t[1]) * optimal_x_1
- s_scalar_bar_t[1] = 0.5 * (optimal_u_1.T * TotalS_1 * optimal_u_1 - optimal_x_1.T * (S_bar_stiff + S_t[1]) * optimal_x_1) + optimal_u_1.T * Totals_1 - optimal_x_1.T * (s_bar_t[1] + s_t[1]) - s_scalar_t[1] + Totals_scalar_1
+ print 'Min u', -numpy.linalg.solve(TotalS_1, Totals_1)
+ print 'Min x_hat', optimal_x_1
+ s_bar_t[1] = -s_t[1] - (S_bar_stiff + S_t[1]) * optimal_x_1
+ s_scalar_bar_t[1] = 0.5 * (
+ optimal_u_1.T * TotalS_1 * optimal_u_1 - optimal_x_1.T *
+ (S_bar_stiff + S_t[1]) *
+ optimal_x_1) + optimal_u_1.T * Totals_1 - optimal_x_1.T * (
+ s_bar_t[1] + s_t[1]) - s_scalar_t[1] + Totals_scalar_1
- print 'optimal_u_1', optimal_u_1
- print 'TotalS_1', TotalS_1
- print 'Totals_1', Totals_1
- print 'Totals_scalar_1', Totals_scalar_1
- print 'overall cost 1', 0.5 * (optimal_u_1.T * TotalS_1 * optimal_u_1) + optimal_u_1.T * Totals_1 + Totals_scalar_1
- print 'overall cost 0', 0.5 * (x_hat_initial.T * S_t[0] * x_hat_initial) + x_hat_initial.T * s_t[0] + s_scalar_t[0]
+ print 'optimal_u_1', optimal_u_1
+ print 'TotalS_1', TotalS_1
+ print 'Totals_1', Totals_1
+ print 'Totals_scalar_1', Totals_scalar_1
+ print 'overall cost 1', 0.5 * (
+ optimal_u_1.T * TotalS_1 *
+ optimal_u_1) + optimal_u_1.T * Totals_1 + Totals_scalar_1
+ print 'overall cost 0', 0.5 * (x_hat_initial.T * S_t[0] * x_hat_initial
+ ) + x_hat_initial.T * s_t[0] + s_scalar_t[0]
- print 't forward 0'
- print 'x_hat_initial[ 0]: %s' % (x_hat_initial)
- print 'x_hat[%2d]: %s' % (0, x_hat.T)
- print 'x_hat_next[%2d]: %s' % (0, x_hat_next.T)
- print 'u[%2d]: %s' % (0, u_t.T)
- print ('L[ 0]: %s' % (L_t[0],)).replace('\n', '\n ')
- print ('l[ 0]: %s' % (l_t[0],)).replace('\n', '\n ')
+ print 't forward 0'
+ print 'x_hat_initial[ 0]: %s' % (x_hat_initial)
+ print 'x_hat[%2d]: %s' % (0, x_hat.T)
+ print 'x_hat_next[%2d]: %s' % (0, x_hat_next.T)
+ print 'u[%2d]: %s' % (0, u_t.T)
+ print('L[ 0]: %s' % (L_t[0], )).replace('\n', '\n ')
+ print('l[ 0]: %s' % (l_t[0], )).replace('\n', '\n ')
- print ('A_t[%2d]: %s' % (0, start_A_t)).replace('\n', '\n ')
- print ('B_t[%2d]: %s' % (0, start_B_t)).replace('\n', '\n ')
- print ('c_t[%2d]: %s' % (0, start_c_t)).replace('\n', '\n ')
+ print('A_t[%2d]: %s' % (0, start_A_t)).replace('\n', '\n ')
+ print('B_t[%2d]: %s' % (0, start_B_t)).replace('\n', '\n ')
+ print('c_t[%2d]: %s' % (0, start_c_t)).replace('\n', '\n ')
- # TODO(austin): optimal_x_1 is x_hat
- x_hat = -numpy.linalg.solve((S_t[1] + S_bar_stiff), (s_t[1] + s_bar_t[1]))
- print 'new xhat', x_hat
+ # TODO(austin): optimal_x_1 is x_hat
+ x_hat = -numpy.linalg.solve((S_t[1] + S_bar_stiff), (s_t[1] + s_bar_t[1]))
+ print 'new xhat', x_hat
- S_bar_t[1] = S_bar_stiff
+ S_bar_t[1] = S_bar_stiff
- last_x_hat_t[1] = x_hat
+ last_x_hat_t[1] = x_hat
- for t in range(1, l):
- print 't forward', t
- u_t = L_t[t] * x_hat + l_t[t]
+ for t in range(1, l):
+ print 't forward', t
+ u_t = L_t[t] * x_hat + l_t[t]
- x_hat_next = discrete_dynamics(x_hat, u_t)
- A_bar_t = numerical_jacobian_x(inverse_discrete_dynamics, x_hat_next, u_t)
- B_bar_t = numerical_jacobian_u(inverse_discrete_dynamics, x_hat_next, u_t)
- c_bar_t = x_hat - A_bar_t * x_hat_next - B_bar_t * u_t
+ x_hat_next = discrete_dynamics(x_hat, u_t)
+ A_bar_t = numerical_jacobian_x(inverse_discrete_dynamics, x_hat_next,
+ u_t)
+ B_bar_t = numerical_jacobian_u(inverse_discrete_dynamics, x_hat_next,
+ u_t)
+ c_bar_t = x_hat - A_bar_t * x_hat_next - B_bar_t * u_t
- print 'x_hat[%2d]: %s' % (t, x_hat.T)
- print 'x_hat_next[%2d]: %s' % (t, x_hat_next.T)
- print ('L[%2d]: %s' % (t, L_t[t],)).replace('\n', '\n ')
- print ('l[%2d]: %s' % (t, l_t[t],)).replace('\n', '\n ')
- print 'u[%2d]: %s' % (t, u_t.T)
+ print 'x_hat[%2d]: %s' % (t, x_hat.T)
+ print 'x_hat_next[%2d]: %s' % (t, x_hat_next.T)
+ print('L[%2d]: %s' % (
+ t,
+ L_t[t],
+ )).replace('\n', '\n ')
+ print('l[%2d]: %s' % (
+ t,
+ l_t[t],
+ )).replace('\n', '\n ')
+ print 'u[%2d]: %s' % (t, u_t.T)
- print ('A_bar_t[%2d]: %s' % (t, A_bar_t)).replace('\n', '\n ')
- print ('B_bar_t[%2d]: %s' % (t, B_bar_t)).replace('\n', '\n ')
- print ('c_bar_t[%2d]: %s' % (t, c_bar_t)).replace('\n', '\n ')
+ print('A_bar_t[%2d]: %s' % (t, A_bar_t)).replace(
+ '\n', '\n ')
+ print('B_bar_t[%2d]: %s' % (t, B_bar_t)).replace(
+ '\n', '\n ')
+ print('c_bar_t[%2d]: %s' % (t, c_bar_t)).replace(
+ '\n', '\n ')
- Q_t = numerical_jacobian_x_x(cost_fn_obj.cost, x_hat, u_t)
- P_t = numerical_jacobian_x_u(cost_fn_obj.cost, x_hat, u_t)
- R_t = numerical_jacobian_u_u(cost_fn_obj.cost, x_hat, u_t)
+ Q_t = numerical_jacobian_x_x(cost_fn_obj.cost, x_hat, u_t)
+ P_t = numerical_jacobian_x_u(cost_fn_obj.cost, x_hat, u_t)
+ R_t = numerical_jacobian_u_u(cost_fn_obj.cost, x_hat, u_t)
- q_t = numerical_jacobian_x(cost_fn_obj.cost, x_hat, u_t).T - Q_t * x_hat - P_t.T * u_t
- r_t = numerical_jacobian_u(cost_fn_obj.cost, x_hat, u_t).T - P_t * x_hat - R_t * u_t
+ q_t = numerical_jacobian_x(cost_fn_obj.cost, x_hat,
+ u_t).T - Q_t * x_hat - P_t.T * u_t
+ r_t = numerical_jacobian_u(cost_fn_obj.cost, x_hat,
+ u_t).T - P_t * x_hat - R_t * u_t
- q_scalar_t = cost_fn_obj.cost(x_hat, u_t) - 0.5 * (x_hat.T * (Q_t * x_hat + P_t.T * u_t) + u_t.T * (P_t * x_hat + R_t * u_t)) - x_hat.T * q_t - u_t.T * r_t
+ q_scalar_t = cost_fn_obj.cost(x_hat, u_t) - 0.5 * (
+ x_hat.T * (Q_t * x_hat + P_t.T * u_t) + u_t.T *
+ (P_t * x_hat + R_t * u_t)) - x_hat.T * q_t - u_t.T * r_t
- C_bar_t = B_bar_t.T * (S_bar_t[t] + Q_t) * A_bar_t + P_t * A_bar_t
- D_bar_t = A_bar_t.T * (S_bar_t[t] + Q_t) * A_bar_t
- E_bar_t = B_bar_t.T * (S_bar_t[t] + Q_t) * B_bar_t + R_t + P_t * B_bar_t + B_bar_t.T * P_t.T
- d_bar_t = A_bar_t.T * (s_bar_t[t] + q_t) + A_bar_t.T * (S_bar_t[t] + Q_t) * c_bar_t
- e_bar_t = r_t + P_t * c_bar_t + B_bar_t.T * s_bar_t[t] + B_bar_t.T * (S_bar_t[t] + Q_t) * c_bar_t
+ C_bar_t = B_bar_t.T * (S_bar_t[t] + Q_t) * A_bar_t + P_t * A_bar_t
+ D_bar_t = A_bar_t.T * (S_bar_t[t] + Q_t) * A_bar_t
+ E_bar_t = B_bar_t.T * (
+ S_bar_t[t] +
+ Q_t) * B_bar_t + R_t + P_t * B_bar_t + B_bar_t.T * P_t.T
+ d_bar_t = A_bar_t.T * (s_bar_t[t] + q_t) + A_bar_t.T * (S_bar_t[t] +
+ Q_t) * c_bar_t
+ e_bar_t = r_t + P_t * c_bar_t + B_bar_t.T * s_bar_t[t] + B_bar_t.T * (
+ S_bar_t[t] + Q_t) * c_bar_t
- L_bar_t[t + 1] = -numpy.linalg.inv(E_bar_t) * C_bar_t
- l_bar_t[t + 1] = -numpy.linalg.inv(E_bar_t) * e_bar_t
+ L_bar_t[t + 1] = -numpy.linalg.inv(E_bar_t) * C_bar_t
+ l_bar_t[t + 1] = -numpy.linalg.inv(E_bar_t) * e_bar_t
- S_bar_t[t + 1] = D_bar_t + C_bar_t.T * L_bar_t[t + 1]
- s_bar_t[t + 1] = d_bar_t + C_bar_t.T * l_bar_t[t + 1]
- s_scalar_bar_t[t + 1] = -0.5 * e_bar_t.T * numpy.linalg.inv(E_bar_t) * e_bar_t + 0.5 * c_bar_t.T * (S_bar_t[t] + Q_t) * c_bar_t + c_bar_t.T * s_bar_t[t] + c_bar_t.T * q_t + s_scalar_bar_t[t] + q_scalar_t
+ S_bar_t[t + 1] = D_bar_t + C_bar_t.T * L_bar_t[t + 1]
+ s_bar_t[t + 1] = d_bar_t + C_bar_t.T * l_bar_t[t + 1]
+ s_scalar_bar_t[t + 1] = -0.5 * e_bar_t.T * numpy.linalg.inv(
+ E_bar_t) * e_bar_t + 0.5 * c_bar_t.T * (
+ S_bar_t[t] + Q_t) * c_bar_t + c_bar_t.T * s_bar_t[
+ t] + c_bar_t.T * q_t + s_scalar_bar_t[t] + q_scalar_t
- x_hat = -numpy.linalg.solve((S_t[t + 1] + S_bar_t[t + 1]), (s_t[t + 1] + s_bar_t[t + 1]))
+ x_hat = -numpy.linalg.solve((S_t[t + 1] + S_bar_t[t + 1]),
+ (s_t[t + 1] + s_bar_t[t + 1]))
- S_t[l] = cost_fn_obj.estimate_Q_final(x_hat)
- s_t[l] = cost_fn_obj.estimate_q_final(x_hat)
- x_hat = -numpy.linalg.inv(S_t[l] + S_bar_t[l]) * (s_t[l] + s_bar_t[l])
+ S_t[l] = cost_fn_obj.estimate_Q_final(x_hat)
+ s_t[l] = cost_fn_obj.estimate_q_final(x_hat)
+ x_hat = -numpy.linalg.inv(S_t[l] + S_bar_t[l]) * (s_t[l] + s_bar_t[l])
- for t in reversed(range(l)):
- print 't backward', t
- # TODO(austin): I don't think we can use L_t like this here.
- # I think we are off by 1 somewhere...
- u_t = L_bar_t[t + 1] * x_hat + l_bar_t[t + 1]
+ for t in reversed(range(l)):
+ print 't backward', t
+ # TODO(austin): I don't think we can use L_t like this here.
+ # I think we are off by 1 somewhere...
+ u_t = L_bar_t[t + 1] * x_hat + l_bar_t[t + 1]
- x_hat_prev = inverse_discrete_dynamics(x_hat, u_t)
- print 'x_hat[%2d]: %s' % (t, x_hat.T)
- print 'x_hat_prev[%2d]: %s' % (t, x_hat_prev.T)
- print ('L_bar[%2d]: %s' % (t + 1, L_bar_t[t + 1])).replace('\n', '\n ')
- print ('l_bar[%2d]: %s' % (t + 1, l_bar_t[t + 1])).replace('\n', '\n ')
- print 'u[%2d]: %s' % (t, u_t.T)
- # Now compute the linearized A, B, and C
- # Start by doing it numerically, and then optimize.
- A_t = numerical_jacobian_x(discrete_dynamics, x_hat_prev, u_t)
- B_t = numerical_jacobian_u(discrete_dynamics, x_hat_prev, u_t)
- c_t = x_hat - A_t * x_hat_prev - B_t * u_t
+ x_hat_prev = inverse_discrete_dynamics(x_hat, u_t)
+ print 'x_hat[%2d]: %s' % (t, x_hat.T)
+ print 'x_hat_prev[%2d]: %s' % (t, x_hat_prev.T)
+ print('L_bar[%2d]: %s' % (t + 1, L_bar_t[t + 1])).replace(
+ '\n', '\n ')
+ print('l_bar[%2d]: %s' % (t + 1, l_bar_t[t + 1])).replace(
+ '\n', '\n ')
+ print 'u[%2d]: %s' % (t, u_t.T)
+ # Now compute the linearized A, B, and C
+ # Start by doing it numerically, and then optimize.
+ A_t = numerical_jacobian_x(discrete_dynamics, x_hat_prev, u_t)
+ B_t = numerical_jacobian_u(discrete_dynamics, x_hat_prev, u_t)
+ c_t = x_hat - A_t * x_hat_prev - B_t * u_t
- print ('A_t[%2d]: %s' % (t, A_t)).replace('\n', '\n ')
- print ('B_t[%2d]: %s' % (t, B_t)).replace('\n', '\n ')
- print ('c_t[%2d]: %s' % (t, c_t)).replace('\n', '\n ')
+ print('A_t[%2d]: %s' % (t, A_t)).replace('\n', '\n ')
+ print('B_t[%2d]: %s' % (t, B_t)).replace('\n', '\n ')
+ print('c_t[%2d]: %s' % (t, c_t)).replace('\n', '\n ')
- Q_t = numerical_jacobian_x_x(cost_fn_obj.cost, x_hat_prev, u_t)
- P_t = numerical_jacobian_x_u(cost_fn_obj.cost, x_hat_prev, u_t)
- P_T_t = numerical_jacobian_u_x(cost_fn_obj.cost, x_hat_prev, u_t)
- R_t = numerical_jacobian_u_u(cost_fn_obj.cost, x_hat_prev, u_t)
+ Q_t = numerical_jacobian_x_x(cost_fn_obj.cost, x_hat_prev, u_t)
+ P_t = numerical_jacobian_x_u(cost_fn_obj.cost, x_hat_prev, u_t)
+ P_T_t = numerical_jacobian_u_x(cost_fn_obj.cost, x_hat_prev, u_t)
+ R_t = numerical_jacobian_u_u(cost_fn_obj.cost, x_hat_prev, u_t)
- q_t = numerical_jacobian_x(cost_fn_obj.cost, x_hat_prev, u_t).T - Q_t * x_hat_prev - P_T_t * u_t
- r_t = numerical_jacobian_u(cost_fn_obj.cost, x_hat_prev, u_t).T - P_t * x_hat_prev - R_t * u_t
+ q_t = numerical_jacobian_x(cost_fn_obj.cost, x_hat_prev,
+ u_t).T - Q_t * x_hat_prev - P_T_t * u_t
+ r_t = numerical_jacobian_u(cost_fn_obj.cost, x_hat_prev,
+ u_t).T - P_t * x_hat_prev - R_t * u_t
- q_scalar_t = cost_fn_obj.cost(x_hat_prev, u_t) - 0.5 * (x_hat_prev.T * (Q_t * x_hat_prev + P_t.T * u_t) + u_t.T * (P_t * x_hat_prev + R_t * u_t)) - x_hat_prev.T * q_t - u_t.T * r_t
+ q_scalar_t = cost_fn_obj.cost(x_hat_prev, u_t) - 0.5 * (
+ x_hat_prev.T * (Q_t * x_hat_prev + P_t.T * u_t) + u_t.T *
+ (P_t * x_hat_prev + R_t * u_t)) - x_hat_prev.T * q_t - u_t.T * r_t
- C_t = P_t + B_t.T * S_t[t + 1] * A_t
- D_t = Q_t + A_t.T * S_t[t + 1] * A_t
- E_t = R_t + B_t.T * S_t[t + 1] * B_t
- d_t = q_t + A_t.T * s_t[t + 1] + A_t.T * S_t[t + 1] * c_t
- e_t = r_t + B_t.T * s_t[t + 1] + B_t.T * S_t[t + 1] * c_t
- L_t[t] = -numpy.linalg.inv(E_t) * C_t
- l_t[t] = -numpy.linalg.inv(E_t) * e_t
- s_t[t] = d_t + C_t.T * l_t[t]
- S_t[t] = D_t + C_t.T * L_t[t]
- s_scalar_t[t] = q_scalar_t - 0.5 * e_t.T * numpy.linalg.inv(E_t) * e_t + 0.5 * c_t.T * S_t[t + 1] * c_t + c_t.T * s_t[t + 1] + s_scalar_t[t + 1]
+ C_t = P_t + B_t.T * S_t[t + 1] * A_t
+ D_t = Q_t + A_t.T * S_t[t + 1] * A_t
+ E_t = R_t + B_t.T * S_t[t + 1] * B_t
+ d_t = q_t + A_t.T * s_t[t + 1] + A_t.T * S_t[t + 1] * c_t
+ e_t = r_t + B_t.T * s_t[t + 1] + B_t.T * S_t[t + 1] * c_t
+ L_t[t] = -numpy.linalg.inv(E_t) * C_t
+ l_t[t] = -numpy.linalg.inv(E_t) * e_t
+ s_t[t] = d_t + C_t.T * l_t[t]
+ S_t[t] = D_t + C_t.T * L_t[t]
+ s_scalar_t[t] = q_scalar_t - 0.5 * e_t.T * numpy.linalg.inv(
+ E_t) * e_t + 0.5 * c_t.T * S_t[t + 1] * c_t + c_t.T * s_t[
+ t + 1] + s_scalar_t[t + 1]
- x_hat = -numpy.linalg.solve((S_t[t] + S_bar_t[t]), (s_t[t] + s_bar_t[t]))
- if t == 0:
- last_x_hat_t[t] = x_hat_initial
- else:
- last_x_hat_t[t] = x_hat
+ x_hat = -numpy.linalg.solve((S_t[t] + S_bar_t[t]),
+ (s_t[t] + s_bar_t[t]))
+ if t == 0:
+ last_x_hat_t[t] = x_hat_initial
+ else:
+ last_x_hat_t[t] = x_hat
- x_hat_t = [x_hat_initial]
+ x_hat_t = [x_hat_initial]
- pylab.figure('states %d' % a)
- pylab.ion()
- for dim in range(num_states):
- pylab.plot(numpy.arange(len(last_x_hat_t)),
- [x_hat_loop[dim, 0] for x_hat_loop in last_x_hat_t], marker='o', label='Xhat[%d]'%dim)
- pylab.legend()
- pylab.draw()
+ pylab.figure('states %d' % a)
+ pylab.ion()
+ for dim in range(num_states):
+ pylab.plot(numpy.arange(len(last_x_hat_t)),
+ [x_hat_loop[dim, 0] for x_hat_loop in last_x_hat_t],
+ marker='o',
+ label='Xhat[%d]' % dim)
+ pylab.legend()
+ pylab.draw()
- pylab.figure('xy %d' % a)
- pylab.ion()
- pylab.plot([x_hat_loop[0, 0] for x_hat_loop in last_x_hat_t],
- [x_hat_loop[1, 0] for x_hat_loop in last_x_hat_t], marker='o', label='trajectory')
- pylab.legend()
- pylab.draw()
+ pylab.figure('xy %d' % a)
+ pylab.ion()
+ pylab.plot([x_hat_loop[0, 0] for x_hat_loop in last_x_hat_t],
+ [x_hat_loop[1, 0] for x_hat_loop in last_x_hat_t],
+ marker='o',
+ label='trajectory')
+ pylab.legend()
+ pylab.draw()
final_u_t = [numpy.matrix(numpy.zeros((num_inputs, 1))) for _ in range(l + 1)]
cost_to_go = []
cost_to_come = []
cost = []
for t in range(l):
- cost_to_go.append((0.5 * last_x_hat_t[t].T * S_t[t] * last_x_hat_t[t] + last_x_hat_t[t].T * s_t[t] + s_scalar_t[t])[0, 0])
- cost_to_come.append((0.5 * last_x_hat_t[t].T * S_bar_t[t] * last_x_hat_t[t] + last_x_hat_t[t].T * s_bar_t[t] + s_scalar_bar_t[t])[0, 0])
- cost.append(cost_to_go[-1] + cost_to_come[-1])
- final_u_t[t] = L_t[t] * last_x_hat_t[t] + l_t[t]
+ cost_to_go.append((0.5 * last_x_hat_t[t].T * S_t[t] * last_x_hat_t[t] +
+ last_x_hat_t[t].T * s_t[t] + s_scalar_t[t])[0, 0])
+ cost_to_come.append(
+ (0.5 * last_x_hat_t[t].T * S_bar_t[t] * last_x_hat_t[t] +
+ last_x_hat_t[t].T * s_bar_t[t] + s_scalar_bar_t[t])[0, 0])
+ cost.append(cost_to_go[-1] + cost_to_come[-1])
+ final_u_t[t] = L_t[t] * last_x_hat_t[t] + l_t[t]
for t in range(l):
- A_t = numerical_jacobian_x(discrete_dynamics, last_x_hat_t[t], final_u_t[t])
- B_t = numerical_jacobian_u(discrete_dynamics, last_x_hat_t[t], final_u_t[t])
- c_t = discrete_dynamics(last_x_hat_t[t], final_u_t[t]) - A_t * last_x_hat_t[t] - B_t * final_u_t[t]
- print("Infeasability at", t, "is", ((A_t * last_x_hat_t[t] + B_t * final_u_t[t] + c_t) - last_x_hat_t[t + 1]).T)
+ A_t = numerical_jacobian_x(discrete_dynamics, last_x_hat_t[t],
+ final_u_t[t])
+ B_t = numerical_jacobian_u(discrete_dynamics, last_x_hat_t[t],
+ final_u_t[t])
+ c_t = discrete_dynamics(
+ last_x_hat_t[t],
+ final_u_t[t]) - A_t * last_x_hat_t[t] - B_t * final_u_t[t]
+ print("Infeasability at", t, "is",
+ ((A_t * last_x_hat_t[t] + B_t * final_u_t[t] + c_t) -
+ last_x_hat_t[t + 1]).T)
pylab.figure('u')
samples = numpy.arange(len(final_u_t))
for i in range(num_inputs):
- pylab.plot(samples, [u[i, 0] for u in final_u_t], label='u[%d]' % i, marker='o')
- pylab.legend()
+ pylab.plot(samples, [u[i, 0] for u in final_u_t],
+ label='u[%d]' % i,
+ marker='o')
+ pylab.legend()
pylab.figure('cost')
cost_samples = numpy.arange(len(cost))
diff --git a/y2014/control_loops/python/extended_lqr_derivation.py b/y2014/control_loops/python/extended_lqr_derivation.py
index 010c5de..6857654 100755
--- a/y2014/control_loops/python/extended_lqr_derivation.py
+++ b/y2014/control_loops/python/extended_lqr_derivation.py
@@ -6,7 +6,6 @@
import random
import sympy
-
'''
* `x_t1` means `x_{t + 1}`. Using `'x_t + 1'` as the symbol name makes the non-
latex output really confusing, so not doing that.
@@ -57,351 +56,335 @@
ub = sympy.MatrixSymbol('ubold', number_of_inputs, 1)
CONSTANTS = set([
- A_t, B_t, cb_t,
- S_t1, sb_t1, s_t1,
- A_tb, B_tb, cb_tb,
- S_tb, sb_tb, s_tb,
- P_t, Q_t, R_t, qb_t, rb_t, q_t,
- ])
+ A_t,
+ B_t,
+ cb_t,
+ S_t1,
+ sb_t1,
+ s_t1,
+ A_tb,
+ B_tb,
+ cb_tb,
+ S_tb,
+ sb_tb,
+ s_tb,
+ P_t,
+ Q_t,
+ R_t,
+ qb_t,
+ rb_t,
+ q_t,
+])
SYMMETRIC_CONSTANTS = set([
- S_t1, S_tb,
- Q_t, R_t,
- ])
+ S_t1,
+ S_tb,
+ Q_t,
+ R_t,
+])
+
def verify_equivalent(a, b, inverses={}):
- def get_matrices(m):
- matrices = m.atoms(sympy.MatrixSymbol)
- new_matrices = set()
- for matrix in matrices:
- if matrix in inverses:
- new_matrices.update(inverses[matrix].atoms(sympy.MatrixSymbol))
- matrices.update(new_matrices)
- a_matrices, b_matrices = get_matrices(a), get_matrices(b)
- if a_matrices != b_matrices:
- raise RuntimeError('matrices different: %s vs %s' % (a_matrices,
- b_matrices))
- a_symbols, b_symbols = a.atoms(sympy.Symbol), b.atoms(sympy.Symbol)
- if a_symbols != b_symbols:
- raise RuntimeError('symbols different: %s vs %s' % (a_symbols, b_symbols))
- if not a_symbols < DIMENSIONS:
- raise RuntimeError('not sure what to do with %s' % (a_symbols - DIMENSIONS))
- if a.shape != b.shape:
- raise RuntimeError('Different shapes: %s and %s' % (a.shape, b.shape))
+ def get_matrices(m):
+ matrices = m.atoms(sympy.MatrixSymbol)
+ new_matrices = set()
+ for matrix in matrices:
+ if matrix in inverses:
+ new_matrices.update(inverses[matrix].atoms(sympy.MatrixSymbol))
+ matrices.update(new_matrices)
- for _ in range(10):
- subs_symbols = {s: random.randint(1, 5) for s in a_symbols}
+ a_matrices, b_matrices = get_matrices(a), get_matrices(b)
+ if a_matrices != b_matrices:
+ raise RuntimeError('matrices different: %s vs %s' %
+ (a_matrices, b_matrices))
+ a_symbols, b_symbols = a.atoms(sympy.Symbol), b.atoms(sympy.Symbol)
+ if a_symbols != b_symbols:
+ raise RuntimeError('symbols different: %s vs %s' %
+ (a_symbols, b_symbols))
+ if not a_symbols < DIMENSIONS:
+ raise RuntimeError('not sure what to do with %s' %
+ (a_symbols - DIMENSIONS))
+
+ if a.shape != b.shape:
+ raise RuntimeError('Different shapes: %s and %s' % (a.shape, b.shape))
+
for _ in range(10):
- diff = a - b
- subs_matrices = {}
- def get_replacement(*args):
- try:
- m = sympy.MatrixSymbol(*args)
- if m not in subs_matrices:
- if m in inverses:
- i = inverses[m].replace(sympy.MatrixSymbol, get_replacement, simultaneous=False)
- i_evaled = sympy.ImmutableMatrix(i.rows, i.cols,
- lambda x,y: i[x, y].evalf())
- subs_matrices[m] = i_evaled.I
- else:
- rows = m.rows.subs(subs_symbols)
- cols = m.cols.subs(subs_symbols)
- new_m = sympy.ImmutableMatrix(rows, cols,
- lambda i,j: random.uniform(-5, 5))
- if m in SYMMETRIC_CONSTANTS:
- if rows != cols:
- raise RuntimeError('Non-square symmetric matrix %s' % m)
- def calculate_cell(i, j):
- if i > j:
- return new_m[i, j]
- else:
- return new_m[j, i]
- new_m = sympy.ImmutableMatrix(rows, cols, calculate_cell)
- subs_matrices[m] = new_m
- return subs_matrices[m]
- except AttributeError as e:
- # Stupid sympy silently eats AttributeErrors and treats them as
- # "no replacement"...
- raise RuntimeError(e)
- # subs fails when it tries doing matrix multiplies between fixed-size ones
- # and the rest of the equation which still has the symbolic-sized ones.
- # subs(simultaneous=True) wants to put dummies in for everything first,
- # and Dummy().transpose() is broken.
- # replace() has the same issue as subs with simultaneous being True.
- # lambdify() has no idea what to do with the transposes if you replace all
- # the matrices with ones of random sizes full of dummies.
- diff = diff.replace(sympy.MatrixSymbol, get_replacement,
- simultaneous=False)
- for row in range(diff.rows):
- for col in range(diff.cols):
- result = diff[row, col].evalf()
- if abs(result) > 1e-7:
- raise RuntimeError('difference at (%s, %s) is %s' % (row, col,
- result))
+ subs_symbols = {s: random.randint(1, 5) for s in a_symbols}
+ for _ in range(10):
+ diff = a - b
+ subs_matrices = {}
+
+ def get_replacement(*args):
+ try:
+ m = sympy.MatrixSymbol(*args)
+ if m not in subs_matrices:
+ if m in inverses:
+ i = inverses[m].replace(sympy.MatrixSymbol,
+ get_replacement,
+ simultaneous=False)
+ i_evaled = sympy.ImmutableMatrix(
+ i.rows, i.cols, lambda x, y: i[x, y].evalf())
+ subs_matrices[m] = i_evaled.I
+ else:
+ rows = m.rows.subs(subs_symbols)
+ cols = m.cols.subs(subs_symbols)
+ new_m = sympy.ImmutableMatrix(
+ rows, cols, lambda i, j: random.uniform(-5, 5))
+ if m in SYMMETRIC_CONSTANTS:
+ if rows != cols:
+ raise RuntimeError(
+ 'Non-square symmetric matrix %s' % m)
+
+ def calculate_cell(i, j):
+ if i > j:
+ return new_m[i, j]
+ else:
+ return new_m[j, i]
+
+ new_m = sympy.ImmutableMatrix(
+ rows, cols, calculate_cell)
+ subs_matrices[m] = new_m
+ return subs_matrices[m]
+ except AttributeError as e:
+ # Stupid sympy silently eats AttributeErrors and treats them as
+ # "no replacement"...
+ raise RuntimeError(e)
+
+ # subs fails when it tries doing matrix multiplies between fixed-size ones
+ # and the rest of the equation which still has the symbolic-sized ones.
+ # subs(simultaneous=True) wants to put dummies in for everything first,
+ # and Dummy().transpose() is broken.
+ # replace() has the same issue as subs with simultaneous being True.
+ # lambdify() has no idea what to do with the transposes if you replace all
+ # the matrices with ones of random sizes full of dummies.
+ diff = diff.replace(sympy.MatrixSymbol,
+ get_replacement,
+ simultaneous=False)
+ for row in range(diff.rows):
+ for col in range(diff.cols):
+ result = diff[row, col].evalf()
+ if abs(result) > 1e-7:
+ raise RuntimeError('difference at (%s, %s) is %s' %
+ (row, col, result))
+
def verify_arguments(f, *args):
- matrix_atoms = f.atoms(sympy.MatrixSymbol) - CONSTANTS
- if matrix_atoms != set(args):
- print('Arguments expected to be %s, but are %s, in:\n%s' % (
- sorted(args), sorted(list(matrix_atoms)), f), file=sys.stderr)
- raise RuntimeError
+ matrix_atoms = f.atoms(sympy.MatrixSymbol) - CONSTANTS
+ if matrix_atoms != set(args):
+ print('Arguments expected to be %s, but are %s, in:\n%s' %
+ (sorted(args), sorted(list(matrix_atoms)), f),
+ file=sys.stderr)
+ raise RuntimeError
+
def make_c_t():
- x_and_u = sympy.BlockMatrix(((xb,), (ub,)))
- c_t = (half * x_and_u.transpose() *
- sympy.BlockMatrix(((Q_t, P_t.T), (P_t, R_t))) * x_and_u +
- x_and_u.transpose() * sympy.BlockMatrix(((qb_t,), (rb_t,))) +
- q_t)
- verify_arguments(c_t, xb, ub)
- return c_t
+ x_and_u = sympy.BlockMatrix(((xb, ), (ub, )))
+ c_t = (half * x_and_u.transpose() * sympy.BlockMatrix(
+ ((Q_t, P_t.T),
+ (P_t, R_t))) * x_and_u + x_and_u.transpose() * sympy.BlockMatrix(
+ ((qb_t, ), (rb_t, ))) + q_t)
+ verify_arguments(c_t, xb, ub)
+ return c_t
+
def check_backwards_cost():
- g_t = A_t * xb_t + B_t * ub_t + cb_t
- verify_arguments(g_t, xb_t, ub_t)
- v_t1 = half * xb.transpose() * S_t1 * xb + xb.transpose() * sb_t1 + s_t1
- verify_arguments(v_t1, xb)
- v_t = (v_t1.subs(xb, g_t) + make_c_t()).subs({xb_t: xb, ub_t: ub})
- verify_arguments(v_t, xb, ub)
+ g_t = A_t * xb_t + B_t * ub_t + cb_t
+ verify_arguments(g_t, xb_t, ub_t)
+ v_t1 = half * xb.transpose() * S_t1 * xb + xb.transpose() * sb_t1 + s_t1
+ verify_arguments(v_t1, xb)
+ v_t = (v_t1.subs(xb, g_t) + make_c_t()).subs({xb_t: xb, ub_t: ub})
+ verify_arguments(v_t, xb, ub)
- v_t_for_cost = (
- half * (
- xb.transpose() * (A_t.transpose() * S_t1 * A_t + Q_t) * xb +
- ub.transpose() * (B_t.transpose() * S_t1 * A_t + P_t) * xb +
- xb.transpose() * (A_t.transpose() * S_t1 * B_t + P_t.T) * ub +
- ub.transpose() * (B_t.transpose() * S_t1 * B_t + R_t) * ub) +
- xb.transpose() * (A_t.transpose() * sb_t1 + qb_t) +
- ub.transpose() * (B_t.transpose() * sb_t1 + rb_t) +
- cb_t.transpose() * sb_t1 +
- s_t1 + q_t +
- half * (cb_t.transpose() * S_t1 * cb_t +
- xb.transpose() * A_t.transpose() * S_t1 * cb_t +
- ub.transpose() * B_t.transpose() * S_t1 * cb_t +
- cb_t.transpose() * S_t1 * B_t * ub +
- cb_t.transpose() * S_t1 * A_t * xb))
- verify_equivalent(v_t, v_t_for_cost)
+ v_t_for_cost = (
+ half * (xb.transpose() *
+ (A_t.transpose() * S_t1 * A_t + Q_t) * xb + ub.transpose() *
+ (B_t.transpose() * S_t1 * A_t + P_t) * xb + xb.transpose() *
+ (A_t.transpose() * S_t1 * B_t + P_t.T) * ub + ub.transpose() *
+ (B_t.transpose() * S_t1 * B_t + R_t) * ub) + xb.transpose() *
+ (A_t.transpose() * sb_t1 + qb_t) + ub.transpose() *
+ (B_t.transpose() * sb_t1 + rb_t) + cb_t.transpose() * sb_t1 + s_t1 +
+ q_t + half *
+ (cb_t.transpose() * S_t1 * cb_t + xb.transpose() * A_t.transpose() *
+ S_t1 * cb_t + ub.transpose() * B_t.transpose() * S_t1 * cb_t +
+ cb_t.transpose() * S_t1 * B_t * ub +
+ cb_t.transpose() * S_t1 * A_t * xb))
+ verify_equivalent(v_t, v_t_for_cost)
- v_t_now = (
- half * (xb.T * (A_t.T * S_t1 * A_t + Q_t) * xb +
- ub.T * (B_t.T * S_t1 * A_t + P_t) * xb +
- xb.T * (A_t.T * S_t1 * B_t + P_t.T) * ub +
- ub.T * (B_t.T * S_t1 * B_t + R_t) * ub) +
- xb.T * (A_t.T * sb_t1 + qb_t) +
- ub.T * (B_t.T * sb_t1 + rb_t) +
- cb_t.T * sb_t1 + s_t1 + q_t +
- half * (cb_t.T * S_t1 * cb_t +
- xb.T * A_t.T * S_t1 * cb_t +
- ub.T * B_t.T * S_t1 * cb_t +
- cb_t.T * S_t1 * B_t * ub +
- cb_t.T * S_t1 * A_t * xb))
+ v_t_now = (
+ half * (xb.T * (A_t.T * S_t1 * A_t + Q_t) * xb + ub.T *
+ (B_t.T * S_t1 * A_t + P_t) * xb + xb.T *
+ (A_t.T * S_t1 * B_t + P_t.T) * ub + ub.T *
+ (B_t.T * S_t1 * B_t + R_t) * ub) + xb.T *
+ (A_t.T * sb_t1 + qb_t) + ub.T * (B_t.T * sb_t1 + rb_t) +
+ cb_t.T * sb_t1 + s_t1 + q_t + half *
+ (cb_t.T * S_t1 * cb_t + xb.T * A_t.T * S_t1 * cb_t + ub.T * B_t.T *
+ S_t1 * cb_t + cb_t.T * S_t1 * B_t * ub + cb_t.T * S_t1 * A_t * xb))
- v_t_now = (
- half * (xb.T * (A_t.T * S_t1 * A_t + Q_t) * xb +
- ub.T * (B_t.T * S_t1 * A_t + P_t) * xb +
- xb.T * (A_t.T * S_t1 * B_t + P_t.T) * ub +
- ub.T * (B_t.T * S_t1 * B_t + R_t) * ub) +
- xb.T * (A_t.T * sb_t1 + qb_t + half * A_t.T * S_t1 * cb_t) +
- ub.T * (B_t.T * sb_t1 + rb_t + half * B_t.T * S_t1 * cb_t) +
- half * cb_t.T * S_t1 * (A_t * xb + B_t * ub + cb_t) +
- cb_t.T * sb_t1 + s_t1 + q_t)
+ v_t_now = (half * (xb.T * (A_t.T * S_t1 * A_t + Q_t) * xb + ub.T *
+ (B_t.T * S_t1 * A_t + P_t) * xb + xb.T *
+ (A_t.T * S_t1 * B_t + P_t.T) * ub + ub.T *
+ (B_t.T * S_t1 * B_t + R_t) * ub) + xb.T *
+ (A_t.T * sb_t1 + qb_t + half * A_t.T * S_t1 * cb_t) + ub.T *
+ (B_t.T * sb_t1 + rb_t + half * B_t.T * S_t1 * cb_t) +
+ half * cb_t.T * S_t1 * (A_t * xb + B_t * ub + cb_t) +
+ cb_t.T * sb_t1 + s_t1 + q_t)
- v_t_now = (
- half * (xb.T * (A_t.T * S_t1 * A_t + Q_t) * xb +
- ub.T * (B_t.T * S_t1 * A_t + P_t) * xb +
- xb.T * (A_t.T * S_t1 * B_t + P_t.T) * ub +
- ub.T * (B_t.T * S_t1 * B_t + R_t) * ub) +
- xb.T * (A_t.T * sb_t1 + qb_t + A_t.T * S_t1 * cb_t) +
- ub.T * (B_t.T * sb_t1 + rb_t + B_t.T * S_t1 * cb_t) +
- half * cb_t.T * S_t1 * cb_t +
- cb_t.T * sb_t1 + s_t1 + q_t)
+ v_t_now = (half * (xb.T * (A_t.T * S_t1 * A_t + Q_t) * xb + ub.T *
+ (B_t.T * S_t1 * A_t + P_t) * xb + xb.T *
+ (A_t.T * S_t1 * B_t + P_t.T) * ub + ub.T *
+ (B_t.T * S_t1 * B_t + R_t) * ub) + xb.T *
+ (A_t.T * sb_t1 + qb_t + A_t.T * S_t1 * cb_t) + ub.T *
+ (B_t.T * sb_t1 + rb_t + B_t.T * S_t1 * cb_t) +
+ half * cb_t.T * S_t1 * cb_t + cb_t.T * sb_t1 + s_t1 + q_t)
- C_t = B_t.T * S_t1 * A_t + P_t
- E_t = B_t.T * S_t1 * B_t + R_t
- E_t_I = sympy.MatrixSymbol('E_t^-1', E_t.cols, E_t.rows)
- L_t = -E_t_I * C_t
- eb_t = B_t.T * S_t1 * cb_t + B_t.T * sb_t1 + rb_t
- lb_t = -E_t_I * eb_t
- D_t = A_t.T * S_t1 * A_t + Q_t
- db_t = A_t.T * S_t1 * cb_t + A_t.T * sb_t1 + qb_t
+ C_t = B_t.T * S_t1 * A_t + P_t
+ E_t = B_t.T * S_t1 * B_t + R_t
+ E_t_I = sympy.MatrixSymbol('E_t^-1', E_t.cols, E_t.rows)
+ L_t = -E_t_I * C_t
+ eb_t = B_t.T * S_t1 * cb_t + B_t.T * sb_t1 + rb_t
+ lb_t = -E_t_I * eb_t
+ D_t = A_t.T * S_t1 * A_t + Q_t
+ db_t = A_t.T * S_t1 * cb_t + A_t.T * sb_t1 + qb_t
- v_t_now = (
- half * (xb.T * D_t * xb + ub.T * C_t * xb +
- xb.T * C_t.T * ub + ub.T * E_t * ub) +
- xb.T * db_t + ub.T * eb_t +
- half * cb_t.T * S_t1 * cb_t +
- cb_t.T * sb_t1 + s_t1 + q_t)
+ v_t_now = (half * (xb.T * D_t * xb + ub.T * C_t * xb + xb.T * C_t.T * ub +
+ ub.T * E_t * ub) + xb.T * db_t + ub.T * eb_t +
+ half * cb_t.T * S_t1 * cb_t + cb_t.T * sb_t1 + s_t1 + q_t)
- v_t_final = (
- half * xb.T * (D_t + L_t.T * C_t + C_t.T * L_t + L_t.T * E_t * L_t) * xb +
- xb.T * (C_t.T * lb_t + L_t.T * E_t * lb_t + db_t + L_t.T * eb_t) +
- half * lb_t.T * E_t * lb_t +
- lb_t.T * eb_t +
- cb_t.T * sb_t1 + s_t1 + q_t + half * cb_t.T * S_t1 * cb_t
- )
- verify_arguments(v_t_final, xb, E_t_I)
- verify_equivalent(v_t.subs(ub, L_t * xb + lb_t), v_t_final, {E_t_I: E_t})
+ v_t_final = (half * xb.T *
+ (D_t + L_t.T * C_t + C_t.T * L_t + L_t.T * E_t * L_t) * xb +
+ xb.T *
+ (C_t.T * lb_t + L_t.T * E_t * lb_t + db_t + L_t.T * eb_t) +
+ half * lb_t.T * E_t * lb_t + lb_t.T * eb_t + cb_t.T * sb_t1 +
+ s_t1 + q_t + half * cb_t.T * S_t1 * cb_t)
+ verify_arguments(v_t_final, xb, E_t_I)
+ verify_equivalent(v_t.subs(ub, L_t * xb + lb_t), v_t_final, {E_t_I: E_t})
- def v_t_from_s(this_S_t, this_sb_t, this_s_t):
- return half * xb.T * this_S_t * xb + xb.T * this_sb_t + this_s_t
+ def v_t_from_s(this_S_t, this_sb_t, this_s_t):
+ return half * xb.T * this_S_t * xb + xb.T * this_sb_t + this_s_t
- S_t_new_first = D_t + L_t.T * C_t + C_t.T * L_t + L_t.T * E_t * L_t
- sb_t_new_first = db_t - C_t.T * E_t_I * eb_t
- s_t_new_first = (half * lb_t.T * E_t * lb_t +
- lb_t.T * eb_t +
- cb_t.T * sb_t1 +
- s_t1 + q_t +
- half * cb_t.T * S_t1 * cb_t)
- verify_equivalent(v_t_from_s(S_t_new_first, sb_t_new_first, s_t_new_first),
- v_t_final, {E_t_I: E_t})
+ S_t_new_first = D_t + L_t.T * C_t + C_t.T * L_t + L_t.T * E_t * L_t
+ sb_t_new_first = db_t - C_t.T * E_t_I * eb_t
+ s_t_new_first = (half * lb_t.T * E_t * lb_t + lb_t.T * eb_t +
+ cb_t.T * sb_t1 + s_t1 + q_t + half * cb_t.T * S_t1 * cb_t)
+ verify_equivalent(v_t_from_s(S_t_new_first, sb_t_new_first, s_t_new_first),
+ v_t_final, {E_t_I: E_t})
- S_t_new_end = D_t - C_t.T * E_t_I * C_t
- sb_t_new_end = db_t - C_t.T * E_t_I * eb_t
- s_t_new_end = (q_t - half * eb_t.T * E_t_I * eb_t +
- half * cb_t.T * S_t1 * cb_t + cb_t.T * sb_t1 + s_t1)
- verify_equivalent(v_t_from_s(S_t_new_end, sb_t_new_end, s_t_new_end),
- v_t_final, {E_t_I: E_t})
+ S_t_new_end = D_t - C_t.T * E_t_I * C_t
+ sb_t_new_end = db_t - C_t.T * E_t_I * eb_t
+ s_t_new_end = (q_t - half * eb_t.T * E_t_I * eb_t +
+ half * cb_t.T * S_t1 * cb_t + cb_t.T * sb_t1 + s_t1)
+ verify_equivalent(v_t_from_s(S_t_new_end, sb_t_new_end, s_t_new_end),
+ v_t_final, {E_t_I: E_t})
+
def check_forwards_cost():
- v_tb = half * xb.T * S_tb * xb + xb.T * sb_tb + s_tb
- verify_arguments(v_tb, xb)
- g_tb = A_tb * xb_t1 + B_tb * ub + cb_tb
- verify_arguments(g_tb, xb_t1, ub)
- c_t1b = make_c_t().subs(xb, g_tb)
- verify_arguments(c_t1b, xb_t1, ub)
- v_t1b = v_tb.subs(xb, g_tb) + c_t1b
- verify_arguments(v_t1b, xb_t1, ub)
+ v_tb = half * xb.T * S_tb * xb + xb.T * sb_tb + s_tb
+ verify_arguments(v_tb, xb)
+ g_tb = A_tb * xb_t1 + B_tb * ub + cb_tb
+ verify_arguments(g_tb, xb_t1, ub)
+ c_t1b = make_c_t().subs(xb, g_tb)
+ verify_arguments(c_t1b, xb_t1, ub)
+ v_t1b = v_tb.subs(xb, g_tb) + c_t1b
+ verify_arguments(v_t1b, xb_t1, ub)
- v_t1b_now = (
- half * g_tb.T * S_tb * g_tb +
- g_tb.T * sb_tb + s_tb +
- half * (g_tb.T * Q_t * g_tb +
- ub.T * P_t * g_tb +
- g_tb.T * P_t.T * ub +
- ub.T * R_t * ub) +
- g_tb.T * qb_t + ub.T * rb_t + q_t)
+ v_t1b_now = (half * g_tb.T * S_tb * g_tb + g_tb.T * sb_tb + s_tb + half *
+ (g_tb.T * Q_t * g_tb + ub.T * P_t * g_tb +
+ g_tb.T * P_t.T * ub + ub.T * R_t * ub) + g_tb.T * qb_t +
+ ub.T * rb_t + q_t)
- v_t1b_for_cost = (
- half * (xb_t1.T * A_tb.T * (S_tb + Q_t) * A_tb * xb_t1 +
- xb_t1.T * A_tb.T * (S_tb + Q_t) * B_tb * ub +
- xb_t1.T * A_tb.T * (S_tb + Q_t) * cb_tb +
- ub.T * B_tb.T * (S_tb + Q_t) * A_tb * xb_t1 +
- ub.T * B_tb.T * (S_tb + Q_t) * B_tb * ub +
- ub.T * B_tb.T * (S_tb + Q_t) * cb_tb +
- cb_tb.T * (S_tb + Q_t) * A_tb * xb_t1 +
- cb_tb.T * (S_tb + Q_t) * B_tb * ub +
- cb_tb.T * (S_tb + Q_t) * cb_tb) +
- xb_t1.T * A_tb.T * sb_tb +
- ub.T * B_tb.T * sb_tb +
- cb_tb.T * sb_tb +
- s_tb +
- half * (ub.T * P_t * A_tb * xb_t1 +
- ub.T * P_t * B_tb * ub +
- ub.T * P_t * cb_tb) +
- half * (xb_t1.T * A_tb.T * P_t.T * ub +
- ub.T * B_tb.T * P_t.T * ub +
- cb_tb.T * P_t.T * ub) +
- half * ub.T * R_t * ub +
- xb_t1.T * A_tb.T * qb_t + ub.T * B_tb.T * qb_t + cb_tb.T * qb_t +
- ub.T * rb_t + q_t)
- verify_equivalent(v_t1b, v_t1b_for_cost)
+ v_t1b_for_cost = (half * (xb_t1.T * A_tb.T *
+ (S_tb + Q_t) * A_tb * xb_t1 + xb_t1.T * A_tb.T *
+ (S_tb + Q_t) * B_tb * ub + xb_t1.T * A_tb.T *
+ (S_tb + Q_t) * cb_tb + ub.T * B_tb.T *
+ (S_tb + Q_t) * A_tb * xb_t1 + ub.T * B_tb.T *
+ (S_tb + Q_t) * B_tb * ub + ub.T * B_tb.T *
+ (S_tb + Q_t) * cb_tb + cb_tb.T *
+ (S_tb + Q_t) * A_tb * xb_t1 + cb_tb.T *
+ (S_tb + Q_t) * B_tb * ub + cb_tb.T *
+ (S_tb + Q_t) * cb_tb) +
+ xb_t1.T * A_tb.T * sb_tb + ub.T * B_tb.T * sb_tb +
+ cb_tb.T * sb_tb + s_tb + half *
+ (ub.T * P_t * A_tb * xb_t1 + ub.T * P_t * B_tb * ub +
+ ub.T * P_t * cb_tb) + half *
+ (xb_t1.T * A_tb.T * P_t.T * ub +
+ ub.T * B_tb.T * P_t.T * ub + cb_tb.T * P_t.T * ub) +
+ half * ub.T * R_t * ub + xb_t1.T * A_tb.T * qb_t +
+ ub.T * B_tb.T * qb_t + cb_tb.T * qb_t + ub.T * rb_t +
+ q_t)
+ verify_equivalent(v_t1b, v_t1b_for_cost)
- S_and_Q = S_tb + Q_t
+ S_and_Q = S_tb + Q_t
- v_t1b_now = (
- half * (xb_t1.T * A_tb.T * S_and_Q * A_tb * xb_t1 +
- xb_t1.T * A_tb.T * S_and_Q * B_tb * ub +
- xb_t1.T * A_tb.T * S_and_Q * cb_tb +
- ub.T * B_tb.T * S_and_Q * A_tb * xb_t1 +
- ub.T * B_tb.T * S_and_Q * B_tb * ub +
- ub.T * B_tb.T * S_and_Q * cb_tb +
- cb_tb.T * S_and_Q * A_tb * xb_t1 +
- cb_tb.T * S_and_Q * B_tb * ub +
- cb_tb.T * S_and_Q * cb_tb) +
- xb_t1.T * A_tb.T * sb_tb +
- ub.T * B_tb.T * sb_tb +
- cb_tb.T * sb_tb +
- s_tb +
- half * (ub.T * P_t * A_tb * xb_t1 +
- ub.T * P_t * B_tb * ub +
- ub.T * P_t * cb_tb) +
- half * (xb_t1.T * A_tb.T * P_t.T * ub +
- ub.T * B_tb.T * P_t.T * ub +
- cb_tb.T * P_t.T * ub) +
- half * ub.T * R_t * ub +
- xb_t1.T * A_tb.T * qb_t +
- ub.T * B_tb.T * qb_t +
- cb_tb.T * qb_t +
- ub.T * rb_t +
- q_t)
+ v_t1b_now = (
+ half *
+ (xb_t1.T * A_tb.T * S_and_Q * A_tb * xb_t1 + xb_t1.T * A_tb.T *
+ S_and_Q * B_tb * ub + xb_t1.T * A_tb.T * S_and_Q * cb_tb + ub.T *
+ B_tb.T * S_and_Q * A_tb * xb_t1 + ub.T * B_tb.T * S_and_Q * B_tb * ub
+ + ub.T * B_tb.T * S_and_Q * cb_tb + cb_tb.T * S_and_Q * A_tb * xb_t1 +
+ cb_tb.T * S_and_Q * B_tb * ub + cb_tb.T * S_and_Q * cb_tb) +
+ xb_t1.T * A_tb.T * sb_tb + ub.T * B_tb.T * sb_tb + cb_tb.T * sb_tb +
+ s_tb + half * (ub.T * P_t * A_tb * xb_t1 + ub.T * P_t * B_tb * ub +
+ ub.T * P_t * cb_tb) + half *
+ (xb_t1.T * A_tb.T * P_t.T * ub + ub.T * B_tb.T * P_t.T * ub +
+ cb_tb.T * P_t.T * ub) + half * ub.T * R_t * ub +
+ xb_t1.T * A_tb.T * qb_t + ub.T * B_tb.T * qb_t + cb_tb.T * qb_t +
+ ub.T * rb_t + q_t)
- C_tb = B_tb.T * S_and_Q * A_tb + P_t * A_tb
- E_tb = B_tb.T * S_and_Q * B_tb + B_tb.T * P_t.T + P_t * B_tb + R_t
- E_tb_I = sympy.MatrixSymbol('Ebar_t^-1', E_tb.cols, E_tb.rows)
- L_tb = -E_tb_I * C_tb
- eb_tb = B_tb.T * S_and_Q * cb_tb + B_tb.T * sb_tb + P_t * cb_tb + B_tb.T * qb_t + rb_t
- lb_tb = -E_tb_I * eb_tb
- D_tb = A_tb.T * S_and_Q * A_tb
- db_tb = A_tb.T * S_and_Q * cb_tb + A_tb.T * (sb_tb + qb_t)
+ C_tb = B_tb.T * S_and_Q * A_tb + P_t * A_tb
+ E_tb = B_tb.T * S_and_Q * B_tb + B_tb.T * P_t.T + P_t * B_tb + R_t
+ E_tb_I = sympy.MatrixSymbol('Ebar_t^-1', E_tb.cols, E_tb.rows)
+ L_tb = -E_tb_I * C_tb
+ eb_tb = B_tb.T * S_and_Q * cb_tb + B_tb.T * sb_tb + P_t * cb_tb + B_tb.T * qb_t + rb_t
+ lb_tb = -E_tb_I * eb_tb
+ D_tb = A_tb.T * S_and_Q * A_tb
+ db_tb = A_tb.T * S_and_Q * cb_tb + A_tb.T * (sb_tb + qb_t)
- v_t1b_now = (
- half * (xb_t1.T * D_tb * xb_t1 +
- xb_t1.T * C_tb.T * ub +
- ub.T * C_tb * xb_t1 +
- ub.T * E_tb * ub) +
- xb_t1.T * db_tb +
- ub.T * eb_tb +
- half * cb_tb.T * S_and_Q * cb_tb +
- cb_tb.T * sb_tb +
- cb_tb.T * qb_t +
- s_tb + q_t)
+ v_t1b_now = (half * (xb_t1.T * D_tb * xb_t1 + xb_t1.T * C_tb.T * ub +
+ ub.T * C_tb * xb_t1 + ub.T * E_tb * ub) +
+ xb_t1.T * db_tb + ub.T * eb_tb +
+ half * cb_tb.T * S_and_Q * cb_tb + cb_tb.T * sb_tb +
+ cb_tb.T * qb_t + s_tb + q_t)
- v_t1b_final = (
- half * xb_t1.T * (D_tb - C_tb.T * E_tb_I * C_tb) * xb_t1 +
- xb_t1.T * (db_tb - C_tb.T * E_tb_I * eb_tb) +
- -half * eb_tb.T * E_tb_I * eb_tb +
- half * cb_tb.T * S_and_Q * cb_tb +
- cb_tb.T * sb_tb +
- cb_tb.T * qb_t +
- s_tb + q_t)
- verify_arguments(v_t1b_final, xb_t1, E_tb_I)
- verify_equivalent(v_t1b.subs(ub, -E_tb_I * C_tb * xb_t1 - E_tb_I * eb_tb),
- v_t1b_final, {E_tb_I: E_tb})
+ v_t1b_final = (half * xb_t1.T * (D_tb - C_tb.T * E_tb_I * C_tb) * xb_t1 +
+ xb_t1.T * (db_tb - C_tb.T * E_tb_I * eb_tb) +
+ -half * eb_tb.T * E_tb_I * eb_tb +
+ half * cb_tb.T * S_and_Q * cb_tb + cb_tb.T * sb_tb +
+ cb_tb.T * qb_t + s_tb + q_t)
+ verify_arguments(v_t1b_final, xb_t1, E_tb_I)
+ verify_equivalent(v_t1b.subs(ub, -E_tb_I * C_tb * xb_t1 - E_tb_I * eb_tb),
+ v_t1b_final, {E_tb_I: E_tb})
+
def check_forwards_u():
- S_and_Q = S_tb + Q_t
+ S_and_Q = S_tb + Q_t
- diff_start = (
- half * (xb_t1.T * A_tb.T * S_and_Q * B_tb +
- (B_tb.T * S_and_Q * A_tb * xb_t1).T +
- 2 * ub.T * B_tb.T * S_and_Q * B_tb +
- (B_tb.T * S_and_Q * cb_tb).T +
- cb_tb.T * S_and_Q * B_tb) +
- sb_tb.T * B_tb +
- half * (P_t * A_tb * xb_t1).T +
- half * xb_t1.T * A_tb.T * P_t.T +
- half * ub.T * (P_t * B_tb + B_tb.T * P_t.T) +
- half * ub.T * (B_tb.T * P_t.T + P_t * B_tb) +
- half * (P_t * cb_tb).T +
- half * cb_tb.T * P_t.T +
- ub.T * R_t +
- (B_tb.T * qb_t).T + rb_t.T)
- verify_arguments(diff_start, xb_t1, ub)
+ diff_start = (half *
+ (xb_t1.T * A_tb.T * S_and_Q * B_tb +
+ (B_tb.T * S_and_Q * A_tb * xb_t1).T +
+ 2 * ub.T * B_tb.T * S_and_Q * B_tb +
+ (B_tb.T * S_and_Q * cb_tb).T + cb_tb.T * S_and_Q * B_tb) +
+ sb_tb.T * B_tb + half * (P_t * A_tb * xb_t1).T +
+ half * xb_t1.T * A_tb.T * P_t.T + half * ub.T *
+ (P_t * B_tb + B_tb.T * P_t.T) + half * ub.T *
+ (B_tb.T * P_t.T + P_t * B_tb) + half * (P_t * cb_tb).T +
+ half * cb_tb.T * P_t.T + ub.T * R_t + (B_tb.T * qb_t).T +
+ rb_t.T)
+ verify_arguments(diff_start, xb_t1, ub)
- diff_end = (
- xb_t1.T * (A_tb.T * S_and_Q * B_tb + A_tb.T * P_t.T) +
- ub.T * (B_tb.T * S_and_Q * B_tb + B_tb.T * P_t.T + P_t * B_tb + R_t) +
- cb_tb.T * S_and_Q * B_tb +
- sb_tb.T * B_tb +
- cb_tb.T * P_t.T +
- qb_t.T * B_tb +
- rb_t.T)
- verify_equivalent(diff_start, diff_end)
+ diff_end = (xb_t1.T * (A_tb.T * S_and_Q * B_tb + A_tb.T * P_t.T) + ub.T *
+ (B_tb.T * S_and_Q * B_tb + B_tb.T * P_t.T + P_t * B_tb + R_t) +
+ cb_tb.T * S_and_Q * B_tb + sb_tb.T * B_tb + cb_tb.T * P_t.T +
+ qb_t.T * B_tb + rb_t.T)
+ verify_equivalent(diff_start, diff_end)
+
def main():
- sympy.init_printing(use_unicode=True)
- check_backwards_cost()
- check_forwards_cost()
- check_forwards_u()
+ sympy.init_printing(use_unicode=True)
+ check_backwards_cost()
+ check_forwards_cost()
+ check_forwards_u()
+
if __name__ == '__main__':
- main()
+ main()
diff --git a/y2014/control_loops/python/polydrivetrain.py b/y2014/control_loops/python/polydrivetrain.py
index cdaee1b..05b6658 100755
--- a/y2014/control_loops/python/polydrivetrain.py
+++ b/y2014/control_loops/python/polydrivetrain.py
@@ -12,20 +12,22 @@
FLAGS = gflags.FLAGS
try:
- gflags.DEFINE_bool('plot', False, 'If true, plot the loop response.')
+ gflags.DEFINE_bool('plot', False, 'If true, plot the loop response.')
except gflags.DuplicateFlagError:
- pass
+ pass
+
def main(argv):
- if FLAGS.plot:
- polydrivetrain.PlotPolyDrivetrainMotions(drivetrain.kDrivetrain)
- elif len(argv) != 7:
- glog.fatal('Expected .h file name and .cc file name')
- else:
- polydrivetrain.WritePolyDrivetrain(argv[1:3], argv[3:5], argv[5:7], 'y2014',
- drivetrain.kDrivetrain)
+ if FLAGS.plot:
+ polydrivetrain.PlotPolyDrivetrainMotions(drivetrain.kDrivetrain)
+ elif len(argv) != 7:
+ glog.fatal('Expected .h file name and .cc file name')
+ else:
+ polydrivetrain.WritePolyDrivetrain(argv[1:3], argv[3:5], argv[5:7],
+ 'y2014', drivetrain.kDrivetrain)
+
if __name__ == '__main__':
- argv = FLAGS(sys.argv)
- glog.init()
- sys.exit(main(argv))
+ argv = FLAGS(sys.argv)
+ glog.init()
+ sys.exit(main(argv))
diff --git a/y2014/control_loops/python/polydrivetrain_test.py b/y2014/control_loops/python/polydrivetrain_test.py
index 8e0176e..a5bac4a 100755
--- a/y2014/control_loops/python/polydrivetrain_test.py
+++ b/y2014/control_loops/python/polydrivetrain_test.py
@@ -10,73 +10,72 @@
class TestVelocityDrivetrain(unittest.TestCase):
- def MakeBox(self, x1_min, x1_max, x2_min, x2_max):
- H = numpy.matrix([[1, 0],
- [-1, 0],
- [0, 1],
- [0, -1]])
- K = numpy.matrix([[x1_max],
- [-x1_min],
- [x2_max],
- [-x2_min]])
- return polytope.HPolytope(H, K)
- def test_coerce_inside(self):
- """Tests coercion when the point is inside the box."""
- box = self.MakeBox(1, 2, 1, 2)
+ def MakeBox(self, x1_min, x1_max, x2_min, x2_max):
+ H = numpy.matrix([[1, 0], [-1, 0], [0, 1], [0, -1]])
+ K = numpy.matrix([[x1_max], [-x1_min], [x2_max], [-x2_min]])
+ return polytope.HPolytope(H, K)
- # x1 = x2
- K = numpy.matrix([[1, -1]])
- w = 0
+ def test_coerce_inside(self):
+ """Tests coercion when the point is inside the box."""
+ box = self.MakeBox(1, 2, 1, 2)
- assert_array_equal(polydrivetrain.CoerceGoal(box, K, w,
- numpy.matrix([[1.5], [1.5]])),
- numpy.matrix([[1.5], [1.5]]))
+ # x1 = x2
+ K = numpy.matrix([[1, -1]])
+ w = 0
- def test_coerce_outside_intersect(self):
- """Tests coercion when the line intersects the box."""
- box = self.MakeBox(1, 2, 1, 2)
+ assert_array_equal(
+ polydrivetrain.CoerceGoal(box, K, w, numpy.matrix([[1.5], [1.5]])),
+ numpy.matrix([[1.5], [1.5]]))
- # x1 = x2
- K = numpy.matrix([[1, -1]])
- w = 0
+ def test_coerce_outside_intersect(self):
+ """Tests coercion when the line intersects the box."""
+ box = self.MakeBox(1, 2, 1, 2)
- assert_array_equal(polydrivetrain.CoerceGoal(box, K, w, numpy.matrix([[5], [5]])),
- numpy.matrix([[2.0], [2.0]]))
+ # x1 = x2
+ K = numpy.matrix([[1, -1]])
+ w = 0
- def test_coerce_outside_no_intersect(self):
- """Tests coercion when the line does not intersect the box."""
- box = self.MakeBox(3, 4, 1, 2)
+ assert_array_equal(
+ polydrivetrain.CoerceGoal(box, K, w, numpy.matrix([[5], [5]])),
+ numpy.matrix([[2.0], [2.0]]))
- # x1 = x2
- K = numpy.matrix([[1, -1]])
- w = 0
+ def test_coerce_outside_no_intersect(self):
+ """Tests coercion when the line does not intersect the box."""
+ box = self.MakeBox(3, 4, 1, 2)
- assert_array_equal(polydrivetrain.CoerceGoal(box, K, w, numpy.matrix([[5], [5]])),
- numpy.matrix([[3.0], [2.0]]))
+ # x1 = x2
+ K = numpy.matrix([[1, -1]])
+ w = 0
- def test_coerce_middle_of_edge(self):
- """Tests coercion when the line intersects the middle of an edge."""
- box = self.MakeBox(0, 4, 1, 2)
+ assert_array_equal(
+ polydrivetrain.CoerceGoal(box, K, w, numpy.matrix([[5], [5]])),
+ numpy.matrix([[3.0], [2.0]]))
- # x1 = x2
- K = numpy.matrix([[-1, 1]])
- w = 0
+ def test_coerce_middle_of_edge(self):
+ """Tests coercion when the line intersects the middle of an edge."""
+ box = self.MakeBox(0, 4, 1, 2)
- assert_array_equal(polydrivetrain.CoerceGoal(box, K, w, numpy.matrix([[5], [5]])),
- numpy.matrix([[2.0], [2.0]]))
+ # x1 = x2
+ K = numpy.matrix([[-1, 1]])
+ w = 0
- def test_coerce_perpendicular_line(self):
- """Tests coercion when the line does not intersect and is in quadrant 2."""
- box = self.MakeBox(1, 2, 1, 2)
+ assert_array_equal(
+ polydrivetrain.CoerceGoal(box, K, w, numpy.matrix([[5], [5]])),
+ numpy.matrix([[2.0], [2.0]]))
- # x1 = -x2
- K = numpy.matrix([[1, 1]])
- w = 0
+ def test_coerce_perpendicular_line(self):
+ """Tests coercion when the line does not intersect and is in quadrant 2."""
+ box = self.MakeBox(1, 2, 1, 2)
- assert_array_equal(polydrivetrain.CoerceGoal(box, K, w, numpy.matrix([[5], [5]])),
- numpy.matrix([[1.0], [1.0]]))
+ # x1 = -x2
+ K = numpy.matrix([[1, 1]])
+ w = 0
+
+ assert_array_equal(
+ polydrivetrain.CoerceGoal(box, K, w, numpy.matrix([[5], [5]])),
+ numpy.matrix([[1.0], [1.0]]))
if __name__ == '__main__':
- unittest.main()
+ unittest.main()
diff --git a/y2014/control_loops/python/shooter.py b/y2014/control_loops/python/shooter.py
index c3e8d86..e3ad903 100755
--- a/y2014/control_loops/python/shooter.py
+++ b/y2014/control_loops/python/shooter.py
@@ -264,8 +264,9 @@
sprung_shooter = SprungShooterDeltaU()
shooter = ShooterDeltaU()
- loop_writer = control_loop.ControlLoopWriter(
- "Shooter", [sprung_shooter, shooter], namespaces=namespaces)
+ loop_writer = control_loop.ControlLoopWriter("Shooter",
+ [sprung_shooter, shooter],
+ namespaces=namespaces)
loop_writer.AddConstant(
control_loop.Constant("kMaxExtension", "%f",