Skip to content

Instantly share code, notes, and snippets.

@manucalop

manucalop/RAL-ICRA-2020_Manuel_Castillo-Lopez_one_horizon_motion_planning_benchmark.m

Last active December 9, 2023 08:52
Show Gist options
  • Star 5 You must be signed in to star a gist
  • Fork 2 You must be signed in to fork a gist
  • Save manucalop/7b2d8bff32c74abeb6c5f8fbd6725ee3 to your computer and use it in GitHub Desktop.
Save manucalop/7b2d8bff32c74abeb6c5f8fbd6725ee3 to your computer and use it in GitHub Desktop.
Download ZIP
RAL-ICRA-2020_Manuel_Castillo-Lopez -- One horizon motion planning benchmark
Raw
RAL-ICRA-2020_Manuel_Castillo-Lopez_one_horizon_motion_planning_benchmark.m
%% Initial setup
clc;
close all;
clear all;
colours = [ 0 0.4470 0.7410
0.8500 0.3250 0.0980
0.9290 0.6940 0.1250
0.4940 0.1840 0.5560
0.4660 0.6740 0.1880
0.3010 0.7450 0.9330
0.6350 0.0780 0.1840];
lw = 1.5;
ms = 8;
addpath('/home/manuel/manuel_ws/casadi_ws/casadi_prog')
import casadi.*
%------------------------------------------------------
T = 8; % Time horizon
N = 40; % number of control intervals
dt = T/N; % length of 1 control interval [s]
tau = 1;
S = [0.4, 0, 0; 0, 0.1, 0; 0, 0, 0.01];
alpha = 0.01;
r_box_x = 1;
r_box_y = 2;
r_obs_x = r_box_x*sqrt(2);
r_obs_y = r_box_y*sqrt(2);
E = [1/r_obs_x, 0, 0;
0 1/r_obs_y, 0;
0 0 0];
xo= 5;
yo= -0.01;
zo= 0;
xo2 = 1;
yo2 = 4;
x_end = 10;
y_end = 0;
%% Continuous dynamics
addpath('/home/manuel/manuel_ws/casadi_ws/casadi_prog')
import casadi.*
%------------------------------------------------------
p = MX.sym('p',3);
yaw = MX.sym('yaw');
v = MX.sym('v',4);
u = MX.sym('u',4);
x = [p; yaw; v];
R = MX([cos(yaw) -sin(yaw) 0;
sin(yaw) cos(yaw) 0;
0 0 1]);
xdot = [ R*v(1:3); v(4); -(v+u)/tau];
% Continuous time dynamics
f = Function('f', {x, u}, {xdot},{'x','u'},{'xdot'});
%% Integrator
addpath('/home/manuel/manuel_ws/casadi_ws/casadi_prog')
import casadi.*
%------------------------------------------------------
%The problem we want to solve
prob = struct('x',x,'p',u,'ode',f(x,u));
% Runge Kutta order 4
rk_opts = struct;
rk_opts.t0 = 0;
rk_opts.tf = T/N;
rk_opts.number_of_finite_elements = 1;
% Legendre order 4
leg_opts = struct;
leg_opts.collocation_scheme = 'legendre';
leg_opts.t0 = 0;
leg_opts.tf = T/N;
leg_opts.number_of_finite_elements = 1;
leg_opts.interpolation_order = 4;
% Reference Runge-Kutta implementation
intg = integrator('intg','rk',prob,rk_opts);
res = intg('x0',x,'p',u);
% Discretized (sampling time dt) system dynamics as a CasADi Function
F = Function('F', {x, u}, {res.xf},{'x(0)','u(0)'},{'x(dt)'});
%%
x0 = zeros(numel(x),1);
x0(1) = 0;
x0(2) = 0;
u0 = zeros(numel(u),1);
%Plane
v = [x0(1) - xo, x0(2) - yo, x0(3) - zo]';
n = v/norm(v);
ref = repmat([x_end;y_end;0],1,N+1);
%% Optimal control problem (Zhu)
disp('==================================================================')
disp('Zhu')
disp('==================================================================')
addpath('/home/manuel/manuel_ws/casadi_ws/casadi_prog')
addpath('/home/manuel/manuel_ws/matlab_ws/shapes')
import casadi.*
%------------------------------------------------------
opti = casadi.Opti();
% Decision variables for states
X = opti.variable(numel(x),N+1);
U = opti.variable(numel(u),N);
% Parameter for initial state
%x0 = opti.parameter(nx);
% Aliases for states
px = X(1,:);
py = X(2,:);
pz = X(3,:);
theta = X(4,:);
vf = X(5,:);
vs = X(6,:);
vu = X(7,:);
vh = X(8,:);
pos = X(1:3,:);
% problem set up
p0 = [0; 0]; % initial position of the robot
pg = [x_end; 0]; % goal location of the robot
q0 = [xo; yo]; % initial position of the obstacle center
Sigma_q = diag([S(1,1), S(2,2)]); % position uncertainty of the obstacle
r_box = [r_box_x; r_box_y]; % size of the box, half length and half width
r_obs = sqrt(2)*r_box; % [a;b] of the enclosed ellipse
Omega_obs = diag([1/r_obs(1)^2, 1/r_obs(2)^2]); % Omega of the obstable
z_alpha = erfinv(1-2.0*alpha/N); % \erf^{-1}(1-2*alpha)
% collision avoidance constraints
obs_pos = q0; % obs pos
obs_pos_cov = Sigma_q; % obs pos cov
a = r_obs(1); % obs size
b = r_obs(2);
Omega_root = [ 1/a, 0; ...
0, 1/b]; % transformation matrix
% Gap-closing shooting constraints
for k=1:N
% Model constraints
opti.subject_to(X(:,k+1)==F(X(:,k),U(:,k)));
% Obstacle constraints
ego_pos = [px(k); py(k)];
obs_pos = [xo; yo];
pos_ro = Omega_root*(ego_pos - obs_pos);
cov_ro = transpose(Omega_root) * obs_pos_cov * Omega_root;
pos_ro_norm = sqrt(transpose(pos_ro)*pos_ro);
c_ro = z_alpha * sqrt(2*transpose(pos_ro)*cov_ro*pos_ro) / pos_ro_norm;
cons_obs = pos_ro_norm - 1 - c_ro;
%opti.subject_to( n(1)*(px(k)-xo) + n(2)*(py(k)-yo) > r_obs + z_alpha.*sqrt(transpose(n)*S*n));
%opti.subject_to( n(1)*(px(k)-xo)/r_obs_x + n(2)*(py(k)-yo)/r_obs_y > 1 + z_alpha.*sqrt(transpose(n)*E*S*E'*n));
opti.subject_to(cons_obs >0);
end
opti.subject_to(X(:,1)==x0);
%Cost
L=[];
% for i= 1:N
% L = L+(pos(:,i) - ref(:,i)).^2 + U(:,i).^2
% end
opti.minimize(sumsqr(pos-ref) + 2*sumsqr(U));
opti.solver('ipopt')
sol = opti.solve();
sol1.px = sol.value(px);
sol1.py = sol.value(py);
sol1.pz = sol.value(pz);
%% Optimal control problem 2 (Kamel)
disp('==================================================================')
disp('Kamel')
disp('==================================================================')
addpath('/home/manuel/manuel_ws/casadi_ws/casadi_prog')
addpath('/home/manuel/manuel_ws/matlab_ws/shapes')
import casadi.*
%------------------------------------------------------
opti = casadi.Opti();
% Decision variables for states
X = opti.variable(numel(x),N+1);
U = opti.variable(numel(u),N);
% Parameter for initial state
%x0 = opti.parameter(nx);
% Aliases for states
px = X(1,:);
py = X(2,:);
pz = X(3,:);
theta = X(4,:);
vf = X(5,:);
vs = X(6,:);
vu = X(7,:);
vh = X(8,:);
pos = X(1:3,:);
r_max = max([r_obs_x r_obs_y]);
s_max = max(max(S));
% Gap-closing shooting constraints
for k=1:N
opti.subject_to(X(:,k+1)==F(X(:,k),U(:,k)));
opti.subject_to( (px(k) - xo).^2 + (py(k)-yo).^2 > (r_max + 3*s_max).^2);
end
opti.subject_to(X(:,1)==x0);
%Cost
L=[];
% for i= 1:N
% L = L+(pos(:,i) - ref(:,i)).^2 + U(:,i).^2
% end
opti.minimize(sumsqr(pos-ref) + 2*sumsqr(U));
opti.solver('ipopt')
sol = opti.solve();
sol2.px = sol.value(px);
sol2.py = sol.value(py);
sol2.pz = sol.value(pz);
%% Optimal control problem (Vasileios)
disp('==================================================================')
disp('Vasileios')
disp('==================================================================')
z_alpha = norminv(1-alpha/(6*N));
addpath('/home/manuel/manuel_ws/casadi_ws/casadi_prog')
addpath('/home/manuel/manuel_ws/matlab_ws/shapes')
import casadi.*
%------------------------------------------------------
opti = casadi.Opti();
% Decision variables for states
X = opti.variable(numel(x),N+1);
U = opti.variable(numel(u),N);
% Parameter for initial state
%x0 = opti.parameter(nx);
% Aliases for states
px = X(1,:);
py = X(2,:);
pz = X(3,:);
theta = X(4,:);
vf = X(5,:);
vs = X(6,:);
vu = X(7,:);
vh = X(8,:);
pos = X(1:3,:);
% Gap-closing shooting constraints
for k=1:N
opti.subject_to(X(:,k+1)==F(X(:,k),U(:,k)));
opti.subject_to( ((px(k) - xo)/(r_box_x + z_alpha*S(1,1))).^2 + ((py(k)-yo)/(r_box_y+ z_alpha*S(2,2))).^2 > 2);
end
opti.subject_to(X(:,1)==x0);
%Cost
L=[];
% for i= 1:N
% L = L+(pos(:,i) - ref(:,i)).^2 + U(:,i).^2
% end
opti.minimize(sumsqr(pos-ref) + 2*sumsqr(U));
opti.solver('ipopt')
sol = opti.solve();
sol4.px = sol.value(px);
sol4.py = sol.value(py);
sol4.pz = sol.value(pz);
%% Optimal control problem (Ours)
disp('==================================================================')
disp('Ours')
disp('==================================================================')
z_alpha = norminv(1-alpha/N);
addpath('/home/manuel/manuel_ws/casadi_ws/casadi_prog')
addpath('/home/manuel/manuel_ws/matlab_ws/shapes')
import casadi.*
%------------------------------------------------------
opti = casadi.Opti();
% Decision variables for states
X = opti.variable(numel(x),N+1);
U = opti.variable(numel(u),N);
% Parameter for initial state
%x0 = opti.parameter(nx);
% Aliases for states
px = X(1,:);
py = X(2,:);
pz = X(3,:);
theta = X(4,:);
vf = X(5,:);
vs = X(6,:);
vu = X(7,:);
vh = X(8,:);
pos = X(1:3,:);
% Gap-closing shooting constraints
for k=1:N
opti.subject_to(X(:,k+1)==F(X(:,k),U(:,k)));
opti.subject_to( ((px(k) - xo)/(r_box_x + z_alpha*S(1,1))).^2 + ((py(k)-yo)/(r_box_y+ z_alpha*S(2,2))).^2 > 2);
end
opti.subject_to(X(:,1)==x0);
%Cost
L=[];
% for i= 1:N
% L = L+(pos(:,i) - ref(:,i)).^2 + U(:,i).^2
% end
opti.minimize(sumsqr(pos-ref) + 2*sumsqr(U));
opti.solver('ipopt')
sol = opti.solve();
sol3.px = sol.value(px);
sol3.py = sol.value(py);
sol3.pz = sol.value(pz);
%% Optimal control problem (MINLP)
disp('==================================================================')
disp('Blackmore')
disp('==================================================================')
z_alpha = norminv(1-alpha/N);
addpath('/home/manuel/manuel_ws/casadi_ws/casadi_prog')
addpath('/home/manuel/manuel_ws/matlab_ws/shapes')
import casadi.*
%------------------------------------------------------
opti = casadi.Opti();
% Decision variables for states
X = opti.variable(numel(x),N+1);
B = opti.variable(4, N+1);
U = opti.variable(numel(u),N);
% Parameter for initial state
%x0 = opti.parameter(nx);
% Aliases for states
px = X(1,:);
py = X(2,:);
pz = X(3,:);
theta = X(4,:);
vf = X(5,:);
vs = X(6,:);
vu = X(7,:);
vh = X(8,:);
b1 = B(1,:);
b2 = B(2,:);
b3 = B(3,:);
b4 = B(4,:);
pos = X(1:3,:);
M = 100;
% Gap-closing shooting constraints
for k=1:N
opti.subject_to(X(:,k+1)==F(X(:,k),U(:,k)));
%opti.subject_to( ((px(k) - xo)/(r_box_x + z_alpha*S(1,1))).^2 + ((py(k)-yo)/(r_box_y+ z_alpha*S(2,2))).^2 > 2);
opti.subject_to( px(k) > xo + r_box_x + z_alpha*S(1,1) - M*b1(k));
opti.subject_to( px(k) < xo - r_box_x - z_alpha*S(1,1) + M*b2(k));
opti.subject_to( py(k) > yo + r_box_y + z_alpha*S(2,2) - M*b3(k));
opti.subject_to( py(k) < yo - r_box_y - z_alpha*S(2,2) + M*b4(k));
opti.subject_to(0<=B(:,k)<=1);
opti.subject_to(b1(k) + b2(k) + b3(k) +b4(k) <= 3)
end
discrete = [zeros(1, numel(X)) ones(1,numel(B)) zeros(1,numel(U))];
opti.subject_to(X(:,1)==x0);
%Cost
L=[];
% for i= 1:N
% L = L+(pos(:,i) - ref(:,i)).^2 + U(:,i).^2
% end
opti.minimize(sumsqr(pos-ref) + 2*sumsqr(U));
opts.discrete = discrete;
opti.solver('bonmin',opts)
sol = opti.solve();
sol5.px = sol.value(px);
sol5.py = sol.value(py);
sol5.pz = sol.value(pz);
%% Plot 2D
addpath('/home/manuel/manuel_ws/matlab_ws/plots')
y = linspace(-3,3,100);
x = xo + r_obs_x*(1 + z_alpha.*sqrt(transpose(n)*E*S*E'*n) - n(2)*(y-yo)/r_obs_y)/n(1);
close all;
fig =figure;
hold on
plot(sol1.px,sol1.py,'-o','Color',colours(5,:), 'LineWidth', lw, 'MarkerSize', ms)
plot(sol2.px,sol2.py,'-*','Color',colours(3,:), 'LineWidth', lw, 'MarkerSize', ms)
plot(sol5.px,sol5.py,'-s','Color',colours(4,:), 'LineWidth', lw, 'MarkerSize', ms)
plot(sol3.px,sol3.py,'->','Color',colours(1,:), 'LineWidth', lw, 'MarkerSize', ms)
%plot(sol4.px,sol4.py,'->','Color',colours(2,:), 'LineWidth', lw, 'MarkerSize', ms)
%plot(x,y,'Color',colours(3,:), 'LineWidth', lw, 'MarkerSize', ms)
rectangle('Position',[xo-r_obs_x yo-r_obs_y 2*r_obs_x 2*r_obs_y],'Curvature',[1 1],'LineWidth', lw, 'FaceColor', [colours(2,:) 0.25], 'EdgeColor', [colours(2,:) 0.85]);
rectangle('Position',[xo-r_box_x yo-r_box_y 2*r_box_x 2*r_box_y], 'LineWidth', lw, 'FaceColor', [colours(2,:) 0.9], 'EdgeColor', [colours(2,:) 0.85]);
%rectangle('Position',[xo-r_obs yo-r_obs 2*r_obs 2*r_obs],'Curvature',[1 1])
%axis([-2 2 0 6])
%rectangle('Position',[xo-0.2 yo-0.2 0.4 0.4],'Curvature',[0.2 0.2], 'LineWidth', lw, 'FaceColor', 'w', 'EdgeColor', 'none');
%text('Position',[xo-0.05, yo],'string','O','FontSize',15)
ylim([-4 4])
xlim([0 10])
daspect([1 1 1]);
legend('Linearized chance constraint (Zhu et. al. [6])', 'Robust constraint (Kamel et. al. [5])','Disjunctive chance constraint (Blackmore et. al. [4])','Our approach','Location','southeast');
xlabel('$x (m)$')
ylabel('$y (m)$')
grid on
width = 6;
height = 5;
fontsize = 14;
printPaperFig(fig, width, height, fontsize,'trajectory');
%% Solution (ipopt comparison)
zhu.obj = 3.2140000085882039e+03;
zhu.sec = 0.595;
kamel.obj = 1.3419447587855689e+03;
kamel.sec = 0.652;
vasi.obj = 1.2474557178331086e+03;
vasi.sec = 0.498;
bnb.obj = 1180.923497060671;
bnb.sec = 136.08;
our.obj = 1.2283172186935908e+03;
our.sec = 0.494;
% Relative values
zhu.obj = zhu.obj/our.obj;
zhu.sec = zhu.sec/our.sec
kamel.obj = kamel.obj/our.obj;
kamel.sec = kamel.sec/our.sec
vasi.obj = vasi.obj/our.obj;
vasi.sec = vasi.sec/our.sec
bnb.obj = bnb.obj/our.obj;
bnb.sec = bnb.sec/our.sec
@KexianShen
Copy link

When it comes to a 2D example, I believe it should be '>2' instead of '>3' for the shooting constraints at line 318.

@KexianShen
Copy link

r_box_z= 2; at line 28?

@manucalop
Copy link
Author

manucalop commented Oct 21, 2021 •
edited

@KexianShen you're totally right! Thanks! It should be >2. And I remember doing it like that for the paper. I don't know why I uploaded it wrong. Regarding r_box_z, no, since I modelled an infinite cylinder. These lines are repeated because I just commented one of the lines to check with different sizes. I must have forgotten that too. I updated the code based on your comments but I can't test them right now... Let me know if you have any trouble! :)

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment