8000 Python: Add translate functionality from LS/NLS/CONL cost to external cost by sandmaennchen · Pull Request #1330 · acados/acados · GitHub
[go: up one dir, main page]
More Web Proxy on the site http://driver.im/
Skip to content

Python: Add translate functionality from LS/NLS/CONL cost to external cost #1330

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
merged 9 commits into from
Oct 30, 2024
Merged

Python: Add translate functionality from LS/NLS/CONL cost to external cost #1330

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
9 commits
Select commit Hold shift + click to select a range
4566661
add translate function
sandmaennchen Oct 29, 2024
e108c03
fix transcription
sandmaennchen Oct 29, 2024
a6dc5d0
test transcription
sandmaennchen Oct 29, 2024
27f10ed
test with parametric yref
sandmaennchen Oct 30, 2024
413dd75
cleanup
sandmaennchen Oct 30, 2024
1ac8ac4
call make consistent before translating
sandmaennchen Oct 30, 2024
c51bdfe
typo
sandmaennchen Oct 30, 2024
d29066f
Revert "call make consistent before translating"
sandmaennchen Oct 30, 2024
7b4b1e8
move translate to very end
sandmaennchen Oct 30, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
10 changes: 8 additions & 2 deletions examples/acados_python/pendulum_on_cart/ocp/ocp_example_cost_formulations.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -198,7 +198,7 @@ def formulate_ocp(cost_version: str) -> AcadosOcp:
return ocp


def main(cost_version: str, formulation_type='ocp', integrator_type='IRK', plot=False):
def main(cost_version: str, formulation_type='ocp', integrator_type='IRK', reformulate_to_external=False, plot=False):

