Full Code of LenaShengzhen/AerialRobotics for AI

Repository: LenaShengzhen/AerialRobotics
Branch: main
Commit: b3fe62f2df62
Files: 32
Total size: 56.2 KB

Directory structure:
gitextract_60fphljc/

├── .gitmodules
├── Motion_Planning/
│   ├── 0Maps/
│   │   ├── 3dCorner.txt
│   │   ├── Z_3blocks.txt
│   │   └── ellipsoid.txt
│   ├── 1Map_Generation/
│   │   ├── GridMap.m
│   │   └── PointCloudMap.m
│   ├── 2Path_Planning/
│   │   ├── JPS_3D.m
│   │   └── simplify_path.m
│   ├── 3Safe_Flight_Corridors/
│   │   ├── Ellipsoid.m
│   │   ├── Hyperplane.m
│   │   ├── LineSegment.m
│   │   ├── MultipleSegments.m
│   │   ├── Polyhedron.m
│   │   ├── SFC_3D.m
│   │   └── rotationMatrix.m
│   ├── 4Time_Allocation/
│   │   ├── averageSpeed_ta.m
│   │   └── trapezoidalSpeed_ta.m
│   ├── 5Trajectory_Planning/
│   │   ├── QPbyUseSFC.m
│   │   ├── TrajectoryPlanning.m
│   │   ├── calc_Q.m
│   │   └── calc_dc.m
│   ├── demo/
│   │   ├── demo_3dCorner.m
│   │   ├── demo_Z_3blocks.m
│   │   └── demo_ellipsoid.m
│   ├── draw/
│   │   ├── draw_ObcPoints.m
│   │   ├── makeGifAndJpg.m
│   │   └── plot_path.m
│   ├── init_data.m
│   └── runsim.m
├── Motion_Planning.md
├── README.md
└── Trajectory_Planning.md

================================================
FILE CONTENTS
================================================

================================================
FILE: .gitmodules
================================================
[submodule "Motion_Planning/QuadrotorSimulation"]
	path = Motion_Planning/QuadrotorSimulation
	url = https://github.com/LenaShengzhen/QuadrotorSimulation.git


================================================
FILE: Motion_Planning/0Maps/3dCorner.txt
================================================
# map 3dCorner
boundary 0.0 0.0 0.0 3.1 3.1 3.0

# downblock
block 2.0 1.5 1.8    3.0 2.5 3.0      30.00 144.00 255.00

# midblock
block 2.3 1.5 1.4    3.0 2.5 1.8      30.00 144.00 255.00

# upblock
block 2.0 1.5 0.1    3.0 2.5 1.4      30.00 144.00 255.00






================================================
FILE: Motion_Planning/0Maps/Z_3blocks.txt
================================================
# map Z_3blocks: Turn left and right

boundary -2.00 0.0 0.00 14.00 3 3.50

block 2.25 0 0       2.65 1.9 3.4       30.00 144.00 255.00


block 0.05 1.1 0    0.45 3 3.4        0.00 191.00 255.00
block 4.5 1.1 0     4.9 3 3.4         0.00 191.00 255.00




================================================
FILE: Motion_Planning/0Maps/ellipsoid.txt
================================================
# map ellipsoid
boundary 0.0 0.0 0.0 3.1 3.1 3.0

block 1.0 0.5 0.1       2.0 0.7 3.0      0.000000 255.000000 0.000000
block 0 1.5 0           1.0 1.7  3.0     0.000000 255.000000 0.000000
block 2.0 1.5 0.1       3.0 1.7 3.0      0.000000 255.000000 0.000000




================================================
FILE: Motion_Planning/1Map_Generation/GridMap.m
================================================
classdef GridMap < handle
    properties
        boundary;       % size of Metric Map(leftdownPoint + rightupPoint)
        blocks;         % coordinate of obstacles on the Metric Map
        res;            % resolution
        margin;
        maxIndex;       % max index of ijk of GridMap
        occ_map;        % flag obstacles on the GridMap
    end
    methods 
        function obj = GridMap()
        end
        
        function load_map(obj, filename, xy_res, z_res, margin)
            fid = fopen(filename);
            tline = fgets(fid);
            obj.blocks = [];
            while ischar(tline)
                % skip empty line and comments
                if numel(tline) > 1 & tline(1)~='#'
                    % convert char array to string
                    if strncmpi(tline, 'boundary', 8)
                        boundarys = strread(tline(9:end));
                    end
                    if strncmpi(tline, 'block', 5)
                        block = strread(tline(6:end));
                        assert(size(block,2) == 9);
                        obj.blocks = [obj.blocks; block];
                    end
                end
                tline = fgets(fid);
            end
            obj.boundary.ld = boundarys(1:3);
            obj.boundary.ru = boundarys(4:6);
            obj.res = [xy_res xy_res z_res];
            obj.margin = margin;
            obj.maxIndex = ceil(abs( (obj.boundary.ru - obj.boundary.ld)./obj.res ));            
        end
        
        % obstacles = block + margin
        function flag_obstacles(obj)
            obj.occ_map = false(obj.maxIndex);    
            for i = 1 : size(obj.blocks,1)
                block = obj.blocks(i, :);
                leftdownPoint = [min(block(1), block(4)) min(block(2), block(5)) min(block(3), block(6))];
                rightUpPoint = [max(block(1), block(4)) max(block(2), block(5)) max(block(3), block(6))];
                obj.blocks(i, 1:3) = leftdownPoint;
                obj.blocks(i, 4:6) = rightUpPoint;
                xyzs = obj.points_to_idx(leftdownPoint - obj.margin);
                xyze = obj.points_to_idx(rightUpPoint + obj.margin);
                obj.occ_map(xyzs(1):xyze(1), xyzs(2):xyze(2), xyzs(3):xyze(3)) = 1;
            end
        end
        
        function ijk = points_to_idx(obj, xyz)
            assert(size(xyz,1) > 0);
            ijk = floor((xyz - obj.boundary.ld)./obj.res + 1);
            ijk = min(max(ijk, 1), obj.maxIndex);
        end
        
        function xyz = idx_to_points(obj, ijk)
            assert(size(ijk,1) > 0);
            xyz = obj.boundary.ld + (ijk - 1 + 0.5).* obj.res;
        end
        
        function ret = idOverflowCheck(obj, i, j, k)
            ret = (i < 1 || i > obj.maxIndex(1) || j < 1 || j > obj.maxIndex(2) || k < 1 || k > obj.maxIndex(3));
        end
        
        % check points collide
        function C = collide(obj, points)
            ijk = obj.points_to_idx(points);
            C = false(size(ijk, 1), 1);
            for i = 1 : size(ijk, 1)
                id = ijk(i,:);
                C(i) =  obj.occ_map(id(1), id(2), id(3)) ~= 0;
            end
        end
                
        %% generate intermedia points
        function pts = generateiIntermediaPoints(obj, start_pos, end_pos)
            %% generate Tiny direction vector
            dd = end_pos - start_pos;
            dir = obj.res(1)*(dd/norm(dd, 2))/(100);
            
            %% generate intermedia points
            lenxy = sqrt(sum((start_pos(1:2) - end_pos(1:2)).^2));
            lenz = sqrt(sum((start_pos(3) - end_pos(3)).^2));
            n_pts = int32(max(lenxy/obj.res(1), lenz/obj.res(3))) + 3;
            pts = [linspace(start_pos(1), end_pos(1), n_pts);
                   linspace(start_pos(2), end_pos(2), n_pts);
                   linspace(start_pos(3), end_pos(3), n_pts)]';
               
            %% mid-point + Tiny direction vector   
            for j = 2 : size(pts,1) - 1
                m1 = mod(pts(j,1), obj.res(1));
                m2 = mod(pts(j,2), obj.res(1));
                m3 = mod(pts(j,3), obj.res(1));
                if(m1*m2*m3 == 0)
                    pts(j,:) = pts(j,:) + dir;
                end
            end
        end
    end
end



================================================
FILE: Motion_Planning/1Map_Generation/PointCloudMap.m
================================================
% blocks of Metric Map change to point cloud
function obps = PointCloudMap(blocks, margin)
    obps = [];
    density = 2 * margin;
    for id = 1 : size(blocks, 1)
         leftdownPoint = blocks(id, 1:3) - margin;
         rightUpPoint = blocks(id, 4:6) + margin;
         plane_Z = [];
         for i = leftdownPoint(1,1) : density : rightUpPoint(1,1)
             for j = leftdownPoint(1,2) : density : rightUpPoint(1,2)
                 pz = leftdownPoint(1,3);
                 plane_Z = [plane_Z; [i, j, pz]];
             end
         end
         
         plane_Y = [];
         for i = leftdownPoint(1,1) : density : rightUpPoint(1,1)
             for k = leftdownPoint(1,3) : density : rightUpPoint(1,3)
                 py = leftdownPoint(1,2);
                 plane_Y = [plane_Y; [i, py, k]];
             end
         end
         
         plane_X = [];
         for j = leftdownPoint(1,2) : density : rightUpPoint(1,2)
             for k = leftdownPoint(1,3) : density : rightUpPoint(1,3)
                 px = leftdownPoint(1,1);
                 plane_X = [plane_X; [px, j, k]];
             end
         end
         
         long   = rightUpPoint(1,1) - leftdownPoint(1,1);
         width  = rightUpPoint(1,2) - leftdownPoint(1,2);
         high   = rightUpPoint(1,3) - leftdownPoint(1,3);
         obps   = [obps; plane_X; plane_X + [long 0 0];
                        plane_Y; plane_Y + [0 width 0];
                        plane_Z; plane_Z + [0 0 high]; ];
    end
