%% A Convex Optimization Approach to Smooth Trajectories for Motion Planning with Car-Like Robots
% Submitted by: Samira Ghanbarian
%
% Under the guidance of: Prof. Oleg Michailovich
%
% Course Code: ECE 602
%
% Department of Electrical and Computer Engineering
%
% Winter 2019

%% Problem Description and Formulation
% 
% The problem of planning a collision-free and dynamically feasible
% trajectory is fundamental in robotics. This mean is often accomplished 
% by running a sampling-based motion planning algorithm such as PRM or RRT.
% While these algorithms are very effective for quickly finding 
% collision-free trajectories in obstacle cluttered environments, they 
% often return jerky, unnatural paths. The end result is that the 
% trajectory returned, requires smoothing and reparametrization so that it
% can be amenable to a real-time implementation. The objective of the 
% paper which is implemented and reproduced in this project is to design a 
% simple heuristic algorithm which turns a jerky trajectory into a smooth 
% one that fulfills strict constraints of the robot.
% 

%% Proposed Solution
% 
% # CES Algorithm
% # Optimization Formulation
%
% *CES Algorithm*
%
% Given a constant velocity profile and initial reference waypoints, the
% proposed solution to optimize the shape of the trajectory is CES (Convex
% Elastic Smoothing) algorithm. In this method, a sequence of "bubbles" is
% placed along the reference trajectory in order to identify a region of
% the workspace that is collision free. Such a region can be thought of as
% a collision-free "tube" within which the reference trajectory imagined as
% an elastic band, can be stretched so as to obtain a smoother trajectory.
% An example of the bubble generation and elastic stretching is shown in
% the figure below.  
% 
%%
figure()
imshow('ex1.jpg')
%%
%
% *Optimization Formulation*
%
% We assume an artificial tensile force $F_{k}$ between the adjacent
% waypoints. Then at each waypoint $Q_{k}$ we can define a balance force
% $N_{k}$ equal to sum of tensile forces. A simple schematic is shown in
% the following figure.     
%%
figure()
imshow('ex2.jpg')
%%
%%
% $$F_{k}:=Q_{k+1}-Q_{k}$$
% 
% $$N_{k}:=F_{k-1}-F_{k}$$
%
% The balance forces $N_{k}$ capture the curvature information along the
% trajectory. To smooth a trajectory, the goal is then to place new
% waypoints $Q_{k}$ in the generated bubbles so as to minimize the sum of
% the norm of balance forces subject to the constraints of the vehicle's
% dynamics. The elastic stretching optimization problem is as follows. 
% 
% $$\min_{Q_{3},...,Q_{n-2}} \sum_{k=2}^{n-1} ||2Q_{k}-Q_{k-1}-Q_{k+1}||^2 $$
% 
% s.t.
% 
% $Q_{1}=P_{1} , Q_{2}=P_{1}+d.\frac{v_{1}}{||v_{1}||}$
% 
% $Q_{n}=P_{n} , Q_{n-1}=P_{n}-d.\frac{v_{n-1}}{||v_{n-1}||}$
%
% $Q_{k} \in B_{k}$, $k=3,..,n-1$
% 
% $||2Q_{k}-Q_{k-1}-Q_{k+1}|| \leq min(\frac{d^2}{R_{min}}
% ,\alpha_{k}.(\frac{d}{v_{k}})^2 )$, $k=2,..,n-1$
%
% where:
%
% $P_{k}$  : Initial Reference Trajectory WayPoints
%
% $Q_{k}$  : Decision Variables, New WayPoints
%
% $d$      : Distance Between Reference Points Assuming Uniform
% Distribution
%
% $v_{k}$  : Velocity Profile
%
% $B_{k}$  : Generated Bubble associated with $P_{k}$
%
% $R_{min}$: The Minimum Turning Radius (Physical Parameter)
%
% $\alpha_{k}$: Maximum Acceleration
%
% This problem is a QCQP convex optimization problem with the new
% intermediate waypoints as the decision variables. First constraint, fixes
% the start position and start angle and similarly, the second constraint,
% fixes the end position and end angle. Third constraint chooses the
% decision variables to be in the generated bubbles so that there is no
% collision with the obstacles. The last two constraints are about the
% dynamic feasibility of the robot.    
%
% Any feasible solution to this problem as a sequence of discrete waypoints
% represents a continuous time trajectory satisfying the constraints.

%Initialization
close all;
clear all;
clc;

%Robot parameters
v = [3.5 3.5]; 
a = 5; 
R_min = 0.3; 

%Map reading
map1 = imread('map.jpg');       
map2 = imbinarize(map1(:,:,1));
map = padarray(flipud(map2),[1,1],0);

