Parameteric OCP with path constraint and LS cost

jay101 · February 20, 2020, 10:21am

Hi everyone,
Thanks a lot for developing such a wonderful toolbox. I am very new to OCP and trying to setup one in python interface but face some issues regarding following points.

My model is defined by nonlinear equation f(x,u,p) where parameter p needs an update at each stage. Currently I defined them as P = MX(‘p’, np, 1) put them in model.p following the guidelines of the example of race cars. Further I also set nlp_dims.np = model.p.size()[0] in acados_settings.py but it throws me an error,
Error: Failed to render ‘main.in.c’
Reason: Variable constraints.p not found in context while rendering ‘main.in.c’

So I also defined,
model_ac.p=constraint.p

throws me another error saying ‘Code generation of ‘filename_expl_ode_fun’ is not possible since variables [p] are free’
How do I properly implement this in the toolbox?

I have nu inputs in the nonlinear model and some these inputs are set to be equal to zero at each stage. I am doing it by updating ‘lbu’ and ‘ubu’ for entire input vector u at each stage. How can I choose some entries of vector u to be set to zero over each stage?
I have certain constraints on u like constant*u1 - u2 <=0. I believe it goes in path constraint as per the problem_formulation_ocp_mex.pdf but still it is not clear which path constraint.
I have linear least square cost for state reference tracking in my formulation ||x-x_ref||^2. I understand that W and W_e are the panalties for the reference tracking. However, I don’t understant how V_x, V_u and V_z should be defined. I see that from race cars example that they are set to Identity matrices. Does it mean that by setting certain diagonal entries in V_x/V_u/V_z to zero I can define which of variables has no tracking objective?

I am sorry if some of the issues raised are trivial but wanted to be clear with the implementation I am doing. Thanks a lot in advance!

FreyJo · February 20, 2020, 3:38pm

Hi @jay101,

First, welcome to the acados forum

These are the values with which the parameters are initialized, so a np array, not CasADi symbolics.
constraints.p

Setting control bounds via lbu, ubu is the correct way.

You can implement this by using the general linear constraints lg <=C*x + D*u <= ug.

Exactly, you want to track y which in the linear leas square case is defined by y= Vx * x + Vu * u + Vz * z.

Since you seem to get started with acados in Python, please use this branch:

github.com/acados/acados

Interface improvements, port GNSF models from Matlab to Python

acados:master ← FreyJo:dev_master

opened 04:32PM - 10 Feb 20 UTC

FreyJo

+1291 -709

GNSF can now be used from Python, after performing the structure detection in Ma…tlab and dumping the GNSF model into a json file. This also contains a few minor changes, e.g.: - bump t_renderer: fixed binaries for Windows - python: fix slack related setters, especially `Js*` - SQP: `print_level = n` will print the first `n` QP inputs + `print_level` > 0 prints all sqp residuals. - python: check dimensions against layout

There are among other changes, some checks on all the dimensions in Python which should be helpful for you.

Best,
Jonathan

jay101 · February 20, 2020, 5:15pm

Hi @FreyJo,
Thanks a lot for your prompt reply.

But to define a parameter in the model f(x,u,p) I will have to use CasADi symbolics right? Could you please tell me the way I should do it starting from i)ow to define it in the model of CasADi symbolics ii)include in the acados settings and iii)then update at each stage?
Thanks for confirming the implementation zeroing some elements of u.
I also wanted to know if there is a way to update bounds on only first two elements (for example) of u without updating bounds on entire u array. Currently, ‘lbu’ and ‘ubu’ has the size of u and I am unable to choose first two element to update the bounds.

Thanks a lot again!
Cheers,
Jay

FreyJo · February 21, 2020, 9:59am

Hi,

this is an example with nonempty parameter vector:

github.com

