Run yapf on all python files in the repo
Signed-off-by: Ravago Jones <ravagojones@gmail.com>
Change-Id: I221e04c3f517fab8535b22551553799e0fee7a80
diff --git a/y2018/control_loops/python/extended_lqr.py b/y2018/control_loops/python/extended_lqr.py
index 4eecee9..9a4705b 100755
--- a/y2018/control_loops/python/extended_lqr.py
+++ b/y2018/control_loops/python/extended_lqr.py
@@ -11,16 +11,17 @@
class ArmDynamics(object):
- def __init__(self, dt):
- self.dt = dt
- self.l1 = 1.0
- self.l2 = 0.8
- self.num_states = 4
- self.num_inputs = 2
+ def __init__(self, dt):
+ self.dt = dt
- def dynamics(self, X, U):
- """Calculates the dynamics for a double jointed arm.
+ self.l1 = 1.0
+ self.l2 = 0.8
+ self.num_states = 4
+ self.num_inputs = 2
+
+ def dynamics(self, X, U):
+ """Calculates the dynamics for a double jointed arm.
Args:
X, numpy.matrix(4, 1), The state. [theta1, omega1, theta2, omega2]
@@ -29,48 +30,53 @@
Returns:
numpy.matrix(4, 1), The derivative of the dynamics.
"""
- return numpy.matrix([[X[1, 0]],
- [U[0, 0]],
- [X[3, 0]],
- [U[1, 0]]])
+ return numpy.matrix([[X[1, 0]], [U[0, 0]], [X[3, 0]], [U[1, 0]]])
- def discrete_dynamics(self, X, U):
- return RungeKutta(lambda startingX: self.dynamics(startingX, U), X, dt)
+ def discrete_dynamics(self, X, U):
+ return RungeKutta(lambda startingX: self.dynamics(startingX, U), X, dt)
- def inverse_discrete_dynamics(self, X, U):
- return RungeKutta(lambda startingX: -self.dynamics(startingX, U), X, dt)
+ def inverse_discrete_dynamics(self, X, U):
+ return RungeKutta(lambda startingX: -self.dynamics(startingX, U), X,
+ dt)
+
# Simple implementation for a quadratic cost function.
class ArmCostFunction:
- def __init__(self, dt, dynamics):
- self.num_states = 4
- self.num_inputs = 2
- self.dt = dt
- self.dynamics = dynamics
- q_pos = 0.5
- q_vel = 1.65
- self.Q = numpy.matrix(numpy.diag([
- 1.0 / (q_pos ** 2.0), 1.0 / (q_vel ** 2.0),
- 1.0 / (q_pos ** 2.0), 1.0 / (q_vel ** 2.0)]))
+ def __init__(self, dt, dynamics):
+ self.num_states = 4
+ self.num_inputs = 2
+ self.dt = dt
+ self.dynamics = dynamics
- self.R = numpy.matrix(numpy.diag([1.0 / (12.0 ** 2.0),
- 1.0 / (12.0 ** 2.0)]))
+ q_pos = 0.5
+ q_vel = 1.65
+ self.Q = numpy.matrix(
+ numpy.diag([
+ 1.0 / (q_pos**2.0), 1.0 / (q_vel**2.0), 1.0 / (q_pos**2.0),
+ 1.0 / (q_vel**2.0)
+ ]))
- final_A = numerical_jacobian_x(self.dynamics.discrete_dynamics,
- numpy.matrix(numpy.zeros((4, 1))),
- numpy.matrix(numpy.zeros((2, 1))))
- final_B = numerical_jacobian_u(self.dynamics.discrete_dynamics,
- numpy.matrix(numpy.zeros((4, 1))),
- numpy.matrix(numpy.zeros((2, 1))))
- print 'Final A', final_A
- print 'Final B', final_B
- K, self.S = controls.dlqr(
- final_A, final_B, self.Q, self.R, optimal_cost_function=True)
- print 'Final eig:', numpy.linalg.eig(final_A - final_B * K)
+ self.R = numpy.matrix(
+ numpy.diag([1.0 / (12.0**2.0), 1.0 / (12.0**2.0)]))
- def final_cost(self, X, U):
- """Computes the final cost of being at X
+ final_A = numerical_jacobian_x(self.dynamics.discrete_dynamics,
+ numpy.matrix(numpy.zeros((4, 1))),
+ numpy.matrix(numpy.zeros((2, 1))))
+ final_B = numerical_jacobian_u(self.dynamics.discrete_dynamics,
+ numpy.matrix(numpy.zeros((4, 1))),
+ numpy.matrix(numpy.zeros((2, 1))))
+ print 'Final A', final_A
+ print 'Final B', final_B
+ K, self.S = controls.dlqr(final_A,
+ final_B,
+ self.Q,
+ self.R,
+ optimal_cost_function=True)
+ print 'Final eig:', numpy.linalg.eig(final_A - final_B * K)
+
+ def final_cost(self, X, U):
+ """Computes the final cost of being at X
Args:
X: numpy.matrix(self.num_states, 1)
@@ -79,10 +85,10 @@
Returns:
numpy.matrix(1, 1), The quadratic cost of being at X
"""
- return 0.5 * X.T * self.S * X
+ return 0.5 * X.T * self.S * X
- 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)
@@ -91,10 +97,10 @@
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
- def estimate_Q_final(self, X_hat):
- """Returns the quadraticized final Q around X_hat.
+ 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)
@@ -104,13 +110,13 @@
Result:
numpy.matrix(self.num_states, self.num_states)
"""
- zero_U = numpy.matrix(numpy.zeros((self.num_inputs, 1)))
- print 'S', self.S
- print 'Q_final', numerical_jacobian_x_x(self.final_cost, X_hat, zero_U)
- return numerical_jacobian_x_x(self.final_cost, X_hat, zero_U)
+ zero_U = numpy.matrix(numpy.zeros((self.num_inputs, 1)))
+ print 'S', self.S
+ print 'Q_final', numerical_jacobian_x_x(self.final_cost, X_hat, zero_U)
+ 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.
@@ -118,24 +124,26 @@
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
class SkidSteerDynamics(object):
- def __init__(self, dt):
- self.width = 0.2
- self.dt = dt
- self.num_states = 3
- self.num_inputs = 2
- def dynamics(self, X, U):
- """Calculates the dynamics for a 2 wheeled robot.
+ def __init__(self, dt):
+ self.width = 0.2
+ self.dt = dt
+ self.num_states = 3
+ self.num_inputs = 2
+
+ def dynamics(self, X, U):
+ """Calculates the dynamics for a 2 wheeled robot.
Args:
X, numpy.matrix(3, 1), The state. [x, y, theta]
@@ -144,34 +152,34 @@
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]) / self.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]) / self.width]])
- def discrete_dynamics(self, X, U):
- return RungeKutta(lambda startingX: self.dynamics(startingX, U), X, dt)
+ def discrete_dynamics(self, X, U):
+ return RungeKutta(lambda startingX: self.dynamics(startingX, U), X, dt)
- def inverse_discrete_dynamics(self, X, U):
- return RungeKutta(lambda startingX: -self.dynamics(startingX, U), X, dt)
+ def inverse_discrete_dynamics(self, X, U):
+ return RungeKutta(lambda startingX: -self.dynamics(startingX, U), X,
+ dt)
# Simple implementation for a quadratic cost function.
class CostFunction:
- def __init__(self, dt):
- self.num_states = 3
- self.num_inputs = 2
- self.dt = dt
- self.Q = numpy.matrix([[0.1, 0, 0],
- [0, 0.6, 0],
- [0, 0, 0.1]]) / self.dt / self.dt
- self.R = numpy.matrix([[0.40, 0],
- [0, 0.40]]) / self.dt / self.dt
- def final_cost(self, X, U):
- """Computes the final cost of being at X
+ def __init__(self, dt):
+ self.num_states = 3
+ self.num_inputs = 2
+ self.dt = dt
+ self.Q = numpy.matrix([[0.1, 0, 0], [0, 0.6, 0], [0, 0, 0.1]
+ ]) / self.dt / self.dt
+ self.R = numpy.matrix([[0.40, 0], [0, 0.40]]) / self.dt / self.dt
+
+ def final_cost(self, X, U):
+ """Computes the final cost of being at X
Args:
X: numpy.matrix(self.num_states, 1)
@@ -180,10 +188,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)
@@ -192,10 +200,10 @@
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
- def estimate_Q_final(self, X_hat):
- """Returns the quadraticized final Q around X_hat.
+ 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)
@@ -205,11 +213,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.
@@ -217,23 +225,27 @@
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 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 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.
@@ -246,20 +258,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.
@@ -272,355 +285,426 @@
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)
class ELQR(object):
- def __init__(self, dynamics, cost):
- self.dynamics = dynamics
- self.cost = cost
-
- def Solve(self, x_hat_initial, horizon, iterations):
- l = horizon
- num_states = self.dynamics.num_states
- num_inputs = self.dynamics.num_inputs
- self.S_bar_t = [numpy.matrix(numpy.zeros((num_states, num_states))) for _ in range(l + 1)]
- self.s_bar_t = [numpy.matrix(numpy.zeros((num_states, 1))) for _ in range(l + 1)]
- self.s_scalar_bar_t = [numpy.matrix(numpy.zeros((1, 1))) for _ in range(l + 1)]
- self.L_t = [numpy.matrix(numpy.zeros((num_inputs, num_states))) for _ in range(l + 1)]
- self.l_t = [numpy.matrix(numpy.zeros((num_inputs, 1))) for _ in range(l + 1)]
- self.L_bar_t = [numpy.matrix(numpy.zeros((num_inputs, num_states))) for _ in range(l + 1)]
- self.l_bar_t = [numpy.matrix(numpy.zeros((num_inputs, 1))) for _ in range(l + 1)]
+ def __init__(self, dynamics, cost):
+ self.dynamics = dynamics
+ self.cost = cost
- self.S_t = [numpy.matrix(numpy.zeros((num_states, num_states))) for _ in range(l + 1)]
- self.s_t = [numpy.matrix(numpy.zeros((num_states, 1))) for _ in range(l + 1)]
- self.s_scalar_t = [numpy.matrix(numpy.zeros((1, 1))) for _ in range(l + 1)]
+ def Solve(self, x_hat_initial, horizon, iterations):
+ l = horizon
+ num_states = self.dynamics.num_states
+ num_inputs = self.dynamics.num_inputs
+ self.S_bar_t = [
+ numpy.matrix(numpy.zeros((num_states, num_states)))
+ for _ in range(l + 1)
+ ]
+ self.s_bar_t = [
+ numpy.matrix(numpy.zeros((num_states, 1))) for _ in range(l + 1)
+ ]
+ self.s_scalar_bar_t = [
+ numpy.matrix(numpy.zeros((1, 1))) for _ in range(l + 1)
+ ]
- self.last_x_hat_t = [numpy.matrix(numpy.zeros((num_states, 1))) for _ in range(l + 1)]
+ self.L_t = [
+ numpy.matrix(numpy.zeros((num_inputs, num_states)))
+ for _ in range(l + 1)
+ ]
+ self.l_t = [
+ numpy.matrix(numpy.zeros((num_inputs, 1))) for _ in range(l + 1)
+ ]
+ self.L_bar_t = [
+ numpy.matrix(numpy.zeros((num_inputs, num_states)))
+ for _ in range(l + 1)
+ ]
+ self.l_bar_t = [
+ numpy.matrix(numpy.zeros((num_inputs, 1))) for _ in range(l + 1)
+ ]
- # Iterate the solver
- for a in range(iterations):
- x_hat = x_hat_initial
- u_t = self.L_t[0] * x_hat + self.l_t[0]
- self.S_bar_t[0] = numpy.matrix(numpy.zeros((num_states, num_states)))
- self.s_bar_t[0] = numpy.matrix(numpy.zeros((num_states, 1)))
- self.s_scalar_bar_t[0] = numpy.matrix([[0]])
+ self.S_t = [
+ numpy.matrix(numpy.zeros((num_states, num_states)))
+ for _ in range(l + 1)
+ ]
+ self.s_t = [
+ numpy.matrix(numpy.zeros((num_states, 1))) for _ in range(l + 1)
+ ]
+ self.s_scalar_t = [
+ numpy.matrix(numpy.zeros((1, 1))) for _ in range(l + 1)
+ ]
- self.last_x_hat_t[0] = x_hat_initial
+ self.last_x_hat_t = [
+ numpy.matrix(numpy.zeros((num_states, 1))) for _ in range(l + 1)
+ ]
- Q_t = numerical_jacobian_x_x(self.cost.cost, x_hat_initial, u_t)
- P_t = numerical_jacobian_x_u(self.cost.cost, x_hat_initial, u_t)
- R_t = numerical_jacobian_u_u(self.cost.cost, x_hat_initial, u_t)
+ # Iterate the solver
+ for a in range(iterations):
+ x_hat = x_hat_initial
+ u_t = self.L_t[0] * x_hat + self.l_t[0]
+ self.S_bar_t[0] = numpy.matrix(
+ numpy.zeros((num_states, num_states)))
+ self.s_bar_t[0] = numpy.matrix(numpy.zeros((num_states, 1)))
+ self.s_scalar_bar_t[0] = numpy.matrix([[0]])
- q_t = numerical_jacobian_x(self.cost.cost, x_hat_initial, u_t).T \
- - Q_t * x_hat_initial - P_t.T * u_t
- r_t = numerical_jacobian_u(self.cost.cost, x_hat_initial, u_t).T \
- - P_t * x_hat_initial - R_t * u_t
+ self.last_x_hat_t[0] = x_hat_initial
- q_scalar_t = self.cost.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_t = numerical_jacobian_x_x(self.cost.cost, x_hat_initial, u_t)
+ P_t = numerical_jacobian_x_u(self.cost.cost, x_hat_initial, u_t)
+ R_t = numerical_jacobian_u_u(self.cost.cost, x_hat_initial, u_t)
- start_A_t = numerical_jacobian_x(self.dynamics.discrete_dynamics, x_hat_initial, u_t)
- start_B_t = numerical_jacobian_u(self.dynamics.discrete_dynamics, x_hat_initial, u_t)
- x_hat_next = self.dynamics.discrete_dynamics(x_hat_initial, u_t)
- start_c_t = x_hat_next - start_A_t * x_hat_initial - start_B_t * u_t
+ q_t = numerical_jacobian_x(self.cost.cost, x_hat_initial, u_t).T \
+ - Q_t * x_hat_initial - P_t.T * u_t
+ r_t = numerical_jacobian_u(self.cost.cost, x_hat_initial, u_t).T \
+ - P_t * x_hat_initial - R_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)
+ q_scalar_t = self.cost.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
- 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
+ start_A_t = numerical_jacobian_x(self.dynamics.discrete_dynamics,
+ x_hat_initial, u_t)
+ start_B_t = numerical_jacobian_u(self.dynamics.discrete_dynamics,
+ x_hat_initial, u_t)
+ x_hat_next = self.dynamics.discrete_dynamics(x_hat_initial, u_t)
+ start_c_t = x_hat_next - start_A_t * x_hat_initial - start_B_t * u_t
- self.L_bar_t[1] = B_svd_inv
- self.l_bar_t[1] = -B_svd_inv * (start_A_t * x_hat_initial + start_c_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)
- self.S_bar_t[1] = self.L_bar_t[1].T * R_t * self.L_bar_t[1]
+ 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
- TotalS_1 = start_B_t.T * self.S_t[1] * start_B_t + R_t
- Totals_1 = start_B_t.T * self.S_t[1] * (start_c_t + start_A_t * x_hat_initial) \
- + start_B_t.T * self.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) * self.S_t[1] * (start_c_t + start_A_t * x_hat_initial) \
- + self.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) * self.s_t[1]
+ self.L_bar_t[1] = B_svd_inv
+ self.l_bar_t[1] = -B_svd_inv * (start_A_t * x_hat_initial +
+ 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
+ self.S_bar_t[1] = self.L_bar_t[1].T * R_t * self.L_bar_t[1]
- # TODO(austin): Disable this if we are controlable. It should not be needed then.
- S_bar_1_eigh_eigenvalues, S_bar_1_eigh_eigenvectors = \
- numpy.linalg.eigh(self.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
+ TotalS_1 = start_B_t.T * self.S_t[1] * start_B_t + R_t
+ Totals_1 = start_B_t.T * self.S_t[1] * (start_c_t + start_A_t * x_hat_initial) \
+ + start_B_t.T * self.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) * self.S_t[1] * (start_c_t + start_A_t * x_hat_initial) \
+ + self.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) * self.s_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
+ 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
- print 'Min u', -numpy.linalg.solve(TotalS_1, Totals_1)
- print 'Min x_hat', optimal_x_1
- self.s_bar_t[1] = -self.s_t[1] - (S_bar_stiff + self.S_t[1]) * optimal_x_1
- self.s_scalar_bar_t[1] = 0.5 * (optimal_u_1.T * TotalS_1 * optimal_u_1 \
- - optimal_x_1.T * (S_bar_stiff + self.S_t[1]) * optimal_x_1) \
- + optimal_u_1.T * Totals_1 \
- - optimal_x_1.T * (self.s_bar_t[1] + self.s_t[1]) \
- - self.s_scalar_t[1] + Totals_scalar_1
+ # TODO(austin): Disable this if we are controlable. It should not be needed then.
+ S_bar_1_eigh_eigenvalues, S_bar_1_eigh_eigenvectors = \
+ numpy.linalg.eigh(self.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 '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 * self.S_t[0] * x_hat_initial) \
- + x_hat_initial.T * self.s_t[0] + self.s_scalar_t[0]
+ 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 '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' % (self.L_t[0],)).replace('\n', '\n ')
- print ('l[ 0]: %s' % (self.l_t[0],)).replace('\n', '\n ')
+ print 'Min u', -numpy.linalg.solve(TotalS_1, Totals_1)
+ print 'Min x_hat', optimal_x_1
+ self.s_bar_t[1] = -self.s_t[1] - (S_bar_stiff +
+ self.S_t[1]) * optimal_x_1
+ self.s_scalar_bar_t[1] = 0.5 * (optimal_u_1.T * TotalS_1 * optimal_u_1 \
+ - optimal_x_1.T * (S_bar_stiff + self.S_t[1]) * optimal_x_1) \
+ + optimal_u_1.T * Totals_1 \
+ - optimal_x_1.T * (self.s_bar_t[1] + self.s_t[1]) \
+ - self.s_scalar_t[1] + Totals_scalar_1
- 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 '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 * self.S_t[0] * x_hat_initial) \
+ + x_hat_initial.T * self.s_t[0] + self.s_scalar_t[0]
- # TODO(austin): optimal_x_1 is x_hat
- x_hat = -numpy.linalg.solve((self.S_t[1] + S_bar_stiff),
- (self.s_t[1] + self.s_bar_t[1]))
- print 'new xhat', x_hat
+ 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' % (self.L_t[0], )).replace('\n', '\n ')
+ print('l[ 0]: %s' % (self.l_t[0], )).replace('\n', '\n ')
- self.S_bar_t[1] = S_bar_stiff
+ 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 ')
- self.last_x_hat_t[1] = x_hat
+ # TODO(austin): optimal_x_1 is x_hat
+ x_hat = -numpy.linalg.solve((self.S_t[1] + S_bar_stiff),
+ (self.s_t[1] + self.s_bar_t[1]))
+ print 'new xhat', x_hat
- for t in range(1, l):
- print 't forward', t
- u_t = self.L_t[t] * x_hat + self.l_t[t]
+ self.S_bar_t[1] = S_bar_stiff
- x_hat_next = self.dynamics.discrete_dynamics(x_hat, u_t)
- A_bar_t = numerical_jacobian_x(self.dynamics.inverse_discrete_dynamics,
- x_hat_next, u_t)
- B_bar_t = numerical_jacobian_u(self.dynamics.inverse_discrete_dynamics,
- x_hat_next, u_t)
- c_bar_t = x_hat - A_bar_t * x_hat_next - B_bar_t * u_t
+ self.last_x_hat_t[1] = x_hat
- 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, self.L_t[t],)).replace('\n', '\n ')
- print ('l[%2d]: %s' % (t, self.l_t[t],)).replace('\n', '\n ')
- print 'u[%2d]: %s' % (t, u_t.T)
+ for t in range(1, l):
+ print 't forward', t
+ u_t = self.L_t[t] * x_hat + self.l_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 ')
+ x_hat_next = self.dynamics.discrete_dynamics(x_hat, u_t)
+ A_bar_t = numerical_jacobian_x(
+ self.dynamics.inverse_discrete_dynamics, x_hat_next, u_t)
+ B_bar_t = numerical_jacobian_u(
+ self.dynamics.inverse_discrete_dynamics, x_hat_next, u_t)
+ c_bar_t = x_hat - A_bar_t * x_hat_next - B_bar_t * u_t
- Q_t = numerical_jacobian_x_x(self.cost.cost, x_hat, u_t)
- P_t = numerical_jacobian_x_u(self.cost.cost, x_hat, u_t)
- R_t = numerical_jacobian_u_u(self.cost.cost, x_hat, 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,
+ self.L_t[t],
+ )).replace('\n', '\n ')
+ print('l[%2d]: %s' % (
+ t,
+ self.l_t[t],
+ )).replace('\n', '\n ')
+ print 'u[%2d]: %s' % (t, u_t.T)
- q_t = numerical_jacobian_x(self.cost.cost, x_hat, u_t).T \
- - Q_t * x_hat - P_t.T * u_t
- r_t = numerical_jacobian_u(self.cost.cost, x_hat, u_t).T \
- - P_t * x_hat - R_t * u_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 ')
- q_scalar_t = self.cost.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_t = numerical_jacobian_x_x(self.cost.cost, x_hat, u_t)
+ P_t = numerical_jacobian_x_u(self.cost.cost, x_hat, u_t)
+ R_t = numerical_jacobian_u_u(self.cost.cost, x_hat, u_t)
- C_bar_t = B_bar_t.T * (self.S_bar_t[t] + Q_t) * A_bar_t + P_t * A_bar_t
- D_bar_t = A_bar_t.T * (self.S_bar_t[t] + Q_t) * A_bar_t
- E_bar_t = B_bar_t.T * (self.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 * (self.s_bar_t[t] + q_t) \
- + A_bar_t.T * (self.S_bar_t[t] + Q_t) * c_bar_t
- e_bar_t = r_t + P_t * c_bar_t + B_bar_t.T * self.s_bar_t[t] \
- + B_bar_t.T * (self.S_bar_t[t] + Q_t) * c_bar_t
+ q_t = numerical_jacobian_x(self.cost.cost, x_hat, u_t).T \
+ - Q_t * x_hat - P_t.T * u_t
+ r_t = numerical_jacobian_u(self.cost.cost, x_hat, u_t).T \
+ - P_t * x_hat - R_t * u_t
- self.L_bar_t[t + 1] = -numpy.linalg.inv(E_bar_t) * C_bar_t
- self.l_bar_t[t + 1] = -numpy.linalg.inv(E_bar_t) * e_bar_t
+ q_scalar_t = self.cost.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
- self.S_bar_t[t + 1] = D_bar_t + C_bar_t.T * self.L_bar_t[t + 1]
- self.s_bar_t[t + 1] = d_bar_t + C_bar_t.T * self.l_bar_t[t + 1]
- self.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 * (self.S_bar_t[t] + Q_t) * c_bar_t \
- + c_bar_t.T * self.s_bar_t[t] + c_bar_t.T * q_t \
- + self.s_scalar_bar_t[t] + q_scalar_t
+ C_bar_t = B_bar_t.T * (self.S_bar_t[t] +
+ Q_t) * A_bar_t + P_t * A_bar_t
+ D_bar_t = A_bar_t.T * (self.S_bar_t[t] + Q_t) * A_bar_t
+ E_bar_t = B_bar_t.T * (self.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 * (self.s_bar_t[t] + q_t) \
+ + A_bar_t.T * (self.S_bar_t[t] + Q_t) * c_bar_t
+ e_bar_t = r_t + P_t * c_bar_t + B_bar_t.T * self.s_bar_t[t] \
+ + B_bar_t.T * (self.S_bar_t[t] + Q_t) * c_bar_t
- x_hat = -numpy.linalg.solve((self.S_t[t + 1] + self.S_bar_t[t + 1]),
- (self.s_t[t + 1] + self.s_bar_t[t + 1]))
- self.S_t[l] = self.cost.estimate_Q_final(x_hat)
- self.s_t[l] = self.cost.estimate_q_final(x_hat)
- x_hat = -numpy.linalg.inv(self.S_t[l] + self.S_bar_t[l]) \
- * (self.s_t[l] + self.s_bar_t[l])
+ self.L_bar_t[t + 1] = -numpy.linalg.inv(E_bar_t) * C_bar_t
+ self.l_bar_t[t + 1] = -numpy.linalg.inv(E_bar_t) * e_bar_t
- 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 = self.L_bar_t[t + 1] * x_hat + self.l_bar_t[t + 1]
+ self.S_bar_t[t + 1] = D_bar_t + C_bar_t.T * self.L_bar_t[t + 1]
+ self.s_bar_t[t + 1] = d_bar_t + C_bar_t.T * self.l_bar_t[t + 1]
+ self.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 * (self.S_bar_t[t] + Q_t) * c_bar_t \
+ + c_bar_t.T * self.s_bar_t[t] + c_bar_t.T * q_t \
+ + self.s_scalar_bar_t[t] + q_scalar_t
- x_hat_prev = self.dynamics.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, self.L_bar_t[t + 1])).replace('\n', '\n ')
- print ('l_bar[%2d]: %s' % (t + 1, self.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(self.dynamics.discrete_dynamics, x_hat_prev, u_t)
- B_t = numerical_jacobian_u(self.dynamics.discrete_dynamics, x_hat_prev, u_t)
- c_t = x_hat - A_t * x_hat_prev - B_t * u_t
+ x_hat = -numpy.linalg.solve(
+ (self.S_t[t + 1] + self.S_bar_t[t + 1]),
+ (self.s_t[t + 1] + self.s_bar_t[t + 1]))
+ self.S_t[l] = self.cost.estimate_Q_final(x_hat)
+ self.s_t[l] = self.cost.estimate_q_final(x_hat)
+ x_hat = -numpy.linalg.inv(self.S_t[l] + self.S_bar_t[l]) \
+ * (self.s_t[l] + self.s_bar_t[l])
- 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 ')
+ 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 = self.L_bar_t[t + 1] * x_hat + self.l_bar_t[t + 1]
- Q_t = numerical_jacobian_x_x(self.cost.cost, x_hat_prev, u_t)
- P_t = numerical_jacobian_x_u(self.cost.cost, x_hat_prev, u_t)
- P_T_t = numerical_jacobian_u_x(self.cost.cost, x_hat_prev, u_t)
- R_t = numerical_jacobian_u_u(self.cost.cost, x_hat_prev, u_t)
+ x_hat_prev = self.dynamics.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, self.L_bar_t[t + 1])).replace(
+ '\n', '\n ')
+ print('l_bar[%2d]: %s' % (t + 1, self.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(self.dynamics.discrete_dynamics,
+ x_hat_prev, u_t)
+ B_t = numerical_jacobian_u(self.dynamics.discrete_dynamics,
+ x_hat_prev, u_t)
+ c_t = x_hat - A_t * x_hat_prev - B_t * u_t
- q_t = numerical_jacobian_x(self.cost.cost, x_hat_prev, u_t).T \
- - Q_t * x_hat_prev - P_T_t * u_t
- r_t = numerical_jacobian_u(self.cost.cost, x_hat_prev, u_t).T \
- - P_t * x_hat_prev - R_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 ')
- q_scalar_t = self.cost.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_t = numerical_jacobian_x_x(self.cost.cost, x_hat_prev, u_t)
+ P_t = numerical_jacobian_x_u(self.cost.cost, x_hat_prev, u_t)
+ P_T_t = numerical_jacobian_u_x(self.cost.cost, x_hat_prev, u_t)
+ R_t = numerical_jacobian_u_u(self.cost.cost, x_hat_prev, u_t)
- C_t = P_t + B_t.T * self.S_t[t + 1] * A_t
- D_t = Q_t + A_t.T * self.S_t[t + 1] * A_t
- E_t = R_t + B_t.T * self.S_t[t + 1] * B_t
- d_t = q_t + A_t.T * self.s_t[t + 1] + A_t.T * self.S_t[t + 1] * c_t
- e_t = r_t + B_t.T * self.s_t[t + 1] + B_t.T * self.S_t[t + 1] * c_t
- self.L_t[t] = -numpy.linalg.inv(E_t) * C_t
- self.l_t[t] = -numpy.linalg.inv(E_t) * e_t
- self.s_t[t] = d_t + C_t.T * self.l_t[t]
- self.S_t[t] = D_t + C_t.T * self.L_t[t]
- self.s_scalar_t[t] = q_scalar_t \
- - 0.5 * e_t.T * numpy.linalg.inv(E_t) * e_t \
- + 0.5 * c_t.T * self.S_t[t + 1] * c_t \
- + c_t.T * self.s_t[t + 1] \
- + self.s_scalar_t[t + 1]
+ q_t = numerical_jacobian_x(self.cost.cost, x_hat_prev, u_t).T \
+ - Q_t * x_hat_prev - P_T_t * u_t
+ r_t = numerical_jacobian_u(self.cost.cost, x_hat_prev, u_t).T \
+ - P_t * x_hat_prev - R_t * u_t
- x_hat = -numpy.linalg.solve((self.S_t[t] + self.S_bar_t[t]),
- (self.s_t[t] + self.s_bar_t[t]))
- if t == 0:
- self.last_x_hat_t[t] = x_hat_initial
- else:
- self.last_x_hat_t[t] = x_hat
+ q_scalar_t = self.cost.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
- x_hat_t = [x_hat_initial]
+ C_t = P_t + B_t.T * self.S_t[t + 1] * A_t
+ D_t = Q_t + A_t.T * self.S_t[t + 1] * A_t
+ E_t = R_t + B_t.T * self.S_t[t + 1] * B_t
+ d_t = q_t + A_t.T * self.s_t[t + 1] + A_t.T * self.S_t[t +
+ 1] * c_t
+ e_t = r_t + B_t.T * self.s_t[t + 1] + B_t.T * self.S_t[t +
+ 1] * c_t
+ self.L_t[t] = -numpy.linalg.inv(E_t) * C_t
+ self.l_t[t] = -numpy.linalg.inv(E_t) * e_t
+ self.s_t[t] = d_t + C_t.T * self.l_t[t]
+ self.S_t[t] = D_t + C_t.T * self.L_t[t]
+ self.s_scalar_t[t] = q_scalar_t \
+ - 0.5 * e_t.T * numpy.linalg.inv(E_t) * e_t \
+ + 0.5 * c_t.T * self.S_t[t + 1] * c_t \
+ + c_t.T * self.s_t[t + 1] \
+ + self.s_scalar_t[t + 1]
- pylab.figure('states %d' % a)
- pylab.ion()
- for dim in range(num_states):
- pylab.plot(numpy.arange(len(self.last_x_hat_t)),
- [x_hat_loop[dim, 0] for x_hat_loop in self.last_x_hat_t],
- marker='o', label='Xhat[%d]' % dim)
- pylab.legend()
- pylab.draw()
+ x_hat = -numpy.linalg.solve((self.S_t[t] + self.S_bar_t[t]),
+ (self.s_t[t] + self.s_bar_t[t]))
+ if t == 0:
+ self.last_x_hat_t[t] = x_hat_initial
+ else:
+ self.last_x_hat_t[t] = x_hat
- pylab.figure('xy %d' % a)
- pylab.ion()
- pylab.plot([x_hat_loop[0, 0] for x_hat_loop in self.last_x_hat_t],
- [x_hat_loop[1, 0] for x_hat_loop in self.last_x_hat_t],
- marker='o', label='trajectory')
- pylab.legend()
- pylab.draw()
+ x_hat_t = [x_hat_initial]
- 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 * self.last_x_hat_t[t].T * self.S_t[t] * self.last_x_hat_t[t] \
- + self.last_x_hat_t[t].T * self.s_t[t] + self.s_scalar_t[t])[0, 0])
- cost_to_come.append(
- (0.5 * self.last_x_hat_t[t].T * self.S_bar_t[t] * self.last_x_hat_t[t] \
- + self.last_x_hat_t[t].T * self.s_bar_t[t] + self.s_scalar_bar_t[t])[0, 0])
- cost.append(cost_to_go[-1] + cost_to_come[-1])
- final_u_t[t] = self.L_t[t] * self.last_x_hat_t[t] + self.l_t[t]
+ pylab.figure('states %d' % a)
+ pylab.ion()
+ for dim in range(num_states):
+ pylab.plot(
+ numpy.arange(len(self.last_x_hat_t)),
+ [x_hat_loop[dim, 0] for x_hat_loop in self.last_x_hat_t],
+ marker='o',
+ label='Xhat[%d]' % dim)
+ pylab.legend()
+ pylab.draw()
- for t in range(l):
- A_t = numerical_jacobian_x(self.dynamics.discrete_dynamics,
- self.last_x_hat_t[t], final_u_t[t])
- B_t = numerical_jacobian_u(self.dynamics.discrete_dynamics,
- self.last_x_hat_t[t], final_u_t[t])
- c_t = self.dynamics.discrete_dynamics(self.last_x_hat_t[t], final_u_t[t]) \
- - A_t * self.last_x_hat_t[t] - B_t * final_u_t[t]
- print("Infeasability at", t, "is",
- ((A_t * self.last_x_hat_t[t] + B_t * final_u_t[t] + c_t) \
- - self.last_x_hat_t[t + 1]).T)
+ pylab.figure('xy %d' % a)
+ pylab.ion()
+ pylab.plot([x_hat_loop[0, 0] for x_hat_loop in self.last_x_hat_t],
+ [x_hat_loop[1, 0] for x_hat_loop in self.last_x_hat_t],
+ marker='o',
+ label='trajectory')
+ pylab.legend()
+ pylab.draw()
- 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()
+ 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 * self.last_x_hat_t[t].T * self.S_t[t] * self.last_x_hat_t[t] \
+ + self.last_x_hat_t[t].T * self.s_t[t] + self.s_scalar_t[t])[0, 0])
+ cost_to_come.append(
+ (0.5 * self.last_x_hat_t[t].T * self.S_bar_t[t] * self.last_x_hat_t[t] \
+ + self.last_x_hat_t[t].T * self.s_bar_t[t] + self.s_scalar_bar_t[t])[0, 0])
+ cost.append(cost_to_go[-1] + cost_to_come[-1])
+ final_u_t[t] = self.L_t[t] * self.last_x_hat_t[t] + self.l_t[t]
- pylab.figure('cost')
- cost_samples = numpy.arange(len(cost))
- pylab.plot(cost_samples, cost_to_go, label='cost to go', marker='o')
- pylab.plot(cost_samples, cost_to_come, label='cost to come', marker='o')
- pylab.plot(cost_samples, cost, label='cost', marker='o')
- pylab.legend()
+ for t in range(l):
+ A_t = numerical_jacobian_x(self.dynamics.discrete_dynamics,
+ self.last_x_hat_t[t], final_u_t[t])
+ B_t = numerical_jacobian_u(self.dynamics.discrete_dynamics,
+ self.last_x_hat_t[t], final_u_t[t])
+ c_t = self.dynamics.discrete_dynamics(self.last_x_hat_t[t], final_u_t[t]) \
+ - A_t * self.last_x_hat_t[t] - B_t * final_u_t[t]
+ print("Infeasability at", t, "is",
+ ((A_t * self.last_x_hat_t[t] + B_t * final_u_t[t] + c_t) \
+ - self.last_x_hat_t[t + 1]).T)
- pylab.ioff()
- pylab.show()
+ 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.figure('cost')
+ cost_samples = numpy.arange(len(cost))
+ pylab.plot(cost_samples, cost_to_go, label='cost to go', marker='o')
+ pylab.plot(cost_samples,
+ cost_to_come,
+ label='cost to come',
+ marker='o')
+ pylab.plot(cost_samples, cost, label='cost', marker='o')
+ pylab.legend()
+
+ pylab.ioff()
+ pylab.show()
+
if __name__ == '__main__':
- dt = 0.05
- #arm_dynamics = ArmDynamics(dt=dt)
- #elqr = ELQR(arm_dynamics, ArmCostFunction(dt=dt, dynamics=arm_dynamics))
- #x_hat_initial = numpy.matrix([[0.10], [1.0], [0.0], [0.0]])
- #elqr.Solve(x_hat_initial, 100, 3)
+ dt = 0.05
+ #arm_dynamics = ArmDynamics(dt=dt)
+ #elqr = ELQR(arm_dynamics, ArmCostFunction(dt=dt, dynamics=arm_dynamics))
+ #x_hat_initial = numpy.matrix([[0.10], [1.0], [0.0], [0.0]])
+ #elqr.Solve(x_hat_initial, 100, 3)
- elqr = ELQR(SkidSteerDynamics(dt=dt), CostFunction(dt=dt))
- x_hat_initial = numpy.matrix([[0.10], [1.0], [0.0]])
- elqr.Solve(x_hat_initial, 100, 15)
- sys.exit(1)
+ elqr = ELQR(SkidSteerDynamics(dt=dt), CostFunction(dt=dt))
+ x_hat_initial = numpy.matrix([[0.10], [1.0], [0.0]])
+ elqr.Solve(x_hat_initial, 100, 15)
+ sys.exit(1)