def test_diag_sparse(self):
self.message("diag sparse")
for n in [[0,1,0,0,2,3,4,5,6,0],[1,2,3,0],[0,1,2,3]]:
d = DM(n)
D = DM(n)
d = sparsify(d)
m = c.diag(d)
M = sparsify(c.diag(D))
self.checkarray(m.sparsity().colind(),M.sparsity().colind())
self.checkarray(m.sparsity().row(),M.sparsity().row())
def test_diag_sparse(self):
self.message("diag sparse")
for n in [[0,1,0,0,2,3,4,5,6,0],[1,2,3,0],[0,1,2,3]]:
d = DM(n)
D = DM(n)
d = sparsify(d)
m = c.diag(d)
M = sparsify(c.diag(D))
self.checkarray(m.sparsity().colind(),M.sparsity().colind())
self.checkarray(m.sparsity().row(),M.sparsity().row())
def test_diag_sparse(self):
self.message("diag sparse")
for n in [[0,1,0,0,2,3,4,5,6,0],[1,2,3,0],[0,1,2,3]]:
d = DMatrix(n)
D = DMatrix(n)
makeSparse(d)
m = c.diag(d)
M = c.diag(D)
makeSparse(M)
self.checkarray(m.sparsity().rowind(),M.sparsity().rowind())
self.checkarray(m.sparsity().col(),M.sparsity().col())
def test_diag_sparse(self):
self.message("diag sparse")
for n in [[0, 1, 0, 0, 2, 3, 4, 5, 6, 0], [1, 2, 3, 0], [0, 1, 2, 3]]:
d = DMatrix(n)
D = DMatrix(n)
makeSparse(d)
m = c.diag(d)
M = c.diag(D)
makeSparse(M)
self.checkarray(m.sparsity().rowind(), M.sparsity().rowind())
self.checkarray(m.sparsity().col(), M.sparsity().col())
def test_cholesky(self):
numpy.random.seed(0)
n = 10
L = self.randDMatrix(n,n,sparsity=0.2) + 1.5*c.diag(range(1,n+1))
L = L[Sparsity.lower(n)]
M = mul(L,L.T)
b = self.randDMatrix(n,1)
M.sparsity().spy()
S = LinearSolver("S", "csparsecholesky",M.sparsity())
S.setInput(M)
S.prepare()
self.checkarray(M,M.T)
C = S.getFactorization()
self.checkarray(mul(C,C.T),M)
self.checkarray(C,L)
print C
S.getFactorizationSparsity().spy()
C = solve(M,b,"csparsecholesky")
self.checkarray(mul(M,C),b)
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 linearized_quad_dynamics(self):
"""
Jacobian J matrix of the linearized dynamics specified in the function quad_dynamics. J[i, j] corresponds to
the partial derivative of f_i(x) wrt x(j).
:return: a CasADi symbolic function that calculates the 13 x 13 Jacobian matrix of the linearized simplified
quadrotor dynamics
"""
jac = cs.MX(self.state_dim, self.state_dim)
# Position derivatives
jac[0:3, 7:10] = cs.diag(cs.MX.ones(3))
# Angle derivatives
jac[3:7, 3:7] = skew_symmetric(self.r) / 2
jac[3, 10:] = 1 / 2 * cs.horzcat(-self.q[1], -self.q[2], -self.q[3])
jac[4, 10:] = 1 / 2 * cs.horzcat(self.q[0], -self.q[3], self.q[2])
jac[5, 10:] = 1 / 2 * cs.horzcat(self.q[3], self.q[0], -self.q[1])
jac[6, 10:] = 1 / 2 * cs.horzcat(-self.q[2], self.q[1], self.q[0])
# Velocity derivatives
a_u = (self.u[0] + self.u[1] + self.u[2] + self.u[3]) * self.quad.max_thrust / self.quad.mass
jac[7, 3:7] = 2 * cs.horzcat(a_u * self.q[2], a_u * self.q[3], a_u * self.q[0], a_u * self.q[1])
jac[8, 3:7] = 2 * cs.horzcat(-a_u * self.q[1], -a_u * self.q[0], a_u * self.q[3], a_u * self.q[2])
jac[9, 3:7] = 2 * cs.horzcat(0, -2 * a_u * self.q[1], -2 * a_u * self.q[1], 0)
# Rate derivatives
jac[10, 10:] = (self.quad.J[1] - self.quad.J[2]) / self.quad.J[0] * cs.horzcat(0, self.r[2], self.r[1])
jac[11, 10:] = (self.quad.J[2] - self.quad.J[0]) / self.quad.J[1] * cs.horzcat(self.r[2], 0, self.r[0])
jac[12, 10:] = (self.quad.J[0] - self.quad.J[1]) / self.quad.J[2] * cs.horzcat(self.r[1], self.r[0], 0)
return cs.Function('J', [self.x, self.u], [jac])
def test_general_convex_sparse(self):
self.message("Convex sparse QP with solvers: " +
str([qpsol for qpsol, options, aux_options in qpsols]))
H = c.diag([2, 1, 0.2, 0.7, 1.3])
H[1, 2] = 0.1
H[2, 1] = 0.1
G = DM([-2, -6, 1, 0, 0])
A = DM([[1, 0, 0.1, 0.7, -1], [0.1, 2, -0.3, 4, 0.1]])
A = sparsify(A)
LBA = DM([-inf])
UBA = DM([2, 2])
LBX = DM([0] * 5)
UBX = DM([inf] * 5)
for qpsol, qp_options, aux_options in qpsols:
self.message("general_convex: " + str(qpsol))
solver = casadi.qpsol("mysolver", qpsol, {
'h': H.sparsity(),
'a': A.sparsity()
}, qp_options)
try:
less_digits = aux_options["less_digits"]
except:
less_digits = 0
solver_in = {}
solver_in["h"] = H
solver_in["g"] = G
solver_in["a"] = A
solver_in["lbx"] = LBX
solver_in["ubx"] = UBX
solver_in["lba"] = LBA
solver_in["uba"] = UBA
solver_out = solver(**solver_in)
self.checkarray(solver_out["x"],
DM([0.873908, 0.95630465, 0, 0, 0]),
str(qpsol),
digits=max(1, 6 - less_digits))
self.checkarray(solver_out["lam_x"],
DM([0, 0, -0.339076, -10.0873907, -0.252185]),
6,
str(qpsol),
digits=max(1, 6 - less_digits))
self.checkarray(solver_out["lam_a"],
DM([0, 2.52184767]),
str(qpsol),
digits=max(1, 6 - less_digits))
self.assertAlmostEqual(solver_out["cost"][0], -6.264669320767,
max(1, 6 - less_digits), str(qpsol))
def test_cholesky(self):
numpy.random.seed(0)
n = 10
L = self.randDM(n,n,sparsity=0.2) + 1.5*c.diag(list(range(1,n+1)))
L = L[Sparsity.lower(n)]
M = mtimes(L,L.T)
b = self.randDM(n,1)
M.sparsity().spy()
S = casadi.linsol("S", "csparsecholesky", M.sparsity(), 1)
S_in = [0]*S.n_in();S_in[0]=M
S.linsol_prepare()
self.checkarray(M,M.T)
C = S.linsol_cholesky()
self.checkarray(mtimes(C,C.T),M)
self.checkarray(C,L)
print(C)
S.linsol_cholesky_sparsity().spy()
C = solve(M,b,"csparsecholesky")
self.checkarray(mtimes(M,C),b)
def test_cholesky(self):
numpy.random.seed(0)
n = 10
L = self.randDM(n, n,
sparsity=0.2) + 1.5 * c.diag(list(range(1, n + 1)))
L = L[Sparsity.lower(n)]
M = mtimes(L, L.T)
b = self.randDM(n, 1)
M.sparsity().spy()
S = casadi.Linsol("S", "csparsecholesky")
S_in = [0] * S.n_in()
S_in[0] = M
S.linsol_prepare()
self.checkarray(M, M.T)
C = S.linsol_cholesky()
self.checkarray(mtimes(C, C.T), M)
self.checkarray(C, L)
print(C)
S.linsol_cholesky_sparsity().spy()
C = solve(M, b, "csparsecholesky")
self.checkarray(mtimes(M, C), b)
def _predict_sym(self, x_test, return_std=False, return_cov=False):
"""
Computes the GP posterior mean and covariance at a given a test sample using CasADi symbolics.
:param x_test: vector of size mx1, where m is the number of features used for the GP prediction
:return: the posterior mean (scalar) and covariance (scalar).
"""
k_s = self.kernel(self._x_train_cs, x_test.T)
# Posterior mean value
mu_s = cs.mtimes(k_s.T, self._K_inv_y_cs) + self.y_mean
if not return_std and not return_cov:
return {'mu': mu_s}
k_ss = self.kernel(x_test, x_test) + 1e-8 * cs.MX.eye(x_test.shape[1])
# Posterior covariance
cov_s = k_ss - cs.mtimes(cs.mtimes(k_s.T, self._K_inv_cs), k_s)
cov_s = cs.diag(cov_s)
if return_std:
return {'mu': mu_s, 'std': np.sqrt(cov_s)}
return {'mu': mu_s, 'cov': cov_s}
def test_cholesky(self):
numpy.random.seed(0)
n = 10
L = self.randDMatrix(n, n,
sparsity=0.2) + 1.5 * c.diag(range(1, n + 1))
L = L[Sparsity.tril(n)]
M = mul(L, L.T)
b = self.randDMatrix(n, 1)
M.sparsity().spy()
S = LinearSolver("csparsecholesky", M.sparsity())
S.init()
S.setInput(M)
S.prepare()
self.checkarray(M, M.T)
C = S.getFactorization()
self.checkarray(mul(C, C.T), M)
self.checkarray(C, L)
print C
S.getFactorizationSparsity().spy()
C = solve(M, b, "csparsecholesky")
self.checkarray(mul(M, C), b)
def test_general_convex_sparse(self):
self.message("Convex sparse QP with solvers: " +
str([qpsolver for qpsolver, options in qpsolvers]))
H = c.diag([2, 1, 0.2, 0.7, 1.3])
H[1, 2] = 0.1
H[2, 1] = 0.1
G = DMatrix([-2, -6, 1, 0, 0])
A = DMatrix([[1, 0, 0.1, 0.7, -1], [0.1, 2, -0.3, 4, 0.1]])
A = sparse(A)
LBA = DMatrix([-inf])
UBA = DMatrix([2, 2])
LBX = DMatrix([0] * 5)
UBX = DMatrix([inf] * 5)
options = {"mutol": 1e-12, "artol": 1e-12, "tol": 1e-12}
for qpsolver, qp_options in qpsolvers:
self.message("general_convex: " + str(qpsolver))
solver = QpSolver(qpsolver, qpStruct(h=H.sparsity(),
a=A.sparsity()))
for key, val in options.iteritems():
if solver.hasOption(key):
solver.setOption(key, val)
solver.setOption(qp_options)
solver.init()
solver.setInput(H, "h")
solver.setInput(G, "g")
solver.setInput(A, "a")
solver.setInput(LBX, "lbx")
solver.setInput(UBX, "ubx")
solver.setInput(LBA, "lba")
solver.setInput(UBA, "uba")
solver.evaluate()
self.checkarray(solver.getOutput(),
DMatrix([0.873908, 0.95630465, 0, 0, 0]),
str(qpsolver),
digits=6)
self.checkarray(solver.getOutput("lam_x"),
DMatrix([0, 0, -0.339076, -10.0873907, -0.252185]),
6,
str(qpsolver),
digits=6)
self.checkarray(solver.getOutput("lam_a"),
DMatrix([0, 2.52184767]),
str(qpsolver),
digits=6)
self.assertAlmostEqual(
solver.getOutput("cost")[0], -6.264669320767, 6, str(qpsolver))
def compute_mass_matrix(dae, conf, f1, f2, f3, t1, t2, t3):
'''
take the dae that has x/z/u/p added to it already and return
the states added to it and return mass matrix and rhs of the dae residual
'''
R_b2n = dae['R_n2b'].T
r_n2b_n = C.veccat([dae['r_n2b_n_x'], dae['r_n2b_n_y'], dae['r_n2b_n_z']])
r_b2bridle_b = C.veccat([0,0,conf['zt']])
v_bn_n = C.veccat([dae['v_bn_n_x'],dae['v_bn_n_y'],dae['v_bn_n_z']])
r_n2bridle_n = r_n2b_n + C.mul(R_b2n, r_b2bridle_b)
mm00 = C.diag([1,1,1]) * (conf['mass'] + conf['tether_mass']/3.0)
mm01 = C.SXMatrix(3,3)
mm10 = mm01.T
mm02 = r_n2bridle_n
mm20 = mm02.T
J = C.vertcat([C.horzcat([ conf['j1'], 0, conf['j31']]),
C.horzcat([ 0, conf['j2'], 0]),
C.horzcat([conf['j31'], 0, conf['j3']])])
mm11 = J
mm12 = C.cross(r_b2bridle_b, C.mul(dae['R_n2b'], r_n2b_n))
mm21 = mm12.T
mm22 = C.SXMatrix(1,1)
mm = C.vertcat([C.horzcat([mm00,mm01,mm02]),
C.horzcat([mm10,mm11,mm12]),
C.horzcat([mm20,mm21,mm22])])
# right hand side
rhs0 = C.veccat([f1,f2,f3 + conf['g']*(conf['mass'] + conf['tether_mass']*0.5)])
rhs1 = C.veccat([t1,t2,t3]) - C.cross(dae['w_bn_b'], C.mul(J, dae['w_bn_b']))
# last element of RHS
R_n2b = dae['R_n2b']
w_bn_b = dae['w_bn_b']
grad_r_cdot = v_bn_n + C.mul(R_b2n, C.cross(dae['w_bn_b'], r_b2bridle_b))
tPR = - C.cross(C.mul(R_n2b, r_n2b_n), C.cross(w_bn_b, r_b2bridle_b)) - \
C.cross(C.mul(R_n2b, v_bn_n), r_b2bridle_b)
rhs2 = -C.mul(grad_r_cdot.T, v_bn_n) - C.mul(tPR.T, w_bn_b) + dae['dr']**2 + dae['r']*dae['ddr']
rhs = C.veccat([rhs0,rhs1,rhs2])
c = 0.5*(C.mul(r_n2bridle_n.T, r_n2bridle_n) - dae['r']**2)
v_bridlen_n = v_bn_n + C.mul(R_b2n, C.cross(w_bn_b, r_b2bridle_b))
cdot = C.mul(r_n2bridle_n.T, v_bridlen_n) - dae['r']*dae['dr']
a_bn_n = C.veccat([dae.ddt(name) for name in ['v_bn_n_x','v_bn_n_y','v_bn_n_z']])
dw_bn_b = C.veccat([dae.ddt(name) for name in ['w_bn_b_x','w_bn_b_y','w_bn_b_z']])
a_bridlen_n = a_bn_n + C.mul(R_b2n, C.cross(dw_bn_b, r_b2bridle_b) + C.cross(w_bn_b, C.cross(w_bn_b, r_b2bridle_b)))
cddot = C.mul(v_bridlen_n.T, v_bridlen_n) + C.mul(r_n2bridle_n.T, a_bridlen_n) - \
dae['dr']**2 - dae['r']*dae['ddr']
dae['c'] = c
dae['cdot'] = cdot
dae['cddot'] = cddot
return (mm, rhs)
def _setup_process_noise_covariance_and_weightings(self):
r_w = np.ones(self.nx)
r_w[self.x_index["T_hts"][0]] = 2.0
r_w[self.x_index["T_hts"][1]] = 2.0
r_w[self.x_index["T_hts"][2]] = 2.0
r_w[self.x_index["T_hts"][3]] = 2.0
r_w[self.x_index["T_lts"][0]] = 2.0
r_w[self.x_index["T_lts"][1]] = 2.0
r_w[self.x_index["T_fpsc"]] = 1.0
r_w[self.x_index["T_fpsc_s"]] = 2.0
r_w[self.x_index["T_vtsc"]] = 1.0
r_w[self.x_index["T_vtsc_s"]] = 2.0
r_w[self.x_index["T_pscf"]] = 1.0
r_w[self.x_index["T_pscr"]] = 1.0
r_w[self.x_index["T_shx_psc"][0]] = 3.0
r_w[self.x_index["T_shx_psc"][1]] = 1.0
r_w[self.x_index["T_shx_psc"][2]] = 1.0
r_w[self.x_index["T_shx_psc"][3]] = 3.0
r_w[self.x_index["T_shx_ssc"][0]] = 3.0
r_w[self.x_index["T_shx_ssc"][1]] = 1.0
r_w[self.x_index["T_shx_ssc"][2]] = 1.0
r_w[self.x_index["T_shx_ssc"][3]] = 3.0
r_w[self.x_index["T_fcu_a"]] = 3.0
r_w[self.x_index["T_fcu_w"]] = 2.0
r_w[self.x_index["T_r_c"][0]] = 2.0
r_w[self.x_index["T_r_c"][1]] = 2.0
r_w[self.x_index["T_r_c"][2]] = 2.0
r_w[self.x_index["T_r_a"][0]] = 2.0
r_w[self.x_index["T_r_a"][1]] = 2.0
r_w[self.x_index["T_r_a"][2]] = 2.0
r_w[self.x_aux_index["dT_amb"]] = 2.0
r_w[self.x_aux_index["dI_vtsc"]] = 10.0
r_w[self.x_aux_index["dI_fpsc"]] = 10.0
for x_aux_idx in self.x_aux_index["Qdot_n_c"]:
r_w[x_aux_idx] = 5.0
for x_aux_idx in self.x_aux_index["Qdot_n_a"]:
r_w[x_aux_idx] = 5.0
r_w[self.x_aux_index["dalpha_vtsc"]] = 1.0
r_w[self.x_aux_index["dalpha_fpsc"]] = 1.0
self.R_w = ca.diag(r_w)
self.W_w = ca.inv(self.R_w)
def compute_mass_matrix(dae, conf, f1, f2, f3, t1, t2, t3):
'''
take the dae that has x/z/u/p added to it already and return
the states added to it and return mass matrix and rhs of the dae residual
'''
R_b2n = dae['R_n2b'].T
r_n2b_n = C.veccat([dae['r_n2b_n_x'], dae['r_n2b_n_y'], dae['r_n2b_n_z']])
r_b2bridle_b = C.veccat([0,0,conf['zt']])
v_bn_n = C.veccat([dae['v_bn_n_x'],dae['v_bn_n_y'],dae['v_bn_n_z']])
r_n2bridle_n = r_n2b_n + C.mul(R_b2n, r_b2bridle_b)
mm00 = C.diag([1,1,1]) * (conf['mass'] + conf['tether_mass']/3.0)
mm01 = C.SXMatrix(3,3)
mm10 = mm01.T
mm02 = r_n2bridle_n
mm20 = mm02.T
J = C.blockcat([[ conf['j1'], 0, conf['j31']],
[ 0, conf['j2'], 0],
[conf['j31'], 0, conf['j3']]])
mm11 = J
mm12 = C.cross(r_b2bridle_b, C.mul(dae['R_n2b'], r_n2b_n))
mm21 = mm12.T
mm22 = C.SXMatrix(1,1)
mm = C.blockcat([[mm00,mm01,mm02],
[mm10,mm11,mm12],
[mm20,mm21,mm22]])
# right hand side
rhs0 = C.veccat([f1,f2,f3 + conf['g']*(conf['mass'] + conf['tether_mass']*0.5)])
rhs1 = C.veccat([t1,t2,t3]) - C.cross(dae['w_bn_b'], C.mul(J, dae['w_bn_b']))
# last element of RHS
R_n2b = dae['R_n2b']
w_bn_b = dae['w_bn_b']
grad_r_cdot = v_bn_n + C.mul(R_b2n, C.cross(dae['w_bn_b'], r_b2bridle_b))
tPR = - C.cross(C.mul(R_n2b, r_n2b_n), C.cross(w_bn_b, r_b2bridle_b)) - \
C.cross(C.mul(R_n2b, v_bn_n), r_b2bridle_b)
rhs2 = -C.mul(grad_r_cdot.T, v_bn_n) - C.mul(tPR.T, w_bn_b) + dae['dr']**2 + dae['r']*dae['ddr']
rhs = C.veccat([rhs0,rhs1,rhs2])
c = 0.5*(C.mul(r_n2bridle_n.T, r_n2bridle_n) - dae['r']**2)
v_bridlen_n = v_bn_n + C.mul(R_b2n, C.cross(w_bn_b, r_b2bridle_b))
cdot = C.mul(r_n2bridle_n.T, v_bridlen_n) - dae['r']*dae['dr']
a_bn_n = C.veccat([dae.ddt(name) for name in ['v_bn_n_x','v_bn_n_y','v_bn_n_z']])
dw_bn_b = C.veccat([dae.ddt(name) for name in ['w_bn_b_x','w_bn_b_y','w_bn_b_z']])
a_bridlen_n = a_bn_n + C.mul(R_b2n, C.cross(dw_bn_b, r_b2bridle_b) + C.cross(w_bn_b, C.cross(w_bn_b, r_b2bridle_b)))
cddot = C.mul(v_bridlen_n.T, v_bridlen_n) + C.mul(r_n2bridle_n.T, a_bridlen_n) - \
dae['dr']**2 - dae['r']*dae['ddr']
dae['c'] = c
dae['cdot'] = cdot
dae['cddot'] = cddot
return (mm, rhs)
def get_regularised_cost_expr(self):
slack_var = self.skill_spec.slack_var
if slack_var is not None:
slack_H = cs.diag(self.weight_shifter + self.slack_var_weights)
slack_cost = cs.mtimes(cs.mtimes(slack_var.T, slack_H), slack_var)
else:
slack_cost = 0.0
return self.weight_shifter * self.cost_expression + slack_cost
def test_general_convex_sparse(self):
self.message("Convex sparse QP with solvers: " +
str([qpsolver for qpsolver, options in qpsolvers]))
H = c.diag([2, 1, 0.2, 0.7, 1.3])
H[1, 2] = 0.1
H[2, 1] = 0.1
G = DMatrix([-2, -6, 1, 0, 0])
A = DMatrix([[1, 0, 0.1, 0.7, -1], [0.1, 2, -0.3, 4, 0.1]])
makeSparse(A)
LBA = DMatrix([-inf])
UBA = DMatrix([2, 2])
LBX = DMatrix([0] * 5)
UBX = DMatrix([inf] * 5)
options = {"mutol": 1e-12, "artol": 1e-12, "tol": 1e-12}
for qpsolver, qp_options in qpsolvers:
self.message("general_convex: " + str(qpsolver))
solver = qpsolver(H.sparsity(), A.sparsity())
for key, val in options.iteritems():
if solver.hasOption(key):
solver.setOption(key, val)
solver.setOption(qp_options)
solver.init()
solver.input(QP_H).set(H)
solver.input(QP_G).set(G)
solver.input(QP_A).set(A)
solver.input(QP_LBX).set(LBX)
solver.input(QP_UBX).set(UBX)
solver.input(QP_LBA).set(LBA)
solver.input(QP_UBA).set(UBA)
solver.solve()
self.checkarray(solver.output(),
DMatrix([0.873908, 0.95630465, 0, 0, 0]),
str(qpsolver),
digits=6)
self.checkarray(solver.output(QP_LAMBDA_X),
DMatrix([0, 0, -0.339076, -10.0873907, -0.252185]),
6,
str(qpsolver),
digits=6)
self.checkarray(solver.output(QP_LAMBDA_A),
DMatrix([0, 2.52184767]),
str(qpsolver),
digits=6)
self.assertAlmostEqual(
solver.output(QP_COST)[0], -6.264669320767, 6, str(qpsolver))
def get_cost_integrand_function(self):
"""Returns a casadi function for the discretized integrand of
the cost expression integrated one timestep. For the rectangle
method, this just amounts to timing by the timestep.
As with the other controllers, the cost is affected by the
weight shifter, giving a regularised cost with the slack
variables.
"""
# Setup new symbols needed
dt = self.timestep
# Setup skill_spec symbols
time_var = self.skill_spec.time_var
robot_var = self.skill_spec.robot_var
list_vars = [time_var, robot_var]
list_names = ["time_var", "robot_var"]
robot_vel_var = self.skill_spec.robot_vel_var
cntrl_vars = [robot_vel_var]
cntrl_names = ["robot_vel_var"]
virtual_var = self.skill_spec.virtual_var
if virtual_var is not None:
list_vars += [virtual_var]
list_names += ["virtual_var"]
virtual_vel_var = self.skill_spec.virtual_vel_var
cntrl_vars += [virtual_vel_var]
cntrl_names += ["virtual_vel_var"]
slack_var = self.skill_spec.slack_var
if slack_var is not None:
list_vars += [slack_var]
list_names += ["slack_var"]
# Full symbol list same way as in other controllers
list_vars += cntrl_vars
list_names += cntrl_names
# Expression for the cost with regularisation:
if slack_var is not None:
slack_H = cs.diag(self.weight_shifter + self.slack_var_weights)
slack_cost = cs.mtimes(cs.mtimes(slack_var.T, slack_H), slack_var)
regularised_cost = self.weight_shifter*self.cost_expression
regularised_cost += slack_cost
else:
regularised_cost = self.cost_expression
# Choose integration method
if self.options["cost_integration_method"].lower() == "rectangle":
cost_integrand = regularised_cost*dt
elif self.options["cost_integration_method"].lower() == "trapezoidal":
# Trapezoidal rule
raise NotImplementedError("Trapezoidal rule integration not"
+ " implemented.")
elif self.options["cost_integration_method"].lower() == "simpson":
# Simpson rule
raise NotImplementedError("Simpson rule integration not"
+ " implemented.")
else:
raise NotImplementedError(self.options["cost_integration_method"]
+ " is not a known integration method.")
return cs.Function("fc_k", list_vars, [cost_integrand],
list_names, ["cost_integrand"])
def test_cholesky2(self):
random.seed(0)
n = 10
L = c.diag(range(n))
M = mul(L,L.T)
print L
S = CSparseCholesky(M.sparsity())
S.init()
S.getFactorizationSparsity().spy()
def test_cholesky(self):
random.seed(1)
n = 10
L = self.randDMatrix(n,n,sparsity=0.2) + c.diag(range(n))
M = mul(L,L.T)
S = CSparseCholesky(M.sparsity())
S.init()
S.getFactorizationSparsity().spy()
S.setInput(M,0)
def test_badscaling(self):
#return
self.message("Badly scaled problem")
N = 50
H = c.diag(list(range(1, N + 1)))
x0 = DM(list(range(N)))
G = -1.0 * mtimes(H, x0)
A = DM(0, N)
LBX = DM([-1000] * N)
UBX = DM([1000] * N)
for conic, qp_options, aux_options in conics:
if not aux_options["quadratic"]: continue
if 'cplex' in str(conic):
continue
if 'worhp' in str(
conic
): # works but occasionaly throws segfaults, ulimit on travis?
continue
solver = casadi.conic("mysolver", conic, {
'h': H.sparsity(),
'a': A.sparsity()
}, qp_options)
try:
less_digits = aux_options["less_digits"]
except:
less_digits = 0
solver_in = {}
solver_in["h"] = H
solver_in["g"] = G
solver_in["a"] = A
solver_in["lbx"] = LBX
solver_in["ubx"] = UBX
solver_out = solver(**solver_in)
self.checkarray(solver_out["x"],
x0,
str(conic) + str(qp_options),
digits=max(1, 2 - less_digits))
self.assertAlmostEqual(solver_out["cost"][0],
-0.5 * mtimes([x0.T, H, x0]),
max(1, 3 - less_digits), str(conic))
if aux_options["dual"]:
self.checkarray(solver_out["lam_x"],
DM.zeros(N, 1),
str(conic),
digits=max(1, 4 - less_digits))
def test_general_convex_sparse(self):
self.message("Convex sparse QP with solvers: " + str([qpsolver for qpsolver,options,aux_options in qpsolvers]))
H = c.diag([2,1,0.2,0.7,1.3])
H[1,2]=0.1
H[2,1]=0.1
G = DMatrix([-2,-6,1,0,0])
A = DMatrix([[1, 0,0.1,0.7,-1],[0.1, 2,-0.3,4,0.1]])
A = sparsify(A)
LBA = DMatrix([-inf])
UBA = DMatrix([2, 2])
LBX = DMatrix([0]*5)
UBX = DMatrix([inf]*5)
options = { "mutol": 1e-12, "artol": 1e-12, "tol":1e-12}
for qpsolver, qp_options, aux_options in qpsolvers:
self.message("general_convex: " + str(qpsolver))
solver = QpSolver(qpsolver,qpStruct(h=H.sparsity(),a=A.sparsity()))
for key, val in options.iteritems():
if solver.hasOption(key):
solver.setOption(key,val)
solver.setOption(qp_options)
solver.init()
try:
less_digits=aux_options["less_digits"]
except:
less_digits=0
solver.setInput(H,"h")
solver.setInput(G,"g")
solver.setInput(A,"a")
solver.setInput(LBX,"lbx")
solver.setInput(UBX,"ubx")
solver.setInput(LBA,"lba")
solver.setInput(UBA,"uba")
solver.evaluate()
self.checkarray(solver.getOutput(),DMatrix([0.873908,0.95630465,0,0,0]),str(qpsolver),digits=max(1,6-less_digits))
self.checkarray(solver.getOutput("lam_x"),DMatrix([0,0,-0.339076,-10.0873907,-0.252185]),6,str(qpsolver),digits=max(1,6-less_digits))
self.checkarray(solver.getOutput("lam_a"),DMatrix([0,2.52184767]),str(qpsolver),digits=max(1,6-less_digits))
self.assertAlmostEqual(solver.getOutput("cost")[0],-6.264669320767,max(1,6-less_digits),str(qpsolver))
def _terminalObj(self):
# estado terminal
XN = self.X_pred[-1] # terminal state
XdN = XN[self.nxs:self.nxs+self.nxd]
Vt = 0
# Adição do custo terminal
# Q terminal
Q_lyap = [email protected]@[email protected]@self.sys.F
Q_bar = solve_discrete_lyapunov(self.sys.F, Q_lyap, method='bilinear')
Vt = csd.dot(XdN**2, csd.diag(Q_bar))
self.Q_bar = Q_bar
return Vt
def test_expm(self):
# Test for eye
correct_res = diag(exp(DM.ones(3)))
a = expm(DM.eye(3))
self.assertAlmostEqual(norm_fro(a - correct_res), 0, 3)
# Test for -magic(3) (compared with MATLAB solution)
a = DM([[-8, -1, -6], [-3, -5, -7], [-4, -9, -2]])
correct_res = DM(
[[3.646628887990924, 32.404567030885005, -36.051195612973601],
[5.022261973341555, 44.720086474306093, -49.742348141745325],
[-8.668890555430160, -77.124653199288772, 85.793544060621244]])
self.assertAlmostEqual(norm_fro(expm(a) - correct_res), 0, 2)
def test_badscaling(self):
#return
self.message("Badly scaled problem")
N = 50
H = c.diag(range(1, N + 1))
x0 = DMatrix(range(N))
G = -1.0 * mul(H, x0)
A = DMatrix(0, N)
LBX = DMatrix([-1000] * N)
UBX = DMatrix([1000] * N)
for qpsolver, qp_options, aux_options in qpsolvers:
if 'cplex' in str(qpsolver):
continue
if 'worhp' in str(
qpsolver
): # works but occasionaly throws segfaults, ulimit on travis?
continue
solver = QpSolver("mysolver", qpsolver, {
'h': H.sparsity(),
'a': A.sparsity()
}, qp_options)
try:
less_digits = aux_options["less_digits"]
except:
less_digits = 0
solver.setInput(H, "h")
solver.setInput(G, "g")
solver.setInput(A, "a")
solver.setInput(LBX, "lbx")
solver.setInput(UBX, "ubx")
solver.evaluate()
self.checkarray(solver.getOutput(),
x0,
str(qpsolver) + str(qp_options),
digits=max(1, 2 - less_digits))
self.assertAlmostEqual(
solver.getOutput("cost")[0], -0.5 * mul([x0.T, H, x0]),
max(1, 3 - less_digits), str(qpsolver))
self.checkarray(solver.getOutput("lam_x"),
DMatrix.zeros(N, 1),
str(qpsolver),
digits=max(1, 4 - less_digits))
def get_obs_cov_func(x):
pos = x[:2]
Nt = (x.shape[0] - 2) // 2
R_nominal = 0.001 * cs.MX.eye(2 * Nt)
dists = cs.MX.zeros(2 * Nt)
for ii in range(Nt):
pos_target_ii = x[2 * (ii + 1):2 * (ii + 2)]
dist_ii = cs.sqrt(cs.sum1((pos - pos_target_ii)**2))
dists[2 * ii] = dist_ii
dists[2 * ii + 1] = dist_ii
R_scale = 0.01 * cs.diag(dists)
R = R_nominal + R_scale
obs_cov = cs.Function('obs_cov', [x], [R], ['x'], ['R'])
return obs_cov
def test_cholesky2(self):
numpy.random.seed(0)
n = 10
L = c.diag(list(range(1,n+1)))
M = mtimes(L,L.T)
print(L)
S = casadi.linsol("S", "csparsecholesky", M.sparsity(), 1)
S.linsol_cholesky_sparsity().spy()
S_in = [0]*S.n_in();S_in[0]=M
S.linsol_prepare()
C = S.linsol_cholesky()
self.checkarray(mtimes(C,C.T),M)
def __init__(self, model, **kwargs):
self.cost = {
"Q": diag([10, 0.1, 0.1, 0.1]),
"R": 0.1,
"Qv": diag([100, 100, 0, 0]),
"x_ref": DM([pi, 0, 0, 0]),
}
self.state_constraints = False
self.control_constraints = False
OptimalControlProblem.__init__(self, model, obj=self.cost)
self.t_f = 5
self.x_0 = [pi / 6, 0, 0, 0]
for (k, v) in kwargs.items():
setattr(self, k, v)
if self.state_constraints:
self.x_max[2] = 10
self.x_min[2] = -10
if self.control_constraints:
self.u_max[0] = 2
self.u_min[0] = -2
def test_cholesky2(self):
numpy.random.seed(0)
n = 10
L = c.diag(range(1, n + 1))
M = mul(L, L.T)
print L
S = LinearSolver("S", "csparsecholesky", M.sparsity())
S.getFactorizationSparsity().spy()
S.setInput(M)
S.prepare()
C = S.getFactorization()
self.checkarray(mul(C, C.T), M)
def get_cost_expr(self):
"""Returns a casadi expression describing the cost.
Return:
H for min_opt_var opt_var^T*H*opt_var"""
nvirt = self.skill_spec.n_virtual_var
nslack = self.skill_spec.n_slack_var
mu = self.weight_shifter
opt_weights = [mu * self.robot_var_weights]
if nvirt > 0:
opt_weights += [mu * self.virtual_var_weights]
if nslack > 0:
opt_weights += [mu + self.slack_var_weights]
H = cs.diag(cs.vertcat(*opt_weights))
return H
def test_cholesky2(self):
numpy.random.seed(0)
n = 10
L = c.diag(range(1,n+1))
M = mul(L,L.T)
print L
S = LinearSolver("S", "csparsecholesky", M.sparsity())
S.getFactorizationSparsity().spy()
S.setInput(M)
S.prepare()
C = S.getFactorization()
self.checkarray(mul(C,C.T),M)
def test_cholesky2(self):
numpy.random.seed(0)
n = 10
L = c.diag(list(range(1, n + 1)))
M = mtimes(L, L.T)
print(L)
S = casadi.linsol("S", "csparsecholesky", M.sparsity(), 0)
S.linsol_cholesky_sparsity().spy()
S_in = [0] * S.n_in()
S_in[0] = M
S.linsol_prepare()
C = S.linsol_cholesky()
self.checkarray(mtimes(C, C.T), M)
def test_badscaling(self):
#return
self.message("Badly scaled problem")
N = 50
H = c.diag(range(1, N + 1))
x0 = DMatrix(range(N))
G = -1.0 * mul(H, x0)
A = DMatrix.sparse(0, N)
LBX = DMatrix([-1000] * N)
UBX = DMatrix([1000] * N)
options = {"mutol": 1e-12, "artol": 1e-12, "tol": 1e-12}
for qpsolver, qp_options in qpsolvers:
if 'cplex' in str(qpsolver):
continue
solver = QpSolver(qpsolver, qpStruct(h=H.sparsity(),
a=A.sparsity()))
for key, val in options.iteritems():
if solver.hasOption(key):
solver.setOption(key, val)
solver.setOption(qp_options)
solver.init()
solver.setInput(H, "h")
solver.setInput(G, "g")
solver.setInput(A, "a")
solver.setInput(LBX, "lbx")
solver.setInput(UBX, "ubx")
solver.evaluate()
self.checkarray(solver.getOutput(),
x0,
str(qpsolver) + str(qp_options),
digits=2)
self.assertAlmostEqual(
solver.getOutput("cost")[0], -0.5 * mul([x0.T, H, x0]), 3,
str(qpsolver))
self.checkarray(solver.getOutput("lam_x"),
DMatrix.zeros(N, 1),
str(qpsolver),
digits=4)
def test_general_convex_sparse(self):
self.message("Convex sparse QP with solvers: " + str([conic for conic,options,aux_options in conics]))
H = c.diag([2,1,0.2,0.7,1.3])
H[1,2]=0.1
H[2,1]=0.1
G = DM([-2,-6,1,0,0])
A = DM([[1, 0,0.1,0.7,-1],[0.1, 2,-0.3,4,0.1]])
A = sparsify(A)
LBA = DM([-inf])
UBA = DM([2, 2])
LBX = DM([0]*5)
UBX = DM([inf]*5)
for conic, qp_options, aux_options in conics:
if not aux_options["quadratic"]: continue
self.message("general_convex: " + str(conic))
solver = casadi.conic("mysolver",conic,{'h':H.sparsity(),'a':A.sparsity()},qp_options)
try:
less_digits=aux_options["less_digits"]
except:
less_digits=0
solver_in = {}
solver_in["h"]=H
solver_in["g"]=G
solver_in["a"]=A
solver_in["lbx"]=LBX
solver_in["ubx"]=UBX
solver_in["lba"]=LBA
solver_in["uba"]=UBA
solver_out = solver(**solver_in)
self.checkarray(solver_out["x"],DM([0.873908,0.95630465,0,0,0]),str(conic),digits=max(1,6-less_digits))
if aux_options["dual"]: self.checkarray(solver_out["lam_x"],DM([0,0,-0.339076,-10.0873907,-0.252185]),6,str(conic),digits=max(1,6-less_digits))
if aux_options["dual"]: self.checkarray(solver_out["lam_a"],DM([0,2.52184767]),str(conic),digits=max(1,6-less_digits))
self.assertAlmostEqual(solver_out["cost"][0],-6.264669320767,max(1,6-less_digits),str(conic))
def test_badscaling(self):
#return
self.message("Badly scaled problem")
N = 50
H = c.diag(range(1, N + 1))
x0 = DMatrix(range(N))
G = -1.0 * mul(H, x0)
A = DMatrix(0, N)
LBX = DMatrix([-1000] * N)
UBX = DMatrix([1000] * N)
options = {"mutol": 1e-12, "artol": 1e-12, "tol": 1e-12}
for qpsolver, qp_options in qpsolvers:
if 'Cplex' in str(qpsolver):
continue
solver = qpsolver(H.sparsity(), A.sparsity())
for key, val in options.iteritems():
if solver.hasOption(key):
solver.setOption(key, val)
solver.setOption(qp_options)
solver.init()
solver.input(QP_H).set(H)
solver.input(QP_G).set(G)
solver.input(QP_A).set(A)
solver.input(QP_LBX).set(LBX)
solver.input(QP_UBX).set(UBX)
solver.solve()
self.checkarray(solver.output(),
x0,
str(qpsolver) + str(qp_options),
digits=2)
self.assertAlmostEqual(
solver.output(QP_COST)[0], -0.5 * mul([x0.T, H, x0]), 3,
str(qpsolver))
self.checkarray(solver.output(QP_LAMBDA_X),
DMatrix.zeros(N, 1),
str(qpsolver),
digits=4)
def generate_model(self, configuration):
""" Logic to construct a model, and smooth representation on BSpline basis """
self.n = configuration['grid_size']
self.K = configuration['basis_number']
self.s = configuration['model_form']['state']
n_ps = configuration['model_form']['parameters']
# setup fine time grid
self.ts = ca.MX.sym("t", self.n, 1)
self.observation_times = np.linspace(*configuration['time_span'][:2],
self.n)
# determine knots and build basis functions
if configuration['knot_function'] is None:
knots = casbasis.choose_knots(self.observation_times, self.K - 2)
else:
knots = configuration['knot_function'](self.observation_times,
self.K - 2,
configuration['dataset'])
self.basis_fns = casbasis.basis_functions(knots)
self.basis = ca.vcat([b(self.ts) for b in self.basis_fns]).reshape(
(self.n, self.K))
self.tssx = ca.SX.sym("t", self.n, 1)
# define basis matrix and gradient matrix
phi = ca.Function('phi', [self.ts], [self.basis])
self.phi = np.array(phi(self.observation_times))
bjac = ca.vcat([
ca.diag(ca.jacobian(self.basis[:, i], self.ts))
for i in range(self.K)
]).reshape((self.n, self.K))
self.basis_jacobian = np.array(
ca.Function('bjac', [self.ts], [bjac])(self.observation_times))
# create the objects that define the smooth, model parameters
self.cs = [ca.SX.sym("c_" + str(i), self.K, 1) for i in range(self.s)]
self.xs = [self.phi @ ci for ci in self.cs]
self.xdash = self.basis_jacobian @ ca.hcat(self.cs)
self.ps = [ca.SX.sym("p_" + str(i)) for i in range(n_ps)]
# model function derived from input model function
self.model = ca.Function(
"model", [self.tssx, *self.cs, *self.ps],
[ca.hcat(configuration['model'](self.tssx, self.xs, self.ps))])
def test_general_convex_sparse(self):
self.message("Convex sparse QP with solvers: " + str([qpsolver for qpsolver,options in qpsolvers]))
H = c.diag([2,1,0.2,0.7,1.3])
H[1,2]=0.1
H[2,1]=0.1
G = DMatrix([-2,-6,1,0,0])
A = DMatrix([[1, 0,0.1,0.7,-1],[0.1, 2,-0.3,4,0.1]])
makeSparse(A)
LBA = DMatrix([-inf])
UBA = DMatrix([2, 2])
LBX = DMatrix([0]*5)
UBX = DMatrix([inf]*5)
options = { "mutol": 1e-12, "artol": 1e-12, "tol":1e-12}
for qpsolver, qp_options in qpsolvers:
self.message("general_convex: " + str(qpsolver))
solver = qpsolver(H.sparsity(),A.sparsity())
for key, val in options.iteritems():
if solver.hasOption(key):
solver.setOption(key,val)
solver.setOption(qp_options)
solver.init()
solver.input(QP_H).set(H)
solver.input(QP_G).set(G)
solver.input(QP_A).set(A)
solver.input(QP_LBX).set(LBX)
solver.input(QP_UBX).set(UBX)
solver.input(QP_LBA).set(LBA)
solver.input(QP_UBA).set(UBA)
solver.solve()
self.checkarray(solver.output(),DMatrix([0.873908,0.95630465,0,0,0]),str(qpsolver),digits=6)
self.checkarray(solver.output(QP_LAMBDA_X),DMatrix([0,0,-0.339076,-10.0873907,-0.252185]),6,str(qpsolver),digits=6)
self.checkarray(solver.output(QP_LAMBDA_A),DMatrix([0,2.52184767]),str(qpsolver),digits=6)
self.assertAlmostEqual(solver.output(QP_COST)[0],-6.264669320767,6,str(qpsolver))
def test_badscaling(self):
#return
self.message("Badly scaled problem")
N = 50
H = c.diag(range(1,N+1))
x0 = DMatrix(range(N))
G = -1.0*mul(H,x0)
A = DMatrix(0,N)
LBX = DMatrix([-1000]*N)
UBX = DMatrix([1000]*N)
options = {"mutol": 1e-12, "artol": 1e-12, "tol":1e-12}
for qpsolver, qp_options, aux_options in qpsolvers:
if 'cplex' in str(qpsolver):
continue
solver = QpSolver(qpsolver,qpStruct(h=H.sparsity(),a=A.sparsity()))
for key, val in options.iteritems():
if solver.hasOption(key):
solver.setOption(key,val)
solver.setOption(qp_options)
solver.init()
try:
less_digits=aux_options["less_digits"]
except:
less_digits=0
solver.setInput(H,"h")
solver.setInput(G,"g")
solver.setInput(A,"a")
solver.setInput(LBX,"lbx")
solver.setInput(UBX,"ubx")
solver.evaluate()
self.checkarray(solver.getOutput(),x0,str(qpsolver)+str(qp_options),digits=max(1,2-less_digits))
self.assertAlmostEqual(solver.getOutput("cost")[0],-0.5*mul([x0.T,H,x0]),max(1,3-less_digits),str(qpsolver))
self.checkarray(solver.getOutput("lam_x"),DMatrix.zeros(N,1),str(qpsolver),digits=max(1,4-less_digits))
def diag(v, k=0):
"""
Extract a diagonal or construct a diagonal array.
See syntax here: https://numpy.org/doc/stable/reference/generated/numpy.diag.html
"""
if not is_casadi_type(v):
return _onp.diag(v, k=k)
else:
if k != 0:
raise NotImplementedError("Should be super possible, just haven't had the need yet.")
if 1 in v.shape:
return _cas.diag(v)
elif v.shape[0] == v.shape[1]:
raise NotImplementedError("Should be super possible, just haven't had the need yet.")
else:
raise ValueError("Cannot return the diagonal of a non-square matrix.")
def test_badscaling(self):
#return
self.message("Badly scaled problem")
N = 50
H = c.diag(list(range(1,N+1)))
x0 = DM(list(range(N)))
G = -1.0*mtimes(H,x0)
A = DM(0,N)
LBX = DM([-1000]*N)
UBX = DM([1000]*N)
for conic, qp_options, aux_options in conics:
if not aux_options["quadratic"]: continue
if 'cplex' in str(conic):
continue
if 'worhp' in str(conic): # works but occasionaly throws segfaults, ulimit on travis?
continue
solver = casadi.conic("mysolver",conic,{'h':H.sparsity(),'a':A.sparsity()},qp_options)
try:
less_digits=aux_options["less_digits"]
except:
less_digits=0
solver_in = {}
solver_in["h"]=H
solver_in["g"]=G
solver_in["a"]=A
solver_in["lbx"]=LBX
solver_in["ubx"]=UBX
solver_out = solver(**solver_in)
self.checkarray(solver_out["x"],x0,str(conic)+str(qp_options),digits=max(1,2-less_digits))
self.assertAlmostEqual(solver_out["cost"][0],-0.5*mtimes([x0.T,H,x0]),max(1,3-less_digits),str(conic))
if aux_options["dual"]: self.checkarray(solver_out["lam_x"],DM.zeros(N,1),str(conic),digits=max(1,4-less_digits))
def test_badscaling(self):
#return
self.message("Badly scaled problem")
N = 50
H = c.diag(range(1,N+1))
x0 = DMatrix(range(N))
G = -1.0*mul(H,x0)
A = DMatrix(0,N)
LBX = DMatrix([-1000]*N)
UBX = DMatrix([1000]*N)
options = {"mutol": 1e-12, "artol": 1e-12, "tol":1e-12}
for qpsolver, qp_options in qpsolvers:
if 'Cplex' in str(qpsolver):
continue
solver = qpsolver(H.sparsity(),A.sparsity())
for key, val in options.iteritems():
if solver.hasOption(key):
solver.setOption(key,val)
solver.setOption(qp_options)
solver.init()
solver.input(QP_H).set(H)
solver.input(QP_G).set(G)
solver.input(QP_A).set(A)
solver.input(QP_LBX).set(LBX)
solver.input(QP_UBX).set(UBX)
solver.solve()
self.checkarray(solver.output(),x0,str(qpsolver)+str(qp_options),digits=2)
self.assertAlmostEqual(solver.output(QP_COST)[0],-0.5*mul([x0.T,H,x0]),3,str(qpsolver))
self.checkarray(solver.output(QP_LAMBDA_X),DMatrix.zeros(N,1),str(qpsolver),digits=4)
def test_badscaling(self):
#return
self.message("Badly scaled problem")
N = 50
H = c.diag(range(1,N+1))
x0 = DMatrix(range(N))
G = -1.0*mul(H,x0)
A = DMatrix(0,N)
LBX = DMatrix([-1000]*N)
UBX = DMatrix([1000]*N)
for qpsolver, qp_options, aux_options in qpsolvers:
if 'cplex' in str(qpsolver):
continue
if 'worhp' in str(qpsolver): # works but occasionaly throws segfaults, ulimit on travis?
continue
solver = QpSolver("mysolver",qpsolver,{'h':H.sparsity(),'a':A.sparsity()},qp_options)
try:
less_digits=aux_options["less_digits"]
except:
less_digits=0
solver.setInput(H,"h")
solver.setInput(G,"g")
solver.setInput(A,"a")
solver.setInput(LBX,"lbx")
solver.setInput(UBX,"ubx")
solver.evaluate()
self.checkarray(solver.getOutput(),x0,str(qpsolver)+str(qp_options),digits=max(1,2-less_digits))
self.assertAlmostEqual(solver.getOutput("cost")[0],-0.5*mul([x0.T,H,x0]),max(1,3-less_digits),str(qpsolver))
self.checkarray(solver.getOutput("lam_x"),DMatrix.zeros(N,1),str(qpsolver),digits=max(1,4-less_digits))
def loadGPModel(name, model, xscaler, yscaler, kernel='RBF'):
""" GP mean and variance as casadi.SX variable
"""
X = model.X_train_
x = cs.SX.sym('x', 1, X.shape[1])
# mean
if kernel == 'RBF':
K1 = CasadiRBF(x, X, model)
K2 = CasadiConstant(x, X, model)
K = K1 + K2
elif kernel == 'Matern':
K = CasadiMatern(x, X, model)
else:
raise NotImplementedError
y_mu = cs.mtimes(K, model.alpha_) + model._y_train_mean
y_mu = y_mu * yscaler.scale_ + yscaler.mean_
# variance
L_inv = solve_triangular(model.L_.T,np.eye(model.L_.shape[0]))
K_inv = L_inv.dot(L_inv.T)
if kernel == 'RBF':
K1_ = CasadiRBF(x, x, model)
K2_ = CasadiConstant(x, x, model)
K_ = K1_ + K2_
elif kernel == 'Matern':
K_ = CasadiMatern(x, x, model)
y_var = cs.diag(K_) - cs.sum2(cs.mtimes(K, K_inv)*K)
y_var = cs.fmax(y_var, 0)
y_std = cs.sqrt(y_var)
y_std *= yscaler.scale_
gpmodel = cs.Function(name, [x], [y_mu, y_std])
return gpmodel
def __constraint(self, mean, covar, H, quantile, ub, lb, eps):
""" Build up chance constraint vectors
"""
r = ca.SX.sym('r')
mean_s = ca.SX.sym('mean', ca.MX.size(mean))
S_s = ca.SX.sym('S', ca.MX.size(covar))
H_s = ca.SX.sym('H', 1, ca.MX.size2(H))
S = covar
con_func = ca.Function('con', [mean_s, S_s, H_s, r],
[H_s @ mean_s + r * H_s @ ca.diag(S_s)])
con = []
con_lb = []
con_ub = []
for i in range(ca.MX.size1(mean)):
con.append(con_func(mean, S, H[i, :], quantile[i]) - eps[i])
con_ub.append(ub[i])
con_lb.append(-np.inf)
con.append(con_func(mean, S, H[i, :], -quantile[i]) + eps[i])
con_ub.append(np.inf)
con_lb.append(lb[i])
cons = dict(con=con, con_lb=con_lb, con_ub=con_ub)
return cons
def test_MX(self):
x = MX.sym("x",2)
y = MX.sym("y",2,2)
f1 = Function("f1", [x,y],[x+y[0,0],mtimes(y,x)])
f2 = Function("f2", [x,y],[mtimes(MX.zeros(0,2),x)])
f3 = Function("f3", [x,y],[MX.zeros(0,0),mtimes(y,x)])
f4 = Function("f4", [x,y],[MX.zeros(0,2),mtimes(y,x)])
ndir = 2
in1 = [x,y]
v1 = [DM([1.1,1.3]),DM([[0.7,1.5],[2.1,0.9]])]
w=x[:]
w[1]*=2
w2=x[:]
w2[1]*=x[0]
ww=x[:]
ww[[0,1]]*=x
wwf=x[:]
wwf[[1,0]]*=x
wwr=x[:]
wwr[[0,0,1,1]]*=2
yy=y[:,:]
yy[:,0] = x
yy2=y[:,:]
yy2[:,0] = x**2
yyy=y[:,:]
yyy[[1,0],0] = x
yyy2=y[:,:]
yyy2[[1,0],0] = x**2
def remove_first(x):
ret = DM(x)
if ret.numel()>0:
ret[0,0] = DM(1,1)
return ret.sparsity()
else:
return ret.sparsity()
def remove_last(x):
ret = DM(x)
if ret.nnz()>0:
ret[ret.sparsity().row()[-1],ret.sparsity().get_col()[-1]] = DM(1,1)
return ret.sparsity()
else:
return x
spmods = [lambda x: x , remove_first, remove_last]
# TODO: sparse seeding
for inputs,values,out, jac in [
(in1,v1,x,DM.eye(2)),
(in1,v1,x.T,DM.eye(2)),
(in1,v1,x**2,2*c.diag(x)),
(in1,v1,(x**2).attachAssert(True),2*c.diag(x)),
(in1,v1,(x**2).T,2*c.diag(x)),
(in1,v1,c.reshape(x,(1,2)),DM.eye(2)),
(in1,v1,c.reshape(x**2,(1,2)),2*c.diag(x)),
(in1,v1,x+y.nz[0],DM.eye(2)),
(in1,v1,x+y[0,0],DM.eye(2)),
(in1,v1,x+x,2*DM.eye(2)),
(in1,v1,x**2+x,2*c.diag(x)+DM.eye(2)),
(in1,v1,x*x,2*c.diag(x)),
(in1,v1,x*y.nz[0],DM.eye(2)*y.nz[0]),
(in1,v1,x*y[0,0],DM.eye(2)*y[0,0]),
(in1,v1,x[0],DM.eye(2)[0,:]),
(in1,v1,(x**2)[0],horzcat(*[2*x[0],MX(1,1)])),
(in1,v1,x[0]+x[1],DM.ones(1,2)),
(in1,v1,sin(repmat(x**2,1,3)),repmat(cos(c.diag(x**2))*2*c.diag(x),3,1)),
(in1,v1,sin(repsum((x**2).T,1,2)),cos(x[0]**2+x[1]**2)*2*x.T),
(in1,v1,vertcat(*[x[1],x[0]]),sparsify(DM([[0,1],[1,0]]))),
(in1,v1,vertsplit(x,[0,1,2])[1],sparsify(DM([[0,1]]))),
(in1,v1,vertcat(*[x[1]**2,x[0]**2]),blockcat([[MX(1,1),2*x[1]],[2*x[0],MX(1,1)]])),
(in1,v1,vertsplit(x**2,[0,1,2])[1],blockcat([[MX(1,1),2*x[1]]])),
(in1,v1,vertsplit(x**2,[0,1,2])[1]**3,blockcat([[MX(1,1),6*x[1]**5]])),
(in1,v1,horzcat(*[x[1],x[0]]).T,sparsify(DM([[0,1],[1,0]]))),
(in1,v1,horzcat(*[x[1]**2,x[0]**2]).T,blockcat([[MX(1,1),2*x[1]],[2*x[0],MX(1,1)]])),
(in1,v1,diagcat(*[x[1]**2,y,x[0]**2]),
blockcat( [[MX(1,1),2*x[1]]] + ([[MX(1,1),MX(1,1)]]*14) + [[2*x[0],MX(1,1)]] )
),
(in1,v1,horzcat(*[x[1]**2,x[0]**2]).T,blockcat([[MX(1,1),2*x[1]],[2*x[0],MX(1,1)]])),
(in1,v1,x[[0,1]],sparsify(DM([[1,0],[0,1]]))),
(in1,v1,(x**2)[[0,1]],2*c.diag(x)),
(in1,v1,x[[0,0,1,1]],sparsify(DM([[1,0],[1,0],[0,1],[0,1]]))),
(in1,v1,(x**2)[[0,0,1,1]],blockcat([[2*x[0],MX(1,1)],[2*x[0],MX(1,1)],[MX(1,1),2*x[1]],[MX(1,1),2*x[1]]])),
(in1,v1,wwr,sparsify(DM([[2,0],[0,2]]))),
(in1,v1,x[[1,0]],sparsify(DM([[0,1],[1,0]]))),
(in1,v1,x[[1,0],0],sparsify(DM([[0,1],[1,0]]))),
(in1,v1,w,sparsify(DM([[1,0],[0,2]]))),
(in1,v1,w2,blockcat([[1,MX(1,1)],[x[1],x[0]]])),
(in1,v1,ww,2*c.diag(x)),
(in1,v1,wwf,vertcat(*[x[[1,0]].T,x[[1,0]].T])),
(in1,v1,yy[:,0],DM.eye(2)),
(in1,v1,yy2[:,0],2*c.diag(x)),
(in1,v1,yyy[:,0],sparsify(DM([[0,1],[1,0]]))),
(in1,v1,mtimes(y,x),y),
(in1,v1,mtimes(x.T,y.T),y),
(in1,v1,mac(y,x,DM.zeros(Sparsity.triplet(2,1,[1],[0]))),y[Sparsity.triplet(2,2,[1,1],[0,1])]),
(in1,v1,mac(x.T,y.T,DM.zeros(Sparsity.triplet(2,1,[1],[0]).T)),y[Sparsity.triplet(2,2,[1,1],[0,1])]),
(in1,v1,mtimes(y[Sparsity.triplet(2,2,[0,1,1],[0,0,1])],x),y[Sparsity.triplet(2,2,[0,1,1],[0,0,1])]),
(in1,v1,mtimes(x.T,y[Sparsity.triplet(2,2,[0,1,1],[0,0,1])].T),y[Sparsity.triplet(2,2,[0,1,1],[0,0,1])]),
(in1,v1,mtimes(y,x**2),y*2*vertcat(*[x.T,x.T])),
(in1,v1,sin(x),c.diag(cos(x))),
(in1,v1,sin(x**2),c.diag(cos(x**2)*2*x)),
(in1,v1,x*y[:,0],c.diag(y[:,0])),
(in1,v1,x*y.nz[[0,1]],c.diag(y.nz[[0,1]])),
(in1,v1,x*y.nz[[1,0]],c.diag(y.nz[[1,0]])),
(in1,v1,x*y[[0,1],0],c.diag(y[[0,1],0])),
(in1,v1,x*y[[1,0],0],c.diag(y[[1,0],0])),
(in1,v1,c.dot(x,x),(2*x).T),
(in1,v1,c.dot(x**2,x),(3*x**2).T),
#(in1,v1,c.det(horzcat(*[x,DM([1,2])])),DM([-1,2])), not implemented
(in1,v1,f1.call(in1)[1],y),
(in1,v1,f1.call([x**2,y])[1],y*2*vertcat(*[x.T,x.T])),
(in1,v1,f2.call(in1)[0],DM.zeros(0,2)),
(in1,v1,f2(x**2,y),DM.zeros(0,2)),
(in1,v1,f3.call(in1)[0],DM.zeros(0,2)),
(in1,v1,f3.call([x**2,y])[0],DM.zeros(0,2)),
(in1,v1,f4.call(in1)[0],DM.zeros(0,2)),
(in1,v1,f4.call([x**2,y])[0],DM.zeros(0,2)),
#(in1,v1,f1([x**2,[]])[1],DM.zeros(2,2)),
#(in1,v1,f1([[],y])[1],DM.zeros(2,2)),
(in1,v1,vertcat(*[x,DM(0,1)]),DM.eye(2)),
(in1,v1,project(x**2, sparsify(DM([0,1])).sparsity()),blockcat([[MX(1,1),MX(1,1)],[MX(1,1),2*x[1]]])),
(in1,v1,c.dot(x,y[:,0]),y[:,0].T),
(in1,v1,x.nz[IM([[1,0]])].T*y.nz[IM([[0,2]])],blockcat([[MX(1,1),y.nz[0]],[y.nz[2],MX(1,1)]])),
(in1,v1,x.nz[c.diag([1,0])]*y.nz[c.diag([0,2])],blockcat([[MX(1,1),y.nz[0]],[MX(1,1),MX(1,1)],[MX(1,1),MX(1,1)],[y.nz[2],MX(1,1)]])),
]:
print(out)
fun = Function("fun", inputs,[out,jac])
funsx = fun.expand("expand_fun")
fun_ad = [Function("fun", inputs,[out,jac], {'ad_weight':w, 'ad_weight_sp':w}) for w in [0,1]]
funsx_ad = [f.expand('expand_'+f.name()) for f in fun_ad]
fun_out = fun.call(values)
funsx_out = funsx.call(values)
self.checkarray(fun_out[0],funsx_out[0])
self.checkarray(fun_out[1],funsx_out[1])
self.check_codegen(fun,inputs=values)
J_ = fun_out[1]
def vec(l):
ret = []
for i in l:
ret.extend(i)
return ret
storage2 = {}
storage = {}
vf_mx = None
for f in [fun, fun.expand('expand_'+fun.name())]:
d1 = f.forward(ndir)
d2 = f.reverse(ndir)
num_in = f.n_in()
num_out = f.n_out()
# evalThings
for sym in [MX.sym, SX.sym]:
if f.is_a('MXFunction') and sym==SX.sym: continue
if f.is_a('SXFunction') and sym==MX.sym: continue
# dense
for spmod,spmod2 in itertools.product(spmods,repeat=2):
fseeds = [[sym("f",spmod(f.sparsity_in(i))) for i in range(f.n_in())] for d in range(ndir)]
aseeds = [[sym("a",spmod2(f.sparsity_out(i))) for i in range(f.n_out())] for d in range(ndir)]
inputss = [sym("i",f.sparsity_in(i)) for i in range(f.n_in())]
res = f.call(inputss,True)
fwdsens = forward(res,inputss,fseeds,dict(always_inline=True))
adjsens = reverse(res,inputss,aseeds,dict(always_inline=True))
fseed = [DM(fseeds[d][0].sparsity(),random.random(fseeds[d][0].nnz())) for d in range(ndir) ]
aseed = [DM(aseeds[d][0].sparsity(),random.random(aseeds[d][0].nnz())) for d in range(ndir) ]
vf = Function("vf", inputss+vec([fseeds[i]+aseeds[i] for i in range(ndir)]),list(res) + vec([list(fwdsens[i])+list(adjsens[i]) for i in range(ndir)]))
vf_in = list(values)
offset = len(inputss)
for d in range(ndir):
vf_in.append(fseed[d])
for i in range(len(values)-1):
vf_in.append(0)
vf_in.append(aseed[d])
vf_in.append(0)
vf_out = vf.call(vf_in)
self.check_codegen(vf,inputs=vf_in)
offset = len(res)
for d in range(ndir):
seed = array(fseed[d].T).ravel()
sens = array(vf_out[offset+0].T).ravel()
offset+=len(inputss)
self.checkarray(sens,mtimes(J_,seed),"eval Fwd %d %s" % (d,str(type(f))+str(sym)))
seed = array(aseed[d].T).ravel()
sens = array(vf_out[offset+0].T).ravel()
offset+=len(inputss)
self.checkarray(sens,mtimes(J_.T,seed),"eval Adj %d %s" % (d,str([vf_out[i] for i in range(vf.n_out())])))
assert(offset==vf.n_out())
# Complete random seeding
random.seed(1)
vf_in = []
for i in range(vf.n_in()):
vf_in.append(DM(vf.sparsity_in(i),random.random(vf.nnz_in(i))))
vf_out = vf.call(vf_in)
self.check_codegen(vf,inputs=vf_in)
storagekey = (spmod,spmod2)
if not(storagekey in storage):
storage[storagekey] = []
storage[storagekey].append(vf_out)
# Added to make sure that the same seeds are used for SX and MX
if sym is MX.sym:
vf_mx = vf
# Second order sensitivities
for sym2 in [MX.sym, SX.sym]:
if vf.is_a('MXFunction') and sym2==SX.sym: continue
if vf.is_a('MXFunction') and sym2==MX.sym: continue
for spmod_2,spmod2_2 in itertools.product(spmods,repeat=2):
fseeds2 = [[sym2("f",vf_mx.sparsity_in(i)) for i in range(vf.n_in())] for d in range(ndir)]
aseeds2 = [[sym2("a",vf_mx.sparsity_out(i)) for i in range(vf.n_out()) ] for d in range(ndir)]
inputss2 = [sym2("i",vf_mx.sparsity_in(i)) for i in range(vf.n_in())]
res2 = vf.call(inputss2,True)
fwdsens2 = forward(res2,inputss2,fseeds2,dict(always_inline=True))
adjsens2 = reverse(res2,inputss2,aseeds2,dict(always_inline=True))
vf2 = Function("vf2", inputss2+vec([fseeds2[i]+aseeds2[i] for i in range(ndir)]),list(res2) + vec([list(fwdsens2[i])+list(adjsens2[i]) for i in range(ndir)]))
random.seed(1)
vf2_in = []
for i in range(vf2.n_in()):
vf2_in.append(DM(vf2.sparsity_in(i),random.random(vf2.nnz_in(i))))
vf2_out = vf2.call(vf2_in)
self.check_codegen(vf2,inputs=vf2_in)
storagekey = (spmod,spmod2)
if not(storagekey in storage2):
storage2[storagekey] = []
storage2[storagekey].append(vf2_out)
# Remainder of eval testing
for store,order in [(storage,"first-order"),(storage2,"second-order")]:
for stk,st in list(store.items()):
for i in range(len(st)-1):
for k,(a,b) in enumerate(zip(st[0],st[i+1])):
if b.numel()==0 and sparsify(a).nnz()==0: continue
if a.numel()==0 and sparsify(b).nnz()==0: continue
self.checkarray(sparsify(a),sparsify(b),("%s, output(%d)" % (order,k)))
for expand in [False, True]:
# jacobian()
for mode in ["forward","reverse"]:
ind = 0 if mode=='forward' else 1
f = fun_ad[ind] if expand else funsx_ad[ind]
Jf=f.jacobian_old(0,0)
Jf_out = Jf.call(values)
self.check_codegen(Jf,inputs=values)
self.checkarray(Jf_out[0],J_)
self.checkarray(DM.ones(Jf.sparsity_out(0)),DM.ones(J_.sparsity()),str(out)+str(mode))
self.checkarray(DM.ones(f.sparsity_jac(0, 0)),DM.ones(J_.sparsity()))
# Scalarized
if out.is_empty(): continue
s_i = out.sparsity().row()[0]
s_j = out.sparsity().get_col()[0]
s_k = s_i*out.size2()+s_j
H_ = None
for expand in [False, True]:
for mode in ["forward","reverse"]:
w = 0 if mode=='forward' else 1
f = Function("fun", inputs,[out[s_i,s_j],jac[s_k,:].T], {'ad_weight':w, 'ad_weight_sp':w})
if expand: f=f.expand('expand_'+f.name())
f_out = f.call(values)
J_ = f_out[1]
Hf=f.hessian_old(0, 0)
Hf_out = Hf.call(values)
self.check_codegen(Hf,inputs=values)
if H_ is None:
H_ = Hf_out[0]
self.checkarray(Hf_out[0],H_,failmessage=("mode: %s" % mode))
p_mean = pl.mean(p_test)
p_std = pl.std(p_test, ddof=0)
pe_test.compute_covariance_matrix()
pe_test.print_estimation_results()
# Generate report
print("\np_mean = " + str(ca.DMatrix(p_mean)))
print("phat_last_exp = " + str(ca.DMatrix(pe_test.estimated_parameters)))
print("\np_sd = " + str(ca.DMatrix(p_std)))
print("sd_from_covmat = " + str(ca.diag(ca.sqrt(pe_test.covariance_matrix))))
print("beta = " + str(pe_test.beta))
print("\ndelta_abs_sd = " + str(ca.fabs(ca.DMatrix(p_std) - \
ca.diag(ca.sqrt(pe_test.covariance_matrix)))))
print("delta_rel_sd = " + str(ca.fabs(ca.DMatrix(p_std) - \
ca.diag(ca.sqrt(pe_test.covariance_matrix))) / ca.DMatrix(p_std)))
fname = os.path.basename(__file__)[:-3] + ".rst"
report = open(fname, "w")
report.write( \
'''Concept test: covariance matrix computation
===========================================
def compute_covariance_matrix(self):
r'''
This function computes the covariance matrix of the estimated
parameters from the inverse of the KKT matrix for the
parameter estimation problem. This allows then for statements on the
quality of the values of the estimated parameters.
For efficiency, only the inverse of the relevant part of the matrix
is computed using the Schur complement.
A more detailed description of this function will follow in future
versions.
'''
intro.pecas_intro()
print('\n' + 20 * '-' + \
' PECas covariance matrix computation ' + 21 * '-')
print('''
Computing the covariance matrix for the estimated parameters,
this might take some time ...
''')
self.tstart_cov_computation = time.time()
try:
N1 = ca.MX(self.Vars.shape[0] - self.w.shape[0], \
self.w.shape[0])
N2 = ca.MX(self.Vars.shape[0] - self.w.shape[0], \
self.Vars.shape[0] - self.w.shape[0])
hess = ca.blockcat([[N2, N1], [N1.T, ca.diag(self.w)],])
# hess = hess + 1e-10 * ca.diag(self.Vars)
# J2 can be re-used from parameter estimation, right?
J2 = ca.jacobian(self.g, self.Vars)
kkt = ca.blockcat( \
[[hess, \
J2.T], \
[J2, \
ca.MX(self.g.size1(), self.g.size1())]] \
)
B1 = kkt[:self.pesetup.np, :self.pesetup.np]
E = kkt[self.pesetup.np:, :self.pesetup.np]
D = kkt[self.pesetup.np:, self.pesetup.np:]
Dinv = ca.solve(D, E, "csparse")
F11 = B1 - ca.mul([E.T, Dinv])
self.fbeta = ca.MXFunction("fbeta", [self.Vars],
[ca.mul([self.R.T, self.R]) / \
(self.yN.size + self.g.size1() - self.Vars.size())])
[self.beta] = self.fbeta([self.Varshat])
self.fcovp = ca.MXFunction("fcovp", [self.Vars], \
[self.beta * ca.solve(F11, ca.MX.eye(F11.size1()))])
[self.Covp] = self.fcovp([self.Varshat])
print( \
'''Covariance matrix computation finished, run show_results() to visualize.''')
except AttributeError as err:
errmsg = '''
You must execute run_parameter_estimation() first before the covariance
matrix for the estimated parameters can be computed.
'''
raise AttributeError(errmsg)
finally:
self.tend_cov_computation = time.time()
self.duration_cov_computation = self.tend_cov_computation - \
self.tstart_cov_computation
def test_eig_symbolic(self):
x = SX.sym("x",2,2)
f = Function("f", [x],[eig_symbolic(x)])
f_in = [0]*f.n_in();f_in[0]=DM([[2,0.1],[0.3,0.7]])
f_out = f.call(f_in)
self.checkarray(f_out[0],DM([0.67732,2.02268]),digits=5)
x = SX.sym("x",2)
f = Function("f", [x],[eig_symbolic(c.diag(x))])
f_in = [0]*f.n_in();f_in[0]=DM([3,7])
f_out = f.call(f_in)
self.checkarray(f_out[0],f_in[0])
x = SX.sym("x",5)
f = Function("f", [x],[eig_symbolic(c.diag(x))])
f_in = [0]*f.n_in();f_in[0]=DM([3,7,2,1,6])
f_out = f.call(f_in)
self.checkarray(f_out[0],f_in[0])
x = SX.sym("x",2,2)
y = SX.sym("y",2)
f = Function("f", [x,y],[eig_symbolic(diagcat(*[x,c.diag(y)]))])
f_in = [0]*f.n_in();f_in[0]=DM([[2,0.1],[0.3,0.7]])
f_in[1]=[3,7]
f_out = f.call(f_in)
self.checkarray(f_out[0],DM([0.67732,2.02268,3,7]),digits=5)
x = SX.sym("x",3,3)
x[2,0] = 0
x[1,0] = 0
x = sparsify(x)
e = eig_symbolic(x)
f = Function("f", [x],[e])
f_in = [0]*f.n_in();f_in[0]=DM(f.sparsity_in(0),list(range(1,8)))
f_in[0].print_dense()
f_out = f.call(f_in)
self.checkarray(f_out[0],DM([1,-0.29150,10.29150]),digits=5)
x = SX.sym("x",3,3)
x[2,0] = 0
x[1,0] = 0
x[2,1] = 0
x = sparsify(x)
e = eig_symbolic(x)
f = Function("f", [x],[e])
f_in = [0]*f.n_in();f_in[0]=DM(f.sparsity_in(0),list(range(1,7)))
f_in[0].print_dense()
f_out = f.call(f_in)
self.checkarray(f_out[0],DM([1,3,6]),digits=5)
x = SX.sym("x",Sparsity.upper(5))
f = Function("f", [x],[eig_symbolic(x)])
fin = DM(x.sparsity(),0)
fin[Sparsity.diag(5)] = c.diag(list(range(5)))
self.checkarray(f(fin), DM(list(range(5))))
L.input().set(1)
L.evaluate()
Lend = DM(L.output()) # Le at the end of the control interval
dLm = numSample1D(dL,DM(tau_root).T) # d-by-d
resf = SXFunction(customIO(t=model.t, x=states, dx= dstates, u=controls, p=model.p,w=model.w),[model.res_w])
resf.init()
def linear_combination(v,w):
return sum([i*j for i,j in zip(v,w)])
### LPDE -- start
# Disturbances have a standard deviation of 5% with respect to their nominal value
Sigma = c.diag(vertcat([scaling_dist,scaling_states])**2)*analysis.noiserel
K = ssym("K",controls.size,states.size)
u = ssym("u",nu)
x = ssym("x",ns)
zj = [ ssym("z",ns) for i in range(d)]
z = vertcat(zj)
w = ssym("w",model.w.size)
G = zj[0]-x
delta = ssym("delta")
for j in range(1,d):
def test_eig_symbolic(self):
x = SX.sym("x",2,2)
f = SXFunction([x],[eig_symbolic(x)])
f.init()
f.setInput(DMatrix([[2,0.1],[0.3,0.7]]))
f.evaluate()
self.checkarray(f.output(),DMatrix([0.67732,2.02268]),digits=5)
x = SX.sym("x",2)
f = SXFunction([x],[eig_symbolic(c.diag(x))])
f.init()
f.setInput([3,7])
f.evaluate()
self.checkarray(f.output(),f.input())
x = SX.sym("x",5)
f = SXFunction([x],[eig_symbolic(c.diag(x))])
f.init()
f.setInput([3,7,2,1,6])
f.evaluate()
self.checkarray(f.output(),f.input())
x = SX.sym("x",2,2)
y = SX.sym("y",2)
f = SXFunction([x,y],[eig_symbolic(blkdiag([x,c.diag(y)]))])
f.init()
f.setInput(DMatrix([[2,0.1],[0.3,0.7]]),0)
f.setInput([3,7],1)
f.evaluate()
self.checkarray(f.output(),DMatrix([0.67732,2.02268,3,7]),digits=5)
x = SX.sym("x",3,3)
x[2,0] = 0
x[1,0] = 0
x = sparse(x)
e = eig_symbolic(x)
f = SXFunction([x],[e])
f.init()
f.setInput(range(1,8))
f.input().printDense()
f.evaluate()
self.checkarray(f.output(),DMatrix([1,-0.29150,10.29150]),digits=5)
x = SX.sym("x",3,3)
x[2,0] = 0
x[1,0] = 0
x[2,1] = 0
x = sparse(x)
e = eig_symbolic(x)
f = SXFunction([x],[e])
f.init()
f.setInput(range(1,7))
f.input().printDense()
f.evaluate()
self.checkarray(f.output(),DMatrix([1,3,6]),digits=5)
x = SX.sym("x",Sparsity.triu(5))
f = SXFunction([x],[eig_symbolic(x)])
f.init()
f.setInput(6)
f.input()[Sparsity.diag(5)] = c.diag(range(5))
f.evaluate()
self.checkarray(f.output(),DMatrix(range(5)))
def test_MX(self):
x = MX.sym("x",2)
y = MX.sym("y",2,2)
f1 = MXFunction([x,y],[x+y[0,0],mul(y,x)])
f1.init()
f2 = MXFunction([x,y],[mul(MX.zeros(0,2),x)])
f2.init()
f3 = MXFunction([x,y],[MX.zeros(0,0),mul(y,x)])
f3.init()
f4 = MXFunction([x,y],[MX.zeros(0,2),mul(y,x)])
f4.init()
ndir = 2
in1 = [x,y]
v1 = [DMatrix([1.1,1.3]),DMatrix([[0.7,1.5],[2.1,0.9]])]
w=x[:]
w[1]*=2
w2=x[:]
w2[1]*=x[0]
ww=x[:]
ww[[0,1]]*=x
wwf=x[:]
wwf[[1,0]]*=x
wwr=x[:]
wwr[[0,0,1,1]]*=2
yy=y[:,:]
yy[:,0] = x
yy2=y[:,:]
yy2[:,0] = x**2
yyy=y[:,:]
yyy[[1,0],0] = x
yyy2=y[:,:]
yyy2[[1,0],0] = x**2
def remove_first(x):
ret = DMatrix(x)
if ret.numel()>0:
ret[0,0] = DMatrix.sparse(1,1)
return ret
else:
return ret
def remove_last(x):
ret = DMatrix(x)
if ret.size()>0:
ret[ret.sparsity().row()[-1],ret.sparsity().getCol()[-1]] = DMatrix.sparse(1,1)
return ret
else:
return x
spmods = [lambda x: x , remove_first, remove_last]
# TODO: sparse seeding
for inputs,values,out, jac in [
(in1,v1,x,DMatrix.eye(2)),
(in1,v1,x.T,DMatrix.eye(2)),
(in1,v1,x**2,2*c.diag(x)),
(in1,v1,(x**2).attachAssert(True),2*c.diag(x)),
(in1,v1,(x**2).T,2*c.diag(x)),
(in1,v1,c.reshape(x,(1,2)),DMatrix.eye(2)),
(in1,v1,c.reshape(x**2,(1,2)),2*c.diag(x)),
(in1,v1,x+y.nz[0],DMatrix.eye(2)),
(in1,v1,x+y[0,0],DMatrix.eye(2)),
(in1,v1,x+x,2*DMatrix.eye(2)),
(in1,v1,x**2+x,2*c.diag(x)+DMatrix.eye(2)),
(in1,v1,x*x,2*c.diag(x)),
(in1,v1,x*y.nz[0],DMatrix.eye(2)*y.nz[0]),
(in1,v1,x*y[0,0],DMatrix.eye(2)*y[0,0]),
(in1,v1,x[0],DMatrix.eye(2)[0,:]),
(in1,v1,(x**2)[0],horzcat([2*x[0],MX.sparse(1,1)])),
(in1,v1,x[0]+x[1],DMatrix.ones(1,2)),
(in1,v1,vertcat([x[1],x[0]]),sparse(DMatrix([[0,1],[1,0]]))),
(in1,v1,vertsplit(x,[0,1,2])[1],sparse(DMatrix([[0,1]]))),
(in1,v1,vertcat([x[1]**2,x[0]**2]),blockcat([[MX.sparse(1,1),2*x[1]],[2*x[0],MX.sparse(1,1)]])),
(in1,v1,vertsplit(x**2,[0,1,2])[1],blockcat([[MX.sparse(1,1),2*x[1]]])),
(in1,v1,vertsplit(x**2,[0,1,2])[1]**3,blockcat([[MX.sparse(1,1),6*x[1]**5]])),
(in1,v1,horzcat([x[1],x[0]]).T,sparse(DMatrix([[0,1],[1,0]]))),
(in1,v1,horzcat([x[1]**2,x[0]**2]).T,blockcat([[MX.sparse(1,1),2*x[1]],[2*x[0],MX.sparse(1,1)]])),
(in1,v1,diagcat([x[1]**2,y,x[0]**2]),
blockcat( [[MX.sparse(1,1),2*x[1]]] + ([[MX.sparse(1,1),MX.sparse(1,1)]]*14) + [[2*x[0],MX.sparse(1,1)]] )
),
(in1,v1,horzcat([x[1]**2,x[0]**2]).T,blockcat([[MX.sparse(1,1),2*x[1]],[2*x[0],MX.sparse(1,1)]])),
(in1,v1,x[[0,1]],sparse(DMatrix([[1,0],[0,1]]))),
(in1,v1,(x**2)[[0,1]],2*c.diag(x)),
(in1,v1,x[[0,0,1,1]],sparse(DMatrix([[1,0],[1,0],[0,1],[0,1]]))),
(in1,v1,(x**2)[[0,0,1,1]],blockcat([[2*x[0],MX.sparse(1,1)],[2*x[0],MX.sparse(1,1)],[MX.sparse(1,1),2*x[1]],[MX.sparse(1,1),2*x[1]]])),
(in1,v1,wwr,sparse(DMatrix([[2,0],[0,2]]))),
(in1,v1,x[[1,0]],sparse(DMatrix([[0,1],[1,0]]))),
(in1,v1,x[[1,0],0],sparse(DMatrix([[0,1],[1,0]]))),
(in1,v1,w,sparse(DMatrix([[1,0],[0,2]]))),
(in1,v1,w2,blockcat([[1,MX.sparse(1,1)],[x[1],x[0]]])),
(in1,v1,ww,2*c.diag(x)),
(in1,v1,wwf,vertcat([x[[1,0]].T,x[[1,0]].T])),
(in1,v1,yy[:,0],DMatrix.eye(2)),
(in1,v1,yy2[:,0],2*c.diag(x)),
(in1,v1,yyy[:,0],sparse(DMatrix([[0,1],[1,0]]))),
(in1,v1,mul(y,x),y),
(in1,v1,mul(x.T,y.T),y),
(in1,v1,mul(y,x,DMatrix.zeros(Sparsity.triplet(2,1,[1],[0]))),y[Sparsity.triplet(2,2,[1,1],[0,1])]),
(in1,v1,mul(x.T,y.T,DMatrix.zeros(Sparsity.triplet(2,1,[1],[0]).T)),y[Sparsity.triplet(2,2,[1,1],[0,1])]),
(in1,v1,mul(y[Sparsity.triplet(2,2,[0,1,1],[0,0,1])],x),y[Sparsity.triplet(2,2,[0,1,1],[0,0,1])]),
(in1,v1,mul(x.T,y[Sparsity.triplet(2,2,[0,1,1],[0,0,1])].T),y[Sparsity.triplet(2,2,[0,1,1],[0,0,1])]),
(in1,v1,mul(y,x**2),y*2*vertcat([x.T,x.T])),
(in1,v1,sin(x),c.diag(cos(x))),
(in1,v1,sin(x**2),c.diag(cos(x**2)*2*x)),
(in1,v1,x*y[:,0],c.diag(y[:,0])),
(in1,v1,x*y.nz[[0,1]],c.diag(y.nz[[0,1]])),
(in1,v1,x*y.nz[[1,0]],c.diag(y.nz[[1,0]])),
(in1,v1,x*y[[0,1],0],c.diag(y[[0,1],0])),
(in1,v1,x*y[[1,0],0],c.diag(y[[1,0],0])),
(in1,v1,inner_prod(x,x),(2*x).T),
(in1,v1,inner_prod(x**2,x),(3*x**2).T),
#(in1,v1,c.det(horzcat([x,DMatrix([1,2])])),DMatrix([-1,2])), not implemented
(in1,v1,f1.call(in1)[1],y),
(in1,v1,f1.call([x**2,y])[1],y*2*vertcat([x.T,x.T])),
(in1,v1,f2.call(in1)[0],DMatrix.zeros(0,2)),
(in1,v1,f2.call([x**2,y])[0],DMatrix.zeros(0,2)),
(in1,v1,f3.call(in1)[0],DMatrix.zeros(0,2)),
(in1,v1,f3.call([x**2,y])[0],DMatrix.zeros(0,2)),
(in1,v1,f4.call(in1)[0],DMatrix.zeros(0,2)),
(in1,v1,f4.call([x**2,y])[0],DMatrix.zeros(0,2)),
#(in1,v1,f1.call([x**2,[]])[1],DMatrix.zeros(2,2)),
#(in1,v1,f1.call([[],y])[1],DMatrix.zeros(2,2)),
(in1,v1,vertcat([x,DMatrix.sparse(0,1)]),DMatrix.eye(2)),
(in1,v1,(x**2).setSparse(sparse(DMatrix([0,1])).sparsity()),blockcat([[MX.sparse(1,1),MX.sparse(1,1)],[MX.sparse(1,1),2*x[1]]])),
(in1,v1,c.inner_prod(x,y[:,0]),y[:,0].T),
(in1,v1,x.nz[IMatrix([[1,0]])]*y.nz[IMatrix([[0,2]])],blockcat([[MX.sparse(1,1),y.nz[0]],[y.nz[2],MX.sparse(1,1)]])),
(in1,v1,x.nz[c.diag([1,0])]*y.nz[c.diag([0,2])],blockcat([[MX.sparse(1,1),y.nz[0]],[MX.sparse(1,1),MX.sparse(1,1)],[MX.sparse(1,1),MX.sparse(1,1)],[y.nz[2],MX.sparse(1,1)]])),
]:
print out
fun = MXFunction(inputs,[out,jac])
fun.init()
funsx = fun.expand()
funsx.init()
for i,v in enumerate(values):
fun.setInput(v,i)
funsx.setInput(v,i)
fun.evaluate()
funsx.evaluate()
self.checkarray(fun.getOutput(0),funsx.getOutput(0))
self.checkarray(fun.getOutput(1),funsx.getOutput(1))
J_ = fun.getOutput(1)
def vec(l):
ret = []
for i in l:
ret.extend(i)
return ret
storage2 = {}
storage = {}
vf_mx = None
for f in [fun,fun.expand()]:
f.init()
d = f.derivative(ndir,ndir)
d.init()
num_in = f.getNumInputs()
num_out = f.getNumOutputs()
"""# Fwd and Adjoint AD
for i,v in enumerate(values):
f.setInput(v,i)
d.setInput(v,i)
for d in range(ndir):
f.setInput(DMatrix(inputs[0].sparsity(),random.random(inputs[0].size())),num_in+d*num_in + d)
f.setAdjSeed(DMatrix(out.sparsity(),random.random(out.size())),num_in+d*num_in + 0)
f.setFwdSeed(0,1,d)
f.setAdjSeed(0,1,d)
f.evaluate()
for d in range(ndir):
seed = array(f.getFwdSeed(0,d)).ravel()
sens = array(f.getFwdSens(0,d)).ravel()
self.checkarray(sens,mul(J_,seed),"Fwd %d %s" % (d,str(type(f))))
seed = array(f.getAdjSeed(0,d)).ravel()
sens = array(f.getAdjSens(0,d)).ravel()
self.checkarray(sens,mul(J_.T,seed),"Adj %d" %d)
"""
# evalThings
for sym, Function in [(MX.sym,MXFunction),(SX.sym,SXFunction)]:
if isinstance(f, MXFunction) and Function is SXFunction: continue
if isinstance(f, SXFunction) and Function is MXFunction: continue
# dense
for spmod,spmod2 in itertools.product(spmods,repeat=2):
fseeds = [[sym("f",spmod(f.getInput(i)).sparsity()) for i in range(f.getNumInputs())] for d in range(ndir)]
aseeds = [[sym("a",spmod2(f.getOutput(i)).sparsity()) for i in range(f.getNumOutputs())] for d in range(ndir)]
inputss = [sym("i",f.input(i).sparsity()) for i in range(f.getNumInputs())]
with internalAPI():
res,fwdsens,adjsens = f.callDerivative(inputss,fseeds,aseeds,True)
fseed = [DMatrix(fseeds[d][0].sparsity(),random.random(fseeds[d][0].size())) for d in range(ndir) ]
aseed = [DMatrix(aseeds[d][0].sparsity(),random.random(aseeds[d][0].size())) for d in range(ndir) ]
vf = Function(inputss+vec([fseeds[i]+aseeds[i] for i in range(ndir)]),list(res) + vec([list(fwdsens[i])+list(adjsens[i]) for i in range(ndir)]))
vf.init()
for i,v in enumerate(values):
vf.setInput(v,i)
offset = len(inputss)
for d in range(ndir):
vf.setInput(fseed[d],offset+0)
for i in range(len(values)-1):
vf.setInput(0,offset+i+1)
offset += len(inputss)
vf.setInput(aseed[d],offset+0)
vf.setInput(0,offset+1)
offset+=2
assert(offset==vf.getNumInputs())
vf.evaluate()
offset = len(res)
for d in range(ndir):
seed = array(fseed[d]).ravel()
sens = array(vf.getOutput(offset+0)).ravel()
offset+=len(inputss)
self.checkarray(sens,mul(J_,seed),"eval Fwd %d %s" % (d,str(type(f))+str(sym)))
seed = array(aseed[d]).ravel()
sens = array(vf.getOutput(offset+0)).ravel()
offset+=len(inputss)
self.checkarray(sens,mul(J_.T,seed),"eval Adj %d %s" % (d,str([vf.getOutput(i) for i in range(vf.getNumOutputs())])))
assert(offset==vf.getNumOutputs())
# Complete random seeding
random.seed(1)
for i in range(vf.getNumInputs()):
vf.setInput(DMatrix(vf.input(i).sparsity(),random.random(vf.input(i).size())),i)
vf.evaluate()
storagekey = (spmod,spmod2)
if not(storagekey in storage):
storage[storagekey] = []
storage[storagekey].append([vf.getOutput(i) for i in range(vf.getNumOutputs())])
# Added to make sure that the same seeds are used for SX and MX
if Function is MXFunction:
vf_mx = vf
# Second order sensitivities
for sym2, Function2 in [(MX.sym,MXFunction),(SX.sym,SXFunction)]:
if isinstance(vf, MXFunction) and Function2 is SXFunction: continue
if isinstance(vf, SXFunction) and Function2 is MXFunction: continue
for spmod_2,spmod2_2 in itertools.product(spmods,repeat=2):
fseeds2 = [[sym2("f",vf_mx.input(i).sparsity()) for i in range(vf.getNumInputs())] for d in range(ndir)]
aseeds2 = [[sym2("a",vf_mx.output(i).sparsity()) for i in range(vf.getNumOutputs()) ] for d in range(ndir)]
inputss2 = [sym2("i",vf_mx.input(i).sparsity()) for i in range(vf.getNumInputs())]
with internalAPI():
res2,fwdsens2,adjsens2 = vf.callDerivative(inputss2,fseeds2,aseeds2,True)
vf2 = Function2(inputss2+vec([fseeds2[i]+aseeds2[i] for i in range(ndir)]),list(res2) + vec([list(fwdsens2[i])+list(adjsens2[i]) for i in range(ndir)]))
vf2.init()
random.seed(1)
for i in range(vf2.getNumInputs()):
vf2.setInput(DMatrix(vf2.input(i).sparsity(),random.random(vf2.input(i).size())),i)
vf2.evaluate()
storagekey = (spmod,spmod2)
if not(storagekey in storage2):
storage2[storagekey] = []
storage2[storagekey].append([vf2.getOutput(i) for i in range(vf2.getNumOutputs())])
# Remainder of eval testing
for store,order in [(storage,"first-order"),(storage2,"second-order")]:
for stk,st in store.items():
for i in range(len(st)-1):
for k,(a,b) in enumerate(zip(st[0],st[i+1])):
if b.numel()==0 and sparse(a).size()==0: continue
if a.numel()==0 and sparse(b).size()==0: continue
self.checkarray(sparse(a),sparse(b),("%s, output(%d)" % (order,k))+str(vf2.getInput(0)))
for f in [fun.expand(),fun]:
# jacobian()
for mode in ["forward","reverse"]:
f.setOption("ad_mode",mode)
f.init()
Jf=f.jacobian(0,0)
Jf.init()
for i,v in enumerate(values):
Jf.setInput(v,i)
Jf.evaluate()
self.checkarray(Jf.getOutput(),J_)
self.checkarray(DMatrix(Jf.output().sparsity(),1),DMatrix(J_.sparsity(),1),str(out)+str(mode))
self.checkarray(DMatrix(f.jacSparsity(),1),DMatrix(J_.sparsity(),1))
# Scalarized
if out.isEmpty(): continue
s_i = out.sparsity().row()[0]
s_j = out.sparsity().getCol()[0]
s_k = s_i*out.size2()+s_j
fun = MXFunction(inputs,[out[s_i,s_j],jac[s_k,:].T])
fun.init()
for i,v in enumerate(values):
fun.setInput(v,i)
fun.evaluate()
J_ = fun.getOutput(1)
for f in [fun,fun.expand()]:
# gradient()
for mode in ["forward","reverse"]:
f.setOption("ad_mode",mode)
f.init()
Gf=f.gradient(0,0)
Gf.init()
for i,v in enumerate(values):
Gf.setInput(v,i)
Gf.evaluate()
self.checkarray(Gf.getOutput(),J_,failmessage=("mode: %s" % mode))
#self.checkarray(DMatrix(Gf.output().sparsity(),1),DMatrix(J_.sparsity(),1),str(mode)+str(out)+str(type(fun)))
H_ = None
for f in [fun,fun.expand()]:
# hessian()
for mode in ["forward","reverse"]:
f.setOption("ad_mode",mode)
f.init()
Hf=f.hessian(0,0)
Hf.init()
for i,v in enumerate(values):
Hf.setInput(v,i)
Hf.evaluate()
if H_ is None:
H_ = Hf.getOutput()
self.checkarray(Hf.getOutput(),H_,failmessage=("mode: %s" % mode))
def diag(inputobj):
return ca.diag(inputobj)
def __init__(self, inertial_frame_id='world'):
Vehicle.__init__(self, inertial_frame_id)
# Declaring state variables
## Generalized position vector
self.eta = casadi.SX.sym('eta', 6)
## Generalized velocity vector
self.nu = casadi.SX.sym('nu', 6)
# Build the Coriolis matrix
self.CMatrix = casadi.SX.zeros(6, 6)
S_12 = - cross_product_operator(
casadi.mtimes(self._Mtotal[0:3, 0:3], self.nu[0:3]) +
casadi.mtimes(self._Mtotal[0:3, 3:6], self.nu[3:6]))
S_22 = - cross_product_operator(
casadi.mtimes(self._Mtotal[3:6, 0:3], self.nu[0:3]) +
casadi.mtimes(self._Mtotal[3:6, 3:6], self.nu[3:6]))
self.CMatrix[0:3, 3:6] = S_12
self.CMatrix[3:6, 0:3] = S_12
self.CMatrix[3:6, 3:6] = S_22
# Build the damping matrix (linear and nonlinear elements)
self.DMatrix = - casadi.diag(self._linear_damping)
self.DMatrix -= casadi.diag(self._linear_damping_forward_speed)
self.DMatrix -= casadi.diag(self._quad_damping * self.nu)
# Build the restoring forces vectors wrt the BODY frame
Rx = np.array([[1, 0, 0],
[0, casadi.cos(self.eta[3]), -1 * casadi.sin(self.eta[3])],
[0, casadi.sin(self.eta[3]), casadi.cos(self.eta[3])]])
Ry = np.array([[casadi.cos(self.eta[4]), 0, casadi.sin(self.eta[4])],
[0, 1, 0],
[-1 * casadi.sin(self.eta[4]), 0, casadi.cos(self.eta[4])]])
Rz = np.array([[casadi.cos(self.eta[5]), -1 * casadi.sin(self.eta[5]), 0],
[casadi.sin(self.eta[5]), casadi.cos(self.eta[5]), 0],
[0, 0, 1]])
R_n_to_b = casadi.transpose(casadi.mtimes(Rz, casadi.mtimes(Ry, Rx)))
if inertial_frame_id == 'world_ned':
Fg = casadi.SX([0, 0, -self.mass * self.gravity])
Fb = casadi.SX([0, 0, self.volume * self.gravity * self.density])
else:
Fg = casadi.SX([0, 0, self.mass * self.gravity])
Fb = casadi.SX([0, 0, -self.volume * self.gravity * self.density])
self.gVec = casadi.SX.zeros(6)
self.gVec[0:3] = -1 * casadi.mtimes(R_n_to_b, Fg + Fb)
self.gVec[3:6] = -1 * casadi.mtimes(
R_n_to_b, casadi.cross(self._cog, Fg) + casadi.cross(self._cob, Fb))
# Build Jacobian
T = 1 / casadi.cos(self.eta[4]) * np.array(
[[0, casadi.sin(self.eta[3]) * casadi.sin(self.eta[4]), casadi.cos(self.eta[3]) * casadi.sin(self.eta[4])],
[0, casadi.cos(self.eta[3]) * casadi.cos(self.eta[4]), -casadi.cos(self.eta[4]) * casadi.sin(self.eta[3])],
[0, casadi.sin(self.eta[3]), casadi.cos(self.eta[3])]])
self.eta_dot = casadi.vertcat(
casadi.mtimes(casadi.transpose(R_n_to_b), self.nu[0:3]),
casadi.mtimes(T, self.nu[3::]))
self.u = casadi.SX.sym('u', 6)
self.nu_dot = casadi.solve(
self._Mtotal,
self.u - casadi.mtimes(self.CMatrix, self.nu) - casadi.mtimes(self.DMatrix, self.nu) - self.gVec)
for j, e in enumerate(p_true):
p_mean.append(pl.mean([k[j] for k in p_test]))
p_std.append(pl.std([k[j] for k in p_test], ddof = 0))
lsqpe_test.compute_covariance_matrix()
# Generate report
print("\np_mean = " + str(ca.DMatrix(p_mean)))
print("phat_last_exp = " + str(ca.DMatrix(lsqpe_test.phat)))
print("\np_sd = " + str(ca.DMatrix(p_std)))
print("sd_from_covmat = " + str(ca.diag(ca.sqrt(lsqpe_test.Covp))))
print("beta = " + str(lsqpe_test.beta))
print("\ndelta_abs_sd = " + str(ca.fabs(ca.DMatrix(p_std) - \
ca.diag(ca.sqrt(lsqpe_test.Covp)))))
print("delta_rel_sd = " + str(ca.fabs(ca.DMatrix(p_std) - \
ca.diag(ca.sqrt(lsqpe_test.Covp))) / ca.DMatrix(p_std)))
fname = os.path.basename(__file__)[:-3] + ".rst"
report = open(fname, "w")
report.write( \
'''Concept test: covariance matrix computation
===========================================
V = ca.vertcat([P, EPS_U, X0])
x_end = X0
obj = [x_end - ydata[0,:].T]
for k in range(int(N)):
x_end = rk4(x0 = x_end, p = ca.vertcat([udata[k], EPS_U[k], P]))["xf"]
obj.append(x_end - ydata_noise[k+1, :].T)
r = ca.vertcat([ca.vertcat(obj), EPS_U])
wv = (1.0 / sigma_y**2) * pl.ones(ydata.shape)
weps_u = (1.0 / sigma_u**2) * pl.ones(udata.shape)
Sigma_y_inv = ca.diag(ca.vec(wv))
Sigma_u_inv = ca.diag(weps_u)
Sigma = ca.blockcat(Sigma_y_inv, ca.DMatrix(pl.zeros((Sigma_y_inv.shape[0], Sigma_u_inv.shape[1]))),\
ca.DMatrix(pl.zeros((Sigma_u_inv.shape[0], Sigma_y_inv.shape[1]))), Sigma_u_inv)
nlp = ca.MXFunction("nlp", ca.nlpIn(x = V), ca.nlpOut(f = ca.mul([r.T, Sigma, r])))
nlpsolver = ca.NlpSolver("nlpsolver", "ipopt", nlp)
V0 = ca.vertcat([
pl.ones(3), \
pl.zeros(N), \
ydata_noise[0,:].T
p_std = []
for j, e in enumerate(p_true):
p_mean.append(pl.mean([k[j] for k in p_test]))
p_std.append(pl.std([k[j] for k in p_test], ddof=0))
pe_test.compute_covariance_matrix()
# Generate report
print("\np_mean = " + str(ca.DM(p_mean)))
print("phat_last_exp = " + str(ca.DM(pe_test.estimated_parameters)))
print("\np_sd = " + str(ca.DM(p_std)))
print("sd_from_covmat = " + str(ca.diag(ca.sqrt(pe_test.covariance_matrix))))
print("beta = " + str(pe_test.beta))
print("\ndelta_abs_sd = " + str(ca.fabs(ca.DM(p_std) - \
ca.diag(ca.sqrt(pe_test.covariance_matrix)))))
print("delta_rel_sd = " + str(ca.fabs(ca.DM(p_std) - \
ca.diag(ca.sqrt(pe_test.covariance_matrix))) / ca.DM(p_std)))
fname = os.path.basename(__file__)[:-3] + ".rst"
report = open(fname, "w")
report.write( \
'''Concept test: covariance matrix computation
===========================================
Simulate system. Then: add gaussian noise N~(0, sigma^2), estimate,
def __init__(self,NR=4,debug=False,quatnorm=False):
"""
Keyword arguments:
NR -- the number of rotors
debug -- wether to print out debug info
quatnorm -- add the quaternion norm to the DAE rhs
"""
# ----------- system states and their derivatives ----
pos = struct_symSX(["x","y","z"]) # rigid body centre of mass position [m] {0}
v = struct_symSX(["vx","vy","vz"]) # rigid body centre of mass position velocity [m/s] {0}
NR = 4 # Number of rotors
states = struct_symSX([
entry("p",struct=pos),
entry("v",struct=v),
entry("q",shape=4), # quaternions {0} -> {1}
entry("w",shape=3), # rigid body angular velocity w_101 [rad/s] {1}
entry("r",shape=NR) # spin speed of rotor, wrt to platform. [rad/s] Should be positive!
# The signs are such that positive means lift generating, regardless of spin direction.
])
pos, v, q, w, r = states[...]
# ------------------------------------------------
dist = struct_symSX([
entry("Faer",shape=NR), # Disturbance on aerodynamic forcing [N]
entry("Caer",shape=NR) # Disturbance on aerodynamic torques [Nm]
])
# ----------------- Controls ---------------------
controls = struct_symSX([
entry("CR",shape=NR) # [Nm]
# Torques of the motors that drive the rotors, acting from platform on propeller
# The torque signs are always positive when putting energy in the propellor,
# regardless of spin direction.
#
])
CR = controls["CR"]
# ------------------------------------------------
# ---------------- Temporary symbols --------------
F = ssym("F",3) # Forces acting on the platform in {1} [N]
C = ssym("C",3) # Torques acting on the platform in {1} [Nm]
rotors_Faer = [ssym("Faer_%d" %i,3,1) for i in range(NR)] # Placeholder for aerodynamic force acting on propeller {1} [N]
rotors_Caer = [ssym("Caer_%d" %i,3,1) for i in range(NR)] # Placeholder for aerodynamic torques acting on propeller {1} [Nm]
# ---------------------------------------------------
# ----------------- Parameters ---------------------
rotor_model = struct_symSX([
"c", # c Cord length [m]
"R", # R Radius of propeller [m]
"CL_alpha", # CL_alpha Lift coefficient [-]
"alpha_0", # alpha_0
"CD_alpha", # CD_alpha Drag coefficient [-]
"CD_i", # CD_i Induced drag coefficient [-]
])
p = struct_symSX([
entry("rotors_model",repeat=NR,struct=rotor_model), # Parameters that describe the rotor model
entry("rotors_I",repeat=NR,shape=sp_diag(3)), # Inertias of rotors [kg.m^2]
entry("rotors_spin",repeat=NR), # Direction of spin from each rotor. 1 means rotation around positive z.
entry("rotors_p",repeat=NR,shape=3), # position of rotors in {1} [m],
entry("I",sym=casadi.diag(ssym("[Ix,Iy,Iz]"))), # Inertia of rigid body [kg.m^2]
"m", # Mass of the whole system [kg]
"g", # gravity [m/s^2]
"rho", # Air density [kg/m^3]
])
I,m,g,rho = p[["I","m","g","rho"]]
# --------------------------------------------------
# ----------------- Parameters fillin's ---------------------
p_ = p()
p_["rotors_spin"] = [1,-1,1,-1]
p_["rotors_model",:,{}] = { "c": 0.01, "R" : 0.127, "CL_alpha": 6.0, "alpha_0": 0.15, "CD_alpha": 0.02, "CD_i": 0.05} # c Cord length [m]
p_["m"] = 0.5 # [kg]
p_["g"] = 9.81 # [N/kg]
p_["rho"] = 1.225 # [kg/m^3]
L = 0.25
I_max = p_["m"] * L**2 # Inertia of a point mass at a distance L
I_ref = I_max/5
p_["I"] = casadi.diag([I_ref/2,I_ref/2,I_ref]) # [N.m^2]
p_["rotors_p",0] = DM([L,0,0])
p_["rotors_p",1] = DM([0,L,0])
p_["rotors_p",2] = DM([-L,0,0])
p_["rotors_p",3] = DM([0,-L,0])
for i in range(NR):
R_ = p_["rotors_model",i,"R"] # Radius of propeller [m]
m_ = 0.01 # Mass of a propeller [kg]
I_max = m_ * R_**2 # Inertia of a point mass
I_ref = I_max/5
p_["rotors_I",i] = casadi.diag([I_ref/2,I_ref/2,I_ref])
if debug:
print p.vecNZcat()
dist_ = dist(0)
# ----------------- Scaling ---------------------
scaling_states = states(1)
scaling_controls = controls(1)
scaling_states["r"] = 500
scaling_controls["CR"] = 0.005
scaling_dist = dist()
scaling_dist["Faer"] = float(p_["m"]*p_["g"]/NR)
scaling_dist["Caer"] = 0.0026
# ----------- Frames ------------------
T_10 = mul(tr(*pos),Tquat(*q))
T_01 = kin_inv(T_10)
R_10 = T2R(T_10)
R_01 = R_10.T
# -------------------------------------
dstates = struct_symSX(states)
dp,dv,dq,dw,dr = dstates[...]
res = struct_SX(states) # DAE residual hand side
# ----------- Dynamics of the body ----
res["p"] = v - dp
# Newton, but transform the force F from {1} to {0}
res["v"] = mul(R_10,F) - m*dv
# Kinematics of the quaterion.
res["q"] = mul(quatDynamics(*q),w)-dq
# This is a trick by Sebastien Gros to stabilize the quaternion evolution equation
res["q"] += -q*(sumAll(q**2)-1)
# Agular impulse H_1011
H = mul(p["I"],w) # Due to platform
for i in range(NR):
H+= mul(p["rotors_I",i], w + vertcat([0,0,p["rotors_spin",i]*r[i]])) # Due to rotor i
dH = mul(jacobian(H,w),dw) + mul(jacobian(H,q),dq) + mul(jacobian(H,r),dr) + casadi.cross(w,H)
res["w"] = C - dH
for i in range(NR):
res["r",i] = CR[i] + p["rotors_spin",i]*rotors_Caer[i][2] - p["rotors_I",i][2]*(dr[i]+dw[2]) # Dynamics of rotor i
# ---------------------------------
# Make a vector of f ?
#if quatnorm:
# f = vertcat(f+[sumAll(q**2)-1])
#else:
# f = vertcat(f)
# ------------ Force model ------------
Fg = mul(R_01,vertcat([0,0,-g*m]))
F_total = Fg + sum(rotors_Faer) # Total force acting on the platform
C_total = SX([0,0,0]) # Total torque acting on the platform
for i in range(NR):
C_total[:2] += rotors_Caer[i][:2] # The x and y components propagate
C_total[2] -= p["rotors_spin",i]*CR[i] # the z compent moves through a serparate system
C_total += casadi.cross(p["rotors_p",i],rotors_Faer[i]) # Torques due to thrust
res = substitute(res,F,F_total)
res = substitute(res,C,C_total)
subs_before = []
subs_after = []
v_global = mul(R_01,v)
u_z = SX([0,0,1])
# Now fill in the aerodynamic forces
for i in range(NR):
c,R,CL_alpha,alpha_0, CD_alpha, CD_i = p["rotors_model",i,...]
#Bristeau P-jean, Martin P, Salaun E, Petit N. The role of propeller aerodynamics in the model of a quadrotor UAV. In: Proceedings of the European Control Conference 2009.; 2009:683-688.
v_local = v_global + (casadi.cross(w,p["rotors_p",i])) # Velocity at rotor i
rotors_Faer_physics = (rho*c*R**3*r[i]**2*CL_alpha*(alpha_0/3.0-v_local[2]/(2.0*R*r[i]))) * u_z
subs_before.append(rotors_Faer[i])
subs_after.append(rotors_Faer_physics + dist["Faer",i])
rotors_Caer_physics = -p["rotors_spin",i]*rho*c*R**4*r[i]**2*(CD_alpha/4.0+CD_i*alpha_0**2*(alpha_0/4.0-2.0*v_local[2]/(3.0*r[i]*R))-CL_alpha*v_local[2]/(r[i]*R)*(alpha_0/3.0-v_local[2]/(2.0*r[i]*R))) * u_z
subs_before.append(rotors_Caer[i])
subs_after.append(rotors_Caer_physics + dist["Caer",i])
res = substitute(res,veccat(subs_before),veccat(subs_after))
# Make an explicit ode
rhs = - casadi.solve(jacobian(res,dstates),substitute(res,dstates,0))
# --------------------------------------
self.res_w = res
self.res = substitute(res,dist,dist_)
self.res_ = substitute(self.res,p,p_)
resf = SXFunction([dstates, states, controls ],[self.res_])
resf.init()
self.resf = resf
self.rhs_w = rhs
self.rhs = substitute(rhs,dist,dist_)
self.rhs_ = substitute(self.rhs,p,p_)
t = SX("t")
# We end up with a DAE that captures the system dynamics
dae = SXFunction(daeIn(t=t,x=states,p=controls),daeOut(ode=self.rhs_))
dae.init()
self.dae = dae
cdae = SXFunction(controldaeIn(t=t, x=states, u= controls,p=p),daeOut(ode=self.rhs))
cdae.init()
self.cdae = cdae
self.states = states
self.dstates = dstates
self.p = p
self.p_ = p_
self.controls = controls
self.NR = NR
self.w = dist
self.w_ = dist_
self.t = t
self.states_ = states()
self.dstates_ = states()
self.controls_ = controls()
self.scaling_states = scaling_states
self.scaling_controls = scaling_controls
self.scaling_dist = scaling_dist
