def create_plan_fc(b0, N_sim):
# Degrees of freedom for the optimizer
V = cat.struct_symSX([
(
cat.entry('X',repeat=N_sim+1,struct=belief),
cat.entry('U',repeat=N_sim,struct=control)
)
])
# Objective function
m_bN = V['X',N_sim,'m',ca.veccat,['x_b','y_b']]
m_cN = V['X',N_sim,'m',ca.veccat,['x_c','y_c']]
dm_bc = m_bN - m_cN
J = 1e2 * ca.mul(dm_bc.T, dm_bc) # m_cN -> m_bN
J += 1e-1 * ca.trace(V['X',N_sim,'S']) # Sigma -> 0
# Regularize controls
J += 1e-2 * ca.sum_square(ca.veccat(V['U'])) * dt # prevent bang-bang
# Multiple shooting constraints and running costs
g = []
for k in range(N_sim):
# Multiple shooting
[x_next] = BF([V['X',k], V['U',k]])
g.append(x_next - V['X',k+1])
# Penalize uncertainty
J += 1e-1 * ca.trace(V['X',k,'S']) * dt
g = ca.vertcat(g)
# log-probability, doesn't work with full collocation
#Sb = V['X',N_sim,'S',['x_b','y_b'], ['x_b','y_b']]
#J += ca.mul([ dm_bc.T, ca.inv(Sb + 1e-8 * ca.SX.eye(2)), dm_bc ]) + \
# ca.log(ca.det(Sb + 1e-8 * ca.SX.eye(2)))
# Formulate the NLP
nlp = ca.SXFunction('nlp', ca.nlpIn(x=V), ca.nlpOut(f=J,g=g))
# Create solver
opts = {}
opts['linear_solver'] = 'ma97'
#opts['hessian_approximation'] = 'limited-memory'
solver = ca.NlpSolver('solver', 'ipopt', nlp, opts)
# Define box constraints
lbx = V(-ca.inf)
ubx = V(ca.inf)
# 0 <= v <= v_max
lbx['U',:,'v'] = 0; ubx['U',:,'v'] = v_max
# -w_max <= w <= w_max
lbx['U',:,'w'] = -w_max; ubx['U',:,'w'] = w_max
# m(t=0) = m0
lbx['X',0,'m'] = ubx['X',0,'m'] = b0['m']
# S(t=0) = S0
lbx['X',0,'S'] = ubx['X',0,'S'] = b0['S']
# Solve the NLP
sol = solver(x0=0, lbg=0, ubg=0, lbx=lbx, ubx=ubx)
return V(sol['x'])
def setup_oed(self, outputs, parameters, sigma, time_points, design="A"):
"""
Transforms an Optimization Problem into an Optimal Experimental Design problem.
Parameters::
outputs --
List of names for outputs.
Type: [string]
parameters --
List of names for parameters to estimate.
Type: [string]
sigma --
Experiment variance matrix.
Type: [[float]]
time_points --
List of measurement time points.
Type: [float]
design --
Design criterion.
Possible values: "A", "T"
"""
# Augment sensitivities and add timed variables
self.augment_sensitivities(parameters)
timed_sens = self.create_timed_sensitivities(outputs, parameters, time_points)
# Create sensitivity and Fisher matrices
Q = []
for j in xrange(len(outputs)):
Q.append(casadi.vertcat([casadi.horzcat([s.getVar() for s in timed_sens[i][j]])
for i in xrange(len(time_points))]))
Fisher = sum([sigma[i, j] * casadi.mul(Q[i].T, Q[j]) for i in xrange(len(outputs))
for j in xrange(len(outputs))])
# Define the objective
if design == "A":
b = casadi.MX.sym("b", Fisher.shape[1], 1)
Fisher_inv = casadi.jacobian(casadi.solve(Fisher, b), b)
obj = casadi.trace(Fisher_inv)
elif design == "T":
obj = -casadi.trace(Fisher)
elif design == "D":
obj = -casadi.det(Fisher)
raise NotImplementedError("D-optimal design is not supported.")
else:
raise ValueError("Invalid design %s." % design)
old_obj = self.getObjective()
self.setObjective(old_obj + obj)
def loss_quadratic(m, z, v=None, W=None):
""" Quadratic cost function
Simple quadratic loss with W as weight matrix, ignoring variance
Parameters
----------
m : dx1 ndarray[float | casadi.Sym]
The mean of the input Gaussian
v : dxd ndarray[float | casadi.Sym]
z: dx1 ndarray[float | casadi.Sym]
The target-state [optional]
W: dxd ndarray[float | casadi.Sym]
The weighting matrix factor for the cost-function (scaling)
Returns
-------
L: float
The quadratic loss
"""
D = np.shape(m)[0]
if W is None:
W = SX.eye(D)
l_var = 0
if not v is None:
l_var = trace(mtimes(W, v))
return mtimes((m - z).T, mtimes(W, m - z)) + l_var
def FIM_t(xpdot, b, criterion):
'''
computes FIM at a given time.
b is the bonary variable which selects or not the time point
'''
n_x = 2
n_theta = 4
FIM_sample = np.zeros([n_theta, n_theta])
FIM_0 = cad.inv((((10.0 - 0.001)**2) / 12) * cad.SX.eye(n_theta))
FIM_sample += FIM_0
for i in range(np.shape(xpdot)[0] - 1):
xp_r = cad.reshape(xpdot[i + 1], (n_x, n_theta))
# vv = np.zeros([ntheta[0], ntheta[0], 1 + N])
# for i in range(0, 1 + N):
FIM_sample += b[i] * xp_r.T @ np.linalg.inv(
np.array([[0.01, 0], [0, 0.05]])
) @ xp_r # + np.linalg.inv(np.array([[0.01, 0, 0, 0], [0, 0.05, 0, 0], [0, 0, 1, 0], [0, 0, 0, 0.2]]))
# FIM = solve(FIM1, SX.eye(FIM1.size1()))
# [Q, R] = qr(FIM1.expand())
if criterion == 'D_optimal':
# objective = -cad.log(cad.det(FIM_sample) + 1e-10)
objective = -2 * cad.sum1(cad.log(cad.diag(
cad.chol(FIM_sample)))) # by Cholesky factorisation
# objective = -cad.log((cad.det(FIM_sample)**2))
# objective = -cad.det(FIM_sample)
elif criterion == 'A_optimal':
objective = -cad.log(cad.trace(FIM_sample) + 1e-10)
return objective
def test(self):
if tools.cross_building(self.settings):
print("NOT RUN (cross-building)")
return
# swig_python
if self.options["casadi"].swig_python:
self.output.info("Try to load 'casadi' python module")
try:
import casadi
A = casadi.SX.eye(2)
if casadi.trace(A) == 2:
self.output.info("Completed conanized casadi climax")
except ModuleNotFoundError:
try:
import numpy
except ModuleNotFoundError:
self.output.error("Casadi requires 'numpy', but it was not found")
self.output.error("Unable to proplerly load python casadi module")
self.output.info("Run consumer tests for library interfaces")
cmake = self._configure_cmake()
if self.options["casadi"].hpmpc:
self.output.warn("HPMPC plugin interface is not tested")
if self.options["casadi"].dsdp:
self.output.warn("DSDP interface is not tested(?)")
env_build = RunEnvironment(self)
with tools.environment_append(env_build.vars):
cmake.test()
self.output.info("Casadi OK!")
def asTwist(self):
acosarg = (casadi.trace(self.R) - 1) / 2
theta = casadi.acos(casadi.if_else(casadi.fabs(acosarg) >= 1., 1., acosarg))
w = casadi.horzcat((self.R[2, 1] - self.R[1, 2]),
(self.R[0, 2] - self.R[2, 0]),
(self.R[1, 0] - self.R[0, 1]))
return casadi.horzcat(casadi.reshape(self.t, 1, 3), theta * w / casadi.norm_2(w))
def get_stage_cost_func(x, u, p, Cs, Cu, x_target, dt):
P = cs.reshape(p, 11, 11)
cost = 0.5 * cs.trace(P @ Cs)
cost += 0.5 * cs.dot(x[:6] - x_target, Cs[:6, :6] @ (x[:6] - x_target))
cost += 0.5 * cs.dot(u, Cu @ u)
cost *= dt
c_stage = cs.Function('c_stage', [x, u, p], [cost], ['x', 'u', 'p'],
['cost'])
return c_stage
def __cost_lf(self, x, x_ref, covar_x, P, s=1):
""" Terminal cost function: Expected Value of Quadratic Cost
"""
P_s = ca.SX.sym('Q', ca.MX.size(P))
x_s = ca.SX.sym('x', ca.MX.size(x))
covar_x_s = ca.SX.sym('covar_x', ca.MX.size(covar_x))
sqnorm_x = ca.Function('sqnorm_x', [x_s, P_s],
[ca.mtimes(x_s.T, ca.mtimes(P_s, x_s))])
trace_x = ca.Function('trace_x', [P_s, covar_x_s],
[s * ca.trace(ca.mtimes(P_s, covar_x_s))])
return sqnorm_x(x - x_ref, P) + trace_x(P, covar_x)
def __cost_l(self, x, x_ref, covar_x, u, covar_u, delta_u, Q, R, S, s=1):
""" Stage cost function: Expected Value of Quadratic Cost
"""
Q_s = ca.SX.sym('Q', ca.MX.size(Q))
R_s = ca.SX.sym('R', ca.MX.size(R))
x_s = ca.SX.sym('x', ca.MX.size(x))
u_s = ca.SX.sym('u', ca.MX.size(u))
covar_x_s = ca.SX.sym('covar_x', ca.MX.size(covar_x))
covar_u_s = ca.SX.sym('covar_u', ca.MX.size(R))
sqnorm_x = ca.Function('sqnorm_x', [x_s, Q_s],
[ca.mtimes(x_s.T, ca.mtimes(Q_s, x_s))])
sqnorm_u = ca.Function('sqnorm_u', [u_s, R_s],
[ca.mtimes(u_s.T, ca.mtimes(R_s, u_s))])
trace_u = ca.Function('trace_u', [R_s, covar_u_s],
[s * ca.trace(ca.mtimes(R_s, covar_u_s))])
trace_x = ca.Function('trace_x', [Q_s, covar_x_s],
[s * ca.trace(ca.mtimes(Q_s, covar_x_s))])
return sqnorm_x(x - x_ref, Q) + sqnorm_u(u, R) + sqnorm_u(delta_u, S) \
+ trace_x(Q, covar_x) + trace_u(R, covar_u)
def cost_func_BSP(self, robot, U, X0, cov_X0):
cost = 0
U = c.reshape(U, robot.nu, self.T)
X = c.MX(robot.nx, self.T + 1)
X[:, 0] = X0
P = cov_X0
for i in range(self.T):
X[:, i + 1] = robot.kinematics(X[:, i], U[0, i], U[1, i], U[2, i])
H, M = self.light_dark_MX(X[:, i + 1])
Sigma_w = (self.epsilon**2) * self.Sigma_w
Sigma_nu = (self.epsilon**2) * self.Sigma_nu
P = c.mtimes(c.mtimes(robot.A, P), robot.A.T) + c.mtimes(
c.mtimes(robot.G, Sigma_w), robot.G.T)
S = c.mtimes(c.mtimes(H, P), H.T) + c.mtimes(
c.mtimes(M, Sigma_nu), M.T)
K = c.mtimes(c.mtimes(P, H.T), c.inv(S))
P = c.mtimes(c.DM.eye(robot.nx) - c.mtimes(K, H), P)
cost = cost + self.gamma * c.trace(
c.mtimes(c.mtimes(self.Q, P), self.Q.T)) + c.mtimes(
c.mtimes(U[:, i].T, self.R), U[:, i])
X_temp = X[0:2, i]
if params.OBSTACLES:
obstacle_cost = self.obstacle_cost_func(X_temp)
cost = cost + obstacle_cost
cost = cost + c.mtimes(c.mtimes((self.Xg - X[:, self.T]).T, self.Qf),
(self.Xg - X[:, self.T]))
return cost
def from_dcm(cls, R):
assert R.shape == (3, 3)
b1 = 0.5 * ca.sqrt(1 + R[0, 0] + R[1, 1] + R[2, 2])
b2 = 0.5 * ca.sqrt(1 + R[0, 0] - R[1, 1] - R[2, 2])
b3 = 0.5 * ca.sqrt(1 - R[0, 0] + R[1, 1] - R[2, 2])
b4 = 0.5 * ca.sqrt(1 - R[0, 0] - R[1, 1] + R[2, 2])
q1 = ca.SX(4, 1)
q1[0] = b1
q1[1] = (R[2, 1] - R[1, 2]) / (4 * b1)
q1[2] = (R[0, 2] - R[2, 0]) / (4 * b1)
q1[3] = (R[1, 0] - R[0, 1]) / (4 * b1)
q2 = ca.SX(4, 1)
q2[0] = (R[2, 1] - R[1, 2]) / (4 * b2)
q2[1] = b2
q2[2] = (R[0, 1] + R[1, 0]) / (4 * b2)
q2[3] = (R[0, 2] + R[2, 0]) / (4 * b2)
q3 = ca.SX(4, 1)
q3[0] = (R[0, 2] - R[2, 0]) / (4 * b3)
q3[1] = (R[0, 1] + R[1, 0]) / (4 * b3)
q3[2] = b3
q3[3] = (R[1, 2] + R[2, 1]) / (4 * b3)
q4 = ca.SX(4, 1)
q4[0] = (R[1, 0] - R[0, 1]) / (4 * b4)
q4[1] = (R[0, 2] + R[2, 0]) / (4 * b4)
q4[2] = (R[1, 2] + R[2, 1]) / (4 * b4)
q4[3] = b4
q = ca.if_else(
ca.trace(R) > 0, q1,
ca.if_else(ca.logic_and(R[0, 0] > R[1, 1], R[0, 0] > R[2, 2]), q2,
ca.if_else(R[1, 1] > R[2, 2], q3, q4)))
return q
def create_plan_pc(b0, BF, dt, N_sim):
# Degrees of freedom for the optimizer
V = cat.struct_symSX([
(
cat.entry('X',repeat=N_sim+1,struct=state),
cat.entry('U',repeat=N_sim,struct=control)
)
])
# Final coordinate
x_bN = V['X',N_sim,ca.veccat,['x_b','y_b']]
x_cN = V['X',N_sim,ca.veccat,['x_c','y_c']]
dx_bc = x_bN - x_cN
# Final velocity
v_bN = V['X',N_sim,ca.veccat,['vx_b','vy_b']]
v_cN = V['X',N_sim,ca.veccat,['vx_c','vy_c']]
dv_bc = v_bN - v_cN
# Angle between gaze and ball velocity
theta = V['X',N_sim,'theta']
phi = V['X',N_sim,'phi']
d = ca.veccat([ ca.cos(theta)*ca.cos(phi), ca.cos(theta)*ca.sin(phi), ca.sin(theta) ])
r = V['X',N_sim,ca.veccat,['vx_b','vy_b','vz_b']]
r_unit = r / (ca.norm_2(r) + 1e-2)
d_gaze = d - r_unit
# Regularize controls
J = 1e-2 * ca.sum_square(ca.veccat(V['U'])) * dt # prevent bang-bang
# Multiple shooting constraints and running costs
bk = cat.struct_SX(belief)
bk['S'] = b0['S']
g, lbg, ubg = [], [], []
for k in range(N_sim):
# Belief propagation
bk['m'] = V['X',k]
[bk_next] = BF([ bk, V['U',k] ])
bk_next = belief(bk_next) # simplify indexing
# Multiple shooting
g.append(bk_next['m'] - V['X',k+1])
lbg.append(ca.DMatrix.zeros(nx))
ubg.append(ca.DMatrix.zeros(nx))
# Control constraints
g.append(V['U',k,'F'] - a - b * ca.cos(V['U',k,'psi']))
lbg.append(-ca.inf)
ubg.append(ca.DMatrix([0]))
# Advance time
bk = bk_next
g = ca.vertcat(g)
lbg = ca.veccat(lbg)
ubg = ca.veccat(ubg)
# Simple cost
J += 1e1 * ca.mul(dx_bc.T, dx_bc) # coordinate
J += 1e0 * ca.mul(dv_bc.T, dv_bc) # velocity
#J += 1e0 * ca.mul(d_gaze.T, d_gaze) # gaze antialigned with ball velocity
J += 1e1 * ca.trace(bk_next['S']) # uncertainty
# log-probability cost
#Sb = bk_next['S', ['x_b','y_b'], ['x_b','y_b']]
#J += 1e1 * (ca.mul([ dm_bc.T, ca.inv(Sb), dm_bc ]) + ca.log(ca.det(Sb)))
# Formulate the NLP
nlp = ca.SXFunction('nlp', ca.nlpIn(x=V), ca.nlpOut(f=J,g=g))
# Create solver
opts = {}
opts['linear_solver'] = 'ma97'
#opts['hessian_approximation'] = 'limited-memory'
solver = ca.NlpSolver('solver', 'ipopt', nlp, opts)
# Define box constraints
lbx = V(-ca.inf)
ubx = V(ca.inf)
# State constraints
# catcher can look within the upper hemisphere
lbx['X',:,'theta'] = 0; ubx['X',:,'theta'] = theta_max
# Control constraints
# 0 <= F
lbx['U',:,'F'] = 0
# -w_max <= w <= w_max
lbx['U',:,'w'] = -w_max; ubx['U',:,'w'] = w_max
# -pi <= psi <= pi
lbx['U',:,'psi'] = -ca.pi; ubx['U',:,'psi'] = ca.pi
# m(t=0) = m0
lbx['X',0] = ubx['X',0] = b0['m']
# Take care of the time
#lbx['X',:,'T'] = 0.5; ubx['X',:,'T'] = ca.inf
# Simulation ends when the ball touches the ground
#lbx['X',N_sim,'z_b'] = ubx['X',N_sim,'z_b'] = 0
# Solve the NLP
sol = solver(x0=0, lbg=lbg, ubg=ubg, lbx=lbx, ubx=ubx)
return V(sol['x'])
def test_mapaccum(self):
x = SX.sym("x", 2)
y = SX.sym("y")
z = SX.sym("z", 2, 2)
v = SX.sym("v", Sparsity.upper(3))
fun = Function("f", [x, y, z, v], [mtimes(z, x) + y, sin(y * x).T, v / y])
n = 2
X = MX.sym("x", x.sparsity())
Y = [MX.sym("y", y.sparsity()) for i in range(n)]
Z = [MX.sym("z", z.sparsity()) for i in range(n)]
V = [MX.sym("v", v.sparsity()) for i in range(n)]
np.random.seed(0)
X_ = DM(x.sparsity(), np.random.random(x.nnz()))
Y_ = [DM(i.sparsity(), np.random.random(i.nnz())) for i in Y]
Z_ = [DM(i.sparsity(), np.random.random(i.nnz())) for i in Z]
V_ = [DM(i.sparsity(), np.random.random(i.nnz())) for i in V]
for ad_weight in range(2):
for ad_weight_sp in range(2):
F = fun.mapaccum("map", n, [0], [0], {"ad_weight_sp": ad_weight_sp, "ad_weight": ad_weight})
F.forward(2)
XP = X
Y0s = []
Y1s = []
Xps = []
for k in range(n):
XP, Y0, Y1 = fun(XP, Y[k], Z[k], V[k])
Y0s.append(Y0)
Y1s.append(Y1)
Xps.append(XP)
Fref = Function(
"f", [X, horzcat(*Y), horzcat(*Z), horzcat(*V)], [horzcat(*Xps), horzcat(*Y0s), horzcat(*Y1s)]
)
inputs = [X_, horzcat(*Y_), horzcat(*Z_), horzcat(*V_)]
for f in [F, toSX_fun(F)]:
self.checkfunction(f, Fref, inputs=inputs)
self.check_codegen(f, inputs=inputs)
fun = Function("f", [y, x, z, v], [mtimes(z, x) + y + c.trace(v) ** 2, sin(y * x).T, v / y])
for ad_weight in range(2):
for ad_weight_sp in range(2):
F = fun.mapaccum("map", n, [1, 3], [0, 2], {"ad_weight_sp": ad_weight_sp, "ad_weight": ad_weight})
XP = X
VP = V[0]
Y0s = []
Y1s = []
Xps = []
Vps = []
for k in range(n):
XP, Y0, VP = fun(Y[k], XP, Z[k], VP)
Y0s.append(Y0)
Xps.append(XP)
Vps.append(VP)
Fref = Function("f", [horzcat(*Y), X, horzcat(*Z), V[0]], [horzcat(*Xps), horzcat(*Y0s), horzcat(*Vps)])
inputs = [horzcat(*Y_), X_, horzcat(*Z_), V_[0]]
for f in [F, toSX_fun(F)]:
self.checkfunction(f, Fref, inputs=inputs)
self.check_codegen(f, inputs=inputs)
def _create_running_uncertainty_cost(self, w_S):
running_uncertainty_cost = 0.5 * w_S * ca.trace(self.b['S']) * self.dt
op = {'input_scheme': ['b'],
'output_scheme': ['cS']}
return ca.SXFunction('Running uncertainty cost', [self.b],
[running_uncertainty_cost], op)
def _create_final_uncertainty_cost(self, w_Sl):
final_uncertainty_cost = 0.5 * w_Sl * ca.trace(self.b['S'])
op = {'input_scheme': ['b'],
'output_scheme': ['cSl']}
return ca.SXFunction('Final uncertainty cost', [self.b],
[final_uncertainty_cost], op)
def create_plan_pc(b0, N_sim):
# Degrees of freedom for the optimizer
V = cat.struct_symSX([
(
cat.entry('X',repeat=N_sim+1,struct=state),
cat.entry('U',repeat=N_sim,struct=control)
)
])
# Final means
m_bN = V['X',N_sim,ca.veccat,['x_b','y_b']]
m_cN = V['X',N_sim,ca.veccat,['x_c','y_c']]
dm_bc = m_bN - m_cN
# Regularize controls
J = 1e-2 * ca.sum_square(ca.veccat(V['U'])) * dt # prevent bang-bang
# Multiple shooting constraints and running costs
bk = cat.struct_SX(belief)
bk['S'] = b0['S']
g = []
lbg = []
ubg = []
for k in range(N_sim):
# Belief propagation
bk['m'] = V['X',k]
[bk_next] = BF([ bk, V['U',k] ])
bk_next = belief(bk_next) # simplify indexing
# Multiple shooting
g.append(bk_next['m'] - V['X',k+1])
lbg.append(ca.DMatrix.zeros(nx))
ubg.append(ca.DMatrix.zeros(nx))
# Control constraints
g.append(V['U',k,'v'] - a - b * ca.cos(V['U',k,'psi']))
lbg.append(-ca.inf)
ubg.append(ca.DMatrix([0]))
# Advance time
bk = bk_next
g = ca.vertcat(g)
lbg = ca.veccat(lbg)
ubg = ca.veccat(ubg)
# Simple cost
J += 1e1 * (ca.mul(dm_bc.T, dm_bc) + ca.trace(bk_next['S']))
# log-probability cost
#Sb = bk_next['S', ['x_b','y_b'], ['x_b','y_b']]
#J += 1e1 * (ca.mul([ dm_bc.T, ca.inv(Sb), dm_bc ]) + ca.log(ca.det(Sb)))
# Formulate the NLP
nlp = ca.SXFunction('nlp', ca.nlpIn(x=V), ca.nlpOut(f=J,g=g))
# Create solver
opts = {}
opts['linear_solver'] = 'ma97'
#opts['hessian_approximation'] = 'limited-memory'
solver = ca.NlpSolver('solver', 'ipopt', nlp, opts)
# Define box constraints
lbx = V(-ca.inf)
ubx = V(ca.inf)
# 0 <= v
lbx['U',:,'v'] = 0
# -w_max <= w <= w_max
lbx['U',:,'w'] = -w_max; ubx['U',:,'w'] = w_max
# -pi <= psi <= pi
lbx['U',:,'psi'] = -ca.pi; ubx['U',:,'psi'] = ca.pi
# m(t=0) = m0
lbx['X',0] = ubx['X',0] = b0['m']
# Solve the NLP
sol = solver(x0=0, lbg=lbg, ubg=ubg, lbx=lbx, ubx=ubx)
return V(sol['x'])
def log(cls, R):
# TODO
theta = ca.arccos((ca.trace(R) - 1) / 2)
return vee(cls.C3(theta) * (R - R.T))
def gp_exact_moment(invK, X, Y, hyper, inputmean, inputcov):
""" Gaussian Process with Exact Moment Matching
Copyright (c) 2018, Eric Bradford, Helge-André Langåker
The first and second moments are used to compute the mean and covariance of the
posterior distribution with a stochastic input distribution. This assumes a
zero prior mean function and the squared exponential kernel.
# Arguments
invK: Array with the inverse covariance matrices of size (Ny x N x N),
with Ny number of outputs from the GP and N number of training points.
X: Training data matrix with inputs of size NxNx, with Nx number of
inputs to the GP.
Y: Training data matrix with outpyts of size (N x Ny).
hyper: Array with hyperparameters [ell_1 .. ell_Nx sf sn].
inputmean: Mean from the last GP iteration of size (1 x Nx)
inputcov: Covariance matrix from the last GP iteration of size (Nx x Nx)
# Returns
mean: Array of the output mean of size (Ny x 1).
covariance: Covariance matrix of size (Ny x Ny).
"""
hyper = ca.log(hyper)
Ny = len(invK)
N, Nx = ca.MX.size(X)
mean = ca.MX.zeros(Ny, 1)
beta = ca.MX.zeros(N, Ny)
log_k = ca.MX.zeros(N, Ny)
v = X - ca.repmat(inputmean, N, 1)
covariance = ca.MX.zeros(Ny, Ny)
#TODO: Fix that LinsolQr don't work with the extended graph?
A = ca.SX.sym('A', inputcov.shape)
[Q, R2] = ca.qr(A)
determinant = ca.Function('determinant', [A], [ca.exp(ca.trace(ca.log(R2)))])
for a in range(Ny):
beta[:, a] = ca.mtimes(invK[a], Y[:, a])
iLambda = ca.diag(ca.exp(-2 * hyper[a, :Nx]))
R = inputcov + ca.diag(ca.exp(2 * hyper[a, :Nx]))
iR = ca.mtimes(iLambda, (ca.MX.eye(Nx) - ca.solve((ca.MX.eye(Nx)
+ ca.mtimes(inputcov, iLambda)), (ca.mtimes(inputcov, iLambda)))))
T = ca.mtimes(v, iR)
c = ca.exp(2 * hyper[a, Nx]) / ca.sqrt(determinant(R)) \
* ca.exp(ca.sum2(hyper[a, :Nx]))
q2 = c * ca.exp(-ca.sum2(T * v) * 0.5)
qb = q2 * beta[:, a]
mean[a] = ca.sum1(qb)
t = ca.repmat(ca.exp(hyper[a, :Nx]), N, 1)
v1 = v / t
log_k[:, a] = 2 * hyper[a, Nx] - ca.sum2(v1 * v1) * 0.5
# covariance with noisy input
for a in range(Ny):
ii = v / ca.repmat(ca.exp(2 * hyper[a, :Nx]), N, 1)
for b in range(a + 1):
R = ca.mtimes(inputcov, ca.diag(ca.exp(-2 * hyper[a, :Nx])
+ ca.exp(-2 * hyper[b, :Nx]))) + ca.MX.eye(Nx)
t = 1.0 / ca.sqrt(determinant(R))
ij = v / ca.repmat(ca.exp(2 * hyper[b, :Nx]), N, 1)
Q = ca.exp(ca.repmat(log_k[:, a], 1, N)
+ ca.repmat(ca.transpose(log_k[:, b]), N, 1)
+ maha(ii, -ij, ca.solve(R, inputcov * 0.5), N))
A = ca.mtimes(beta[:, a], ca.transpose(beta[:, b]))
if b == a:
A = A - invK[a]
A = A * Q
covariance[a, b] = t * ca.sum2(ca.sum1(A))
covariance[b, a] = covariance[a, b]
covariance[a, a] = covariance[a, a] + ca.exp(2 * hyper[a, Nx])
covariance = covariance - ca.mtimes(mean, ca.transpose(mean))
return [mean, covariance]
def rotationAngle(mx_R):
acosarg = (casadi.trace(mx_R) - 1.) / 2.
return casadi.if_else(casadi.fabs(acosarg) >= 1., 0., casadi.acos(acosarg))
def rotation_to_axis(R):
vec = cs.vertcat(R[3, 2] - R[2, 3], R[1, 3] - R[3, 1], R[2, 1] - R[1, 2])
ang_core = (cs.trace(R) - 1.0) / 2.0
denom = 1.0 / (2 * (cs.sqrt(1 - ang_core)))
return cs.if_else(ang_core == 1.0, cs.vertcat(1.0, 0.0, 0.0), vec * denom)
def trace(inputobj):
return ca.trace(inputobj)
def cost_func_BSP(self, robot, U, X0, cov_X0):
cost = 0
U = c.reshape(U, robot.nu * self.N, self.T)
X = c.MX(robot.nx * self.N, self.T + 1)
X[:, 0] = np.reshape(X0, (robot.nx * self.N, ))
for i in range(self.T):
for n in range(self.N):
X[robot.nx * n:robot.nx * (n + 1), i + 1] = robot.kinematics(
X[robot.nx * n:robot.nx * (n + 1), i], U[robot.nu * n, i],
U[robot.nu * n + 1, i], U[robot.nu * n + 2, i])
X_temp = X[robot.nx * n:robot.nx * (n + 1), i + 1]
if i == 0:
P_temp = cov_X0[robot.nx * n:robot.nx * (n + 1), :]
if n == 0:
P = c.MX(robot.nx * self.N, robot.nx)
else:
P_temp = P[robot.nx * n:robot.nx * (n + 1), :]
M = c.MX(robot.nx, robot.nx)
M[0,
0] = (X_temp[0] - 3)**2 #1/(c.mtimes(X[0,i+1],X[0,i+1])+1);
M[0, 1] = 0
M[0, 2] = 0
M[1, 0] = 0
M[1, 1] = 1
M[1, 2] = 0
M[2, 0] = 0
M[2, 1] = 0
M[2, 2] = 1
#M = c.DM.eye(robot.nx)
Sigma_w = (self.epsilon**2) * self.Sigma_w
Sigma_nu = (self.epsilon**2) * self.Sigma_nu
P_temp = P_temp + c.mtimes(c.mtimes(robot.G, Sigma_w),
robot.G.T)
S = P_temp + c.mtimes(c.mtimes(M, Sigma_nu), M.T)
K = c.mtimes(P_temp, c.inv(S))
P_temp = c.mtimes(c.DM.eye(robot.nx) - K, P_temp)
P[robot.nx * n:robot.nx * (n + 1), :] = P_temp
cost = cost + self.gamma*c.trace(c.mtimes(c.mtimes(self.Q,P_temp),self.Q.T)) + \
c.mtimes(c.mtimes(U[robot.nu*n:robot.nu*(n+1),i].T,self.R),U[robot.nu*n:robot.nu*(n+1),i])
if params.OBSTACLES:
obstacle_cost = self.obstacle_cost_func(X_temp[0:2])
cost = cost + obstacle_cost
if self.N > 1:
inter_agent_cost = self.inter_agent_cost_func(robot, X[:, i])
cost = cost + inter_agent_cost
for n in range(self.N):
cost = cost + c.mtimes(
c.mtimes((self.Xg[n, :] - X[robot.nx * n:robot.nx *
(n + 1), self.T]).T, self.Qf),
(self.Xg[n, :] - X[robot.nx * n:robot.nx * (n + 1), self.T]))
return cost
def trace(inputobj):
return ca.trace(inputobj)
def test_mapaccum(self):
x = SX.sym("x", 2)
y = SX.sym("y")
z = SX.sym("z", 2, 2)
v = SX.sym("v", Sparsity.upper(3))
fun = Function(
"f", [x, y, z, v],
[mtimes(z, x) + y, sin(y * x).T, v / y])
n = 2
X = MX.sym("x", x.sparsity())
Y = [MX.sym("y", y.sparsity()) for i in range(n)]
Z = [MX.sym("z", z.sparsity()) for i in range(n)]
V = [MX.sym("v", v.sparsity()) for i in range(n)]
np.random.seed(0)
X_ = DM(x.sparsity(), np.random.random(x.nnz()))
Y_ = [DM(i.sparsity(), np.random.random(i.nnz())) for i in Y]
Z_ = [DM(i.sparsity(), np.random.random(i.nnz())) for i in Z]
V_ = [DM(i.sparsity(), np.random.random(i.nnz())) for i in V]
for ad_weight in range(2):
for ad_weight_sp in range(2):
F = fun.mapaccum("map", n, [0], [0], {
"ad_weight_sp": ad_weight_sp,
"ad_weight": ad_weight
})
F.forward(2)
XP = X
Y0s = []
Y1s = []
Xps = []
for k in range(n):
XP, Y0, Y1 = fun(XP, Y[k], Z[k], V[k])
Y0s.append(Y0)
Y1s.append(Y1)
Xps.append(XP)
Fref = Function("f", [
X, horzcat(*Y), horzcat(*Z),
horzcat(*V)
], [horzcat(*Xps), horzcat(*Y0s),
horzcat(*Y1s)])
inputs = [X_, horzcat(*Y_), horzcat(*Z_), horzcat(*V_)]
for f in [F, toSX_fun(F)]:
self.checkfunction(f, Fref, inputs=inputs)
self.check_codegen(f, inputs=inputs)
fun = Function("f", [y, x, z, v],
[mtimes(z, x) + y + c.trace(v)**2,
sin(y * x).T, v / y])
for ad_weight in range(2):
for ad_weight_sp in range(2):
F = fun.mapaccum("map", n, [1, 3], [0, 2], {
"ad_weight_sp": ad_weight_sp,
"ad_weight": ad_weight
})
XP = X
VP = V[0]
Y0s = []
Y1s = []
Xps = []
Vps = []
for k in range(n):
XP, Y0, VP = fun(Y[k], XP, Z[k], VP)
Y0s.append(Y0)
Xps.append(XP)
Vps.append(VP)
Fref = Function(
"f", [horzcat(*Y), X, horzcat(*Z), V[0]],
[horzcat(*Xps),
horzcat(*Y0s),
horzcat(*Vps)])
inputs = [horzcat(*Y_), X_, horzcat(*Z_), V_[0]]
for f in [F, toSX_fun(F)]:
self.checkfunction(f, Fref, inputs=inputs)
self.check_codegen(f, inputs=inputs)
for i in range(np.shape(xp_opt)[1] - 1):
xp_r = np.reshape(xp_opt[:, i + 1], (2, 4))
# vv = np.zeros([ntheta[0], ntheta[0], 1 + N])
# for i in range(0, 1 + N):
FIM_t += [xp_r.T @ np.linalg.inv(np.array([[0.01, 0], [0, 0.05]])) @ xp_r]
obsFIM = []
for i in range(np.shape(FIM_t)[0]):
obsFIM += [
np.linalg.inv((((10.0 - 0.001)**2) / 12) * np.identity(n_theta)) +
sum(FIM_t[:i + 1])
]
for i in range(np.shape(xp_opt)[1] - 1):
FIM1_det += [2 * cad.sum1(cad.log(cad.diag(cad.chol(obsFIM[i]))))]
FIM1_trace += [cad.trace(obsFIM[i])]
FIM_det += [2 * np.sum(np.log(np.diag(np.linalg.cholesky(obsFIM[i]))))]
FIM_trace += [np.trace(obsFIM[i])]
FIM1_mineigv += [min(np.linalg.eig(obsFIM[i])[0])]
FIM1_maxeigv += [max(np.linalg.eig(obsFIM[i])[0])]
V_theta = np.linalg.inv(sum(FIM_t[i] for i in range(len(FIM_t))))
theta1 = w_opt[n_x + n_x * n_theta:n_x + n_x * n_theta + n_theta]
t_value = np.zeros(n_theta)
for i in range(n_theta):
t_value[i] = theta1[i] / np.sqrt(V_theta[i, i])
FIM1_trace_array = np.array(FIM1_trace)
FIM1_det_array = np.array(FIM1_det)
FIM1_maxeigv_array = np.array(FIM1_maxeigv)
def gp_taylor_approx(invK, X, Y, hyper, inputmean, inputcovar,
meanFunc='zero', diag=False, log=False):
""" Gaussian Process with Taylor Approximation
Copyright (c) 2018, Helge-André Langåker
This uses a first order taylor for the mean evaluation (a normal GP mean),
and a second order taylor for estimating the variance.
# Arguments
invK: Array with the inverse covariance matrices of size (Ny x N x N),
with Ny number of outputs from the GP and N number of training points.
X: Training data matrix with inputs of size NxNx, with Nx number of
inputs to the GP.
Y: Training data matrix with outpyts of size (N x Ny).
hyper: Array with hyperparameters [ell_1 .. ell_Nx sf sn].
inputmean: Mean from the last GP iteration of size (1 x Nx)
inputvar: Variance from the last GP iteration of size (1 x Ny)
# Returns
mean: Array with estimated mean of size (Ny x 1).
covariance: The estimated covariance matrix with the output variance in the
diagonal of size (Ny x Ny).
"""
if log:
X = ca.log(X)
Y = ca.log(Y)
inputmean = ca.log(inputmean)
Ny = len(invK)
N, Nx = ca.MX.size(X)
mean = ca.MX.zeros(Ny, 1)
var = ca.MX.zeros(Nx, 1)
v = X - ca.repmat(inputmean, N, 1)
covar_temp = ca.MX.zeros(Ny, Ny)
covariance = ca.MX.zeros(Ny, Ny)
d_mean = ca.MX.zeros(Ny, 1)
dd_var = ca.MX.zeros(Ny, Ny)
# Casadi symbols
x_s = ca.SX.sym('x', Nx)
z_s = ca.SX.sym('z', Nx)
ell_s = ca.SX.sym('ell', Nx)
sf2_s = ca.SX.sym('sf2')
covSE = ca.Function('covSE', [x_s, z_s, ell_s, sf2_s],
[covSEard(x_s, z_s, ell_s, sf2_s)])
for a in range(Ny):
ell = hyper[a, :Nx]
w = 1 / ell**2
sf2 = ca.MX(hyper[a, Nx]**2)
m = get_mean_function(hyper[a, :], inputmean, func=meanFunc)
iK = ca.MX(invK[a])
alpha = ca.mtimes(iK, Y[:, a] - m(inputmean)) + m(inputmean)
kss = sf2
ks = ca.MX.zeros(N, 1)
for i in range(N):
ks[i] = covSE(X[i, :], inputmean, ell, sf2)
invKks = ca.mtimes(iK, ks)
mean[a] = ca.mtimes(ks.T, alpha)
var[a] = kss - ca.mtimes(ks.T, invKks)
d_mean[a] = ca.mtimes(ca.transpose(w[a] * v[:, a] * ks), alpha)
#BUG: This don't take into account the covariance between states
for d in range(Ny):
for e in range(Ny):
dd_var1a = ca.mtimes(ca.transpose(v[:, d] * ks), iK)
dd_var1b = ca.mtimes(dd_var1a, v[e] * ks)
dd_var2 = ca.mtimes(ca.transpose(v[d] * v[e] * ks), invKks)
dd_var[d, e] = -2 * w[d] * w[e] * (dd_var1b + dd_var2)
if d == e:
dd_var[d, e] = dd_var[d, e] + 2 * w[d] * (kss - var[d])
mean_mat = ca.mtimes(d_mean, d_mean.T)
covar_temp[0, 0] = inputcovar[a, a]
covariance[a, a] = var[a] + ca.trace(ca.mtimes(covar_temp, .5
* dd_var + mean_mat))
return [mean, covariance]
def get_term_cost_func(x, p, Cs, x_target):
P = cs.reshape(p, 11, 11)
cost = 0.5 * cs.trace(P @ Cs)
cost += 0.5 * cs.dot(x[:6] - x_target, Cs[:6, :6] @ (x[:6] - x_target))
c_term = cs.Function('c_term', [x, p], [cost], ['x', 'p'], ['cost'])
return c_term
def rotation_to_angle(R):
# Source:
# https://en.wikipedia.org/wiki/Axis%E2%80%93angle_representation
return cs.arccos((cs.trace(R) - 1.0) / 2.0)
