%% ECE 602 FINAL PROJECT - Convex Optimization Strategies for Coordinating Large-Scale Robot Formations
% Pranav Ganti (20663913), Leonid Koppel (20433722)
%% Introduction
%
% Multi-robot teams are becoming increasingly viable as a solution in a
% variety of different fields, due to advances in wireless communications
% and embedded computing. Potential applications include surveillance,
% search and rescue, or military operations. Dr. Jason Derenick and Dr.
% John Spletzer, in their paper "Convex Optimization Strategies for
% Coordinating Large-Scale Robot Formations" [1], investigate the use of convex
% optimization strategies to coordinate these multi-agent teams. This
% problem can be formulated as a a Second-Order Cone Program (SOCP), and
% either the maximum distance or total distance travelled by these robots
% between formations can be minimized. In this project, we look to use CVX
% to solve these SOCPs, and implement the extensions proposed by the
% authors, such as additional workspace constraints and path-planning in a
% polygonal environment.

%% Problem Formulation
% Consider $m$ robots in a plane with initial pose
% $P \in \mathbf R^{m\times 2} $
% and a target formation $S \in \mathbf R^{m\times 2}$. We want the robots to
% move to a final pose $Q$ which is similar to $S$ - that is, having the same shape 
% up to translation, rotation, and scale. This similarity is denoted by $Q \sim S$.
% We constrain scale to be positive, disallowing reflection.
%
% The two related problems are as follows: given an initial pose $P$, choose $Q\sim S$ to
% either
%
% minimize $$ \max_{i=1,\ldots,m} ||q_i - p_i||_2 $$, the maximum distance travelled; or
%
% minimize $$ \sum_{i=1}^m ||q_i - p_i||_2 $$, the total distance travelled 
%
% where $p_i = (p_i^x, p_i^y)$ and $q_i = (q_i^x, q_i^y),\quad i=1,\ldots,m$ 
% represent the coordinates of each point in the initial and final pose.
% The desired shape is represented by $s_i = (s_i^x, s_i^y)$.
%
% Note that we do not consider the _assignment problem_, the problem of
% choosing which robot corresponds to which $s_i$ in the target shape. We
% only find the optimal translation, orientation, and scale of the shape given
% a fixed correspondence. This problem is convex, while the assignment
% problem is not.
%
% Additionally, we reproduce two extensions demonstrated in the original
% paper: adding polygonal workplace constraints, and motion planning
% through an environment defined as a series of polygons. These problems are
% formulated in later sections.
%
%% Proposed solution
%
% Derenick and Spletzer start with the constraints on the final pose,
% given the desired shape. The constraints are, for $i=2,\ldots,m$:
%
% $$
% \begin{array}{cl}
% q_i^x - q_1^x &= \alpha(s_i^x \cos\theta - s_i^y \sin\theta) \\
% q_i^y - q_1^y &= \alpha(s_i^x \sin\theta + s_i^y \cos\theta)
% \end{array}
% $$
% 
% These constraints simply express an affine transformation of the
% target shape. We can thus express them as affine functions.
% 
% Here there appears to be an omission in Derenick and Spletzer's
% paper. They specify $s_1 := \mathbf 0$, and state, "without loss of
% generality, we can define the formation orientation and scale,
% repectively, as"
% 
% $$
% \theta = \arctan \frac{q_2^y - q_1^y}{q_2^x - q_1^x},\quad
% \alpha =  \frac{||q_2 - q_1||}{||s_2||_2}
% $$
%
% However, the above expression for $\theta$ is only valid if we also define
% $s_2^y = 0$. That is, the target shape must be expressed such that the
% first two points lie on the horizontal axis. Otherwise, it is
% impossible for $\theta$, which obviously expresses a relationship between $S$ and $Q$,
% to be independent of $S$.
% We obtained incorrect results until we added this requirement, which is
% demonstrated in the Analysis section.
%
% With the requirement $s_2^y = 0$ in place, the affine form of the
% constraints is found to be
%
% $$
% \begin{array}{cl}
% ||s_2||(q_i^x - q_1^x) &= (s_i^x, -s_i^y)(q_2 - q_1) \\
% ||s_2||(q_i^y - q_1^y) &= (s_i^y, s_i^x)(q_2 - q_1)
% \end{array}
% $$
%
% for $i=3,\ldots,m$. Note that there are $2m-4$ constraints, leaving four free variables:
% the 2D translation, rotation, and scale of the target shape.
%
% The constraints can be expressed in the matrix form $Aq = 0$. Then the
% problems are
% 
% $$
% \begin{array}{llll}
% \min_q &\max_{i=1,\ldots,m} ||q_i - p_i||_2 \quad \textnormal{or} \quad 
% &\min_q &\sum_{i=1}^m ||q_i - p_i||_2 \\
% \textnormal{s.t. } &Aq = 0 & \textnormal{s.t. } &Aq = 0
% \end{array}
% $$
% 
% Using the epigraph form, the problems can be restated as second-order cone programs:
%
% $$
% \begin{array}{llll}
% \min_q &t \qquad\qquad \textnormal{or} \quad 
% &\min_q &\mathbf 1^T s \\
% \textnormal{s.t. } &y_i \leq t & \textnormal{s.t. } &y_i \leq s \\
% & Aq = 0 && Aq = 0
% \end{array}
% $$
%
% where $y_i = ||q_i - p_i||_2$, $t \in \mathbf R$, $s \in \mathbf R^m$.
%
% In this report, we demonstrate only the second problem, minimizing total
% distance. This matches the demonstrations in the original paper.
%
%

