taekkeun/AerialRobotics

Motion_Planning/0Maps/forest_environment.txt

@@ -0,0 +1,13 @@
+# map forest environment
+boundary 0.0 0.0 0.0 5 5 3.0
+
+block 1.0 0.5 0.1       1.3 0.8 3.0      0.000000 255.000000 0.000000
+block 0 1.5 0           0.3 1.8  3.0     0.000000 255.000000 0.000000
+block 2.0 1.5 0.1       2.3 1.8 3.0      0.000000 255.000000 0.000000
+block 1.0 3 0.1         1.3 3.3 3.0      0.000000 255.000000 0.000000
+block 3 3 0.1           3.3 3.3 3.0      0.000000 255.000000 0.000000
+block 0 4.5 0.1         0.3 4.8 3.0      0.000000 255.000000 0.000000
+block 2 4.5 0.1         2.3 4.8 3.0      0.000000 255.000000 0.000000
+
+
+

Motion_Planning/0Maps/urban_environment.txt

@@ -0,0 +1,15 @@
+# map urban environment
+boundary 0.0 0.0 0.0 40 40 3.0
+
+block 32 5 0            35  8   3.0      0 0 255
+block 28 8 0            35  9.5 3.0      0 0 255
+block 25 5 0            28  9.5 3.0      0 0 255
+block 5 13.5 0           15  29.5 3.0    255 0 0
+block 5 2 0           12.5  9.5 3.0       0 0 255
+block 25 13.5 0            35  25.5   3.0      0 0 255
+
+
+
+
+
+

Motion_Planning/3Safe_Flight_Corridors/LineSegment.m

@@ -27,7 +27,7 @@ function set_local_bbox(obj, local_bbox)
         end		
 
         function set_obs(obj, obs)
-            Vs = Polyhedron();
+            Vs = Polyhedron_();
             obj.add_local_bbox(Vs);		
             obj.obs_ = Vs.points_inside(obs);	
         end		
@@ -144,7 +144,7 @@ function find_ellipsoid(obj, offset_x)
         end
 
         function find_polyhedron(obj)
-            Vs = Polyhedron();
+            Vs = Polyhedron_();
             obs_remain = obj.obs_;
             while(length(obs_remain))
                 plane = obj.ellipsoid_.closest_hyperplane(obs_remain);

Motion_Planning/3Safe_Flight_Corridors/Polyhedron.m → Motion_Planning/3Safe_Flight_Corridors/Polyhedron_.m

@@ -1,24 +1,25 @@
-classdef Polyhedron < handle
+classdef Polyhedron_ < handle
     properties               
         polys_;    
         epsilon_;  
     end
     methods                 
-        function obj = Polyhedron()   
+        function obj = Polyhedron_()   
             obj.polys_ = cell(0);
             obj.epsilon_ = 1e-10;
         end
 
         % Append Hyperplane
         function add(obj, plane)
+            % Does normal vector of the plane point inside or outside?
             obj.polys_{end+1} = plane;
         end
         % Check if the point is inside polyhedron,
         function isinside = inside(obj, pt)
             isinside = false;
-            [len, ~]=size(obj.polys_);
+            [~, len]=size(obj.polys_);
                 for i = 1 : len
-                    if(obj.polys_{i}.signed_dist(pt) > obj.epsilon_)
+                    if(obj.polys_{i}.signed_dist(pt) < -obj.epsilon_)
                         return;
                     end
                 end

Motion_Planning/6Star_Convex/LargeConvexPolytopes.m

