%% Hold Or take Optimal Plan
% By Tristan Walker
% 


%% Problem
% This paper addresses the labeled path planning problem, where a team of n
% robots each must navigate to a pre-assigned goal location. This paper
% specifically proposes a method to plan safe (collision free) and
% dynamically feasible paths. 

%% Proposed Solution
% To solve this problem the authors present a 2 part algorithm. 
% 
% <<HOOPAlgo.JPG>>
% 
% The Plan Motions step creates a collision-free linear-piecewise 
% motion plan for each robot, and the Generate Trajectories takes these
% motion plans and generates smooth, dynamically feasible trajectories. The
% key to the Plan Motions algorithm is the introduction of circular holding
% patterns that the robots enter to avoid collisions, called CHOPs. The
% algorithm follows these basic steps:
% 
% # Plan straight paths from starts to goals
% # Check for collisions
% # Create CHOP for colliding robots
% # repeat checking for collisions and generating new CHOPs until a motion
% plan is generated that satisfies all requirements
% 
% <<CHOPFig.JPG>>
% 
% To determine where in the CHOP each robot enters, the authors solve the
% following minimization:
%
% <<CAPT.JPG>>
%
% This problem is solved in O(n^3) time by the Hungarian Algorithm (Kuhn,
% 1995).
% While in CHOPs, robots have priorities assigned to prevent them from
% exiting too early from the CHOP if it will cause them to block another
% robot
% 
% <<CHOPPriority.JPG>>
% 
% The following code creates a motion plan to navigate a CHOP from a given
% set of robots:

%%
%
%     function h = findCHOP(tau,robots,s,g,R,vmax)
%     Nm = length(robots);
%     %find CHOP center
%     xh = [sum(s(:,1)), sum(s(:,2))]/Nm;
%     %find number of intermediate waypoints
%     Nh = 2*Nm;
%     delta_theta = 2*pi/Nh;
%     theta = zeros(length(Nh),1);
%     for i = 1:length(theta)
%        theta(i) = (i-1)*delta_theta; 
%     end
%     min_radius = sqrt(2)*R/sin(delta_theta/2);
%     collision_radius = zeros(length(g),2);
%     for i = 1:length(g)
%         dist = sqrt((xh(1)-g(i,1))^2 + (xh(2)-g(i,2))^2);
%         collision_radius(i,:) = [dist - sqrt(2)*R, dist+sqrt(2)*R];
%     end
%     Rh = max(collision_radius, 'all');
%     for i = 1:length(collision_radius)
%         temp = collision_radius(i,1);
%         for j = 1:length(collision_radius)
%            if( temp > collision_radius(i,1) && temp < collision_radius(i,2))
%                temp = Rh;
%            end
%         end
%         if( temp < Rh )
%             Rh = temp;
%         end
%         temp = collision_radius(i,2);
%         for j = 1:length(collision_radius)
%            if( temp > collision_radius(i,1) && temp < collision_radius(i,2))
%                temp = Rh;
%            end
%         end
%         if( temp < Rh )
%             Rh = temp;
%         end
%     end
%     temp = min_radius;
%     for j = 1:length(collision_radius)
%        if( temp > collision_radius(i,1) && temp < collision_radius(i,2))
%            temp = Rh;
%        end
%     end
%     if( temp < Rh )
%         Rh = temp;
%     end
%     xw = zeros(length(theta),2);
%     for i = 1:length(Rh)
%         xw(i,:) = [xh(i,1)+Rh*cos(theta(i)), xh(i,2)+Rh*sin(theta(i))];
%     end
%     %identify entry waypoints
%     Xw = zeros(ceil(length(xw)/2),2);
%     for i = 1:length(Xw)
%         Xw(i,:) = xw(2*i-1,:);
%     end
%     phi_g = zeros(length(g),2);
%     for i = 1:length(g)
%         min_dist = sqrt((xw(1,1)-g(i,1))^2 + (xw(1,2)-g(i,2))^2);
%         min_idx = 1;
%         for j = 2:length(xw)
%             dist = sqrt((xw(j,1)-g(i,1))^2 + (xw(j,2)-g(i,2))^2);
%             if dist < min_dist
%                min_dist = dist;
%                min_idx = j;
%             end
%         end
%         phi_g(i,:) = xw(min_idx,:);
%     end
%     P = cell(length(phi_g),1);
%     for i = 1:length(P)
%         for j = 1:length(P)
%             if i~=j
%                 angle = atan2(g(j,2)-phi_g(j,2),g(j,1)-phi_g(j,1));
%                 xp = [phi_g(j,1)+R*cos(angle+pi/2), g(j,1)+R*cos(angle+pi/2), g(j,1)-R*cos(angle-pi/2), phi_g(j,1)-R*cos(angle-pi/2), phi_g(j,1)+R*cos(angle+pi/2)];
%                 yp = [phi_g(j,2)+R*sin(angle+pi/2), g(j,2)+R*sin(angle+pi/2), g(j,2)-R*sin(angle-pi/2), phi_g(j,2)-R*sin(angle-pi/2), phi_g(j,2)+R*cos(angle+pi/2)];
%                 if(inpolygon(g(i,1),g(i,2),xp,yp))
%                     P(i) = {[P(i); j]};
%                 end
%             end
%         end
%     end
%     [Mh,taufh] = findCHOPMotionPlan(taush,Rh,sh,xw,Nh,Xw,phi_g,P,gh,vmax);
%     h={taush,Rh,sh,gh,xh,Nh,Rh,taufh,Mh};
%     end

