Use predicted states to update constraints

User Questions
, , ,
1

Hi :wave:

I’m new to use acados, and I want to build an optimal control problem that will use predicted states to update my constrains.

  1. problitem

Specifically, for example, a constraint maybe like

(x-2)^2+ (y-2)^2 - 4 > 0

when doing prediction, the x and y will update, I can’t just use the initial state( at the beginning) to make constraints, that may not come out what I desire.
So can you give suggestions on how to do this?

  1. what I am using
    I use python interface to build my robot model and my ocp model, and generated it into c code.
    then I use C and h files to build my ros mpc nodes with cpp.

(what I think till now maybe use ocp_nlp_constraints_model_set to update, but the expression itself needs to be change?( like what I said, x and y will change))

I would appreciate it if anyone could give me help!! :pray:

2

Hi Robin, welcome to the acados forum! :wave:

I am not quite sure if I understand your question correctly. When you say “the x and y will update” does that mean their values are known, i.e. they aren’t optimization variables?

3

Hello kaethe!
Oh I feel sorry for not giving clear description, let me add more.
x, y are my system states, so in real control process, the SQP solver will know their values at the beginning of building an optimal control problem, and use mpc to predict a set of future values which is relative to the future inputs, so if I denote x0 , y0 as known start state in a period mpc control programming.
Then in the process of building ocp, I want to update my constraints cause it’s relative to my state, but I don’t know how to do it.

(plus: I’m now working on python code and try to add nonlinear constraints when building my robot model, I may show you some part of it, I mentioned ’ “”“here are my constraints”“” ', so you may understand my problem)

from acados_template import AcadosModel
import casadi as ca
import numpy as np
import yaml


def bluerov2_model_cbf(param):
    # colom
    sym_tau = ca.SX.sym("tau", 6, 1)

    # system state and its derivative
    x, y, z = ca.SX.sym("x"), ca.SX.sym("y"), ca.SX.sym("z")
    u, v, w = ca.SX.sym("u"), ca.SX.sym("v"), ca.SX.sym("w")
    eta_linear = ca.vertcat(x, y, z)
    nu_linear = ca.vertcat(u, v, w)
    phi, theta, psi = ca.SX.sym("phi"), ca.SX.sym("theta"), ca.SX.sym("psi")
    p, q, r = ca.SX.sym("p"), ca.SX.sym("q"), ca.SX.sym("r")
    eta_angluar = ca.vertcat(phi, theta, psi)
    nu_angluar = ca.vertcat(p, q, r)
    nu = ca.vertcat(nu_linear, nu_angluar)
    state = ca.vertcat(eta_linear, eta_angluar, nu_linear, nu_angluar)

    #.............


    # state equation
    # @ means mtime
    eta_dot = J @ nu
    nu_dot = ca.inv(M) @ (
        sym_tau - (C_RB + C_A) @ nu - (D_linear + D_nonlinear) @ nu - g_eta
    )
    state_dot = ca.vertcat(eta_dot, nu_dot)

    model = AcadosModel()
     #.............
    model.x = state
    model.xdot = state_dot


    """this is special part for obstacles  cased constraints"""
    obstacles = param["obstacles"]
    n_ob = len(obstacles)
    constr = []

    """here are my constraints"""
    for i in range(n_ob):
            h_fcn = (
                (x - obstacles[i][1]) ** 2
                + (y - obstacles[i][2]) ** 2
                - obstacles[i][7] ** 2
            )
     constr.apped(h)
4

If you want to update some constant values within your optimal control problem formulation after solver creation you should implement these values as parameters model.p.

But as x and y are states in your problem, I do not see why you would want to update their values.

5

Oh, thank you so much for noticing me about putting the parameters into model.p (I haven’t paid much attention to this before)
And for the second, I don’t want to update states x and y by myself, I just want my constraints to vary with these states, and I guess model.con_h_expr = constr can do this

again, thank you a lot for giving me so instant reply. :smiling_face_with_three_hearts:

6

Hi Robin, then I think your implementation is quite right. The constraints h(x, u) will be enforced at all intermediate shooting nodes, cf. the acados problem formulation

8

A few remarks on your implementation:

  • model definition: evaluation of trigonometric functions is quite expensive. If you need \sin(\theta) multiple times, it makes sens to define sin_theta = ca.sin(theta) once and then use sin_theta. This way, the sine is computed only once.
  • At the beginning, I would recommend to use ocp.solver_options.nlp_solver_type = 'SQP' and check the output of solver.print_statistics() to see whether the solver converges
1 Like
9

Oh, I couldn’t agree more with your suggestions! And thankfully, I managed to figure out why:

I gave the improper mpc and cbf parameters(since the latter ones are hard constraints), and it definitely can’t be solved

So after make some parameters smaller, the solver can run properly and my question here is over for now.

Thank you for your help and advice!!! :smiling_face_with_three_hearts: :smiling_face_with_three_hearts: :smiling_face_with_three_hearts: :face_holding_back_tears: :face_holding_back_tears: :face_holding_back_tears:

1 Like