Hello, do you ignore Arrival cost? How can I calculate it?

Inside the file Draw_MPC_tracking_v1.m there is the same code as Draw_MPC_point_stabilization_v1.m.
Could you mohamed fix it ? thanks

Hi,

Python = 3.9
Casadi = 3.6.4

I was trying out the python code shared by Mr. Mehrez during his workshop on Casadi (https://github.com/MMehrez/MPC-and-MHE-implementation-in-MATLAB-using-Casadi) and it's working fine.

While slightly modifying the problem to include acceleration as input in-lieu of velocity for similar system, I am facing an issue. The solver is returning "over constrained" warning and output is not correct (the control outputs are near zero).

"WARNING("NLP is overconstrained: There are 99 equality constraints but only 86 variables.") [.../casadi/core/nlpsol.cpp:630]"

I have rechecked the number of equality constraints given by "g" and is same as specified during the problem which is 66 (the warning specifies 99 equality constraints).

The system dynamics is based on following set of equations.

The modified code is attached herewith. Can you please see what can be wrong. Any help will be much appreciated.

FFR_casadi_v3.txt

Thanks and Regards
Karthik

For the simulation of Sim_4_MPC_Robot_tracking_mul_shooting.m. When constructing the Objective function, why the last opt_variables (X) are not been considered?

For example, in the case of N = 3 with the following setup:

K>> X

X = 
[[X_0, X_3, X_6, X_9], 
 [X_1, X_4, X_7, X_10], 
 [X_2, X_5, X_8, X_11]]
K>> P

P = 
[P_0, P_1, P_2, P_3, P_4, P_5, P_6, P_7, P_8, P_9, P_10, P_11, P_12, P_13, P_14, P_15, P_16, P_17]
K>> U

U = 
[[U_0, U_2, U_4], 
 [U_1, U_3, U_5]]

The first iteration of the objective function is comparing the first set of opt_variables (X_0, X_1, X_2) with the second set of parameters (P_3, P_4, P_5), as could be seen here:

K>> obj
obj = 
@1=0.5, @2=0.05, (((((sq((X_0-P_3))+sq((X_1-P_4)))+((@1*(X_2-P_5))*(X_2-P_5)))+(((@1*(U_0-P_6))*(U_0-P_6))+((@2*(U_1-P_7))*(U_1-P_7))))

In the last iteration the last optimization variables (X_9, X_10, X_11), neither the first parameters (P_0, P_1, P_2) are used for generating the obj function:

K>> obj
@1=0.5, @2=0.05, (((((((sq((X_0-P_3))+sq((X_1-P_4)))+((@1*(X_2-P_5))*(X_2-P_5)))+(((@1*(U_0-P_6))*(U_0-P_6))+((@2*(U_1-P_7))*(U_1-P_7))))+
((sq((X_3-P_8))+sq((X_4-P_9)))+((@1*(X_5-P_10))*(X_5-P_10))))+(((@1*(U_2-P_11))*(U_2-P_11))+((@2*(U_3-P_12))*(U_3-P_12))))+
((sq((X_6-P_13))+sq((X_7-P_14)))+((@1*(X_8-P_15))*(X_8-P_15))))+(((@1*(U_4-P_16))*(U_4-P_16))+((@2*(U_5-P_17))*(U_5-P_17))))

Why is that?

Hi

I am taking your example in python and similar to what you do in Matlab, I want my robot to avoid the obstacle.

I added the constraints to my controller but the robot goes through the inside of the object. What could I be missing?

Add constraints for collision avoidance

for k in range(N+1):   
    g = ca.vertcat(g , -ca.sqrt((X[1,k]-obs_x)**2+(X[2,k]-obs_y)**2) + (rob_diam/2 + obs_diam/2))

lbg = ca.DM.zeros((n_states*(N+1) + N+1, 1))
ubg = ca.DM.zeros((n_states*(N+1) + N+1, 1))

lbg[0: n_states*(N)] = 0
ubg[0: n_states*(N)] = 0
lbg[n_states*(N+1): n_states*(N+1) + N+1] = -ca.inf
ubg[n_states*(N+1): n_states*(N+1) + N+1] = 0

Thanks for help

Hi Professor Mehrez,

Is there any reason why the covariance matrix is powered to ^2 and later calculated back with sqrt?

Why are we setting X as an optimization variable as well in this case? I think I may be missing something here.
OPT_variables = ca.vertcat( X.reshape((-1, 1)), # Example: 3x11 ---> 33x1 where 3=states, 11=N+1 Why are we optimizing over X U.reshape((-1, 1)) )