end



================================================
FILE: Motion_Planning/2Path_Planning/JPS_3D.m
================================================
function [path] = JPS_3D(map, start, goal)
    
    map_start = map.points_to_idx(start);
    map_goal = map.points_to_idx(goal);
    
    map_visit = map.occ_map;
    
    fn = [0];  % fn = gn(EuclideanDistance) + hn(ManhattanDistance)
    all_direction = make_all_dir();
    nodeStart = struct( ...
        'pos', map_start, ...   % index of the point on the grid-map
        'fa', 1, ...        % fa records the index of the parent node in the closelist
        'id', 1, ...        % id records the index of node in closelist
        'dir', all_direction);
    
    openlist = [nodeStart];
    closelist = [nodeStart];
    map_visit(map_start(1), map_start(2), map_start(3)) = 1;
     
    findgoal = 0;
    while isempty(openlist) == 0
        
        [~, temid] = min(fn);
        node = openlist(temid);
       
        fn(temid) = [];
        openlist(temid) = [];
        fa = node.id;
        
        [e1, ~] = size(node.dir);
        for j = 1 : e1
            nodeNew = jump(map, map_goal, node.pos, node.dir(j,:));
            if (length(nodeNew.pos) == 0 )
                continue;
            end
            if (map_visit(nodeNew.pos(1), nodeNew.pos(2), nodeNew.pos(3)) == 0)
                nodeNew.fa = fa;
                nodeNew.id = length(closelist) + 1;  
                ds = ManhattanDistance(map_goal, nodeNew.pos) + EuclideanDistance(node.pos, nodeNew.pos);
                fn = [fn ds];
                openlist = [openlist; nodeNew];
                closelist = [closelist; nodeNew];
                map_visit(nodeNew.pos(1), nodeNew.pos(2), nodeNew.pos(3)) = 1;
                
                % find the goal
                if (nodeNew.pos == map_goal)
                    findgoal = 1;
                    break;
                end
            end
        end
        % find the goal
        if (findgoal == 1)
           break;
        end
    end
    
    
    path = [];
    k = length(closelist);
    while k > 1
        renode = closelist(k);
        float_pos = map.idx_to_points(renode.pos);     
        path = [float_pos; path];
        k = renode.fa;
    end
end

function dis = EuclideanDistance(pos1, pos2)
    dis = norm(pos1 - pos2, 2);
end

function dis = ManhattanDistance(pos1, pos2)
    dis = norm(pos1 - pos2, 1);
end

function ret = obCheck(map, pos)
    x = pos(1); y = pos(2); z = pos(3);
    ret = map.occ_map(x, y, z);
end

function all_direction = make_all_dir()
    x = [1 1 1 -1 -1 -1 0 0];
    b = unique(nchoosek(x,3),'rows');   
    A = []; 
    for i = 1 : 9                        
	    A = [A; perms(b(i,:))];         
    end
    all_direction = unique(A,'rows');   
end

function newnode = jump(map, goal, startpos, d)
    
    newnode = struct( ...
        'pos', [], ...
        'fa', 1, ...
        'id', 1, ...
        'dir', []);
    
    
    dir_Dimension = sum(d.^2);
    
    if (dir_Dimension == 0)
        return;
    end
    
    newpos = step(startpos, d);
    x = newpos(1); y = newpos(2); z = newpos(3);
    % ourside the grid on the newpos
    if outmap(map, newpos) == 1
        return;
    % meet the obstacle on the newpos 
    elseif map.occ_map(x, y, z) == 1
        return;
    elseif newpos == goal
        newnode.pos = newpos;
        return;
    end
    
   
    if dir_Dimension == 2 
        dside = dir2Dto1D(d);    
       
        if (obCheck(map, startpos+dside(1,:)) == 1) && (obCheck(map, startpos+dside(2,:)) == 1)
            return;
        end
    end
    
   
    dirs = hasForceNeighbour(map, newpos, d);
    if ~isempty(dirs)
        newnode.pos = newpos;
        newnode.dir = dirs;
        return;
    end
    
    % go 3d diagonal
        % sum(d.^2) = d(1)^2 + d(2)^2 + d(3)^2
    if(dir_Dimension == 3)
        % one 3d-diagonal change three 2d-diagonal + three straght line
        node1 = jump(map, goal, newpos, [0 d(2) d(3)]);
        node2 = jump(map, goal, newpos, [d(1) 0 d(3)]);
        node3 = jump(map, goal, newpos, [d(1) d(2) 0]);
        % three straght line
        node4 = jump(map, goal, newpos, [d(1) 0 0]);
        node5 = jump(map, goal, newpos, [0 d(2) 0]);
        node6 = jump(map, goal, newpos, [0 0 d(3)]);
        % find a ForceNegihbour
        if(length(node1.pos) + length(node2.pos) + length(node3.pos) + length(node4.pos) + length(node5.pos) + length(node6.pos) > 0)
            newnode.pos = newpos;
            newnode.dir = [d];
            
            newnode.dir = addDir(node1.pos, [0 d(2) d(3)], newnode.dir);
            newnode.dir = addDir(node2.pos, [d(1) 0 d(3)], newnode.dir);
            newnode.dir = addDir(node3.pos, [d(1) d(2) 0], newnode.dir);
            newnode.dir = addDir(node4.pos, [d(1) 0 0], newnode.dir);
            newnode.dir = addDir(node5.pos, [0 d(2) 0], newnode.dir);
            newnode.dir = addDir(node6.pos, [0 0 d(3)], newnode.dir);
            return;
        end
    end
    
    % go 2d diagonal
    if(dir_Dimension == 2)
        node4 = jump(map, goal, newpos, [d(1) 0 0]);
        node5 = jump(map, goal, newpos, [0 d(2) 0]);
        node6 = jump(map, goal, newpos, [0 0 d(3)]);
        % find a ForceNegihbour
        if(length(node4.pos) + length(node5.pos) + length(node6.pos) > 0)
            newnode.pos = newpos;
            newnode.dir = [d];
            % Add the direction of find ForceNegihbour
            newnode.dir = addDir(node4.pos, [d(1) 0 0], newnode.dir);
            newnode.dir = addDir(node5.pos, [0 d(2) 0], newnode.dir);
            newnode.dir = addDir(node6.pos, [0 0 d(3)], newnode.dir);
            return;
        end
    end
    
    % [go straght] or [go diagonal not find ForceNegihbour]
    newnode = jump(map, goal, newpos, d);
    return;
end

% if pos ~= Null, dirs = [dirs; d];
function dirs = addDir(pos, d, dirs)
    if(length(pos) > 0)
        dirs = [dirs; d];
    end
end


function dside = dir2Dto1D(d)
    dside = [[0 0 0] ; [0 0 0]];
    temi = 1;
    if(d(1) ~= 0) 
        dside(temi,:) = [d(1) 0 0];
        temi = temi + 1;
    end
    if(d(2) ~= 0) 
        dside(temi,:) = [0 d(2) 0];
        temi = temi + 1;
    end        
    if(d(3) ~= 0) 
        dside(temi,:) = [0 0 d(3)];
    end     
end


function ret = outmap(map, pos) 
    ret = false;
    x = pos(1); y = pos(2); z = pos(3);
    if(x < 1 || x > map.maxIndex(1) || y < 1 || y > map.maxIndex(2) || z < 1 || z > map.maxIndex(3))
        ret = true;
    end
end   


function ret = isforceNeighbourExist(map, pos, d)
    ret = false;
    x = pos(1); y = pos(2); z = pos(3);
    posNeighbour = pos + d;
    if outmap(map, pos) == 1 || ...   
        map.occ_map(x, y, z) == 0 || ... 
        outmap(map, posNeighbour) == 1 || ... 
        map.occ_map(posNeighbour(1), posNeighbour(2), posNeighbour(3)) == 1 
        return;     
    end
    ret = true;
end

