%% Multi-Robot Navigation in Formation via Sequential Convex Programming [1]
% Alex Blidaru, Wesley Fisher
%
%
% This paper presents an optimization-based method for navigating
% formations of robots through spaces containing static and dynamic
% obstacles. In separate phases, the algorithm presented in this paper
% computes areas of collision-free spaces, optimizes a goal formation
% within this area, and allows robots to move towards the goal formation
% using local planners.


%% Motivation
%
% Many tasks in robotics may require the robots to move while maintaining
% a formation. Some of these instances may include [1]:
%
% * Maintaining communications
% * Collaborative handling of objects
% * Surveillance
% * Improved navigation
%
% This paper presents a method for navigating a team of robots in
% formation, while reconfiguring their formation for optimal progression
% and obstacle avoidance [1].

%% Overall Algorithm
%
% A high-level view of the algorithm is as follows:
%
% # Information about the world such as obstacle positions and goal point(s) is loaded.
% # Convex, collision-free polytopes are calculated
% # A goal formation is calculated in (ideally) free-space at a low
% frequency
% # Robots are navigated to their goal positions at a higher frequency

%% Scope
%
% The paper presents formulations for both homogeneous robotic swarms,
% imagined in the paper as quadrotor swarms, and for mobile manipulators.
% This project attempts to reproduce the method for robotic swarms moving
% in 3 dimensions. See section 5 of [1] for the extension to mobile
% manipulators.
%
% This project, similar to the original paper, makes use of the IRIS [2]
% algorithm in order to compute areas of free space.
%
% The paper leaves the navigation of individual robots to a local planner
% for each robot, which can handle the robot's dynamics and local collision
% avoidance. Local planning and control are outside of the scope of this project, and robots will 
% simply be driven directly towards their goal position. Due to this,
% advanced object-avoidance capabilities will not be exhibited. As such, we
% do not attempt to re-create Figure 4 in [1], as the object avoidance
% behaviour is considered to be critical to that experiment. See section 4
% in [1] for information on this.
%
% We consider full run-time optimization to be outside the scope of this
% project, and thus do not attempt to re-create the experiment shown in Tabl
% 1 of [1]. The authors of the paper themselves note that a C++
% implementation would function much faster than their Matlab implementation,
% which seems to render profiling the execution of the Matlab code
% unnecessary, except for potentially verifying time complexity, etc.
%
% We do not perform experiments with large swarms of 16 robots, as do the
% authors of the paper. As the authors note, the formation optimization
% algorithms can be made to work only with the convex hull of a formation,
% rendering the process independent of the number of robots. The use of
% other algorithms (goal assignment, etc) for large numbers of robots is
% outside the scope of this project as well. We are able to fully
% demonstrate the swarm formation planning and navigation aspects of the
% paper's algorithm using 4-robot swarms.



%% Problem Formulation Preliminaries
%
% Define the position of robot $i$ at time $t$ as $p_i(t)$. In the
% robot swarm formulation, robots are considered to be cylindrical with
% radius $r$ and height $2h$, and the volume of all robots is $V$.
%
% The volume taken up by obstacles, defined as a list of polytopes, is
% $O$, which is dialated by the robot volume to $O+V$. The remaining
% collision free workspace is $W=R^3\\(O+V)$. At this point, the robots
% can be treated as particles thanks to the object dilation.
%
% Dynamic obstacles take up volume $D_j(t)$ for obstacle $j$ at time
% $t$.
% 


%% Largest Convex Collision-Free Region Formulation
%
% To calculate the largest collision-free region, the IRIS algorithm is
% used [2], with the MOSEK optimization toolbox (third party code sources given in references). This problem involves calculating the largest convex
% collision free region containing a starting point. The IRIS algorithm [2] involves maximizing an ellipsoid inside of
% a polytope region of collision-free space. The paper uses the IRIS
% algorithm to calculate the largest convex area containing the desired
% formation goal point $g_dir$, and a given set of additional points, $I$. If no such area
% exists, $g_dir$ is moved linearly towards the current formation
% centroid until an area can be found containing all $I$ and this goal.
%
% In this project, this is implemented by first growing a convex region
% from the desired goal point. The result is a polytope expressed as $\{x|
% Ax <= b\}$ with $A$ and $b$ returned by the algorithm. If all
% positions $x_i$ in $I$ satisfy the inequalities, the result is returned. If
% not, a point between the formation centroid and $g_dir$ is calculated,
% moving closer to the centroid with each iteration. This process of starting
% with a point near the goal and moving closer is what is refered to when a polytope is described as 'directed towards the goal'. The final point, from
% which growing the polytope was successful, is termed the 'growth' point
% in this report.
%
% Three polytopes are generated in [1], each requiring a different set of
% points contained in $I$:
%
% * $P_{fog}$: $I$ is set to be the locations of all robots. This polytope is
% then the largest polytope, directed towards the goal, which contains all
% current robot positions.
% * $P_{og}$: $I$ is set to be the location of the formation centroid. This polytope is
% then the largest polytope, directed towards the goal, which contains the formation centroid, but may not contain the robot positions.
% * $P_{g}$: $I$ is set to be the goal point. This polytope is
% then the largest polytope expanded from the goal position. There is no guarantee that the formation centroid, or robot positions, will be contained within.
%
% These regions are also shown in Figure 2 of [1], shown below. $P_{fog}$
% is shaded blue, $P_{og}$ is shaded red, and $P_{g}$ is shaded green. The
% grey squares are obstacles, and the robots are shown as yellow dots.
% 
% <<fig2_from_paper.png>>
% 