%% Data synthesis
% For our reproduction, we used a shape repesenting the letters "UW". We
% generate points for any $m$ by interpolating along a series of curves
% representing the letters, as implemented in <../UW_points.m UW_points.m>. 


m = 100;
S = UW_points(m);

%%
% We produce the initial pose by arbitrarily transforming the
% shape, and adding random noise to each point. We do this because, as
% noted above, the assignment problem is not addressed; this way, we start
% with a somewhat sensible assignment where each robot's initial point $p_i$
% is correlated to its target point. This appears to be what
% Derenick and Spletzer did in their paper, as choosing completely random $p_i$ does
% not produce such pleasing results.

p_theta = -pi/2 + 0.2;
p_scale = 1;
R = [cos(p_theta) sin(p_theta); -sin(p_theta) cos(p_theta)];
p_trans = [5 10];
P = p_scale * S*R + p_trans; % Transform
P = P + 6*rand(m,2); % Add uniform random noise

%%
% An example is presented below:

figure(1); clf;
plot_shape;

%% Solution
%
% To implement the constraints in matrix form $Aq = 0$, we let
% $q \in \mathbf R^{2m}$ be the "flattened" vector
% form of $Q$, where odd elements represent $x$-values
% and even elements represent $y$-values. Then
% $A \in \mathbf R^{2(m-2)\times 2m}$.
% The matrix $A$ is generated in <../constraint_matrix.m constraint_matrix.m>

A = constraint_matrix(S);

%%
% Here we formulate the problem to minimize total distance:

cvx_begin quiet
variable q(2*m) % vector form of q: odd elements x, even elements y
Q = reshape(q, 2, m)'; % matrix form, each row has (x, y) point
y = norms(Q-P, 2, 2);  % y_i = || q_i - p_i ||_2
variable t(m)   % auxiliary variable for epigraph form

minimize ones(1,m) * t
subject to
    y <= t
    A*q == 0
cvx_end


%% Results
%
% The figure below shows the initial pose (filled circles) and the final pose
% (empty black circles) which forms the target shape while minimizing
% total distance travelled, represented by line segments.

figure(2); clf;
plot_robots(P, Q);

%% Integrating Workspace Constraints
%
% An additional constraint that can be implemented is that of a "workspace
% constraint". A convex shape, such as a polyhedron, can be integrated as a
% constraint for the final configuration to lie within. The workspace C can
% be expressed as:
%
% $C = \{x \in \mathbf{R}^2: Cx \leq d\}$
%
% For the purpose of demonstration, let us specify an arbitrary 5-sided polyhedron
% which lies within the original operating environment.
%
% The 5 equations which specify the polyhedra are as follows:
%
% $-x_2 \leq -6$
%
% $x_2 \leq 18$
%
% $x_1 \leq 20$
%
% $-x_1 \leq -8$
%
% $2/3x_1 -x_2 \leq 10/3$
%
% This would be placed into the following matrix inequality:
%
% $A_{c}x \leq b_c$, with:

