[ { "path": ".gitmodules", "content": "[submodule \"Motion_Planning/QuadrotorSimulation\"]\n\tpath = Motion_Planning/QuadrotorSimulation\n\turl = https://github.com/LenaShengzhen/QuadrotorSimulation.git\n" }, { "path": "Motion_Planning/0Maps/3dCorner.txt", "content": "# map 3dCorner\nboundary 0.0 0.0 0.0 3.1 3.1 3.0\n\n# downblock\nblock 2.0 1.5 1.8 3.0 2.5 3.0 30.00 144.00 255.00\n\n# midblock\nblock 2.3 1.5 1.4 3.0 2.5 1.8 30.00 144.00 255.00\n\n# upblock\nblock 2.0 1.5 0.1 3.0 2.5 1.4 30.00 144.00 255.00\n\n\n\n\n" }, { "path": "Motion_Planning/0Maps/Z_3blocks.txt", "content": "# map Z_3blocks: Turn left and right\n\nboundary -2.00 0.0 0.00 14.00 3 3.50\n\nblock 2.25 0 0 2.65 1.9 3.4 30.00 144.00 255.00\n\n\nblock 0.05 1.1 0 0.45 3 3.4 0.00 191.00 255.00\nblock 4.5 1.1 0 4.9 3 3.4 0.00 191.00 255.00\n\n\n" }, { "path": "Motion_Planning/0Maps/ellipsoid.txt", "content": "# map ellipsoid\nboundary 0.0 0.0 0.0 3.1 3.1 3.0\n\nblock 1.0 0.5 0.1 2.0 0.7 3.0 0.000000 255.000000 0.000000\nblock 0 1.5 0 1.0 1.7 3.0 0.000000 255.000000 0.000000\nblock 2.0 1.5 0.1 3.0 1.7 3.0 0.000000 255.000000 0.000000\n\n\n" }, { "path": "Motion_Planning/1Map_Generation/GridMap.m", "content": "classdef GridMap < handle\n properties\n boundary; % size of Metric Map(leftdownPoint + rightupPoint)\n blocks; % coordinate of obstacles on the Metric Map\n res; % resolution\n margin;\n maxIndex; % max index of ijk of GridMap\n occ_map; % flag obstacles on the GridMap\n end\n methods \n function obj = GridMap()\n end\n \n function load_map(obj, filename, xy_res, z_res, margin)\n fid = fopen(filename);\n tline = fgets(fid);\n obj.blocks = [];\n while ischar(tline)\n % skip empty line and comments\n if numel(tline) > 1 & tline(1)~='#'\n % convert char array to string\n if strncmpi(tline, 'boundary', 8)\n boundarys = strread(tline(9:end));\n end\n if strncmpi(tline, 'block', 5)\n block = strread(tline(6:end));\n assert(size(block,2) == 9);\n obj.blocks = [obj.blocks; block];\n end\n end\n tline = fgets(fid);\n end\n obj.boundary.ld = boundarys(1:3);\n obj.boundary.ru = boundarys(4:6);\n obj.res = [xy_res xy_res z_res];\n obj.margin = margin;\n obj.maxIndex = ceil(abs( (obj.boundary.ru - obj.boundary.ld)./obj.res )); \n end\n \n % obstacles = block + margin\n function flag_obstacles(obj)\n obj.occ_map = false(obj.maxIndex); \n for i = 1 : size(obj.blocks,1)\n block = obj.blocks(i, :);\n leftdownPoint = [min(block(1), block(4)) min(block(2), block(5)) min(block(3), block(6))];\n rightUpPoint = [max(block(1), block(4)) max(block(2), block(5)) max(block(3), block(6))];\n obj.blocks(i, 1:3) = leftdownPoint;\n obj.blocks(i, 4:6) = rightUpPoint;\n xyzs = obj.points_to_idx(leftdownPoint - obj.margin);\n xyze = obj.points_to_idx(rightUpPoint + obj.margin);\n obj.occ_map(xyzs(1):xyze(1), xyzs(2):xyze(2), xyzs(3):xyze(3)) = 1;\n end\n end\n \n function ijk = points_to_idx(obj, xyz)\n assert(size(xyz,1) > 0);\n ijk = floor((xyz - obj.boundary.ld)./obj.res + 1);\n ijk = min(max(ijk, 1), obj.maxIndex);\n end\n \n function xyz = idx_to_points(obj, ijk)\n assert(size(ijk,1) > 0);\n xyz = obj.boundary.ld + (ijk - 1 + 0.5).* obj.res;\n end\n \n function ret = idOverflowCheck(obj, i, j, k)\n ret = (i < 1 || i > obj.maxIndex(1) || j < 1 || j > obj.maxIndex(2) || k < 1 || k > obj.maxIndex(3));\n end\n \n % check points collide\n function C = collide(obj, points)\n ijk = obj.points_to_idx(points);\n C = false(size(ijk, 1), 1);\n for i = 1 : size(ijk, 1)\n id = ijk(i,:);\n C(i) = obj.occ_map(id(1), id(2), id(3)) ~= 0;\n end\n end\n \n %% generate intermedia points\n function pts = generateiIntermediaPoints(obj, start_pos, end_pos)\n %% generate Tiny direction vector\n dd = end_pos - start_pos;\n dir = obj.res(1)*(dd/norm(dd, 2))/(100);\n \n %% generate intermedia points\n lenxy = sqrt(sum((start_pos(1:2) - end_pos(1:2)).^2));\n lenz = sqrt(sum((start_pos(3) - end_pos(3)).^2));\n n_pts = int32(max(lenxy/obj.res(1), lenz/obj.res(3))) + 3;\n pts = [linspace(start_pos(1), end_pos(1), n_pts);\n linspace(start_pos(2), end_pos(2), n_pts);\n linspace(start_pos(3), end_pos(3), n_pts)]';\n \n %% mid-point + Tiny direction vector \n for j = 2 : size(pts,1) - 1\n m1 = mod(pts(j,1), obj.res(1));\n m2 = mod(pts(j,2), obj.res(1));\n m3 = mod(pts(j,3), obj.res(1));\n if(m1*m2*m3 == 0)\n pts(j,:) = pts(j,:) + dir;\n end\n end\n end\n end\nend\n\n" }, { "path": "Motion_Planning/1Map_Generation/PointCloudMap.m", "content": "% blocks of Metric Map change to point cloud\nfunction obps = PointCloudMap(blocks, margin)\n obps = [];\n density = 2 * margin;\n for id = 1 : size(blocks, 1)\n leftdownPoint = blocks(id, 1:3) - margin;\n rightUpPoint = blocks(id, 4:6) + margin;\n plane_Z = [];\n for i = leftdownPoint(1,1) : density : rightUpPoint(1,1)\n for j = leftdownPoint(1,2) : density : rightUpPoint(1,2)\n pz = leftdownPoint(1,3);\n plane_Z = [plane_Z; [i, j, pz]];\n end\n end\n \n plane_Y = [];\n for i = leftdownPoint(1,1) : density : rightUpPoint(1,1)\n for k = leftdownPoint(1,3) : density : rightUpPoint(1,3)\n py = leftdownPoint(1,2);\n plane_Y = [plane_Y; [i, py, k]];\n end\n end\n \n plane_X = [];\n for j = leftdownPoint(1,2) : density : rightUpPoint(1,2)\n for k = leftdownPoint(1,3) : density : rightUpPoint(1,3)\n px = leftdownPoint(1,1);\n plane_X = [plane_X; [px, j, k]];\n end\n end\n \n long = rightUpPoint(1,1) - leftdownPoint(1,1);\n width = rightUpPoint(1,2) - leftdownPoint(1,2);\n high = rightUpPoint(1,3) - leftdownPoint(1,3);\n obps = [obps; plane_X; plane_X + [long 0 0];\n plane_Y; plane_Y + [0 width 0];\n plane_Z; plane_Z + [0 0 high]; ];\n end\nend\n\n" }, { "path": "Motion_Planning/2Path_Planning/JPS_3D.m", "content": "function [path] = JPS_3D(map, start, goal)\n \n map_start = map.points_to_idx(start);\n map_goal = map.points_to_idx(goal);\n \n map_visit = map.occ_map;\n \n fn = [0]; % fn = gn(EuclideanDistance) + hn(ManhattanDistance)\n all_direction = make_all_dir();\n nodeStart = struct( ...\n 'pos', map_start, ... % index of the point on the grid-map\n 'fa', 1, ... % fa records the index of the parent node in the closelist\n 'id', 1, ... % id records the index of node in closelist\n 'dir', all_direction);\n \n openlist = [nodeStart];\n closelist = [nodeStart];\n map_visit(map_start(1), map_start(2), map_start(3)) = 1;\n \n findgoal = 0;\n while isempty(openlist) == 0\n \n [~, temid] = min(fn);\n node = openlist(temid);\n \n fn(temid) = [];\n openlist(temid) = [];\n fa = node.id;\n \n [e1, ~] = size(node.dir);\n for j = 1 : e1\n nodeNew = jump(map, map_goal, node.pos, node.dir(j,:));\n if (length(nodeNew.pos) == 0 )\n continue;\n end\n if (map_visit(nodeNew.pos(1), nodeNew.pos(2), nodeNew.pos(3)) == 0)\n nodeNew.fa = fa;\n nodeNew.id = length(closelist) + 1; \n ds = ManhattanDistance(map_goal, nodeNew.pos) + EuclideanDistance(node.pos, nodeNew.pos);\n fn = [fn ds];\n openlist = [openlist; nodeNew];\n closelist = [closelist; nodeNew];\n map_visit(nodeNew.pos(1), nodeNew.pos(2), nodeNew.pos(3)) = 1;\n \n % find the goal\n if (nodeNew.pos == map_goal)\n findgoal = 1;\n break;\n end\n end\n end\n % find the goal\n if (findgoal == 1)\n break;\n end\n end\n \n \n path = [];\n k = length(closelist);\n while k > 1\n renode = closelist(k);\n float_pos = map.idx_to_points(renode.pos); \n path = [float_pos; path];\n k = renode.fa;\n end\nend\n\nfunction dis = EuclideanDistance(pos1, pos2)\n dis = norm(pos1 - pos2, 2);\nend\n\nfunction dis = ManhattanDistance(pos1, pos2)\n dis = norm(pos1 - pos2, 1);\nend\n\nfunction ret = obCheck(map, pos)\n x = pos(1); y = pos(2); z = pos(3);\n ret = map.occ_map(x, y, z);\nend\n\nfunction all_direction = make_all_dir()\n x = [1 1 1 -1 -1 -1 0 0];\n b = unique(nchoosek(x,3),'rows'); \n A = []; \n for i = 1 : 9 \n\t A = [A; perms(b(i,:))]; \n end\n all_direction = unique(A,'rows'); \nend\n\nfunction newnode = jump(map, goal, startpos, d)\n \n newnode = struct( ...\n 'pos', [], ...\n 'fa', 1, ...\n 'id', 1, ...\n 'dir', []);\n \n \n dir_Dimension = sum(d.^2);\n \n if (dir_Dimension == 0)\n return;\n end\n \n newpos = step(startpos, d);\n x = newpos(1); y = newpos(2); z = newpos(3);\n % ourside the grid on the newpos\n if outmap(map, newpos) == 1\n return;\n % meet the obstacle on the newpos \n elseif map.occ_map(x, y, z) == 1\n return;\n elseif newpos == goal\n newnode.pos = newpos;\n return;\n end\n \n \n if dir_Dimension == 2 \n dside = dir2Dto1D(d); \n \n if (obCheck(map, startpos+dside(1,:)) == 1) && (obCheck(map, startpos+dside(2,:)) == 1)\n return;\n end\n end\n \n \n dirs = hasForceNeighbour(map, newpos, d);\n if ~isempty(dirs)\n newnode.pos = newpos;\n newnode.dir = dirs;\n return;\n end\n \n % go 3d diagonal\n % sum(d.^2) = d(1)^2 + d(2)^2 + d(3)^2\n if(dir_Dimension == 3)\n % one 3d-diagonal change three 2d-diagonal + three straght line\n node1 = jump(map, goal, newpos, [0 d(2) d(3)]);\n node2 = jump(map, goal, newpos, [d(1) 0 d(3)]);\n node3 = jump(map, goal, newpos, [d(1) d(2) 0]);\n % three straght line\n node4 = jump(map, goal, newpos, [d(1) 0 0]);\n node5 = jump(map, goal, newpos, [0 d(2) 0]);\n node6 = jump(map, goal, newpos, [0 0 d(3)]);\n % find a ForceNegihbour\n if(length(node1.pos) + length(node2.pos) + length(node3.pos) + length(node4.pos) + length(node5.pos) + length(node6.pos) > 0)\n newnode.pos = newpos;\n newnode.dir = [d];\n \n newnode.dir = addDir(node1.pos, [0 d(2) d(3)], newnode.dir);\n newnode.dir = addDir(node2.pos, [d(1) 0 d(3)], newnode.dir);\n newnode.dir = addDir(node3.pos, [d(1) d(2) 0], newnode.dir);\n newnode.dir = addDir(node4.pos, [d(1) 0 0], newnode.dir);\n newnode.dir = addDir(node5.pos, [0 d(2) 0], newnode.dir);\n newnode.dir = addDir(node6.pos, [0 0 d(3)], newnode.dir);\n return;\n end\n end\n \n % go 2d diagonal\n if(dir_Dimension == 2)\n node4 = jump(map, goal, newpos, [d(1) 0 0]);\n node5 = jump(map, goal, newpos, [0 d(2) 0]);\n node6 = jump(map, goal, newpos, [0 0 d(3)]);\n % find a ForceNegihbour\n if(length(node4.pos) + length(node5.pos) + length(node6.pos) > 0)\n newnode.pos = newpos;\n newnode.dir = [d];\n % Add the direction of find ForceNegihbour\n newnode.dir = addDir(node4.pos, [d(1) 0 0], newnode.dir);\n newnode.dir = addDir(node5.pos, [0 d(2) 0], newnode.dir);\n newnode.dir = addDir(node6.pos, [0 0 d(3)], newnode.dir);\n return;\n end\n end\n \n % [go straght] or [go diagonal not find ForceNegihbour]\n newnode = jump(map, goal, newpos, d);\n return;\nend\n\n% if pos ~= Null, dirs = [dirs; d];\nfunction dirs = addDir(pos, d, dirs)\n if(length(pos) > 0)\n dirs = [dirs; d];\n end\nend\n\n\nfunction dside = dir2Dto1D(d)\n dside = [[0 0 0] ; [0 0 0]];\n temi = 1;\n if(d(1) ~= 0) \n dside(temi,:) = [d(1) 0 0];\n temi = temi + 1;\n end\n if(d(2) ~= 0) \n dside(temi,:) = [0 d(2) 0];\n temi = temi + 1;\n end \n if(d(3) ~= 0) \n dside(temi,:) = [0 0 d(3)];\n end \nend\n\n\nfunction ret = outmap(map, pos) \n ret = false;\n x = pos(1); y = pos(2); z = pos(3);\n if(x < 1 || x > map.maxIndex(1) || y < 1 || y > map.maxIndex(2) || z < 1 || z > map.maxIndex(3))\n ret = true;\n end\nend \n\n\nfunction ret = isforceNeighbourExist(map, pos, d)\n ret = false;\n x = pos(1); y = pos(2); z = pos(3);\n posNeighbour = pos + d;\n if outmap(map, pos) == 1 || ... \n map.occ_map(x, y, z) == 0 || ... \n outmap(map, posNeighbour) == 1 || ... \n map.occ_map(posNeighbour(1), posNeighbour(2), posNeighbour(3)) == 1 \n return; \n end\n ret = true;\nend\n\nfunction dirs = hasForceNeighbour(map, pos, d)\n dirs = [];\n \n if outmap(map, pos + d) == 1 \n return;\n end\n \n % [go straght] ----------------------------------------------\n if(sum(d.^2) == 1)\n \n if obCheck(map, pos + d) == 1\n return;\n end\n \n d1 = [abs(d(3)) abs(d(1)) abs(d(2))];\n d2 = [abs(d(2)) abs(d(3)) abs(d(1))];\n \n dob = [d1; -d1; d2; -d2; d1+d2; -d1-d2; d1-d2; -d1+d2];\n for i = 1 : 8\n if isforceNeighbourExist(map, pos + dob(i,:), d) == 1\n dirs = [dirs; dob(i,:) + d];\n end\n end\n \n % go 2D-diagonal ----------------------------------------------\n elseif (sum(d.^2) == 2)\n dside = dir2Dto1D(d);\n \n for i = 1 : 2\n if isforceNeighbourExist(map, pos - dside(i,:), d - dside(i,:) ) == 1\n dirs = [dirs; d - 2*dside(i,:)];\n end\n end\n \n d1 = [abs(d(1))-1 abs(d(2))-1 abs(d(3))-1];\n if isforceNeighbourExist(map, pos + d1, d) == 1\n dirs = [dirs; d + d1];\n end\n if isforceNeighbourExist(map, pos - d1, d) == 1\n dirs = [dirs; d - d1];\n end\n \n % go 3D-diagonal ----------------------------------------------\n else % (sum(d.^2) == 3)\n \n dirs = hasForceNeighbour(map, pos, [0 d(2) d(3)]);\n dirs = [dirs; hasForceNeighbour(map, pos, [d(1) 0 d(3)])];\n dirs = [dirs; hasForceNeighbour(map, pos, [d(1) d(2) 0])];\n dirs = unique(dirs,'rows'); \n \n [e1, ~] = size(dirs);\n for i = 1 : e1\n if(dirs(i,:) == d) \n dirs(i,:) = [];\n end\n end\n end\nend\n\nfunction newpos = step(pos, d)\n newpos = pos + d;\nend\n" }, { "path": "Motion_Planning/2Path_Planning/simplify_path.m", "content": "function path = simplify_path(map, path)\n if size(path,1) == 0, return; end\n \n keep_pBooL = false(size(path,1),1);\n keep_pBooL(1) = 1;\n keep_pBooL(end) = 1;\n last_keep = path(1,:);\n\n for i = 2:size(path,1)-1\n % ~check_line_collision() == 1 : No collision\n\t % check_line_collision() == 1 : collision\n if (~check_line_collision(last_keep, path(i,:)) & check_line_collision(last_keep, path(i+1,:))) \n last_keep = path(i,:);\n keep_pBooL(i) = 1;\n end\n end\n path = path(keep_pBooL,:);\n \n function c = check_line_collision(start_pos, end_pos)\n \n pts = map.generateiIntermediaPoints(start_pos, end_pos);\n \n %% Collision detection\n c = map.collide(pts);\n c = any(c);\n end\n\nend\n" }, { "path": "Motion_Planning/3Safe_Flight_Corridors/Ellipsoid.m", "content": "classdef Ellipsoid < handle\n properties \n C_; % Record the matrix of the ellipse, the semi-axis length of the ellipse without square\n d_; % Center point of ellipse\n axes_; % Length of each axis of ellipse\n invC_; % inv(obj.C_)\n end\n methods \n function obj = Ellipsoid (a,b) \n obj.d_ = b;\n obj.setC(a);\n end\n \n function setC(obj, a)\n obj.C_ = a;\n obj.invC_ = inv(a);\n if isnan(obj.invC_), assert(0 >= 42); end\n if (obj.invC_ == inf), assert(0 >= 42); end\n end\n\n % Calculate distance to the center\n function dis = dist(obj, pt)\n dis = sqrt(obj.square_of_dist(pt));\n end\n \n % Calculate the square of the distance to the center\n function dis = square_of_dist(obj, pt)\n dd = obj.invC_ * (pt - obj.d_)';\n dis = dot(dd, dd); % dot(A,B)\n end\n\n % Check if the point is inside\n function ret = inside(obj, pt)\n ret = false;\n if (obj.square_of_dist(pt) <= 1)\n ret = true;\n end\n end\n\n % Calculate points inside ellipsoid\n function pinside = points_inside(obj, points)\n pinside = [];\n [len, ~]=size(points);\n for i = 1 : len\n if(obj.inside(points(i,:)))\n pinside = [pinside; points(i,:)];\n end\n end\n end\n\n % Find the closest point\n function p = closest_point(obj, points)\n p = points(1,:);\n min_dist = 1e8;\n [len, ~]=size(points);\n for i = 1 : len\n d = obj.dist(points(i,:));\n if(d < min_dist)\n min_dist = d;\n p = points(i,:);\n end\n end\t\t\t\t\t\t\n end\n\n % Find the closest hyperplane from the closest point, \n function plane = closest_hyperplane(obj, points)\n closest_pt = obj.closest_point(points);\n n = ( obj.invC_ * (obj.invC_)' )*(closest_pt - obj.d_)';\n plane = Hyperplane(closest_pt, n);\n end\n end\nend\n" }, { "path": "Motion_Planning/3Safe_Flight_Corridors/Hyperplane.m", "content": "classdef Hyperplane < handle\n properties \n p_; % point on the plane\n n_; % Normal of the plane, directional,\n local_bbox_; \n end\n methods \n function obj = Hyperplane(p, n)\n obj.p_ = p;\n obj.n_ = n;\n end\n % Calculate the signed distance from point\n function dis = signed_dist(obj, pt)\n dis = dot(obj.n_, pt - obj.p_);\n end\n % Calculate the distance from point\n function dis = dist(obj, pt)\n dis = abs(obj.signed_dist(pt));\n end\n end\nend" }, { "path": "Motion_Planning/3Safe_Flight_Corridors/LineSegment.m", "content": "classdef LineSegment < handle\n properties \n p1_; \n p2_; \n R_;\n obs_; \n ellipsoid_; \n polyhedron_;\n local_bbox_; % Bounding Box of map\n epsilon_; \n end\n methods \n function obj = LineSegment(p1, p2) \n obj.p1_ = p1;\n obj.p2_ = p2;\n obj.epsilon_ = 1e-10;\n end\n\n function dilate(obj, radius)\n obj.find_ellipsoid(radius); \n obj.find_polyhedron(); \n obj.add_local_bbox(obj.polyhedron_); \t\t\t\t\n end\n\n function set_local_bbox(obj, local_bbox)\n obj.local_bbox_ = local_bbox;\t\t\t\t\n end\t\t\n\n function set_obs(obj, obs)\n Vs = Polyhedron();\n obj.add_local_bbox(Vs);\t\t\n obj.obs_ = Vs.points_inside(obs);\t\n end\t\t\n\n function add_local_bbox(obj, Vs)\n r = norm(obj.p2_ - obj.p1_)/2; \n dir = (obj.p2_ - obj.p1_)/norm(obj.p2_ - obj.p1_);\n dir_h = [dir(2) -dir(1) 0];\n if(norm(dir_h) == 0)\n dir_h = [-1 0 0]; \n end\n dir_h = dir_h/norm(dir_h);\n\n % along x\n pp1 = obj.p1_ + dir_h*(r);\n pp2 = obj.p1_ - dir_h*(r);\n Vs.add(Hyperplane(pp1, dir_h));\n Vs.add(Hyperplane(pp2, -dir_h));\n\n % along y\t\t\n pp3 = obj.p2_ + dir*(r);\n pp4 = obj.p1_ - dir*(r);\n Vs.add(Hyperplane(pp3, dir));\n Vs.add(Hyperplane(pp4, -dir));\t\t\n\n % along z, \n dir_v = [0 0 0];\t\n dir_v(1) = dir(2) * dir_h(3) - dir(3) * dir_h(2);\n dir_v(2) = dir(3) * dir_h(1) - dir(1) * dir_h(3); \n dir_v(3) = dir(1) * dir_h(2) - dir(2) * dir_h(1); \n pp5 = obj.p1_ + dir_v*(r);\n pp6 = obj.p1_ - dir_v*(r);\n Vs.add(Hyperplane(pp5, dir_v));\n Vs.add(Hyperplane(pp6, -dir_v));\n end\n\n % 3D \n function find_ellipsoid(obj, offset_x)\n f = norm(obj.p1_ - obj.p2_)/2; \n C = [f 0 0; 0 f 0; 0 0 f];\n % f is the radius of the ellipse, the initialized ellipse is a circle\n % C = [f 0 0]\t\n % [0 f 0]\n % [0 0 f]\n \n % h = (R^T)*x, (1 0 0)^T = R^T*(p2_ - p1_) => (p2_ - p1_) = R*(1 0 0)^T\n Ri = rotationMatrix([1 0 0]', (obj.p2_ - obj.p1_)');\n C = Ri * C * (Ri');\n axes = [f f f];\n obj.R_ = Ri;\n\n E = Ellipsoid(C, (obj.p1_ + obj.p2_)/2 ); \n\n obs = E.points_inside(obj.obs_);\n obs_inside = obs;\n\n % decide short axes-1\n while (~isempty(obs_inside))\n cp = E.closest_point(obs_inside);\n rcp = (Ri')*(cp - E.d_)'; \n\n % Generate a new ellipse\n if(abs(rcp(1)) < axes(1)) \n newy = sqrt((rcp(2))^2 + (rcp(3))^2);\n axes(2) = newy / sqrt(1 - (rcp(1)/axes(1))^2);\n end\n \n new_C = [axes(1) 0 0; 0 axes(2) 0; 0 0 axes(2)];\n E.setC(Ri * new_C * (Ri'));\n \n % Delete all the points outside the new ellipse\n obs_new = [];\n [len, ~] = size(obs_inside);\n for i = 1 : len\n % constexpr decimal_t epsilon_ = 1e-10; \n if(1 - E.dist(obs_inside(i,:)) > obj.epsilon_ )\n obs_new = [obs_new; obs_inside(i,:)];\n end\n end\n obs_inside = obs_new;\n end\n\n % decide short axes-2\n C = [axes(1) 0 0; 0 axes(2) 0; 0 0 axes(3)];\n E.setC(Ri * C * (Ri'));\n \n obs_inside = E.points_inside(obs);\n while (~isempty(obs_inside))\n cp = E.closest_point(obs_inside);\n rcp = (Ri')*(cp - E.d_)';\n dd = sqrt( 1 - (rcp(1)/axes(1))^2 - (rcp(2)/axes(2))^2 );\n if(dd > obj.epsilon_)\n axes(3) = abs(rcp(3)) / dd;\n end\n \n new_C = [axes(1) 0 0; 0 axes(2) 0; 0 0 axes(3)];\n E.setC(Ri * new_C * (Ri'));\n \n % Delete all the points outside the new ellipse\n obs_new = [];\n [len, ~] = size(obs_inside);\n for i = 1 : len\n % constexpr decimal_t epsilon_ = 1e-10; \n if(1 - E.dist(obs_inside(i,:)) > obj.epsilon_ )\n obs_new = [obs_new; obs_inside(i,:)];\n end\n end\n obs_inside = obs_new;\n end\n \n E.axes_ = axes;\n obj.ellipsoid_ = E;\n \n end\n\n function find_polyhedron(obj)\n Vs = Polyhedron();\n obs_remain = obj.obs_;\n while(length(obs_remain))\n plane = obj.ellipsoid_.closest_hyperplane(obs_remain);\n Vs.add(plane);\n obs_tmp = [];\n [len, ~] = size(obs_remain);\n for i = 1 : len\n p = obs_remain(i,:);\n if(plane.signed_dist(p) < 0)\n obs_tmp = [obs_tmp; p];\n end\n end\n obs_remain = obs_tmp;\n end\n obj.polyhedron_ = Vs;\n end\n end\nend\n" }, { "path": "Motion_Planning/3Safe_Flight_Corridors/MultipleSegments.m", "content": "classdef MultipleSegments < handle\n properties\n lines_;\n obs_;\n local_bbox_;\n end\n methods \n function obj = MultipleSegments(local_bbox) \n obj.local_bbox_ = local_bbox;\n end\n\n function set_obs(obj, obs)\n obj.obs_ = obs;\n end\n\n function dilate(obj, path, offset_x)\n [len, ~] = size(path);\n obj.lines_ = cell(1, len-1);\n\n for i = 1 : (len - 1)\n p1 = path(i,:); p2 = path(i+1,:);\n obj.lines_{i} = LineSegment(p1, p2);\n obj.lines_{i}.set_local_bbox(obj.local_bbox_);\n obj.lines_{i}.set_obs(obj.obs_);\n obj.lines_{i}.dilate(offset_x);\n end\t\n \n end\t\n \n function drawLines(obj)\n len = length(obj.lines_);\n for i = 1 : len\t\n obj.lines_{i}.p1_;\n end\n end\n \n function drawEllipsoids(obj)\n hold on;\n len = length(obj.lines_);\n for i = 1 : len\t\n center = obj.lines_{i}.ellipsoid_.d_;\n R = obj.lines_{i}.R_;\n axes = obj.lines_{i}.ellipsoid_.axes_; \n obj.draw_Ellipsoid(center, axes, R');\n end\n hold off;\n end\n \n function draw_Ellipsoid(obj, center, axes, R)\n [X, Y, Z] = ellipsoid(0,0,0, axes(1), axes(2), axes(3));\n [row, col] = size(X);\n A = [X(:), Y(:), Z(:)];\n B = A*R + center;\n X = reshape(B(:,1),row,col);\n Y = reshape(B(:,2),row,col);\n Z = reshape(B(:,3),row,col);\n mesh(X, Y, Z, 'FaceAlpha','0.5');\n end\n \n % draw the point of obs on the polyhedron \n function drawPlanePoint(obj)\n hold on;\n len = length(obj.lines_);\n planesCell = obj.lines_{len}.polyhedron_.polys_;\n [~, lenj] = size(planesCell);\n for i = 1 : lenj\t\n point = planesCell{i}.p_;\n plot3(point(1), point(2), point(3),'r*');\n end\n hold off;\n end\n \n function drawpolyhedron(obj)\n hold on;\n len = length(obj.lines_);\n for i = len : len\n planesCell = obj.lines_{i}.polyhedron_.polys_;\n p1 = obj.lines_{i}.p1_;\n p2 = obj.lines_{i}.p2_;\n r = norm(p2 - p1)/2;\n pmid = (p2 + p1)/2;\n [~, lenj] = size(planesCell);\n for j = 1 : lenj\n obj.drawPlane(planesCell{j}.n_, planesCell{j}.p_, pmid, r);\n end\n end\n hold off;\n end\n \n function drawPlane(obj, n, p, pmid, r)\n r = r*2;\n xL = pmid(1) - r;\n xR = pmid(1) + r;\n yU = pmid(2) + r;\n yD = pmid(2) - r;\n \n [X,Y]=meshgrid(xL:0.5:xR, yD:0.5:yU);\n Z = p(3) - ((X - p(1))*n(1)+(Y - p(2))*n(2))/n(3);\n surf(X, Y, Z, 'FaceAlpha','0.5','EdgeColor','flat','FaceColor', '[0 1 1]');\n end\n end\nend" }, { "path": "Motion_Planning/3Safe_Flight_Corridors/Polyhedron.m", "content": "classdef Polyhedron < handle\n properties \n polys_; \n epsilon_; \n end\n methods \n function obj = Polyhedron() \n obj.polys_ = cell(0);\n obj.epsilon_ = 1e-10;\n end\n\n % Append Hyperplane\n function add(obj, plane)\n obj.polys_{end+1} = plane;\n end\n % Check if the point is inside polyhedron,\n function isinside = inside(obj, pt)\n isinside = false;\n [len, ~]=size(obj.polys_);\n for i = 1 : len\n if(obj.polys_{i}.signed_dist(pt) > obj.epsilon_)\n return;\n end\n end\n isinside = true;\n end\n\n % Calculate points inside polyhedron\n function pinside = points_inside(obj, points)\n pinside = [];\n [len, ~]=size(points);\n for i = 1 : len\n if(obj.inside(points(i,:)))\n pinside = [pinside; points(i,:)];\n end\n end\n end\n end\nend" }, { "path": "Motion_Planning/3Safe_Flight_Corridors/SFC_3D.m", "content": "function decomp = SFC_3D(path, obs, boundary)\n % Initialize MultipleSegments\n\tlocal_bbox = boundary; offset_x = 0;\n decomp = MultipleSegments(local_bbox);\n decomp.set_obs(obs);\n decomp.dilate(path, offset_x);\nend\n\n" }, { "path": "Motion_Planning/3Safe_Flight_Corridors/rotationMatrix.m", "content": "function R = rotationMatrix(v1col, v2col)\n if (norm(v2col) == 0), assert(0 >= 42); end % There is no concept of collinear zero vector\n \n v1 = v1col';\n v2 = v2col';\n nv1 = v1/norm(v1);\n nv2 = v2/norm(v2);\n\n if norm(nv1+nv2)==0\n q = [0 0 0 0];\n else\n u = cross(nv1,nv2); % If nv1 and nv2 are collinear, u = [0 0 0]\n un = norm(u); % If u = [0 0 0], norm(u) = 0;\n if(un == 0)\n R = [1 0 0; 0 1 0; 0 0 1];\n return;\n end\n u = u/un;\n\n theta = acos(sum(nv1.*nv2))/2;\n q = [cos(theta) sin(theta)*u];\n end\n\n 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));\n 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));\n 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];\n\tR = R';\nend\n" }, { "path": "Motion_Planning/4Time_Allocation/averageSpeed_ta.m", "content": "% averageSpeed_TimeAllocation:\n% avg_v: the estimated average speed\nfunction [ts, total_time] = averageSpeed_ta(path, avg_v)\n path_seg_len = sqrt(sum((path(2:end, :) - path(1:end-1,:)).^2,2));\n path_len = sum(path_seg_len);\n total_time = path_len/avg_v;\n\n ts = cumsum(path_seg_len);\n ts = ts/ts(end);\n ts = [0; ts]';\n ts = ts*total_time;\nend\n\n\n" }, { "path": "Motion_Planning/4Time_Allocation/trapezoidalSpeed_ta.m", "content": "% trapezoidalSpeed_TimeAllocation:\n% avg_v: the estimated average speed\n% acc: Acceleration trapezoidalSpeed\nfunction [ts, total_time] = trapezoidalSpeed_ta(path, avg_v, acc)\n\n t_acc = avg_v/acc;\n s_acc = 0.5*acc*(t_acc^2);\n \n path_seg_len = sqrt(sum((path(2:end, :) - path(1:end-1,:)).^2,2));\n path_len = sum(path_seg_len);\n total_time = path_len/avg_v;\n \n ts = cumsum(path_seg_len);\n ts = ts/ts(end);\n ts = [0; ts]';\n ts = ts*total_time;\n \n \n % trapezoid Area = 0.5(a + b) h\n % math ep\n% a1 = ts(2) - t_acc;\n% b1 = a1 - speed_h/acc;\n% ep1 = 0.5*(a1 + b1)*speed_h + t_add1*(avg_v + speed_h) == s_acc;\n% a2 = ts(end) - ts(end-1) - t_acc;\n% b2 = a2 - speed_h/acc;\n% ep2 = 0.5*(a2 + b2)*speed_h + t_add2*(avg_v + speed_h) == s_acc;\n% t_mid = (ts(end-1) - ts(2));\n% ep3 = (t_mid-t_add1-t_add2)*speed_h - (t_add1 + t_add2)*avg_v == 0;\n \n\n %% solve\n syms t_add1 t_add2 speed_h;\n a1 = ts(2) - t_acc;\n ep1 = 0.5*(a1 + a1 - speed_h/acc)*speed_h + t_add1*(avg_v + speed_h) == s_acc;\n a2 = ts(end) - ts(end-1) - t_acc;\n ep2 = 0.5*(a2 + a2 - speed_h/acc)*speed_h + t_add2*(avg_v + speed_h) == s_acc;\n t_mid = (ts(end-1) - ts(2));\n ep3 = (t_mid-t_add1-t_add2)*speed_h - (t_add1 + t_add2)*avg_v == 0;\n [x0,y0,z0] = solve(ep1,ep2,ep3, t_add1, t_add2, speed_h);\n ans = double([x0,y0,z0]);\n \n sort_ans = sortrows(ans,3);\n \n ts(2) = ts(2) + sort_ans(1,1);\n ts(end-1) = ts(end-1) - sort_ans(1,2); \n \n if(path_len < 2*s_acc), assert(1 >= 2); end\n if(path_seg_len(1) < s_acc), assert(1 >= 2); end\n if(path_seg_len(end) < s_acc), assert(1 >= 2); end\n \nend\n\n" }, { "path": "Motion_Planning/5Trajectory_Planning/QPbyUseSFC.m", "content": "function X = QPbyUseSFC(waypts, ts, decomp)\n\n%% condition\ntraj.n_order = 7;\ntraj.n_poly = size(waypts, 1) - 1;\ntraj.p0 = waypts(1,:);\ntraj.pe = waypts(end,:);\ntraj.v0 = [0,0,0];\ntraj.ve = [0,0,0];\ntraj.a0 = [0,0,0];\ntraj.ae = [0,0,0];\n\n\n%% trajectory plan\n[minSnapValue, px, py, pz] = minimum_snap_three_axis_SFC(traj, ts, decomp);\n\ndisp(['minSnapValue is : ',num2str(minSnapValue)]);\n\nX = [px py pz];\nend\n\n% v0 = [1*3]\nfunction [minValue, px, py, pz] = minimum_snap_three_axis_SFC(traj, ts, decomp)\n % Dimension\n dim = 3;\n n_order = traj.n_order;\n\tp0 = traj.p0;\n\tpe = traj.pe;\n v0 = traj.v0;\n ve = traj.ve;\n a0 = traj.a0;\n ae = traj.ae;\n\t\n\tn_poly = traj.n_poly;\n\tn_coef = n_order+1;\n\t\n\t%% compute Q, Q is a Symmetric matrix\n % Q_1 = [0 0 0]\n % [0 f f]\n % [0 f f] % f = integral of ts(i) ~ ts(i+1) \n % Q_x = [Q_1 0 0 0]\n % [0 Q_2 0 0]\n % [0 0 Q_... 0]\n % [0 0 0 Q_n] % n = n_poly\n % Q_all = [Q_x 0 0]\n % [0 Q_y 0]\n % [0 0 Q_z] % Q_x == Q_y == Q_z\n Q_x = [];\n\tfor i=1:n_poly\n\t Q_x = blkdiag(Q_x,calc_Q(n_order,4,ts(i),ts(i+1)));\n end\n zeroM = zeros(size(Q_x));\n Q_all = [Q_x zeroM zeroM; zeroM Q_x zeroM; zeroM zeroM Q_x];\n xlen = size(Q_all,1);\n\tf = zeros(xlen,1); %% min (1/2p^TQp + f^Tp)\n\t\n %% compute Aeq x = beq\n % beacuse Aeq_x == Aeq_y == Aeq_z\n % Aeq = [Aeq_x 0 0]\n % [0 Aeq_y 0]\n % [0 0 Aeq_z] \n\tneq = 8; %% (8 equations)\n eqNum_x = (7*(n_poly-1)+neq);\n\tAeq_x = zeros(eqNum_x, n_coef*n_poly);\n\tbeq = zeros(dim*eqNum_x, 1);\n\t\n\t% start/terminal pva constraints (8 equations)\n\tAeq_x(1:4,1:n_coef) = [calc_dc(ts(1),n_order,0);\n\t calc_dc(ts(1),n_order,1);\n\t calc_dc(ts(1),n_order,2);\n\t calc_dc(ts(1),n_order,3)];\n\tAeq_x(5:8,n_coef*(n_poly-1)+1:n_coef*n_poly) = ...\n\t [calc_dc(ts(end),n_order,0);\n\t calc_dc(ts(end),n_order,1);\n\t calc_dc(ts(end),n_order,2);\n\t calc_dc(ts(end),n_order,3)];\n\tbeq(1:8,1) = [p0(1),v0(1),a0(1),0,pe(1),ve(1),ae(1),0]';\n beq((eqNum_x + 1):(eqNum_x + 8),1) = [p0(2),v0(2),a0(2),0,pe(2),ve(2),ae(2),0]';\n beq((eqNum_x*2 + 1):(eqNum_x*2 + 8),1) = [p0(3),v0(3),a0(3),0,pe(3),ve(3),ae(3),0]';\n\t\n\t% continuous constraints ((n_poly-1)*7 equations)\n\tfor i=1:n_poly-1\n\t\tt_derc_p = calc_dc(ts(i+1),n_order,0);\n t_derc_v = calc_dc(ts(i+1),n_order,1);\n t_derc_a = calc_dc(ts(i+1),n_order,2);\n t_derc_j = calc_dc(ts(i+1),n_order,3); % jerk\n t_derc_4 = calc_dc(ts(i+1),n_order,4);\n t_derc_5 = calc_dc(ts(i+1),n_order,5);\n t_derc_6 = calc_dc(ts(i+1),n_order,6);\n Aeq_x(neq+1,n_coef*(i-1)+1:n_coef*(i+1))=[t_derc_p,-t_derc_p];\n Aeq_x(neq+2,n_coef*(i-1)+1:n_coef*(i+1))=[t_derc_v,-t_derc_v];\n Aeq_x(neq+3,n_coef*(i-1)+1:n_coef*(i+1))=[t_derc_a,-t_derc_a];\n Aeq_x(neq+4,n_coef*(i-1)+1:n_coef*(i+1))=[t_derc_j,-t_derc_j];\n Aeq_x(neq+5,n_coef*(i-1)+1:n_coef*(i+1))=[t_derc_4,-t_derc_4];\n Aeq_x(neq+6,n_coef*(i-1)+1:n_coef*(i+1))=[t_derc_5,-t_derc_5];\n Aeq_x(neq+7,n_coef*(i-1)+1:n_coef*(i+1))=[t_derc_6,-t_derc_6];\n neq = neq + 7;\n end\n\t\n \n zeroM = zeros(size(Aeq_x)); \n Aeq = [Aeq_x zeroM zeroM; zeroM Aeq_x zeroM; zeroM zeroM Aeq_x];\n \n %% compute Ax <= b\n A = [];\n b = [];\n ieq = 1;\n xll = xlen / dim; % 1/3 of xlen, = n_coef * (n_poly-1)\n for i = 1 : n_poly\n planesCell = decomp.lines_{i}.polyhedron_.polys_;\n [~, lenj] = size(planesCell);\n for j = 1 : lenj\n n = planesCell{j}.n_;\n p = planesCell{j}.p_;\n nx = zeros(1, xlen);\n % add (Denominator + 1) points on the poly's Ax 0\n for k = 0 : Denominator\n tvec_mid = calc_dc(ts(i) + k*(ts(i+1) - ts(i))/Denominator, n_order, 0); % [1*8]vector\n nx(1, 8*(i-1) + 1 : 8*(i-1) + 8) = tvec_mid.*n(1);\n nx(1, 8*(i-1) + 1 + xll : 8*(i-1) + 8 + xll) = tvec_mid.*n(2);\n nx(1, 8*(i-1) + 1 + 2*xll : 8*(i-1) + 8 + 2*xll)= tvec_mid.*n(3);\n A(ieq,:) = nx;\n b(ieq,:) = dot(n, p);\n % dot(plane.n, p1 - plane.p) < 0 \n % => dot(plane.n, p1) - dot(plane.n, plane.p) < 0 \n % => dot(plane.n, p1) < dot(plane.n, plane.p) \n %% Ax < b\n ieq = ieq + 1;\n end\n end\n end\n \n \n% % Modify the number of iterations\n options = optimoptions('fmincon');\n options.MaxIterations = 2000; % Defaults = 1000\n lb = []; ub = []; x0 = []; \n p = quadprog(Q_all,f,A,b,Aeq,beq, lb,ub,x0, options);\n\n% \tp = quadprog(Q_all,f,A,b,Aeq,beq);\n \n minValue = p'*Q_all*p;\n\t\n px = p(1:xll,1);\n py = p(xll + 1:2*xll,1);\n pz = p(xll*2 + 1:3*xll,1);\nend\n\n" }, { "path": "Motion_Planning/5Trajectory_Planning/TrajectoryPlanning.m", "content": "function [total_time, ts, X] = TrajectoryPlanning(path, decomp, time_allocation)\n speed = time_allocation.avg_speed;\n acc = time_allocation.acc;\n \n disp(['The planned speed is : ', num2str(speed)]);\n tic\n if strcmp(time_allocation.type, 'averageSpeed')\n [ts, total_time] = averageSpeed_ta(path0, speed);\n elseif strcmp(time_allocation.type, 'trapzoidSpeed')\n [ts, total_time] = trapezoidalSpeed_ta(path, speed, acc);\n end\n disp('TimeAllocation time is :');\n toc\n disp(['time management: total_time is ', num2str(total_time), 'seconds']);\n disp(['Split time is : ', num2str(ts)]);\n \n \n tic \n % use Ax=b get Trajectory planning. ===========================\n % X = Ax_equal_b(path, total_time, ts);\n\n % use 'Quadratic Programming' and SFC(which make by Ax < b) get Trajectory planning. ========================\n % use SFC make Inequality constraints\n X = QPbyUseSFC(path, ts, decomp);\n\n disp('generator trajectory time is :');\n toc\nend\n\n" }, { "path": "Motion_Planning/5Trajectory_Planning/calc_Q.m", "content": "% Calculation Q_1 of Q_x of Q of p^TQp \n% n:Polynomial equation maximum order\n% d:derivertive order, 1:minimum vel 2:minimum acc 3:minimum jerk 4:minimum snap\n% t1:start timestamp for polynormial\n% t2:end timestap for polynormial\nfunction Q1 = calc_Q(n_order, d, t1, t2)\n T = zeros((n_order-d)*2+1,1);\n for n = 1:(n_order-d)*2+1\n T(n) = t2^n-t1^n;\n end\n Q1 = zeros(n_order);\n for i = d+1 : n_order+1\n for j = i : n_order+1\n c = i + j - 2*d - 1;\n Q1(i,j) = prod(i-d:i-1) * prod(j-d:j-1) / c * T(c);\n Q1(j,i) = Q1(i,j);\n end\n end\n\nend" }, { "path": "Motion_Planning/5Trajectory_Planning/calc_dc.m", "content": "% Calculation Derivative coefficient\n% n_order = Polynomial equation maximum order\n% d = derivertive order = 0:pos 1:vel 2:acc 3:jerk\nfunction t_derC = calc_dc(t, n_order, d)\n t_derC = zeros(1,n_order+1);\n for i = d+1 : n_order+1\n t_derC(i) = prod(i-d:i-1) * t^(i-d-1);\n end\nend\n" }, { "path": "Motion_Planning/demo/demo_3dCorner.m", "content": "close all;\nclear all;\nclc;\n\ndemoActivate = true;\nchoose_map = 2; \npath_id = 2;\n\nrunsim" }, { "path": "Motion_Planning/demo/demo_Z_3blocks.m", "content": "close all;\nclear all;\nclc;\n\ndemoActivate = true;\nchoose_map = 3; \npath_id = 2;\n\nrunsim" }, { "path": "Motion_Planning/demo/demo_ellipsoid.m", "content": "close all;\nclear all;\nclc;\n\ndemoActivate = true;\nchoose_map = 1; \npath_id = 2;\n\nrunsim" }, { "path": "Motion_Planning/draw/draw_ObcPoints.m", "content": " hold on;\n len = length(obps);\n for i = 1 : len\n plot3(obps(i,1), obps(i,2), obps(i,3), '*', 'Color', 'r');\n end\n\n\n hold off;\n" }, { "path": "Motion_Planning/draw/makeGifAndJpg.m", "content": "function makeGifAndJpg(fig_num)\n \n saveas(figure(4), 'postion','jpg');\n pause(0.1);\n saveas(figure(5), 'velocity','jpg');\n pause(0.1);\n filename = 'testAnimated6.gif';\n \n h = figure(fig_num);\n for i = 1:90\n view(4*i+15, 15);\n drawnow \n\n % Capture the plot as an image \n frame = getframe(h); \n im = frame2im(frame); \n [imind,cm] = rgb2ind(im,256); \n\n % Write to the GIF File \n if i == 1 \n imwrite(imind,cm,filename,'gif', 'Loopcount',inf); \n else \n imwrite(imind,cm,filename,'gif','WriteMode','append'); \n end \n end\n\nend\n\n" }, { "path": "Motion_Planning/draw/plot_path.m", "content": "function plot_path(paths, mapClass, SFCs)\n% PLOT_PATH Visualize a path and sfc's ellipses\n% PLOT_PATH(path mapClass, SFCs) creates a figure showing a path through\n% the environment. \n% SFCs provide functions for drawing ellipses: drawEllipsoids();\n\npersistent map decomps init;\nif isempty(init)\n map = mapClass;\n decomps = SFCs;\n init = 1;\nend\n\nfigure('Name','Animation');\n\nhold on;\nfor i = 1:size(map.blocks,1)\n block = map.blocks(i, :);\n drawBlock(block);\nend\n\npath = paths{1};\n\n% draw Inflated obstacle\nfor i = 1:size(map.blocks,1)\n margin = map.margin;\n block = map.blocks(i, :);\n block(1,1:3) = block(1,1:3) - margin;\n block(1,4:6) = block(1,4:6) + margin;\n block(7) = max(150, block(7)); block(8) = max(150, block(8)); block(9) = max(150, block(9)); %% set color\n drawBlock(block);\nend\n\nif size(path,1) > 0\n % pcshow(path, [0,1,0],'MarkerSize', 0.1);\n % The lower left corner position, width and height. 260 here is exactly 7cm\n% set(gcf,'position', [500 500 260 220]);\n% set(gcf,'position', [500 500 400 400]); % big pic\n% set(gcf,'position', [500 500 300 300]); % small pic\n set(gcf,'color','w');\n set(gca,'color','w');\n \n %% Display the unit of x y z\n% xlabel('x length(m)');\n% ylabel('y length(m)');\n% zlabel('z length(m)');\n \nend\n\n% Set the axis range\naxis_add = 2*map.res(1);\naxis([map.boundary.ld(1)-axis_add, map.boundary.ru(1)+axis_add, ... \n map.boundary.ld(2)-axis_add, map.boundary.ru(2)+axis_add, ...\n map.boundary.ld(3)-axis_add, map.boundary.ru(3)+axis_add])\n\nhold on\nfor j = 1 : size(paths, 2)\n if j == 1\n % JPS's result\n% plot3(paths{j}(:,1), paths{j}(:,2), paths{j}(:,3),'-*','color','k','LineWidth',3);\n elseif j == 2\n % JPS() + simple path() result: \n% plot3(paths{j}(:,1), paths{j}(:,2), paths{j}(:,3),'-*','color','#A2142F','LineWidth',3);\n plot3(paths{j}(:,1), paths{j}(:,2), paths{j}(:,3),'-*','color','k','LineWidth',3);\n end\nend\nhold off\n\nset(gca,'DataAspectRatio',[0.1 0.1 0.1]);\n\n% drawEllipsoid = true;\nif (exist('drawEllipsoid') && (drawEllipsoid == true))\n decomps{1}.drawEllipsoids();\n% decomps{2}.drawEllipsoids();\nend\n\ngrid on\n\nview(30, 10);\nhold off;\n\nend\n\nfunction drawBlock(block)\n \n x = [ones(4,1) * block(1); ones(4,1) * block(4)];\n y = [ones(2,1) * block(5); ones(2,1) * block(2); ones(2,1) * block(5); ones(2,1) * block(2)];\n z = [block(3);block(6);block(3);block(6);block(3);block(6);block(3);block(6)];\n\n\n vert = [x, y, z];\n 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];\n c = block(7:9)/255;\n patch('Vertices',vert,'Faces',fac,...\n 'FaceVertexCData',hsv(6),'FaceColor',c,'FaceAlpha',.2);\n\n \n x = [ones(4,1) * block(1); ones(4,1) * block(4)];\n y = [block(2);block(5);block(2);block(5);block(2);block(5);block(2);block(5)];\n z = [ones(2,1) * block(3); ones(2,1) * block(6); ones(2,1) * block(3); ones(2,1) * block(6)];\n\n vert = [x, y, z];\n 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];\n c = block(7:9)/255;\n patch('Vertices',vert,'Faces',fac,...\n 'FaceVertexCData',hsv(6),'FaceColor',c,'FaceAlpha',.2);\nend" }, { "path": "Motion_Planning/init_data.m", "content": "start = {[]};\nstop = {[]};\n\nif (~exist('demoActivate')) || (demoActivate == false)\n choose_map = 2;\nend\ndemoActivate = false;\nif (~exist('path_id'))\n path_id = 2; % Path planning: 2 = JPS, 4 = JPS-PF [This feature has not been uploaded yet]\nend\nSFC_id = path_id/2; % Path planning: 2 = path_id(4)'s SFC, 1 = path_id(2)'s SFC\nspeed = 1;\nacc = 0;\n\ntime_allocation.type = 'trapzoidSpeed';\n%time_allocation.type = 'averageSpeed';\n\nswitch choose_map\n case 1 %% 3 blocks\n map.load_map('0Maps/ellipsoid.txt', 0.2, 0.2, 0.1);\n start = {[1.5 0.2 0.2]};\n stop = {[1.5 2.0 1.9]};\n speed = 1;\n acc = 2; \n case 2\n map.load_map('0Maps/3dCorner.txt', 0.1, 0.1, 0.1);\n start = {[2.6 1.0 1.0]};\n stop = {[2.5 3.0 2.6]};\n acc = 2; % acceleration \n if path_id == 4\n speed = 1.0767; % for path_id = 4, time = 4.3682, snap = 350.3038, fly use 4.1s\n else\n speed = 0.8; % for path_id = 2, time = 4.3681, snap = 895.7681, fly use 4.15s\n end\n case 3 %% Z_3blocks\n map.load_map('0Maps/Z_3blocks.txt', 0.2, 0.2, 0.1);\n start = {[-1 1.5 2]};\n stop = {[11 1.5 1]};\n acc = 2;\n if path_id == 4\n speed = 1.4; %for path_id = 4, time = 12.9343, snap = 58.376, fly use 12.45s\n else\n speed = 1.10423; %for path_id = 2, time = 12.9343, snap = 300.3111, fly use 13.9s \n end\n\n otherwise \nend\n\n% Must be after acc assignment above\ntime_allocation.avg_speed = speed;\ntime_allocation.acc = acc;\n" }, { "path": "Motion_Planning/runsim.m", "content": "% if demoActivate exist and equal true, not clean all variable.\n% runsim.m called by the demo, not clean all variable\n% runsim.m called by itself, clean all variable\nif (~exist('demoActivate')) || (demoActivate == false)\n close all;\n clear all;\n clc;\nend\n\naddpath(genpath('./'));\n\n\n%% map generation\nmap = GridMap();\ninit_data\nmap.flag_obstacles();\n\n\n%% path planning\nnquad = length(start);\nfor qn = 1:nquad \n disp('JPS time is :');\n tic\n path{qn} = JPS_3D(map, start{qn}, stop{qn});\n path{qn} = [start{qn}; path{qn}(1:end,:)];\n path{qn}(end + 1,:) = stop{qn};\n toc\nend\n\n%% delete the points of no-use\npath{2} = simplify_path(map, path{1});\n\n%% Generating Convex Polytopes\nobps = PointCloudMap(map.blocks, map.margin); % blocks of Metric Map change to point cloud\n\ndecomps{2} = []\ndisp('JPS -> SFC time is :');\ntic\ndecomps{1} = SFC_3D(path{2}, obps, map.boundary); % call SFC\ntoc\n\npath{path_id}\n\n\n%% draw path and blocks\nif nquad == 1\n plot_path(path, map, decomps); \nelse\n % you could modify your plot_path to handle cell input for multiple robots\nend\n\n% draw_ObcPoints\n% makeGifAndJpg(1); %figure(1): Graph without trajectory\n\n%% Trajectory planning\n[t_time, ts_par, x_par] = TrajectoryPlanning(path{path_id}, decomps{SFC_id}, time_allocation);\n\n\n%% Trajectory tracking\ndisp('Generating Trajectory ...');\ntrajectory_generator([], [], path{path_id}, t_time, ts_par, x_par);\ntrajectory = test_trajectory(start, stop, path, true); % with visualization\ndisp('Blue line is Trajectory planning.');\ndisp('Red line is Trajectory tracking.');\n\n%% Gif\n% makeGifAndJpg(3); %figure(3): Graph with trajectory\n" }, { "path": "Motion_Planning.md", "content": "# Motion Planning\n\nA new method to improve path planning\n\n\"polyhedron-1\"\n\n\n\n- Path planning algorithms (Jump point search, JPS-PF )\n\n- Generating Convex Polytopes(Safe Flight Corridors)\n\n- Time Allocation(Average time allocation, Trapezoidal time allocation)\n\n- Trajectory planning(QP)\n\n- Trajectory following control(PD controller)\n\n\n\n## Required\n\nMATLAB(R2019b is tested)\n\n\n\n## Demo\n\nRun the demo map script (Only three maps have been released so far) ：\n\n```\ndemo_3dCorner.m\ndemo_ellipsoid.m\ndemo_Z_3blocks.m\n```\n\n\n\n### Map: 3dCorner\n\n- Same starting point and ending point\n\n- same total planning time, use trapezoidal time allocation\n\n| Method | JPS | JPS-PF |\n| ---------- | ------------------------------------------------------------ | ------------------------------------------------------------ |\n| trajectory | \"3dCorner-jps-ys\" | \"3dCorner-new-ys\" |\n| SFC | \"3dCorner-jps-sfc\" | \"3dCorner-new-sfc\" |\n| Avg_speed | 0.8 | 1.0767 |\n| Time | 4.3681 | 4.3681 |\n| miniSnap | 895.7681 | 350.3038 |\n\n \n\n### Map: Z_3blocks\n\n| Method | JPS | JPS-PF |\n| ---------- | ------------------------------------------------------------ | ------------------------------------------------------------ |\n| trajectory | \"Z_3blocks\" | \"Z_3blocks-new\" |\n| Top view | \"Z_3blocks\" | \"Z_3blocks-new\" |\n| Avg_speed | 1.10423 | 1.4 |\n| Time | 12.9343 | 12.9343 |\n| miniSnap | 300.3111 | 58.376 |\n\n\n\n## Reference\n\n##### Paper:\n\n[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.\n\n[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.\n\n[3] D.W.Mellinger,\"**Trajectory generation and control for quadrotors**\"Ph.D. dissertation, Univ. Pennsylvania, Philadelphia, PA, 2012.\n\n[4] D. Mellinger and V. Kumar, \"**Minimum snap trajectory generation and control for quadrotors**\", inProc. 2011 IEEE Int. Conf. Robot.Autom.,2011\n\n[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.\n\n" }, { "path": "README.md", "content": "# AerialRobotics\nSimulate the path planning and trajectory planning of quadrotors/UAVs.\n\n\n\n## Motion Planning project\n#### The main purpose:\n- A new method to improve path planning\n\n- Compare JPS and new method\n\n#### Functions already implemented:\n\n- Path planning algorithms (Jump point search, JPS-PF )\n\n- Generating Convex Polytopes(Safe Flight Corridors)\n\n- Time Allocation(Average time allocation, Trapezoidal time allocation)\n\n- Trajectory planning(QP)\n\n- Trajectory following control(PD controller)\n\nThe more detailed data is in [Motion_Planning.md](Motion_Planning.md)\n\n#### Demo animation: \n\n\"polyhedron-1\"\n\n\"3dCorner-new-ys\"\n\n## Trajectory Planning project\n#### The main purpose:\n- Compare different pathfinding algorithms(Dijkstra, Astar, Jump point search)\n- Compare different trajectory planning methods(Ax = b, QP)\n\n#### Functions already implemented:\n- Path planning algorithms (Dijkstra, Astar, Jump point search)\n- Generating Convex Polytopes(Safe Flight Corridors)\n- Trajectory planning(Ax = b, QP)\n- Trajectory following control(PD controller)\n\nThe more detailed data is in [Trajectory_Planning.md](Trajectory_Planning.md)\n\n#### Demo animation: \n\n\"polyhedron-1\"\n\n\n\n\n\n\n\n**More related knowledge introduction on [Wiki](https://github.com/LenaShengzhen/AerialRobotics/wiki)**\n\n**New feature schedule on [projects](https://github.com/LenaShengzhen/AerialRobotics/projects/2)**\n\n**Bug and issue tracking on [projects](https://github.com/LenaShengzhen/AerialRobotics/projects/1)**\n\n\n\n\n\n## Reference\n\n[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.\n\n[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.\n\n[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.\n\n[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.\n\n[5] Xingguang Zhong, Yuwei Wu, Dong Wang, Qianhao Wang, Chao Xu, and Fei Gao, \"**Generating Large Convex Polytopes Directly on Point Clouds**\",2020\n\n[6] D.W.Mellinger,\"**Trajectory generation and control for quadrotors**\"Ph.D. dissertation, Univ. Pennsylvania, Philadelphia, PA, 2012.\n\n[7] D. Mellinger and V. Kumar, \"**Minimum snap trajectory generation and control for quadrotors**\", inProc. 2011 IEEE Int. Conf. Robot.Autom.,2011\n\n[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.\n\n[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.\n\n[10] Daniel Mellinger, Alex Kushleyev and Vijay Kumar,\"**mixed-integer quadratic program trajectory generation for heterogeneous quadrotor teams**\", IEEE, 2012" }, { "path": "Trajectory_Planning.md", "content": "# Trajectory Planning\n\n\n\n\"polyhedron-1\"\n\n\n\n- Path planning algorithms (Dijkstra, Astar, Jump point search)\n- Generating Convex Polytopes(Safe Flight Corridors)\n- Trajectory planning(Ax = b, QP)\n- Trajectory following control(PD controller)\n\n\n\n## Required\n\nMATLAB(R2019b is tested)\n\n\n\n## Pic\n\n\n\n### 1. Path Planning\n\n| dijkstra | Astar | Jump Point Search |\n| -------------------------------------------------------- | -------------------------------------------------- | ---------------------------------------------- |\n| \"dijkstra\" | \"Astar\" | \"JPS\" |\n| 1.642271 seconds | 0.755504 seconds | 0.451664 seconds |\n\n \n\n### 2. Generating Convex Polytopes\n\n### \tSafe Flight Corridors\n\n- #### find_ellipsoid\n\n \"polyhedron-1\"\"polyhedron-2\"\n\n- #### find_polyhedron\n\n\"polyhedron-1\"\"polyhedron-2\"\n\n\n\n### 3. Trajectory planning\n\nCompare different Trajectory planning：\n\n- #### Trajectory 1: use Ax = b get Trajectory.\n\n\"closeForm\"\"closeForm\"\"closeForm\" \n\nThe Trajectory pass every Path point.\n\n- #### Trajectory 2: use 'Quadratic Programming' get Trajectory. use corridor constraints make Ax< b. x y z separately find minimum-snap.\n\n\"corridorConstraints\"\"corridorConstraints\"\"corridorConstraints\" \n\nThe Trajectory don't need to pass every Path point.\n\nminSnapValue(X + Y + Z) is : 9376.0901\n\n\n\n- #### Trajectory 3: use 'Quadratic Programming' get Trajectory. use SFC make Ax < b.\n\n\"SFC\"\"SFC\"\"SFC\" \n\nThe Trajectory don't need to pass every Path point.\n\nminSnapValue(X + Y + Z) is : 2958.5877\n\n\n\n### 4. Trajectory following control\n\nTrajectory following by use PD Controller, you can learn by [coursera](https://www.coursera.org/learn/robotics-flight/home/welcome).\n\n\n\n## Reference\n\n##### Paper:\n\n[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.\n\n[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.\n\n[3] D.W.Mellinger,\"**Trajectory generation and control for quadrotors**\"Ph.D. dissertation, Univ. Pennsylvania, Philadelphia, PA, 2012.\n\n[4] D. Mellinger and V. Kumar, \"**Minimum snap trajectory generation and control for quadrotors**\", inProc. 2011 IEEE Int. Conf. Robot.Autom.,2011\n\n[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.\n\n##### Code:\n\nsafe flight corridors: [C++ version](https://github.com/sikang/DecompUtil)\n\n" } ]