%%
% The original paper formulates the regions in a 4-dimensional spaces, with
% the fourth dimension being time to account for dynamic obstacles. This is
% expressed as:
%
% $\bar{O} = (O+V) \times [0,\tau] \cup_{j \in J} (D_j(t_o + t) + V) \times t \in R^4$
%
% We take a more conservative approach: dynamic obstacles are expressed as the
% convex hull of their verticies at time $t$ in union with their vertices
% at time $t+2*dt$, where $dt$ is the outer-loop planning frequency,
% typically 2s in this project. Our expression then becomes:
%
% $\bar{O} = (O+V) \cup_{j \in J} (D_j(t) + D_j(t+2*dt)) \in R^3$
%
% Our approach is more conservative, and works well for objects with
% linear and constant velocities. For objects with changing velocities, an
% additional scaling factor could be applied to the convex hull of vertices.
% The formulation in the original paper will allow for more
% flexibility in implementing local planners for individual robots.
%

%% Optimal Formation Exploration Region
%
% The optimization of a goal formation, described below, is performed
% within some region of the world. There are four potential regions that are
% explored. For each region, an attempt to find a robot formation is made. If
% successful, the results are returned. Otherwise (ex: if there is not enough room to find a valid formation within the region), the next region in the
% list is tested.
%
% # $P_{fog} \cap P_{og}$ : This is the ideal region. $P_{fog}$ ensures
% that there is a collision-free path from each robot to the new formation
% goal. $P_{og}$ suggests that there is a collision-free path towards the
% goal from this new location. The intersection means that the area being
% searched should demonstrate both of these properties, as both are
% collision-free convex polytopes. The intersection is performed by
% stacking $A$ and $b$ for each polytope.
% # $P_{fog}$ : As the first fall-back option, a formation is found that all
% robots have a clear path towards
% # $P_{og}$ : As the second fallback option, a formation is found that may
% be directed towards the goal
% # $P_{g}$ : The final fallback option is expanded from the goal location, and only needs to contain the goal point.
% No advantage to this region is suggested by the authors.
% 
% Both 3) and 4) require local planning of the robots, as there is no
% guaranteed collision-free path towards the formation goal. As such, this
% report attempts to work only within the 1) and 2) regions.
%
% For intuition on the use of $P_{fog} \cap P_{og}$, one can review Figure
% 2 from [1] above. The region to be explored is the part of the map shaded
% both red and blue. One can see that all robots have a clear path to any
% point in this area, due to $P_{fog}$, and that any point in this area has
% a clear line of sight to the goal, due to $P_{og}$. While not all
% situations line up as fully as in this example, the intuition is useful.
%

%% Formation Definition
% 
% Although the exact definition of formations is robot and problem
% dependent, the paper looks at two formation types: one for
% quadrotor UAVs, and another for mobile manipulators. Our focus in this
% report is exclusively on the quadrotor UAV formulation. With that mind,
% a formation $F_0^{i}$ is defined by the following parameters:
% 
% * ${r_{0,1}^{i}, ..., r_{0,n}^{i}}$: the position of each robot in the formation
% * ${f_{0,1}^{i}, ..., f_{0,n}^{i}}$: the convex hull of the robot positions in the formation
% * $d_0^{i}$: the minimum distance between any two robots in formation $i$
% * $c^i$: cost of formation $i$
%
%%
% In the above, the robot positions and the points defining the convex hull are defined
% relative to the centroid of each formation. For simplicity, in defining
% our formations, we have shifted the positions of the robots so that their centroid matches the origin point of $(0,0,0)$. 