acados/acados/blob/c63e3ebb437cf1c8fe0b62f37ea22af81f821a5e/examples/acados_python/rsm_example/generate_c_code.py#L119


      
          
          # set up xdot
          psi_d_dot = SX.sym('psi_d_dot')
          psi_q_dot = SX.sym('psi_q_dot')
          xdot = vertcat(psi_d_dot, psi_q_dot)
          
          # set up parameters
          w      = SX.sym('w') # speed
          dist_d = SX.sym('dist_d') # d disturbance
          dist_q = SX.sym('dist_q') # q disturbance
          p      = vertcat(w, dist_d, dist_q)
          
          # build flux expression
          Psi = vertcat(psi_d_num(i_d, i_q), psi_q_num(i_d, i_q))
          
          # dynamics
          f_impl = vertcat(   psi_d_dot - u_d + Rs*i_d - w*psi_q - dist_d, \
                              psi_q_dot - u_q + Rs*i_q + w*psi_d - dist_q, \
                              psi_d - Psi[0], \
                              psi_q - Psi[1])

Here the parameter values are set/updated stagewise:

github.com

acados/acados/blob/c63e3ebb437cf1c8fe0b62f37ea22af81f821a5e/examples/acados_python/rsm_example/generate_c_code.py#L384


      
              simU[i,j] = u0[j]
          
          field_name = "u"
          
          # update initial condition
          x0 = acados_solver.get(1, "x")
          
          if i > Nsim/3 and i < Nsim/2:
              # update params
              for i in range(N):
                  acados_solver.set(i, "p", nmp.array([w_val/2.0, 0, 0]))
          else:
              # update params
              for i in range(N):
                  acados_solver.set(i, "p", nmp.array([w_val, 0, 0]))
          
          acados_solver.set(0, "lbx", x0)
          acados_solver.set(0, "ubx", x0)
          
          # update initial condition
          x0 = acados_solver.get(1, "x")

This is not possible. I dont think this would simplify things really.

Cheers,
Jonathan

jay101 · February 27, 2020, 9:24am

Hi @FreyJo,
Thanks a lot again for the answers. I was able to run the code successfully.
Can you please let me know if there is a way to get the Cost value after the solution? I see that acados_solver.get_stats() provide other values but not the cost.

FreyJo · March 3, 2020, 10:58am

I recently added the option to access the residuals of each SQP iteration from Python.
The cost value is not as easily available internally.
You could simulate a system with an extra cost state to obtain the cost value with high accuracy.

jay101 · March 3, 2020, 6:59pm

Thanks for you answer!
I have one more doubt. How can I add delta_u (input regulation) and penalty related to it for LINEAR_LS cost type? where delta_u = u_{k+1}-u_{k} and k denotes certain stage.

FreyJo · March 4, 2020, 10:26am

To do so, I would modify the model, and define the derivative of your u as the control.
Such that you have:
\dot{u_{\textrm{old}}} = u_{\textrm{new}}
making u_{\textrm{old}} part of x and penalizing u_{\textrm{new}}.

jay101 · March 9, 2020, 10:38am

I will try your suggestion and let you know if I succeed.
Although, I was able to run the OCP I am not getting same results with one iteration of SQP_RTI algorithm and the same implementation in Matlab with Yalmip. The only difference I can see in two implementations is that, in Matlab I use exact hessian and I can’t find the an option to use ‘EXACT’ with the current installation of acados. I could find some example where the exact hessian was used

github.com

bcbarbara/acados/blob/7a8845e17ce15524beb5643aa454789e27a3724f/examples/acados_python/getting_started/ocp_example_cost_formulations.py

#
# 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 file has been truncated. show original

Can you please let me know if the master branch includes the feature to use exact hessian?

FreyJo · March 9, 2020, 12:33pm

Hi @jay101,

the exact hessian for the Python interface is not merged into master yet.
Still waiting for some feedback.

But you can already use the corresponding branch.

jay101 · March 13, 2020, 12:36pm

Hi @FreyJo,
Thanks I have it working! Apparently Hessian approximation is not an issue so I wanted to check the sensitivities. I found in following script example for simulation

github.com

acados/acados/blob/master/examples/acados_python/getting_started/sim/minimal_example_sim.py