@@ -0,0 +1,70 @@
+function [A, b] = LargeConvexPolytopes(pointclouds_xyz, pos, R)
+    % Given a set of pointclouds coordinates n*3 in 3d, and the current position.
+    % With the user defined radius R as well.
+    % Return a convex region denoted by Ax <= b
+    dim = length(pos);
+    assert(dim == 2 || dim == 3, "Wrong dimension! Check input data!");
+
+    points_inside = FindInsidePoints(pointclouds_xyz, pos, R);
+
+%     pointclouds_xyz = AddSurroundingPoints(pointclouds_xyz, pos, R);
+    flipping_xyz = zeros(size(points_inside));
+    for i = 1: size(points_inside, 1)
+        flipping_xyz(i, :) = SphereFlipping(pos, points_inside(i, :), R);
+    end
+    k = convhull(flipping_xyz);
+    if dim == 2
+        k = k(1:end-1);
+        sc = StarConvex(points_inside(k, 1),...
+            points_inside(k, 2), pos);
+    else
+        k = unique(k);
+        sc = StarConvex(points_inside(k, 1), points_inside(k, 2),...
+            pos, points_inside(k, 3));
+    end
+    sc.ConstructConvexFromStar();
+    sc.ShrinkToConvex();
+    A = sc.A;
+    b = sc.b;
+%     sc.VisualizeResult();
+end
+
+function points_inside = FindInsidePoints(pointclouds_xyz, pos, R)
+    % Find all the points inside the circle with R as radius and pos as the
+    % center.
+    distance = vecnorm(pointclouds_xyz - pos, 2, 2);
+    points_inside = pointclouds_xyz(distance <= R, :);
+    points_inside = AddSurroundingPoints(points_inside, pos, R);
+end
+
+function p_xyz = AddSurroundingPoints(pointclouds_xyz, pos, R)
+    % If there are not enough surrounding points, add surrounding points
+    % manually. For 2d, add them as a square, for 3d, add them as a cube
+    dim = length(pos);
+    if dim == 2
+        % Add four points as a square.
+        rc = R / sqrt(2);
+        pointclouds_xyz(end+1, :) = [pos(1)+rc, pos(2)+rc];
+        pointclouds_xyz(end+1, :) = [pos(1)-rc, pos(2)+rc];
+        pointclouds_xyz(end+1, :) = [pos(1)-rc, pos(2)-rc];
+        pointclouds_xyz(end+1, :) = [pos(1)+rc, pos(2)-rc];
+    else
+        % Add eight points as a cube.
+        rc = R / sqrt(3);
+        pointclouds_xyz(end+1, :) = [pos(1)+rc, pos(2)+rc, pos(3)-rc];
+        pointclouds_xyz(end+1, :) = [pos(1)+rc, pos(2)-rc, pos(3)+rc];
+        pointclouds_xyz(end+1, :) = [pos(1)-rc, pos(2)+rc, pos(3)+rc];
+        pointclouds_xyz(end+1, :) = [pos(1)+rc, pos(2)-rc, pos(3)-rc];
+        pointclouds_xyz(end+1, :) = [pos(1)-rc, pos(2)+rc, pos(3)-rc];
+        pointclouds_xyz(end+1, :) = [pos(1)-rc, pos(2)-rc, pos(3)+rc];
+        pointclouds_xyz(end+1, :) = [pos(1)-rc, pos(2)-rc, pos(3)-rc];
+        pointclouds_xyz(end+1, :) = [pos(1)+rc, pos(2)+rc, pos(3)+rc];
+%         disp(pointclouds_xyz(end-7:end, :));
+    end
+    p_xyz = pointclouds_xyz;
+end
+
+function p2 = SphereFlipping(pos, p1, R)
+    % Sphere flipping p1 from pos by a radius R to p2
+    p2 = pos + (p1 - pos) * (2 * R - norm(p1 - pos)) / norm(p1 - pos);
+end

Motion_Planning/6Star_Convex/SCMFromPath.m

@@ -0,0 +1,56 @@
+function [A, b] = SCMFromPath(path, point_cloud_obs, map_boundary)
+% LargeConvexPolytopes generates A and b from a single point, SCMFrom path
+% generate A and b from a path.
+    map_size = min(abs(map_boundary.ld - map_boundary.ru));
+%     fprintf("Map size is: %f \n", map_size);
+    R = map_size/2;
+    tolerance = 0.01;
+    A = {};
+    b = {};
+    ieq = 1;
+    n_node = size(path, 1) - 1;
+    pos = path(1, :);
+    [cur_A, cur_b] = LargeConvexPolytopes(point_cloud_obs, pos, R);
+    A{1} = cur_A;
+%     cur_b = cur_A * pos' + cur_b;
+    b{1} = cur_b;
+    for i = 1:n_node
+%         fprintf("Node number %d \n", i);
+%         if i == 7
+%             ;
+%         end
+        cur_pt = path(i, :);
+        next_pt = path(i+1, :);
+%         disp(cur_pt);
+%         disp(next_pt);
+        while any(cur_A * next_pt' >= cur_b)
+            if any(cur_A * cur_pt' >= cur_b)
+                disp("Wrong! Current point is not in the polytope!");
+            end
+%             fprintf("Checking node number %d \n", i);
+            start = 0;
+            goal = 1;
+            distance = norm(next_pt - cur_pt);
+%             fprintf("Distance between start and goal is %f .\n", distance);
+            while distance*(goal - start) > tolerance
+                mid = start + (goal - start) / 2;
+                mid_pt = cur_pt + (next_pt - cur_pt) * mid;
+%                 fprintf("Start is %f, middle is %f, goal is %f.\n", start, mid, goal);
+                if any(cur_A * mid_pt' >= cur_b)
+                    goal = mid;
+                else
+                    start = mid;
+                end
+            end
+            cur_pt = cur_pt + (next_pt - cur_pt) * goal;
+            [cur_A, cur_b] = LargeConvexPolytopes(point_cloud_obs, cur_pt, R);
+            if any(cur_A * cur_pt' >= cur_b)
+                disp("Wrong! Current point is not in the polytope!");
+            end
+            A{ieq+1} = cur_A;
+%             cur_b = cur_A * cur_pt' + cur_b;
+            b{ieq+1} = cur_b;
+            ieq = ieq + 1;
+        end
+    end
+end