def compute_quadratic_state_integral_ALDS(H, x0, T, dt=None):
''' Assuming the state x(t) evolves in time according to a linear dynamic:
dx(t)/dt = H * x(t)
Compute the following integral:
int_{0}^{T} x(t)^T * x(t) dt
'''
if (dt is None):
w, V = eig(H) # H = V*matlib.diagflat(w)*V^{-1}
print "Eigenvalues H:", np.sort_complex(w).T
Lambda_inv = matlib.diagflat(1.0 / w)
e_2T_Lambda = matlib.diagflat(np.exp(2 * T * w))
int_e_2T_Lambda = 0.5 * Lambda_inv * (e_2T_Lambda - matlib.eye(n))
# V_inv = np.linalg.inv(V)
# cost = x0.T*(V_inv.T*(int_e_2T_Lambda*(V_inv*x0)))
V_inv_x0 = np.linalg.solve(V, x0)
cost = V_inv_x0.T * int_e_2T_Lambda * V_inv_x0
return cost[0, 0]
N = int(T / dt)
x = simulate_ALDS(H, x0, dt, N)
cost = 0.0
not_finite_warning_printed = False
for i in range(N):
if (np.all(np.isfinite(x[:, i]))):
cost += dt * (x[:, i].T * x[:, i])[0, 0]
elif (not not_finite_warning_printed):
print 'WARNING: x is not finite at time step %d' % (i) #, x[:,i].T
not_finite_warning_printed = True
return cost
def compute_cost_function_matrix(n, w_x, w_dx, w_d2x, w_d3x, w_d4x):
''' Convert the cost function of the CoM state in the cost function
of the state of the linearized closed-loop dynamic
'''
Q_diag = np.matrix(n * [w_x] + n * [w_dx] + n * [w_d2x] + n * [w_d3x] +
n * [w_d4x])
return matlib.diagflat(Q_diag)
def compute_matrix_exponential_integral(H, T, dt=None):
''' Compute the following integral:
int_{0}^{T} e^{t*H} dt
'''
n = H.shape[0]
if (dt is None):
w, V = eig(H)
# H = V*diagflat(w)*V^{-1}
# H^{-1} = V*diagflat(1.0/w)*V^{-1}
V_inv = np.linalg.inv(V)
H_inv = V * matlib.diagflat(1.0 / w) * V_inv
e_TH = V * matlib.diagflat(np.exp(T * w)) * V_inv
return H_inv * (e_TH - matlib.eye(n))
N = int(T / dt)
res = matlib.zeros((n, n))
for i in range(N):
res += dt * expm(i * dt * H)
return res
def convert_cost_function(w_x, w_dx, w_d2x, w_d3x, w_d4x):
''' Convert the cost function of the CoM state in the cost function
of the momentum+force state.
'''
nl = 2
Q_diag = np.matrix(nl*[w_x] + nl*[w_dx] + nl*[w_d2x] + nl*[w_d3x] + nl*[w_d4x])
Q_c = matlib.diagflat(Q_diag) # weight matrix for CoM state
P = compute_projection_to_com_state()
Q = P.T * Q_c * P
return Q
def compute_integral_e_t_Lambda_Q_e_t_Lambda(Lambda_diag, Q_diag, T, dt=None):
''' Compute the following integral:
int_{0}^{T} e^{t*Lambda}*Q*e^{t*Lambda} dt
where both Lambda and Q are diagonal matrices
'''
n = Lambda_diag.shape[0]
if (dt is None):
# H = V*diagflat(w)*V^{-1}
# H^{-1} = V*diagflat(1.0/w)*V^{-1}
Lambda_inv = matlib.diagflat(1.0 / Lambda_diag)
e_2_T_Lambda = matlib.diagflat(np.exp(2 * T * Lambda_diag))
Q = matlib.diagflat(Q_diag)
return 0.5 * Q * Lambda_inv * (e_2_T_Lambda - matlib.eye(n))
N = int(T / dt)
res = matlib.zeros((n, n))
Lambda = matlib.diagflat(Lambda_diag)
Q = matlib.diagflat(Q_diag)
for i in range(N):
e_t_Lambda = expm(i * dt * Lambda)
res += dt * (e_t_Lambda * Q * e_t_Lambda)
return res
SAVE_DATA = conf.SAVE_DATA
LOAD_DATA = conf.LOAD_DATA # if 1 it tries to load the gains from the specified binary file
N = int(conf.T_cost_function / conf.dt_cost_function)
dt = conf.dt_cost_function
w_x = conf.w_x
w_dx = conf.w_dx
w_d2x = conf.w_d2x
w_d3x = conf.w_d3x
w_d4x_list = conf.w_d4x_list
x0 = conf.x0
do_plots = conf.do_plots # if true it shows the plots
K_contact = conf.K_contact
(H, A, B) = compute_integrator_dynamics(matlib.zeros((1, 4)))
Q = matlib.diagflat([w_x, w_dx, w_d2x, w_d3x])
R = matlib.diagflat([1.0])
if (LOAD_DATA):
try:
f = open(DATA_DIR + OUTPUT_DATA_FILE_NAME + '.pkl', 'rb')
optimal_gains = pickle.load(f)
f.close()
except:
print("Impossible to open file",
DATA_DIR + OUTPUT_DATA_FILE_NAME + '.pkl')
LOAD_DATA = 0
if (not LOAD_DATA):
optimal_gains = {}
print "Q_diag:", Q_diag
print "x0:", x0.T
# int_e_tH_approx = compute_matrix_exponential_integral(H, T, dt=1e-3)
# int_e_tH = compute_matrix_exponential_integral(H, T)
# print "Integral of e^{tH}:\n", int_e_tH
# print "Approximated integral of e^{tH}:\n", int_e_tH_approx
# print "Approximation error:", np.max(np.abs(int_e_tH_approx-int_e_tH)), '\n'
#
# Lambda_diag = matlib.diag(H)
# int_e_t_Lambda_Q_e_t_Lambda = compute_integral_e_t_Lambda_Q_e_t_Lambda(Lambda_diag, Q_diag, T)
# int_e_t_Lambda_Q_e_t_Lambda_approx = compute_integral_e_t_Lambda_Q_e_t_Lambda(Lambda_diag, Q_diag, T, 1e-3)
# print "integral e^{t*Lambda}*Q*e^{t*Lambda}:\n", int_e_t_Lambda_Q_e_t_Lambda
# print "Approximated integral e^{t*Lambda}*Q*e^{t*Lambda}:\n", int_e_t_Lambda_Q_e_t_Lambda_approx
# print "Approximation error:", np.max(np.abs(int_e_t_Lambda_Q_e_t_Lambda_approx-int_e_t_Lambda_Q_e_t_Lambda)), '\n'
cost_approx = compute_quadratic_state_integral_ALDS(H, x0, T, dt)
cost = compute_quadratic_state_integral_ALDS(H, x0, T)
print "Approximated cost is:", cost_approx
print "Exact cost is: ", cost
print "Approximation error: ", abs(cost - cost_approx), '\n'
Q = matlib.diagflat(Q_diag)
Q_inv = matlib.diagflat(1.0 / Q_diag)
cost_approx = compute_weighted_quadratic_state_integral_ALDS(
H, x0, T, Q, Q_inv, dt)
cost = compute_weighted_quadratic_state_integral_ALDS(H, x0, T, Q, Q_inv)
print "Approximated cost is:", cost_approx
print "Exact cost is: ", cost
print "Approximation error: ", abs(cost - cost_approx)
def __init__(self, hyp):
self.mean = hyp['mean']
self.cov = np.exp(hyp['cov'][:-1])
self.covd = np.asmatrix(np.diagflat(1/self.cov))
self.sf2 = np.exp(2*hyp['cov'][-1])
self.lik = hyp['lik']