%% Optimal Formation Formulation
% 
% Once an appropriate convex collision-free region is found, the optimal
% formation contained within the convex region needs to be computed. This
% was done by minimizing the following cost function for each potential
% formation $i$:
%
% $C(x_i) = \omega_t * ||t - g||_2^2 + \omega_s * ||s - \bar{s}||_2^2 +
% \omega_q * ||q - \bar{q}||_2^2 + c_i$
%
% Where:
%
% * $x_i$ represents the parameters to optimize, and contains position $t$,
% rotation $q$, and scale $s$
% * $\omega$ are design weights to encourage optimization for position,
% scale and/or rotation
% * $t$ is the formation centroid position, and $g$ is the goal position
% * $s$ is the scale, and $\bar{s}$ is the desired scaling factor (default
% $1.0$)
% * $q$ is the formation rotation in quaternion format, and $\bar{q}$ is
% the desired rotation (default $0,0,0,0$ - no rotation)
% * $c_i$ is a static cost assigned to each formation
%
% The formation, and its optimized parameters, with the lowest cost is
% chosen. The points in this formation are then used as the robot goal
% positions.
% 
% [1] also uses three constraint functions as part of their optimization,
% to ensure that feasible formations are found. The first constraint is
%
% $C_1^j = \{A*(t + s*rot(q, f_j^i))\ <= b\}$ for $j=1, ..., n
%
% where $f_j^i$ is the position of robot $j$ in formation $i$, and $n$ is the number of robots in the formation. This
% constraint requires the transformed robot location to be within the
% collision-free region defined by $A$ and $b$, as described above, and is required for all robots in the formation: $C_1 = \cap_j
% C_1^j$. In the original work of Alonso-Mora et al. constraint $C_1^j$ is
% defined for each vertex of the formation's convex hull, instead of the
% position of each robot in the formation, which provides computational
% advantages when optimizing for formations with a large number of robots,
% since fewer constraints en up being created.
%
% The second constraint is given by
%
% $C_2 = \{s*d_0^i >= 2*max(r,h)\}$
%
% where $d_0^i$ is the minimum inter-robot distance in formation $i$, while $r$
% and $h$ are the physical dimensions of the robot, considered to be a cylindrical quadrotor. This
% constraint prevents robots from being assigned goal positions too close
% to one another, and thus prevents inter-robot collisions. Note that a
% local planner is needed to enforce this constraint during motion; the
% optimization constraint only guarantees that the goal formation is free
% of inter-robot collisions.
%
% The final constrain is 
%
% $C_3 = \{||q||_2^2 = 1\}$
%
% This enforces the constraint that $q$ is a unit quaternion, and thus
% represents a rotation. The following two additional constraints were also added on q to
% ensure that formations will remain planar.
%
% $C_4 = q*[0,1,0,0]^T = 0$
%
% $C_5 = q*[0,0,1,0]^T = 0$
%
% The above optimization problem was solved using the |fmincon| Matlab
% function for each of the defined formations, ultimately selecting for the
% formation with the smallest incurred cost. Although initially the optimization
% problem was solved through both an interior point algorithm, as well as a
% Sequential Quadratic Program, it quickly became apparent that the SQP
% solver performed significantly better, converging to a solution up to 1.5
% times faster than the Interior Point algorithm. Further improvements to
% the convergence time were achieved by explicitly providing the solvers
% with the objective function gradient
%
% $\partial C(x)/\partial x = 2*[\omega_t * (t - g), \omega_s * (s - \bar{s}), \omega_q * (q - \bar{q})]$
%
% and the constraints gradients
%
% $\partial C_1^j/\partial x = [A, A*rot(q, f_j^i), s*A*\partial rot(q,
% f_j^i)/\partial q]$ for $j=1, ..., n$
%
% $\partial C_2/\partial s = d_0^i$
%
% $\partial C_3/\partial q = 2 * q$
%
% $\partial C_4/\partial q = [0,1,0,0]^T$
%
% $\partial C_5/\partial q = [0,0,1,0]^T$
%
% While the explicit use of gradients improved the runtime of both
% algorithms, the SQP solver still performed slightly faster than the
% Interior Point approach. Furthermore, the converge speed-up was achieved
% primarily from the use of the explicit cost gradient, and as such all of
% our results in later sections are obtained using just the explicit cost
% gradient. The explicit constraint gradient can be activated by modifying
% the appropriate parameter in |optimize_formation.m|.
%
% Due to the lack of quaternion support in the base verion of Matlab,
% several functions providing quaternion arithmetic were used from the
% Drake Robotics Toolbox, which included the function that computed the result
% and derivative of $rot(q, f_j^i)$ in the objective and constraint
% functions above, the results of which was also verified symbolically. 
%