#
# 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 file has been truncated. show original

But after running this example I found that it gives me zeros for acados_integrator(“Sx”) whereas I get some values for (“Su”). Can you please tell me how to check them properly?

Also is it possible to check the references and parameter values passed to OCP problem formulation after compiling the AcadosOcpSolver object?

FreyJo · March 14, 2020, 1:10pm

Outputting sensitivities from Python was not really implemented somehow.

I just fixed it on the following branch:

The ones from the formulation can only be checked by getting it from the object again, and it can also be double checked that they are correct in the generated C code.
Also, you can just update the parameters and references after compiling.

Best,
Jonathan

jay101 · March 14, 2020, 4:23pm

Hi @FreyJo,
Thanks for the information.

Does this branch allow to check the sensitivities from AcadosOcpSolver object?

How can you get the values from the AcadosOcpSolver object before compiling? When I run AcadosOcpSolver object.get() method I can only query “x” and “u”. How can I query “yref”, “p”, etc?

After compiling acados_solver_xxx_model.c file has only the value of the parameters, yref, etc only for stage one if I am not wrong. Once I update after compiling it for every stage how can I check if the values are updated correctly for each stage?

Thanks a lot,
Jay

FreyJo · March 17, 2020, 5:49pm

Hi @jay101,

sorry for the late response.

Are you asking for the sensitivities of the optimal solution of the OCP here?
I thought you are interested in the sensitivities of the integrator, which you can get in Python using this branch: (sorry, the branch I pointed to before is where I push also other stuff these days…)
https://github.com/FreyJo/acados/tree/python_integrator_sensitivities

This is not be possible, the AcadosOcpSolver instance compiles in its constructor.

There are no getters for these quantities available, since they are just supposed to be set from outside.
Setting them can be done like here:

github.com

acados/acados/blob/52c67fe6864042a8d672a4b74441a5f7798bc1f8/examples/acados_python/race_cars/main.py#L80


      
          tcomp_sum = 0
          tcomp_max = 0
          
          # simulate
          for i in range(Nsim):
              # update reference
              sref = s0 + sref_N
              for j in range(N):
                  yref = np.array([s0 + (sref - s0) * j / N, 0, 0, 0, 0, 0, 0, 0])
                  # yref=np.array([1,0,0,1,0,0,0,0])
                  acados_solver.set(j, "yref", yref)
              yref_N = np.array([sref, 0, 0, 0, 0, 0])
              # yref_N=np.array([0,0,0,0,0,0])
              acados_solver.set(N, "yref", yref_N)
          
              # solve ocp
              t = time.time()
          
              status = acados_solver.solve()
              if status != 0:
                  raise Exception("acados returned status {} in closed loop iteration {}. Exiting.".format(status, i))

Do you think there is something wrong when updating these quantities? Or why do you think they are not set correctly?
Maybe you can make a minimal example.

I see, here you mean rendering.
Indeed, when rendering the template, y_ref and parameter values are initialized with the same value for every shooting node.

Best,
Jonathan

jay101 · March 18, 2020, 1:57pm

Hi @FreyJo,
Please don’t be sorry and I am very grateful for you patience to answer all of my questions.

No I was referring to the integrator sensitivities used to build OCP problem. I wanted to be sure that all the values of references and parameters are updated after compiling the OCP object and the integrator sensitivities used by OCP object are right. Just to be sure that I have properly configured the OCP.

I followed this example to construct OCP with only difference that I also have parameters in my formulation. For parameter updates I referred this example

github.com

acados/acados/blob/52c67fe6864042a8d672a4b74441a5f7798bc1f8/examples/acados_python/getting_started/mhe/minimal_example_mhe.py

#
# 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 file has been truncated. show original

However, I’m not sure about this but it seems like the parameters are not properly updated at every stage in my formulation. It gives me meaningful values when I don’t update the parameters at every stage (passing only initial values of parameters while building OCP object) whereas, when I update them at every stage the solution is completely different.

