def test_svd_test_case():
# a test case from Golub and Reinsch
# (see wilkinson/reinsch: handbook for auto. comp., vol ii-linear algebra, 134-151(1971).)
eps = mp.exp(0.8 * mp.log(mp.eps))
a = [[22, 10, 2, 3, 7],
[14, 7, 10, 0, 8],
[-1, 13, -1, -11, 3],
[-3, -2, 13, -2, 4],
[ 9, 8, 1, -2, 4],
[ 9, 1, -7, 5, -1],
[ 2, -6, 6, 5, 1],
[ 4, 5, 0, -2, 2]]
a = mp.matrix(a)
b = mp.matrix([mp.sqrt(1248), 20, mp.sqrt(384), 0, 0])
S = mp.svd_r(a, compute_uv = False)
S -= b
assert mp.mnorm(S) < eps
S = mp.svd_c(a, compute_uv = False)
S -= b
assert mp.mnorm(S) < eps
def run_svd_r(A, full_matrices=False, verbose=True):
m, n = A.rows, A.cols
eps = mp.exp(0.8 * mp.log(mp.eps))
if verbose:
print("original matrix:\n", str(A))
print("full", full_matrices)
U, S0, V = mp.svd_r(A, full_matrices=full_matrices)
S = mp.zeros(U.cols, V.rows)
for j in xrange(min(m, n)):
S[j, j] = S0[j]
if verbose:
print("U:\n", str(U))
print("S:\n", str(S0))
print("V:\n", str(V))
C = U * S * V - A
err = mp.mnorm(C)
if verbose:
print("C\n", str(C), "\n", err)
assert err < eps
D = V * V.transpose() - mp.eye(V.rows)
err = mp.mnorm(D)
if verbose:
print("D:\n", str(D), "\n", err)
assert err < eps
E = U.transpose() * U - mp.eye(U.cols)
err = mp.mnorm(E)
if verbose:
print("E:\n", str(E), "\n", err)
assert err < eps
def run_svd_r(A, full_matrices = False, verbose = True):
m, n = A.rows, A.cols
eps = mp.exp(0.8 * mp.log(mp.eps))
if verbose:
print("original matrix:\n", str(A))
print("full", full_matrices)
U, S0, V = mp.svd_r(A, full_matrices = full_matrices)
S = mp.zeros(U.cols, V.rows)
for j in xrange(min(m, n)):
S[j,j] = S0[j]
if verbose:
print("U:\n", str(U))
print("S:\n", str(S0))
print("V:\n", str(V))
C = U * S * V - A
err = mp.mnorm(C)
if verbose:
print("C\n", str(C), "\n", err)
assert err < eps
D = V * V.transpose() - mp.eye(V.rows)
err = mp.mnorm(D)
if verbose:
print("D:\n", str(D), "\n", err)
assert err < eps
E = U.transpose() * U - mp.eye(U.cols)
err = mp.mnorm(E)
if verbose:
print("E:\n", str(E), "\n", err)
assert err < eps
for i in range(END):
j = (i + 1) % L
if i % 2 == 0:
A0[i, j] = -w
A0[j, i] = A0[i, j]
elif j % 2 == 0:
A0[i, j] = -1
A0[j, i] = A0[i, j]
B0 = mp.matrix([[
delta0 if i == j - 1 else -delta0 if i == j + 1 else mp.mpf(0.0)
for j in list(range(L))
] for i in list(range(L))])
EP0, eigvalsi, EF0 = mp.svd_r(A0 + B0, compute_uv=True)
eigvecf0 = EF0.T
eigvecp0 = EP0
#### t>0 #####
for i in range(END):
j = (i + 1) % L
if i % 2 == 0:
At[i, j] = -v
At[j, i] = At[i, j]
elif j % 2 == 0:
At[i, j] = -1
At[j, i] = At[i, j]
Bt = mp.matrix([[
])
J_6 = np.hstack(
(Td6[0, 3], Td6[1, 3], Td6[2, 3], Td6[2, 1], Td6[0, 2], Td6[1, 0]))
J_ = np.vstack((J_1, J_2, J_3, J_4, J_5, J_6))
J_ = sp.Matrix(J_)
J_ = J_.subs([(l1, 346), (l2, 324), (l3, 321), (l4, 1075), (l5, 225),
(l6, 1280), (l7, 215), (q1, np.pi / 6), (q2, np.pi / 6),
(q3, np.pi / 6), (q4, np.pi / 6), (q5, np.pi / 6),
(q6, np.pi / 6)])
# let's check
print(np.array(J - J_))
X = np.vstack((J1, J2, J3, J4, J5, J6))
J11 = X[:3, :3]
J11 = sp.Matrix(J11)
print(np.shape(J11))
print("det:", sp.simplify(J11.det()))
print(sp.simplify(J11.det()))
from mpmath import mp
Z = mp.matrix(J)
U, D, Vt = mp.svd_r(Z, compute_uv=True)
print("U =", U)
print("D =", D)
print("V(transpose) =", Vt)
mat = np.array([[5, 2, 1, 0, 0, 4], [8, 4, 6, 9, 6, 1], [4, 2, 0, 1, 2, 7],
[6, 6, 8, 5, 2, 0], [0, 2, 3, 5, 9, 6], [3, 7, 8, 9, 5, 2]])
print(mat[:3, :3])
def get_osqp_mat(self,
x0_v: np.ndarray,
x0_s_t: np.ndarray,
v_ini: float,
v_max: np.ndarray,
v_end: float,
F_ini: float,
kappa: np.ndarray,
delta_s: np.ndarray,
P_max: np.ndarray = None,
ax_max: np.ndarray = None,
ay_max: np.ndarray = None,
v_max_cstr: np.ndarray = None) -> tuple:
"""Constructs necessary QP matrices from the linearized versions of the physically necessary
equations for the velocity optimization of a vehicle.
:param x0_v: initial guess velocity [m/s]
:param x0_s_t: initial guess slack variables tire [-]
:param v_ini: initial hard constrained velocity [m/s]; currently unused here as we remove the first velocity
point from the optimization problem and and set it manually afterwards
:param v_max: max. allowed velocity (in objective function) [m/s]
:param v_end: hard constrained max. allowed value of end velocity in optimization horizon [m/s]
:param F_ini: hard constrained initial force [kN]
:param kappa: curvature profile of given path [rad/m]
:param delta_s: discretization step length of given path [m]
:param P_max: max. allowed power [kW]
:param ax_max: max. allowed longitudinal acceleration [m/s^2]
:param ay_max: max. allowed lateral accelereation [m/s^2]
:param vmax_cstr: max. allowed spatially dependend velocity (hard constraint) [m/s]; can be used for
e.g. adaptive cruise control (following)
:return: P: Sparse version of Hessian matrix of nonlinear objective function J\n
q: Jacobian of nonlinear objective function J\n
Am_csc: Sparse version of jacobian of nonlinear constraints\n
lo: lower boundaries of linearized constraints\n
up: upper boundaries of linearized constraints
:Authors:
Thomas Herrmann <*****@*****.**>
:Created on:
01.11.2019
"""
m = self.m
n = self.n
################################################################################################################
# Pre-calculations: need to be done online but only once within this loop
################################################################################################################
# Inverse of discretization step lengths [1/m]
delta_s_inv = 1 / delta_s
# Inverse of discretization step lengths * 1/2 [1/m]
delta_2s_inv = 0.5 * delta_s_inv
# Initial velocity guess squared [m^2/s^2]
x0_squ = np.square(x0_v[0:m - 1])
# Initial velocity guess cubed [m^3/s^3]
x0_cub = x0_v[0:m - 1]**3
# Initial velocity guess squared reduced by first entry [m^2/s^2]
x0_squ_wh = np.square(x0_v[1:m])
# Initial velocity guess reduced by last entry [m/s]
x0_red = x0_v[0:m - 1]
# Initial velocity guess reduced by first entry [m/s]
x0_wh = x0_v[1:m]
# curvature profile reduced by last entry [rad/m]
kappa = kappa[0:m - 1]
# Inverse of friction limits [s^2/m] (constant or spatially variable)
if ax_max is not None:
ax_max_inv = np.divide(np.ones((1, m - 1), dtype=np.float64),
ax_max[0:-1])
ay_max_inv = np.divide(np.ones((1, m - 1), dtype=np.float64),
ay_max[0:-1])
else:
ax_max_inv = 1 / self.sym_sc_['axmax_mps2_']
ay_max_inv = 1 / self.sym_sc_['aymax_mps2_']
E_mat_red = self.E_mat_red
E_mat_redd = self.E_mat_redd
EP_mat_red = self.EP_mat_red
T_mat_inv = self.T_mat_inv
################################################################################################################
# Calculate matrices for OSQP solver
################################################################################################################
self.x0 = x0_v
self.x0_s_t = x0_s_t
# Inverse of delta t for every discretization step length [1/s]
t_inv = x0_red * delta_s_inv
# Inverse of delta t for every discretization step length [1/s] with shifted velocity entries: v1/d0, v2/d1, ...
t_inv_wh = x0_wh * delta_s_inv
# --- Construction of jacobian of objective function J
# fJ_jac(x0_v, v_max)
self.J_jac = np.zeros(m, dtype=np.float64)
# first 2 lines
self.J_jac[0] = self.sym_sc_['dvdv_w_'] * \
(2 * x0_v[0] - 4 * x0_v[1] + 2 * x0_v[2]) + 2 * (x0_v[0] - v_max[0])
self.J_jac[1] = self.sym_sc_['dvdv_w_'] * \
((2 * x0_v[1] - 4 * x0_v[2] + 2 * x0_v[3]) + (- 4 * x0_v[0] + 8 * x0_v[1] - 4 * x0_v[2])) + \
2 * (x0_v[1] - v_max[1])
# mid part
self.J_jac[2:-2] = self.sym_sc_['dvdv_w_'] * \
(2 * x0_v[0:-4] - 8 * x0_v[1:-3] + 12 * x0_v[2:-2] - 8 * x0_v[3:-1] + 2 * x0_v[4:]) + \
2 * (x0_v[2:-2] - v_max[2:-2])
# last 2 lines
self.J_jac[-2] = self.sym_sc_['dvdv_w_'] * \
((2 * x0_v[-4] - 4 * x0_v[-3] + 2 * x0_v[-2]) + (-4 * x0_v[-3] + 8 * x0_v[-2] - 4 * x0_v[-1])) + \
2 * (x0_v[-2] - v_max[-2])
self.J_jac[-1] = self.sym_sc_['dvdv_w_'] * \
(2 * x0_v[-3] - 4 * x0_v[-2] + 2 * x0_v[-1]) + 2 * (x0_v[-1] - v_max[-1])
# Slack-contribution
self.J_jac =\
np.append(self.J_jac, 2 * self.sym_sc_['s_tre_w_quad_'] * self.sym_sc_['s_v_t_unit_'] ** 2 * x0_s_t
+ self.sym_sc_['s_tre_w_lin_'] * self.sym_sc_['s_v_t_unit_'])
# --- Do not convert Hessian if not necessary (e.g., if constant)
if self.J_Hess is None:
self.J_Hess = np.eye(m + n, dtype=np.float64)
h_diag = 12 * self.sym_sc_['dvdv_w_'] * np.ones(m,
dtype=np.float64)
h_diag[0] = 2 * self.sym_sc_['dvdv_w_']
h_diag[1] = 10 * self.sym_sc_['dvdv_w_']
h_diag[-2] = 10 * self.sym_sc_['dvdv_w_']
h_diag[-1] = 2 * self.sym_sc_['dvdv_w_']
np.fill_diagonal(self.J_Hess[:m, :], h_diag + 2)
h_diag_k1 = -8 * self.sym_sc_['dvdv_w_'] * np.ones(
m - 1, dtype=np.float64)
h_diag_k1[0] = -4 * self.sym_sc_['dvdv_w_']
h_diag_k1[-1] = -4 * self.sym_sc_['dvdv_w_']
np.fill_diagonal(self.J_Hess[:m - 1, 1:], h_diag_k1)
np.fill_diagonal(self.J_Hess[1:m], h_diag_k1)
h_diag_k2 = 2 * self.sym_sc_['dvdv_w_'] * np.ones(m - 2,
dtype=np.float64)
np.fill_diagonal(self.J_Hess[:m - 2, 2:], h_diag_k2)
np.fill_diagonal(self.J_Hess[2:m], h_diag_k2)
s_diag = 2 * self.sym_sc_['s_tre_w_quad_'] * self.sym_sc_[
's_v_t_unit_']**2 * np.ones((n, ))
np.fill_diagonal(self.J_Hess[m:, m:], s_diag)
# Print condition number of Hessian, useful for debugging
if self.sqp_stgs['b_print_condition_number']:
S = mp.svd_r(mp.matrix(self.J_Hess), compute_uv=False)
print(self.sid)
print("Singular values of Hessian in " + self.sid, S)
cond_number = np.max(S) / np.min(S)
print("Condition-number of Hessian:", cond_number)
else:
pass
# linearized force box constraint [kN], fF_cst(x0_v, delta_s)
self.F_cst = (-self.sym_sc_['Fmax_kN_'] * np.ones(
(1, m - 1)) + 0.001 * self.sym_sc_['c_res_'] * x0_squ +
self.sym_sc_['m_t_'] *
(x0_squ_wh - x0_squ) * delta_2s_inv).T
# jacobian
np.fill_diagonal(E_mat_red[:, 1:], self.sym_sc_['m_t_'] * t_inv_wh)
np.fill_diagonal(
E_mat_red, 0.002 * self.sym_sc_['c_res_'] * x0_red -
self.sym_sc_['m_t_'] * t_inv)
self.F_cst_jac = E_mat_red
# initial force constraint including small tolerance [kN]
self.F_ini_cst = - F_ini - self.sym_sc_['Fini_tol_'] + \
0.001 * self.sym_sc_['c_res_'] * x0_squ[0] + \
self.sym_sc_['m_t_'] * (x0_squ_wh[0] - x0_squ[0]) * delta_2s_inv[0]
# jacobian
self.F_ini_cst_jac = np.zeros((1, m + n), dtype=np.float64)
self.F_ini_cst_jac[0, 0] = 0.002 * self.sym_sc_['c_res_'] * x0_v[
0] - self.sym_sc_['m_t_'] * t_inv[0]
self.F_ini_cst_jac[0, 1] = self.sym_sc_['m_t_'] * t_inv_wh[0]
# max. end velocity hard constraint [m/s]
self.v_cst_end = x0_v[-1] - v_end
# constant jacobian
if not self.b_ini_done:
self.v_cst_end_jac = np.zeros((1, m + self.n), dtype=np.float64)
self.v_cst_end_jac[-1, -1 - n] = 1
# power constraint [kW]
if self.sqp_stgs['b_var_power']:
self.P_cst = (-P_max + (self.sym_sc_['m_t_'] * x0_red *
(x0_squ_wh - x0_squ) * delta_2s_inv) +
self.sym_sc_['c_res_'] * x0_cub * 0.001).reshape(
(self.m - 1, 1))
else:
self.P_cst = (-self.sym_sc_['Pmax_kW_'] * np.ones(
(m - 1, 1)).T + (self.sym_sc_['m_t_'] * x0_red *
(x0_squ_wh - x0_squ) * delta_2s_inv) +
self.sym_sc_['c_res_'] * x0_cub * 0.001).T
# jacobian
pw1_ = 0.001 * self.sym_sc_['c_res_'] * x0_squ + \
x0_red * (0.002 * self.sym_sc_['c_res_'] * x0_red - self.sym_sc_['m_t_'] * t_inv) + \
self.sym_sc_['m_t_'] * (x0_squ_wh - x0_squ) * delta_2s_inv
pw2_ = self.sym_sc_['m_t_'] * x0_red * x0_v[1:m] * delta_s_inv
np.fill_diagonal(EP_mat_red, pw1_)
np.fill_diagonal(EP_mat_red[:, 1:], pw2_)
self.P_cst_jac = EP_mat_red
# slack contribution values
s_t_len = len(self.ones_vec_red)
s_t_contrib = \
np.reshape(np.repeat(self.sym_sc_['s_v_t_unit_'] * x0_s_t, self.slack_every_v)[0:s_t_len], (s_t_len, 1))
tre_dv = (x0_squ_wh - x0_squ) * delta_2s_inv * ax_max_inv + \
0.001 * self.sym_sc_['c_res_'] * x0_squ * ax_max_inv * self.m_inv
tre_v2 = kappa * x0_squ * ay_max_inv
# Tire diamond constraint [-]
self.Tre_cst1 = (tre_dv + tre_v2 - self.ones_vec_red.T).T
self.Tre_cst1 -= s_t_contrib
# jacobian
p1_ = 2 * kappa * x0_red * ay_max_inv
p2_ = (0.002 * self.sym_sc_['c_res_'] * x0_red - self.sym_sc_['m_t_'] * t_inv) * \
(ax_max_inv / self.sym_sc_['m_t_'])
np.fill_diagonal(T_mat_inv[:, 1:], t_inv_wh * ax_max_inv)
T_mat_inv_tre = T_mat_inv
T_mat_inv_tre_neg = -T_mat_inv
# keep the "-1" in these entries fixed (stemming from the slack variables)
T_mat_inv_tre_neg[:, -self.n:] *= -1
np.fill_diagonal(T_mat_inv_tre, p1_ + p2_)
self.Tre_cst1_jac = copy.deepcopy(T_mat_inv_tre)
# Tire diamond constraint [-]
self.Tre_cst2 = (-tre_dv + tre_v2 - self.ones_vec_red.T).T
self.Tre_cst2 -= s_t_contrib
# jacobian
np.fill_diagonal(T_mat_inv_tre_neg, p1_ - p2_)
self.Tre_cst2_jac = copy.deepcopy(T_mat_inv_tre_neg)
# Tire diamond constraint [-]
self.Tre_cst3 = (-tre_dv - tre_v2 - self.ones_vec_red.T).T
self.Tre_cst3 -= s_t_contrib
# jacobian
np.fill_diagonal(T_mat_inv_tre_neg, -p1_ - p2_)
self.Tre_cst3_jac = copy.deepcopy(T_mat_inv_tre_neg)
# Tire diamond constraint [-]
self.Tre_cst4 = (+tre_dv - tre_v2 - self.ones_vec_red.T).T
self.Tre_cst4 -= s_t_contrib
# jacobian
np.fill_diagonal(T_mat_inv_tre, -p1_ + p2_)
self.Tre_cst4_jac = copy.deepcopy(T_mat_inv_tre)
# delta Force constraint [kN]
self.dF_cst = -self.sym_sc_['dF_kN_pos_'] * np.ones(
(m - 2, 1)) + self.F_cst[1:m] - self.F_cst[0:m - 2]
# jacobian
d1_ = self.sym_sc_['m_t_'] * t_inv[0:m - 2] - 0.002 * self.sym_sc_[
'c_res_'] * x0_red[0:m - 2]
d2_ = \
self.sym_sc_['m_t_'] * (- t_inv[1:m - 1] - t_inv_wh[0:m - 2]) + \
0.002 * self.sym_sc_['c_res_'] * x0_wh[0:m - 2]
d3_ = self.sym_sc_['m_t_'] * t_inv_wh[1:m]
np.fill_diagonal(E_mat_redd, d1_)
np.fill_diagonal(E_mat_redd[:, 1:], d2_)
np.fill_diagonal(E_mat_redd[:, 2:], d3_)
self.dF_cst_jac = E_mat_redd
################################################################################################################
# P, sparse CSC
################################################################################################################
if not self.b_ini_done:
# Exclude first velocity point
P = sparse.csc_matrix(self.J_Hess[1:, 1:])
else:
P = None
################################################################################################################
# q
################################################################################################################
# Exclude first velocity point
q = self.J_jac.T[1:]
################################################################################################################
# A, sparse CSC
################################################################################################################
# --- Initialize sparse matrix A only once with following sparsity pattern
if self.Am_csc is None or not self.sqp_stgs['b_sparse_matrix_fill']:
ir_ = 0
# Lower box on every velocity value to be > 0 (for numerical issues in emergency profile)
if not self.b_ini_done:
# Velocity floor constraint to avoid v < 0 on v__1, ..., v__m-1 (in total m-2 points)
self.Am[ir_:ir_ + m - 1, :] = 0
self.Am[ir_:ir_ + m - 2, 0:m - 2] = np.eye(m - 2,
m - 2,
dtype=np.float64)
ir_ += m - 2
# Slack ceil constraint to ensure slack < specified value and slack > 0
self.Am[ir_:ir_ + n, :] = 0
self.Am[ir_:ir_ + n,
-n:] = self.sym_sc_['s_v_t_unit_'] * np.eye(
n, n, dtype=np.float64)
ir_ += n
# end velocity constraint: v_m < v_end
self.Am[ir_, :] = 0
self.Am[ir_, 0:m - 1] = self.v_cst_end_jac[0, 1:m]
ir_ += 1
# --- Only move pointer ir_ to valid position
else:
ir_ += m - 2
ir_ += n
ir_ += 1
# --- Don't fill Emergency SQP with F_ini constraint
if self.sid != 'EmergSQP':
self.Am[ir_:ir_ + 1, :] = 0.1 * self.F_ini_cst_jac[:, 1:]
ir_ += 1
# Fill PerfSQP with F-box-constraint apart from first force point
self.Am[ir_:ir_ + m - 2,
0:m - 1 + n] = 0.1 * self.F_cst_jac[1:, 1:]
ir_ += m - 2
# --- Fill Emergency SQP with force box constraint
else:
self.Am[ir_:ir_ + m - 1,
0:m - 1 + n] = 0.1 * self.F_cst_jac[:, 1:]
ir_ += m - 1
# Power constraint
self.Am[ir_:ir_ + m - 1,
0:m - 1 + n] = 0.01 * self.P_cst_jac[:, 1:]
ir_ += m - 1
# Tire constraint
self.Am[ir_:ir_ + m - 1, 0:m - 1 +
n] = self.sym_sc_['tre_cst_w_'] * self.Tre_cst1_jac[:, 1:]
ir_ += m - 1
# Tire constraint
self.Am[ir_:ir_ + m - 1, 0:m - 1 +
n] = self.sym_sc_['tre_cst_w_'] * self.Tre_cst2_jac[:, 1:]
ir_ += m - 1
# Tire constraint
self.Am[ir_:ir_ + m - 1, 0:m - 1 +
n] = self.sym_sc_['tre_cst_w_'] * self.Tre_cst3_jac[:, 1:]
ir_ += m - 1
# Tire constraint
self.Am[ir_:ir_ + m - 1, 0:m - 1 +
n] = self.sym_sc_['tre_cst_w_'] * self.Tre_cst4_jac[:, 1:]
ir_ += m - 1
# delta Force hard constraint (currently included in objective funciton)
# self.Am[ir_:ir_ + m-2, 0:m-1] = self.dF_cst_jac[:,1:] # A = np.append(A, self.dF_pos_cst_jac_, axis=0)
# ir += m-2
# --- If sparse version of matrix Am is available, fill sparsity pattern
else:
# --- Skip constant entries in Am_csc
ir_ = 0
ir_ += m - 2
ir_ += n
ir_ += 1
# --- Don't fill Emergency SQP with F_ini constraint
if self.sid != 'EmergSQP':
self.Am_csc[ir_ + self.sparsity_pat['F_ini_r'], self.sparsity_pat['F_ini_c']] = \
0.1 * self.F_ini_cst_jac[:, 1:][self.sparsity_pat['F_ini_r'], self.sparsity_pat['F_ini_c']]
ir_ += 1
# Fill PerfSQP with F-box-constraint apart from first force point
self.Am_csc[ir_ + self.sparsity_pat['F_r'], self.sparsity_pat['F_c']] = \
0.1 * self.F_cst_jac[1:, 1:][self.sparsity_pat['F_r'], self.sparsity_pat['F_c']]
ir_ += m - 2
else:
# Force box constraint
self.Am_csc[ir_ + self.sparsity_pat['F_r'], self.sparsity_pat['F_c']] = \
0.1 * self.F_cst_jac[:, 1:][self.sparsity_pat['F_r'], self.sparsity_pat['F_c']]
ir_ += m - 1
# Power constraint
self.Am_csc[ir_ + self.sparsity_pat['P_r'], self.sparsity_pat['P_c']] = \
0.01 * self.P_cst_jac[:, 1:][self.sparsity_pat['P_r'], self.sparsity_pat['P_c']]
ir_ += m - 1
# Tire constraint
self.Am_csc[ir_ + self.sparsity_pat['Tre_r'], self.sparsity_pat['Tre_c']] = \
self.sym_sc_['tre_cst_w_'] * \
self.Tre_cst1_jac[:, 1:][self.sparsity_pat['Tre_r'], self.sparsity_pat['Tre_c']]
ir_ += m - 1
# Tire constraint
self.Am_csc[ir_ + self.sparsity_pat['Tre_r'], self.sparsity_pat['Tre_c']] = \
self.sym_sc_['tre_cst_w_'] * \
self.Tre_cst2_jac[:, 1:][self.sparsity_pat['Tre_r'], self.sparsity_pat['Tre_c']]
ir_ += m - 1
# Tire constraint
self.Am_csc[ir_ + self.sparsity_pat['Tre_r'], self.sparsity_pat['Tre_c']] = \
self.sym_sc_['tre_cst_w_'] * \
self.Tre_cst3_jac[:, 1:][self.sparsity_pat['Tre_r'], self.sparsity_pat['Tre_c']]
ir_ += m - 1
# Tire constraint
self.Am_csc[ir_ + self.sparsity_pat['Tre_r'], self.sparsity_pat['Tre_c']] = \
self.sym_sc_['tre_cst_w_'] * \
self.Tre_cst4_jac[:, 1:][self.sparsity_pat['Tre_r'], self.sparsity_pat['Tre_c']]
ir_ += m - 1
# delta Force constraint
# self.Am[ir_:ir_ + m-2, 0:m-1] = self.dF_cst_jac[:,1:] # A = np.append(A, self.dF_pos_cst_jac_, axis=0)
# ir += m-2
if self.Am_csc is None or not self.sqp_stgs['b_sparse_matrix_fill']:
# define A as sparse CSC
self.Am_csc = sparse.csc_matrix(self.Am)
################################################################################################################
# l, val_min - val_max - g(x0)
################################################################################################################
ir_ = 0
# Lower box on every velocity to avoid v < 0 on v__1, ..., v__m-1
self.lo[ir_:ir_ + m - 2, 0] = -x0_wh[:-1]
ir_ += m - 2
# Floor '0' on tire slack variables
self.lo[ir_:ir_ + n, 0] = -self.sym_sc_['s_v_t_unit_'] * x0_s_t
ir_ += n
# End velocity lower bound
if not self.b_ini_done:
self.lo[ir_, 0] = -np.inf
ir_ += 1
# --- Don't fill Emergency SQP with F_ini constraint
if self.sid != 'EmergSQP':
# F_ini lower bound
self.lo[ir_, 0] = 0.1 * (-self.F_ini_cst -
2 * self.sym_sc_['Fini_tol_'])
ir_ += 1
# Force box lower bound
self.lo[ir_:ir_ + m - 2, 0] = 0.1 * \
([self.sym_sc_['Fmin_kN_'] - self.sym_sc_['Fmax_kN_']] * (m - 2) - self.F_cst[1:, :].T)
ir_ += m - 2
else:
# Force box lower bound
self.lo[ir_:ir_ + m - 1, 0] = 0.1 * \
([self.sym_sc_['Fmin_kN_'] - self.sym_sc_['Fmax_kN_']] * (m - 1) - self.F_cst.T)
ir_ += m - 1
if not self.b_ini_done:
# Power lower bound
self.lo[ir_:ir_ + m - 1, 0] = [-np.inf] * (m - 1)
ir_ += m - 1
# Tire lower bound
self.lo[ir_:ir_ + m - 1, 0] = [-np.inf] * (m - 1)
ir_ += m - 1
# Tire lower bound
self.lo[ir_:ir_ + m - 1, 0] = [-np.inf] * (m - 1)
ir_ += m - 1
# Tire lower bound
self.lo[ir_:ir_ + m - 1, 0] = [-np.inf] * (m - 1)
ir_ += m - 1
# Tire lower bound
self.lo[ir_:ir_ + m - 1, 0] = [-np.inf] * (m - 1)
ir_ += m - 1
else:
# Move pointer ir_
ir_ += (m - 1) * 5
# delta force box lower bound
# self.lo[ir_:ir_ + m-2, 0] = [self.sym_sc_['dF_kN_neg_'] - self.sym_sc_['dF_kN_pos_']] * (m - 2) - \
# (self.dF_cst).T
################################################################################################################
# u: - g(x0)
################################################################################################################
ir_ = 0
# Velocity floor constraint to avoid v < 0 on v__1, ..., v__m-1
if v_max_cstr is None:
self.up[ir_:ir_ + m - 2, 0] = [np.inf] * (m - 2)
# Put hard constraint on v: to be used only when objects appear in optimization horizon from its end
else:
self.up[ir_:ir_ + m - 2, 0] = -x0_wh[:-1] + v_max_cstr[1:-1]
ir_ += m - 2
# Ceiling on tire slack variables
self.up[ir_:ir_ + n,
0] = self.sym_sc_['s_v_t_unit_'] * (-x0_s_t +
self.sym_sc_['s_v_t_lim_'])
ir_ += n
# End velocity upper bound
self.up[ir_, 0] = -self.v_cst_end
ir_ += 1
# --- Don't fill Emergency SQP with F_ini constraint
if self.sid != 'EmergSQP':
# F_ini upper bound
self.up[ir_, 0] = -0.1 * self.F_ini_cst
ir_ += 1
# Force box upper bound
self.up[ir_:ir_ + m - 2, 0] = -0.1 * self.F_cst[1:, :].T
ir_ += m - 2
else:
# Force box upper bound
self.up[ir_:ir_ + m - 1, 0] = -0.1 * self.F_cst.T
ir_ += m - 1
# Power constraint upper bound
self.up[ir_:ir_ + m - 1, 0] = -0.01 * self.P_cst.T
ir_ += m - 1
# Tire constraint upper bound
self.up[ir_:ir_ + m - 1, 0] = -self.sym_sc_[
'tre_cst_w_'] * self.Tre_cst1.T # tire 1 hard constraint
ir_ += m - 1
# Tire constraint upper bound
self.up[ir_:ir_ + m - 1, 0] = -self.sym_sc_[
'tre_cst_w_'] * self.Tre_cst2.T # tire 2 hard constraint
ir_ += m - 1
# Tire constraint upper bound
self.up[ir_:ir_ + m - 1, 0] = -self.sym_sc_[
'tre_cst_w_'] * self.Tre_cst3.T # tire 1 hard constraint
ir_ += m - 1
# Tire constraint upper bound
self.up[ir_:ir_ + m - 1, 0] = -self.sym_sc_[
'tre_cst_w_'] * self.Tre_cst4.T # tire 2 hard constraint
ir_ += m - 1
# Delta-force box upper bound
# self.up[ir_:ir_+ m-2, 0] = - (self.dF_cst).T # u = np.append(u, np.array(- (self.dF_pos_cst_).T))
# ir_ += m-2
# --- Set flag after first Matrix-initialization
self.b_ini_done = True
return P, q, self.Am_csc, self.lo, self.up
from sympy import Matrix, pretty, Transpose
from numpy import sqrt
from mpmath import mp
A = mp.matrix([
[
3 / sqrt(2),
3 / sqrt(2),
0,
0,
],
[
0,
0,
1 / 2,
1 / 2,
],
[
0,
0,
1 / 2,
1 / 2,
],
])
U, S, V = mp.svd_r(A)
print("U = \n{}\n\n".format(pretty(U)))
print("sigma = \n{}\n\n".format(pretty(S)))
print("VT = \n{}\n\n".format(pretty(V)))