%% Formations Used
%
% In our tests, the optimization algorithm detailed above was employed
% over 5 4-robot configurations, which are shown below.
%%
% 
% <<formations.png>>
% 

%%
% The Square, T, Diamond and Line formations are planar in the XY plane, and the Vertical
% Line formation is planar in the YZ plane. This was simply a design choice,
% and non-planar constraints are supported and would be more appropriate
% for a higher number of robots (e.g. 8 robots in a cuboid formation).
% Based on the cost assigned to each formation, the optimization process
% has the following preference order: Square, Diamond, T, Horizontal Line,
% and finally Vertical Line.
% 



%% Goal Assignment and Robot Motion
%
% Each robot is assigned a goal within the recently computed optimal
% formation. Assignments are done to minimize:
%
% $min_{\sigma} \sum_{i \in I} ||p_i - r_{\sigma(i)} ||_2^2$
%
% Where $r_{\sigma(i)}$ represents the assigned goal of robot $i$. This
% optimization is done by brute force, as we do not work with large groups
% of robots in this project. The authors suggest methods which are more
% appropriate for large groups of robots in section 4-A of [1].
%
% Once robots are assigned their goal, they are moved at their maximum
% speed towards that goal at each timestep. A deadzone of 55 percent of a
% robot's maximum travelled distance per timestep is introduced to prevent oscillation.
%
% The goal is considered reached when all of the below are met:
%
% # The formation centroid is near the goal centroid position
% # All robots are motionless
% # The end of an outer planning loop is reached
%
% At this point, a new goal is selected, or the experiment ends if all
% goals have been reached.

%% Data Sources
%
% Artificial scenes are generated similar to Figures 2 and 6 in the paper [1].
% Obstacles are defined as the vertices of cubes. Dynamic obstacles are
% introduced with linear velocities. The large world with many obstacles
% was generated by an automated process that took a bitmap image as input,
% and automatically detected the exterior boundary of all polygons
% contained in the image, using that information to automatically create
% the envrionment in a format appropriate for IRIS. This approach makes it
% easy to quickly test out new static environments, and could be further
% extended to generate environments with random dynamic obstacles.

%% Initial Demonstration
%
% Two animations are shown below to first demonstrate the algorithm as
% implemented. We use a swarm of 4 robots, with a maximum velocity of
% 0.5m/s. The formation optimization runs every 2s, and robots are moved
% towards their goal at 10 Hz.
%
% The animations provide a main view in the XY plane, and secondary views in the XZ and YZ planes.
% Seen in each animation are:
%
% * Annotated time
% * Annotated case for formation optimization
% * Blue X: current robot positions
% * Red X: robot goal positions in current goal formation
% * Green Dot: growth point of final $P_fog$
% * Blue Dot: current goal point (multiple are used to navigate a large map)
%
% The red polygon is the 2d projection of the polytope used for calculating a goal formation. In
% the left animation, this is kept as $P_{fog}$ (also shown with the ellipsoid
% used in IRIS when generating it). In the right
% animation (and all subsequent animations), this is $P_{fog} \cap P_{og}$. At around 19s, it can be seen
% that the second polygon is more restrictive than the first. No ellipsoid
% is shown for this polygon, as it was not generated from growing an
% ellipsoid.
%
% One notable behaviour can be seen between 6 and 10 seconds, as annotated
% in the animations:
%
% * t=6s: the closest point to the goal point is the far
% right corner of the polygon. At this point, only a vertical line
% formation will fit, and thus it is selected.
% * t=8s: the robots have all
% moved inwards enough that they are all contained in a polytope grown from a point much
% closer to the goal point.
% * t=12s, the object has moved out of
% the way. A larger polytope can now be grown, and the formation returns to
% a more preferred planar formation.
%
% Also of note is that at t=20s, the polytope on the right is re-planned and
% does not include all current robot positions. This is expected, as it now
% shows only the union $P_{fog} \cap P_{og}$. However, a collision-free transition 'into' the polytope should still be guaranteed from $P_{fogs}$. 
%
% <html><IMG SRC="default_ununioned.gif"><IMG SRC="default.gif"></html>