function dirs = hasForceNeighbour(map, pos, d)
    dirs = [];
    
    if outmap(map, pos + d) == 1 
        return;
    end
    
    % [go straght]  ----------------------------------------------
    if(sum(d.^2) == 1)
       
        if obCheck(map, pos + d) == 1
            return;
        end
        
        d1 = [abs(d(3)) abs(d(1)) abs(d(2))];
        d2 = [abs(d(2)) abs(d(3)) abs(d(1))];
        
        dob = [d1; -d1; d2; -d2; d1+d2; -d1-d2; d1-d2; -d1+d2];
        for i = 1 : 8
            if isforceNeighbourExist(map, pos + dob(i,:), d) == 1
                dirs = [dirs; dob(i,:) + d];
            end
        end
        
    % go 2D-diagonal ----------------------------------------------
    elseif  (sum(d.^2) == 2)
        dside = dir2Dto1D(d);
        
        for i = 1 : 2
            if isforceNeighbourExist(map, pos - dside(i,:), d - dside(i,:) ) == 1
                dirs = [dirs; d - 2*dside(i,:)];
            end
        end
        
        d1 = [abs(d(1))-1 abs(d(2))-1 abs(d(3))-1];
        if isforceNeighbourExist(map, pos + d1, d) == 1
            dirs = [dirs; d + d1];
        end
        if isforceNeighbourExist(map, pos - d1, d) == 1
            dirs = [dirs; d - d1];
        end
        
    % go 3D-diagonal  ----------------------------------------------
    else  % (sum(d.^2) == 3)
        
        dirs = hasForceNeighbour(map, pos, [0 d(2) d(3)]);
        dirs = [dirs; hasForceNeighbour(map, pos, [d(1) 0 d(3)])];
        dirs = [dirs; hasForceNeighbour(map, pos, [d(1) d(2) 0])];
        dirs = unique(dirs,'rows');        
        
        [e1, ~] = size(dirs);
        for i = 1 : e1
            if(dirs(i,:) == d) 
                dirs(i,:) = [];
            end
        end
    end
end

function newpos = step(pos, d)
    newpos = pos + d;
end


================================================
FILE: Motion_Planning/2Path_Planning/simplify_path.m
================================================
function path = simplify_path(map, path)
    if size(path,1) == 0, return; end
    
    keep_pBooL = false(size(path,1),1);
    keep_pBooL(1) = 1;
    keep_pBooL(end) = 1;
    last_keep = path(1,:);

    for i = 2:size(path,1)-1
        % ~check_line_collision() == 1 : No collision
	    %  check_line_collision() == 1 : collision
        if (~check_line_collision(last_keep, path(i,:)) & check_line_collision(last_keep, path(i+1,:))) 
            last_keep = path(i,:);
            keep_pBooL(i) = 1;
        end
    end
    path = path(keep_pBooL,:);
    
    function c = check_line_collision(start_pos, end_pos)
        
        pts = map.generateiIntermediaPoints(start_pos, end_pos);
        
        %% Collision detection
        c = map.collide(pts);
        c = any(c);
    end

end


