Python: Integrator DAE support + Update OCP solver tolerances

acados/sim/sim_common.c

@@ -318,7 +318,7 @@ int sim_out_get_(void *config_, void *dims_, sim_out *out, const char *field, vo
         for (int ii=0; ii < nx; ii++)
             xn[ii] = out->xn[ii];
     }
-    else if (!strcmp(field, "zn"))
+    else if (!strcmp(field, "zn") || !strcmp(field, "z"))
     {
         int nz;
         config->dims_get(config_, dims_, "nz", &nz);

examples/acados_python/getting_started/common/export_pendulum_ode_model.py → examples/acados_python/getting_started/common/pendulum_model.py

@@ -70,7 +70,7 @@ def export_pendulum_ode_model():
     # parameters
     p = []
 
-    # dynamics     
+    # dynamics
     denominator = M + m - m*cos(theta)*cos(theta)
     f_expl = vertcat(v1,
                      dtheta,
@@ -114,3 +114,21 @@ def export_pendulum_ode_model_with_discrete_rk4(dT):
     print("built RK4 for pendulum model with dT = ", dT)
     print(xf)
     return model
+
+
+def export_augmented_pendulum_model():
+    # pendulum model augmented with algebraic variable just for testing
+    model = export_pendulum_ode_model()
+    model_name = 'augmented_pendulum'
+
+    z = SX.sym('z')
+
+    f_impl = vertcat( model.xdot - model.f_expl_expr, \
+        z - model.u*model.u)
+
+    model.f_impl_expr = f_impl
+    model.z = z
+    model.name = model_name
+
+    return model
+

examples/acados_python/getting_started/mhe/closed_loop_mhe_ocp.py

@@ -36,7 +36,7 @@
 import sys
 sys.path.insert(0, '../common')
 
-from export_pendulum_ode_model import export_pendulum_ode_model
+from pendulum_model import export_pendulum_ode_model
 from export_mhe_ode_model import export_mhe_ode_model
 
 from export_ocp_solver import export_ocp_solver

examples/acados_python/getting_started/mhe/minimal_example_mhe.py

@@ -34,7 +34,7 @@
 import sys
 sys.path.insert(0, '../common')
 
-from export_pendulum_ode_model import export_pendulum_ode_model
+from pendulum_model import export_pendulum_ode_model
 from export_mhe_ode_model import export_mhe_ode_model
 
 from export_ocp_solver import export_ocp_solver

examples/acados_python/getting_started/mhe/minimal_example_mhe_with_noisy_param.py

@@ -34,7 +34,7 @@
 import sys
 sys.path.insert(0, '../common')
 
-from export_pendulum_ode_model import export_pendulum_ode_model
+from pendulum_model import export_pendulum_ode_model
 from export_mhe_ode_model_with_noisy_param import export_mhe_ode_model_with_noisy_param
 
 from export_ocp_solver import export_ocp_solver

examples/acados_python/getting_started/mhe/minimal_example_mhe_with_param.py

@@ -34,7 +34,7 @@
 import sys
 sys.path.insert(0, '../common')
 
-from export_pendulum_ode_model import export_pendulum_ode_model
+from pendulum_model import export_pendulum_ode_model
 from export_mhe_ode_model_with_param import export_mhe_ode_model_with_param
 
 from export_ocp_solver import export_ocp_solver

examples/acados_python/getting_started/minimal_example_closed_loop.py

@@ -35,7 +35,7 @@
 sys.path.insert(0, 'common')
 
 from acados_template import AcadosOcp, AcadosOcpSolver, AcadosSimSolver
-from export_pendulum_ode_model import export_pendulum_ode_model
+from pendulum_model import export_pendulum_ode_model
 from utils import plot_pendulum
 import numpy as np
 import scipy.linalg

examples/acados_python/getting_started/ocp/example_ocp_dynamics_formulations.py

@@ -36,7 +36,7 @@
 
 import json
 from acados_template import AcadosOcp, AcadosOcpSolver
-from export_pendulum_ode_model import export_pendulum_ode_model, export_pendulum_ode_model_with_discrete_rk4
+from pendulum_model import export_pendulum_ode_model, export_pendulum_ode_model_with_discrete_rk4
 import numpy as np
 import scipy.linalg
 from utils import plot_pendulum

examples/acados_python/getting_started/ocp/example_sqp_qpDUNES.py

@@ -35,7 +35,7 @@
 sys.path.insert(0, '../common')
 
 from acados_template import AcadosOcp, AcadosOcpSolver
-from export_pendulum_ode_model import export_pendulum_ode_model
+from pendulum_model import export_pendulum_ode_model
 import numpy as np
 import scipy.linalg
 from utils import plot_pendulum

examples/acados_python/getting_started/ocp/example_sqp_rti_loop.py

@@ -35,7 +35,7 @@
 sys.path.insert(0, '../common')
 
 from acados_template import AcadosOcp, AcadosOcpSolver
-from export_pendulum_ode_model import export_pendulum_ode_model
+from pendulum_model import export_pendulum_ode_model
 import numpy as np
 import scipy.linalg
 from utils import plot_pendulum

examples/acados_python/getting_started/ocp/minimal_example_ocp.py

@@ -35,7 +35,7 @@
 sys.path.insert(0, '../common')
 
 from acados_template import AcadosOcp, AcadosOcpSolver
-from export_pendulum_ode_model import export_pendulum_ode_model
+from pendulum_model import export_pendulum_ode_model
 import numpy as np
 import scipy.linalg
 from utils import plot_pendulum

examples/acados_python/getting_started/ocp/nonuniform_discretization_example.py

@@ -35,7 +35,7 @@
 sys.path.insert(0, '../common')
 
 from acados_template import AcadosOcp, AcadosOcpSolver, acados_dae_model_json_dump
-from export_pendulum_ode_model import export_pendulum_ode_model
+from pendulum_model import export_pendulum_ode_model
 import numpy as np
 import scipy.linalg
 from utils import plot_pendulum
@@ -128,8 +128,13 @@
 simX = np.ndarray((N+1, nx))
 simU = np.ndarray((N, nu))
 
+# change options after creating ocp_solver
 ocp_solver.options_set("step_length", 0.99999)
 ocp_solver.options_set("globalization", "fixed_step") # fixed_step, merit_backtracking
+ocp_solver.options_set("tol_eq", 1e-2)
+ocp_solver.options_set("tol_stat", 1e-2)
+ocp_solver.options_set("tol_ineq", 1e-2)
+ocp_solver.options_set("tol_comp", 1e-2)
 
 # initialize solver
 for i in range(N):

examples/acados_python/getting_started/ocp/ocp_example_cost_formulations.py

@@ -35,7 +35,7 @@
 sys.path.insert(0, '../common')
 
 from acados_template import AcadosOcp, AcadosOcpSolver
-from export_pendulum_ode_model import export_pendulum_ode_model
+from pendulum_model import export_pendulum_ode_model
 import numpy as np
 import scipy.linalg
 from utils import plot_pendulum

examples/acados_python/getting_started/sim/extensive_example_sim.py

@@ -35,7 +35,7 @@
 sys.path.insert(0, '../common')
 
 from acados_template import AcadosSim, AcadosSimSolver, acados_dae_model_json_dump
-from export_pendulum_ode_model import export_pendulum_ode_model
+from pendulum_model import export_pendulum_ode_model
 from utils import plot_pendulum
 import numpy as np
 import matplotlib.pyplot as plt

examples/acados_python/getting_started/sim/minimal_example_sim.py

@@ -35,7 +35,7 @@
 sys.path.insert(0, '../common')
 
 from acados_template import AcadosSim, AcadosSimSolver
-from export_pendulum_ode_model import export_pendulum_ode_model
+from pendulum_model import export_pendulum_ode_model
 from utils import plot_pendulum
 import numpy as np
 import matplotlib.pyplot as plt
@@ -56,6 +56,7 @@
 # set simulation time
 sim.solver_options.T = Tf
 # set options
+sim.solver_options.integrator_type = 'ERK'
 sim.solver_options.num_stages = 4
 sim.solver_options.num_steps = 3
 sim.solver_options.newton_iter = 3 # for implicit integrator

examples/acados_python/tests/soft_constraint_test.py

@@ -35,7 +35,7 @@
 sys.path.insert(0, '../getting_started/common')
 
 from acados_template import AcadosOcp, AcadosOcpSolver, AcadosSimSolver
-from export_pendulum_ode_model import export_pendulum_ode_model
+from pendulum_model import export_pendulum_ode_model
 from utils import plot_pendulum
 import numpy as np
 import scipy.linalg

examples/acados_python/tests/test_ocp_setting.py

@@ -35,7 +35,7 @@
 sys.path.insert(0, '../getting_started/common')
 
 from acados_template import *
-from export_pendulum_ode_model import export_pendulum_ode_model
+from pendulum_model import export_pendulum_ode_model
 import numpy as np
 import scipy.linalg
 import argparse

examples/acados_python/tests/test_osqp.py

@@ -35,7 +35,7 @@
 sys.path.insert(0, '../getting_started/common')
 
 from acados_template import AcadosOcp, AcadosOcpSolver
-from export_pendulum_ode_model import export_pendulum_ode_model
+from pendulum_model import export_pendulum_ode_model
 import numpy as np
 import scipy.linalg
 from utils import plot_pendulum

examples/acados_python/tests/test_parametric_nonlinear_constraint_h.py

@@ -35,7 +35,7 @@
 sys.path.insert(0, '../getting_started/common')
 
 from acados_template import AcadosOcp, AcadosOcpSolver
-from export_pendulum_ode_model import export_pendulum_ode_model
+from pendulum_model import export_pendulum_ode_model
 import numpy as np
 import scipy.linalg
 from utils import plot_pendulum

examples/acados_python/tests/test_sim_dae.py

@@ -0,0 +1,104 @@
+#
+# Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
+# Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
+# Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
+# Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
+#
+# This file is part of acados.
+#
+# The 2-Clause BSD License
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions are met:
+#
+# 1. Redistributions of source code must retain the above copyright notice,
+# this list of conditions and the following disclaimer.
+#
+# 2. Redistributions in binary form must reproduce the above copyright notice,
+# this list of conditions and the following disclaimer in the documentation
+# and/or other materials provided with the distribution.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.;
+#
+
+import sys
+sys.path.insert(0, '../getting_started/common')
+
+from acados_template import AcadosSim, AcadosSimSolver
+from pendulum_model import export_augmented_pendulum_model
+from utils import plot_pendulum
+import numpy as np
+import matplotlib.pyplot as plt
+
+sim = AcadosSim()
+
+# export model 
+model = export_augmented_pendulum_model()
+
+# set model_name 
+sim.model = model
+
+Tf = 0.1
+nx = model.x.size()[0]
+nu = model.u.size()[0]
+N = 200
+
+# set simulation time
+sim.solver_options.T = Tf
+# set options
+sim.solver_options.integrator_type = 'IRK'
+sim.solver_options.num_stages = 4
+sim.solver_options.num_steps = 3
+sim.solver_options.newton_iter = 3 # for implicit integrator
+
+sim.solver_options.sens_forw = True
+sim.solver_options.sens_adj = True
+sim.solver_options.sens_algebraic = True
+sim.solver_options.sens_hess = True
+sim.solver_options.output_z = True
+sim.solver_options.sens_algebraic = True
+sim.solver_options.sim_method_jac_reuse = True
+
+
+# create
+acados_integrator = AcadosSimSolver(sim)
+
+simX = np.ndarray((N+1, nx))
+x0 = np.array([0.0, np.pi+1, 0.0, 0.0])
+u0 = np.array([2.0])
+acados_integrator.set("u", u0)
+
+simX[0,:] = x0
+
+for i in range(N):
+    # set initial state
+    acados_integrator.set("x", simX[i,:])
+    # solve
+    status = acados_integrator.solve()
+    # get solution
+    simX[i+1,:] = acados_integrator.get("x")
+
+if status != 0:
+    raise Exception('acados returned status {}. Exiting.'.format(status))
+
+S_algebraic = acados_integrator.get("S_algebraic")
+print("S_algebraic (dz_dxu) = ", S_algebraic)
+
+z = acados_integrator.get("z")
+print("z = ", z)
+
+S_forw = acados_integrator.get("S_forw")
+print("S_forw, sensitivities of simulaition result wrt x,u:\n", S_forw)
+
+# plot results
+plot_pendulum(np.linspace(0, Tf, N+1), 10, np.zeros((N, nu)), simX, latexify=False)

interfaces/CMakeLists.txt

@@ -172,6 +172,10 @@ add_test(NAME python_pendulum_soft_constraints_example
 add_test(NAME python_pendulum_parametric_nonlinear_constraint_h_test
         COMMAND "${CMAKE_COMMAND}" -E chdir ${PROJECT_SOURCE_DIR}/examples/acados_python/tests
         python test_parametric_nonlinear_constraint_h.py)
+add_test(NAME python_test_sim_dae
+        COMMAND "${CMAKE_COMMAND}" -E chdir ${PROJECT_SOURCE_DIR}/examples/acados_python/tests
+        python test_sim_dae.py)
+
 add_test(NAME python_pmsm_example
         COMMAND "${CMAKE_COMMAND}" -E chdir ${PROJECT_SOURCE_DIR}/examples/acados_python/pmsm_example
         python main.py)

interfaces/acados_template/acados_template/acados_ocp_solver.py

@@ -1182,9 +1182,10 @@ def options_set(self, field_, value_):
             :param value_: of type int, float
         """
         int_fields = ['print_level', 'rti_phase', 'initialize_t_slacks']
-        double_fields = ['step_length']
+        double_fields = ['step_length', 'tol_eq', 'tol_stat', 'tol_ineq', 'tol_comp']
         string_fields = ['globalization']
 
+        # check field availability and type
         if field_ in int_fields:
             if not isinstance(value_, int):
                 raise Exception('solver option {} must be of type int. You have {}.'.format(field_, type(value_)))
@@ -1202,6 +1203,10 @@ def options_set(self, field_, value_):
                 raise Exception('solver option {} must be of type str. You have {}.'.format(field_, type(value_)))
             else:
                 value_ctypes = value_.encode('utf-8')
+        else:
+            raise Exception('AcadosOcpSolver.options_set() does not support field {}.'\
+                '\n Possible values are {}.'.format(field_, ', '.join(int_fields + double_fields + string_fields)))
+
 
         if field_ == 'rti_phase':
             if value_ < 0 or value_ > 2:
@@ -1211,9 +1216,11 @@ def options_set(self, field_, value_):
                 raise Exception('AcadosOcpSolver.solve(): argument \'rti_phase\' can '
                     'take only value 0 for SQP-type solvers')
 
+        # encode
         field = field_
         field = field.encode('utf-8')
 
+        # call C interface
         if field_ in string_fields:
             self.shared_lib.ocp_nlp_solver_opts_set.argtypes = \
                 [c_void_p, c_void_p, c_char_p, c_char_p]