%% A Convex Optimization Approach to Smooth Trajectories for Motion Planning with Car-Like Robots

%% Problem Formulation
% Given collision-free waypoints in a maze, our goal is to drive a robot
% along this trajectory. However, though collision-free is achieved, some 
% parts of the trajectory may be impractical and(or) inefficient for the 
% real robot to move along. Thus, we need to smooth the trajectory, in this 
% demonstration, using convex optimization.
%
%% Proposed Solution
% * *minimization problem:* imagine there is an artificial tensile force
% between points, the aim is to minimize the sum of these forces, subject 
% to friction constraints, turning radius constraints, initial orientation
% of the vehicle.
%
% <<formula.jpeg>>
% 
% * *minimization variable:* in the implementation of this algorithm, the
% optimization variables are those forces represented by the l2-norm of 
% adjacent points.
%
% <<anotation.jpeg>>
% 
% * *algorithmic workflow:* first, find the collision-free circles of
% original waypoints. Second, due to fact that the circles found have a
% great proportion of intersection, this step will sort out some necessary
% waypoints and their corresponding circles. Third, Check if there is a gap
% (no intersection) between any two adjacent points. If is, then give a new point to
% intersect both of its adjacent two points. Fourth, using convex
% optimization method to smooth the trajectory.
%% Image Initialization
clc;clear;close all;
I = imread('maze.jpg');
I2 = imbinarize(I(:,:,1), 0.8); % Convert to 0-1 image
% I3 = image_to_binary_map(I2);
I4 = flipud(I2); % To make Y-axis correspond to startpos and endpos
I5 = padarray(flipud(I2),[1,1],0);

%% Calculate Collision-free Radius of Each Point
load('P.mat'); % load original waypoints
P = flipud(P);
r=[];
for i = 1 : 1 : length(P)
    [r_temp, p_trouble, P(i,:)]= findRadius(P(i,:), I5); % find collision-free circles
    r = [r r_temp];% radius of cicles
end

%% Sort Out Necessary Points
r_shrkRate = 0.40; % shrink radius of circles by this rate 
r_pos = find(r >= (1/2) * max(r));
r(r_pos)= r_shrkRate * max(r);
[sortedPoint, sortedRadius] = sortEffectivePoint(P, r); % sort out necessary points
[P, r] = intervalCheck(sortedPoint, sortedRadius, I5);% check if there is interval

%% Initialization of Optimizaiton
d = 0;
n = length(P);
for i = 2 : 1 : n
    d = d + norm(P(i,:) - P(i-1,:), 2);
end
d = d * 1./(n-1); % average distance between two points

R_min = 0.5; % Minimum turning raidus of the vehicle
alpha_k = 3; % coefficient
v_k = [0.5 0.8]; % x and y velocities of the vehicle
friction_cstr = alpha_k * (d/ norm(v_k, 2)).^2; % friction constraints
radius_cstr = d.^2 ./ R_min; % turning radius constraints

%% Convex Optimization
cvx_begin
    variables Q(n,2) % waypoints to be obtained from optimization 
    Q(1,:) == P(1,:);
    Q(2,:) == P(1,:) + d .* 0.6;
    Q(n,:) == P(n,:);
    Q(n-1,:) == P(n,:) - d .* 0.6;
    sum = 0;
    
    for k = 2 : 1 : n-1
       norm(Q(k, :) - P(k, :), 2) <= r(k); % restrict points inside the collision-free circles
       N_k(k-1) = norm(2.* Q(k, :) - Q(k-1, :) - Q(k+1, :), 2);
       N_k(k-1) <= min(friction_cstr, radius_cstr); 
       sum = sum + pow_pos(N_k(k-1),2); % sum of "forces"
    end
       minimize sum
cvx_end

%% Plot Results
figure(1)
imshow(I5)
title('Overview of result');
hold on
axis on
set(gca,'YDir','normal');
plot(P(:,1), P(:,2),'r-*');
plot(Q(:,1), Q(:,2),'b-o');
for i = 1 : 1 : length(r)
    drawCircle( P(i,:),r(i) );
end
legend('original waypoints','optimized waypoints');

% * *inefficient original trajectory*
figure(2)
imshow(I5)
hold on
axis on
set(gca,'YDir','normal');
title('inefficient original trajectory');
plot(P(:,1), P(:,2),'r-*');
plot(Q(:,1), Q(:,2),'b-o');
legend('original waypoints','optimized waypoints');
axis([100 260 80 180]);

% * *rugged original trajectory*
figure(3)
imshow(I5)
hold on
axis on
set(gca,'YDir','normal');
title('rugged original trajectory');
plot(P(:,1), P(:,2),'r-*');
plot(Q(:,1), Q(:,2),'b-o');
legend('original waypoints','optimized waypoints');
axis([180 240 595 630]);

figure(4)
imshow('intervalCheck.jpg')
title('Gap between adjacent points');
figure(5)
imshow('intervalCheck_2.jpg')
title('New point on the gap');

%% Analysis and Conclusions
% This algorithm succeeds in reproducing the result in the original
% paper. The origianl paper has two optimization process. One of them is on
% the trajectory, which we demonstrate in this report, the other process is
% on the velocity of the vehicle. It, indeed, obtains a smoothed trajectory
% after the implementation of the algorithm but there are still some points
% to rethink.
%
% * *Ignore robot configuration:* The smoothed trajectory is based on the
% fact that the robot is moving at a constant speed and its heading
% constrains are ignored
%
% * *Check the interval:* This algorithm has a new function that is not
% contained in the thoughts of the original paper. A function inside this
% algorithm implementation can find out if there is disconnection of any
% two adjacent points and produce a point compensate for this.

%% Links and References
% [1] Zhijie Zhu, Edward Schmerling, Marco Pavone, "A Convex Optimization 
% Approach to Smooth Trajectories for Motion Planning with Car-Like Robots"

%% Sources files
% * The following custom functions are used
%
% * findRadius.m --- find the raidus of collision-free circle
%
% * drawCircle.m --- draw collision-free circle
%
% * intervalCheck.m --- check if there is gap between any adjacent points
%
% * translatePoint.m --- translate points that are so closed to the obstables
%
% * sortEffectivePoint.m --- sort out some of necessary points