if cost_version == 'EXTERNAL':
ext_cost_use_num_hess = True
Expand All @@ -220,7 +220,6 @@ def main(cost_version: str, formulation_type='ocp', integrator_type='IRK', plot=
else:
ocp = formulate_ocp(cost_version)


# set options
ocp.solver_options.qp_solver = 'PARTIAL_CONDENSING_HPIPM' # FULL_CONDENSING_QPOASES, FULL_CONDENSING_DAQP, FULL_CONDENSING_HPIPM
ocp.solver_options.hessian_approx = HESSIAN_APPROXIMATION
Expand All @@ -246,6 +245,10 @@ def main(cost_version: str, formulation_type='ocp', integrator_type='IRK', plot=
# [00, 00, 00, @1, 00],
# [00, 00, 00, 00, @1]])

if reformulate_to_external:
ocp.solver_options.fixed_hess = 0
ocp.translate_cost_to_external_cost(parametric_yref=True)

# create solver
ocp_solver = AcadosOcpSolver(ocp)

Expand Down Expand Up @@ -299,6 +302,9 @@ def main(cost_version: str, formulation_type='ocp', integrator_type='IRK', plot=
print(f"cost version: {cost_version}, formulation type: {formulation_type}")
main(cost_version=cost_version, formulation_type=formulation_type, plot=False)

print(f"cost version: {cost_version} reformulated as EXTERNAL cost")
main(cost_version=cost_version, formulation_type='ocp', plot=False, reformulate_to_external=True)

# timings
# time_tot = 1e8
# time_lin = 1e8
Expand Down
98 changes: 98 additions & 0 deletions interfaces/acados_template/acados_template/acados_ocp.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -1179,6 +1179,104 @@ def translate_nls_cost_to_conl(self):
return


def translate_cost_to_external_cost(self, parametric_yref: bool = False):
"""
Translates cost to EXTERNAL cost.
parametric_yref: If true, augment with additional parameters for yref_0, yref, yref_e.
"""
# make yref a parameter
yref_0 = self.cost.yref_0
yref = self.cost.yref
yref_e = self.cost.yref_e

if parametric_yref:
symbol = self.model.get_casadi_symbol()
if self.cost.yref_0 is not None:
param_yref_0 = symbol('param_yref_0', len(self.cost.yref_0))
self.model.p = ca.vertcat(self.model.p, param_yref_0)
self.parameter_values = np.concatenate((self.parameter_values, self.cost.yref_0))
yref_0 = param_yref_0

if self.cost.yref is not None:
param_yref = symbol('param_yref', len(self.cost.yref))
self.model.p = ca.vertcat(self.model.p, param_yref)
self.parameter_values = np.concatenate((self.parameter_values, self.cost.yref))
yref = param_yref

if self.cost.yref_e is not None:
param_yref_e = symbol('param_yref_e', len(self.cost.yref_e))
self.model.p = ca.vertcat(self.model.p, param_yref_e)
self.parameter_values = np.concatenate((self.parameter_values, self.cost.yref_e))
yref_e = param_yref_e

# initial stage
if self.cost.cost_type_0 == "LINEAR_LS":
self.model.cost_expr_ext_cost_0 = \
self.__translate_ls_cost_to_external_cost(self.model.x, self.model.u, self.model.z,
self.cost.Vx_0, self.cost.Vu_0, self.cost.Vz_0,
yref_0, self.cost.W_0)
elif self.cost.cost_type_0 == "NONLINEAR_LS":
self.model.cost_expr_ext_cost_0 = \
self.__translate_nls_cost_to_external_cost(self.model.cost_y_expr_0, yref_0, self.cost.W_0)

elif self.cost.cost_type_0 == "CONVEX_OVER_NONLINEAR":
self.model.cost_expr_ext_cost_0 = \
self.__translate_conl_cost_to_external_cost(self.model.cost_r_in_psi_expr_0, self.model.cost_psi_expr_0,
self.model.cost_y_expr_0, yref_0)
# intermediate stages
if self.cost.cost_type == "LINEAR_LS":
self.model.cost_expr_ext_cost = \
self.__translate_ls_cost_to_external_cost(self.model.x, self.model.u, self.model.z,
self.cost.Vx, self.cost.Vu, self.cost.Vz,
yref, self.cost.W)
elif self.cost.cost_type == "NONLINEAR_LS":
self.model.cost_expr_ext_cost = \
self.__translate_nls_cost_to_external_cost(self.model.cost_y_expr, yref, self.cost.W)
elif self.cost.cost_type == "CONVEX_OVER_NONLINEAR":
self.model.cost_expr_ext_cost = \
self.__translate_conl_cost_to_external_cost(self.model.cost_r_in_psi_expr, self.model.cost_psi_expr,
self.model.cost_y_expr, yref)
# terminal stage
if self.cost.cost_type_e == "LINEAR_LS":
self.model.cost_expr_ext_cost_e = \
self.__translate_ls_cost_to_external_cost(self.model.x, self.model.u, self.model.z,
self.cost.Vx_e, None, None,
yref_e, self.cost.W_e)
elif self.cost.cost_type_e == "NONLINEAR_LS":
self.model.cost_expr_ext_cost_e = \
self.__translate_nls_cost_to_external_cost(self.model.cost_y_expr_e, yref_e, self.cost.W_e)
elif self.cost.cost_type_e == "CONVEX_OVER_NONLINEAR":
self.model.cost_expr_ext_cost_e = \
self.__translate_conl_cost_to_external_cost(self.model.cost_r_in_psi_expr_e, self.model.cost_psi_expr_e,
self.model.cost_y_expr_e, yref_e)
if self.cost.cost_type_0 is not None:
self.cost.cost_type_0 = 'EXTERNAL'
self.cost.cost_type = 'EXTERNAL'
self.cost.cost_type_e = 'EXTERNAL'


@staticmethod
def __translate_ls_cost_to_external_cost(x, u, z, Vx, Vu, Vz, yref, W):
res = 0
if Vx is not None:
res += Vx @ x
if Vu is not None and casadi_length(u) > 0:
res += Vu @ u
if Vz is not None and casadi_length(z) > 0:
res += Vz @ z
res -= yref

return 0.5 * (res.T @ W @ res)

@staticmethod
def __translate_nls_cost_to_external_cost(y_expr, yref, W):
res = y_expr - yref
return 0.5 * (res.T @ W @ res)

@staticmethod
def __translate_conl_cost_to_external_cost(r, psi, y_expr, yref):
return ca.substitute(psi, r, y_expr - yref)

def formulate_constraint_as_L2_penalty(
self,
constr_expr: ca.SX,
Expand Down
Loading
0