================================================
FILE: Motion_Planning/3Safe_Flight_Corridors/Ellipsoid.m
================================================
classdef Ellipsoid < handle
    properties               
        C_;     % Record the matrix of the ellipse, the semi-axis length of the ellipse without square
        d_;     % Center point of ellipse
        axes_;  % Length of each axis of ellipse
        invC_;  % inv(obj.C_)
    end
    methods                   
        function obj = Ellipsoid (a,b)   
            obj.d_ = b;
            obj.setC(a);
        end
        
        function setC(obj, a)
            obj.C_ = a;
            obj.invC_ = inv(a);
            if isnan(obj.invC_), assert(0 >= 42); end
            if (obj.invC_ == inf), assert(0 >= 42); end
        end

        % Calculate distance to the center
        function dis = dist(obj, pt)
            dis = sqrt(obj.square_of_dist(pt));
        end
        
        % Calculate the square of the distance to the center
        function dis = square_of_dist(obj, pt)
            dd = obj.invC_ * (pt - obj.d_)';
            dis = dot(dd, dd);   % dot(A,B)
        end

        % Check if the point is inside
        function ret = inside(obj, pt)
            ret = false;
            if (obj.square_of_dist(pt) <= 1)
                ret = true;
            end
        end

        % Calculate points inside ellipsoid
        function pinside = points_inside(obj, points)
            pinside = [];
            [len, ~]=size(points);
            for i = 1 : len
                if(obj.inside(points(i,:)))
                    pinside = [pinside; points(i,:)];
                end
            end
        end

        % Find the closest point
        function p = closest_point(obj, points)
            p = points(1,:);
            min_dist = 1e8;
            [len, ~]=size(points);
            for i = 1 : len
                d = obj.dist(points(i,:));
                if(d < min_dist)
                    min_dist = d;
                    p = points(i,:);
                end
            end						
        end

        % Find the closest hyperplane from the closest point, 
        function plane = closest_hyperplane(obj, points)
            closest_pt = obj.closest_point(points);
            n = ( obj.invC_ * (obj.invC_)' )*(closest_pt - obj.d_)';
            plane = Hyperplane(closest_pt, n);
        end
    end
end


================================================
FILE: Motion_Planning/3Safe_Flight_Corridors/Hyperplane.m
================================================
classdef Hyperplane < handle
    properties               
        p_;    % point on the plane
        n_;    % Normal of the plane, directional,
        local_bbox_; 
    end
    methods                 
        function obj = Hyperplane(p, n)
            obj.p_ = p;
            obj.n_ = n;
        end
        % Calculate the signed distance from point
        function dis = signed_dist(obj, pt)
            dis = dot(obj.n_, pt - obj.p_);
        end
        % Calculate the distance from point
        function dis = dist(obj, pt)
            dis = abs(obj.signed_dist(pt));
        end
    end
end

================================================
FILE: Motion_Planning/3Safe_Flight_Corridors/LineSegment.m
================================================
classdef LineSegment < handle
    properties               
        p1_;         
        p2_;        
        R_;
        obs_;        
        ellipsoid_;  
        polyhedron_;
        local_bbox_; % Bounding Box of map
        epsilon_;   
    end
    methods                  
        function obj = LineSegment(p1, p2)   
            obj.p1_ = p1;
            obj.p2_ = p2;
            obj.epsilon_ = 1e-10;
        end

        function dilate(obj, radius)
            obj.find_ellipsoid(radius);  
            obj.find_polyhedron();       
            obj.add_local_bbox(obj.polyhedron_); 				
        end

        function set_local_bbox(obj, local_bbox)
            obj.local_bbox_ = local_bbox;				
        end		

        function set_obs(obj, obs)
            Vs = Polyhedron();
            obj.add_local_bbox(Vs);		
            obj.obs_ = Vs.points_inside(obs);	
        end		

        function add_local_bbox(obj, Vs)
            r = norm(obj.p2_ - obj.p1_)/2;  
            dir = (obj.p2_ - obj.p1_)/norm(obj.p2_ - obj.p1_);
            dir_h = [dir(2) -dir(1) 0];
            if(norm(dir_h) == 0)
                    dir_h = [-1 0 0];  
            end
            dir_h = dir_h/norm(dir_h);

            % along x
            pp1 = obj.p1_ + dir_h*(r);
            pp2 = obj.p1_ - dir_h*(r);
            Vs.add(Hyperplane(pp1, dir_h));
            Vs.add(Hyperplane(pp2, -dir_h));

            % along y		
            pp3 = obj.p2_ + dir*(r);
            pp4 = obj.p1_ - dir*(r);
            Vs.add(Hyperplane(pp3, dir));
            Vs.add(Hyperplane(pp4, -dir));		

            % along z, 
            dir_v = [0 0 0];	
            dir_v(1) =  dir(2) * dir_h(3) - dir(3) * dir_h(2);
            dir_v(2) =  dir(3) * dir_h(1) - dir(1) * dir_h(3); 
            dir_v(3) =  dir(1) * dir_h(2) - dir(2) * dir_h(1); 
            pp5 = obj.p1_ + dir_v*(r);
            pp6 = obj.p1_ - dir_v*(r);
            Vs.add(Hyperplane(pp5, dir_v));
            Vs.add(Hyperplane(pp6, -dir_v));
        end

        % 3D 
        function find_ellipsoid(obj, offset_x)
            f = norm(obj.p1_ - obj.p2_)/2;  
            C = [f 0 0; 0 f 0; 0 0 f];
            % f is the radius of the ellipse, the initialized ellipse is a circle
            % C =  [f  0  0]	
            %      [0  f  0]
            %      [0  0  f]
            
            % h = (R^T)*x, (1 0 0)^T = R^T*(p2_ - p1_)  => (p2_ - p1_) = R*(1 0 0)^T
            Ri = rotationMatrix([1 0 0]', (obj.p2_ - obj.p1_)');
            C = Ri * C * (Ri');
            axes = [f f f];
            obj.R_ = Ri;

            E = Ellipsoid(C, (obj.p1_ + obj.p2_)/2 ); 

            obs = E.points_inside(obj.obs_);
            obs_inside = obs;

            % decide short axes-1
            while (~isempty(obs_inside))
                cp = E.closest_point(obs_inside);
                rcp = (Ri')*(cp - E.d_)';   

                % Generate a new ellipse
                if(abs(rcp(1)) < axes(1)) 
                    newy = sqrt((rcp(2))^2 + (rcp(3))^2);
                    axes(2) = newy / sqrt(1 - (rcp(1)/axes(1))^2);
                end
                
                new_C = [axes(1) 0 0; 0 axes(2) 0; 0 0 axes(2)];
                E.setC(Ri * new_C * (Ri'));
                
                % Delete all the points outside the new ellipse
                obs_new = [];
                [len, ~] = size(obs_inside);
                for i = 1 : len
                % constexpr decimal_t epsilon_ = 1e-10; 
                    if(1 - E.dist(obs_inside(i,:)) > obj.epsilon_ )
                        obs_new = [obs_new; obs_inside(i,:)];
                    end
                end
                obs_inside = obs_new;
            end

            % decide short axes-2
            C = [axes(1) 0 0; 0 axes(2) 0; 0 0 axes(3)];
            E.setC(Ri * C * (Ri'));
            
            obs_inside = E.points_inside(obs);
            while (~isempty(obs_inside))
                cp = E.closest_point(obs_inside);
                rcp = (Ri')*(cp - E.d_)';
                dd = sqrt( 1 - (rcp(1)/axes(1))^2 - (rcp(2)/axes(2))^2 );
                if(dd > obj.epsilon_)
                    axes(3) = abs(rcp(3)) / dd;
                end
                
                new_C = [axes(1) 0 0; 0 axes(2) 0; 0 0 axes(3)];
                E.setC(Ri * new_C * (Ri'));
                
                % Delete all the points outside the new ellipse
                obs_new = [];
                [len, ~] = size(obs_inside);
                for i = 1 : len
                % constexpr decimal_t epsilon_ = 1e-10; 
                    if(1 - E.dist(obs_inside(i,:)) > obj.epsilon_ )
                        obs_new = [obs_new; obs_inside(i,:)];
                    end
                end
                obs_inside = obs_new;
            end
            
            E.axes_ = axes;
            obj.ellipsoid_ = E;
            
        end

        function find_polyhedron(obj)
            Vs = Polyhedron();
            obs_remain = obj.obs_;
            while(length(obs_remain))
                plane = obj.ellipsoid_.closest_hyperplane(obs_remain);
                Vs.add(plane);
                obs_tmp = [];
                [len, ~] = size(obs_remain);
                for i = 1 : len
                    p = obs_remain(i,:);
                    if(plane.signed_dist(p) < 0)
                        obs_tmp = [obs_tmp; p];
                    end
                end
                obs_remain = obs_tmp;
            end
            obj.polyhedron_ = Vs;
        end
    end
end


================================================
FILE: Motion_Planning/3Safe_Flight_Corridors/MultipleSegments.m
================================================
classdef MultipleSegments < handle
    properties
        lines_;
        obs_;
        local_bbox_;
    end
    methods 
        function obj = MultipleSegments(local_bbox)   
            obj.local_bbox_ = local_bbox;
        end

        function set_obs(obj, obs)
            obj.obs_ = obs;
        end

        function dilate(obj, path, offset_x)
            [len, ~] = size(path);
            obj.lines_ = cell(1, len-1);

            for i = 1 : (len - 1)
                p1 = path(i,:); p2 = path(i+1,:);
                obj.lines_{i} = LineSegment(p1, p2);
                obj.lines_{i}.set_local_bbox(obj.local_bbox_);
                obj.lines_{i}.set_obs(obj.obs_);
                obj.lines_{i}.dilate(offset_x);
            end	
            
        end	
        
        function drawLines(obj)
            len = length(obj.lines_);
            for i = 1 : len	
                obj.lines_{i}.p1_;
            end
        end
        
        function drawEllipsoids(obj)
            hold on;
            len = length(obj.lines_);
            for i = 1 : len	
                center = obj.lines_{i}.ellipsoid_.d_;
                R = obj.lines_{i}.R_;
                axes = obj.lines_{i}.ellipsoid_.axes_;                
                obj.draw_Ellipsoid(center, axes, R');
            end
            hold off;
        end
        
        function draw_Ellipsoid(obj, center, axes, R)
            [X, Y, Z] = ellipsoid(0,0,0, axes(1), axes(2), axes(3));
            [row, col] = size(X);
            A = [X(:), Y(:), Z(:)];
            B = A*R + center;
            X = reshape(B(:,1),row,col);
            Y = reshape(B(:,2),row,col);
            Z = reshape(B(:,3),row,col);
            mesh(X, Y, Z, 'FaceAlpha','0.5');
        end
        
        % draw the point of obs on the polyhedron 
        function drawPlanePoint(obj)
            hold on;
            len = length(obj.lines_);
            planesCell = obj.lines_{len}.polyhedron_.polys_;
            [~, lenj] = size(planesCell);
            for i = 1 : lenj	
                point = planesCell{i}.p_;
                plot3(point(1), point(2), point(3),'r*');
            end
            hold off;
        end
        
        function drawpolyhedron(obj)
            hold on;
            len = length(obj.lines_);
            for i = len : len
                planesCell = obj.lines_{i}.polyhedron_.polys_;
                p1 = obj.lines_{i}.p1_;
                p2 = obj.lines_{i}.p2_;
                r = norm(p2 - p1)/2;
                pmid = (p2 + p1)/2;
                [~, lenj] = size(planesCell);
                for j = 1 : lenj
                    obj.drawPlane(planesCell{j}.n_, planesCell{j}.p_, pmid, r);
                end
            end
            hold off;
        end
        
        function drawPlane(obj, n, p, pmid, r)
            r = r*2;
            xL = pmid(1) - r;
            xR = pmid(1) + r;
            yU = pmid(2) + r;
            yD = pmid(2) - r;
            
            [X,Y]=meshgrid(xL:0.5:xR, yD:0.5:yU);
            Z = p(3) - ((X - p(1))*n(1)+(Y - p(2))*n(2))/n(3);
            surf(X, Y, Z, 'FaceAlpha','0.5','EdgeColor','flat','FaceColor', '[0 1 1]');
        end
    end
end

================================================
FILE: Motion_Planning/3Safe_Flight_Corridors/Polyhedron.m
================================================
classdef Polyhedron < handle
    properties               
        polys_;    
        epsilon_;  
    end
    methods                 
        function obj = Polyhedron()   
            obj.polys_ = cell(0);
            obj.epsilon_ = 1e-10;
        end

        % Append Hyperplane
        function add(obj, plane)
            obj.polys_{end+1} = plane;
        end
        % Check if the point is inside polyhedron,
        function isinside = inside(obj, pt)
            isinside = false;
            [len, ~]=size(obj.polys_);
                for i = 1 : len
                    if(obj.polys_{i}.signed_dist(pt) > obj.epsilon_)
                        return;
                    end
                end
            isinside = true;
        end

        % Calculate points inside polyhedron
        function pinside = points_inside(obj, points)
            pinside = [];
            [len, ~]=size(points);
            for i = 1 : len
                if(obj.inside(points(i,:)))
                    pinside = [pinside; points(i,:)];
                end
            end
        end
    end
end

================================================
FILE: Motion_Planning/3Safe_Flight_Corridors/SFC_3D.m
================================================
function decomp = SFC_3D(path, obs, boundary)
    % Initialize MultipleSegments
	local_bbox = boundary; offset_x = 0;
    decomp = MultipleSegments(local_bbox);
    decomp.set_obs(obs);
    decomp.dilate(path, offset_x);
end



================================================
FILE: Motion_Planning/3Safe_Flight_Corridors/rotationMatrix.m
================================================
function R = rotationMatrix(v1col, v2col)
    if (norm(v2col) == 0), assert(0 >= 42); end   % There is no concept of collinear zero vector
    
    v1 = v1col';
    v2 = v2col';
    nv1 = v1/norm(v1);
    nv2 = v2/norm(v2);

    if norm(nv1+nv2)==0
        q = [0 0 0 0];
    else
        u = cross(nv1,nv2);  % If nv1 and nv2 are collinear, u = [0 0 0]
        un = norm(u);        % If u = [0 0 0], norm(u) = 0;
        if(un == 0)
            R = [1 0 0; 0 1 0; 0 0 1];
            return;
        end
        u = u/un;

        theta = acos(sum(nv1.*nv2))/2;
        q = [cos(theta) sin(theta)*u];
    end

    R=[2*q(1).^2-1+2*q(2)^2  2*(q(2)*q(3)+q(1)*q(4)) 2*(q(2)*q(4)-q(1)*q(3));
        2*(q(2)*q(3)-q(1)*q(4)) 2*q(1)^2-1+2*q(3)^2 2*(q(3)*q(4)+q(1)*q(2));
        2*(q(2)*q(4)+q(1)*q(3)) 2*(q(3)*q(4)-q(1)*q(2)) 2*q(1)^2-1+2*q(4)^2];
	R = R';
end


================================================
FILE: Motion_Planning/4Time_Allocation/averageSpeed_ta.m
================================================
%   averageSpeed_TimeAllocation:
%   avg_v: the estimated average speed
function [ts, total_time] = averageSpeed_ta(path, avg_v)
    path_seg_len = sqrt(sum((path(2:end, :) - path(1:end-1,:)).^2,2));
    path_len = sum(path_seg_len);
    total_time = path_len/avg_v;

    ts = cumsum(path_seg_len);
    ts = ts/ts(end);
    ts = [0; ts]';
    ts = ts*total_time;
end




================================================
FILE: Motion_Planning/4Time_Allocation/trapezoidalSpeed_ta.m
================================================
%   trapezoidalSpeed_TimeAllocation:
%   avg_v: the estimated average speed
%   acc: Acceleration  trapezoidalSpeed
function [ts, total_time] = trapezoidalSpeed_ta(path, avg_v, acc)

    t_acc = avg_v/acc;
    s_acc = 0.5*acc*(t_acc^2);
    
    path_seg_len = sqrt(sum((path(2:end, :) - path(1:end-1,:)).^2,2));
    path_len = sum(path_seg_len);
    total_time = path_len/avg_v;
    
    ts = cumsum(path_seg_len);
    ts = ts/ts(end);
    ts = [0; ts]';
    ts = ts*total_time;
    
    
    % trapezoid Area = 0.5(a + b)  h
    % math ep
%     a1 = ts(2) - t_acc;
%     b1 = a1 - speed_h/acc;
%     ep1 = 0.5*(a1 + b1)*speed_h + t_add1*(avg_v + speed_h) == s_acc;
%     a2 = ts(end) - ts(end-1) - t_acc;
%     b2 = a2 - speed_h/acc;
%     ep2 = 0.5*(a2 + b2)*speed_h + t_add2*(avg_v + speed_h) == s_acc;
%     t_mid = (ts(end-1) - ts(2));
%     ep3 = (t_mid-t_add1-t_add2)*speed_h - (t_add1 + t_add2)*avg_v == 0;
     

    %% solve
    syms t_add1 t_add2 speed_h;
    a1 = ts(2) - t_acc;
    ep1 = 0.5*(a1 + a1 - speed_h/acc)*speed_h + t_add1*(avg_v + speed_h) == s_acc;
    a2 = ts(end) - ts(end-1) - t_acc;
    ep2 = 0.5*(a2 + a2 - speed_h/acc)*speed_h + t_add2*(avg_v + speed_h) == s_acc;
    t_mid = (ts(end-1) - ts(2));
    ep3 = (t_mid-t_add1-t_add2)*speed_h - (t_add1 + t_add2)*avg_v == 0;
    [x0,y0,z0] = solve(ep1,ep2,ep3, t_add1, t_add2, speed_h);
    ans = double([x0,y0,z0]);
    
    sort_ans = sortrows(ans,3);
    
    ts(2) = ts(2) + sort_ans(1,1);
    ts(end-1) = ts(end-1) - sort_ans(1,2); 
    
    if(path_len < 2*s_acc), assert(1 >= 2); end
    if(path_seg_len(1)   < s_acc), assert(1 >= 2); end
    if(path_seg_len(end) < s_acc), assert(1 >= 2); end
    
end



================================================
FILE: Motion_Planning/5Trajectory_Planning/QPbyUseSFC.m
================================================
function X = QPbyUseSFC(waypts, ts, decomp)

%% condition
traj.n_order = 7;
traj.n_poly = size(waypts, 1) - 1;
traj.p0 = waypts(1,:);
traj.pe = waypts(end,:);
traj.v0 = [0,0,0];
traj.ve = [0,0,0];
traj.a0 = [0,0,0];
traj.ae = [0,0,0];


%% trajectory plan
[minSnapValue, px, py, pz] = minimum_snap_three_axis_SFC(traj, ts, decomp);

disp(['minSnapValue is : ',num2str(minSnapValue)]);

X = [px py pz];
end

% v0 = [1*3]
function [minValue, px, py, pz] = minimum_snap_three_axis_SFC(traj, ts, decomp)
    % Dimension
    dim = 3;
    n_order = traj.n_order;
	p0 = traj.p0;
	pe = traj.pe;
    v0 = traj.v0;
    ve = traj.ve;
    a0 = traj.a0;
    ae = traj.ae;
	
	n_poly = traj.n_poly;
	n_coef = n_order+1;
	
	%% compute Q,  Q is a Symmetric matrix
    % Q_1 =   [0    0   0]
    %         [0    f   f]
    %         [0    f   f]          % f =  integral of ts(i) ~ ts(i+1) 
    % Q_x =   [Q_1  0     0    0]
    %         [0    Q_2   0    0]
    %         [0    0   Q_...  0]
    %         [0    0     0  Q_n]   % n = n_poly
    % Q_all = [Q_x  0     0]
    %         [0    Q_y   0]
    %         [0    0   Q_z]        % Q_x == Q_y == Q_z
    Q_x = [];
	for i=1:n_poly
	    Q_x = blkdiag(Q_x,calc_Q(n_order,4,ts(i),ts(i+1)));
    end
    zeroM = zeros(size(Q_x));
    Q_all = [Q_x zeroM zeroM; zeroM Q_x zeroM; zeroM zeroM Q_x];
    xlen = size(Q_all,1);
	f = zeros(xlen,1);      %% min (1/2p^TQp + f^Tp)
	
    %% compute Aeq x = beq
    % beacuse Aeq_x == Aeq_y == Aeq_z
    % Aeq = [Aeq_x  0      0]
    %       [0    Aeq_y    0]
    %       [0      0  Aeq_z] 
	neq = 8;  %% (8 equations)
    eqNum_x = (7*(n_poly-1)+neq);
	Aeq_x = zeros(eqNum_x, n_coef*n_poly);
	beq = zeros(dim*eqNum_x, 1);
	
	% start/terminal pva constraints  (8 equations)
	Aeq_x(1:4,1:n_coef) = [calc_dc(ts(1),n_order,0);
	                     calc_dc(ts(1),n_order,1);
	                     calc_dc(ts(1),n_order,2);
	                     calc_dc(ts(1),n_order,3)];
	Aeq_x(5:8,n_coef*(n_poly-1)+1:n_coef*n_poly) = ...
	                    [calc_dc(ts(end),n_order,0);
	                     calc_dc(ts(end),n_order,1);
	                     calc_dc(ts(end),n_order,2);
	                     calc_dc(ts(end),n_order,3)];
	beq(1:8,1)                              = [p0(1),v0(1),a0(1),0,pe(1),ve(1),ae(1),0]';
    beq((eqNum_x   + 1):(eqNum_x   + 8),1)  = [p0(2),v0(2),a0(2),0,pe(2),ve(2),ae(2),0]';
    beq((eqNum_x*2 + 1):(eqNum_x*2 + 8),1)  = [p0(3),v0(3),a0(3),0,pe(3),ve(3),ae(3),0]';
	
	% continuous constraints  ((n_poly-1)*7 equations)
	for i=1:n_poly-1
		t_derc_p = calc_dc(ts(i+1),n_order,0);
        t_derc_v = calc_dc(ts(i+1),n_order,1);
        t_derc_a = calc_dc(ts(i+1),n_order,2);
        t_derc_j = calc_dc(ts(i+1),n_order,3);  % jerk
        t_derc_4 = calc_dc(ts(i+1),n_order,4);
        t_derc_5 = calc_dc(ts(i+1),n_order,5);
        t_derc_6 = calc_dc(ts(i+1),n_order,6);
        Aeq_x(neq+1,n_coef*(i-1)+1:n_coef*(i+1))=[t_derc_p,-t_derc_p];
        Aeq_x(neq+2,n_coef*(i-1)+1:n_coef*(i+1))=[t_derc_v,-t_derc_v];
        Aeq_x(neq+3,n_coef*(i-1)+1:n_coef*(i+1))=[t_derc_a,-t_derc_a];
        Aeq_x(neq+4,n_coef*(i-1)+1:n_coef*(i+1))=[t_derc_j,-t_derc_j];
        Aeq_x(neq+5,n_coef*(i-1)+1:n_coef*(i+1))=[t_derc_4,-t_derc_4];
        Aeq_x(neq+6,n_coef*(i-1)+1:n_coef*(i+1))=[t_derc_5,-t_derc_5];
        Aeq_x(neq+7,n_coef*(i-1)+1:n_coef*(i+1))=[t_derc_6,-t_derc_6];
        neq = neq + 7;
    end
	
  
    zeroM = zeros(size(Aeq_x)); 
    Aeq = [Aeq_x zeroM zeroM; zeroM Aeq_x zeroM; zeroM zeroM Aeq_x];
    
    %% compute Ax <= b
    A = [];
    b = [];
    ieq = 1;
    xll = xlen / dim;  % 1/3 of xlen,  = n_coef * (n_poly-1)
    for i = 1 : n_poly
        planesCell = decomp.lines_{i}.polyhedron_.polys_;
        [~, lenj] = size(planesCell);
        for j = 1 : lenj
            n = planesCell{j}.n_;
            p = planesCell{j}.p_;
            nx = zeros(1, xlen);
            % add (Denominator + 1) points on the poly's Ax<b 
            Denominator = 2;  % Denominator > 0
            for k = 0 : Denominator
                tvec_mid = calc_dc(ts(i) +  k*(ts(i+1) - ts(i))/Denominator, n_order, 0);   % [1*8]vector
                nx(1, 8*(i-1) + 1 : 8*(i-1) + 8)                = tvec_mid.*n(1);
                nx(1, 8*(i-1) + 1 + xll : 8*(i-1) + 8 + xll)    = tvec_mid.*n(2);
                nx(1, 8*(i-1) + 1 + 2*xll : 8*(i-1) + 8 + 2*xll)= tvec_mid.*n(3);
                A(ieq,:) = nx;
                b(ieq,:) = dot(n, p);
                % dot(plane.n, p1 - plane.p) < 0 
                % => dot(plane.n, p1) - dot(plane.n, plane.p) < 0 
                % => dot(plane.n, p1) < dot(plane.n, plane.p) 
                %% Ax < b
                ieq = ieq + 1;
            end
        end
    end
   
    
%     % Modify the number of iterations
    options = optimoptions('fmincon');
    options.MaxIterations = 2000;   % Defaults = 1000
    lb = []; ub = []; x0 = []; 
    p = quadprog(Q_all,f,A,b,Aeq,beq, lb,ub,x0, options);

% 	p = quadprog(Q_all,f,A,b,Aeq,beq);
    
    minValue = p'*Q_all*p;
	
    px = p(1:xll,1);
    py = p(xll + 1:2*xll,1);
    pz = p(xll*2 + 1:3*xll,1);
end



================================================
FILE: Motion_Planning/5Trajectory_Planning/TrajectoryPlanning.m
================================================
function [total_time, ts, X] = TrajectoryPlanning(path, decomp, time_allocation)
    speed   = time_allocation.avg_speed;
    acc     = time_allocation.acc;
    
    disp(['The planned speed is : ', num2str(speed)]);
    tic
    if strcmp(time_allocation.type, 'averageSpeed')
        [ts, total_time] = averageSpeed_ta(path0, speed);
    elseif strcmp(time_allocation.type, 'trapzoidSpeed')
        [ts, total_time] = trapezoidalSpeed_ta(path, speed, acc);
    end
    disp('TimeAllocation time is :');
    toc
    disp(['time management: total_time is ', num2str(total_time), 'seconds']);
    disp(['Split time is : ', num2str(ts)]);
    
    
    tic 
    %  use Ax=b get Trajectory planning. ===========================
    %  X = Ax_equal_b(path, total_time, ts);

    % use 'Quadratic Programming' and SFC(which make by Ax < b) get Trajectory planning. ========================
    % use SFC make Inequality constraints
    X = QPbyUseSFC(path, ts, decomp);

    disp('generator trajectory time is :');
    toc
end



================================================
FILE: Motion_Planning/5Trajectory_Planning/calc_Q.m
================================================
% Calculation Q_1 of Q_x of Q of p^TQp 
% n:Polynomial equation maximum order
% d:derivertive order, 1:minimum vel 2:minimum acc 3:minimum jerk 4:minimum snap
% t1:start timestamp for polynormial
% t2:end timestap for polynormial
function Q1 = calc_Q(n_order, d, t1, t2)
    T = zeros((n_order-d)*2+1,1);
    for n = 1:(n_order-d)*2+1
        T(n) = t2^n-t1^n;
    end
    Q1 = zeros(n_order);
    for i = d+1 : n_order+1
        for j = i : n_order+1
            c = i + j - 2*d - 1;
            Q1(i,j) = prod(i-d:i-1) * prod(j-d:j-1) / c * T(c);
            Q1(j,i) = Q1(i,j);
        end
    end

end

================================================
FILE: Motion_Planning/5Trajectory_Planning/calc_dc.m
================================================
% Calculation Derivative coefficient
% n_order = Polynomial equation maximum order
% d = derivertive order =  0:pos  1:vel  2:acc 3:jerk
function t_derC = calc_dc(t, n_order, d)
    t_derC = zeros(1,n_order+1);
    for i = d+1 : n_order+1
        t_derC(i) = prod(i-d:i-1) * t^(i-d-1);
    end
end


================================================
FILE: Motion_Planning/demo/demo_3dCorner.m
================================================
close all;
clear all;
clc;

demoActivate = true;
choose_map  = 2;   
path_id     = 2;

runsim

================================================
FILE: Motion_Planning/demo/demo_Z_3blocks.m
================================================
close all;
clear all;
clc;

demoActivate = true;
choose_map  = 3; 
path_id     = 2;

runsim

================================================
FILE: Motion_Planning/demo/demo_ellipsoid.m
================================================
close all;
clear all;
clc;

demoActivate = true;
choose_map  = 1;   
path_id     = 2;

runsim

================================================
FILE: Motion_Planning/draw/draw_ObcPoints.m
================================================
 hold on;
 len = length(obps);
 for i = 1 : len
     plot3(obps(i,1), obps(i,2), obps(i,3), '*', 'Color', 'r');
 end


 hold off;


================================================
FILE: Motion_Planning/draw/makeGifAndJpg.m
================================================
function makeGifAndJpg(fig_num)
    
    saveas(figure(4), 'postion','jpg');
    pause(0.1);
    saveas(figure(5), 'velocity','jpg');
    pause(0.1);
    filename = 'testAnimated6.gif';
    
    h = figure(fig_num);
    for i = 1:90
        view(4*i+15, 15);
        drawnow 

        % Capture the plot as an image 
        frame = getframe(h); 
        im = frame2im(frame); 
        [imind,cm] = rgb2ind(im,256); 

        % Write to the GIF File 
        if i == 1 
          imwrite(imind,cm,filename,'gif', 'Loopcount',inf); 
        else 
          imwrite(imind,cm,filename,'gif','WriteMode','append'); 
        end 
    end

end



================================================
FILE: Motion_Planning/draw/plot_path.m
================================================
function plot_path(paths, mapClass, SFCs)
% PLOT_PATH Visualize a path and sfc's ellipses
%   PLOT_PATH(path mapClass, SFCs) creates a figure showing a path through
%   the environment.  
%   SFCs provide functions for drawing ellipses: drawEllipsoids();

persistent map decomps init;
if isempty(init)
    map = mapClass;
    decomps = SFCs;
    init = 1;
end

figure('Name','Animation');

hold on;
for i = 1:size(map.blocks,1)
    block = map.blocks(i, :);
    drawBlock(block);
end

path = paths{1};

% draw Inflated obstacle
for i = 1:size(map.blocks,1)
    margin = map.margin;
    block = map.blocks(i, :);
    block(1,1:3) = block(1,1:3) - margin;
    block(1,4:6) = block(1,4:6) + margin;
    block(7) = max(150, block(7));  block(8) = max(150, block(8));   block(9) = max(150, block(9));     %% set color
    drawBlock(block);
end

if size(path,1) > 0
    % pcshow(path, [0,1,0],'MarkerSize', 0.1);
    % The lower left corner position, width and height. 260 here is exactly 7cm
%     set(gcf,'position', [500 500 260 220]);
%     set(gcf,'position', [500 500 400 400]);   % big pic
%     set(gcf,'position', [500 500 300 300]);   % small pic
    set(gcf,'color','w');
    set(gca,'color','w');
    
    %% Display the unit of x y z
%     xlabel('x length(m)');
%     ylabel('y length(m)');
%     zlabel('z length(m)');
    
end

% Set the axis range
axis_add = 2*map.res(1);
axis([map.boundary.ld(1)-axis_add, map.boundary.ru(1)+axis_add,   ... 
      map.boundary.ld(2)-axis_add, map.boundary.ru(2)+axis_add,   ...
      map.boundary.ld(3)-axis_add, map.boundary.ru(3)+axis_add])

hold on
for j = 1 : size(paths, 2)
    if j == 1
        % JPS's result
%         plot3(paths{j}(:,1), paths{j}(:,2), paths{j}(:,3),'-*','color','k','LineWidth',3);
    elseif j == 2
        % JPS() + simple path() result: 
%         plot3(paths{j}(:,1), paths{j}(:,2), paths{j}(:,3),'-*','color','#A2142F','LineWidth',3);
        plot3(paths{j}(:,1), paths{j}(:,2), paths{j}(:,3),'-*','color','k','LineWidth',3);
    end
end
hold off

set(gca,'DataAspectRatio',[0.1 0.1 0.1]);

% drawEllipsoid = true;
if (exist('drawEllipsoid') && (drawEllipsoid == true))
    decomps{1}.drawEllipsoids();
%     decomps{2}.drawEllipsoids();
end

grid on

view(30, 10);
hold off;

end

function drawBlock(block)
    
    x = [ones(4,1) * block(1); ones(4,1) * block(4)];
    y = [ones(2,1) * block(5); ones(2,1) * block(2); ones(2,1) * block(5); ones(2,1) * block(2)];
    z = [block(3);block(6);block(3);block(6);block(3);block(6);block(3);block(6)];


    vert = [x, y, z];
    fac = [1 2 6 5;2 3 7 6;3 4 8 7;4 1 5 8;1 2 3 4;5 6 7 8];
    c = block(7:9)/255;
    patch('Vertices',vert,'Faces',fac,...
          'FaceVertexCData',hsv(6),'FaceColor',c,'FaceAlpha',.2);

    
    x = [ones(4,1) * block(1); ones(4,1) * block(4)];
    y = [block(2);block(5);block(2);block(5);block(2);block(5);block(2);block(5)];
    z = [ones(2,1) * block(3); ones(2,1) * block(6); ones(2,1) * block(3); ones(2,1) * block(6)];

    vert = [x, y, z];
    fac = [1 2 6 5;2 3 7 6;3 4 8 7;4 1 5 8;1 2 3 4;5 6 7 8];
    c = block(7:9)/255;
    patch('Vertices',vert,'Faces',fac,...
          'FaceVertexCData',hsv(6),'FaceColor',c,'FaceAlpha',.2);
end

================================================
FILE: Motion_Planning/init_data.m
================================================
start = {[]};
stop  = {[]};

if (~exist('demoActivate')) || (demoActivate == false)
    choose_map  = 2;
end
demoActivate    = false;
if (~exist('path_id'))
    path_id     = 2;            % Path planning: 2 = JPS, 4 = JPS-PF [This feature has not been uploaded yet]
end
SFC_id      = path_id/2;        % Path planning: 2 = path_id(4)'s SFC, 1 = path_id(2)'s SFC
speed       = 1;
acc         = 0;

time_allocation.type = 'trapzoidSpeed';
%time_allocation.type = 'averageSpeed';

switch choose_map
   case 1   %% 3 blocks
      map.load_map('0Maps/ellipsoid.txt', 0.2, 0.2, 0.1);
      start = {[1.5 0.2 0.2]};
      stop  = {[1.5 2.0 1.9]};
      speed = 1;
      acc   = 2; 
   case 2
      map.load_map('0Maps/3dCorner.txt', 0.1, 0.1, 0.1);
      start = {[2.6  1.0 1.0]};
      stop  = {[2.5  3.0 2.6]};
      acc   = 2;        % acceleration 
      if path_id == 4
          speed = 1.0767; % for path_id = 4, time = 4.3682, snap = 350.3038, fly use  4.1s
      else
          speed = 0.8; % for path_id = 2, time = 4.3681, snap = 895.7681, fly use  4.15s
      end
   case 3   %% Z_3blocks
      map.load_map('0Maps/Z_3blocks.txt', 0.2, 0.2, 0.1);
      start = {[-1 1.5 2]};
      stop  = {[11 1.5 1]};
      acc   = 2;
      if path_id == 4
          speed = 1.4;      %for path_id = 4, time = 12.9343, snap = 58.376, fly use  12.45s
      else
          speed = 1.10423;  %for path_id = 2, time = 12.9343, snap = 300.3111, fly use  13.9s  
      end

    otherwise 
end

% Must be after acc assignment above
time_allocation.avg_speed   = speed;
time_allocation.acc         = acc;


================================================
FILE: Motion_Planning/runsim.m
================================================
% if demoActivate exist and equal true, not clean all variable.
% runsim.m called by the demo,  not clean all variable
% runsim.m called by itself,  clean all variable
if (~exist('demoActivate')) || (demoActivate == false)
    close all;
    clear all;
    clc;
end

addpath(genpath('./'));


%% map generation
map = GridMap();
init_data
map.flag_obstacles();


%% path planning
nquad = length(start);
for qn = 1:nquad    
    disp('JPS time is :');
    tic
    path{qn} = JPS_3D(map, start{qn}, stop{qn});
    path{qn} = [start{qn}; path{qn}(1:end,:)];
    path{qn}(end + 1,:) = stop{qn};
    toc
end

%% delete the points of no-use
path{2} = simplify_path(map, path{1});

%% Generating Convex Polytopes
obps = PointCloudMap(map.blocks, map.margin);   % blocks of Metric Map change to point cloud

decomps{2} = []
disp('JPS -> SFC time is :');
tic
decomps{1} = SFC_3D(path{2}, obps, map.boundary); %  call SFC
toc

path{path_id}


%% draw path and blocks
if nquad == 1
    plot_path(path, map, decomps); 
else
    % you could modify your plot_path to handle cell input for multiple robots
end

% draw_ObcPoints
% makeGifAndJpg(1);     %figure(1): Graph without trajectory

%% Trajectory planning
[t_time, ts_par, x_par] = TrajectoryPlanning(path{path_id}, decomps{SFC_id}, time_allocation);


%% Trajectory tracking
disp('Generating Trajectory ...');
trajectory_generator([], [], path{path_id}, t_time, ts_par, x_par);
trajectory = test_trajectory(start, stop, path, true); % with visualization
disp('Blue line is Trajectory planning.');
disp('Red line is Trajectory tracking.');

%% Gif
% makeGifAndJpg(3);     %figure(3): Graph with trajectory


================================================
FILE: Motion_Planning.md
================================================
# Motion Planning

A new method to improve path planning

<img src="gifs/pro2/head.gif" alt="polyhedron-1" width="700">



- Path planning algorithms (Jump point search,  JPS-PF )

- Generating Convex Polytopes(Safe Flight Corridors)

- Time Allocation(Average time allocation, Trapezoidal time allocation)

- Trajectory planning(QP)

- Trajectory following control(PD controller)



## Required

MATLAB(R2019b is tested)



## Demo

Run the demo map script (Only three maps have been released so far) ：

```
demo_3dCorner.m
demo_ellipsoid.m
demo_Z_3blocks.m
```



### Map: 3dCorner

- Same starting point and ending point

- same total planning time, use trapezoidal time allocation

| Method     | JPS                                                          | JPS-PF                                                       |
| ---------- | ------------------------------------------------------------ | ------------------------------------------------------------ |
| trajectory | <img src="gifs/pro2/3dCorner-jps-ys.gif" alt="3dCorner-jps-ys" width="350"> | <img src="gifs/pro2/3dCorner-new-ys.gif" alt="3dCorner-new-ys" width="350"> |
| SFC        | <img src="gifs/pro2/3dCorner-jps-sfc.gif" alt="3dCorner-jps-sfc" width="350"> | <img src="gifs/pro2/3dCorner-new-sfc.gif" alt="3dCorner-new-sfc" width="350"> |
| Avg_speed  | 0.8                                                          | 1.0767                                                       |
| Time       | 4.3681                                                       | 4.3681                                                       |
| miniSnap   | 895.7681                                                     | 350.3038                                                     |

 

### Map: Z_3blocks

| Method     | JPS                                                          | JPS-PF                                                       |
| ---------- | ------------------------------------------------------------ | ------------------------------------------------------------ |
| trajectory | <img src="gifs/pro2/Z_3blocks.gif" alt="Z_3blocks" width="350"> | <img src="gifs/pro2/Z_3blocks-new.gif" alt="Z_3blocks-new" width="350"> |
| Top view   | <img src="imgs/pro2/p2.png" alt="Z_3blocks" width="350">     | <img src="imgs/pro2/p1.png" alt="Z_3blocks-new" width="350"> |
| Avg_speed  | 1.10423                                                      | 1.4                                                          |
| Time       | 12.9343                                                      | 12.9343                                                      |
| miniSnap   | 300.3111                                                     | 58.376                                                       |



## Reference

##### Paper:

[1] D. Harabor and A. Grastien. 2011. "**Online Graph Pruning for Pathfinding on Grid Maps**". In Proceedings of the 25th National Conference on Artificial Intelligence (AAAI), San Francisco, USA.

[2] S. Liu, M. Watterson, K. Mohta, K. Sun, S. Bhattacharya, C.J. Taylor, et al., "**Planning dynamically feasible trajectories for quadrotors using safe flight corridors in 3-d complex environments**", IEEE Robotics and Automation Let- ters, vol. 2, no. 3, pp. 1688-1695, July 2017.

[3] D.W.Mellinger,"**Trajectory generation and control for quadrotors**"Ph.D. dissertation, Univ. Pennsylvania, Philadelphia, PA, 2012.

[4] D. Mellinger and V. Kumar, "**Minimum snap trajectory generation and control for quadrotors**", inProc. 2011 IEEE Int. Conf. Robot.Autom.,2011

[5] T. Lee, M. Leoky, and N. H. McClamroch, "**Geometric tracking control of a quadrotor UAV on SE (3)**" in *Proc. 49th IEEE Conf. Decis. Control*. IEEE, 2010, pp. 5420–5425.



================================================
FILE: README.md
================================================
# AerialRobotics
Simulate the path planning and trajectory planning of quadrotors/UAVs.



## Motion Planning project
#### The main purpose:
- A new method to improve path planning

- Compare JPS and new method

#### Functions already implemented:

- Path planning algorithms (Jump point search,  JPS-PF )

- Generating Convex Polytopes(Safe Flight Corridors)

- Time Allocation(Average time allocation, Trapezoidal time allocation)

- Trajectory planning(QP)

- Trajectory following control(PD controller)

The more detailed data is in [Motion_Planning.md](Motion_Planning.md)

#### Demo animation: 

<img src="gifs/pro2/head.gif" alt="polyhedron-1" width="700">

<img src="gifs/pro2/3dCorner-new-ys.gif" alt="3dCorner-new-ys" width="550">

## Trajectory Planning project
#### The main purpose:
- Compare different pathfinding algorithms(Dijkstra,  Astar,  Jump point search)
- Compare different trajectory planning methods(Ax = b, QP)

#### Functions already implemented:
- Path planning algorithms (Dijkstra,  Astar,  Jump point search)
- Generating Convex Polytopes(Safe Flight Corridors)
- Trajectory planning(Ax = b, QP)
- Trajectory following control(PD controller)

The more detailed data is in [Trajectory_Planning.md](Trajectory_Planning.md)

#### Demo animation: 

<img src="gifs/pro1/head.gif" alt="polyhedron-1" width="400">







**More related knowledge introduction on  [Wiki](https://github.com/LenaShengzhen/AerialRobotics/wiki)**

**New feature schedule on [projects](https://github.com/LenaShengzhen/AerialRobotics/projects/2)**

**Bug and issue tracking on [projects](https://github.com/LenaShengzhen/AerialRobotics/projects/1)**





## Reference

[1] D. Harabor and A. Grastien. 2011. "**Online Graph Pruning for Pathfinding on Grid Maps**". In Proceedings of the 25th National Conference on Artificial Intelligence (AAAI), San Francisco, USA.

[2] R. Deits and R. Tedrake, "**Computing large convex regions of obstacle-free space through semidefinite programming**",in Algorithmic Foundations of Robotics XI. Berlin, Germany: Springer, 2015, pp. 109–124.

[3] S. Liu, M. Watterson, K. Mohta, K. Sun, S. Bhattacharya, C.J. Taylor, et al., "**Planning dynamically feasible trajectories for quadrotors using safe flight corridors in 3-d complex environments**", IEEE Robotics and Automation Let- ters, vol. 2, no. 3, pp. 1688-1695, July 2017.

[4] S. Savin, "**An algorithm for generating convex obstacle-free regions based on stereographic projection**,” in International Siberian Confer- ence on Control and Communications, 2017.

[5] Xingguang Zhong, Yuwei Wu, Dong Wang, Qianhao Wang, Chao Xu, and Fei Gao, "**Generating Large Convex Polytopes Directly on Point Clouds**",2020

[6] D.W.Mellinger,"**Trajectory generation and control for quadrotors**"Ph.D. dissertation, Univ. Pennsylvania, Philadelphia, PA, 2012.

[7] D. Mellinger and V. Kumar, "**Minimum snap trajectory generation and control for quadrotors**", inProc. 2011 IEEE Int. Conf. Robot.Autom.,2011

[8] C. Richter, A. Bry, and N. Roy, “**Polynomial trajectory planning for aggressive quadrotor flight in dense indoor environments**,” in *Proc. Int. Symp. Robot. Res.*, 2016.

[9] T. Lee, M. Leoky, and N. H. McClamroch, "**Geometric tracking control of a quadrotor UAV on SE (3)**", in *Proc. 49th IEEE Conf. Decis. Control*. IEEE, 2010, pp. 5420–5425.

[10] Daniel Mellinger, Alex Kushleyev and Vijay Kumar,"**mixed-integer quadratic program trajectory generation for heterogeneous quadrotor teams**", IEEE, 2012

================================================
FILE: Trajectory_Planning.md
================================================
# Trajectory Planning



<img src="gifs/pro1/head.gif" alt="polyhedron-1" width="400">



- Path planning algorithms (Dijkstra,  Astar,  Jump point search)
- Generating Convex Polytopes(Safe Flight Corridors)
- Trajectory planning(Ax = b, QP)
- Trajectory following control(PD controller)



## Required

MATLAB(R2019b is tested)



## Pic



### 1. Path Planning

| dijkstra                                                 | Astar                                              | Jump Point Search                              |
| -------------------------------------------------------- | -------------------------------------------------- | ---------------------------------------------- |
| <img src="gifs/pro1/dijkstra.gif" alt="dijkstra" width="270"> | <img src="gifs/pro1/Astar.gif" alt="Astar" width="270"> | <img src="gifs/pro1/JPS.gif" alt="JPS" width="270"> |
| 1.642271 seconds                                         | 0.755504 seconds                                   | 0.451664 seconds                               |

 

### 2. Generating Convex Polytopes

### 	Safe Flight Corridors

- ####  find_ellipsoid

  <img src="imgs/pro1/ellipsoid-1.png" alt="polyhedron-1" width="400"><img src="imgs/pro1/ellipsoid-2.png" alt="polyhedron-2" width="400">

- #### find_polyhedron

<img src="gifs/pro1/polyhedron-1.gif" alt="polyhedron-1" width="400"><img src="gifs/pro1/polyhedron-2.gif" alt="polyhedron-2" width="400">



### 3. Trajectory planning

Compare different Trajectory planning：

- #### Trajectory 1: use Ax = b get Trajectory.

<img src="gifs/pro1/closeForm.gif" alt="closeForm" width="270"><img src="imgs/pro1/close_postion.jpg" alt="closeForm" width="270"><img src="imgs/pro1/close_velocity.jpg" alt="closeForm" width="270"> 

The Trajectory pass every Path point.

- #### Trajectory 2: use 'Quadratic Programming' get Trajectory. use corridor constraints make  Ax< b. x y z separately find minimum-snap.

<img src="gifs/pro1/corridorConstraints.gif" alt="corridorConstraints" width="270"><img src="imgs/pro1/corridorConstraints_postion.jpg" alt="corridorConstraints" width="270"><img src="imgs/pro1/corridorConstraints_velocity.jpg" alt="corridorConstraints" width="270"> 

The Trajectory don't need to pass every Path point.

minSnapValue(X + Y + Z) is : 9376.0901



- #### Trajectory 3: use 'Quadratic Programming' get Trajectory. use SFC make Ax < b.

<img src="gifs/pro1/SFC.gif" alt="SFC" width="270"><img src="imgs/pro1/SFC_postion.jpg" alt="SFC" width="270"><img src="imgs/pro1/SFC_velocity.jpg" alt="SFC" width="270"> 

The Trajectory don't need to pass every Path point.

minSnapValue(X + Y + Z) is : 2958.5877



### 4. Trajectory following control

Trajectory following by use PD Controller, you can learn by [coursera](https://www.coursera.org/learn/robotics-flight/home/welcome).



## Reference

##### Paper:

[1] D. Harabor and A. Grastien. 2011. "**Online Graph Pruning for Pathfinding on Grid Maps**". In Proceedings of the 25th National Conference on Artificial Intelligence (AAAI), San Francisco, USA.

[2] S. Liu, M. Watterson, K. Mohta, K. Sun, S. Bhattacharya, C.J. Taylor, et al., "**Planning dynamically feasible trajectories for quadrotors using safe flight corridors in 3-d complex environments**", IEEE Robotics and Automation Let- ters, vol. 2, no. 3, pp. 1688-1695, July 2017.

[3] D.W.Mellinger,"**Trajectory generation and control for quadrotors**"Ph.D. dissertation, Univ. Pennsylvania, Philadelphia, PA, 2012.

[4] D. Mellinger and V. Kumar, "**Minimum snap trajectory generation and control for quadrotors**", inProc. 2011 IEEE Int. Conf. Robot.Autom.,2011

[5] T. Lee, M. Leoky, and N. H. McClamroch, "**Geometric tracking control of a quadrotor UAV on SE (3)**" in *Proc. 49th IEEE Conf. Decis. Control*. IEEE, 2010, pp. 5420–5425.

##### Code:

safe flight corridors: [C++ version](https://github.com/sikang/DecompUtil)