def sol_init(self, Hm: np.ndarray, Am: np.ndarray):
"""Function to initialize the qpOASES solver.
:param Hm: Hessian problem matrix
:param Am: Linearized constraints matrix (Jacobian)
:Authors:
Thomas Herrmann <*****@*****.**>
:Created on:
01.01.2020
"""
opts_qpOASES = {
"terminationTolerance": 1e-2,
"printLevel": "low",
"hessian_type": "posdef",
"error_on_fail": False,
"sparse": True
}
# --- Create solver size
Hm = cs.DM(Hm)
Am = cs.DM(Am)
# --- Initialize QP-structure
QP = dict()
QP['h'] = Hm.sparsity()
QP['a'] = Am.sparsity()
self.solver = cs.conic('solver', 'qpoases', QP, opts_qpOASES)
def __construct_qp_solver(self):
Hlag = self.__H_fun(self.__w, self.__p,
ca.MX.sym('lam_g', self.__g.shape))
jacg = self.__jacg_fun(self.__w, self.__p)
# qp cost function and constraints
qp = {'h': Hlag.sparsity(), 'a': jacg.sparsity()}
# qp options
opts = {
'enableEqualities': True,
'printLevel': 'none',
'sparse': True,
'enableInertiaCorrection': True,
'enableCholeskyRefactorisation': 1,
# 'enableRegularisation': True,
# 'epsRegularisation': 1e-6
# 'enableFlippingBounds':True
# 'enableFarBounds': False,
# 'enableFlippingBounds': True,
# 'epsFlipping': 1e-8,
# 'initialStatusBounds': 'inactive',
}
self.__solver = ca.conic('qpsol', 'qpoases', qp, opts)
return None
def PAS():
# X will be the number of tickets
# X = [x1 x2] : x1 - first class tickets; x2 - second class tickets
lb_x1 = 20
lb_x2 = 35
profit_x1 = 2500
profit_x2 = 2000
max_passengers = 130
# x1 + x2 <= 130 -> [1 1] @ [x1 x2] <= 130
A = ca.DM.ones(1, 2)
ub_a = max_passengers
lb_a = lb_x1 + lb_x2
ub_x = np.inf * np.ones((2))
lb_x = -np.inf * np.ones((2))
lb_x[0] = lb_x1
lb_x[1] = lb_x2
H = ca.DM.zeros(2, 2)
g = np.zeros((2, 1))
g[0, 0] = profit_x1
g[1, 0] = profit_x2
g = -1 * g
print(H.shape)
qp = {'h': H.sparsity(), "a": A.sparsity()}
S = ca.conic("S", 'osqp', qp)
# S = ca.conic("S", 'qpoases', qp)
r = S(h=H, g=g, a=A, lbx=lb_x, ubx=ub_x, lba=lb_a, uba=ub_a)
print(f'r[x]= {r["x"]}')
def hanging_chain(N):
m = 4
ki = 1000
gc = 9.81
# x = [y1 z1 y2 z2 ... yn zn]
# 1 2 n
H = ca.DM.zeros(2 * N, 2 * N)
for i in range(0, 2 * N - 2):
H[i, i + 2] = -1
H[i + 2, i] = -1
for i in range(0, 2 * N):
H[i, i] = 2.0
H[0, 0] = H[1, 1] = H[-2, -2] = H[-1, -1] = 1
H = ki * H
g = ca.DM.zeros(2 * N)
g[1:-2:2] = gc * m
# Initialize the lower bound
lbx = -np.inf * np.ones(2 * N)
ubx = np.inf * np.ones(2 * N)
lbx[0] = ubx[0] = -2
lbx[1] = ubx[1] = 1
lbx[-2] = ubx[-2] = 2
lbx[-1] = ubx[-1] = 1
# lbx[3:-2:2] = 0.5
# Tilted ground constraints
A = ca.DM.zeros(N, 2 * N)
for k in range(N):
A[k, 2 * k] = -0.1
A[k, 2 * k + 1] = 0.1
lba = 0.5 * ca.DM.ones(N, 1)
uba = np.inf * ca.DM.ones(N, 1)
# print("Bound for our sysyem \n LBx: ", lbx, "\nUBx", ubx)
# qp = {'h': H.sparsity()}
# S = ca.conic('hc', 'osqp', qp)
# sol = S(h=H, g=g, lbx=lbx, ubx=ubx)
print("Bound for our sysyem \n LBx: ", lbx, "\nUBx", ubx)
qp = {'h': H.sparsity(), 'a': A.sparsity()}
S = ca.conic('hc', 'osqp', qp)
sol = S(h=H, g=g, a=A, lbx=lbx, ubx=ubx, lba=lba, uba=uba)
x_opt = sol['x']
Y0 = x_opt[0::2]
Z0 = x_opt[1::2]
plt.plot(Y0, Z0, 'b-o')
plt.show()
def setup_solver(self):
"""Initialize the QP solver.
This uses the casadi low-level interface for QP problems. It
uses the sparsity of the H, A, B_lb and B_ub matrices.
"""
H_expr = self.get_cost_expr()
A_expr, Blb_expr, Bub_expr = self.get_constraints_expr()
self.solver = cs.conic("solver", self.options["solver_name"], {
"h": H_expr.sparsity(),
"a": A_expr.sparsity()
}, self.options["solver_opts"])
profit_x1 = 2000
profit_x2 = 2000
max_passengers = 150
# x1 + x2 <= 130 -> [1 1] @ [x1; x2] <= 130
A = ca.DM.ones(1,2)
ub_a = max_passengers
lb_a = lb_x1 + lb_x2
# Initializing the bounds for our problem
ub_x = np.inf*np.ones(2)
lb_x = -np.inf*np.ones(2)
lb_x[0] = lb_x1
lb_x[1] = lb_x2
H = ca.DM.zeros(2,2)
g = np.zeros((2,1))
g[0,0] = profit_x1
g[1,0] = profit_x2
g = -1*g
qp = {'h': H.sparsity(), 'a': A.sparsity() }
S = ca.conic('S','osqp',qp)
r = S(h=H, g=g, a=A, lbx=lb_x, ubx=ub_x, lba=lb_a, uba=ub_a)
print("r[x]", r['x'])
print("Optimal sol is selling", int(round(float(r['x'][0]))), "first class and", int(round(float(r['x'][1]))), "second class tickets")
def solve(self, ):
self.isSolved = True
# prepare QP
QSet, ASet, BSet, AeqSet, BeqSet = self.getQPset()
if self.algorithm == 'end-derivative' and ASet is not None:
mapMat = self.coeff2endDerivatives(AeqSet[0])
QSet, HSet, ASet, BSet = self.mapQP(QSet, ASet, BSet, AeqSet,
BeqSet)
elif self.algorithm == 'poly-coeff' or ASet is None:
x_sym = ca.SX.sym('x', QSet[0].shape[0])
opts_setting = {
'ipopt.max_iter': 100,
'ipopt.print_level': 0,
'print_time': 0,
'ipopt.acceptable_tol': 1e-8,
'ipopt.acceptable_obj_change_tol': 1e-6
}
else:
print('unsupported algorithm')
for dd in range(self.dim):
print('soving {}th dimension ...'.format(dd))
if self.algorithm == 'poly-coeff' or ASet is None:
obj = ca.mtimes([x_sym.T, QSet[dd], x_sym])
if ASet is not None:
a_set = np.concatenate((ASet[dd], AeqSet[dd]))
else:
a_set = AeqSet[dd].copy()
Ax_sym = ca.mtimes([a_set, x_sym])
if BSet is not None:
b_set_u = np.concatenate(
(BSet[dd], BeqSet[dd])) # Ax <= b_set_u
b_set_l = np.concatenate(
(-np.inf * np.ones(BSet[dd].shape),
BeqSet[dd])) # Ax >= b_set_l
else:
b_set_u = BeqSet[dd]
b_set_l = BeqSet[dd]
nlp_prob = {'f': obj, 'x': x_sym, 'g': Ax_sym}
solver = ca.nlpsol('solver', 'ipopt', nlp_prob, opts_setting)
try:
result = solver(
lbg=b_set_l,
ubg=b_set_u,
)
Phat_ = result['x']
flag_ = True
except:
Phat_ = None
flag_ = False
else:
## qpoases version
qp = {}
qp['h'] = ca.DM(QSet[dd]).sparsity()
qp['a'] = ca.DM(ASet[dd]).sparsity()
options_ = {
'print_time': False,
"jit": True,
'verbose': False,
'print_problem': False,
}
solver_ = ca.conic('solver_', 'qpoases', qp, options_)
try:
result = solver_(h=QSet[dd],
g=HSet[dd],
a=ASet[dd],
uba=BSet[dd])
dP_ = result['x']
dF_ = BeqSet[dd]
Phat_ = solve(mapMat, np.concatenate((dF_, dP_)))
flag_ = True
except:
dP_ = None
flag_ = False
# ## ipopt version
# x_sym = ca.SX.sym('x', QSet[0].shape[0])
# opts_setting = {'ipopt.max_iter':100, 'ipopt.print_level':0, 'print_time':0, 'ipopt.acceptable_tol':1e-8, 'ipopt.acceptable_obj_change_tol':1e-6}
# print(HSet[dd].shape)
# obj = 0.5* ca.mtimes([x_sym.T, QSet[dd], x_sym]) + ca.mtimes([HSet[dd].reshape(1, -1), x_sym])
# Ax_sym = ca.mtimes([ASet[dd], x_sym])
# nlp_prob = {'f': obj, 'x': x_sym, 'g':Ax_sym}
# solver = ca.nlpsol('solver', 'ipopt', nlp_prob, opts_setting)
# try:
# result = solver(ubg=BSet[dd],)
# dP_ = result['x']
# # print(dP_)
# dF_ = BeqSet[dd]
# Phat_ = solve(mapMat, np.concatenate((dF_, dP_)))
# flag_ = True
# except:
# dP_ = None
# flag_ = False
if flag_:
print("success !")
P_ = np.dot(self.scaleMatBigInv(), Phat_)
self.polyCoeffSet[dd] = P_.reshape(-1, self.N + 1).T
print("done")
'''
#high-level method
# x = ca.SX.sym('x')
# y = ca.SX.sym('y')
# qp = {'x':ca.vertcat(x,y),'f':x**2+y**2,'g':x+y-10}
# S = ca.qpsol('S','qpoases',qp)
# # 这里由于期望的解是唯一的，所以初始guess不那么重要
# r = S(lbg=0)
# x_opt = r['x']
# print('x_opt: ',x_opt)
#low-level method
# 这一部分是对问题的范式转换
H = 2 * ca.DM.eye(2)
A = ca.DM.ones(1, 2)
g = ca.DM.zeros(2)
lba = 10
# 由于运用的是DM（数值形式），对于高效计算需要转换成稀疏模式（sparsity pattern)
qp = {}
qp['h'] = H.sparsity()
qp['a'] = A.sparsity()
S = ca.conic('S', 'qpoases', qp)
r = S(h=H, g=g, a=A, lba=lba)
x_opt = r['x']
print('x_opt: ', x_opt)
def setup_initial_problem_solver(self):
"""Sets up the initial problem solver, for finding slack and virtual
variables before the solver should run."""
# Test if we don't need to do anything
shortcut = self.skill_spec._has_virtual is None
shortcut = shortcut and self.skill_spec.slack_var is None
if shortcut:
# If no slack, and no virtual, nothing to initializes
self._has_initial = False
return None
# Prepare variables
time_var = self.skill_spec.time_var
robot_var = self.skill_spec.robot_var
robot_vel_var = self.skill_spec.robot_vel_var
virtual_var = self.skill_spec.virtual_var
virtual_vel_var = self.skill_spec.virtual_vel_var
input_var = self.skill_spec.input_var
slack_var = self.skill_spec.slack_var
nvirt = self.skill_spec.n_virtual_var
nslack = self.skill_spec.n_slack_var
mu = self.weight_shifter
# Prepare cost expression
opt_var = []
opt_weights = []
if nvirt > 0:
opt_var += [virtual_vel_var]
opt_weights += [mu * self.virtual_var_weights]
if nslack > 0:
opt_var += [slack_var]
opt_weights += [(1 + mu) * self.slack_var_weights]
H_expr = cs.diag(cs.vertcat(*opt_weights))
# Prepare constraints expressions
cnstr_expr_list = []
lb_cnstr_expr_list = []
ub_cnstr_expr_list = []
slack_ind = 0
virt_ind = 0
for cnstr in self.skill_spec.constraints:
found_virt = False
found_slack = False
expr_size = cnstr.expression.size()
# Look for virtual variables
if nvirt > 0:
J_virt = cs.jacobian(cnstr.expression, virtual_var)
if J_virt.nnz() > 0: # if it has non-zero elements
cnstr_expr = J_virt
found_virt = True
virt_ind += 1
else:
cnstr_expr = cs.DM.zeros((expr_size[0], nvirt))
# Setup bounds/functions for numerics
rob_der = cnstr.jtimes(robot_var, robot_vel_var)
lb_cnstr_expr = -cnstr.jacobian(time_var) - rob_der
ub_cnstr_expr = -cnstr.jacobian(time_var) - rob_der
if isinstance(cnstr, EqualityConstraint):
lb_cnstr_expr += -cs.mtimes(cnstr.gain, cnstr.expression)
ub_cnstr_expr += -cs.mtimes(cnstr.gain, cnstr.expression)
elif isinstance(cnstr, SetConstraint):
lb_cnstr_expr += cs.mtimes(cnstr.gain,
cnstr.set_min - cnstr.expression)
ub_cnstr_expr += cs.mtimes(cnstr.gain,
cnstr.set_max - cnstr.expression)
elif isinstance(cnstr, VelocityEqualityConstraint):
lb_cnstr_expr += cnstr.target
ub_cnstr_expr += cnstr.target
elif isinstance(cnstr, VelocitySetConstraint):
lb_cnstr_expr += cnstr.set_min
ub_cnstr_expr += cnstr.set_max
# Look for slack variables
if nslack > 0:
slack_mat = cs.DM.zeros((expr_size[0], nslack))
if cnstr.constraint_type == "soft":
slack_mat[:, slack_ind:slack_ind +
expr_size[0]] = -cs.DM.eye(expr_size[0])
slack_ind += expr_size[0]
found_slack = True
if nvirt > 0:
cnstr_expr = cs.horzcat(cnstr_expr, slack_mat)
else:
cnstr_expr = slack_mat
# Only care about this expression if it's actually relevant
if (found_virt or found_slack):
cnstr_expr_list += [cnstr_expr]
lb_cnstr_expr_list += [lb_cnstr_expr]
ub_cnstr_expr_list += [ub_cnstr_expr]
if slack_ind == 0 and virt_ind == 0:
# Didn't find any of them.. return
self._has_initial = False
return None
A_expr = cs.vertcat(*cnstr_expr_list)
Blb_expr = cs.vertcat(*lb_cnstr_expr_list)
Bub_expr = cs.vertcat(*ub_cnstr_expr_list)
currval_vars = [time_var, robot_var, robot_vel_var]
currval_names = ["time_var", "robot_var", "robot_vel_var"]
if self.skill_spec._has_virtual:
currval_vars += [virtual_var]
currval_names += ["virtual_var"]
if self.skill_spec._has_input:
currval_vars += [input_var]
currval_names += ["input_var"]
func_opts = self.options["function_opts"]
self._initial_problem = {
"H":
cs.Function("H_initial", currval_vars, [H_expr], currval_names,
["H"], func_opts),
"A":
cs.Function("A_initial", currval_vars, [A_expr], currval_names,
["A"], func_opts),
"Blb":
cs.Function("Blb_initial", currval_vars, [Blb_expr], currval_names,
["Blb"], func_opts),
"Bub":
cs.Function("Bub_initial", currval_vars, [Bub_expr], currval_names,
["Bub"], func_opts)
}
self.initial_solver = cs.conic("solver", self.options["solver_name"], {
"h": H_expr.sparsity(),
"a": A_expr.sparsity()
}, self.options["initial_solver_opts"])
self._has_initial = True
def qp_solve(prob, obj, p_init, x_init, y_init, lam_opt, mu_opt, case):
"""
QP solver for path-following algorithm
inputs: prob - problem description
obj - problem equations
p_init - initial parameter
x_init - initial primal variable
y_init - initial dual variable
lam_opt - Lagrange multipliers of equality and active constraints
mu_opt - Lagrange multipliers of inequality constraints
outputs: y - solution primal variable
qp_val - objective function value
qp_exit - return status of QP solver
deriv - derivatives of the problem
k_zero_tilde - active set index
k_plus_tilde - inactive set index
grad - gradient of objective function
"""
print 'Current point x:', x_init
#Importing problem to be solved
nx, np, neq, niq, name = prob()
x, p, f, f_fun, con, conf, ubx, lbx, ubg, lbg = obj(
x_init, y_init, p_init, neq, niq, nx, np)
#Deteriming constraint types
eq_con_ind = array([]) #indices of equality constraints
iq_con_ind = array([]) #indices of inequality constraints
eq_con = array([]) #equality constraints
iq_con = array([]) #inequality constraints
for i in range(0, len(lbg[0])):
if lbg[0, i] == 0:
eq_con = vertcat(eq_con, con[i])
eq_con_ind = append(eq_con_ind, i)
elif lbg[0, i] < 0:
iq_con = vertcat(iq_con, con[i])
iq_con_ind = append(iq_con_ind, i)
# print 'Equality Constraint:', eq_con
# print 'Inequality Constraint:', iq_con
# if case == 'pure-predictor':
# return qp_exit, optimal, x_qpopt, lam_qpopt, mu_qpopt
if case == 'predictor-corrector':
#Evaluating constraints at current iteration point
con_vals = conf(x_init, p_init)
#Determining which inequality constraints are active
k_plus_tilde = array([]) #active constraint
k_zero_tilde = array([]) #inactive constraint
tol = 10e-5 #tolerance
for i in range(0, len(iq_con_ind)):
if ubg[0, i] - tol <= con_vals[i] and con_vals[i] <= ubg[0,
i] + tol:
k_plus_tilde = append(k_plus_tilde, i)
else:
k_zero_tilde = append(k_zero_tilde, i)
# print 'Active constraints:', k_plus_tilde
# print 'Inactive constraints:', k_zero_tilde
# print 'Constraint values:', con_vals
nk_pt = len(k_plus_tilde) #number of active constraints
nk_zt = len(k_zero_tilde) #number of inactive constraints
#Calculating Lagrangian
lam = SX.sym('lam', neq) #Lagrangian multiplier equality constraints
mu = SX.sym('mu', niq) #Lagrangian multiplier inequality constraints
lag_f = f + mtimes(lam.T, eq_con) + mtimes(
mu.T, iq_con) #Lagrangian equation
#Calculating derivatives
g = gradient(f, x) #Derivative of objective function
g_fun = Function('g_fun', [x, p], [gradient(f, x)])
H = 2 * jacobian(gradient(lag_f, x),
x) #Second derivative of the Lagrangian
H_fun = Function('H_fun', [x, p, lam, mu],
[jacobian(jacobian(lag_f, x), x)])
if len(eq_con_ind) > 0:
deq = jacobian(eq_con, x) #Derivative of equality constraints
else:
deq = array([])
if len(iq_con_ind) > 0:
diq = jacobian(iq_con, x) #Derivative of inequality constraints
else:
diq = array([])
#Creating constraint matrices
nc = niq + neq #Total number of constraints
if (niq > 0) and (neq > 0): #Equality and inequality constraints
#this part needs to be tested
if (nk_zt > 0): #Inactive constraints exist
A = SX.zeros((nc, nx))
print deq
A[0, :] = deq #A matrix
lba = -1e16 * SX.zeros((nc, 1))
lba[0, :] = -eq_con #lower bound of A
uba = 1e16 * SX.zeros((nc, 1))
uba[0, :] = -eq_con #upper bound of A
for j in range(0, nk_pt): #adding active constraints
A[neq + j + 1, :] = diq[int(k_plus_tilde[j]), :]
lba[neq + j + 1] = -iq_con[int(k_plus_tilde[j])]
uba[neq + j + 1] = -iq_con[int(k_plus_tilde[i])]
for i in range(0, nk_zt): #adding inactive constraints
A[neq + nk_pt + i + 1, :] = diq[int(k_zero_tilde[i]), :]
uba[neq + nk_pt + i + 1] = -iq_con[int(k_zero_tilde[i])]
#inactive constraints don't have lower bounds
else: #Active constraints only
A = vertcat(deq, diq)
lba = vertcat(-eq_con, -iq_con)
uba = vertcat(-eq_con, -iq_con)
elif (niq > 0) and (neq == 0): #Inquality constraints
if (nk_zt > 0): #Inactive constraints exist
A = SX.zeros((nc, nx))
lba = -1e16 * SX.ones((nc, 1))
uba = 1e16 * SX.ones((nc, 1))
for j in range(0, nk_pt): #adding active constraints
A[j, :] = diq[int(k_plus_tilde[j]), :]
lba[j] = -iq_con[int(k_plus_tilde[j])]
uba[j] = -iq_con[int(k_plus_tilde[j])]
for i in range(0, nk_zt): #adding inactive constraints
A[nk_pt + i, :] = diq[int(k_zero_tilde[i]), :]
uba[nk_pt + i] = -iq_con[int(k_zero_tilde[i])]
#inactive constraints don't have lower bounds
else:
raw_input()
A = vertcat(deq, diq)
lba = -iq_con
uba = -iq_con
elif (niq == 0) and (neq > 0): #Equality constriants
A = deq
lba = -eq_con
uba = -eq_con
A_fun = Function('A_fun', [x, p], [A])
lba_fun = Function('lba_fun', [x, p], [lba])
uba_fun = Function('uba_fun', [x, p], [uba])
#Checking that matrices are correct sizes and types
if (H.size1() != nx) or (H.size2() != nx) or (H.is_dense() == 'False'):
#H matrix should be a sparse (nxn) and symmetrical
print(
'WARNING: H matrix is not the correct dimensions or matrix type'
)
if (g.size1() != nx) or (g.size2() != 1) or g.is_dense() == 'True':
#g matrix should be a dense (nx1)
print(
'WARNING: g matrix is not the correct dimensions or matrix type'
)
if (A.size1() !=
(neq + niq)) or (A.size2() != nx) or (A.is_dense() == 'False'):
#A should be a sparse (nc x n)
print(
'WARNING: A matrix is not the correct dimensions or matrix type'
)
if lba.size1() != (neq + niq) or (lba.size2() !=
1) or lba.is_dense() == 'False':
print(
'WARNING: lba matrix is not the correct dimensions or matrix type'
)
if uba.size1() != (neq + niq) or (uba.size2() !=
1) or uba.is_dense() == 'False':
print(
'WARNING: uba matrix is not the correct dimensions or matrix type'
)
#Evaluating QP matrices at optimal points
H_opt = H_fun(x_init, p_init, lam_opt, mu_opt)
g_opt = g_fun(x_init, p_init)
A_opt = A_fun(x_init, p_init)
lba_opt = lba_fun(x_init, p_init)
uba_opt = uba_fun(x_init, p_init)
# print 'Lower bounds', lba_opt
# print 'Upper bounds', uba_opt
# print 'Bound matrix', A_opt
#Defining QP structure
qp = {}
qp['h'] = H_opt.sparsity()
qp['a'] = A_opt.sparsity()
optimize = conic('optimize', 'qpoases', qp)
optimal = optimize(h=H_opt,
g=g_opt,
a=A_opt,
lba=lba_opt,
uba=uba_opt,
x0=x_init)
x_qpopt = optimal['x']
if x_qpopt.shape == x_init.shape:
qp_exit = 'optimal'
else:
qp_exit = ''
lag_qpopt = optimal['lam_a']
#Determing Lagrangian multipliers (lambda and mu)
lam_qpopt = zeros(
(nk_pt, 1)) #Lagrange multiplier of active constraints
mu_qpopt = zeros(
(nk_zt, 1)) #Lagrange multiplier of inactive constraints
if nk_pt > 0:
for j in range(0, len(k_plus_tilde)):
lam_qpopt[j] = lag_qpopt[int(k_plus_tilde[j])]
if nk_zt > 0:
for k in range(0, len(k_zero_tilde)):
print lag_qpopt[int(k_zero_tilde[k])]
return qp_exit, optimal, x_qpopt, lam_qpopt, mu_qpopt
lba = 0.5 * ca.DM.ones(N, 1)
uba = np.inf * ca.DM.ones(N, 1)
print("Bounds for our system:\nLBx: ", lbx, "\nUBx: ", ubx)
#### Just with slanted ground
# qp = {'h': H.sparsity()}
# S = ca.conic('hc', 'qpoases', qp)
# sol = S(h=H, g=g, lbx=lbx, ubx=ubx)
# print("Sol: \n", sol['x'])
# x_opt = sol['x']
# With A matrix
qp = {'h': H.sparsity(), 'a': A.sparsity()}
S = ca.conic('hc', 'qpoases', qp)
sol = S(h=H, g=g, a=A, lbx=lbx, ubx=ubx, lba=lba, uba=uba)
print("Sol: \n", sol['x'])
x_opt = sol['x']
Y0 = x_opt[0::2]
Z0 = x_opt[1::2]
import matplotlib.pyplot as plt
plt.plot(Y0, Z0, 'o-')
ys = ca.linspace(-2., 2., 100)
# zs = 0.5*ca.DM.ones(100,1) #+ 0.1*ys
zs = 0.5 + 0.1 * ys
plt.plot(ys, zs, '--')
plt.xlabel('y [m]')
plt.ylabel('z [m]')