A_c = [0 -1;
       0  1;
       1  0;
      -1 0;
       2/3 -1];

b_c = [-6, 18, 20, -8, 10/3]';



%%
% Once again, we formulate the problem to minimize total distance:

cvx_begin quiet
variable q(2*m) % vector form of q: odd elements x, even elements y
Q = reshape(q, 2, m)'; % matrix form, each row has (x, y) point
y = norms(Q-P, 2, 2);  % y_i = || q_i - p_i ||_2
variable t(m)   % auxiliary variable for epigraph form

minimize ones(1,m) * t
subject to
    y <= t
    A*q == 0

    % The following inequalities constrain the workspace to the polyhedron
    % defined above. 
    for i = 1:m
        A_c*Q(i, :)' <= b_c;
    end
cvx_end

%%
% The figure below shows the final pose minimizing
% total distance travelled, constrained to the arbitrary workspace (black lines).
% The initial pose is the same as before.

figure(3); clf;
plot_robots(P, Q);
draw_polyhedron;
%%
% We can see here now that the optimal robot locations are constrained
% within the polyhedron.

%% Notes on Complexity
%
% One of Derenick and Spletzer's contributions to this paper was the
% formulation of this problem in a manner that can be solved efficiently,
% even as the number of robots increases past 1000. 
%
% To do this, however, they first look to reformulate the problem as a
% inverse log-barrier problem. We can recall from Boyd that the log-barrier
% method is an interior-point method, which works to solve equality
% constraints using Newton's method. A barrier method can be formulated as:
%
% $$
% \begin{array}{lll}
% \min &f_{o}(x) + \sum_{i=1}^m I_{\_}(f_{i}(x)) \\
% \textnormal{s.t. } &Ax = b 
% \end{array}
% $$
%
% where $I_{\_}$ represents the indicator function for the inequality
% constraint. For the log-barrier method, however, the indicator function
% is replaced with the log function, which acts as a sufficient 
% approximation. As per Boyd, this implicitly defines the inequality 
% constraints, with only the equality constraints explicitly stated.
% 
% As seen above, the minimum total distance problem had been reformulated
% in epigraph form. This has been illustrated by Derenick and Spletzer as follows:
%
% $$
% \begin{array}{lll}
% \min_{q,t_{1}} &\tau_{k}t_{1} - \sum_{i = 1}^m \log(t_{1}^2 - (q_{i} -
% p_{i})^T(q_{i} - p_{i}))\\
% \textnormal{s.t. } &Aq = 0 
% \end{array}
% $$
%
% where $A$ and $q$ have been previously defined. Derenick and Spletzer define
% the variable $\tau_{k}$ as the "inverse log-barrier scaler for the
% kth iteration". This has been defined as:
%
% $$
% \tau_{k + 1} = \mu^k\tau_{k},\quad \mu > 1
% $$
%
% We believe that this is a typo in the paper; $\tau$ should be
% defined as either $\tau_{k+1} = \mu^k\tau_{1}$, or  $\tau_{k+1} = \mu\tau_{k}$.
%
% This formulation alone, however, is not sufficient for scalability.
% In order to maintain O(m) complexity with an increasing number of robots,
% Derenick and Spletzer reformulate the Karush-Kuhn-Tucker system as a 
% monobanded system; the full derivation for this is discussed within the
% paper. The final set of constraints are then defined as:
%
% $$
% \begin{array}{lll}
% \min_{q,t} &\frac{\tau_{k}}{m}\sum_{i=1}^mt_{i} - \sum_{i = 1}^m \log(t_{1}^2 - (q_{i} -
% p_{i})^T(q_{i} - p_{i}))\\
% \textnormal{s.t. } &||s_{2}||_{2}(q_{l}^x - d_{j}^x) = [s_{l}^x,
% -s_{l}^y](d_{j + 1} - d_{j})\\
% \quad &||s_{2}||_{2}(q_{l}^y - d_{j}^y) = [s_{l}^y, s_{l}^x](d_{j+1} -
% d_{j})\\
% \quad &t_{i + 1} = t_{i}, \quad i = 1, \ldots, m-1\\
% \quad &d_{2i + 1} = d_{2i-1}, \quad i = 1, \ldots, m-3\\
% \quad &d_{2(i+1)} = d_{2i}, \quad i = 1, \ldots, m-3\\
% \quad &d_{i} = q_{i},\: i \in {1, 2}
% \end{array}
% $$
%
% for $l = 3, \ldots, m$ and $j = 2(i -3) + 1$.
%
% This formulation of the problem is not solvable using CVX. The
% objective function actually violates the Disciplined Convex Programming
% (DCP) ruleset, as the log function consists of two convex terms being
% summed together; CVX deems this an illegal operation. It would be possible
% to write a custom solver, but since this section 
% merely describes an optimized method of solving the same problem, we feel
% that it is outside the scope of the ECE 602 Final project.