I know it’s difficult for you tell without looking at the formulation and I will try to make a minimal formulation that I might be able to share.

I have written an SQP_RTI algorithm in MATLAB using Yalmip and Gurobi solver with “Gauss-Newton” hessian approx. method without any condensing. In Acados, however I have to choose partial or full condensing for HPIPM and QPOASES. How can I use this solvers without any condensing to do fare comparison with MATLAB solution?

I know that I should compare with same solvers in both the formulations but I am unable to install QPOASES in MATLAB and HPIPM is not supported by Yalmip.

On top of that I also compared the solution of “SQP” (fully converged solution) solver in acados with Ipopt obtained in Matlab and they differ.

FreyJo · March 20, 2020, 9:45am

You are looking for this option:
If you set it to N, there is no condensing happening.

https://docs.acados.org/interfaces/index.html#acados_template.AcadosOcp.AcadosOcpOptions.qp_solver_cond_N

I guess if they differ much more than the solver precision, it is quite likely that the formulations are not equivalent.
It is hard to say more without looking into the details.

Indeed it is.
This would be great, please open another thread for this to keep things structured.

Gaofei · October 6, 2020, 5:51am

@FreyJo @jay101
I also got the same error in matlab interface ,when parameter p needs an update at each stage.

.../casadi/core/sx_function.cpp:147: Code generation is not possible since variables [P_0, P_1] are free.

1.this is an example with nonempty parameter vector:

github.com

acados/acados/blob/c63e3ebb437cf1c8fe0b62f37ea22af81f821a5e/examples/acados_python/rsm_example/generate_c_code.py#L119



# set up xdot
psi_d_dot = SX.sym('psi_d_dot')
psi_q_dot = SX.sym('psi_q_dot')
xdot = vertcat(psi_d_dot, psi_q_dot)

# set up parameters
w      = SX.sym('w') # speed
dist_d = SX.sym('dist_d') # d disturbance
dist_q = SX.sym('dist_q') # q disturbance
p      = vertcat(w, dist_d, dist_q)

# build flux expression
Psi = vertcat(psi_d_num(i_d, i_q), psi_q_num(i_d, i_q))

# dynamics
f_impl = vertcat(   psi_d_dot - u_d + Rs*i_d - w*psi_q - dist_d, \
                    psi_q_dot - u_q + Rs*i_q + w*psi_d - dist_q, \
                    psi_d - Psi[0], \
                    psi_q - Psi[1])

2.Here the parameter values are set/updated stagewise:

github.com

acados/acados/blob/c63e3ebb437cf1c8fe0b62f37ea22af81f821a5e/examples/acados_python/rsm_example/generate_c_code.py#L384


    simU[i,j] = u0[j]

field_name = "u"

# update initial condition
x0 = acados_solver.get(1, "x")

if i > Nsim/3 and i < Nsim/2:
    # update params
    for i in range(N):
        acados_solver.set(i, "p", nmp.array([w_val/2.0, 0, 0]))
else:
    # update params
    for i in range(N):
        acados_solver.set(i, "p", nmp.array([w_val, 0, 0]))

acados_solver.set(0, "lbx", x0)
acados_solver.set(0, "ubx", x0)

# update initial condition
x0 = acados_solver.get(1, "x")

I tried the solution you mentioned above . But its not work.

Maybe, its different between Python and Matlab.

So, Are there any examples of MATLAB？

Thank you in advance.

Best，
Gaofei

FreyJo · October 6, 2020, 7:28am

Sorry, but this is not the same error at all.
If you are not sure that it is, please open a new topic instead of posting somewhere.

This is how to update parameter values stage wise in Matlab.

github.com

acados/acados/blob/d0ef292abba784be327ada4ccd6392d1a337bc43/examples/acados_matlab_octave/test/test_ocp_wtnx6.m#L393-L396


      
          % set parameter
          for jj=0:ocp_N-1
              ocp.set('p', wind0_ref(:,ii+jj), jj);
          end

Cheers,
Jonathan