%% Corridor World Demonstration
%
% In the animation below, we show the navigation of a robot swarm through a
% corridor with objects on both sides. Notable elements of the execution
% are:
%
% * t=0s: a collision-free region can be found expanding the polytope just
% outside of the corridor opening
% * t=8s: the closest point to the new goal location is a corner slightly
% inside the corridor. The goal formation is set to a vertical line in
% order to be as close as possible to the goal.
% * t=10s: now that each robot can be included in a polytope generated
% inside the corridor, the region for formation optimization opens up. The
% robots are given a smaller goal formation which can fit inside the small
% first corridor.
% * t=20s: inside the larger section, the formation can expand into a
% preferred (ie: non-scaled) formation
% * t=28s: once again, the optimal goal position is a vertical line
% centred on the right corner of the polytope
% * t=30s: the formation can now re-plan to a slightly un-shrunk goal
% formation inside the final stretch of the corridor. Note that it only
% opens up enough so that the robots are at the boundaries of objects.
%
% <html><IMG SRC="corridor.gif"></html>
%

%% Obstacle World Demonstration
%
% Finally, we demonstrate the algorithm in a larger map with many more
% obstacles. At the very start, and at around t=48s (and at other various times), we see cases where optimizing
% the formation in $P_{fog} \cap P_{og}$ fails and falls back to $P_{fog}$.
%
% <html><IMG SRC="obstacles.gif"></html>
%

%% Summary, Conclusions, and Critiques
%
% We have implemented the collision-free region generation, and formation
% optimization defined in [1]. Combined with simple individual robot
% control, we are able to demonstrate robot navigation while in formation.
% The demonstrations show the ability to alter formation based on
% surrounding objects.
%
% The algorithm appears to be reasonably successful in aiding navigation.
% However, it only serves as part of a full navigation solution. A method
% of generating a planned path through fields of obstacles, such as an RRT
% or PRM planner, is required. In addition, this algorithm appears to need
% a well-designed local planner for each robot in order to perform at its
% best and to achieve good obstacle-avoidance.
%
% The experimental results from Figure 4 and Table 1 of [1] would be better if the
% experimental setups we more well-described. For example, it is not stated
% how many trials at each combination of robot and object speed were used
% when generating Figure 4.
%
% The addition of an ablation-type study might add clarity to the
% usefulness of different parts of the algorithm. For example, it was
% qualitatively observed that optimizing the formation within $P_{fog} \cap
% P_{og}$ performed better than just $P_{fog}$, but this could be better
% quantified. The addition of quantitative metrics, such as distance to
% goal over time, might be useful in such cases.
%
% Based on our experimental results, it is currently unclear of the
% practical effects on formation generation of different values for the $w_t$,
% $w_s$, and $w_q$ optimization weights, as adequate performance was observed for a variety of chosen weights.
% The paper offers no insight regarding the choice of these weights.
% However, certain applications and environments might be more sensitive to
% weight choices based on the needs of the system. For example, in the case
% of security robots on patrol, it might be desired for the algorithm to
% optimize for the orientation of robots, to ensure that they are always
% pointed in the right direction, even if that may come at the cost of
% accuracy in position. While one approach to determining the appropriate
% optimization weights is trial-and-error, better heuristic-based
% approaches could be developed in order to more easily obtain the desired
% formation behaviour.

%% References
%
% [1] - Javier Alonso-Mora, Stuart Baker and Daniela Rus. "Multi-robot
% navigation in formation via sequential convex programming" in 2015 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)
%
% [2] - R. Deits and R. Tedrake. "Computing large convex regions of obstacle-free space through semidefinite programming" in Proceedings of the eleventh international workshop on the Algorithmic Foundations of Robotics, Istanbul, 2014.

%% Third-Party Code
%
% * IRIS - https://github.com/rdeits/iris-distro
% * GIF Animation Code - https://www.mathworks.com/matlabcentral/fileexchange/63239-gif?s_tid=mwa_osa_a
% * MOSEK - https://www.mosek.com/
% * DRAKE Quaternion Functions - https://github.com/RobotLocomotion/drake
% * Plot 2D/3D Region Debug Function - https://www.mathworks.com/matlabcentral/fileexchange/9261-plot-2d-3d-region

%% Code: Growing Collision-Free Polytope
%
% <include>growing_poly.m</include>
% 

%% Code: Formation Optimization Cost Function
%
% <include>optimization_scripts/costFunc.m</include>
%

%% Code: Formation Optimization Constraint Function
%
% <include>optimization_scripts/constraints.m</include>

%% Code: Formation Optimization Caller
%
% <include>optimization_scripts/optimize_formation.m</include>

%% Code: Running Experiment
%
% <include>run_experiment.m</include>


function  main()

clear all;
close all;
dbstop if error;

run_experiment(@get_def_world_objects, 'default_ununioned', 0, 0);
run_experiment(@get_def_world_objects, 'default', 1, 0);
run_experiment(@get_corridor_world_objects, 'corridor', 1, 0);
run_experiment(@get_obstacles_world_objects, 'obstacles', 1, 0);

close all;
end