%% Motion planning in polygonal environments
% Derenick and Spletzer extend the above problem by considering a robot
% formation moving through a polygonal environment.
% We assume we already have a map of the environment decomposed into
% triangles, such that any two consecutive triangles form a convex region,
% and that a path through $k$ triangles has been found by some high-level
% planner. We seek to find a path for each robot which minimizes the total
% distance travelled while maintaining formation and not colliding with the
% environment.
% 
% The constraints for each triangle are expressed as three halfspace
% inequalities of the form $c^T x \leq d$.
%
% We generate a randomized environment with 16 triangles and the
% corresponding constraints in the functions below.

k = 16; % number of triangles
triangles = generate_env(k);
[C,d] = triangle_constraints(triangles);


%%
% There is a slight hitch: in this problem, the optimal path tends toward $\alpha = 0$, thus 
% a minimum scale constraint is desired. Unfortunately, while _maximum_ scale
% constraints are convex, a _minimum_ scale constraint of the form
% 
% $$ \alpha \geq \alpha_{min} $$
% 
% is not in convex (recall $\alpha = ||q_2 - q_1||/||s_2||$,
% thus its sublevel sets but not superlevel sets are convex).
% 
% Spletzer's earlier work [2] includes an approximation scheme to allow a minimum scale
% constraint, but also notes that a scale constraint _is_ convex if
% orientation is fixed.
%
% For this demonstration we use the latter approach, and set a fixed
% orientation, which appears to be what Derenick and Spletzer used for
% their demonstration. Following their example, we use a circular formation of
% robots, generating the points in <../circle_points.m circle_points.m>.
% We place the initial pose $P$ inside the first triangle.
%

m = 12; % number of robots
S = circle_points(m);
P = 3*S - [5.5 1];

%%
% We formulate the problem as
% 
% $$
% \begin{array}{lll}
% \min_q &\sum_{i=1}^k \mathbf 1^T t_i \\
% \textnormal{s.t. } &|| q_{ij} - q_{i-1,j}||_2 \leq t_{ij}, 
% \quad & i = 1,\ldots,k,\quad j = 1,\ldots,m \\
% &q_{ij} - q_{i,1} = \alpha s_{i}, 
% \quad & i = 1,\ldots,k,\quad j = 2,\ldots,m \\
% &C_{i} q_{ij} \leq d_i,
% \quad & i = 1,\ldots,k,\quad j = 1,\ldots,m \\
% &\alpha \geq \alpha_{min}
% \end{array}
% $$
%
% where $t_i \in \mathbf R^m, i = 1,\ldots,k$. This is expressed in CVX
% below.
%

min_scale = 2;
PrevQ = P;

cvx_begin quiet
variable q(2*m, k-1) % vector form of q, for each triangle 1..k
variable scale nonnegative
Q = reshape(q, 2, m, k-1); % matrix form, with 3rd dimension for each triangle
Q = permute(Q, [2 1 3]);
variable t(m, k-1)   % auxiliary variable for epigraph form