%Assigning reference waypoints
ref = 40.53846153846154 * [2.0000 1.0000; 2.4822 0.4653; 2.6791 3.5829;
                           4.6539 7.6582; 7.5974 8.3888; 11.0693 9.4455;
                           11.0000 10.0000];
ref = Addfoodpoints(ref',3)'; 
ref = floor(ref);

%Finding the distance between reference waypoints (assuming uniform
%distribution)
d = 0;
n = length(ref);
for i = 2 : 1 : n
    d = d + norm(ref(i,:) - ref(i-1,:), 2);
end
d = d /(n-1);

%Generating bubbles for each reference point
radius = zeros(1,n);
center = zeros(2,n);
for i = 1 : 1 : n
    [r, c]= Bubblegeneration(ref(i,:), map);
    radius(i) = r;
    center(:,i) = c;
end

%Optimizaiton

cvx_begin

    variables Q(n,2)
    cost = 0;
        for i = 2 : n-1
            N(i) = norm(2*Q(i,:) - Q(i-1,:) - Q(i+1,:), 2);
            cost = cost + pow_pos(N(i),2);
        end
    minimize cost
    subject to
    Q(1,:) == ref(1,:);
    Q(2,:) == ref(1,:) + d*v/norm(v,2);
    Q(n,:) == ref(n,:);
    Q(n-1,:) == ref(n,:) - d*v/norm(v,2);
    for j = 3 : n-1
        norm(Q(j,:) - ref(j,:), 2) <= radius(j);  
    end
    for j = 2 : n-1
     N(j) <= min(d^2/R_min , a*(d/norm(v,2))^2); 
    end
cvx_end

%% Numerical Examples
% 
% Below figure shows the result of the implementation of CES algorithm on
% a unicycle robot. The work space is the SimpleMap generated by MATLAB
% examples [2]. The initial reference trajectory is computed by running the
% PRM algorithm in the work space [3]. Robot parameters are as $v=[3.5,3.5]$
% , $\alpha=5$ and $R_{min}=0.3$ [4].  It is obvious, the trajectory has
% been smoothed around the sharp turns. 
% 

figure(1)
imshow(map)
hold on
plot(ref(:,1), ref(:,2),'-sb');
plot(Q(:,1), Q(:,2),'-xg');
legend('Reference Path','Smoothed Path');
for i=1:n
theta = 0 : 0.01 : 2 * pi;
x = center(1,i) + radius(i) * cos(theta);
y = center(2,i) + radius(i) * sin(theta);
circle = [x; y]';          
plot(circle(:, 1), circle(:, 2),'-r');
end
axis on
set(gca,'YDir','normal');
xlabel('X');
ylabel('Y');
title('Implementatio of CES Algorithm');

%% Conclusion
% 
% The above numerical experiment shows success in reproducing the results
% of the original paper. Three major trends can be derived from the above
% results. First, CES algorithm appears robust with respect to the model
% used to generate the initial reference trajectory. (This claim is proved
% by using the data for reference trajectory from [3] which has been
% computed with a different algorithm in comparison to the original paper).
% This is a fundamental property which allows the use of this algorithm
% even with more sophisticated robot models. Second, the smoothed
% trajectory often results in a noticeable length and traversal time
% reduction and in general a sequence of new waypoints that may be easier
% to track for simpler controllers. Third, the reduced time makes this
% algorithm more applicable to the real-case scenarios.
% 
%% References
%
% [1] Zh. Zhu, E. Schmerling and M. Pavone, "A Convex Optimization 
% Approach to Smooth Trajectories for Motion Planning with Car-Like
% Robots," 54th IEEE Conference on Decision and Control (CDC), Osaka,
% Japan, 2015.
%
% [2] map = ExampleHelperRobotSimulator('simpleMap',2)
%
% [3] D. Agarwal, P.S. Bharti, "A Review on Comparative Analysis of Path
% Planning and Collision Avoidance Algorithms," International Journal of
% Mechanical and Mechatronics Engineering, Vol.12, No.6, 2018.
%
% [4] M. Missura, S. Behnke, "Efficient Kinodynamic Trajectory Generation
% for Wheeled Robots," IEEE International Conference on Robotics and
% Automation, Shanghai, China, 2011.
%
%% Appendix A: List of functions
%
% List of functions used in this project is as follows.
%
% Bubblegeneration.m: This function generates biggest collision-free
% bubble which is centered at the waypoint. If such bubble cannot be found,
% then the center is moved in small steps until such a bubble is found.
%
% Addfoodpoints.m: For the parts of path that is linear, only the start and
% end of the line is required; however, this increases the distance between
% waypoints considerably and can cause infeasibility. This function uses
% linear interpolation to avoid such drawbacks.