alphaville / optimization-engine

Nonconvex embedded optimization: code generation for fast real-time optimization + ROS support
https://alphaville.github.io/optimization-engine/
Other
499 stars 53 forks source link

An error when optimizing #321

Closed MuXingchui closed 3 months ago

MuXingchui commented 1 year ago

Problem description: This problem occurs when I solve NMPC in trajectory tracking, calling the solver. In the Terminal

thread 'main' panicked at 'didn't receive data: Os { code: 10040, kind: Uncategorized, message: "A message sent on a datagram socket was larger than the internal message buffer or some other network limit, or the User received datagram in buffer smaller than datagram." }', src\main.rs:40:70
note: run with `RUST_BACKTRACE=1` environment variable to display a backtrace
error: process didn't exit successfully: `target\debug\track_trajectory.exe` (exit code: 101)

In the MATLAB, it prompts

wrong use of jsondecode
JSON syntax error: expected value but found end of text.

Part of the code: In MATLAB

dt = 1;    
L =8.57;    % wheelbase
% weights
q = 10; qtheta = 2; r = 20;
qN = 100*q; qthetaN = 50*qtheta;
N = 10;  % The length of the receding horizon

nu = 2; nx = 3;

u = casadi.SX.sym('u', nu*N); % u:= [v delta] 
p = casadi.SX.sym('p', nx+nx*(N+1)+1); % init state(1-nx),reference point(xref yref yawref), p(end)=vref
x = p(1); y = p(2); yaw = p(3);
vref = p(end); 
cost = 0;
for t = 1 : N    
    xref = p(t+3); yref = p(t+N+4); yawref = p(2*(N+1)+t+3); 
    x_error  = x - xref;
    y_error = y - yref;
    latError = y_error*cos(yawref) - x_error*sin(yawref);
    StationErr = y_error * sin(yawref) - x_error* cos(yawref);
    cost = cost + q*latError^2 + q*StationErr^2 + qtheta*(yaw-yawref)^2 ;         
    u_t = u((t-1)*2+1 : (t-1)*2+2);
    cost = cost + r*(u_t(1) - vref);
    [x,y,yaw] = updateState(x, y, yaw, u_t(1) , u_t(2), dt, L, max_steer); 
end
xref = p(N+4); yref = p(2*N+5); yawref = p(3*N+6);
x_error  = x - xref;
y_error = y - yref;
latError = y_error * cos(yawref) - x_error * sin(yawref);
StationErr = y_error * sin(yawref) - x_error * cos(yawref);
cost = cost + qN*latError^2 + qN*StationErr^2 + qthetaN*(yaw-yawref)^2 ;

constraints = OpEnConstraints.make_no_constraints();
builder = OpEnOptimizerBuilder()...
    .with_problem(u, p, cost, constraints)...
    .with_build_name('track_trajectory')...
    .with_fpr_tolerance(1e-4)...
    .with_max_iterations(100);
optimizer = builder.build();
optimizer.run();
optimizer.connect();

%% main
% state_init
x = 2; 
y = 2; 
yaw = 0.02; 

U = zeros(2*N, 1);
target_v = 1;
idx =0;
pos_actual = [refPos_x,refPos_y];
z_init = [x,y,yaw]';
z_ref = [];

while idx < length(refPos_x) - 1
    % Calculate first reference point
    idx = calc_target_index(x, y, refPos_x, refPos_y); 

    %  reference point  
    z_ref = [refPos_x(idx : idx+N); refPos_y(idx : idx+N); refYaw(idx : idx+N)];
    P = [z_init; z_ref; target_v];

    out = optimizer.consume(P); % Prompt here is an error, the error description is consistent with the above

    % update states
    [x,y,yaw] = updateState(x, y, yaw, out.u(1), out.u(2), dt, L, max_steer); 

    z_init = [x;y;yaw];    
    % draw result
    scatter(x,y,150,'r.')
    pause(0.01);
end

System information:

stable-x86_64-pc-windows-msvc (default) rustc 1.67.1 (d5a82bbd2 2023-02-07)

 - What is the output of `rustc -V`?

rustc 1.67.1 (d5a82bbd2 2023-02-07)


 - MATLAB: R2022b
----
what is wrong? Please help me.
niezifei commented 3 months ago

Exactly same issue from my side when solving a trajectory optimization problem in MATLAB interface: `Error using jsondecode JSON syntax error: expected value but found end of text.

Error in OpEnOptimizer/consume (line 63) out = jsondecode(json_response);

Error in PANOC (line 114) out = optimizer.consume([predicted_Xp(:,1:1+Np),x]);`

@alphaville

niezifei commented 3 months ago

Hi @MuXingchui, I met with the exactly same problem while running my nmpc simulation. I would like to ask have you solved the problem and how? Thank you.

MuXingchui commented 3 months ago

Hi @MuXingchui, I met with the exactly same problem while running my nmpc simulation. I would like to ask have you solved the problem and how? Thank you.

As @alphaville reply: The MATLAB interface of OpEn has not been supported since v0.5. We have focused on the Python interface instead. You can't impose (hard) constraints in MATLAB, but you can do this in Python where you can use the method with_penalty_constraints. See this link and this link