Assigning values to parameters in external cost function

User Questions
,
1

Hi, I’m implementing a simple mpc in python such that a car follows a reference point on a track. The position of reference point keeps changing and the car has to go to that point. I’m adding the important lines of code(and not the entire model/problem definition)
I’ve declared my cost function as:

    #defining the variables
    x = ca.SX.sym("x", 5)
    u = ca.SX.sym("u", 2)
    xdot = ca.SX.sym("xdot", 5)
    xd = ca.SX.sym("xd", 5)
    p = ca.vertcat(xd)

    cost_expr = (x - xd).T @ Q @ (x - xd) + u.T @ R @ u
    
    model.p = p

and assigned it to ocp.cost.cost_type, where x is the state of the car and x is the state of the reference point.

My control loop is implemented as follows:

while True:
    ref_pos = ... #retriving the reference point pose
    car_pos = ... #retriving the current car pose

#I want to assign values of ref_pos to xd and values of car_pos to x in the cost function
for i in range(horizon):
    ocp_solver.set(i, 'p', ref_pos)

ocp_solver.set(0, 'x', car_pos)

The result I’m getting is very different. my cost reduces when I move the reference point towards origin of my simulated environment. This shouldn’t happen since the world has no role to play in this and all the errors should be w.r.t the reference point and the car.
If I move the car in any direction, there’s no change in value of cost function, which according to me means the values are not getting assigned properly.
What am I doing wrong here?

Thanks

2

Maybe you should check your terminal cost. Do you also have a parametric reference there?
Here you might be missing the last point, depending on what horizon is.

3

Hi, I just found out my mistake. I assumed ocp_solver.set(0, “x”, car_pos) is used to update the car position, but instead it is used as initial guess. I used

ocp_solver.set(0, "lbx", car_pos) 
ocp_solver.set(0, "ubx", car_pos)

and it solved my issue.
Thanks