alphaville / optimization-engine

Nonconvex embedded optimization: code generation for fast real-time optimization

Home Page:https://alphaville.github.io/optimization-engine/

Geek Repo:Geek Repo

Github PK Tool:Github PK Tool

An error when optimizing

MuXingchui opened this issue · comments

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:

  • OS: Windows
  • What is the output of rustup show?
Default host: x86_64-pc-windows-msvc
rustup home:  C:\Users\****\.rustup

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.