optimization_engine 0.3.1

A pure Rust framework for embedded nonconvex optimization. Ideal for robotics!
Documentation 
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89

% parameters
L = 0.5; ts = 0.1;

% Prediction horizon
N = 50; 

% target point and bearing
xref=1; yref=1; thetaref = 0;

% weights
q = 10; qtheta = .1; r = 1;
qN = 10*q; qthetaN = 10*qtheta;

nu = 2; nx = 3;
u = casadi.SX.sym('u', nu*N); 
z0 = casadi.SX.sym('z0', nx);

x = z0(1); y = z0(2); theta = z0(3);
cost = 0;
for t = 1:nu:nu*N
    cost = cost + q*((x-xref)^2 + (y-yref)^2) + qtheta*(theta-thetaref)^2 ;  
    u_t = u(t:t+1);
    theta_dot = (1/L)*(u_t(2)*cos(theta) - u_t(1)*sin(theta));
    cost = cost + r*(u_t'*u_t);
    x = x + ts * (u_t(1) + L * sin(theta) * theta_dot);
    y = y + ts * (u_t(2) - L * cos(theta) * theta_dot);
    theta = theta + ts * theta_dot;
end
cost = cost + qN*((x-xref)^2 + (y-yref)^2) + qthetaN*(theta-thetaref)^2;

constraints = OpEnConstraints.make_no_constraints();
builder = OpEnOptimizerBuilder()...
    .with_problem(u, z0, cost, constraints)...
    .with_build_name('navigation')...
    .with_build_mode('release')...
    .with_fpr_tolerance(1e-4)...
    .with_max_iterations(1000);
optimizer = builder.build();

%%
optimizer.run();
optimizer.connect();
%%
z_init = [0.55;0.25;deg2rad(80)];
out = optimizer.consume(z_init);
out


% Simulate
Z = z_init; x = Z(1); y = Z(2); theta = Z(3);
U = out.u;
for t=1:2:2*N    
    u_t = U(t:t+1);
    theta_dot = (1/L)*(u_t(2)*cos(theta) - u_t(1)*sin(theta));
    x = x + ts * (u_t(1) + L * sin(theta) * theta_dot);
    y = y + ts * (u_t(2) - L * cos(theta) * theta_dot);
    theta = theta + ts * theta_dot;
    Z = [Z [x;y;theta]];    
end

figure;
subplot(311); plot(reshape(out.u,  2, N)'); grid on;
subplot(312); plot(Z(1,:), Z(2,:)); grid on;
subplot(313); plot(Z(3,:)); grid on;

%%
optimizer.stop();

%%
plot(reshape(out.u,  2, N)', 'linewidth', 2);
grid on;
xlabel('Time instant');
ylabel('Position (x,y)');
legend('x-position', 'y-position')

%%
plot(Z(1,:), Z(2,:), 'linewidth', 2);
hold on;
plot(xref, yref, 'r+', 'linewidth', 3)
grid on;
xlabel('x');
ylabel('y');
legend('trajectory','target')

%%
plot(0:N, rad2deg(Z(3,:)), 'linewidth', 2);
grid on;
xlabel('Time instant');
ylabel('theta (deg)');