def construct_upd_l(self):
# create parameters
x_i = struct_symSX(self.q_i_struct)
z_i = struct_symSX(self.q_i_struct)
z_ij = struct_symSX(self.q_ij_struct)
l_i = struct_symSX(self.q_i_struct)
l_ij = struct_symSX(self.q_ij_struct)
x_j = struct_symSX(self.q_ij_struct)
t = SX.sym('t')
T = SX.sym('T')
rho = SX.sym('rho')
t0 = t/T
inp = [x_i, z_i, z_ij, l_i, l_ij, x_j, t, T, rho]
# put symbols in SX structs (necessary for transformation)
x_i = self.q_i_struct(x_i)
z_i = self.q_i_struct(z_i)
z_ij = self.q_ij_struct(z_ij)
l_i = self.q_i_struct(l_i)
l_ij = self.q_ij_struct(l_ij)
x_j = self.q_ij_struct(x_j)
# transform spline variables: only consider future piece of spline
tf = lambda cfs, basis: shift_knot1_fwd(cfs, basis, t0)
self._transform_spline([x_i, z_i, l_i], tf, self.q_i)
self._transform_spline([x_j, z_ij, l_ij], tf, self.q_ij)
# update lambda
l_i_new = self.q_i_struct(l_i.cat + rho*(x_i.cat - z_i.cat))
l_ij_new = self.q_ij_struct(l_ij.cat + rho*(x_j.cat - z_ij.cat))
tf = lambda cfs, basis: shift_knot1_bwd(cfs, basis, t0)
self._transform_spline(l_i_new, tf, self.q_i)
self._transform_spline(l_ij_new, tf, self.q_ij)
out = [l_i_new, l_ij_new]
# create problem
prob, compile_time = self.father.create_function(
'upd_l', inp, out, self.options)
self.problem_upd_l = prob
def create_nlp(self, var, par, obj, con, options):
jit = options['codegen']
if options['verbose'] >= 2:
print 'Building nlp ... ',
if jit['jit']:
print('[jit compilation with flags %s]' %
(','.join(jit['jit_options']['flags']))),
t0 = time.time()
nlp = MXFunction('nlp', nlpIn(x=var, p=par), nlpOut(f=obj, g=con))
solver = NlpSolver('solver', 'ipopt', nlp, options['solver'])
grad_f, jac_g = nlp.gradient('x', 'f'), nlp.jacobian('x', 'g')
hess_lag = solver.hessLag()
var, par = struct_symSX(var), struct_symSX(par)
nlp = SXFunction('nlp', [var, par], nlp([var, par]), jit)
grad_f = SXFunction('grad_f', [var, par], grad_f([var, par]), jit)
jac_g = SXFunction('jac_g', [var, par], jac_g([var, par]), jit)
lam_f, lam_g = SX.sym('lam_f'), SX.sym('lam_g', con.size)
hess_lag = SXFunction('hess_lag', [var, par, lam_f, lam_g],
hess_lag([var, par, lam_f, lam_g]), jit)
options['solver'].update({'grad_f': grad_f, 'jac_g': jac_g,
'hess_lag': hess_lag})
problem = NlpSolver('solver', 'ipopt', nlp, options['solver'])
t1 = time.time()
if options['verbose'] >= 2:
print 'in %5f s' % (t1-t0)
return problem, (t1-t0)
def chen_model():
""" The following ODE model comes from Chen1998. """
nx, nu = (2, 1)
x = SX.sym('x', nx)
u = SX.sym('u', nu)
mu = 0.5
rhs = vertcat(x[1] + u*(mu + (1.-mu)*x[0]), x[0] + u*(mu - 4.*(1.-mu)*x[1]))
return Function('chen', [x, u], [rhs]), nx, nu
def shift_knot1_bwd(cfs, basis, t_shift):
if isinstance(cfs, (SX, MX)):
cfs_sym = SX.sym('cfs', cfs.shape)
t_shift_sym = SX.sym('t_shift')
T, Tinv = shiftfirstknot_T(basis, t_shift_sym, inverse=True)
cfs2_sym = mul(Tinv, cfs_sym)
fun = SXFunction('fun', [cfs_sym, t_shift_sym], [cfs2_sym])
return fun([cfs, t_shift])[0]
else:
T, Tinv = shiftfirstknot_T(basis, t_shift, inverse=True)
return Tinv.dot(cfs)
def shift_knot1_bwd(cfs, basis, t_shift):
if isinstance(cfs, (SX, MX)):
cfs_sym = SX.sym('cfs', cfs.shape)
t_shift_sym = SX.sym('t_shift')
_, Tinv = shiftfirstknot_T(basis, t_shift_sym, inverse=True)
cfs2_sym = mtimes(Tinv, cfs_sym)
fun = Function('fun', [cfs_sym, t_shift_sym], [cfs2_sym]).expand()
return fun(cfs, t_shift)
else:
_, Tinv = shiftfirstknot_T(basis, t_shift, inverse=True)
return Tinv.dot(cfs)
def construct_upd_z(self):
# check if we have linear equality constraints
self._lineq_updz, A, b = self._check_for_lineq()
if not self._lineq_updz:
self._construct_upd_z_nlp()
x_i = struct_symSX(self.q_i_struct)
x_j = struct_symSX(self.q_ij_struct)
l_i = struct_symSX(self.q_i_struct)
l_ij = struct_symSX(self.q_ij_struct)
t = SX.sym('t')
T = SX.sym('T')
rho = SX.sym('rho')
par = struct_symSX(self.par_struct)
inp = [x_i.cat, l_i.cat, l_ij.cat, x_j.cat, t, T, rho, par.cat]
t0 = t/T
# put symbols in SX structs (necessary for transformation)
x_i = self.q_i_struct(x_i)
x_j = self.q_ij_struct(x_j)
l_i = self.q_i_struct(l_i)
l_ij = self.q_ij_struct(l_ij)
# transform spline variables: only consider future piece of spline
tf = lambda cfs, basis: shift_knot1_fwd(cfs, basis, t0)
self._transform_spline([x_i, l_i], tf, self.q_i)
self._transform_spline([x_j, l_ij], tf, self.q_ij)
# fill in parameters
A = A([par])[0]
b = b([par])[0]
# build KKT system
E = rho*SX.eye(A.shape[1])
l, x = vertcat([l_i.cat, l_ij.cat]), vertcat([x_i.cat, x_j.cat])
f = -(l + rho*x)
G = vertcat([horzcat([E, A.T]),
horzcat([A, SX.zeros(A.shape[0], A.shape[0])])])
h = vertcat([-f, b])
z = solve(G, h)
l_qi = self.q_i_struct.shape[0]
l_qij = self.q_ij_struct.shape[0]
z_i_new = self.q_i_struct(z[:l_qi])
z_ij_new = self.q_ij_struct(z[l_qi:l_qi+l_qij])
# transform back
tf = lambda cfs, basis: shift_knot1_bwd(cfs, basis, t0)
self._transform_spline(z_i_new, tf, self.q_i)
self._transform_spline(z_ij_new, tf, self.q_ij)
out = [z_i_new.cat, z_ij_new.cat]
# create problem
prob, compile_time = self.father.create_function(
'upd_z', inp, out, self.options)
self.problem_upd_z = prob
def pendulum_model():
""" Nonlinear inverse pendulum model. """
M = 1 # mass of the cart [kg]
m = 0.1 # mass of the ball [kg]
g = 9.81 # gravity constant [m/s^2]
l = 0.8 # length of the rod [m]
p = SX.sym('p') # horizontal displacement [m]
theta = SX.sym('theta') # angle with the vertical [rad]
v = SX.sym('v') # horizontal velocity [m/s]
omega = SX.sym('omega') # angular velocity [rad/s]
F = SX.sym('F') # horizontal force [N]
ode_rhs = vertcat(v,
omega,
(- l*m*sin(theta)*omega**2 + F + g*m*cos(theta)*sin(theta))/(M + m - m*cos(theta)**2),
(- l*m*cos(theta)*sin(theta)*omega**2 + F*cos(theta) + g*m*sin(theta) + M*g*sin(theta))/(l*(M + m - m*cos(theta)**2)))
nx = 4
# for IRK
xdot = SX.sym('xdot', nx, 1)
z = SX.sym('z',0,1)
return (Function('pendulum', [vertcat(p, theta, v, omega), F], [ode_rhs], ['x', 'u'], ['xdot']),
nx, # number of states
1, # number of controls
Function('impl_pendulum', [vertcat(p, theta, v, omega), F, xdot, z], [ode_rhs-xdot],
['x', 'u','xdot','z'], ['rhs']))
def deform(
vertices_val,
adjacency,
target_to_base_indices,
targets_val,
):
n_vertices = vertices_val.shape[1]
n_targets = targets_val.shape[1]
assert vertices_val.shape[0] == 3
assert targets_val.shape[0] == 3
assert len(adjacency) == n_vertices
assert len(target_to_base_indices) == n_targets
start = time.time()
old_vertices = SX.sym("old_vertices", 3, n_vertices)
targets = SX.sym("targets", 3, n_targets)
vertices = SX.sym("vertices", 3, n_vertices)
rotations = SX.sym("rotations", 3, 3 * n_vertices)
vertices_to_fit = vertices[:, target_to_base_indices]
start_data = time.time()
data = (vertices_to_fit - targets).reshape((-1, 1))
print(f"data elapsed {time.time() - start_data}")
start_arap = time.time()
arap_residual = arap.make_rot_arap_residuals(adjacency, old_vertices, rotations, vertices)
print(f"arap elapsed {time.time() - start_arap}")
start_rigid = time.time()
rigid = arap.make_rigid_residuals(rotations)
print(f"rigid elapsed {time.time() - start_rigid}")
residuals = casadi.vertcat(
data,
arap_residual,
10.0 * rigid
)
variables = casadi.vertcat(
rotations.reshape((-1, 1)),
vertices.reshape((-1, 1))
)
start_jac = time.time()
jac = casadi.jacobian(residuals, variables)
print(f"jac elapsed {time.time() - start_jac}")
print("jac.shape", jac.shape, "jac.nnz()", jac.nnz())
fixed_values = [
old_vertices,
targets,
]
start_res = time.time()
residual_func = Function("res_func", [variables] + fixed_values, [residuals])
print(f"res elapsed {time.time() - start_res}")
start_jac_func = time.time()
jac_func = Function("jac_func", [variables] + fixed_values, [jac])
print(f"jac_func elapsed {time.time() - start_jac_func}")
print(f"construction elapsed {time.time() - start}")
exit(0)
def compute_residuals_and_jac(x):
start = time.time()
residuals_val = residual_func(x, vertices_val, targets_val).toarray()
print(f"Residual elapsed {time.time() - start}")
start = time.time()
jacobian_val = jac_func(x, vertices_val, targets_val)
print(f"Jacobian elapsed {time.time() - start}")
jacobian_val = jacobian_val.sparse()
return residuals_val, jacobian_val
init_rot = np.hstack([np.eye(3)] * n_vertices)
init_vertices = vertices_val
init_vars = np.hstack((
init_rot.T.reshape(-1),
init_vertices.reshape(-1)
))
res = perform_gauss_newton(init_vars, compute_residuals_and_jac, 50, dumping_factor=0.5)
new_vertices = res[9 * n_vertices:].reshape(-1, 3).T
return new_vertices
def __set_costs(self, ocp):
if ocp.nb_phases != 1:
raise NotImplementedError("ACADOS with more than one phase is not implemented yet.")
# costs handling in self.acados_ocp
self.y_ref = []
self.y_ref_end = []
self.lagrange_costs = SX()
self.mayer_costs = SX()
self.W = np.zeros((0, 0))
self.W_e = np.zeros((0, 0))
if self.acados_ocp.cost.cost_type == "LINEAR_LS":
raise RuntimeError("LINEAR_LS is not interfaced yet.")
elif self.acados_ocp.cost.cost_type == "NONLINEAR_LS":
for i in range(ocp.nb_phases):
for j, J in enumerate(ocp.nlp[i].J):
if J[0]["objective"].type.get_type() == ObjectiveFunction.LagrangeFunction:
self.lagrange_costs = vertcat(self.lagrange_costs, J[0]["val"].reshape((-1, 1)))
self.W = linalg.block_diag(self.W, np.diag([J[0]["objective"].weight] * J[0]["val"].numel()))
if J[0]["target"] is not None:
self.y_ref.append([J_tp["target"].T.reshape((-1, 1)) for J_tp in J])
else:
self.y_ref.append([np.zeros((J_tp["val"].numel(), 1)) for J_tp in J])
elif J[0]["objective"].type.get_type() == ObjectiveFunction.MayerFunction:
mayer_func_tp = Function(f"cas_mayer_func_{i}_{j}", [ocp.nlp[i].X[-1]], [J[0]["val"]])
self.W_e = linalg.block_diag(
self.W_e, np.diag([J[0]["objective"].weight] * J[0]["val"].numel())
)
self.mayer_costs = vertcat(self.mayer_costs, mayer_func_tp(ocp.nlp[i].X[0]))
if J[0]["target"] is not None:
self.y_ref_end.append(
[J[0]["target"]] if isinstance(J[0]["target"], (int, float)) else J[0]["target"]
)
else:
self.y_ref_end.append([0] * (J[0]["val"].numel()))
else:
raise RuntimeError("The objective function is not Lagrange nor Mayer.")
# parameter as mayer function
# IMPORTANT: it is considered that only parameters are stored in ocp.J, for now.
if self.params:
for j, J in enumerate(ocp.J):
mayer_func_tp = Function(f"cas_J_mayer_func_{i}_{j}", [ocp.nlp[i].X[-1]], [J[0]["val"]])
self.W_e = linalg.block_diag(
self.W_e, np.diag(([J[0]["objective"].weight] * J[0]["val"].numel()))
)
self.mayer_costs = vertcat(self.mayer_costs, mayer_func_tp(ocp.nlp[i].X[0]))
if J[0]["target"] is not None:
self.y_ref_end.append(
[J[0]["target"]] if isinstance(J[0]["target"], (int, float)) else J[0]["target"]
)
else:
self.y_ref_end.append([0] * (J[0]["val"].numel()))
# Set costs
self.acados_ocp.model.cost_y_expr = self.lagrange_costs if self.lagrange_costs.numel() else SX(1, 1)
self.acados_ocp.model.cost_y_expr_e = self.mayer_costs if self.mayer_costs.numel() else SX(1, 1)
# Set dimensions
self.acados_ocp.dims.ny = self.acados_ocp.model.cost_y_expr.shape[0]
self.acados_ocp.dims.ny_e = self.acados_ocp.model.cost_y_expr_e.shape[0]
# Set weight
self.acados_ocp.cost.W = np.zeros((1, 1)) if self.W.shape == (0, 0) else self.W
self.acados_ocp.cost.W_e = np.zeros((1, 1)) if self.W_e.shape == (0, 0) else self.W_e
# Set target shape
self.acados_ocp.cost.yref = np.zeros((self.acados_ocp.cost.W.shape[0],))
self.acados_ocp.cost.yref_e = np.zeros((self.acados_ocp.cost.W_e.shape[0],))
elif self.acados_ocp.cost.cost_type == "EXTERNAL":
raise RuntimeError("External is not interfaced yet, please use NONLINEAR_LS")
else:
raise RuntimeError("Available acados cost type: 'LINEAR_LS', 'NONLINEAR_LS' and 'EXTERNAL'.")
def gen_lat_model():
model = AcadosModel()
model.name = 'lat'
# set up states & controls
x_ego = SX.sym('x_ego')
y_ego = SX.sym('y_ego')
psi_ego = SX.sym('psi_ego')
curv_ego = SX.sym('curv_ego')
v_ego = SX.sym('v_ego')
rotation_radius = SX.sym('rotation_radius')
model.x = vertcat(x_ego, y_ego, psi_ego, curv_ego, v_ego, rotation_radius)
# controls
curv_rate = SX.sym('curv_rate')
model.u = vertcat(curv_rate)
# xdot
x_ego_dot = SX.sym('x_ego_dot')
y_ego_dot = SX.sym('y_ego_dot')
psi_ego_dot = SX.sym('psi_ego_dot')
curv_ego_dot = SX.sym('curv_ego_dot')
v_ego_dot = SX.sym('v_ego_dot')
rotation_radius_dot = SX.sym('rotation_radius_dot')
model.xdot = vertcat(x_ego_dot, y_ego_dot, psi_ego_dot, curv_ego_dot,
v_ego_dot, rotation_radius_dot)
# dynamics model
f_expl = vertcat(
v_ego * cos(psi_ego) - rotation_radius * sin(psi_ego) *
(v_ego * curv_ego),
v_ego * sin(psi_ego) + rotation_radius * cos(psi_ego) *
(v_ego * curv_ego), v_ego * curv_ego, curv_rate, 0.0, 0.0)
model.f_impl_expr = model.xdot - f_expl
model.f_expl_expr = f_expl
return model
h_lb = 0
eta_ub = 200
eta_lb = 0
# Abtastzeit und Prediction horizon
T = 0.1
N_pred = 3
# Zielruhelage von den Zustaenden
# Referenz von h, h_p, eta
state_r = [0.8, 0, 48]
# activate_ips_on_exception()
# IPS()
# Zustaende als Symbol definieren
h = SX.sym('h')
h_p = SX.sym('h_p')
eta = SX.sym('eta')
# Zustandsvektor
states = vertcat(h, h_p, eta)
# print(states.shape)
# kriegen die Anzahl der Zeiler("shape" ist ein Tuple)
n_states = states.shape[0] # p 22.
# Eingang symbolisch definieren
u_pwm = SX.sym('u_pwm')
# Eingangsverktor
controls = vertcat(u_pwm)
# Eingangsanzahl
n_controls = controls.shape[0]
# Zustandsdarstellung
def __set_costs(self, ocp):
if ocp.nb_phases != 1:
raise NotImplementedError(
"ACADOS with more than one phase is not implemented yet.")
# costs handling in self.acados_ocp
self.y_ref = []
self.y_ref_end = []
self.lagrange_costs = SX()
self.mayer_costs = SX()
self.W = np.zeros((0, 0))
self.W_e = np.zeros((0, 0))
ctrl_objs = [
PenaltyType.MINIMIZE_TORQUE,
PenaltyType.MINIMIZE_MUSCLES_CONTROL,
PenaltyType.MINIMIZE_ALL_CONTROLS,
]
state_objs = [
PenaltyType.MINIMIZE_STATE,
]
if self.acados_ocp.cost.cost_type == "LINEAR_LS":
self.Vu = np.array([], dtype=np.int64).reshape(0, ocp.nlp[0].nu)
self.Vx = np.array([], dtype=np.int64).reshape(0, ocp.nlp[0].nx)
self.Vxe = np.array([], dtype=np.int64).reshape(0, ocp.nlp[0].nx)
for i in range(ocp.nb_phases):
for j, J in enumerate(ocp.nlp[i].J):
if J[0]["objective"].type.get_type(
) == ObjectiveFunction.LagrangeFunction:
if J[0]["objective"].type.value[0] in ctrl_objs:
index = (J[0]["objective"].index
if J[0]["objective"].index else list(
np.arange(ocp.nlp[0].nu)))
vu = np.zeros(ocp.nlp[0].nu)
vu[index] = 1.0
self.Vu = np.vstack((self.Vu, np.diag(vu)))
self.Vx = np.vstack(
(self.Vx,
np.zeros((ocp.nlp[0].nu, ocp.nlp[0].nx))))
self.W = linalg.block_diag(
self.W,
np.diag([J[0]["objective"].weight] *
ocp.nlp[0].nu))
if J[0]["target"] is not None:
y_tp = np.zeros((ocp.nlp[0].nu, 1))
y_ref_tp = []
for J_tp in J:
y_tp[index] = J_tp["target"].T.reshape(
(-1, 1))
y_ref_tp.append(y_tp)
self.y_ref.append(y_ref_tp)
else:
self.y_ref.append([
np.zeros((ocp.nlp[0].nu, 1)) for J_tp in J
])
elif J[0]["objective"].type.value[0] in state_objs:
index = (J[0]["objective"].index
if J[0]["objective"].index else list(
np.arange(ocp.nlp[0].nx)))
vx = np.zeros(ocp.nlp[0].nx)
vx[index] = 1.0
self.Vx = np.vstack((self.Vx, np.diag(vx)))
self.Vu = np.vstack(
(self.Vu,
np.zeros((ocp.nlp[0].nx, ocp.nlp[0].nu))))
self.W = linalg.block_diag(
self.W,
np.diag([J[0]["objective"].weight] *
ocp.nlp[0].nx))
if J[0]["target"] is not None:
y_ref_tp = []
for J_tp in J:
y_tp = np.zeros((ocp.nlp[0].nx, 1))
y_tp[index] = J_tp["target"].T.reshape(
(-1, 1))
y_ref_tp.append(y_tp)
self.y_ref.append(y_ref_tp)
else:
self.y_ref.append([
np.zeros((ocp.nlp[0].nx, 1)) for J_tp in J
])
else:
raise RuntimeError(
f"{J[0]['objective'].type.name} is an incompatible objective term with "
f"LINEAR_LS cost type")
# Deal with last node to match ipopt formulation
if J[0]["objective"].node[0].value == "all" and len(
J) > ocp.nlp[0].ns:
index = (J[0]["objective"].index
if J[0]["objective"].index else list(
np.arange(ocp.nlp[0].nx)))
vxe = np.zeros(ocp.nlp[0].nx)
vxe[index] = 1.0
self.Vxe = np.vstack((self.Vxe, np.diag(vxe)))
self.W_e = linalg.block_diag(
self.W_e,
np.diag([J[0]["objective"].weight] *
ocp.nlp[0].nx))
if J[0]["target"] is not None:
y_tp = np.zeros((ocp.nlp[0].nx, 1))
y_tp[index] = J[-1]["target"].T.reshape(
(-1, 1))
self.y_ref_end.append(y_tp)
else:
self.y_ref_end.append(
np.zeros((ocp.nlp[0].nx, 1)))
elif J[0]["objective"].type.get_type(
) == ObjectiveFunction.MayerFunction:
if J[0]["objective"].type.value[0] in state_objs:
index = (J[0]["objective"].index
if J[0]["objective"].index else list(
np.arange(ocp.nlp[0].nx)))
vxe = np.zeros(ocp.nlp[0].nx)
vxe[index] = 1.0
self.Vxe = np.vstack((self.Vxe, np.diag(vxe)))
self.W_e = linalg.block_diag(
self.W_e,
np.diag([J[0]["objective"].weight] *
ocp.nlp[0].nx))
if J[0]["target"] is not None:
y_tp = np.zeros((ocp.nlp[0].nx, 1))
y_tp[index] = J[-1]["target"].T.reshape(
(-1, 1))
self.y_ref_end.append(y_tp)
else:
self.y_ref_end.append(
np.zeros((ocp.nlp[0].nx, 1)))
else:
raise RuntimeError(
f"{J[0]['objective'].type.name} is an incompatible objective term "
f"with LINEAR_LS cost type")
else:
raise RuntimeError(
"The objective function is not Lagrange nor Mayer."
)
if self.params:
raise RuntimeError(
"Params not yet handled with LINEAR_LS cost type")
# Set costs
self.acados_ocp.cost.Vx = self.Vx if self.Vx.shape[0] else np.zeros(
(0, 0))
self.acados_ocp.cost.Vu = self.Vu if self.Vu.shape[0] else np.zeros(
(0, 0))
self.acados_ocp.cost.Vx_e = self.Vxe if self.Vxe.shape[
0] else np.zeros((0, 0))
# Set dimensions
self.acados_ocp.dims.ny = sum(
[len(data[0]) for data in self.y_ref])
self.acados_ocp.dims.ny_e = sum(
[len(data) for data in self.y_ref_end])
# Set weight
self.acados_ocp.cost.W = self.W
self.acados_ocp.cost.W_e = self.W_e
# Set target shape
self.acados_ocp.cost.yref = np.zeros(
(self.acados_ocp.cost.W.shape[0], ))
self.acados_ocp.cost.yref_e = np.zeros(
(self.acados_ocp.cost.W_e.shape[0], ))
elif self.acados_ocp.cost.cost_type == "NONLINEAR_LS":
for i in range(ocp.nb_phases):
for j, J in enumerate(ocp.nlp[i].J):
if J[0]["objective"].type.get_type(
) == ObjectiveFunction.LagrangeFunction:
self.lagrange_costs = vertcat(
self.lagrange_costs, J[0]["val"].reshape((-1, 1)))
self.W = linalg.block_diag(
self.W,
np.diag([J[0]["objective"].weight] *
J[0]["val"].numel()))
if J[0]["target"] is not None:
self.y_ref.append([
J_tp["target"].T.reshape((-1, 1)) for J_tp in J
])
else:
self.y_ref.append([
np.zeros((J_tp["val"].numel(), 1))
for J_tp in J
])
# Deal with last node to match ipopt formulation
if J[0]["objective"].node[0].value == "all" and len(
J) > ocp.nlp[0].ns:
mayer_func_tp = Function(f"cas_mayer_func_{i}_{j}",
[ocp.nlp[i].X[-1]],
[J[0]["val"]])
self.W_e = linalg.block_diag(
self.W_e,
np.diag([J[0]["objective"].weight] *
J[0]["val"].numel()))
self.mayer_costs = vertcat(
self.mayer_costs,
mayer_func_tp(ocp.nlp[i].X[0]).reshape(
(-1, 1)))
if J[0]["target"] is not None:
self.y_ref_end.append(
J[-1]["target"].T.reshape((-1, 1)))
else:
self.y_ref_end.append(
np.zeros((J[-1]["val"].numel(), 1)))
elif J[0]["objective"].type.get_type(
) == ObjectiveFunction.MayerFunction:
mayer_func_tp = Function(f"cas_mayer_func_{i}_{j}",
[ocp.nlp[i].X[-1]],
[J[0]["val"]])
self.W_e = linalg.block_diag(
self.W_e,
np.diag([J[0]["objective"].weight] *
J[0]["val"].numel()))
self.mayer_costs = vertcat(
self.mayer_costs,
mayer_func_tp(ocp.nlp[i].X[0]).reshape((-1, 1)))
if J[0]["target"] is not None:
self.y_ref_end.append(J[0]["target"].T.reshape(
(-1, 1)))
else:
self.y_ref_end.append(
np.zeros((J[0]["val"].numel(), 1)))
else:
raise RuntimeError(
"The objective function is not Lagrange nor Mayer."
)
# parameter as mayer function
# IMPORTANT: it is considered that only parameters are stored in ocp.J, for now.
if self.params:
for j, J in enumerate(ocp.J):
mayer_func_tp = Function(f"cas_J_mayer_func_{i}_{j}",
[ocp.nlp[i].X[-1]],
[J[0]["val"]])
self.W_e = linalg.block_diag(
self.W_e,
np.diag(([J[0]["objective"].weight] *
J[0]["val"].numel())))
self.mayer_costs = vertcat(
self.mayer_costs,
mayer_func_tp(ocp.nlp[i].X[0]).reshape((-1, 1)))
if J[0]["target"] is not None:
self.y_ref_end.append(J[0]["target"].T.reshape(
(-1, 1)))
else:
self.y_ref_end.append(
np.zeros((J[0]["val"].numel(), 1)))
# Set costs
self.acados_ocp.model.cost_y_expr = self.lagrange_costs if self.lagrange_costs.numel(
) else SX(1, 1)
self.acados_ocp.model.cost_y_expr_e = self.mayer_costs if self.mayer_costs.numel(
) else SX(1, 1)
# Set dimensions
self.acados_ocp.dims.ny = self.acados_ocp.model.cost_y_expr.shape[
0]
self.acados_ocp.dims.ny_e = self.acados_ocp.model.cost_y_expr_e.shape[
0]
# Set weight
self.acados_ocp.cost.W = np.zeros(
(1, 1)) if self.W.shape == (0, 0) else self.W
self.acados_ocp.cost.W_e = np.zeros(
(1, 1)) if self.W_e.shape == (0, 0) else self.W_e
# Set target shape
self.acados_ocp.cost.yref = np.zeros(
(self.acados_ocp.cost.W.shape[0], ))
self.acados_ocp.cost.yref_e = np.zeros(
(self.acados_ocp.cost.W_e.shape[0], ))
elif self.acados_ocp.cost.cost_type == "EXTERNAL":
raise RuntimeError(
"EXTERNAL is not interfaced yet, please use NONLINEAR_LS")
else:
raise RuntimeError(
"Available acados cost type: 'LINEAR_LS', 'NONLINEAR_LS' and 'EXTERNAL'."
)
def __set_constrs(self, ocp):
# constraints handling in self.acados_ocp
u_min = np.array(ocp.nlp[0].u_bounds.min)
u_max = np.array(ocp.nlp[0].u_bounds.max)
x_min = np.array(ocp.nlp[0].x_bounds.min)
x_max = np.array(ocp.nlp[0].x_bounds.max)
self.all_constr = SX()
self.end_constr = SX()
##TODO:change for more node flexibility on bounds
self.all_g_bounds = Bounds(interpolation=InterpolationType.CONSTANT)
self.end_g_bounds = Bounds(interpolation=InterpolationType.CONSTANT)
for i in range(ocp.nb_phases):
for g, G in enumerate(ocp.nlp[i].g):
if not G:
continue
if G[0]["constraint"].node[0] is Node.ALL:
self.all_constr = vertcat(self.all_constr,
G[0]["val"].reshape((-1, 1)))
self.all_g_bounds.concatenate(G[0]["bounds"])
if len(G) > ocp.nlp[0].ns:
constr_end_func_tp = Function(
f"cas_constr_end_func_{i}_{g}", [ocp.nlp[i].X[-1]],
[G[0]["val"]])
self.end_constr = vertcat(
self.end_constr,
constr_end_func_tp(ocp.nlp[i].X[0]).reshape(
(-1, 1)))
self.end_g_bounds.concatenate(G[0]["bounds"])
elif G[0]["constraint"].node[0] is Node.END:
constr_end_func_tp = Function(
f"cas_constr_end_func_{i}_{g}", [ocp.nlp[i].X[-1]],
[G[0]["val"]])
self.end_constr = vertcat(
self.end_constr,
constr_end_func_tp(ocp.nlp[i].X[0]).reshape((-1, 1)))
self.end_g_bounds.concatenate(G[0]["bounds"])
else:
raise RuntimeError(
"Except for states and controls, Acados solver only handles constraints on last or all nodes."
)
self.acados_model.con_h_expr = self.all_constr
self.acados_model.con_h_expr_e = self.end_constr
if not np.all(np.all(u_min.T == u_min.T[0, :], axis=0)):
raise NotImplementedError(
"u_bounds min must be the same at each shooting point with ACADOS"
)
if not np.all(np.all(u_max.T == u_max.T[0, :], axis=0)):
raise NotImplementedError(
"u_bounds max must be the same at each shooting point with ACADOS"
)
if (not np.isfinite(u_min).all() or not np.isfinite(x_min).all()
or not np.isfinite(u_max).all()
or not np.isfinite(x_max).all()):
raise NotImplementedError(
"u_bounds and x_bounds cannot be set to infinity in ACADOS. Consider changing it"
"to a big value instead.")
# setup state constraints
self.x_bound_max = np.ndarray((self.acados_ocp.dims.nx, 3))
self.x_bound_min = np.ndarray((self.acados_ocp.dims.nx, 3))
param_bounds_max = []
param_bounds_min = []
if self.params:
param_bounds_max = np.concatenate(
[self.params[key].bounds.max for key in self.params.keys()])[:,
0]
param_bounds_min = np.concatenate(
[self.params[key].bounds.min for key in self.params.keys()])[:,
0]
for i in range(3):
self.x_bound_max[:, i] = np.concatenate(
(param_bounds_max, np.array(ocp.nlp[0].x_bounds.max[:, i])))
self.x_bound_min[:, i] = np.concatenate(
(param_bounds_min, np.array(ocp.nlp[0].x_bounds.min[:, i])))
# setup control constraints
self.acados_ocp.constraints.lbu = np.array(ocp.nlp[0].u_bounds.min[:,
0])
self.acados_ocp.constraints.ubu = np.array(ocp.nlp[0].u_bounds.max[:,
0])
self.acados_ocp.constraints.idxbu = np.array(
range(self.acados_ocp.dims.nu))
self.acados_ocp.dims.nbu = self.acados_ocp.dims.nu
# initial state constraints
self.acados_ocp.constraints.lbx_0 = self.x_bound_min[:, 0]
self.acados_ocp.constraints.ubx_0 = self.x_bound_max[:, 0]
self.acados_ocp.constraints.idxbx_0 = np.array(
range(self.acados_ocp.dims.nx))
self.acados_ocp.dims.nbx_0 = self.acados_ocp.dims.nx
# setup path state constraints
self.acados_ocp.constraints.Jbx = np.eye(self.acados_ocp.dims.nx)
self.acados_ocp.constraints.lbx = self.x_bound_min[:, 1]
self.acados_ocp.constraints.ubx = self.x_bound_max[:, 1]
self.acados_ocp.constraints.idxbx = np.array(
range(self.acados_ocp.dims.nx))
self.acados_ocp.dims.nbx = self.acados_ocp.dims.nx
# setup terminal state constraints
self.acados_ocp.constraints.Jbx_e = np.eye(self.acados_ocp.dims.nx)
self.acados_ocp.constraints.lbx_e = self.x_bound_min[:, -1]
self.acados_ocp.constraints.ubx_e = self.x_bound_max[:, -1]
self.acados_ocp.constraints.idxbx_e = np.array(
range(self.acados_ocp.dims.nx))
self.acados_ocp.dims.nbx_e = self.acados_ocp.dims.nx
# setup algebraic constraint
self.acados_ocp.constraints.lh = np.array(self.all_g_bounds.min[:, 0])
self.acados_ocp.constraints.uh = np.array(self.all_g_bounds.max[:, 0])
# setup terminal algebraic constraint
self.acados_ocp.constraints.lh_e = np.array(self.end_g_bounds.min[:,
0])
self.acados_ocp.constraints.uh_e = np.array(self.end_g_bounds.max[:,
0])
def __init__(self, ocp, **solver_options):
"""
Parameters
----------
ocp: OptimalControlProgram
A reference to the current OptimalControlProgram
solver_options: dict
The options to pass to the solver
"""
if not isinstance(ocp.cx(), SX):
raise RuntimeError(
"CasADi graph must be SX to be solved with ACADOS. Please set use_sx to True in OCP"
)
super().__init__(ocp)
# If Acados is installed using the acados_install.sh file, you probably can leave this to unset
acados_path = ""
if "acados_dir" in solver_options:
acados_path = solver_options["acados_dir"]
self.acados_ocp = AcadosOcp(acados_path=acados_path)
self.acados_model = AcadosModel()
if "cost_type" in solver_options:
self.__set_cost_type(solver_options["cost_type"])
else:
self.__set_cost_type()
if "constr_type" in solver_options:
self.__set_constr_type(solver_options["constr_type"])
else:
self.__set_constr_type()
self.lagrange_costs = SX()
self.mayer_costs = SX()
self.y_ref = []
self.y_ref_end = []
self.nparams = 0
self.params_initial_guess = None
self.params_bounds = None
self.__acados_export_model(ocp)
self.__prepare_acados(ocp)
self.ocp_solver = None
self.W = np.zeros((0, 0))
self.W_e = np.zeros((0, 0))
self.status = None
self.out = {}
self.all_constr = None
self.end_constr = SX()
self.all_g_bounds = Bounds(interpolation=InterpolationType.CONSTANT)
self.end_g_bounds = Bounds(interpolation=InterpolationType.CONSTANT)
self.x_bound_max = np.ndarray((self.acados_ocp.dims.nx, 3))
self.x_bound_min = np.ndarray((self.acados_ocp.dims.nx, 3))
self.Vu = np.array([],
dtype=np.int64).reshape(0,
ocp.nlp[0].controls.shape)
self.Vx = np.array([], dtype=np.int64).reshape(0,
ocp.nlp[0].states.shape)
self.Vxe = np.array([],
dtype=np.int64).reshape(0, ocp.nlp[0].states.shape)
from casadi import SX, DM, vertcat, reshape, Function, nlpsol, inf, norm_2, sqrt, gradient, dot
import matplotlib.pyplot as plt
import random
if __name__ == '__main__':
# Parameter konfiguration
A_B = 2.8274e-3 # [m**2]
A_SP = 0.4299e-3 # [m**2]
m = 2.8e-3 # [kg]
g = 9.81 # [m/(s**2)]
T_M = 0.57 # [s]
k_M = 0.31 # [s**-1]
k_V = 6e-5 # [m**3]
k_L = 2.18e-4 # [kg/m]
eta_0 = 1900 / 60 # / 60 * 2 * pi
A_d = SX([[1, 0.1, 0], [0, -0.14957, 0.024395], [0, 0, 0.82456]])
B_d = SX([[0], [0], [0.054386]])
T = 0.1
h = SX.sym('h')
h_p = SX.sym('h_p')
eta = SX.sym('eta')
states = vertcat(h, h_p, eta)
u_pwm = SX.sym('u_pwm')
controls = vertcat(u_pwm)
rhs = vertcat(h_p,
k_L / m * ((k_V * (eta + eta_0) - A_B * h_p) / A_SP)**2 - g,
-1 / T_M * eta + k_M / T_M * u_pwm)
f = Function('f', [states, controls], [rhs * T + states])
def solve(problem_spec):
# problem_spec is dummy
t = sp.Symbol('t') # time variable
np = 2
nq = 2
ns = 2
n = np + nq + ns
p1, p2 = pp = st.symb_vector("p1:{0}".format(np + 1))
q1, q2 = qq = st.symb_vector("q1:{0}".format(nq + 1))
s1, s2 = ss = st.symb_vector("s1:{0}".format(ns + 1))
ttheta = st.row_stack(qq[0], pp[0], ss[0], qq[1], pp[1], ss[1])
qdot1, pdot1, sdot1, qdot2, pdot2, sdot2 = tthetad = st.time_deriv(ttheta, ttheta)
tthetadd = st.time_deriv(ttheta, ttheta, order=2)
ttheta_all = st.concat_rows(ttheta, tthetad, tthetadd)
c1, c2, c3, c4, c5, c6, m1, m2, m3, m4, m5, m6, J1, J2, J3, J4, J5, J6, l1, l2, l3, l4, l5, l6, d, g = params = sp.symbols(
'c1, c2, c3, c4, c5, c6, m1, m2, m3, m4, m5, m6, J1, J2, J3, J4, J5, J6, l1, l2, l3, l4, l5, l6, d, g')
tau1, tau2, tau3, tau4, tau5, tau6 = ttau = st.symb_vector("tau1, tau2, tau3, tau4, tau5, tau6 ")
# unit vectors
ex = sp.Matrix([1, 0])
ey = sp.Matrix([0, 1])
# coordinates of centers of mass and joints
# left
G0 = 0 * ex ##:
C1 = G0 + Rz(q1) * ex * c1 ##:
G1 = G0 + Rz(q1) * ex * l1 ##:
C2 = G1 + Rz(q1 + p1) * ex * c2 ##:
G2 = G1 + Rz(q1 + p1) * ex * l2 ##:
C3 = G2 + Rz(q1 + p1 + s1) * ex * c3 ##:
G3 = G2 + Rz(q1 + p1 + s1) * ex * l3 ##:
# right
G6 = d * ex ##:
C6 = G6 + Rz(q2) * ex * c6 ##:
G5 = G6 + Rz(q2) * ex * l6 ##:
C5 = G5 + Rz(q2 + p2) * ex * c5 ##:
G4 = G5 + Rz(q2 + p2) * ex * l5 ##:
C4 = G4 + Rz(q2 + p2 + s2) * ex * c4 ##:
G3b = G4 + Rz(q2 + p2 + s2) * ex * l4 ##:
# time derivatives of centers of mass
Sd1, Sd2, Sd3, Sd4, Sd5, Sd6 = st.col_split(st.time_deriv(st.col_stack(C1, C2, C3, C4, C5, C6), ttheta))
# Kinetic Energy (note that angles are relative)
T_rot = (J1 * qdot1 ** 2) / 2 + (J2 * (qdot1 + pdot1) ** 2) / 2 + (J3 * (qdot1 + pdot1 + sdot1) ** 2) / 2 + \
(J4 * (qdot2 + pdot2 + sdot2) ** 2) / 2 + (J5 * (qdot2 + pdot2) ** 2) / 2 + (J6 * qdot2 ** 2) / 2
T_trans = (
m1 * Sd1.T * Sd1 + m2 * Sd2.T * Sd2 + m3 * Sd3.T * Sd3 + m4 * Sd4.T * Sd4 + m5 * Sd5.T * Sd5 + m6 * Sd6.T * Sd6) / 2
T = T_rot + T_trans[0]
# Potential Energy
V = m1 * g * C1[1] + m2 * g * C2[1] + m3 * g * C3[1] + m4 * g * C4[1] + m5 * g * C5[1] + m6 * g * C6[1]
parameter_values = list(dict(c1=0.4 / 2, c2=0.42 / 2, c3=0.55 / 2, c4=0.55 / 2, c5=0.42 / 2, c6=0.4 / 2,
m1=6.0, m2=12.0, m3=39.6, m4=39.6, m5=12.0, m6=6.0,
J1=(6 * 0.4 ** 2) / 12, J2=(12 * 0.42 ** 2) / 12, J3=(39.6 * 0.55 ** 2) / 12,
J4=(39.6 * 0.55 ** 2) / 12, J5=(12 * 0.42 ** 2) / 12, J6=(6 * 0.4 ** 2) / 12,
l1=0.4, l2=0.42, l3=0.55, l4=0.55, l5=0.42, l6=0.4, d=0.26, g=9.81).items())
external_forces = [tau1, tau2, tau3, tau4, tau5, tau6]
dir_of_this_file = os.path.dirname(os.path.abspath(sys.modules.get(__name__).__file__))
fpath = os.path.join(dir_of_this_file, "7L-dae-2020-07-15.pcl")
if not os.path.isfile(fpath):
# if model is not present it could be regenerated
# however this might take long (approx. 20min)
mod = mt.generate_symbolic_model(T, V, ttheta, external_forces, constraints=[G3 - G3b], simplify=False)
mod.calc_state_eq(simplify=False)
mod.f_sympy = mod.f.subs(parameter_values)
mod.G_sympy = mod.g.subs(parameter_values)
with open(fpath, "wb") as pfile:
pickle.dump(mod, pfile)
else:
with open(fpath, "rb") as pfile:
mod = pickle.load(pfile)
# calculate DAE equations from symbolic model
dae = mod.calc_dae_eq(parameter_values)
dae.generate_eqns_funcs()
torso1_unit = Rz(q1 + p1 + s1) * ex
torso2_unit = Rz(q2 + p2 + s2) * ex
neck_length = 0.12
head_radius = 0.1
body_width = 15
neck_width = 15
H1 = G3 + neck_length * torso1_unit
H1r = G3 + (neck_length - head_radius) * torso1_unit
H2 = G3b + neck_length * torso2_unit
H2r = G3b + (neck_length - head_radius) * torso2_unit
vis = vt.Visualiser(ttheta, xlim=(-1.5, 1.5), ylim=(-.2, 2))
# get default colors and set them explicitly
# this prevents color changes in onion skin plot
default_colors = plt.get_cmap("tab10")
guy1_color = default_colors(0)
guy1_joint_color = "darkblue"
guy2_color = default_colors(1)
guy2_joint_color = "red"
guy1_head_fc = guy1_color # facecolor
guy1_head_ec = guy1_head_fc # edgecolor
guy2_head_fc = guy2_color # facecolor
guy2_head_ec = guy2_head_fc # edgecolor
# guy 1 body
vis.add_linkage(st.col_stack(G0, G1, G2, G3).subs(parameter_values),
color=guy1_color,
solid_capstyle='round',
lw=body_width,
ms=body_width,
mfc=guy1_joint_color)
# guy 1 neck
# vis.add_linkage(st.col_stack(G3, H1r).subs(parameter_values), color=head_color, solid_capstyle='round', lw=neck_width)
# guy 1 head
vis.add_disk(st.col_stack(H1, H1r).subs(parameter_values), fc=guy1_head_fc, ec=guy1_head_ec, plot_radius=False,
fill=True)
# guy 2 body
vis.add_linkage(st.col_stack(G6, G5, G4, G3b).subs(parameter_values),
color=guy2_color,
solid_capstyle='round',
lw=body_width,
ms=body_width,
mfc=guy2_joint_color)
# guy 2 neck
# vis.add_linkage(st.col_stack(G3b, H2r).subs(parameter_values), color=head_color, solid_capstyle='round', lw=neck_width)
# guy 2 head
vis.add_disk(st.col_stack(H2, H2r).subs(parameter_values), fc=guy2_head_fc, ec=guy2_head_ec, plot_radius=False,
fill=True)
eq_stat = mod.eqns.subz0(tthetadd).subz0(tthetad).subs(parameter_values)
# vector for tau and lambda together
ttau_symbols = sp.Matrix(mod.uu) ##:T
mmu = st.row_stack(ttau_symbols, mod.llmd) ##:T
# linear system of equations (and convert to function w.r.t. ttheta)
K0_expr = eq_stat.subz0(mmu) ##:i
K1_expr = eq_stat.jacobian(mmu) ##:i
K0_func = st.expr_to_func(ttheta, K0_expr)
K1_func = st.expr_to_func(ttheta, K1_expr, keep_shape=True)
def get_mu_stat_for_theta(ttheta_arg, rho=10):
# weighting matrix for mu
K0 = K0_func(*ttheta_arg)
K1 = K1_func(*ttheta_arg)
return solve_qlp(K0, K1, rho)
def solve_qlp(K0, K1, rho):
R_mu = npy.diag([1, 1, 1, rho, rho, rho, .1, .1])
n1, n2 = K1.shape
# construct the equation system for least squares with linear constraints
M1 = npy.column_stack((R_mu, K1.T))
M2 = npy.column_stack((K1, npy.zeros((n1, n1))))
M_coeff = npy.row_stack((M1, M2))
M_rhs = npy.concatenate((npy.zeros(n2), -K0))
mmu_stat = npy.linalg.solve(M_coeff, M_rhs)[:n2]
return mmu_stat
ttheta_start = npy.r_[0.9, 1.5, -1.9, 2.1, -2.175799453493845, 1.7471971159642905]
mmu_start = get_mu_stat_for_theta(ttheta_start)
connection_point_func = st.expr_to_func(ttheta, G3.subs(parameter_values))
cs_ttau = mpc.casidify(mod.uu, mod.uu)[0]
cs_llmd = mpc.casidify(mod.llmd, mod.llmd)[0]
controls_sp = mmu
controls_cs = cs.vertcat(cs_ttau, cs_llmd)
coords_cs, _ = mpc.casidify(ttheta, ttheta)
# parameters: 0: value of y_connection, 1: x_connection_last,
# 2: y_connection_last, 3: delta_r_max, 4: rho (penalty factor for 2nd persons torques),
# 5:11: ttheta_old[6], 11:17: ttheta:old2
#
P = SX.sym('P', 5 + 12)
rho = P[10]
# weightning of inputs
R = mpc.SX_diag_matrix((1, 1, 1, rho, rho, rho, 0.1, 0.1))
# Construction of Constraints
g1 = [] # constraints vector (system dynamics)
g2 = [] # inequality-constraints
closed_chain_constraint, _ = mpc.casidify(mod.dae.constraints, ttheta, cs_vars=coords_cs)
connection_position, _ = mpc.casidify(list(G3.subs(parameter_values)), ttheta, cs_vars=coords_cs) ##:i
connection_y_value, _ = mpc.casidify([G3[1].subs(parameter_values)], ttheta, cs_vars=coords_cs) ##:i
stationary_eqns, _, _ = mpc.casidify(eq_stat, ttheta, controls_sp, cs_vars=(coords_cs, controls_cs)) ##:i
g1.extend(mpc.unpack(stationary_eqns))
g1.extend(mpc.unpack(closed_chain_constraint))
# force the connecting joint to a given hight (which will be provided later)
g1.append(connection_y_value - P[0])
ng1 = len(g1)
# squared distance from the last reference should be smaller than P[3] (delta_r_max):
# this will be a restriction between -inf and 0
r = connection_position - P[1:3]
g2.append(r.T @ r - P[3])
# change of angles should be smaller than a given bound (P[5:11] are the old coords)
coords_old = P[5:11]
coords_old2 = P[11:17]
pseudo_vel = (coords_cs - coords_old) / 1
pseudo_acc = (coords_cs - 2 * coords_old + coords_old2) / 1
g2.extend(mpc.unpack(pseudo_vel))
g2.extend(mpc.unpack(pseudo_acc))
g_all = mpc.seq_to_SX_matrix(g1 + g2)
### Construction of objective Function
obj = controls_cs.T @ R @ controls_cs + 1e5 * pseudo_acc.T @ pseudo_acc + 0.3e6 * pseudo_vel.T @ pseudo_vel
OPT_variables = cs.vertcat(coords_cs, controls_cs)
# for debugging
g_all_cs_func = cs.Function("g_all_cs_func", (OPT_variables, P), (g_all,))
nlp_prob = dict(f=obj, x=OPT_variables, g=g_all, p=P)
ipopt_settings = dict(max_iter=5000, print_level=0,
acceptable_tol=1e-8, acceptable_obj_change_tol=1e-6)
opts = dict(print_time=False, ipopt=ipopt_settings)
xx_guess = npy.r_[ttheta_start, mmu_start]
# note: g1 contains the equality constraints (system dynamics) (lower bound = upper bound)
delta_phi = .05
d_delta_phi = .02
eps = 1e-9
lbg = npy.r_[[-eps] * ng1 + [-inf] + [-delta_phi] * n, [-d_delta_phi] * n]
ubg = npy.r_[[eps] * ng1 + [0] + [delta_phi] * n, [d_delta_phi] * n]
# ubx = [inf]*OPT_variables.shape[0]##:
# lower and upper bounds for decision variables:
# lbx = [-inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf]*N1 + [tau1_min, tau4_min, -inf, -inf]*N
# ubx = [inf, inf, inf, inf, inf, inf, inf, inf]*N1 + [tau1_max, tau4_max, inf, inf]*N
rho = 3
P_init = npy.r_[connection_point_func(*ttheta_start)[1],
connection_point_func(*ttheta_start), 0.01, rho, ttheta_start, ttheta_start]
args = dict(lbx=-inf, ubx=inf, lbg=lbg, ubg=ubg, # unconstrained optimization
p=P_init, # initial and final state
x0=xx_guess # initial guess
)
solver = cs.nlpsol("solver", "ipopt", nlp_prob, opts)
sol = solver(**args)
global_vars = ipydex.Container(old_sol=xx_guess, old_sol2=xx_guess)
def get_optimal_equilibrium(y_value, rho=3):
ttheta_old = global_vars.old_sol[:n]
ttheta_old2 = global_vars.old_sol2[:n]
opt_prob_params = npy.r_[y_value, connection_point_func(*ttheta_old),
0.01, rho, ttheta_old, ttheta_old2]
args.update(dict(p=opt_prob_params, x0=global_vars.old_sol))
sol = solver(**args)
stats = solver.stats()
if not stats['success']:
raise ValueError(stats["return_status"])
XX = sol["x"].full().squeeze()
# save the last two results
global_vars.old_sol2 = global_vars.old_sol
global_vars.old_sol = XX
return XX
y_start = connection_point_func(*ttheta_start)[1]
N = 100
y_end = 1.36
y_func = st.expr_to_func(t, st.condition_poly(t, (0, y_start, 0, 0), (1, y_end, 0, 0)))
def get_qs_trajectory(rho):
pseudo_time = npy.linspace(0, 1, N)
yy_connection = y_func(pseudo_time)
# reset the initial guess
global_vars.old_sol = xx_guess
global_vars.old_sol2 = xx_guess
XX_list = []
for i, y_value in enumerate(yy_connection):
# print(i, y_value)
XX_list.append(get_optimal_equilibrium(y_value, rho=rho))
XX = npy.array(XX_list)
return XX
rho = 30
XX = get_qs_trajectory(rho=rho)
def smooth_time_scaling(Tend, N, phase_fraction=.5):
"""
:param Tend:
:param N:
:param phase_fraction: fraction of Tend for smooth initial and end phase
"""
T0 = 0
T1 = Tend * phase_fraction
y0 = 0
y1 = 1
# for initial phase
poly1 = st.condition_poly(t, (T0, y0, 0, 0), (T1, y1, 0, 0))
# for end phase
poly2 = poly1.subs(t, Tend - t)
# there should be a phase in the middle with constant slope
deriv_transition = st.piece_wise((y0, t < T0), (poly1, t < T1), (y1, t < Tend - T1),
(poly2, t < Tend), (y0, True))
scaling = sp.integrate(deriv_transition, (t, T0, Tend))
time_transition = sp.integrate(deriv_transition * N / scaling, t)
# deriv_transition_func = st.expr_to_func(t, full_transition)
time_transition_func = st.expr_to_func(t, time_transition)
deriv_func = st.expr_to_func(t, deriv_transition * N / scaling)
deriv_func2 = st.expr_to_func(t, deriv_transition.diff(t) * N / scaling)
C = ipydex.Container(fetch_locals=True)
return C
N = XX.shape[0]
Tend = 4
res = smooth_time_scaling(Tend, N)
def get_derivatives(XX, time_scaling, res=100):
"""
:param XX: Nxm array
:param time_scaling: container for time scaling
:param res: time resolution of the returned arrays
"""
N = XX.shape[0]
Tend = time_scaling.Tend
assert npy.isclose(time_scaling.time_transition_func([0, Tend])[-1], N)
tt = npy.linspace(time_scaling.T0, time_scaling.Tend, res)
NN = npy.arange(N)
# high_resolution version of index arry
NN2 = npy.linspace(0, N, res, endpoint=False)
# time-scaled verion of index-array
NN3 = time_scaling.time_transition_func(tt)
NN3d = time_scaling.deriv_func(tt)
NN3dd = time_scaling.deriv_func2(tt)
XX_num, XXd_num, XXdd_num = [], [], []
# iterate over every column
for col in XX.T:
spl = splrep(NN, col)
# function value and derivatives
XX_num.append(splev(NN3, spl))
XXd_num.append(splev(NN3, spl, der=1))
XXdd_num.append(splev(NN3, spl, der=2))
XX_num = npy.array(XX_num).T
XXd_num = npy.array(XXd_num).T
XXdd_num = npy.array(XXdd_num).T
NN3d_bc = npy.broadcast_to(NN3d, XX_num.T.shape).T
NN3dd_bc = npy.broadcast_to(NN3dd, XX_num.T.shape).T
XXd_n = XXd_num * NN3d_bc
# apply chain rule
XXdd_n = XXdd_num * NN3d_bc ** 2 + XXd_num * NN3dd_bc
C = ipydex.Container(fetch_locals=True)
return C
C = XX_derivs = get_derivatives(XX[:, :], time_scaling=res)
expr = mod.eqns.subz0(mod.uu, mod.llmd).subs(parameter_values)
dynterm_func = st.expr_to_func(ttheta_all, expr)
def get_torques(dyn_term_func, XX_derivs, static1=False, static2=False):
ttheta_num_all = npy.c_[XX_derivs.XX_num[:, :n], XX_derivs.XXd_n[:, :n], XX_derivs.XXdd_n[:, :n]] ##:S
if static1:
# set velocities to 0
ttheta_num_all[:, n:2 * n] = 0
if static2:
# set accelerations to 0
ttheta_num_all[:, 2 * n:] = 0
res = dynterm_func(*ttheta_num_all.T)
return res
lhs_static = get_torques(dynterm_func, XX_derivs, static1=True, static2=True) ##:i
lhs_dynamic = get_torques(dynterm_func, XX_derivs, static2=False) ##:i
mmu_stat_list = []
for L_k_stat, L_k_dyn, ttheta_k in zip(lhs_static, lhs_dynamic, XX_derivs.XX_num[:, :n]):
K1_k = K1_func(*ttheta_k)
mmu_stat_k = solve_qlp(L_k_stat, K1_k, rho)
mmu_stat_list.append(mmu_stat_k)
mmu_stat_all = npy.array(mmu_stat_list)
solution_data = SolutionData()
solution_data.tt = XX_derivs.tt
solution_data.xx = XX_derivs.XX_num
solution_data.mmu = mmu_stat_all
solution_data.vis = vis
save_plot(problem_spec, solution_data)
return solution_data
from ipydex import IPS, activate_ips_on_exception
from sys import path
from casadi import sin, cos, SX, DM, vertcat, reshape, Function, nlpsol, inf, norm_2, sqrt, gradient, dot
import matplotlib.pyplot as plt
import random
if __name__ == '__main__':
# Parameter konfiguration
A_d = SX([[1, 0, 0], [0, 1, 0], [0, 0, 1]])
B_d = SX([[0, 0], [1, 0], [0, 1]])
T = 0.1
temp_x1 = 0.2
temp_x2 = 5
temp_x3 = 0
N_fe_range = 10
# Zustaende als Symbol definieren
x = SX.sym('x')
y = SX.sym('y')
theta = SX.sym('theta')
# ZustandsvekFtor
states = vertcat(x, y, theta)
# print(states.shape)
# Eingang symbolisch definieren
u_1 = SX.sym('u_1')
u_2 = SX.sym("u_2")
# Eingangsverktor
controls = vertcat(u_1, u_2)
def __set_costs(self, ocp):
# set weight for states and controls (default: 1.00)
# Q = 1.00 * np.eye(self.acados_ocp.dims.nx)
# R = 1.00 * np.eye(self.acados_ocp.dims.nu)
# self.acados_ocp.cost.W = linalg.block_diag(Q, R)
# self.acados_ocp.cost.W_e = Q
self.y_ref = []
self.y_ref_end = []
if self.acados_ocp.cost.cost_type == "LINEAR_LS":
# set Lagrange terms
self.acados_ocp.cost.Vx = np.zeros((self.acados_ocp.dims.ny, self.acados_ocp.dims.nx))
self.acados_ocp.cost.Vx[: self.acados_ocp.dims.nx, :] = np.eye(self.acados_ocp.dims.nx)
Vu = np.zeros((self.acados_ocp.dims.ny, self.acados_ocp.dims.nu))
Vu[self.acados_ocp.dims.nx :, :] = np.eye(self.acados_ocp.dims.nu)
self.acados_ocp.cost.Vu = Vu
# set Mayer term
self.acados_ocp.cost.Vx_e = np.zeros((self.acados_ocp.dims.nx, self.acados_ocp.dims.nx))
elif self.acados_ocp.cost.cost_type == "NONLINEAR_LS":
if ocp.nb_phases != 1:
# TODO: Please confirm this
raise NotImplementedError("ACADOS with more than one phase is not implemented yet")
for i in range(ocp.nb_phases):
# TODO: I think ocp.J is missing here (the parameters would be stored there)
for j, J in enumerate(ocp.nlp[i]["J"]):
if J[0]["objective"].type.get_type() == ObjectiveFunction.LagrangeFunction:
self.lagrange_costs = vertcat(self.lagrange_costs, J[0]["val"].reshape((-1, 1)))
if J[0]["target"] is not None:
self.y_ref.append([J_tp["target"].T.reshape((-1, 1)) for J_tp in J])
else:
raise RuntimeError("Should we put y_ref = zeros?")
elif J[0]["objective"].type.get_type() == ObjectiveFunction.MayerFunction:
raise RuntimeError("TODO: I may have broken this (is this the right J?)")
mayer_func_tp = Function(f"cas_mayer_func_{i}_{j}", [ocp.nlp[i]["X"][-1]], [J[0]["val"]])
self.mayer_costs = vertcat(self.mayer_costs, mayer_func_tp(ocp.nlp[i]["X"][0]))
if J[0]["target"] is not None:
self.y_ref_end.append([J[0]["target"]])
else:
raise RuntimeError("TODO: Should we put y_ref_end = zeros?")
else:
raise RuntimeError("The objective function is not Lagrange nor Mayer.")
if self.lagrange_costs.numel():
self.acados_ocp.model.cost_y_expr = self.lagrange_costs
else:
self.acados_ocp.model.cost_y_expr = SX(1, 1)
if self.mayer_costs.numel():
self.acados_ocp.model.cost_y_expr_e = self.mayer_costs
else:
self.acados_ocp.model.cost_y_expr_e = SX(1, 1)
self.acados_ocp.dims.ny = self.acados_ocp.model.cost_y_expr.shape[0]
self.acados_ocp.dims.ny_e = self.acados_ocp.model.cost_y_expr_e.shape[0]
self.acados_ocp.cost.yref = np.zeros((max(self.acados_ocp.dims.ny, 1),))
self.acados_ocp.cost.yref_e = np.zeros((max(self.acados_ocp.dims.ny_e, 1),))
# TODO changed hard coded values below (you can use J["weight"])
Q_ocp = np.zeros((15, 15))
np.fill_diagonal(Q_ocp, 1000)
R_ocp = np.zeros((4, 4))
np.fill_diagonal(R_ocp, 1000)
self.acados_ocp.cost.W = linalg.block_diag(Q_ocp, R_ocp)
self.acados_ocp.cost.W_e = np.zeros((1, 1))
# TODO: Is the following useful?
# if len(self.y_ref):
# self.y_ref = np.vstack(self.y_ref)
# else:
# self.y_ref = [np.zeros((1, 1))] * self.ocp
# if len(self.y_ref_end):
# self.y_ref_end = np.vstack(self.y_ref_end)
# else:
# self.y_ref_end = np.zeros((1, 1))
elif self.acados_ocp.cost.cost_type == "EXTERNAL":
for i in range(ocp.nb_phases):
for j in range(len(ocp.nlp[i]["J"])):
J = ocp.nlp[i]["J"][j][0]
raise RuntimeError("TODO: The target is not right currently")
if J["type"] == ObjectiveFunction.LagrangeFunction:
self.lagrange_costs = vertcat(self.lagrange_costs, J["val"][0] - J["target"][0])
elif J["type"] == ObjectiveFunction.MayerFunction:
raise RuntimeError("TODO: I may have broken this (is this the right J?)")
mayer_func_tp = Function(f"cas_mayer_func_{i}_{j}", [ocp.nlp[i]["X"][-1]], [J["val"]])
self.mayer_costs = vertcat(self.mayer_costs, mayer_func_tp(ocp.nlp[i]["X"][0]))
else:
raise RuntimeError("The objective function is not Lagrange nor Mayer.")
self.acados_ocp.model.cost_expr_ext_cost = sum1(self.lagrange_costs)
self.acados_ocp.model.cost_expr_ext_cost_e = sum1(self.mayer_costs)
else:
raise RuntimeError("Available acados cost type: 'LINEAR_LS', 'NONLINEAR_LS' and 'EXTERNAL'.")
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
y_ub = 10
y_lb = -10
# Abtastzeit und Prediction horizon
T = 0.1
N_pred = 9
# Zielruhelage von den Zustaenden
# Referenz von h, h_p, eta
state_r = [0, 0, 0]
input_r = [0, 0]
if __name__ == '__main__':
NMPC_init()
# Zustaende als Symbol definieren
x = SX.sym('x')
y = SX.sym('y')
theta = SX.sym('theta')
# ZustandsvekFtor
states = vertcat(x, y, theta)
# print(states.shape)
# Eingang symbolisch definieren
u_1 = SX.sym('u_1')
u_2 = SX.sym("u_2")
# Eingangsverktor
controls = vertcat(u_1, u_2)
# kriegen die Anzahl der Zeiler("shape" ist ein Tuple)
n_states = states.shape[0] # p 22.
def main(use_cython=True):
# (very) simple crane model
beta = 0.001
k = 0.9
a_max = 10
dt_max = 2.0
# states
p1 = SX.sym('p1')
v1 = SX.sym('v1')
p2 = SX.sym('p2')
v2 = SX.sym('v2')
x = vertcat(p1, v1, p2, v2)
# controls
a = SX.sym('a')
dt = SX.sym('dt')
u = vertcat(a, dt)
f_expl = dt * vertcat(v1, a, v2, -beta * v2 - k * (p2 - p1))
model = AcadosModel()
model.f_expl_expr = f_expl
model.x = x
model.u = u
model.name = 'crane_time_opt'
# create ocp object to formulate the OCP
x0 = np.array([2.0, 0.0, 2.0, 0.0])
xf = np.array([0.0, 0.0, 0.0, 0.0])
ocp = AcadosOcp()
ocp.model = model
# N - maximum number of bangs
N = 7
Tf = N
nx = model.x.size()[0]
nu = model.u.size()[0]
# set dimensions
ocp.dims.N = N
# set cost
ocp.cost.cost_type = 'EXTERNAL'
ocp.cost.cost_type_e = 'EXTERNAL'
ocp.model.cost_expr_ext_cost = dt
ocp.model.cost_expr_ext_cost_e = 0
ocp.constraints.lbu = np.array([-a_max, 0.0])
ocp.constraints.ubu = np.array([+a_max, dt_max])
ocp.constraints.idxbu = np.array([0, 1])
ocp.constraints.x0 = x0
ocp.constraints.lbx_e = xf
ocp.constraints.ubx_e = xf
ocp.constraints.idxbx_e = np.array([0, 1, 2, 3])
# set prediction horizon
ocp.solver_options.tf = Tf
# set options
ocp.solver_options.qp_solver = 'FULL_CONDENSING_QPOASES' #'PARTIAL_CONDENSING_HPIPM' # FULL_CONDENSING_QPOASES
ocp.solver_options.integrator_type = 'ERK'
ocp.solver_options.print_level = 3
ocp.solver_options.nlp_solver_type = 'SQP' # SQP_RTI, SQP
ocp.solver_options.globalization = 'MERIT_BACKTRACKING'
ocp.solver_options.nlp_solver_max_iter = 5000
ocp.solver_options.nlp_solver_tol_stat = 1e-6
ocp.solver_options.levenberg_marquardt = 0.1
ocp.solver_options.sim_method_num_steps = 15
ocp.solver_options.qp_solver_iter_max = 100
ocp.code_export_directory = 'c_generated_code'
ocp.solver_options.hessian_approx = 'EXACT'
ocp.solver_options.exact_hess_constr = 0
ocp.solver_options.exact_hess_dyn = 0
if use_cython:
AcadosOcpSolver.generate(ocp, json_file='acados_ocp.json')
AcadosOcpSolver.build(ocp.code_export_directory, with_cython=True)
ocp_solver = AcadosOcpSolver.create_cython_solver('acados_ocp.json')
else: # ctypes
## Note: skip generate and build assuming this is done before (in cython run)
ocp_solver = AcadosOcpSolver(ocp,
json_file='acados_ocp.json',
build=False,
generate=False)
ocp_solver.reset()
for i, tau in enumerate(np.linspace(0, 1, N)):
ocp_solver.set(i, 'x', (1 - tau) * x0 + tau * xf)
ocp_solver.set(i, 'u', np.array([0.1, 0.5]))
simX = np.zeros((N + 1, nx))
simU = np.zeros((N, nu))
status = ocp_solver.solve()
if status != 0:
ocp_solver.print_statistics()
raise Exception(f'acados returned status {status}.')
# get solution
for i in range(N):
simX[i, :] = ocp_solver.get(i, "x")
simU[i, :] = ocp_solver.get(i, "u")
simX[N, :] = ocp_solver.get(N, "x")
dts = simU[:, 1]
print(
"acados solved OCP successfully, creating integrator to simulate the solution"
)
# simulate on finer grid
sim = AcadosSim()
# set model
sim.model = model
# set options
sim.solver_options.integrator_type = 'ERK'
sim.solver_options.num_stages = 4
sim.solver_options.num_steps = 3
sim.solver_options.T = 1.0 # dummy value
dt_approx = 0.0005
dts_fine = np.zeros((N, ))
Ns_fine = np.zeros((N, ), dtype='int16')
# compute number of simulation steps for bang interval + dt_fine
for i in range(N):
N_approx = max(int(dts[i] / dt_approx), 1)
dts_fine[i] = dts[i] / N_approx
Ns_fine[i] = int(round(dts[i] / dts_fine[i]))
N_fine = int(np.sum(Ns_fine))
simU_fine = np.zeros((N_fine, nu))
ts_fine = np.zeros((N_fine + 1, ))
simX_fine = np.zeros((N_fine + 1, nx))
simX_fine[0, :] = x0
acados_integrator = AcadosSimSolver(sim)
k = 0
for i in range(N):
u = simU[i, 0]
acados_integrator.set("u", np.hstack((u, np.ones(1, ))))
# set simulation time
acados_integrator.set("T", dts_fine[i])
for j in range(Ns_fine[i]):
acados_integrator.set("x", simX_fine[k, :])
status = acados_integrator.solve()
if status != 0:
raise Exception(f'acados returned status {status}.')
simX_fine[k + 1, :] = acados_integrator.get("x")
simU_fine[k, :] = u
ts_fine[k + 1] = ts_fine[k] + dts_fine[i]
k += 1
# visualize
if os.environ.get('ACADOS_ON_TRAVIS'):
plt.figure()
state_labels = ['p1', 'v1', 'p2', 'v2']
for i, l in enumerate(state_labels):
plt.subplot(5, 1, i + 1)
plt.plot(ts_fine, simX_fine[:, i], label='time optimal solution')
plt.grid(True)
plt.ylabel(l)
if i == 0:
plt.legend(loc=1)
plt.subplot(5, 1, 5)
plt.step(ts_fine,
np.hstack((simU_fine[:, 0], simU_fine[-1, 0])),
'-',
where='post')
plt.grid(True)
plt.ylabel('a')
plt.xlabel('t')
plt.show()
from casadi import SX, Function, vertcat
from acados import ocp_nlp_function, ocp_nlp_ls_cost, ocp_nlp, ocp_nlp_solver
from models import chen_model
N = 10
ode_fun, nx, nu = chen_model(implicit=True)
nlp = ocp_nlp({'N': N, 'nx': nx, 'nu': nu, 'ng': N * [1] + [0]})
# ODE Model
step = 0.1
nlp.set_model(ode_fun, step)
# Cost function
Q = diag([1.0, 1.0])
R = 1e-2
x = SX.sym('x', nx)
u = SX.sym('u', nu)
u_N = SX.sym('u', 0)
f = ocp_nlp_function(Function('ls_cost', [x, u], [vertcat(x, u)]))
f_N = ocp_nlp_function(Function('ls_cost_N', [x, u_N], [x]))
ls_cost = ocp_nlp_ls_cost(N, N * [f] + [f_N])
ls_cost.ls_cost_matrix = N * [block_diag(Q, R)] + [Q]
nlp.set_cost(ls_cost)
# Constraints
g = ocp_nlp_function(Function('path_constraint', [x, u], [u]))
g_N = ocp_nlp_function(Function('path_constraintN', [x, u], [SX([])]))
nlp.set_path_constraints(N * [g] + [g_N])
for i in range(N):
nlp.lg[i] = -0.5
nlp.ug[i] = +0.5
def __set_costs(self, ocp):
"""
Set the cost functions from ocp
Parameters
----------
ocp: OptimalControlProgram
A reference to the current OptimalControlProgram
"""
def add_linear_ls_lagrange(acados, objectives):
def add_objective(n_variables, is_state):
v_var = np.zeros(n_variables)
var_type = acados.ocp.nlp[
0].states if is_state else acados.ocp.nlp[0].controls
rows = objectives.rows + var_type[
objectives.params["key"]].index[0]
v_var[rows] = 1.0
if is_state:
acados.Vx = np.vstack((acados.Vx, np.diag(v_var)))
acados.Vu = np.vstack(
(acados.Vu, np.zeros((n_states, n_controls))))
else:
acados.Vx = np.vstack(
(acados.Vx, np.zeros((n_controls, n_states))))
acados.Vu = np.vstack((acados.Vu, np.diag(v_var)))
acados.W = linalg.block_diag(
acados.W, np.diag([objectives.weight] * n_variables))
node_idx = objectives.node_idx[:-1] if objectives.node[
0] == Node.ALL else objectives.node_idx
y_ref = [
np.zeros((n_states if is_state else n_controls, 1))
for _ in node_idx
]
if objectives.target is not None:
for idx in node_idx:
y_ref[idx][rows] = objectives.target[...,
idx].T.reshape(
(-1, 1))
acados.y_ref.append(y_ref)
if objectives.type in allowed_control_objectives:
add_objective(n_controls, False)
elif objectives.type in allowed_state_objectives:
add_objective(n_states, True)
else:
raise RuntimeError(
f"{objectives[0]['objective'].type.name} is an incompatible objective term with LINEAR_LS cost type"
)
def add_linear_ls_mayer(acados, objectives):
if objectives.type in allowed_state_objectives:
vxe = np.zeros(n_states)
rows = objectives.rows + acados.ocp.nlp[0].states[
objectives.params["key"]].index[0]
vxe[rows] = 1.0
acados.Vxe = np.vstack((acados.Vxe, np.diag(vxe)))
acados.W_e = linalg.block_diag(
acados.W_e, np.diag([objectives.weight] * n_states))
y_ref_end = np.zeros((n_states, 1))
if objectives.target is not None:
y_ref_end[rows] = objectives.target[..., -1].T.reshape(
(-1, 1))
acados.y_ref_end.append(y_ref_end)
else:
raise RuntimeError(
f"{objectives.type.name} is an incompatible objective term with LINEAR_LS cost type"
)
def add_nonlinear_ls_lagrange(acados, objectives, x, u, p):
acados.lagrange_costs = vertcat(
acados.lagrange_costs,
objectives.function(x, u, p).reshape((-1, 1)))
acados.W = linalg.block_diag(
acados.W,
np.diag([objectives.weight] * objectives.function.numel_out()))
node_idx = objectives.node_idx[:-1] if objectives.node[
0] == Node.ALL else objectives.node_idx
if objectives.target is not None:
acados.y_ref.append([
objectives.target[..., idx].T.reshape((-1, 1))
for idx in node_idx
])
else:
acados.y_ref.append([
np.zeros((objectives.function.numel_out(), 1))
for _ in node_idx
])
def add_nonlinear_ls_mayer(acados, objectives, x, u, p):
acados.W_e = linalg.block_diag(
acados.W_e,
np.diag([objectives.weight] * objectives.function.numel_out()))
x = x if objectives.function.sparsity_in("i0").shape != (
0, 0) else []
u = u if objectives.function.sparsity_in("i1").shape != (
0, 0) else []
acados.mayer_costs = vertcat(
acados.mayer_costs,
objectives.function(x, u, p).reshape((-1, 1)))
if objectives.target is not None:
acados.y_ref_end.append(objectives.target[..., -1].T.reshape(
(-1, 1)))
else:
acados.y_ref_end.append(
np.zeros((objectives.function.numel_out(), 1)))
if ocp.n_phases != 1:
raise NotImplementedError(
"ACADOS with more than one phase is not implemented yet.")
# costs handling in self.acados_ocp
self.y_ref = []
self.y_ref_end = []
self.lagrange_costs = SX()
self.mayer_costs = SX()
self.W = np.zeros((0, 0))
self.W_e = np.zeros((0, 0))
allowed_control_objectives = [ObjectiveFcn.Lagrange.MINIMIZE_CONTROL]
allowed_state_objectives = [
ObjectiveFcn.Lagrange.MINIMIZE_STATE,
ObjectiveFcn.Mayer.TRACK_STATE
]
if self.acados_ocp.cost.cost_type == "LINEAR_LS":
n_states = ocp.nlp[0].states.shape
n_controls = ocp.nlp[0].controls.shape
self.Vu = np.array([], dtype=np.int64).reshape(0, n_controls)
self.Vx = np.array([], dtype=np.int64).reshape(0, n_states)
self.Vxe = np.array([], dtype=np.int64).reshape(0, n_states)
for i in range(ocp.n_phases):
for J in ocp.nlp[i].J:
if not J:
continue
if J.multi_thread:
raise RuntimeError(
f"The objective function {J.name} was declared with multi_thread=True, "
f"but this is not possible to multi_thread objective function with ACADOS"
)
if J.type.get_type() == ObjectiveFunction.LagrangeFunction:
add_linear_ls_lagrange(self, J)
# Deal with last node to match ipopt formulation
if J.node[0] == Node.ALL:
add_linear_ls_mayer(self, J)
elif J.type.get_type() == ObjectiveFunction.MayerFunction:
add_linear_ls_mayer(self, J)
else:
raise RuntimeError(
"The objective function is not Lagrange nor Mayer."
)
if self.nparams:
raise RuntimeError(
"Params not yet handled with LINEAR_LS cost type")
# Set costs
self.acados_ocp.cost.Vx = self.Vx if self.Vx.shape[0] else np.zeros(
(0, 0))
self.acados_ocp.cost.Vu = self.Vu if self.Vu.shape[0] else np.zeros(
(0, 0))
self.acados_ocp.cost.Vx_e = self.Vxe if self.Vxe.shape[
0] else np.zeros((0, 0))
# Set dimensions
self.acados_ocp.dims.ny = sum(
[len(data[0]) for data in self.y_ref])
self.acados_ocp.dims.ny_e = sum(
[len(data) for data in self.y_ref_end])
# Set weight
self.acados_ocp.cost.W = self.W
self.acados_ocp.cost.W_e = self.W_e
# Set target shape
self.acados_ocp.cost.yref = np.zeros(
(self.acados_ocp.cost.W.shape[0], ))
self.acados_ocp.cost.yref_e = np.zeros(
(self.acados_ocp.cost.W_e.shape[0], ))
elif self.acados_ocp.cost.cost_type == "NONLINEAR_LS":
for i, nlp in enumerate(ocp.nlp):
for j, J in enumerate(nlp.J):
if not J:
continue
if J.multi_thread:
raise RuntimeError(
f"The objective function {J.name} was declared with multi_thread=True, "
f"but this is not possible to multi_thread objective function with ACADOS"
)
if J.type.get_type() == ObjectiveFunction.LagrangeFunction:
add_nonlinear_ls_lagrange(self, J, nlp.states.cx,
nlp.controls.cx,
nlp.parameters.cx)
# Deal with last node to match ipopt formulation
if J.node[0] == Node.ALL:
add_nonlinear_ls_mayer(self, J, nlp.states.cx,
nlp.controls.cx,
nlp.parameters.cx)
elif J.type.get_type() == ObjectiveFunction.MayerFunction:
add_nonlinear_ls_mayer(self, J, nlp.states.cx,
nlp.controls.cx,
nlp.parameters.cx)
else:
raise RuntimeError(
"The objective function is not Lagrange nor Mayer."
)
# parameter as mayer function
# IMPORTANT: it is considered that only parameters are stored in ocp.objectives, for now.
if self.nparams:
nlp = ocp.nlp[0] # Assume 1 phase
for j, J in enumerate(ocp.J):
add_nonlinear_ls_mayer(self, J, nlp.states.cx,
nlp.controls.cx, nlp.parameters.cx)
# Set costs
self.acados_ocp.model.cost_y_expr = (self.lagrange_costs.reshape(
(-1, 1)) if self.lagrange_costs.numel() else SX(1, 1))
self.acados_ocp.model.cost_y_expr_e = (self.mayer_costs.reshape(
(-1, 1)) if self.mayer_costs.numel() else SX(1, 1))
# Set dimensions
self.acados_ocp.dims.ny = self.acados_ocp.model.cost_y_expr.shape[
0]
self.acados_ocp.dims.ny_e = self.acados_ocp.model.cost_y_expr_e.shape[
0]
# Set weight
self.acados_ocp.cost.W = np.zeros(
(1, 1)) if self.W.shape == (0, 0) else self.W
self.acados_ocp.cost.W_e = np.zeros(
(1, 1)) if self.W_e.shape == (0, 0) else self.W_e
# Set target shape
self.acados_ocp.cost.yref = np.zeros(
(self.acados_ocp.cost.W.shape[0], ))
self.acados_ocp.cost.yref_e = np.zeros(
(self.acados_ocp.cost.W_e.shape[0], ))
elif self.acados_ocp.cost.cost_type == "EXTERNAL":
raise RuntimeError(
"EXTERNAL is not interfaced yet, please use NONLINEAR_LS")
else:
raise RuntimeError(
"Available acados cost type: 'LINEAR_LS', 'NONLINEAR_LS' and 'EXTERNAL'."
)
def test_multistep_trajectory(before_test_onestep_reachability):
""" Compare multi-steps 'by hand' with the function """
mu_0, _, gp, k_fb, k_ff, L_mu, L_sigm, c_safety, a, b = before_test_onestep_reachability
T = 3
n_u, n_s = np.shape(k_fb)
k_fb_cas_single = SX.sym("k_fb_single", (n_u, n_s))
k_ff_cas_single = SX.sym("k_ff_single", (n_u, 1))
k_ff_cas_all = SX.sym("k_ff_single", (T, n_u))
k_fb_cas_all = SX.sym("k_fb_all", (T - 1, n_s * n_u))
k_fb_cas_all_inp = [
k_fb_cas_all[i, :].reshape((n_u, n_s)) for i in range(T - 1)
]
mu_0_cas = SX.sym("mu_0", (n_s, 1))
sigma_0_cas = SX.sym("sigma_0", (n_s, n_s))
mu_onestep_no_var_in, sigm_onestep_no_var_in, _ = prop_casadi.one_step_taylor(
mu_0_cas, gp, k_ff_cas_single, a=a, b=b)
mu_one_step, sigm_onestep, _ = prop_casadi.one_step_taylor(
mu_0_cas,
gp,
k_ff_cas_single,
k_fb=k_fb_cas_single,
sigma_x=sigma_0_cas,
a=a,
b=b)
mu_multistep, sigma_multistep, _ = prop_casadi.multi_step_taylor_symbolic(
mu_0_cas, gp, k_ff_cas_all, k_fb_cas_all_inp, a=a, b=b)
on_step_no_var_in = Function(
"on_step_no_var_in", [mu_0_cas, k_ff_cas_single],
[mu_onestep_no_var_in, sigm_onestep_no_var_in])
one_step = Function(
"one_step", [mu_0_cas, sigma_0_cas, k_ff_cas_single, k_fb_cas_single],
[mu_one_step, sigm_onestep])
multi_step = Function("multi_step", [mu_0_cas, k_ff_cas_all, k_fb_cas_all],
[mu_multistep, sigma_multistep])
# TODO: Need mu, sigma as input aswell
mu_1, sigma_1 = on_step_no_var_in(mu_0, k_ff)
mu_2, sigma_2 = one_step(mu_1, sigma_1, k_ff, k_fb)
mu_3, sigma_3 = one_step(mu_2, sigma_2, k_ff, k_fb)
# TODO: stack k_ff and k_fb
k_fb_mult = np.array(cas_reshape(k_fb, (1, n_u * n_s)))
k_fb_mult = np.array(vertcat(*[k_fb_mult] * (T - 1)))
k_ff_mult = vertcat(*[k_ff] * T)
mu_all, sigma_all = multi_step(mu_0, k_ff_mult, k_fb_mult)
assert np.allclose(
np.array(mu_all[0, :]),
np.array(mu_1).T), "Are the centers of the first prediction the same?"
assert np.allclose(
np.array(mu_all[-1, :]),
np.array(mu_3).T), "Are the centers of the final prediction the same?"
assert np.allclose(cas_reshape(sigma_all[-1, :], (n_s, n_s)),
sigma_3), "Are the last covariance matrices the same?"
import matplotlib.pyplot as plt
from numpy import array, diag, eye, zeros
from scipy.linalg import block_diag
from acados import ocp_nlp
from casadi import SX, Function, vertcat
nx, nu = 2, 1
x = SX.sym('x', nx)
u = SX.sym('u', nu)
ode_fun = Function('ode_fun', [x, u], [vertcat(x[1], u)], ['x', 'u'], ['rhs'])
N = 15
nlp = ocp_nlp(N, nx, nu)
nlp.set_dynamics(ode_fun, {'integrator': 'rk4', 'step': 0.1})
q, r = 1, 1
P = eye(nx)
nlp.set_stage_cost(eye(nx+nu), zeros(nx+nu), diag([q, q, r]))
nlp.set_terminal_cost(eye(nx), zeros(nx), P)
x0 = array([1, 1])
nlp.set_field("lbx", 0, x0)
nlp.set_field("ubx", 0, x0)
nlp.initialize_solver("sqp", {"qp_solver": "qpoases"})
class AcadosInterface(SolverInterface):
def __init__(self, ocp, **solver_options):
if not isinstance(ocp.CX(), SX):
raise RuntimeError(
"CasADi graph must be SX to be solved with ACADOS")
super().__init__(ocp)
# If Acados is installed using the acados_install.sh file, you probably can leave this to unset
acados_path = ""
if "acados_dir" in solver_options:
acados_path = solver_options["acados_dir"]
self.acados_ocp = AcadosOcp(acados_path=acados_path)
self.acados_model = AcadosModel()
if "cost_type" in solver_options:
self.__set_cost_type(solver_options["cost_type"])
else:
self.__set_cost_type()
self.lagrange_costs = SX()
self.mayer_costs = SX()
self.y_ref = []
self.y_ref_end = []
self.__acados_export_model(ocp)
self.__prepare_acados(ocp)
self.ocp_solver = None
self.W = np.zeros((0, 0))
self.W_e = np.zeros((0, 0))
def __acados_export_model(self, ocp):
# Declare model variables
x = ocp.nlp[0]["X"][0]
u = ocp.nlp[0]["U"][0]
p = ocp.nlp[0]["p"]
mod = ocp.nlp[0]["model"]
x_dot = SX.sym("x_dot", mod.nbQdot() * 2, 1)
f_expl = ocp.nlp[0]["dynamics_func"](x, u, p)
f_impl = x_dot - f_expl
self.acados_model.f_impl_expr = f_impl
self.acados_model.f_expl_expr = f_expl
self.acados_model.x = x
self.acados_model.xdot = x_dot
self.acados_model.u = u
self.acados_model.p = []
self.acados_model.name = "model_name"
def __prepare_acados(self, ocp):
if ocp.nb_phases > 1:
raise NotImplementedError(
"more than 1 phase is not implemented yet with ACADOS backend")
if ocp.param_to_optimize:
raise NotImplementedError(
"Parameters optimization is not implemented yet with ACADOS")
# set model
self.acados_ocp.model = self.acados_model
# set dimensions
for i in range(ocp.nb_phases):
# set time
self.acados_ocp.solver_options.tf = ocp.nlp[i]["tf"]
# set dimensions
self.acados_ocp.dims.nx = ocp.nlp[i]["nx"]
self.acados_ocp.dims.nu = ocp.nlp[i]["nu"]
self.acados_ocp.dims.ny = self.acados_ocp.dims.nx + self.acados_ocp.dims.nu
self.acados_ocp.dims.ny_e = ocp.nlp[i]["nx"]
self.acados_ocp.dims.N = ocp.nlp[i]["ns"]
for i in range(ocp.nb_phases):
# set constraints
for j in range(ocp.nlp[i]["nx"]):
if ocp.nlp[i]["X_bounds"].min[j, 0] == -np.inf and ocp.nlp[i][
"X_bounds"].max[j, 0] == np.inf:
pass
elif ocp.nlp[i]["X_bounds"].min[
j, 0] != ocp.nlp[i]["X_bounds"].max[j, 0]:
raise RuntimeError(
"Initial constraint on state must be hard. Hint: you can pass it as an objective"
)
else:
self.acados_ocp.constraints.x0 = np.array(
ocp.nlp[i]["X_bounds"].min[:, 0])
self.acados_ocp.dims.nbx_0 = self.acados_ocp.dims.nx
# control constraints
self.acados_ocp.constraints.constr_type = "BGH"
self.acados_ocp.constraints.lbu = np.array(
ocp.nlp[i]["U_bounds"].min[:, 0])
self.acados_ocp.constraints.ubu = np.array(
ocp.nlp[i]["U_bounds"].max[:, 0])
self.acados_ocp.constraints.idxbu = np.array(
range(self.acados_ocp.dims.nu))
self.acados_ocp.dims.nbu = self.acados_ocp.dims.nu
# path constraints
self.acados_ocp.constraints.Jbx = np.eye(self.acados_ocp.dims.nx)
self.acados_ocp.constraints.ubx = np.array(
ocp.nlp[i]["X_bounds"].max[:, 1])
self.acados_ocp.constraints.lbx = np.array(
ocp.nlp[i]["X_bounds"].min[:, 1])
self.acados_ocp.constraints.idxbx = np.array(
range(self.acados_ocp.dims.nx))
self.acados_ocp.dims.nbx = self.acados_ocp.dims.nx
# terminal constraints
self.acados_ocp.constraints.Jbx_e = np.eye(self.acados_ocp.dims.nx)
self.acados_ocp.constraints.ubx_e = np.array(
ocp.nlp[i]["X_bounds"].max[:, -1])
self.acados_ocp.constraints.lbx_e = np.array(
ocp.nlp[i]["X_bounds"].min[:, -1])
self.acados_ocp.constraints.idxbx_e = np.array(
range(self.acados_ocp.dims.nx))
self.acados_ocp.dims.nbx_e = self.acados_ocp.dims.nx
return self.acados_ocp
def __set_cost_type(self, cost_type="NONLINEAR_LS"):
self.acados_ocp.cost.cost_type = cost_type
self.acados_ocp.cost.cost_type_e = cost_type
def __set_costs(self, ocp):
self.y_ref = []
self.y_ref_end = []
self.lagrange_costs = SX()
self.mayer_costs = SX()
self.W = np.zeros((0, 0))
self.W_e = np.zeros((0, 0))
if self.acados_ocp.cost.cost_type == "LINEAR_LS":
# set Lagrange terms
self.acados_ocp.cost.Vx = np.zeros(
(self.acados_ocp.dims.ny, self.acados_ocp.dims.nx))
self.acados_ocp.cost.Vx[:self.acados_ocp.dims.nx, :] = np.eye(
self.acados_ocp.dims.nx)
Vu = np.zeros((self.acados_ocp.dims.ny, self.acados_ocp.dims.nu))
Vu[self.acados_ocp.dims.nx:, :] = np.eye(self.acados_ocp.dims.nu)
self.acados_ocp.cost.Vu = Vu
# set Mayer term
self.acados_ocp.cost.Vx_e = np.zeros(
(self.acados_ocp.dims.nx, self.acados_ocp.dims.nx))
elif self.acados_ocp.cost.cost_type == "NONLINEAR_LS":
if ocp.nb_phases != 1:
raise NotImplementedError(
"ACADOS with more than one phase is not implemented yet")
for i in range(ocp.nb_phases):
# TODO: I think ocp.J is missing here (the parameters would be stored there)
# TODO: Yes the objectives in ocp are not dealt with,
# does that makes sense since we only work with 1 phase ?
for j, J in enumerate(ocp.nlp[i]["J"]):
if J[0]["objective"].type.get_type(
) == ObjectiveFunction.LagrangeFunction:
self.lagrange_costs = vertcat(
self.lagrange_costs, J[0]["val"].reshape((-1, 1)))
self.W = linalg.block_diag(
self.W,
np.diag([J[0]["objective"].weight] *
J[0]["val"].numel()))
if J[0]["target"] is not None:
self.y_ref.append([
J_tp["target"].T.reshape((-1, 1)) for J_tp in J
])
else:
self.y_ref.append([
np.zeros((J_tp["val"].numel(), 1))
for J_tp in J
])
elif J[0]["objective"].type.get_type(
) == ObjectiveFunction.MayerFunction:
mayer_func_tp = Function(f"cas_mayer_func_{i}_{j}",
[ocp.nlp[i]["X"][-1]],
[J[0]["val"]])
self.W_e = linalg.block_diag(
self.W_e,
np.diag([J[0]["objective"].weight] *
J[0]["val"].numel()))
self.mayer_costs = vertcat(
self.mayer_costs,
mayer_func_tp(ocp.nlp[i]["X"][0]))
if J[0]["target"] is not None:
self.y_ref_end.append([J[0]["target"]])
else:
self.y_ref_end.append(
[np.zeros((J[0]["val"].numel(), 1))])
else:
raise RuntimeError(
"The objective function is not Lagrange nor Mayer."
)
if self.lagrange_costs.numel():
self.acados_ocp.model.cost_y_expr = self.lagrange_costs
else:
self.acados_ocp.model.cost_y_expr = SX(1, 1)
if self.mayer_costs.numel():
self.acados_ocp.model.cost_y_expr_e = self.mayer_costs
else:
self.acados_ocp.model.cost_y_expr_e = SX(1, 1)
self.acados_ocp.dims.ny = self.acados_ocp.model.cost_y_expr.shape[
0]
self.acados_ocp.dims.ny_e = self.acados_ocp.model.cost_y_expr_e.shape[
0]
self.acados_ocp.cost.yref = np.zeros((max(self.acados_ocp.dims.ny,
1), ))
self.acados_ocp.cost.yref_e = np.concatenate(self.y_ref_end,
-1).T.squeeze()
if self.W.shape == (0, 0):
self.acados_ocp.cost.W = np.zeros((1, 1))
else:
self.acados_ocp.cost.W = self.W
if self.W_e.shape == (0, 0):
self.acados_ocp.cost.W_e = np.zeros((1, 1))
else:
self.acados_ocp.cost.W_e = self.W_e
elif self.acados_ocp.cost.cost_type == "EXTERNAL":
for i in range(ocp.nb_phases):
for j in range(len(ocp.nlp[i]["J"])):
J = ocp.nlp[i]["J"][j][0]
raise RuntimeError(
"TODO: The target is not right currently")
if J["type"] == ObjectiveFunction.LagrangeFunction:
self.lagrange_costs = vertcat(
self.lagrange_costs, J["val"][0] - J["target"][0])
elif J["type"] == ObjectiveFunction.MayerFunction:
raise RuntimeError(
"TODO: I may have broken this (is this the right J?)"
)
mayer_func_tp = Function(f"cas_mayer_func_{i}_{j}",
[ocp.nlp[i]["X"][-1]],
[J["val"]])
self.mayer_costs = vertcat(
self.mayer_costs,
mayer_func_tp(ocp.nlp[i]["X"][0]))
else:
raise RuntimeError(
"The objective function is not Lagrange nor Mayer."
)
self.acados_ocp.model.cost_expr_ext_cost = sum1(
self.lagrange_costs)
self.acados_ocp.model.cost_expr_ext_cost_e = sum1(self.mayer_costs)
else:
raise RuntimeError(
"Available acados cost type: 'LINEAR_LS', 'NONLINEAR_LS' and 'EXTERNAL'."
)
def configure(self, options):
if "acados_dir" in options:
del options["acados_dir"]
if "cost_type" in options:
del options["cost_type"]
self.acados_ocp.solver_options.qp_solver = "FULL_CONDENSING_QPOASES" # FULL_CONDENSING_QPOASES
self.acados_ocp.solver_options.hessian_approx = "GAUSS_NEWTON"
self.acados_ocp.solver_options.integrator_type = "ERK"
self.acados_ocp.solver_options.nlp_solver_type = "SQP"
self.acados_ocp.solver_options.nlp_solver_tol_comp = 1e-06
self.acados_ocp.solver_options.nlp_solver_tol_eq = 1e-06
self.acados_ocp.solver_options.nlp_solver_tol_ineq = 1e-06
self.acados_ocp.solver_options.nlp_solver_tol_stat = 1e-06
self.acados_ocp.solver_options.nlp_solver_max_iter = 200
self.acados_ocp.solver_options.sim_method_newton_iter = 5
self.acados_ocp.solver_options.sim_method_num_stages = 4
self.acados_ocp.solver_options.sim_method_num_steps = 1
self.acados_ocp.solver_options.print_level = 1
for key in options:
setattr(self.acados_ocp.solver_options, key, options[key])
def get_iterations(self):
raise NotImplementedError(
"return_iterations is not implemented yet with ACADOS backend")
def online_optim(self, ocp):
raise NotImplementedError(
"online_optim is not implemented yet with ACADOS backend")
def get_optimized_value(self, ocp):
acados_x = np.array([
self.ocp_solver.get(i, "x") for i in range(ocp.nlp[0]["ns"] + 1)
]).T
acados_q = acados_x[:ocp.nlp[0]["nu"], :]
acados_qdot = acados_x[ocp.nlp[0]["nu"]:, :]
acados_u = np.array(
[self.ocp_solver.get(i, "u") for i in range(ocp.nlp[0]["ns"])]).T
out = {
"qqdot": acados_x,
"x": [],
"u": acados_u,
"time_tot": self.ocp_solver.get_stats("time_tot")[0],
}
for i in range(ocp.nlp[0]["ns"]):
out["x"] = vertcat(out["x"], acados_q[:, i])
out["x"] = vertcat(out["x"], acados_qdot[:, i])
out["x"] = vertcat(out["x"], acados_u[:, i])
out["x"] = vertcat(out["x"], acados_q[:, ocp.nlp[0]["ns"]])
out["x"] = vertcat(out["x"], acados_qdot[:, ocp.nlp[0]["ns"]])
return out
def solve(self):
# populate costs vectors
self.__set_costs(self.ocp)
if self.ocp_solver is None:
self.ocp_solver = AcadosOcpSolver(self.acados_ocp,
json_file="acados_ocp.json")
for n in range(self.acados_ocp.dims.N):
self.ocp_solver.cost_set(
n, "yref",
np.concatenate([data[n] for data in self.y_ref])[:, 0])
self.ocp_solver.solve()
return self
class AcadosInterface(SolverInterface):
def __init__(self, ocp, **solver_options):
if not isinstance(ocp.CX(), SX):
raise RuntimeError(
"CasADi graph must be SX to be solved with ACADOS. Please set use_SX to True in OCP"
)
super().__init__(ocp)
# If Acados is installed using the acados_install.sh file, you probably can leave this to unset
acados_path = ""
if "acados_dir" in solver_options:
acados_path = solver_options["acados_dir"]
self.acados_ocp = AcadosOcp(acados_path=acados_path)
self.acados_model = AcadosModel()
if "cost_type" in solver_options:
self.__set_cost_type(solver_options["cost_type"])
else:
self.__set_cost_type()
if "constr_type" in solver_options:
self.__set_constr_type(solver_options["constr_type"])
else:
self.__set_constr_type()
self.lagrange_costs = SX()
self.mayer_costs = SX()
self.y_ref = []
self.y_ref_end = []
self.__acados_export_model(ocp)
self.__prepare_acados(ocp)
self.ocp_solver = None
self.W = np.zeros((0, 0))
self.W_e = np.zeros((0, 0))
self.status = None
self.out = {}
def __acados_export_model(self, ocp):
if ocp.nb_phases > 1:
raise NotImplementedError(
"More than 1 phase is not implemented yet with ACADOS backend")
# Declare model variables
x = ocp.nlp[0].X[0]
u = ocp.nlp[0].U[0]
p = ocp.nlp[0].p
if ocp.nlp[0].parameters_to_optimize:
for n in range(ocp.nb_phases):
for i in range(len(ocp.nlp[0].parameters_to_optimize)):
if str(ocp.nlp[0].p[i]) == f"time_phase_{n}":
raise RuntimeError(
"Time constraint not implemented yet with Acados.")
self.params = ocp.nlp[0].parameters_to_optimize
x = vertcat(p, x)
x_dot = SX.sym("x_dot", x.shape[0], x.shape[1])
f_expl = vertcat([0] * ocp.nlp[0].np,
ocp.nlp[0].dynamics_func(x[ocp.nlp[0].np:, :], u, p))
f_impl = x_dot - f_expl
self.acados_model.f_impl_expr = f_impl
self.acados_model.f_expl_expr = f_expl
self.acados_model.x = x
self.acados_model.xdot = x_dot
self.acados_model.u = u
self.acados_model.con_h_expr = np.zeros((0, 0))
self.acados_model.con_h_expr_e = np.zeros((0, 0))
self.acados_model.p = []
now = datetime.now() # current date and time
self.acados_model.name = f"model_{now.strftime('%Y_%m_%d_%H%M%S%f')[:-4]}"
def __prepare_acados(self, ocp):
# set model
self.acados_ocp.model = self.acados_model
# set time
self.acados_ocp.solver_options.tf = ocp.nlp[0].tf
# set dimensions
self.acados_ocp.dims.nx = ocp.nlp[0].nx + ocp.nlp[0].np
self.acados_ocp.dims.nu = ocp.nlp[0].nu
self.acados_ocp.dims.N = ocp.nlp[0].ns
def __set_constr_type(self, constr_type="BGH"):
self.acados_ocp.constraints.constr_type = constr_type
self.acados_ocp.constraints.constr_type_e = constr_type
def __set_constrs(self, ocp):
# constraints handling in self.acados_ocp
u_min = np.array(ocp.nlp[0].u_bounds.min)
u_max = np.array(ocp.nlp[0].u_bounds.max)
x_min = np.array(ocp.nlp[0].x_bounds.min)
x_max = np.array(ocp.nlp[0].x_bounds.max)
self.all_constr = SX()
self.end_constr = SX()
##TODO:change for more node flexibility on bounds
self.all_g_bounds = Bounds(interpolation=InterpolationType.CONSTANT)
self.end_g_bounds = Bounds(interpolation=InterpolationType.CONSTANT)
for i in range(ocp.nb_phases):
for g, G in enumerate(ocp.nlp[i].g):
if not G:
continue
if G[0]["constraint"].node[0] is Node.ALL:
self.all_constr = vertcat(self.all_constr,
G[0]["val"].reshape((-1, 1)))
self.all_g_bounds.concatenate(G[0]["bounds"])
if len(G) > ocp.nlp[0].ns:
constr_end_func_tp = Function(
f"cas_constr_end_func_{i}_{g}", [ocp.nlp[i].X[-1]],
[G[0]["val"]])
self.end_constr = vertcat(
self.end_constr,
constr_end_func_tp(ocp.nlp[i].X[0]).reshape(
(-1, 1)))
self.end_g_bounds.concatenate(G[0]["bounds"])
elif G[0]["constraint"].node[0] is Node.END:
constr_end_func_tp = Function(
f"cas_constr_end_func_{i}_{g}", [ocp.nlp[i].X[-1]],
[G[0]["val"]])
self.end_constr = vertcat(
self.end_constr,
constr_end_func_tp(ocp.nlp[i].X[0]).reshape((-1, 1)))
self.end_g_bounds.concatenate(G[0]["bounds"])
else:
raise RuntimeError(
"Except for states and controls, Acados solver only handles constraints on last or all nodes."
)
self.acados_model.con_h_expr = self.all_constr
self.acados_model.con_h_expr_e = self.end_constr
if not np.all(np.all(u_min.T == u_min.T[0, :], axis=0)):
raise NotImplementedError(
"u_bounds min must be the same at each shooting point with ACADOS"
)
if not np.all(np.all(u_max.T == u_max.T[0, :], axis=0)):
raise NotImplementedError(
"u_bounds max must be the same at each shooting point with ACADOS"
)
if (not np.isfinite(u_min).all() or not np.isfinite(x_min).all()
or not np.isfinite(u_max).all()
or not np.isfinite(x_max).all()):
raise NotImplementedError(
"u_bounds and x_bounds cannot be set to infinity in ACADOS. Consider changing it"
"to a big value instead.")
# setup state constraints
self.x_bound_max = np.ndarray((self.acados_ocp.dims.nx, 3))
self.x_bound_min = np.ndarray((self.acados_ocp.dims.nx, 3))
param_bounds_max = []
param_bounds_min = []
if self.params:
param_bounds_max = np.concatenate(
[self.params[key].bounds.max for key in self.params.keys()])[:,
0]
param_bounds_min = np.concatenate(
[self.params[key].bounds.min for key in self.params.keys()])[:,
0]
for i in range(3):
self.x_bound_max[:, i] = np.concatenate(
(param_bounds_max, np.array(ocp.nlp[0].x_bounds.max[:, i])))
self.x_bound_min[:, i] = np.concatenate(
(param_bounds_min, np.array(ocp.nlp[0].x_bounds.min[:, i])))
# setup control constraints
self.acados_ocp.constraints.lbu = np.array(ocp.nlp[0].u_bounds.min[:,
0])
self.acados_ocp.constraints.ubu = np.array(ocp.nlp[0].u_bounds.max[:,
0])
self.acados_ocp.constraints.idxbu = np.array(
range(self.acados_ocp.dims.nu))
self.acados_ocp.dims.nbu = self.acados_ocp.dims.nu
# initial state constraints
self.acados_ocp.constraints.lbx_0 = self.x_bound_min[:, 0]
self.acados_ocp.constraints.ubx_0 = self.x_bound_max[:, 0]
self.acados_ocp.constraints.idxbx_0 = np.array(
range(self.acados_ocp.dims.nx))
self.acados_ocp.dims.nbx_0 = self.acados_ocp.dims.nx
# setup path state constraints
self.acados_ocp.constraints.Jbx = np.eye(self.acados_ocp.dims.nx)
self.acados_ocp.constraints.lbx = self.x_bound_min[:, 1]
self.acados_ocp.constraints.ubx = self.x_bound_max[:, 1]
self.acados_ocp.constraints.idxbx = np.array(
range(self.acados_ocp.dims.nx))
self.acados_ocp.dims.nbx = self.acados_ocp.dims.nx
# setup terminal state constraints
self.acados_ocp.constraints.Jbx_e = np.eye(self.acados_ocp.dims.nx)
self.acados_ocp.constraints.lbx_e = self.x_bound_min[:, -1]
self.acados_ocp.constraints.ubx_e = self.x_bound_max[:, -1]
self.acados_ocp.constraints.idxbx_e = np.array(
range(self.acados_ocp.dims.nx))
self.acados_ocp.dims.nbx_e = self.acados_ocp.dims.nx
# setup algebraic constraint
self.acados_ocp.constraints.lh = np.array(self.all_g_bounds.min[:, 0])
self.acados_ocp.constraints.uh = np.array(self.all_g_bounds.max[:, 0])
# setup terminal algebraic constraint
self.acados_ocp.constraints.lh_e = np.array(self.end_g_bounds.min[:,
0])
self.acados_ocp.constraints.uh_e = np.array(self.end_g_bounds.max[:,
0])
def __set_cost_type(self, cost_type="NONLINEAR_LS"):
self.acados_ocp.cost.cost_type = cost_type
self.acados_ocp.cost.cost_type_e = cost_type
def __set_costs(self, ocp):
if ocp.nb_phases != 1:
raise NotImplementedError(
"ACADOS with more than one phase is not implemented yet.")
# costs handling in self.acados_ocp
self.y_ref = []
self.y_ref_end = []
self.lagrange_costs = SX()
self.mayer_costs = SX()
self.W = np.zeros((0, 0))
self.W_e = np.zeros((0, 0))
ctrl_objs = [
PenaltyType.MINIMIZE_TORQUE,
PenaltyType.MINIMIZE_MUSCLES_CONTROL,
PenaltyType.MINIMIZE_ALL_CONTROLS,
]
state_objs = [
PenaltyType.MINIMIZE_STATE,
]
if self.acados_ocp.cost.cost_type == "LINEAR_LS":
self.Vu = np.array([], dtype=np.int64).reshape(0, ocp.nlp[0].nu)
self.Vx = np.array([], dtype=np.int64).reshape(0, ocp.nlp[0].nx)
self.Vxe = np.array([], dtype=np.int64).reshape(0, ocp.nlp[0].nx)
for i in range(ocp.nb_phases):
for j, J in enumerate(ocp.nlp[i].J):
if J[0]["objective"].type.get_type(
) == ObjectiveFunction.LagrangeFunction:
if J[0]["objective"].type.value[0] in ctrl_objs:
index = (J[0]["objective"].index
if J[0]["objective"].index else list(
np.arange(ocp.nlp[0].nu)))
vu = np.zeros(ocp.nlp[0].nu)
vu[index] = 1.0
self.Vu = np.vstack((self.Vu, np.diag(vu)))
self.Vx = np.vstack(
(self.Vx,
np.zeros((ocp.nlp[0].nu, ocp.nlp[0].nx))))
self.W = linalg.block_diag(
self.W,
np.diag([J[0]["objective"].weight] *
ocp.nlp[0].nu))
if J[0]["target"] is not None:
y_tp = np.zeros((ocp.nlp[0].nu, 1))
y_ref_tp = []
for J_tp in J:
y_tp[index] = J_tp["target"].T.reshape(
(-1, 1))
y_ref_tp.append(y_tp)
self.y_ref.append(y_ref_tp)
else:
self.y_ref.append([
np.zeros((ocp.nlp[0].nu, 1)) for J_tp in J
])
elif J[0]["objective"].type.value[0] in state_objs:
index = (J[0]["objective"].index
if J[0]["objective"].index else list(
np.arange(ocp.nlp[0].nx)))
vx = np.zeros(ocp.nlp[0].nx)
vx[index] = 1.0
self.Vx = np.vstack((self.Vx, np.diag(vx)))
self.Vu = np.vstack(
(self.Vu,
np.zeros((ocp.nlp[0].nx, ocp.nlp[0].nu))))
self.W = linalg.block_diag(
self.W,
np.diag([J[0]["objective"].weight] *
ocp.nlp[0].nx))
if J[0]["target"] is not None:
y_ref_tp = []
for J_tp in J:
y_tp = np.zeros((ocp.nlp[0].nx, 1))
y_tp[index] = J_tp["target"].T.reshape(
(-1, 1))
y_ref_tp.append(y_tp)
self.y_ref.append(y_ref_tp)
else:
self.y_ref.append([
np.zeros((ocp.nlp[0].nx, 1)) for J_tp in J
])
else:
raise RuntimeError(
f"{J[0]['objective'].type.name} is an incompatible objective term with "
f"LINEAR_LS cost type")
# Deal with last node to match ipopt formulation
if J[0]["objective"].node[0].value == "all" and len(
J) > ocp.nlp[0].ns:
index = (J[0]["objective"].index
if J[0]["objective"].index else list(
np.arange(ocp.nlp[0].nx)))
vxe = np.zeros(ocp.nlp[0].nx)
vxe[index] = 1.0
self.Vxe = np.vstack((self.Vxe, np.diag(vxe)))
self.W_e = linalg.block_diag(
self.W_e,
np.diag([J[0]["objective"].weight] *
ocp.nlp[0].nx))
if J[0]["target"] is not None:
y_tp = np.zeros((ocp.nlp[0].nx, 1))
y_tp[index] = J[-1]["target"].T.reshape(
(-1, 1))
self.y_ref_end.append(y_tp)
else:
self.y_ref_end.append(
np.zeros((ocp.nlp[0].nx, 1)))
elif J[0]["objective"].type.get_type(
) == ObjectiveFunction.MayerFunction:
if J[0]["objective"].type.value[0] in state_objs:
index = (J[0]["objective"].index
if J[0]["objective"].index else list(
np.arange(ocp.nlp[0].nx)))
vxe = np.zeros(ocp.nlp[0].nx)
vxe[index] = 1.0
self.Vxe = np.vstack((self.Vxe, np.diag(vxe)))
self.W_e = linalg.block_diag(
self.W_e,
np.diag([J[0]["objective"].weight] *
ocp.nlp[0].nx))
if J[0]["target"] is not None:
y_tp = np.zeros((ocp.nlp[0].nx, 1))
y_tp[index] = J[-1]["target"].T.reshape(
(-1, 1))
self.y_ref_end.append(y_tp)
else:
self.y_ref_end.append(
np.zeros((ocp.nlp[0].nx, 1)))
else:
raise RuntimeError(
f"{J[0]['objective'].type.name} is an incompatible objective term "
f"with LINEAR_LS cost type")
else:
raise RuntimeError(
"The objective function is not Lagrange nor Mayer."
)
if self.params:
raise RuntimeError(
"Params not yet handled with LINEAR_LS cost type")
# Set costs
self.acados_ocp.cost.Vx = self.Vx if self.Vx.shape[0] else np.zeros(
(0, 0))
self.acados_ocp.cost.Vu = self.Vu if self.Vu.shape[0] else np.zeros(
(0, 0))
self.acados_ocp.cost.Vx_e = self.Vxe if self.Vxe.shape[
0] else np.zeros((0, 0))
# Set dimensions
self.acados_ocp.dims.ny = sum(
[len(data[0]) for data in self.y_ref])
self.acados_ocp.dims.ny_e = sum(
[len(data) for data in self.y_ref_end])
# Set weight
self.acados_ocp.cost.W = self.W
self.acados_ocp.cost.W_e = self.W_e
# Set target shape
self.acados_ocp.cost.yref = np.zeros(
(self.acados_ocp.cost.W.shape[0], ))
self.acados_ocp.cost.yref_e = np.zeros(
(self.acados_ocp.cost.W_e.shape[0], ))
elif self.acados_ocp.cost.cost_type == "NONLINEAR_LS":
for i in range(ocp.nb_phases):
for j, J in enumerate(ocp.nlp[i].J):
if J[0]["objective"].type.get_type(
) == ObjectiveFunction.LagrangeFunction:
self.lagrange_costs = vertcat(
self.lagrange_costs, J[0]["val"].reshape((-1, 1)))
self.W = linalg.block_diag(
self.W,
np.diag([J[0]["objective"].weight] *
J[0]["val"].numel()))
if J[0]["target"] is not None:
self.y_ref.append([
J_tp["target"].T.reshape((-1, 1)) for J_tp in J
])
else:
self.y_ref.append([
np.zeros((J_tp["val"].numel(), 1))
for J_tp in J
])
# Deal with last node to match ipopt formulation
if J[0]["objective"].node[0].value == "all" and len(
J) > ocp.nlp[0].ns:
mayer_func_tp = Function(f"cas_mayer_func_{i}_{j}",
[ocp.nlp[i].X[-1]],
[J[0]["val"]])
self.W_e = linalg.block_diag(
self.W_e,
np.diag([J[0]["objective"].weight] *
J[0]["val"].numel()))
self.mayer_costs = vertcat(
self.mayer_costs,
mayer_func_tp(ocp.nlp[i].X[0]).reshape(
(-1, 1)))
if J[0]["target"] is not None:
self.y_ref_end.append(
J[-1]["target"].T.reshape((-1, 1)))
else:
self.y_ref_end.append(
np.zeros((J[-1]["val"].numel(), 1)))
elif J[0]["objective"].type.get_type(
) == ObjectiveFunction.MayerFunction:
mayer_func_tp = Function(f"cas_mayer_func_{i}_{j}",
[ocp.nlp[i].X[-1]],
[J[0]["val"]])
self.W_e = linalg.block_diag(
self.W_e,
np.diag([J[0]["objective"].weight] *
J[0]["val"].numel()))
self.mayer_costs = vertcat(
self.mayer_costs,
mayer_func_tp(ocp.nlp[i].X[0]).reshape((-1, 1)))
if J[0]["target"] is not None:
self.y_ref_end.append(J[0]["target"].T.reshape(
(-1, 1)))
else:
self.y_ref_end.append(
np.zeros((J[0]["val"].numel(), 1)))
else:
raise RuntimeError(
"The objective function is not Lagrange nor Mayer."
)
# parameter as mayer function
# IMPORTANT: it is considered that only parameters are stored in ocp.J, for now.
if self.params:
for j, J in enumerate(ocp.J):
mayer_func_tp = Function(f"cas_J_mayer_func_{i}_{j}",
[ocp.nlp[i].X[-1]],
[J[0]["val"]])
self.W_e = linalg.block_diag(
self.W_e,
np.diag(([J[0]["objective"].weight] *
J[0]["val"].numel())))
self.mayer_costs = vertcat(
self.mayer_costs,
mayer_func_tp(ocp.nlp[i].X[0]).reshape((-1, 1)))
if J[0]["target"] is not None:
self.y_ref_end.append(J[0]["target"].T.reshape(
(-1, 1)))
else:
self.y_ref_end.append(
np.zeros((J[0]["val"].numel(), 1)))
# Set costs
self.acados_ocp.model.cost_y_expr = self.lagrange_costs if self.lagrange_costs.numel(
) else SX(1, 1)
self.acados_ocp.model.cost_y_expr_e = self.mayer_costs if self.mayer_costs.numel(
) else SX(1, 1)
# Set dimensions
self.acados_ocp.dims.ny = self.acados_ocp.model.cost_y_expr.shape[
0]
self.acados_ocp.dims.ny_e = self.acados_ocp.model.cost_y_expr_e.shape[
0]
# Set weight
self.acados_ocp.cost.W = np.zeros(
(1, 1)) if self.W.shape == (0, 0) else self.W
self.acados_ocp.cost.W_e = np.zeros(
(1, 1)) if self.W_e.shape == (0, 0) else self.W_e
# Set target shape
self.acados_ocp.cost.yref = np.zeros(
(self.acados_ocp.cost.W.shape[0], ))
self.acados_ocp.cost.yref_e = np.zeros(
(self.acados_ocp.cost.W_e.shape[0], ))
elif self.acados_ocp.cost.cost_type == "EXTERNAL":
raise RuntimeError(
"EXTERNAL is not interfaced yet, please use NONLINEAR_LS")
else:
raise RuntimeError(
"Available acados cost type: 'LINEAR_LS', 'NONLINEAR_LS' and 'EXTERNAL'."
)
def __update_solver(self):
param_init = []
for n in range(self.acados_ocp.dims.N):
if self.y_ref:
self.ocp_solver.cost_set(
n, "yref",
np.vstack([data[n] for data in self.y_ref])[:, 0])
# check following line
# self.ocp_solver.cost_set(n, "W", self.W)
if self.params:
param_init = np.concatenate([
self.params[key].initial_guess.init.evaluate_at(n)
for key in self.params.keys()
])
self.ocp_solver.set(
n, "x",
np.concatenate(
(param_init, self.ocp.nlp[0].x_init.init.evaluate_at(n))))
self.ocp_solver.set(n, "u",
self.ocp.nlp[0].u_init.init.evaluate_at(n))
self.ocp_solver.constraints_set(n, "lbu",
self.ocp.nlp[0].u_bounds.min[:, 0])
self.ocp_solver.constraints_set(n, "ubu",
self.ocp.nlp[0].u_bounds.max[:, 0])
self.ocp_solver.constraints_set(n, "uh", self.all_g_bounds.max[:,
0])
self.ocp_solver.constraints_set(n, "lh", self.all_g_bounds.min[:,
0])
if n == 0:
self.ocp_solver.constraints_set(n, "lbx", self.x_bound_min[:,
0])
self.ocp_solver.constraints_set(n, "ubx", self.x_bound_max[:,
0])
else:
self.ocp_solver.constraints_set(n, "lbx", self.x_bound_min[:,
1])
self.ocp_solver.constraints_set(n, "ubx", self.x_bound_max[:,
1])
if self.y_ref_end:
if len(self.y_ref_end) == 1:
self.ocp_solver.cost_set(self.acados_ocp.dims.N, "yref",
np.array(self.y_ref_end[0])[:, 0])
else:
self.ocp_solver.cost_set(
self.acados_ocp.dims.N, "yref",
np.concatenate([data for data in self.y_ref_end])[:, 0])
# check following line
# self.ocp_solver.cost_set(self.acados_ocp.dims.N, "W", self.W_e)
self.ocp_solver.constraints_set(self.acados_ocp.dims.N, "lbx",
self.x_bound_min[:, -1])
self.ocp_solver.constraints_set(self.acados_ocp.dims.N, "ubx",
self.x_bound_max[:, -1])
if len(self.end_g_bounds.max[:, 0]):
self.ocp_solver.constraints_set(self.acados_ocp.dims.N, "uh",
self.end_g_bounds.max[:, 0])
self.ocp_solver.constraints_set(self.acados_ocp.dims.N, "lh",
self.end_g_bounds.min[:, 0])
if self.ocp.nlp[0].x_init.init.shape[1] == self.acados_ocp.dims.N + 1:
if self.params:
self.ocp_solver.set(
self.acados_ocp.dims.N,
"x",
np.concatenate((
np.concatenate([
self.params[key]["initial_guess"].init
for key in self.params.keys()
])[:, 0],
self.ocp.nlp[0].x_init.init[:, self.acados_ocp.dims.N],
)),
)
else:
self.ocp_solver.set(
self.acados_ocp.dims.N, "x",
self.ocp.nlp[0].x_init.init[:, self.acados_ocp.dims.N])
def configure(self, options):
if "acados_dir" in options:
del options["acados_dir"]
if "cost_type" in options:
del options["cost_type"]
if self.ocp_solver is None:
self.acados_ocp.solver_options.qp_solver = "PARTIAL_CONDENSING_HPIPM" # FULL_CONDENSING_QPOASES
self.acados_ocp.solver_options.hessian_approx = "GAUSS_NEWTON"
self.acados_ocp.solver_options.integrator_type = "IRK"
self.acados_ocp.solver_options.nlp_solver_type = "SQP"
self.acados_ocp.solver_options.nlp_solver_tol_comp = 1e-06
self.acados_ocp.solver_options.nlp_solver_tol_eq = 1e-06
self.acados_ocp.solver_options.nlp_solver_tol_ineq = 1e-06
self.acados_ocp.solver_options.nlp_solver_tol_stat = 1e-06
self.acados_ocp.solver_options.nlp_solver_max_iter = 200
self.acados_ocp.solver_options.sim_method_newton_iter = 5
self.acados_ocp.solver_options.sim_method_num_stages = 4
self.acados_ocp.solver_options.sim_method_num_steps = 1
self.acados_ocp.solver_options.print_level = 1
for key in options:
setattr(self.acados_ocp.solver_options, key, options[key])
else:
available_options = [
"nlp_solver_tol_comp",
"nlp_solver_tol_eq",
"nlp_solver_tol_ineq",
"nlp_solver_tol_stat",
]
for key in options:
if key in available_options:
short_key = key[11:]
self.ocp_solver.options_set(short_key, options[key])
else:
raise RuntimeError(
f"[ACADOS] Only editable solver options after solver creation are :\n {available_options}"
)
def get_iterations(self):
raise NotImplementedError(
"return_iterations is not implemented yet with ACADOS backend")
def online_optim(self, ocp):
raise NotImplementedError(
"online_optim is not implemented yet with ACADOS backend")
def get_optimized_value(self):
ns = self.acados_ocp.dims.N
nx = self.acados_ocp.dims.nx
nq = self.ocp.nlp[0].q.shape[0]
nparams = self.ocp.nlp[0].np
acados_x = np.array(
[self.ocp_solver.get(i, "x") for i in range(ns + 1)]).T
acados_p = acados_x[:nparams, :]
acados_q = acados_x[nparams:nq + nparams, :]
acados_qdot = acados_x[nq + nparams:nx, :]
acados_u = np.array([self.ocp_solver.get(i, "u") for i in range(ns)]).T
out = {
"qqdot": acados_x,
"x": [],
"u": acados_u,
"time_tot": self.ocp_solver.get_stats("time_tot")[0],
"iter": self.ocp_solver.get_stats("sqp_iter")[0],
"status": self.status,
}
for i in range(ns):
out["x"] = vertcat(out["x"], acados_q[:, i])
out["x"] = vertcat(out["x"], acados_qdot[:, i])
out["x"] = vertcat(out["x"], acados_u[:, i])
out["x"] = vertcat(out["x"], acados_q[:, ns])
out["x"] = vertcat(out["x"], acados_qdot[:, ns])
out["x"] = vertcat(acados_p[:, 0], out["x"])
self.out["sol"] = out
out = []
for key in self.out.keys():
out.append(self.out[key])
return out[0] if len(out) == 1 else out
def solve(self):
# Populate costs and constrs vectors
self.__set_costs(self.ocp)
self.__set_constrs(self.ocp)
if self.ocp_solver is None:
self.ocp_solver = AcadosOcpSolver(self.acados_ocp,
json_file="acados_ocp.json")
self.__update_solver()
self.status = self.ocp_solver.solve()
self.get_optimized_value()
return self
def __set_costs(self, ocp):
self.y_ref = []
self.y_ref_end = []
self.lagrange_costs = SX()
self.mayer_costs = SX()
self.W = np.zeros((0, 0))
self.W_e = np.zeros((0, 0))
if self.acados_ocp.cost.cost_type == "LINEAR_LS":
# set Lagrange terms
self.acados_ocp.cost.Vx = np.zeros(
(self.acados_ocp.dims.ny, self.acados_ocp.dims.nx))
self.acados_ocp.cost.Vx[:self.acados_ocp.dims.nx, :] = np.eye(
self.acados_ocp.dims.nx)
Vu = np.zeros((self.acados_ocp.dims.ny, self.acados_ocp.dims.nu))
Vu[self.acados_ocp.dims.nx:, :] = np.eye(self.acados_ocp.dims.nu)
self.acados_ocp.cost.Vu = Vu
# set Mayer term
self.acados_ocp.cost.Vx_e = np.zeros(
(self.acados_ocp.dims.nx, self.acados_ocp.dims.nx))
elif self.acados_ocp.cost.cost_type == "NONLINEAR_LS":
if ocp.nb_phases != 1:
raise NotImplementedError(
"ACADOS with more than one phase is not implemented yet")
for i in range(ocp.nb_phases):
# TODO: I think ocp.J is missing here (the parameters would be stored there)
# TODO: Yes the objectives in ocp are not dealt with,
# does that makes sense since we only work with 1 phase ?
for j, J in enumerate(ocp.nlp[i]["J"]):
if J[0]["objective"].type.get_type(
) == ObjectiveFunction.LagrangeFunction:
self.lagrange_costs = vertcat(
self.lagrange_costs, J[0]["val"].reshape((-1, 1)))
self.W = linalg.block_diag(
self.W,
np.diag([J[0]["objective"].weight] *
J[0]["val"].numel()))
if J[0]["target"] is not None:
self.y_ref.append([
J_tp["target"].T.reshape((-1, 1)) for J_tp in J
])
else:
self.y_ref.append([
np.zeros((J_tp["val"].numel(), 1))
for J_tp in J
])
elif J[0]["objective"].type.get_type(
) == ObjectiveFunction.MayerFunction:
mayer_func_tp = Function(f"cas_mayer_func_{i}_{j}",
[ocp.nlp[i]["X"][-1]],
[J[0]["val"]])
self.W_e = linalg.block_diag(
self.W_e,
np.diag([J[0]["objective"].weight] *
J[0]["val"].numel()))
self.mayer_costs = vertcat(
self.mayer_costs,
mayer_func_tp(ocp.nlp[i]["X"][0]))
if J[0]["target"] is not None:
self.y_ref_end.append([J[0]["target"]])
else:
self.y_ref_end.append(
[np.zeros((J[0]["val"].numel(), 1))])
else:
raise RuntimeError(
"The objective function is not Lagrange nor Mayer."
)
if self.lagrange_costs.numel():
self.acados_ocp.model.cost_y_expr = self.lagrange_costs
else:
self.acados_ocp.model.cost_y_expr = SX(1, 1)
if self.mayer_costs.numel():
self.acados_ocp.model.cost_y_expr_e = self.mayer_costs
else:
self.acados_ocp.model.cost_y_expr_e = SX(1, 1)
self.acados_ocp.dims.ny = self.acados_ocp.model.cost_y_expr.shape[
0]
self.acados_ocp.dims.ny_e = self.acados_ocp.model.cost_y_expr_e.shape[
0]
self.acados_ocp.cost.yref = np.zeros((max(self.acados_ocp.dims.ny,
1), ))
self.acados_ocp.cost.yref_e = np.concatenate(self.y_ref_end,
-1).T.squeeze()
if self.W.shape == (0, 0):
self.acados_ocp.cost.W = np.zeros((1, 1))
else:
self.acados_ocp.cost.W = self.W
if self.W_e.shape == (0, 0):
self.acados_ocp.cost.W_e = np.zeros((1, 1))
else:
self.acados_ocp.cost.W_e = self.W_e
elif self.acados_ocp.cost.cost_type == "EXTERNAL":
for i in range(ocp.nb_phases):
for j in range(len(ocp.nlp[i]["J"])):
J = ocp.nlp[i]["J"][j][0]
raise RuntimeError(
"TODO: The target is not right currently")
if J["type"] == ObjectiveFunction.LagrangeFunction:
self.lagrange_costs = vertcat(
self.lagrange_costs, J["val"][0] - J["target"][0])
elif J["type"] == ObjectiveFunction.MayerFunction:
raise RuntimeError(
"TODO: I may have broken this (is this the right J?)"
)
mayer_func_tp = Function(f"cas_mayer_func_{i}_{j}",
[ocp.nlp[i]["X"][-1]],
[J["val"]])
self.mayer_costs = vertcat(
self.mayer_costs,
mayer_func_tp(ocp.nlp[i]["X"][0]))
else:
raise RuntimeError(
"The objective function is not Lagrange nor Mayer."
)
self.acados_ocp.model.cost_expr_ext_cost = sum1(
self.lagrange_costs)
self.acados_ocp.model.cost_expr_ext_cost_e = sum1(self.mayer_costs)
else:
raise RuntimeError(
"Available acados cost type: 'LINEAR_LS', 'NONLINEAR_LS' and 'EXTERNAL'."
)
Vu[4,0] = 1.0
ocp.cost.Vu = Vu
ocp.cost.Vx_e = np.eye(nx)
ocp.cost.yref = np.zeros((ny, ))
ocp.cost.yref_e = np.zeros((ny_e, ))
# set constraints
Fmax = 80
# use equivalent formulation with h constraint
# ocp.constraints.lbu = np.array([-Fmax])
# ocp.constraints.ubu = np.array([+Fmax])
# ocp.constraints.idxbu = np.array([0])
p = SX.sym('p')
ocp.model.p = p
ocp.constraints.lh = np.array([-Fmax])
ocp.constraints.uh = np.array([+Fmax])
ocp.model.con_h_expr = model.u / p
ocp.parameter_values = np.array([0])
ocp.constraints.x0 = np.array([0.0, np.pi, 0.0, 0.0])
ocp.solver_options.qp_solver = 'PARTIAL_CONDENSING_HPIPM' # FULL_CONDENSING_QPOASES
ocp.solver_options.hessian_approx = 'EXACT' # GAUSS_NEWTON, EXACT
ocp.solver_options.regularize_method = 'CONVEXIFY' # GAUSS_NEWTON, EXACT
ocp.solver_options.integrator_type = 'ERK'
ocp.solver_options.print_level = 0
def wrapperRWSC(IC, args, optimal):
# Converting the Optimal Control Problem into a Non-Linear Programming Problem
numStates = 6
numInputs = 2
nodes = 30 # Keep this Number Small to Reduce Runtime
dt = SX.sym("dt")
states = SX.sym("state", nodes, numStates)
controls = SX.sym("controls", nodes, numInputs)
variables_list = [dt, states, controls]
variables_name = ["dt", "states", "controls"]
variables_flat = casadi.vertcat(*[casadi.reshape(e, -1, 1) for e in variables_list])
pack_variables_fn = casadi.Function(
"pack_variables_fn", variables_list, [variables_flat], variables_name, ["flat"]
)
unpack_variables_fn = casadi.Function(
"unpack_variables_fn",
[variables_flat],
variables_list,
["flat"],
variables_name,
)
# Bounds
bds = [
[np.sqrt(np.finfo(float).eps), np.inf],
[-100, 300],
[0, np.inf],
[-np.inf, np.inf],
[-np.inf, np.inf],
[np.sqrt(np.finfo(float).eps), np.inf],
[-1, 1],
[np.sqrt(np.finfo(float).eps), 1],
]
lower_bounds = unpack_variables_fn(flat=-float("inf"))
lower_bounds["dt"][:, :] = bds[0][0]
lower_bounds["states"][:, 0] = bds[1][0]
lower_bounds["states"][:, 1] = bds[2][0]
lower_bounds["states"][:, 4] = bds[5][0]
lower_bounds["controls"][:, 0] = bds[6][0]
lower_bounds["controls"][:, 1] = bds[7][0]
upper_bounds = unpack_variables_fn(flat=float("inf"))
upper_bounds["dt"][:, :] = bds[0][1]
upper_bounds["states"][:, 0] = bds[1][1]
upper_bounds["controls"][:, 0] = bds[6][1]
upper_bounds["controls"][:, 1] = bds[7][1]
# Set Initial Conditions
# Casadi does not accept equality constraints, so boundary constraints are
# set as box constraints with 0 area.
lower_bounds["states"][0, 0] = IC[0]
lower_bounds["states"][0, 1] = IC[1]
lower_bounds["states"][0, 2] = IC[2]
lower_bounds["states"][0, 3] = IC[3]
lower_bounds["states"][0, 4] = IC[4]
lower_bounds["states"][0, 5] = IC[5]
upper_bounds["states"][0, 0] = IC[0]
upper_bounds["states"][0, 1] = IC[1]
upper_bounds["states"][0, 2] = IC[2]
upper_bounds["states"][0, 3] = IC[3]
upper_bounds["states"][0, 4] = IC[4]
upper_bounds["states"][0, 5] = IC[5]
# Set Final Conditions
# Currently set for a soft touchdown at the origin
lower_bounds["states"][-1, 0] = 0
lower_bounds["states"][-1, 1] = 0
lower_bounds["states"][-1, 2] = 0
lower_bounds["states"][-1, 3] = 0
lower_bounds["states"][-1, 5] = 0
upper_bounds["states"][-1, 0] = 0
upper_bounds["states"][-1, 1] = 0
upper_bounds["states"][-1, 2] = 0
upper_bounds["states"][-1, 3] = 0
upper_bounds["states"][-1, 5] = 0
# Initial Guess Generation
# Generate the initial guess as a line between initial and final conditions
xIG = np.array(
[
np.linspace(IC[0], 0, nodes),
np.linspace(IC[1], 0, nodes),
np.linspace(IC[2], 0, nodes),
np.linspace(IC[3], 0, nodes),
np.linspace(IC[4], IC[4] * 0.5, nodes),
np.linspace(IC[5], 0, nodes),
]
).T
uIG = np.array([np.linspace(0, 1, nodes), np.linspace(1, 1, nodes)]).T
ig_list = [60 / nodes, xIG, uIG]
ig_flat = casadi.vertcat(*[casadi.reshape(e, -1, 1) for e in ig_list])
# Generating Defect Vector
xLow = states[0: (nodes - 1), :]
xHigh = states[1:nodes, :]
contLow = controls[0: (nodes - 1), :]
contHi = controls[1:nodes, :]
contMid = (contLow + contHi) / 2
# Use a RK4 Method for Generating the Defects
k1 = RWSC(xLow, contLow, args)
k2 = RWSC(xLow + (0.5 * dt * k1), contMid, args)
k3 = RWSC(xLow + (0.5 * dt * k2), contMid, args)
k4 = RWSC(xLow + k3, contHi, args)
xNew = xLow + ((dt / 6) * (k1 + (2 * k2) + (2 * k3) + k4))
defect = casadi.reshape(xNew - xHigh, -1, 1)
# Choose the Cost Function
if optimal == 1:
J = -states[-1, 4] # Mass Optimal Cost Function
elif optimal == 2:
J = dt[0] # Time Optimal Cost Function
# Put the OCP into a form that Casadi can solve
nlp = {"x": variables_flat, "f": J, "g": defect}
solver = casadi.nlpsol(
"solver", "bonmin", nlp
) # Use bonmin instead of ipopt due to speed
result = solver(
x0=ig_flat,
lbg=0.0,
ubg=0.0,
lbx=pack_variables_fn(**lower_bounds)["flat"],
ubx=pack_variables_fn(**upper_bounds)["flat"],
)
results = unpack_variables_fn(flat=result["x"])
return results
import matplotlib.pyplot as plt
from acados_template import AcadosOcp, AcadosOcpSolver, AcadosModel, AcadosSimSolver, AcadosSim
import numpy as np
from casadi import SX, vertcat
import pickle
# (very) simple crane model
beta = 0.001
k = 0.9
a_max = 10
dt_max = 2.0
# states
p1 = SX.sym('p1')
v1 = SX.sym('v1')
p2 = SX.sym('p2')
v2 = SX.sym('v2')
x = vertcat(p1, v1, p2, v2)
# controls
a = SX.sym('a')
dt = SX.sym('dt')
u = vertcat(a, dt)
f_expl = dt * vertcat(v1, a, v2, -beta * v2 - k * (p2 - p1))
model = AcadosModel()
eta_ub = 200
eta_lb = 0
# Abtastzeit und Prediction horizon
T = 0.1
N_pred = 3
# Zielruhelage von den Zustaenden
# Referenz von h, h_p, eta
state_r = [0.8, 0, 48]
input_r = [157.29]
if __name__ == '__main__':
plant_init()
NMPC_init()
# Zustaende als Symbol definieren
h = SX.sym('h')
h_p = SX.sym('h_p')
eta = SX.sym('eta')
# Zustandsvektor
states = vertcat(h, h_p, eta)
# print(states.shape)
# Eingang symbolisch definieren
u_pwm = SX.sym('u_pwm')
# Eingangsverktor
controls = vertcat(u_pwm)
# kriegen die Anzahl der Zeiler("shape" ist ein Tuple)
n_states = states.shape[0] # p 22.
# Eingangsanzahl
def gen_long_model():
model = AcadosModel()
model.name = 'long'
# set up states & controls
x_ego = SX.sym('x_ego')
v_ego = SX.sym('v_ego')
a_ego = SX.sym('a_ego')
model.x = vertcat(x_ego, v_ego, a_ego)
# controls
j_ego = SX.sym('j_ego')
model.u = vertcat(j_ego)
# xdot
x_ego_dot = SX.sym('x_ego_dot')
v_ego_dot = SX.sym('v_ego_dot')
a_ego_dot = SX.sym('a_ego_dot')
model.xdot = vertcat(x_ego_dot, v_ego_dot, a_ego_dot)
# live parameters
x_obstacle = SX.sym('x_obstacle')
a_min = SX.sym('a_min')
a_max = SX.sym('a_max')
prev_a = SX.sym('prev_a')
model.p = vertcat(a_min, a_max, x_obstacle, prev_a)
# dynamics model
f_expl = vertcat(v_ego, a_ego, j_ego)
model.f_impl_expr = model.xdot - f_expl
model.f_expl_expr = f_expl
return model
class AcadosInterface(SolverInterface):
def __init__(self, ocp, **solver_options):
if not isinstance(ocp.CX(), SX):
raise RuntimeError("CasADi graph must be SX to be solved with ACADOS. Please set use_SX to True in OCP")
super().__init__(ocp)
# If Acados is installed using the acados_install.sh file, you probably can leave this to unset
acados_path = ""
if "acados_dir" in solver_options:
acados_path = solver_options["acados_dir"]
self.acados_ocp = AcadosOcp(acados_path=acados_path)
self.acados_model = AcadosModel()
if "cost_type" in solver_options:
self.__set_cost_type(solver_options["cost_type"])
else:
self.__set_cost_type()
if "constr_type" in solver_options:
self.__set_constr_type(solver_options["constr_type"])
else:
self.__set_constr_type()
self.lagrange_costs = SX()
self.mayer_costs = SX()
self.y_ref = []
self.y_ref_end = []
self.__acados_export_model(ocp)
self.__prepare_acados(ocp)
self.ocp_solver = None
self.W = np.zeros((0, 0))
self.W_e = np.zeros((0, 0))
self.status = None
self.out = {}
def __acados_export_model(self, ocp):
# Declare model variables
x = ocp.nlp[0].X[0]
u = ocp.nlp[0].U[0]
p = ocp.nlp[0].p
self.params = ocp.nlp[0].parameters_to_optimize
x = vertcat(p, x)
x_dot = SX.sym("x_dot", x.shape[0], x.shape[1])
f_expl = vertcat([0] * ocp.nlp[0].np, ocp.nlp[0].dynamics_func(x[ocp.nlp[0].np :, :], u, p))
f_impl = x_dot - f_expl
self.acados_model.f_impl_expr = f_impl
self.acados_model.f_expl_expr = f_expl
self.acados_model.x = x
self.acados_model.xdot = x_dot
self.acados_model.u = u
self.acados_model.p = []
self.acados_model.name = "model_name"
def __prepare_acados(self, ocp):
if ocp.nb_phases > 1:
raise NotImplementedError("More than 1 phase is not implemented yet with ACADOS backend")
# set model
self.acados_ocp.model = self.acados_model
# set time
self.acados_ocp.solver_options.tf = ocp.nlp[0].tf
# set dimensions
self.acados_ocp.dims.nx = ocp.nlp[0].nx + ocp.nlp[0].np
self.acados_ocp.dims.nu = ocp.nlp[0].nu
self.acados_ocp.dims.N = ocp.nlp[0].ns
def __set_constr_type(self, constr_type="BGH"):
self.acados_ocp.constraints.constr_type = constr_type
self.acados_ocp.constraints.constr_type_e = constr_type
def __set_constrs(self, ocp):
# constraints handling in self.acados_ocp
u_min = np.array(ocp.nlp[0].u_bounds.min)
u_max = np.array(ocp.nlp[0].u_bounds.max)
x_min = np.array(ocp.nlp[0].x_bounds.min)
x_max = np.array(ocp.nlp[0].x_bounds.max)
if not np.all(np.all(u_min.T == u_min.T[0, :], axis=0)):
raise NotImplementedError("u_bounds min must be the same at each shooting point with ACADOS")
if not np.all(np.all(u_max.T == u_max.T[0, :], axis=0)):
raise NotImplementedError("u_bounds max must be the same at each shooting point with ACADOS")
if (
not np.isfinite(u_min).all()
or not np.isfinite(x_min).all()
or not np.isfinite(u_max).all()
or not np.isfinite(x_max).all()
):
raise NotImplementedError(
"u_bounds and x_bounds cannot be set to infinity in ACADOS. Consider changing it"
"to a big value instead."
)
## TODO: implement constraints in g
# setup state constraints
self.x_bound_max = np.ndarray((self.acados_ocp.dims.nx, 3))
self.x_bound_min = np.ndarray((self.acados_ocp.dims.nx, 3))
param_bounds_max = []
param_bounds_min = []
if self.params:
param_bounds_max = np.concatenate([self.params[key]["bounds"].max for key in self.params.keys()])[:, 0]
param_bounds_min = np.concatenate([self.params[key]["bounds"].min for key in self.params.keys()])[:, 0]
for i in range(3):
self.x_bound_max[:, i] = np.concatenate((param_bounds_max, np.array(ocp.nlp[0].x_bounds.max[:, i])))
self.x_bound_min[:, i] = np.concatenate((param_bounds_min, np.array(ocp.nlp[0].x_bounds.min[:, i])))
# setup control constraints
self.acados_ocp.constraints.lbu = np.array(ocp.nlp[0].u_bounds.min[:, 0])
self.acados_ocp.constraints.ubu = np.array(ocp.nlp[0].u_bounds.max[:, 0])
self.acados_ocp.constraints.idxbu = np.array(range(self.acados_ocp.dims.nu))
self.acados_ocp.dims.nbu = self.acados_ocp.dims.nu
# initial state constraints
self.acados_ocp.constraints.lbx_0 = self.x_bound_min[:, 0]
self.acados_ocp.constraints.ubx_0 = self.x_bound_max[:, 0]
self.acados_ocp.constraints.idxbx_0 = np.array(range(self.acados_ocp.dims.nx))
self.acados_ocp.dims.nbx_0 = self.acados_ocp.dims.nx
# setup path state constraints
self.acados_ocp.constraints.Jbx = np.eye(self.acados_ocp.dims.nx)
self.acados_ocp.constraints.lbx = self.x_bound_min[:, 1]
self.acados_ocp.constraints.ubx = self.x_bound_max[:, 1]
self.acados_ocp.constraints.idxbx = np.array(range(self.acados_ocp.dims.nx))
self.acados_ocp.dims.nbx = self.acados_ocp.dims.nx
# setup terminal state constraints
self.acados_ocp.constraints.Jbx_e = np.eye(self.acados_ocp.dims.nx)
self.acados_ocp.constraints.lbx_e = self.x_bound_min[:, -1]
self.acados_ocp.constraints.ubx_e = self.x_bound_max[:, -1]
self.acados_ocp.constraints.idxbx_e = np.array(range(self.acados_ocp.dims.nx))
self.acados_ocp.dims.nbx_e = self.acados_ocp.dims.nx
def __set_cost_type(self, cost_type="NONLINEAR_LS"):
self.acados_ocp.cost.cost_type = cost_type
self.acados_ocp.cost.cost_type_e = cost_type
def __set_costs(self, ocp):
if ocp.nb_phases != 1:
raise NotImplementedError("ACADOS with more than one phase is not implemented yet.")
# costs handling in self.acados_ocp
self.y_ref = []
self.y_ref_end = []
self.lagrange_costs = SX()
self.mayer_costs = SX()
self.W = np.zeros((0, 0))
self.W_e = np.zeros((0, 0))
if self.acados_ocp.cost.cost_type == "LINEAR_LS":
raise RuntimeError("LINEAR_LS is not interfaced yet.")
elif self.acados_ocp.cost.cost_type == "NONLINEAR_LS":
for i in range(ocp.nb_phases):
for j, J in enumerate(ocp.nlp[i].J):
if J[0]["objective"].type.get_type() == ObjectiveFunction.LagrangeFunction:
self.lagrange_costs = vertcat(self.lagrange_costs, J[0]["val"].reshape((-1, 1)))
self.W = linalg.block_diag(self.W, np.diag([J[0]["objective"].weight] * J[0]["val"].numel()))
if J[0]["target"] is not None:
self.y_ref.append([J_tp["target"].T.reshape((-1, 1)) for J_tp in J])
else:
self.y_ref.append([np.zeros((J_tp["val"].numel(), 1)) for J_tp in J])
elif J[0]["objective"].type.get_type() == ObjectiveFunction.MayerFunction:
mayer_func_tp = Function(f"cas_mayer_func_{i}_{j}", [ocp.nlp[i].X[-1]], [J[0]["val"]])
self.W_e = linalg.block_diag(
self.W_e, np.diag([J[0]["objective"].weight] * J[0]["val"].numel())
)
self.mayer_costs = vertcat(self.mayer_costs, mayer_func_tp(ocp.nlp[i].X[0]))
if J[0]["target"] is not None:
self.y_ref_end.append(
[J[0]["target"]] if isinstance(J[0]["target"], (int, float)) else J[0]["target"]
)
else:
self.y_ref_end.append([0] * (J[0]["val"].numel()))
else:
raise RuntimeError("The objective function is not Lagrange nor Mayer.")
# parameter as mayer function
# IMPORTANT: it is considered that only parameters are stored in ocp.J, for now.
if self.params:
for j, J in enumerate(ocp.J):
mayer_func_tp = Function(f"cas_J_mayer_func_{i}_{j}", [ocp.nlp[i].X[-1]], [J[0]["val"]])
self.W_e = linalg.block_diag(
self.W_e, np.diag(([J[0]["objective"].weight] * J[0]["val"].numel()))
)
self.mayer_costs = vertcat(self.mayer_costs, mayer_func_tp(ocp.nlp[i].X[0]))
if J[0]["target"] is not None:
self.y_ref_end.append(
[J[0]["target"]] if isinstance(J[0]["target"], (int, float)) else J[0]["target"]
)
else:
self.y_ref_end.append([0] * (J[0]["val"].numel()))
# Set costs
self.acados_ocp.model.cost_y_expr = self.lagrange_costs if self.lagrange_costs.numel() else SX(1, 1)
self.acados_ocp.model.cost_y_expr_e = self.mayer_costs if self.mayer_costs.numel() else SX(1, 1)
# Set dimensions
self.acados_ocp.dims.ny = self.acados_ocp.model.cost_y_expr.shape[0]
self.acados_ocp.dims.ny_e = self.acados_ocp.model.cost_y_expr_e.shape[0]
# Set weight
self.acados_ocp.cost.W = np.zeros((1, 1)) if self.W.shape == (0, 0) else self.W
self.acados_ocp.cost.W_e = np.zeros((1, 1)) if self.W_e.shape == (0, 0) else self.W_e
# Set target shape
self.acados_ocp.cost.yref = np.zeros((self.acados_ocp.cost.W.shape[0],))
self.acados_ocp.cost.yref_e = np.zeros((self.acados_ocp.cost.W_e.shape[0],))
elif self.acados_ocp.cost.cost_type == "EXTERNAL":
raise RuntimeError("External is not interfaced yet, please use NONLINEAR_LS")
else:
raise RuntimeError("Available acados cost type: 'LINEAR_LS', 'NONLINEAR_LS' and 'EXTERNAL'.")
def __update_solver(self):
param_init = []
for n in range(self.acados_ocp.dims.N):
self.ocp_solver.cost_set(n, "yref", np.concatenate([data[n] for data in self.y_ref])[:, 0])
self.ocp_solver.cost_set(n, "W", self.W)
if self.params:
param_init = np.concatenate(
[self.params[key]["initial_guess"].init.evaluate_at(n) for key in self.params.keys()]
)
self.ocp_solver.set(n, "x", np.concatenate((param_init, self.ocp.nlp[0].x_init.init.evaluate_at(n))))
self.ocp_solver.set(n, "u", self.ocp.nlp[0].u_init.init.evaluate_at(n))
self.ocp_solver.constraints_set(n, "lbu", self.ocp.nlp[0].u_bounds.min[:, 0])
self.ocp_solver.constraints_set(n, "ubu", self.ocp.nlp[0].u_bounds.max[:, 0])
if n == 0:
self.ocp_solver.constraints_set(n, "lbx", self.x_bound_min[:, 0])
self.ocp_solver.constraints_set(n, "ubx", self.x_bound_max[:, 0])
else:
self.ocp_solver.constraints_set(n, "lbx", self.x_bound_min[:, 1])
self.ocp_solver.constraints_set(n, "ubx", self.x_bound_max[:, 1])
if self.y_ref_end:
self.ocp_solver.cost_set(self.acados_ocp.dims.N, "yref", np.concatenate([data for data in self.y_ref_end]))
self.ocp_solver.cost_set(self.acados_ocp.dims.N, "W", self.W_e)
self.ocp_solver.constraints_set(self.acados_ocp.dims.N, "lbx", self.x_bound_min[:, -1])
self.ocp_solver.constraints_set(self.acados_ocp.dims.N, "ubx", self.x_bound_max[:, -1])
if self.ocp.nlp[0].x_init.init.shape[1] == self.acados_ocp.dims.N + 1:
if self.params:
self.ocp_solver.set(
self.acados_ocp.dims.N,
"x",
np.concatenate(
(
np.concatenate([self.params[key]["initial_guess"].init for key in self.params.keys()])[
:, 0
],
self.ocp.nlp[0].x_init.init[:, self.acados_ocp.dims.N],
)
),
)
else:
self.ocp_solver.set(self.acados_ocp.dims.N, "x", self.ocp.nlp[0].x_init.init[:, self.acados_ocp.dims.N])
def configure(self, options):
if "acados_dir" in options:
del options["acados_dir"]
if "cost_type" in options:
del options["cost_type"]
if self.ocp_solver is None:
self.acados_ocp.solver_options.qp_solver = "PARTIAL_CONDENSING_HPIPM" # FULL_CONDENSING_QPOASES
self.acados_ocp.solver_options.hessian_approx = "GAUSS_NEWTON"
self.acados_ocp.solver_options.integrator_type = "IRK"
self.acados_ocp.solver_options.nlp_solver_type = "SQP"
self.acados_ocp.solver_options.nlp_solver_tol_comp = 1e-06
self.acados_ocp.solver_options.nlp_solver_tol_eq = 1e-06
self.acados_ocp.solver_options.nlp_solver_tol_ineq = 1e-06
self.acados_ocp.solver_options.nlp_solver_tol_stat = 1e-06
self.acados_ocp.solver_options.nlp_solver_max_iter = 200
self.acados_ocp.solver_options.sim_method_newton_iter = 5
self.acados_ocp.solver_options.sim_method_num_stages = 4
self.acados_ocp.solver_options.sim_method_num_steps = 1
self.acados_ocp.solver_options.print_level = 1
for key in options:
setattr(self.acados_ocp.solver_options, key, options[key])
else:
for key in options:
if key[:11] == "nlp_solver_":
short_key = key[11:]
self.ocp_solver.options_set(short_key, options[key])
else:
raise RuntimeError(
"[ACADOS] Only editable solver options after solver creation are :\n"
"nlp_solver_tol_comp\n"
"nlp_solver_tol_eq\n"
"nlp_solver_tol_ineq\n"
"nlp_solver_tol_stat\n"
)
def get_iterations(self):
raise NotImplementedError("return_iterations is not implemented yet with ACADOS backend")
def online_optim(self, ocp):
raise NotImplementedError("online_optim is not implemented yet with ACADOS backend")
def get_optimized_value(self):
ns = self.acados_ocp.dims.N
nx = self.acados_ocp.dims.nx
nq = self.ocp.nlp[0].q.shape[0]
nparams = self.ocp.nlp[0].np
acados_x = np.array([self.ocp_solver.get(i, "x") for i in range(ns + 1)]).T
acados_p = acados_x[:nparams, :]
acados_q = acados_x[nparams : nq + nparams, :]
acados_qdot = acados_x[nq + nparams : nx, :]
acados_u = np.array([self.ocp_solver.get(i, "u") for i in range(ns)]).T
out = {
"qqdot": acados_x,
"x": [],
"u": acados_u,
"time_tot": self.ocp_solver.get_stats("time_tot")[0],
"status": self.status,
}
for i in range(ns):
out["x"] = vertcat(out["x"], acados_q[:, i])
out["x"] = vertcat(out["x"], acados_qdot[:, i])
out["x"] = vertcat(out["x"], acados_u[:, i])
out["x"] = vertcat(out["x"], acados_q[:, ns])
out["x"] = vertcat(out["x"], acados_qdot[:, ns])
out["x"] = vertcat(acados_p[:, 0], out["x"])
self.out["sol"] = out
out = []
for key in self.out.keys():
out.append(self.out[key])
return out[0] if len(out) == 1 else out
def solve(self):
# Populate costs and constrs vectors
self.__set_costs(self.ocp)
self.__set_constrs(self.ocp)
if self.ocp_solver is None:
self.ocp_solver = AcadosOcpSolver(self.acados_ocp, json_file="acados_ocp.json")
self.__update_solver()
self.status = self.ocp_solver.solve()
self.get_optimized_value()
return self
class AcadosInterface(SolverInterface):
"""
The ACADOS solver interface
Attributes
----------
acados_ocp: AcadosOcp
The current AcadosOcp reference
acados_model: AcadosModel
The current AcadosModel reference
lagrange_costs: SX
The lagrange cost function
mayer_costs: SX
The mayer cost function
y_ref = list[np.ndarray]
The lagrange targets
y_ref_end = list[np.ndarray]
The mayer targets
params = dict
All the parameters to optimize
W: np.ndarray
The Lagrange weights
W_e: np.ndarray
The Mayer weights
status: int
The status of the optimization
all_constr: SX
All the Lagrange constraints
end_constr: SX
All the Mayer constraints
all_g_bounds = Bounds
All the Lagrange bounds on the variables
end_g_bounds = Bounds
All the Mayer bounds on the variables
x_bound_max = np.ndarray
All the bounds max
x_bound_min = np.ndarray
All the bounds min
Vu: np.ndarray
The control objective functions
Vx: np.ndarray
The Lagrange state objective functions
Vxe: np.ndarray
The Mayer state objective functions
Methods
-------
__acados_export_model(self, ocp: OptimalControlProgram)
Creating a generic ACADOS model
__prepare_acados(self, ocp: OptimalControlProgram)
Set some important ACADOS variables
__set_constr_type(self, constr_type: str = "BGH")
Set the type of constraints
__set_constraints(self, ocp: OptimalControlProgram)
Set the constraints from the ocp
__set_cost_type(self, cost_type: str = "NONLINEAR_LS")
Set the type of cost functions
__set_costs(self, ocp: OptimalControlProgram)
Set the cost functions from ocp
__update_solver(self)
Update the ACADOS solver to new values
configure(self, options: dict)
Set some ACADOS options
get_optimized_value(self) -> Union[list[dict], dict]
Get the previously optimized solution
solve(self) -> "AcadosInterface"
Solve the prepared ocp
"""
def __init__(self, ocp, **solver_options):
"""
Parameters
----------
ocp: OptimalControlProgram
A reference to the current OptimalControlProgram
solver_options: dict
The options to pass to the solver
"""
if not isinstance(ocp.cx(), SX):
raise RuntimeError(
"CasADi graph must be SX to be solved with ACADOS. Please set use_sx to True in OCP"
)
super().__init__(ocp)
# If Acados is installed using the acados_install.sh file, you probably can leave this to unset
acados_path = ""
if "acados_dir" in solver_options:
acados_path = solver_options["acados_dir"]
self.acados_ocp = AcadosOcp(acados_path=acados_path)
self.acados_model = AcadosModel()
if "cost_type" in solver_options:
self.__set_cost_type(solver_options["cost_type"])
else:
self.__set_cost_type()
if "constr_type" in solver_options:
self.__set_constr_type(solver_options["constr_type"])
else:
self.__set_constr_type()
self.lagrange_costs = SX()
self.mayer_costs = SX()
self.y_ref = []
self.y_ref_end = []
self.nparams = 0
self.params_initial_guess = None
self.params_bounds = None
self.__acados_export_model(ocp)
self.__prepare_acados(ocp)
self.ocp_solver = None
self.W = np.zeros((0, 0))
self.W_e = np.zeros((0, 0))
self.status = None
self.out = {}
self.all_constr = None
self.end_constr = SX()
self.all_g_bounds = Bounds(interpolation=InterpolationType.CONSTANT)
self.end_g_bounds = Bounds(interpolation=InterpolationType.CONSTANT)
self.x_bound_max = np.ndarray((self.acados_ocp.dims.nx, 3))
self.x_bound_min = np.ndarray((self.acados_ocp.dims.nx, 3))
self.Vu = np.array([],
dtype=np.int64).reshape(0,
ocp.nlp[0].controls.shape)
self.Vx = np.array([], dtype=np.int64).reshape(0,
ocp.nlp[0].states.shape)
self.Vxe = np.array([],
dtype=np.int64).reshape(0, ocp.nlp[0].states.shape)
def __acados_export_model(self, ocp):
"""
Creating a generic ACADOS model
Parameters
----------
ocp: OptimalControlProgram
A reference to the current OptimalControlProgram
"""
if ocp.n_phases > 1:
raise NotImplementedError(
"More than 1 phase is not implemented yet with ACADOS backend")
# Declare model variables
x = ocp.nlp[0].states.cx
u = ocp.nlp[0].controls.cx
p = ocp.nlp[0].parameters.cx
if ocp.v.parameters_in_list:
for param in ocp.v.parameters_in_list:
if str(param.cx)[:11] == f"time_phase_":
raise RuntimeError(
"Time constraint not implemented yet with Acados.")
self.nparams = ocp.nlp[0].parameters.shape
self.params_initial_guess = ocp.v.parameters_in_list.initial_guess
self.params_initial_guess.check_and_adjust_dimensions(self.nparams, 1)
self.params_bounds = ocp.v.parameters_in_list.bounds
self.params_bounds.check_and_adjust_dimensions(self.nparams, 1)
x = vertcat(p, x)
x_dot = SX.sym("x_dot", x.shape[0], x.shape[1])
f_expl = vertcat([0] * self.nparams,
ocp.nlp[0].dynamics_func(x[self.nparams:, :], u, p))
f_impl = x_dot - f_expl
self.acados_model.f_impl_expr = f_impl
self.acados_model.f_expl_expr = f_expl
self.acados_model.x = x
self.acados_model.xdot = x_dot
self.acados_model.u = u
self.acados_model.con_h_expr = np.zeros((0, 0))
self.acados_model.con_h_expr_e = np.zeros((0, 0))
self.acados_model.p = []
now = datetime.now() # current date and time
self.acados_model.name = f"model_{now.strftime('%Y_%m_%d_%H%M%S%f')[:-4]}"
def __prepare_acados(self, ocp):
"""
Set some important ACADOS variables
Parameters
----------
ocp: OptimalControlProgram
A reference to the current OptimalControlProgram
"""
# set model
self.acados_ocp.model = self.acados_model
# set time
self.acados_ocp.solver_options.tf = ocp.nlp[0].tf
# set dimensions
self.acados_ocp.dims.nx = ocp.nlp[0].states.shape + ocp.nlp[
0].parameters.shape
self.acados_ocp.dims.nu = ocp.nlp[0].controls.shape
self.acados_ocp.dims.N = ocp.nlp[0].ns
def __set_constr_type(self, constr_type: str = "BGH"):
"""
Set the type of constraints
Parameters
----------
constr_type: str
The requested type of constraints
"""
self.acados_ocp.constraints.constr_type = constr_type
self.acados_ocp.constraints.constr_type_e = constr_type
def __set_constraints(self, ocp):
"""
Set the constraints from the ocp
Parameters
----------
ocp: OptimalControlProgram
A reference to the current OptimalControlProgram
"""
# constraints handling in self.acados_ocp
if ocp.nlp[
0].x_bounds.type != InterpolationType.CONSTANT_WITH_FIRST_AND_LAST_DIFFERENT:
raise NotImplementedError(
"ACADOS must declare an InterpolationType.CONSTANT_WITH_FIRST_AND_LAST_DIFFERENT "
"for the x_bounds")
if ocp.nlp[
0].u_bounds.type != InterpolationType.CONSTANT_WITH_FIRST_AND_LAST_DIFFERENT:
raise NotImplementedError(
"ACADOS must declare an InterpolationType.CONSTANT_WITH_FIRST_AND_LAST_DIFFERENT "
"for the u_bounds")
u_min = np.array(ocp.nlp[0].u_bounds.min)
u_max = np.array(ocp.nlp[0].u_bounds.max)
x_min = np.array(ocp.nlp[0].x_bounds.min)
x_max = np.array(ocp.nlp[0].x_bounds.max)
self.all_constr = SX()
self.end_constr = SX()
# TODO:change for more node flexibility on bounds
self.all_g_bounds = Bounds(interpolation=InterpolationType.CONSTANT)
self.end_g_bounds = Bounds(interpolation=InterpolationType.CONSTANT)
for i, nlp in enumerate(ocp.nlp):
x = nlp.states.cx
u = nlp.controls.cx
p = nlp.parameters.cx
for g, G in enumerate(nlp.g):
if not G:
continue
if G.node[0] == Node.ALL or G.node[0] == Node.ALL_SHOOTING:
self.all_constr = vertcat(self.all_constr,
G.function(x, u, p))
self.all_g_bounds.concatenate(G.bounds)
if G.node[0] == Node.ALL:
self.end_constr = vertcat(self.end_constr,
G.function(x, u, p))
self.end_g_bounds.concatenate(G.bounds)
elif G.node[0] == Node.END:
self.end_constr = vertcat(self.end_constr,
G.function(x, u, p))
self.end_g_bounds.concatenate(G.bounds)
else:
raise RuntimeError(
"Except for states and controls, Acados solver only handles constraints on last or all nodes."
)
self.acados_model.con_h_expr = self.all_constr
self.acados_model.con_h_expr_e = self.end_constr
if not np.all(np.all(u_min.T == u_min.T[0, :], axis=0)):
raise NotImplementedError(
"u_bounds min must be the same at each shooting point with ACADOS"
)
if not np.all(np.all(u_max.T == u_max.T[0, :], axis=0)):
raise NotImplementedError(
"u_bounds max must be the same at each shooting point with ACADOS"
)
if (not np.isfinite(u_min).all() or not np.isfinite(x_min).all()
or not np.isfinite(u_max).all()
or not np.isfinite(x_max).all()):
raise NotImplementedError(
"u_bounds and x_bounds cannot be set to infinity in ACADOS. Consider changing it "
"to a big value instead.")
# setup state constraints
# TODO replace all these np.concatenate by proper bound and initial_guess classes
self.x_bound_max = np.ndarray((self.acados_ocp.dims.nx, 3))
self.x_bound_min = np.ndarray((self.acados_ocp.dims.nx, 3))
param_bounds_max = []
param_bounds_min = []
if self.nparams:
param_bounds_max = self.params_bounds.max[:, 0]
param_bounds_min = self.params_bounds.min[:, 0]
for i in range(3):
self.x_bound_max[:, i] = np.concatenate(
(param_bounds_max, np.array(ocp.nlp[0].x_bounds.max[:, i])))
self.x_bound_min[:, i] = np.concatenate(
(param_bounds_min, np.array(ocp.nlp[0].x_bounds.min[:, i])))
# setup control constraints
self.acados_ocp.constraints.lbu = np.array(ocp.nlp[0].u_bounds.min[:,
0])
self.acados_ocp.constraints.ubu = np.array(ocp.nlp[0].u_bounds.max[:,
0])
self.acados_ocp.constraints.idxbu = np.array(
range(self.acados_ocp.dims.nu))
self.acados_ocp.dims.nbu = self.acados_ocp.dims.nu
# initial state constraints
self.acados_ocp.constraints.lbx_0 = self.x_bound_min[:, 0]
self.acados_ocp.constraints.ubx_0 = self.x_bound_max[:, 0]
self.acados_ocp.constraints.idxbx_0 = np.array(
range(self.acados_ocp.dims.nx))
self.acados_ocp.dims.nbx_0 = self.acados_ocp.dims.nx
# setup path state constraints
self.acados_ocp.constraints.Jbx = np.eye(self.acados_ocp.dims.nx)
self.acados_ocp.constraints.lbx = self.x_bound_min[:, 1]
self.acados_ocp.constraints.ubx = self.x_bound_max[:, 1]
self.acados_ocp.constraints.idxbx = np.array(
range(self.acados_ocp.dims.nx))
self.acados_ocp.dims.nbx = self.acados_ocp.dims.nx
# setup terminal state constraints
self.acados_ocp.constraints.Jbx_e = np.eye(self.acados_ocp.dims.nx)
self.acados_ocp.constraints.lbx_e = self.x_bound_min[:, -1]
self.acados_ocp.constraints.ubx_e = self.x_bound_max[:, -1]
self.acados_ocp.constraints.idxbx_e = np.array(
range(self.acados_ocp.dims.nx))
self.acados_ocp.dims.nbx_e = self.acados_ocp.dims.nx
# setup algebraic constraint
self.acados_ocp.constraints.lh = np.array(self.all_g_bounds.min[:, 0])
self.acados_ocp.constraints.uh = np.array(self.all_g_bounds.max[:, 0])
# setup terminal algebraic constraint
self.acados_ocp.constraints.lh_e = np.array(self.end_g_bounds.min[:,
0])
self.acados_ocp.constraints.uh_e = np.array(self.end_g_bounds.max[:,
0])
def __set_cost_type(self, cost_type: str = "NONLINEAR_LS"):
"""
Set the type of cost functions
Parameters
----------
cost_type: str
The type of cost function
"""
self.acados_ocp.cost.cost_type = cost_type
self.acados_ocp.cost.cost_type_e = cost_type
def __set_costs(self, ocp):
"""
Set the cost functions from ocp
Parameters
----------
ocp: OptimalControlProgram
A reference to the current OptimalControlProgram
"""
def add_linear_ls_lagrange(acados, objectives):
def add_objective(n_variables, is_state):
v_var = np.zeros(n_variables)
var_type = acados.ocp.nlp[
0].states if is_state else acados.ocp.nlp[0].controls
rows = objectives.rows + var_type[
objectives.params["key"]].index[0]
v_var[rows] = 1.0
if is_state:
acados.Vx = np.vstack((acados.Vx, np.diag(v_var)))
acados.Vu = np.vstack(
(acados.Vu, np.zeros((n_states, n_controls))))
else:
acados.Vx = np.vstack(
(acados.Vx, np.zeros((n_controls, n_states))))
acados.Vu = np.vstack((acados.Vu, np.diag(v_var)))
acados.W = linalg.block_diag(
acados.W, np.diag([objectives.weight] * n_variables))
node_idx = objectives.node_idx[:-1] if objectives.node[
0] == Node.ALL else objectives.node_idx
y_ref = [
np.zeros((n_states if is_state else n_controls, 1))
for _ in node_idx
]
if objectives.target is not None:
for idx in node_idx:
y_ref[idx][rows] = objectives.target[...,
idx].T.reshape(
(-1, 1))
acados.y_ref.append(y_ref)
if objectives.type in allowed_control_objectives:
add_objective(n_controls, False)
elif objectives.type in allowed_state_objectives:
add_objective(n_states, True)
else:
raise RuntimeError(
f"{objectives[0]['objective'].type.name} is an incompatible objective term with LINEAR_LS cost type"
)
def add_linear_ls_mayer(acados, objectives):
if objectives.type in allowed_state_objectives:
vxe = np.zeros(n_states)
rows = objectives.rows + acados.ocp.nlp[0].states[
objectives.params["key"]].index[0]
vxe[rows] = 1.0
acados.Vxe = np.vstack((acados.Vxe, np.diag(vxe)))
acados.W_e = linalg.block_diag(
acados.W_e, np.diag([objectives.weight] * n_states))
y_ref_end = np.zeros((n_states, 1))
if objectives.target is not None:
y_ref_end[rows] = objectives.target[..., -1].T.reshape(
(-1, 1))
acados.y_ref_end.append(y_ref_end)
else:
raise RuntimeError(
f"{objectives.type.name} is an incompatible objective term with LINEAR_LS cost type"
)
def add_nonlinear_ls_lagrange(acados, objectives, x, u, p):
acados.lagrange_costs = vertcat(
acados.lagrange_costs,
objectives.function(x, u, p).reshape((-1, 1)))
acados.W = linalg.block_diag(
acados.W,
np.diag([objectives.weight] * objectives.function.numel_out()))
node_idx = objectives.node_idx[:-1] if objectives.node[
0] == Node.ALL else objectives.node_idx
if objectives.target is not None:
acados.y_ref.append([
objectives.target[..., idx].T.reshape((-1, 1))
for idx in node_idx
])
else:
acados.y_ref.append([
np.zeros((objectives.function.numel_out(), 1))
for _ in node_idx
])
def add_nonlinear_ls_mayer(acados, objectives, x, u, p):
acados.W_e = linalg.block_diag(
acados.W_e,
np.diag([objectives.weight] * objectives.function.numel_out()))
x = x if objectives.function.sparsity_in("i0").shape != (
0, 0) else []
u = u if objectives.function.sparsity_in("i1").shape != (
0, 0) else []
acados.mayer_costs = vertcat(
acados.mayer_costs,
objectives.function(x, u, p).reshape((-1, 1)))
if objectives.target is not None:
acados.y_ref_end.append(objectives.target[..., -1].T.reshape(
(-1, 1)))
else:
acados.y_ref_end.append(
np.zeros((objectives.function.numel_out(), 1)))
if ocp.n_phases != 1:
raise NotImplementedError(
"ACADOS with more than one phase is not implemented yet.")
# costs handling in self.acados_ocp
self.y_ref = []
self.y_ref_end = []
self.lagrange_costs = SX()
self.mayer_costs = SX()
self.W = np.zeros((0, 0))
self.W_e = np.zeros((0, 0))
allowed_control_objectives = [ObjectiveFcn.Lagrange.MINIMIZE_CONTROL]
allowed_state_objectives = [
ObjectiveFcn.Lagrange.MINIMIZE_STATE,
ObjectiveFcn.Mayer.TRACK_STATE
]
if self.acados_ocp.cost.cost_type == "LINEAR_LS":
n_states = ocp.nlp[0].states.shape
n_controls = ocp.nlp[0].controls.shape
self.Vu = np.array([], dtype=np.int64).reshape(0, n_controls)
self.Vx = np.array([], dtype=np.int64).reshape(0, n_states)
self.Vxe = np.array([], dtype=np.int64).reshape(0, n_states)
for i in range(ocp.n_phases):
for J in ocp.nlp[i].J:
if not J:
continue
if J.multi_thread:
raise RuntimeError(
f"The objective function {J.name} was declared with multi_thread=True, "
f"but this is not possible to multi_thread objective function with ACADOS"
)
if J.type.get_type() == ObjectiveFunction.LagrangeFunction:
add_linear_ls_lagrange(self, J)
# Deal with last node to match ipopt formulation
if J.node[0] == Node.ALL:
add_linear_ls_mayer(self, J)
elif J.type.get_type() == ObjectiveFunction.MayerFunction:
add_linear_ls_mayer(self, J)
else:
raise RuntimeError(
"The objective function is not Lagrange nor Mayer."
)
if self.nparams:
raise RuntimeError(
"Params not yet handled with LINEAR_LS cost type")
# Set costs
self.acados_ocp.cost.Vx = self.Vx if self.Vx.shape[0] else np.zeros(
(0, 0))
self.acados_ocp.cost.Vu = self.Vu if self.Vu.shape[0] else np.zeros(
(0, 0))
self.acados_ocp.cost.Vx_e = self.Vxe if self.Vxe.shape[
0] else np.zeros((0, 0))
# Set dimensions
self.acados_ocp.dims.ny = sum(
[len(data[0]) for data in self.y_ref])
self.acados_ocp.dims.ny_e = sum(
[len(data) for data in self.y_ref_end])
# Set weight
self.acados_ocp.cost.W = self.W
self.acados_ocp.cost.W_e = self.W_e
# Set target shape
self.acados_ocp.cost.yref = np.zeros(
(self.acados_ocp.cost.W.shape[0], ))
self.acados_ocp.cost.yref_e = np.zeros(
(self.acados_ocp.cost.W_e.shape[0], ))
elif self.acados_ocp.cost.cost_type == "NONLINEAR_LS":
for i, nlp in enumerate(ocp.nlp):
for j, J in enumerate(nlp.J):
if not J:
continue
if J.multi_thread:
raise RuntimeError(
f"The objective function {J.name} was declared with multi_thread=True, "
f"but this is not possible to multi_thread objective function with ACADOS"
)
if J.type.get_type() == ObjectiveFunction.LagrangeFunction:
add_nonlinear_ls_lagrange(self, J, nlp.states.cx,
nlp.controls.cx,
nlp.parameters.cx)
# Deal with last node to match ipopt formulation
if J.node[0] == Node.ALL:
add_nonlinear_ls_mayer(self, J, nlp.states.cx,
nlp.controls.cx,
nlp.parameters.cx)
elif J.type.get_type() == ObjectiveFunction.MayerFunction:
add_nonlinear_ls_mayer(self, J, nlp.states.cx,
nlp.controls.cx,
nlp.parameters.cx)
else:
raise RuntimeError(
"The objective function is not Lagrange nor Mayer."
)
# parameter as mayer function
# IMPORTANT: it is considered that only parameters are stored in ocp.objectives, for now.
if self.nparams:
nlp = ocp.nlp[0] # Assume 1 phase
for j, J in enumerate(ocp.J):
add_nonlinear_ls_mayer(self, J, nlp.states.cx,
nlp.controls.cx, nlp.parameters.cx)
# Set costs
self.acados_ocp.model.cost_y_expr = (self.lagrange_costs.reshape(
(-1, 1)) if self.lagrange_costs.numel() else SX(1, 1))
self.acados_ocp.model.cost_y_expr_e = (self.mayer_costs.reshape(
(-1, 1)) if self.mayer_costs.numel() else SX(1, 1))
# Set dimensions
self.acados_ocp.dims.ny = self.acados_ocp.model.cost_y_expr.shape[
0]
self.acados_ocp.dims.ny_e = self.acados_ocp.model.cost_y_expr_e.shape[
0]
# Set weight
self.acados_ocp.cost.W = np.zeros(
(1, 1)) if self.W.shape == (0, 0) else self.W
self.acados_ocp.cost.W_e = np.zeros(
(1, 1)) if self.W_e.shape == (0, 0) else self.W_e
# Set target shape
self.acados_ocp.cost.yref = np.zeros(
(self.acados_ocp.cost.W.shape[0], ))
self.acados_ocp.cost.yref_e = np.zeros(
(self.acados_ocp.cost.W_e.shape[0], ))
elif self.acados_ocp.cost.cost_type == "EXTERNAL":
raise RuntimeError(
"EXTERNAL is not interfaced yet, please use NONLINEAR_LS")
else:
raise RuntimeError(
"Available acados cost type: 'LINEAR_LS', 'NONLINEAR_LS' and 'EXTERNAL'."
)
def __update_solver(self):
"""
Update the ACADOS solver to new values
"""
param_init = []
for n in range(self.acados_ocp.dims.N):
if self.y_ref: # Target
self.ocp_solver.cost_set(
n, "yref",
np.vstack([data[n] for data in self.y_ref])[:, 0])
# check following line
# self.ocp_solver.cost_set(n, "W", self.W)
if self.nparams:
param_init = self.params_initial_guess.init.evaluate_at(n)
self.ocp_solver.set(
n, "x",
np.concatenate(
(param_init, self.ocp.nlp[0].x_init.init.evaluate_at(n))))
self.ocp_solver.set(n, "u",
self.ocp.nlp[0].u_init.init.evaluate_at(n))
self.ocp_solver.constraints_set(n, "lbu",
self.ocp.nlp[0].u_bounds.min[:, 0])
self.ocp_solver.constraints_set(n, "ubu",
self.ocp.nlp[0].u_bounds.max[:, 0])
self.ocp_solver.constraints_set(n, "uh", self.all_g_bounds.max[:,
0])
self.ocp_solver.constraints_set(n, "lh", self.all_g_bounds.min[:,
0])
if n == 0:
self.ocp_solver.constraints_set(n, "lbx", self.x_bound_min[:,
0])
self.ocp_solver.constraints_set(n, "ubx", self.x_bound_max[:,
0])
else:
self.ocp_solver.constraints_set(n, "lbx", self.x_bound_min[:,
1])
self.ocp_solver.constraints_set(n, "ubx", self.x_bound_max[:,
1])
if self.y_ref_end:
if len(self.y_ref_end) == 1:
self.ocp_solver.cost_set(self.acados_ocp.dims.N, "yref",
np.array(self.y_ref_end[0])[:, 0])
else:
self.ocp_solver.cost_set(self.acados_ocp.dims.N, "yref",
np.concatenate(self.y_ref_end)[:, 0])
# check following line
# self.ocp_solver.cost_set(self.acados_ocp.dims.N, "W", self.W_e)
self.ocp_solver.constraints_set(self.acados_ocp.dims.N, "lbx",
self.x_bound_min[:, -1])
self.ocp_solver.constraints_set(self.acados_ocp.dims.N, "ubx",
self.x_bound_max[:, -1])
if len(self.end_g_bounds.max[:, 0]):
self.ocp_solver.constraints_set(self.acados_ocp.dims.N, "uh",
self.end_g_bounds.max[:, 0])
self.ocp_solver.constraints_set(self.acados_ocp.dims.N, "lh",
self.end_g_bounds.min[:, 0])
if self.ocp.nlp[0].x_init.init.shape[1] == self.acados_ocp.dims.N + 1:
if self.nparams:
self.ocp_solver.set(
self.acados_ocp.dims.N,
"x",
np.concatenate(
(self.params_initial_guess.init[:, 0],
self.ocp.nlp[0].x_init.init[:,
self.acados_ocp.dims.N])),
)
else:
self.ocp_solver.set(
self.acados_ocp.dims.N, "x",
self.ocp.nlp[0].x_init.init[:, self.acados_ocp.dims.N])
def configure(self, options: dict):
"""
Set some ACADOS options
Parameters
----------
options: dict
The dictionary of options
"""
if options is None:
options = {}
if "acados_dir" in options:
del options["acados_dir"]
if "cost_type" in options:
del options["cost_type"]
if self.ocp_solver is None:
self.acados_ocp.solver_options.qp_solver = "PARTIAL_CONDENSING_HPIPM" # FULL_CONDENSING_QPOASES
self.acados_ocp.solver_options.hessian_approx = "GAUSS_NEWTON"
self.acados_ocp.solver_options.integrator_type = "IRK"
self.acados_ocp.solver_options.nlp_solver_type = "SQP"
self.acados_ocp.solver_options.nlp_solver_tol_comp = 1e-06
self.acados_ocp.solver_options.nlp_solver_tol_eq = 1e-06
self.acados_ocp.solver_options.nlp_solver_tol_ineq = 1e-06
self.acados_ocp.solver_options.nlp_solver_tol_stat = 1e-06
self.acados_ocp.solver_options.nlp_solver_max_iter = 200
self.acados_ocp.solver_options.sim_method_newton_iter = 5
self.acados_ocp.solver_options.sim_method_num_stages = 4
self.acados_ocp.solver_options.sim_method_num_steps = 1
self.acados_ocp.solver_options.print_level = 1
for key in options:
setattr(self.acados_ocp.solver_options, key, options[key])
else:
available_options = [
"nlp_solver_tol_comp",
"nlp_solver_tol_eq",
"nlp_solver_tol_ineq",
"nlp_solver_tol_stat",
]
for key in options:
if key in available_options:
short_key = key[11:]
self.ocp_solver.options_set(short_key, options[key])
else:
raise RuntimeError(
f"[ACADOS] Only editable solver options after solver creation are :\n {available_options}"
)
def online_optim(self, ocp):
raise NotImplementedError(
"online_optim is not implemented yet with ACADOS backend")
def get_optimized_value(self) -> Union[list, dict]:
"""
Get the previously optimized solution
Returns
-------
A solution or a list of solution depending on the number of phases
"""
ns = self.acados_ocp.dims.N
n_params = self.ocp.nlp[0].parameters.shape
acados_x = np.array(
[self.ocp_solver.get(i, "x") for i in range(ns + 1)]).T
acados_p = acados_x[:n_params, :]
acados_x = acados_x[n_params:, :]
acados_u = np.array([self.ocp_solver.get(i, "u") for i in range(ns)]).T
out = {
"x": [],
"u": acados_u,
"time_tot": self.ocp_solver.get_stats("time_tot")[0],
"iter": self.ocp_solver.get_stats("sqp_iter")[0],
"status": self.status,
}
out["x"] = vertcat(out["x"], acados_x.reshape(-1, 1, order="F"))
out["x"] = vertcat(out["x"], acados_u.reshape(-1, 1, order="F"))
out["x"] = vertcat(out["x"], acados_p[:, 0])
self.out["sol"] = out
out = []
for key in self.out.keys():
out.append(self.out[key])
return out[0] if len(out) == 1 else out
def solve(self) -> "AcadosInterface":
"""
Solve the prepared ocp
Returns
-------
A reference to the solution
"""
# Populate costs and constraints vectors
self.__set_costs(self.ocp)
self.__set_constraints(self.ocp)
if self.ocp_solver is None:
self.ocp_solver = AcadosOcpSolver(self.acados_ocp,
json_file="acados_ocp.json")
self.__update_solver()
self.status = self.ocp_solver.solve()
self.get_optimized_value()
return self
def __set_constraints(self, ocp):
"""
Set the constraints from the ocp
Parameters
----------
ocp: OptimalControlProgram
A reference to the current OptimalControlProgram
"""
# constraints handling in self.acados_ocp
if ocp.nlp[
0].x_bounds.type != InterpolationType.CONSTANT_WITH_FIRST_AND_LAST_DIFFERENT:
raise NotImplementedError(
"ACADOS must declare an InterpolationType.CONSTANT_WITH_FIRST_AND_LAST_DIFFERENT "
"for the x_bounds")
if ocp.nlp[
0].u_bounds.type != InterpolationType.CONSTANT_WITH_FIRST_AND_LAST_DIFFERENT:
raise NotImplementedError(
"ACADOS must declare an InterpolationType.CONSTANT_WITH_FIRST_AND_LAST_DIFFERENT "
"for the u_bounds")
u_min = np.array(ocp.nlp[0].u_bounds.min)
u_max = np.array(ocp.nlp[0].u_bounds.max)
x_min = np.array(ocp.nlp[0].x_bounds.min)
x_max = np.array(ocp.nlp[0].x_bounds.max)
self.all_constr = SX()
self.end_constr = SX()
# TODO:change for more node flexibility on bounds
self.all_g_bounds = Bounds(interpolation=InterpolationType.CONSTANT)
self.end_g_bounds = Bounds(interpolation=InterpolationType.CONSTANT)
for i, nlp in enumerate(ocp.nlp):
x = nlp.states.cx
u = nlp.controls.cx
p = nlp.parameters.cx
for g, G in enumerate(nlp.g):
if not G:
continue
if G.node[0] == Node.ALL or G.node[0] == Node.ALL_SHOOTING:
self.all_constr = vertcat(self.all_constr,
G.function(x, u, p))
self.all_g_bounds.concatenate(G.bounds)
if G.node[0] == Node.ALL:
self.end_constr = vertcat(self.end_constr,
G.function(x, u, p))
self.end_g_bounds.concatenate(G.bounds)
elif G.node[0] == Node.END:
self.end_constr = vertcat(self.end_constr,
G.function(x, u, p))
self.end_g_bounds.concatenate(G.bounds)
else:
raise RuntimeError(
"Except for states and controls, Acados solver only handles constraints on last or all nodes."
)
self.acados_model.con_h_expr = self.all_constr
self.acados_model.con_h_expr_e = self.end_constr
if not np.all(np.all(u_min.T == u_min.T[0, :], axis=0)):
raise NotImplementedError(
"u_bounds min must be the same at each shooting point with ACADOS"
)
if not np.all(np.all(u_max.T == u_max.T[0, :], axis=0)):
raise NotImplementedError(
"u_bounds max must be the same at each shooting point with ACADOS"
)
if (not np.isfinite(u_min).all() or not np.isfinite(x_min).all()
or not np.isfinite(u_max).all()
or not np.isfinite(x_max).all()):
raise NotImplementedError(
"u_bounds and x_bounds cannot be set to infinity in ACADOS. Consider changing it "
"to a big value instead.")
# setup state constraints
# TODO replace all these np.concatenate by proper bound and initial_guess classes
self.x_bound_max = np.ndarray((self.acados_ocp.dims.nx, 3))
self.x_bound_min = np.ndarray((self.acados_ocp.dims.nx, 3))
param_bounds_max = []
param_bounds_min = []
if self.nparams:
param_bounds_max = self.params_bounds.max[:, 0]
param_bounds_min = self.params_bounds.min[:, 0]
for i in range(3):
self.x_bound_max[:, i] = np.concatenate(
(param_bounds_max, np.array(ocp.nlp[0].x_bounds.max[:, i])))
self.x_bound_min[:, i] = np.concatenate(
(param_bounds_min, np.array(ocp.nlp[0].x_bounds.min[:, i])))
# setup control constraints
self.acados_ocp.constraints.lbu = np.array(ocp.nlp[0].u_bounds.min[:,
0])
self.acados_ocp.constraints.ubu = np.array(ocp.nlp[0].u_bounds.max[:,
0])
self.acados_ocp.constraints.idxbu = np.array(
range(self.acados_ocp.dims.nu))
self.acados_ocp.dims.nbu = self.acados_ocp.dims.nu
# initial state constraints
self.acados_ocp.constraints.lbx_0 = self.x_bound_min[:, 0]
self.acados_ocp.constraints.ubx_0 = self.x_bound_max[:, 0]
self.acados_ocp.constraints.idxbx_0 = np.array(
range(self.acados_ocp.dims.nx))
self.acados_ocp.dims.nbx_0 = self.acados_ocp.dims.nx
# setup path state constraints
self.acados_ocp.constraints.Jbx = np.eye(self.acados_ocp.dims.nx)
self.acados_ocp.constraints.lbx = self.x_bound_min[:, 1]
self.acados_ocp.constraints.ubx = self.x_bound_max[:, 1]
self.acados_ocp.constraints.idxbx = np.array(
range(self.acados_ocp.dims.nx))
self.acados_ocp.dims.nbx = self.acados_ocp.dims.nx
# setup terminal state constraints
self.acados_ocp.constraints.Jbx_e = np.eye(self.acados_ocp.dims.nx)
self.acados_ocp.constraints.lbx_e = self.x_bound_min[:, -1]
self.acados_ocp.constraints.ubx_e = self.x_bound_max[:, -1]
self.acados_ocp.constraints.idxbx_e = np.array(
range(self.acados_ocp.dims.nx))
self.acados_ocp.dims.nbx_e = self.acados_ocp.dims.nx
# setup algebraic constraint
self.acados_ocp.constraints.lh = np.array(self.all_g_bounds.min[:, 0])
self.acados_ocp.constraints.uh = np.array(self.all_g_bounds.max[:, 0])
# setup terminal algebraic constraint
self.acados_ocp.constraints.lh_e = np.array(self.end_g_bounds.min[:,
0])
self.acados_ocp.constraints.uh_e = np.array(self.end_g_bounds.max[:,
0])
def acados_nonlinear_quad_model(num_states, g=9.81):
model_name = 'nonlinear_quad'
position = SX.sym('o', 3)
velocity = SX.sym('v', 3)
phi = SX.sym('phi')
theta = SX.sym('theta')
psi = SX.sym('psi')
gravity = SX.zeros(3)
gravity[2] = g
x = vertcat(position, phi, theta, psi, velocity)
p = SX.sym('p')
q = SX.sym('q')
r = SX.sym('r')
F = SX.sym('F')
u = vertcat(p, q, r, F)
r1 = SX(3, 3)
r1[0, 0] = 1
r1[1, 1] = cos(phi)
r1[1, 2] = -sin(phi)
r1[2, 1] = sin(phi)
r1[2, 2] = cos(phi)
r2 = SX(3, 3)
r2[0, 0] = cos(theta)
r2[0, 2] = sin(theta)
r2[1, 1] = 1
r2[2, 0] = -sin(theta)
r2[2, 2] = cos(theta)
r3 = SX(3, 3)
r3[0, 0] = cos(psi)
r3[0, 1] = -sin(psi)
r3[1, 0] = sin(psi)
r3[1, 1] = cos(psi)
r3[2, 2] = 1
tau = SX(3, 3)
tau[:, 0] = (inv(r2))[:, 0]
tau[:, 1] = (inv(r2))[:, 1]
tau[:, 2] = (inv(r2 @ r1))[:, 2]
# tau[:,0] = (inv(r1))[:,0]
# tau[:,1] = (inv(r1))[:,1]
# tau[:,2] = (inv(r1 @ r2))[:,2]
# tau[0,0] = cos(theta)
# tau[0,2] = -sin(theta)
# tau[1,1] = 1
# tau[1,2] = sin(phi) * cos(theta)
# tau[2,0] = sin(theta)
# tau[2,2] = cos(phi) * cos(theta)
tc = 0.59375 # mapping control to throttle
# f_const = 100
f_const = 20
# tc = 0.75
#
# scaling_control_mat = np.array([1, 1, 1, (g/tc)])
# f_expl = vertcat(
# velocity,
# inv(tau) @ u[0:3],
# - ((r3 @ r1 @ r2)[:,2]) * (u[3] * ((g)/tc)) + gravity
# )
f_expl = vertcat(
velocity,
inv(tau) @ u[0:3], -((r3 @ r1 @ r2)[:, 2]) *
(u[3] * ((g + f_const) / tc) - f_const) + gravity)
# f_expl = vertcat(
# velocity,
# inv(tau) @ u[0:3],
# - ((r3 @ r1 @ r2)[:,2]) * ((g - f_const)*(1 - tc) / (1 - u[3]) + f_const) + gravity
# )
# xdot
xdot = SX.sym('xdot', 9)
#parameters
p = []
f_impl = xdot - f_expl
model = AcadosModel()
model.f_impl_expr = f_impl
model.f_expl_expr = f_expl
model.x = x
model.xdot = xdot
model.u = u
# model.z = z
model.p = p
model.name = model_name
return model
def gp_pred_function(x,
hyp,
kern_types,
x_train=None,
beta=None,
k_inv_training=None,
pred_var=True,
compute_grads=False):
"""
"""
n_gps = len(kern_types)
inp = SX.sym("input", x.shape)
out_dict = dict()
mu_all = []
pred_sigma_all = []
jac_mu_all = []
for i in range(n_gps):
kern_i = _get_kernel_function(kern_types[i], hyp[i])
if beta is None:
beta_i = None
else:
beta_i = beta[:, i]
k_inv_i = None
if not k_inv_training is None:
k_inv_i = k_inv_training[i]
if pred_var:
mu_new, sigma_new = gp_pred(inp, kern_i, beta_i, x_train, k_inv_i,
pred_var)
pred_func = Function("pred_func", [inp], [mu_new, sigma_new],
["inp"], ["mu_1", "sigma_1"])
F_1 = pred_func(inp=x)
pred_sigma = F_1["sigma_1"]
pred_sigma_all = horzcat(pred_sigma_all, pred_sigma)
else:
mu_new = gp_pred(inp, kern_i, beta_i, x_train, k_inv_i, pred_var)
pred_func = Function("pred_func", [inp], [mu_new], ["inp"],
["mu_1"])
F_1 = pred_func(inp=x)
mu_1 = F_1["mu_1"]
mu_all = horzcat(mu_all, mu_1)
if compute_grads:
# jac_func = pred_func.jacobian("inp","mu_1")
jac_func = pred_func.factory('dmudinp', ['inp'], ['jac:mu_1:inp'])
F_1_jac = jac_func(inp=x)
# print(F_1_jac)
jac_mu = F_1_jac['jac_mu_1_inp']
jac_mu_all = vertcat(jac_mu_all, jac_mu)
out_dict["pred_mu"] = mu_all
if pred_var:
out_dict["pred_sigma"] = pred_sigma_all
if compute_grads:
out_dict["jac_mu"] = jac_mu_all
return out_dict
from casadi import SX, Function, vertcat
from acados import ocp_nlp_function, ocp_nlp_ls_cost, ocp_nlp, ocp_nlp_solver
from models import chen_model
N = 10
ode_fun, nx, nu = chen_model()
nlp = ocp_nlp({'N': N, 'nx': nx, 'nu': nu, 'ng': N*[1] + [0]})
# ODE Model
step = 0.1
nlp.set_model(ode_fun, step)
# Cost function
Q = diag([1.0, 1.0])
R = 1e-2
x = SX.sym('x', nx)
u = SX.sym('u', nu)
u_N = SX.sym('u', 0)
f = ocp_nlp_function(Function('ls_cost', [x, u], [vertcat(x, u)]))
f_N = ocp_nlp_function(Function('ls_cost_N', [x, u_N], [x]))
ls_cost = ocp_nlp_ls_cost(N, N*[f]+[f_N])
ls_cost.ls_cost_matrix = N*[block_diag(Q, R)] + [Q]
nlp.set_cost(ls_cost)
# Constraints
g = ocp_nlp_function(Function('path_constraint', [x, u], [u]))
g_N = ocp_nlp_function(Function('path_constraintN', [x, u], [SX([])]))
nlp.set_path_constraints(N*[g] + [g_N])
for i in range(N):
nlp.lg[i] = -0.5
nlp.ug[i] = +0.5
def acados_linear_quad_model_moving_eq(num_states, g=9.81):
model_name = 'linear_quad_v2'
tc = 0.59375
# scaling_control_mat = np.array([1, 1, 1, (g/tc)])
gravity = SX.zeros(3)
gravity[2] = g
x = SX.sym('x', 9)
u = SX.sym('u', 4)
xdot = SX.sym('xdot', 9)
r1 = SX(3, 3)
r1[0, 0] = 1
r1[1, 1] = cos(x[3])
r1[1, 2] = -sin(x[3])
r1[2, 1] = sin(x[3])
r1[2, 2] = cos(x[3])
r2 = SX(3, 3)
r2[0, 0] = cos(x[4])
r2[0, 2] = sin(x[4])
r2[1, 1] = 1
r2[2, 0] = -sin(x[4])
r2[2, 2] = cos(x[4])
r3 = SX(3, 3)
r3[0, 0] = cos(x[5])
r3[0, 1] = -sin(x[5])
r3[1, 0] = sin(x[5])
r3[1, 1] = cos(x[5])
r3[2, 2] = 1
tau = SX(3, 3)
tau[:, 0] = (inv(r2))[:, 0]
tau[:, 1] = (inv(r2))[:, 1]
tau[:, 2] = (inv(r2 @ r1))[:, 2]
f_nl = vertcat(x[6:9],
inv(tau) @ u[0:3],
-((r3 @ r1 @ r2)[:, 2]) * u[3] * (g / tc) + gravity)
# f_const = 20
# f_nl = vertcat(
# x[6:9],
# inv(tau) @ u[0:3],
# - ((r3 @ r1 @ r2)[:,2]) * (u[3] * ((g + f_const)/tc) - f_const) + gravity
# )
A = jacobian(f_nl, x)
B = jacobian(f_nl, u)
p = []
f_expl = mtimes(A, x) + mtimes(B, u)
f_impl = xdot - f_expl
model = AcadosModel()
model.f_impl_expr = f_impl
model.f_expl_expr = f_expl
model.x = x
model.xdot = xdot
model.u = u
# model.z = z
model.p = p
model.name = model_name
return model