minimize sum(t(:))
subject to
    for i = 1:k-1
        % Minimize total distance
        norms(Q(:,:,i) - PrevQ, 2, 2) <= t(:, i);
        % Constrain shape with fixed orientation
        for j = 2:m
            Q(j,:,i) - Q(1,:,i) == scale * S(j,:)
        end
        % Constraint to inside triangle
        for j = 1:m
             C(:,:,i+1) * Q(j,:,i)' <= d(:,:,i+1)
        end
        % Constraint minimum scale
        scale >= min_scale
        
        PrevQ = Q(:,:,i);
    end
cvx_end

%%  Results
% The below figure presents the polygonal environment (black dashed lines)
% and the optimal sequential path which minimizes total distance travelled
% while avoiding collisions.
% Orientation and minimum scale of the robot formation are constrained.
%

figure(4); clf;
plot_triangles(triangles);
plot_robot_path(P, Q);

%% Analysis
%
% We were able to recreate the results obtained by the
% authors. However, we identified an unstated assumption in the paper.
% This section will discuss that finding in detail, and then discuss the 
% applications and limitations of the work in general.
%
% *Limitation on $\theta$*
%
% The main objective of this paper was to formulate
%  swarm robotic motion as a convex optimization problem, where 
% the swarm of robots maintained a "shape", the "geometric information
% that remains when location, scale, and rotational effects are
% removed". Derenick and Spletzer's definition for the 
% formation orientation, $\theta$, is incorrect without the additional 
% choice $s^y_2 = 0$.
%
% Shown below is the simplest example illustrating this shortcoming. Let us
% construct a triangle, with vertices at (0,0), (0, 1), and (1,0). The
% initial location of the robots are directly at the vertices of the shape.
% In theory, the robots should _already_ be at their optimal location, and
% no motion should be required. 

% Define triangle shape
sx = [0, 0, 1];
sy = [0, 1, 0];
S = [sx; sy]';
 
% Define number of points
m = 3;

% Define initial position to be exactly the location of the shape
px = sx;
py = sy;
P = [px; py]';

% Define constraint matrix
A = constraint_matrix(S);

%%
% Here we formulate the problem to minimize total distance:

cvx_begin quiet
variable q(2*m) % vector form of q: odd elements x, even elements y
Q = reshape(q, 2, m)'; % matrix form, each row has (x, y) point
y = norms(Q-P, 2, 2);  % y_i = || q_i - p_i ||_2
variable t(m)   % auxiliary variable for epigraph form

minimize ones(1,m) * t
subject to
    y <= t
    A*q == 0
cvx_end

figure(5); 
clf; hold on;
plot_triangle_example(Q, P, S);

%%
% Clearly, this result is incorrect. The expected result is that the final pose
% (blue stars) and initial pose (red circles) are identical.
% For comparison, if the vertices in the shape are 
% defined as:
sx = [0, 1, 0];
sy = [0, 0, 1];
S = [sx; sy]';

%%
% That is, the 2nd point is (1,0) instead of (0,1),
% we see that the solution is correctly determined. 

figure(6); 
openfig('expected_triangle_example.fig');
%%
% This demonstrates that the requirement $s^y_2 = 0$ is important.
% Note this choice can still be made without loss of generality; it just requires
% that the representation of the target shape ,$s$, be correctly rotated when the problem
% is formulated.
%
% *Limitations in our reproduction* 
%
% We only demonstrated the solution for the "total distance" problem, though
% the paper also formulates the "minimax" distance problem. However, 
% only the former problem was demonstrated in the figures in the paper.
% Since the solutions are very similar, our implementation could easily be 
% changed to solve the minimax problem.
% 
% We did not reproduce the optimization discussed in the "Notes on Complexity"
% section. However, we found that our solution for minimum total distance,
% as implemented in CVX, took approximately 0.15 seconds of CPU time for
% $m=1000$. This matches the authors' result for their optimized algorithm. 
% This suggests that a decade's worth of CPU advances can make up for a less efficient
% implementation. (Of course, an opmimized algorithm would still be
% faster, especially for greater $m$).
%
% *Applications and limitations*
%
% The optimization methods demonstrated can be useful for controlling a
% swarm of robots, but have several shortcomings. These shortcomings mostly
% stem from considering only convex optimization problems.
%
% First, Derenick and Spletzer's method does not address the assignment problem.
% For example, if the initial pose is a
% uniformly random distribution of robots, we obtain a result similar to
% one below:

figure(7);
openfig('assignment_problem.fig');

%%
% That is, when the assignmentof the robots is completely random,
% the distance-mininimizing pose tends toward a point in the centre of the initial pose.
% In a real-world situation, this result is unlikely to be useful, and a
% better assignment of robots is desired. A suboptimal, approximate assignment may suffice;
% alternatively, the methods in this paper may be useful as part of a solution for the (nonconvex) assignment
% problem.
%
% Another limitation, which is noted by the authors, is that the robots' kinematic
% constraints are not considered. Nonholonomic kinematics, in particular,
% present a non-convex problem. However, the authors claim, "although the
% results will no longer be optimal for such robots, they should still be
% quite good in practice."
%
% The path planning solution also does not consider collisions between the
% robots. Thus, in practice it can only be used as an input to some lower-level motion
% planner.
%
% In the path-planning case, the convexity requirement is a considerable
% drawback: the minimum-scale constraint is convex only if orientation is
% fixed. In a real-world situation, it is desirable to have a minimum-scale
% constraint (since robots physically must maintain some distance from each other)
% and often not desirable to have a fixed orientation. In this case, the
% approximations proposed in the authors' other work [2] may be useful.
%
% One higher-level limitation of this work is that it assumes a central
% planner which is aware of all of the robots' positions. In reality,
% control of the robot swarm may be decentralized.
%
% Some of these limitations are addressed in the authors' later works. For
% example, [3] addresses collision avoidance between the robots.

%% Conclusion
%
% We successfully reproduced the results obtained
% by Derenick and Spletzer for the minimimum total distance problem, and 
% identified an unstated assumption in their derivation. We also reproduced
% the two extensions demonstrated in the paper: the addition
% of workspace constraints, and path planning in a polygonal environment.
% We found the solution to be useful for high-level planning,
% but too limited to stand alone without additional algorithms for assignment and
% collision avoidance.

%% References
%
% * [1] J. Derenick and J. Spletzer, "Convex Optimization Strategies for Coordinating Large-Scale Robot Formations", IEEE Transactions on Robotics, vol. 23, no. 6, pp. 1252-1259, 2007.
% * [2] J. Spletzer and R.Fierro, “Optimal positioning strategies for shape changes in robot teams,” in Proc. IEEE Int. Conf. Robot. Automat., 2005, pp. 742– 747.
% * [3] J. Derenick, J. Spletzer and V. Kumar, “A Semidefinite Programming Framework for Multi–robot Systems Operating in Dynamic Environments”, Proceedings of the 49th IEEE Conference on Decision and Control (CDC 2010), Atlanta, Georgia, Dec, 2010.

%% Source code
% 
% <../main.m main.m> is the source file used to generate
% this report.
%
% The following functions are used for data synthesis:
%
% * <../UW_points.m UW_points.m>: generates "UW" shape
% * <../circle_points.m circle_points.m>: generates circle shape 
% * <../generate_env.m generate_env.m>: generates triangular environment 
% 
% The following functions are used to formulate the optimization problems:
%
% * <../constraint_matrix.m constraint_matrix.m>: formulates matrix for Aq = 0
% * <../triangle_constraints.m triangle_constraints.m>:
% formulates matrices for Cq <= d
%
% The following functions are used for plotting results:
%
% * <../plot_robots.m plot_robots.m>
% * <../plot_robot_path.m plot_robot_path.m>
% * <../plot_shape.m plot_shape.m>
% * <../draw_line_segment.m draw_line_segment.m>
% * <../draw_polyhedron.m draw_polyhedron.m>
% * <../plot_triangles.m plot_triangles.m>
% * <../plot_triangle_example.m plot_triangle_example.m>