%%
% The primary guarantee of the Plan Motions algorithm is that it is safe.

%% Optimization Problem
% To generate feasible trajectories, the authors use a QP to minimize the
% following cost function to minimize control input:
%
% <<Cost.JPG>>
%
% Subject to
%
% <<Constraints.JPG>>
%
% To formulate this as a QP, the authors choose the following decision
% vector, composed of piecewise polynomial coefficients for the x and y
% components of each robot's motion plan.
%
% <<Coefficients.JPG>>
%
% With this selection, the cost function can be represented as:
%
% <<QP.JPG>>
%
% The equality constraints (Waypoint and Continuity constraints) can then
% be represented as linear with respect to c:
%
% <<Equality.JPG>>
%
% The following code derives these equality constraints from a motion plan:
%%
% 
%     degree = 2*n-1;
%     pieces = length(M{i,1});
%     c_length = (degree+1)*pieces;
%     Tg = M{i,1}(end);
%     Aeq = zeros(1,c_length);
%     Aeq(1) = 1;
%     beq = M{i,2}(1);
%     newrow = zeros(1,c_length);
%     for k = 0:degree
%         newrow(c_length-degree+k) = Tg^k;
%     end
%     Aeq = [Aeq; newrow];
%     beq = [beq; M{i,2}(end)];
%     for k = 1:n-1
%        newrow = zeros(1,c_length);
%        for n = 0:degree
%            newrow(n+1) = subs(diff(t^n,t,k),t,0);
%        end
%        Aeq = [Aeq; newrow];
%        beq = [beq; 0 0];
%     end
%     for k = 1:n-1
%        newrow = zeros(1,c_length);
%        for n = 0:degree
%            newrow(c_length - degree - 1 + n+1) = subs(diff(t^n,t,k),t,Tg);
%        end
%        Aeq = [Aeq; newrow];
%        beq = [beq; 0 0];
%     end
%     for s = 0:pieces-1
%         for k = 1:n-1%continuity constraints
%             newrow = zeros(1,c_length);
%             for n = 0:degree
%                 newrow(s*degree+j+1) = subs(diff(t^n,t,k),t,M{i,1}(s+1));
%                 newrow((s+1)*degree+j+1) = -subs(diff(t^n,t,k),t,M{i,1}(s+1));
%             end
%             Aeq = [Aeq; newrow];
%             beq = [0 0];
%         end
%     end
% 
% The authors propose 2 different ways to formulate trajectories. The first
% is a nominal method where they introduce additional constraints forcing
% the trajectories to stop in between each segment of the piecewise
% trajectory.
%
% <<Boundaries.JPG>>
%
% The second method formulates smooth trajectories with continuous motion.
% To achieve this, at the start of each segment of the motion plan, they
% introduce hyperplanes between the robot and every other robot to form a
% convex region to restrict the robot to for the next segment. This
% half-plane restriction can be written as an inequality as follows:
% 
% <<Hyperplane.JPG>>
%
% <<HyperplaneFig.JPG>>
%
% The authors then use results from previous works to constrain the entire
% trajectory segment within the half-plane by representing the polynomial
% trajectory segment as a Bezier curve:
% 
% <<HyperplanePoly.JPG>>
%
% <<Bezier.JPG>>
%
% By then applying the linear relationship between the control points p of
% the Bezier Curve and the basis coefficients c, these half-plane
% restrictions can be represented by a set of linear inequality
% constraints:
%
% <<Inequality.JPG>>
%
% Thus, we can formulate the control problem as the following QP:
%
% <<Min.JPG>>
%
% subject to
%
% <<QPConstraints.JPG>>
%
%% Results
% Unfortunately due to the complexity of the paper and the proposed 
% solution I was not able to create a working implementation of the
% algorithm.
%
%% Conclusions
% The authors guarantee that this algorithm is safe and complete. However,
% they cannot guarantee optimality and in fact state that the generated
% trajectories are suboptimal if any CHOPs are used. Thus, the
% suboptimality of this algorithm lies in the motion planning step, and not
% the QP in the trajectory generation step. Additionally, the authors claim
% that the suboptimality is related to the problem density, defined as a
% ratio between the square of the number of robots and the area of the
% workspace. The worst case occurs when every robot must enter a holding
% pattern. Each robot can only enter at most 1 holding pattern. 

%% References
% 
% # S. Tang, J. Thomas, and V. Kumar, “Hold or take optimal plan (HOOP): A quadratic programming approach to multi-robot trajectory generation,” Int. J. Robot. Res., p. 0278364917741532, 2018
% # Kuhn H (1955) The hungarian method for the assignment problem. Naval Research Logistics.
% # Hungarian Algorithm code from https://www.mathworks.com/matlabcentral/fileexchange/20652-hungarian-algorithm-for-linear-assignment-problems-v2-3
% 






