├── .gitignore ├── examples ├── grid │ ├── .gitignore │ └── main_grid.m ├── octomap │ ├── .gitignore │ ├── warehouse4.bt │ ├── warehouse4.stl │ └── main_octomap.m └── no_obstacles │ ├── .gitignore │ └── main_no_obstacles.m ├── octomap_corridor ├── .gitignore ├── Makefile ├── cvxgen │ ├── util.c │ └── solver.h ├── array2d.hpp └── octomap_corridor.cpp ├── external ├── README.md ├── jsonlab │ ├── README.txt │ ├── examples │ │ ├── example3.json │ │ ├── example1.json │ │ ├── example2.json │ │ ├── example4.json │ │ ├── jsonlab_speedtest.m │ │ ├── jsonlab_selftest.m │ │ ├── jsonlab_selftest.matlab │ │ ├── demo_jsonlab_basic.m │ │ └── demo_ubjson_basic.m │ ├── mergestruct.m │ ├── jsonopt.m │ ├── varargin2struct.m │ ├── LICENSE_BSD.txt │ ├── license.txt │ ├── AUTHORS.txt │ ├── struct2jdata.m │ └── ChangeLog.txt ├── stlread │ ├── stldemo.m │ ├── license.txt │ └── stlread.m └── noredund.m ├── .gitmodules ├── Makefile ├── utils ├── deriv_matrix.m ├── dim_collect_matrix.m ├── pp_sample_piece.m ├── diff_matrix.m ├── bernstein.m ├── read_grid_map.m ├── polystretchtime.m ├── refine_schedule.m ├── plot3n.m ├── corners_to_box_vertices.m ├── polytope_erode_by_ellipsoid.m ├── read_schedule.m ├── int_sqr_deriv_matrix.m ├── pp2csv.m ├── path_linear_pps.m ├── read_octomap_bbox_mex.cpp ├── analyze_schedule.m ├── box_cover.m ├── read_halfspaces.m ├── read_pptrajs.m └── write_pptrajs.m ├── mex_octomap.m ├── .gitattributes ├── mex_required.m ├── tests ├── test_pptrajs_fileio.m └── test_Qmat.m ├── pp_obs_sep_none.m ├── svm_sephyp_2point ├── make_csolve.m ├── description.cvxgen ├── cvxsolve.m ├── Makefile ├── csolve.m ├── matrix_support.c ├── testsolver.c ├── util.c ├── all_hyperplanes_waypoints_mex.c ├── solver.h ├── csolve.c └── solver.c ├── svm_sephyp_32point ├── make_csolve.m ├── description.cvxgen ├── cvxsolve.m ├── Makefile ├── svm32_mex.m ├── util.c ├── all_hyperplanes_pps_mex.c ├── solver.h ├── all_hyperplanes_robot_obstacle_mex.c ├── svm32_mex.c └── solver.c ├── pp_obs_sep_octomap.m ├── all_hyperplanes_pps.m ├── pp_obs_sep_grid.m ├── smoothener.m ├── corridor_trajectory_optimize_sequence.m ├── README.md └── corridor_trajectory_optimize.m /.gitignore: -------------------------------------------------------------------------------- 1 | *.mexa64 2 | -------------------------------------------------------------------------------- /examples/grid/.gitignore: -------------------------------------------------------------------------------- 1 | maze50_pps/* 2 | -------------------------------------------------------------------------------- /examples/octomap/.gitignore: -------------------------------------------------------------------------------- 1 | warehouse4_pps/* 2 | -------------------------------------------------------------------------------- /examples/no_obstacles/.gitignore: -------------------------------------------------------------------------------- 1 | no_obstacles_pps/* 2 | -------------------------------------------------------------------------------- /octomap_corridor/.gitignore: -------------------------------------------------------------------------------- 1 | # binaries 2 | octomap_corridor 3 | *.o 4 | -------------------------------------------------------------------------------- /external/README.md: -------------------------------------------------------------------------------- 1 | This directory contains third-party code from the Matlab file exchange. 2 | -------------------------------------------------------------------------------- /external/jsonlab/README.txt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jpreiss/smoothener/HEAD/external/jsonlab/README.txt -------------------------------------------------------------------------------- /examples/octomap/warehouse4.bt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jpreiss/smoothener/HEAD/examples/octomap/warehouse4.bt -------------------------------------------------------------------------------- /examples/octomap/warehouse4.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jpreiss/smoothener/HEAD/examples/octomap/warehouse4.stl -------------------------------------------------------------------------------- /.gitmodules: -------------------------------------------------------------------------------- 1 | [submodule "octomap_corridor/octomap"] 2 | path = octomap_corridor/octomap 3 | url = https://github.com/OctoMap/octomap 4 | -------------------------------------------------------------------------------- /Makefile: -------------------------------------------------------------------------------- 1 | smoothener: 2 | matlab -nosplash -nodesktop -nojvm -r "mex_required,quit" 3 | 4 | octomap: 5 | matlab -nosplash -nodesktop -nojvm -r "mex_octomap,quit" 6 | make -C octomap_corridor 7 | 8 | clean: 9 | make clean -C octomap_corridor 10 | find . -name "*.mexa64" -type f -delete 11 | -------------------------------------------------------------------------------- /utils/deriv_matrix.m: -------------------------------------------------------------------------------- 1 | % returns the matrix M such that, if x is coefficients of 2 | % a k-degree polynomial in descending order (a polyval input), 3 | % M * x is the coefficients of x's derivative. 4 | function D = deriv_matrix(k) 5 | s = k:(-1):0; 6 | D = diag(s); 7 | D(2:end,:) = D(1:(end-1),:); 8 | D(1,:) = 0; 9 | end 10 | -------------------------------------------------------------------------------- /mex_octomap.m: -------------------------------------------------------------------------------- 1 | % compiles the optional mex functions that use Octomap. "make octomap" runs this for you. 2 | 3 | cd utils 4 | mex CXXFLAGS='$CXXFLAGS -I../octomap_corridor/octomap/octomap/include -std=c++11' read_octomap_bbox_mex.cpp ../octomap_corridor/octomap/octomap/lib/liboctomap.a ../octomap_corridor/octomap/octomap/lib/liboctomath.a 5 | cd .. 6 | -------------------------------------------------------------------------------- /external/jsonlab/examples/example3.json: -------------------------------------------------------------------------------- 1 | {"menu": { 2 | "id": "file", 3 | "value": "_&File", 4 | "popup": { 5 | "menuitem": [ 6 | {"value": "_&New", "onclick": "CreateNewDoc(\"'\\\"Untitled\\\"'\")"}, 7 | {"value": "_&Open", "onclick": "OpenDoc()"}, 8 | {"value": "_&Close", "onclick": "CloseDoc()"} 9 | ] 10 | } 11 | }} 12 | -------------------------------------------------------------------------------- /utils/dim_collect_matrix.m: -------------------------------------------------------------------------------- 1 | % permutation matrix to convert [xyz ... xyz]' into [x...x y...y z...z]' 2 | % d: dimension (i.e. 3 for 3d points), k: number of points 3 | function P = dim_collect_matrix(d, k) 4 | P = []; 5 | for i=1:d 6 | s = zeros(1,d); 7 | s(i) = 1; 8 | P = [P; kron(eye(k), s)]; 9 | end 10 | assert(all(size(P) == k * d)); 11 | end 12 | -------------------------------------------------------------------------------- /utils/pp_sample_piece.m: -------------------------------------------------------------------------------- 1 | % samples equally spaced points of the th piece of ppform struct 2 | function pts = pp_sample_piece(pp, ipiece, npts) 3 | t0 = pp.breaks(ipiece); 4 | t1 = pp.breaks(ipiece + 1); 5 | dt = (t1 - t0) / (npts - 1); 6 | t = t0 + dt * (0:(npts - 1)); 7 | assert(abs(t(end) - t1) < 10e-9); 8 | pts = ppval(pp, t); 9 | end 10 | -------------------------------------------------------------------------------- /utils/diff_matrix.m: -------------------------------------------------------------------------------- 1 | function D = diff_matrix(dim, npts, dt, order) 2 | first_row = [-1 1 zeros(1, npts - 2)]; 3 | first_col = [-1 0 zeros(1, npts - 3)]; 4 | D1 = kron(toeplitz(first_col, first_row), eye(dim) ./ dt); 5 | D = D1; 6 | dims = size(D1); 7 | for i=order:(-1):2 8 | dims = dims - dim; 9 | D1 = D1(1:dims(1),1:dims(2)); 10 | D = D1 * D; 11 | end 12 | end 13 | -------------------------------------------------------------------------------- /octomap_corridor/Makefile: -------------------------------------------------------------------------------- 1 | CXXFLAGS = -std=c++11 -Ioctomap/octomap/include -O3 2 | CFLAGS = -O3 -x c 3 | 4 | CC = $(CXX) 5 | 6 | octomap_corridor : octomap_corridor.o \ 7 | octomap/octomap/lib/liboctomap.a octomap/octomap/lib/liboctomath.a \ 8 | cvxgen/ldl.o cvxgen/matrix_support.o cvxgen/solver.o cvxgen/util.o 9 | 10 | clean: 11 | rm -rf cvxgen/*.o 12 | rm -rf octomap_corridor *.o 13 | -------------------------------------------------------------------------------- /utils/bernstein.m: -------------------------------------------------------------------------------- 1 | % compute the Berenstein polynomials of degree k. 2 | % returns a (k+1) x (k+1) matrix where each row is a polynomial 3 | % descending from x^k coefficient to constant coefficient. 4 | function B = bernstein(n) 5 | B = zeros(n+1); 6 | for k = 0:n 7 | for i=k:n 8 | B(k+1,i+1) = (-1)^(i-k) * nchoosek(n, i) * nchoosek(i, k); 9 | end 10 | end 11 | B = fliplr(B); 12 | end 13 | -------------------------------------------------------------------------------- /.gitattributes: -------------------------------------------------------------------------------- 1 | *csolve.c linguist-generated=true 2 | *csolve.m linguist-generated=true 3 | *cvxsolve.m linguist-generated=true 4 | *ldl.c linguist-generated=true 5 | *make_csolve.m linguist-generated=true 6 | *matrix_support.c linguist-generated=true 7 | *solver.c linguist-generated=true 8 | *solver.h linguist-generated=true 9 | *testsolver.c linguist-generated=true 10 | *util.c linguist-generated=true 11 | -------------------------------------------------------------------------------- /utils/read_grid_map.m: -------------------------------------------------------------------------------- 1 | % read a map from Wolfgang's discrete planner 2 | % dims: room box dimensions - grid cells 0 thru dims, inclusive, are valid 3 | % obstacles: 3xN list of obstacle grid cells. 1unit cubes surround 4 | % 5 | function [dims, obstacles] = read_map(fname) 6 | json = loadjson(fname); 7 | dims = json.dimensions; 8 | obstacles = json.obstacles'; 9 | if isempty(obstacles) 10 | obstacles = []; 11 | end 12 | end 13 | -------------------------------------------------------------------------------- /mex_required.m: -------------------------------------------------------------------------------- 1 | % compiles all non-optional mex functions. "make" runs this for you. 2 | 3 | cd svm_sephyp_2point 4 | mex CFLAGS='$CFLAGS -std=c99' all_hyperplanes_waypoints_mex.c ldl.c matrix_support.c solver.c util.c 5 | cd .. 6 | 7 | cd svm_sephyp_32point 8 | mex CFLAGS='$CFLAGS -std=c99' all_hyperplanes_pps_mex.c ldl.c matrix_support.c solver.c util.c 9 | mex CFLAGS='$CFLAGS -std=c99' svm32_mex.c ldl.c matrix_support.c solver.c util.c 10 | cd .. 11 | -------------------------------------------------------------------------------- /tests/test_pptrajs_fileio.m: -------------------------------------------------------------------------------- 1 | function test_pptrajs_fileio() 2 | L = load('drones32b.mat'); 3 | pps = L.pps(end,:); 4 | fname = 'temp.pptrajs'; 5 | write_pptrajs(pps, fname); 6 | pps2 = read_pptrajs(fname); 7 | assert(length(pps) == length(pps2)); 8 | N = length(pps); 9 | for i=1:N 10 | pp1 = pps{i}; 11 | pp2 = pps2{i}; 12 | assert_pps_match(pp1, pp2); 13 | assert(all(single(pp1.coefs(:)) == single(pp2.coefs(:)))); 14 | end 15 | end 16 | -------------------------------------------------------------------------------- /utils/polystretchtime.m: -------------------------------------------------------------------------------- 1 | % inputs 2 | % p: polynomial coefs, descending, i.e. a polyval input 3 | % s: time scale, e.g. if s==2 the new polynomial will take 2x longer 4 | % 5 | % output: 6 | % stretched polynomial 7 | % 8 | function p = polystretchtime(p, s) 9 | p = flip(p); 10 | recip = 1.0 / s; 11 | scale = recip; 12 | order = length(p); 13 | for i=2:order 14 | p(i) = scale * p(i); 15 | scale = scale * recip; 16 | end 17 | p = flip(p); 18 | end 19 | -------------------------------------------------------------------------------- /utils/refine_schedule.m: -------------------------------------------------------------------------------- 1 | % given a waypoint plan, create a new waypoint plan 2 | % with a waypoint added at the middle of each segment. 3 | % 4 | % input: [3 k N] waypoints 5 | % output: [3 (2k - 1) N] waypoints 6 | % 7 | function r = refine_schedule(s) 8 | [~, k, N] = size(s); 9 | k2 = 2 * k - 1; 10 | r = zeros(3, k2, N); 11 | r(:,1,:) = s(:,1,:); 12 | delta = diff(s, [], 2) ./ 2; 13 | r(:,1:2:k2,:) = s; 14 | r(:,2:2:(k2-1),:) = s(:,1:(k-1),:) + delta; 15 | end 16 | -------------------------------------------------------------------------------- /utils/plot3n.m: -------------------------------------------------------------------------------- 1 | % for [3 n] array X, replace 2 | % plot3(X(1,:), X(2,:), X(3,:), args...) 3 | % with 4 | % plot3n(X, args...) 5 | % 6 | % also handles [n 3] arrays. 7 | % 8 | function h = plot3n(x, varargin) 9 | sz = size(x); 10 | assert(length(sz) == 2); 11 | if sz(1) == 3 12 | h = plot3(x(1,:), x(2,:), x(3,:), varargin{:}); 13 | elseif sz(2) == 3 14 | h = plot3(x(:,1), x(:,2), x(:,3), varargin{:}); 15 | else 16 | error('argument x must be 3xN or Nx3'); 17 | end 18 | end 19 | 20 | -------------------------------------------------------------------------------- /tests/test_Qmat.m: -------------------------------------------------------------------------------- 1 | function test_qmat() 2 | trials = 100; 3 | for i=1:trials 4 | deg = 4 + floor(rand() * 6); 5 | p = rand(1,deg+1); 6 | 7 | T = 2*rand() + 1; 8 | assert(T >= 1); 9 | Q = int_sqr_deriv_matrix(deg, 2, T); 10 | 11 | x1 = p * Q * p'; 12 | x2 = non_matrix_version(p, T); 13 | 14 | assert(abs(x1 - x2) < 0.001); 15 | end 16 | end 17 | 18 | function i = non_matrix_version(p, T) 19 | d2p = polyder(polyder(p)); 20 | dsqr = conv(d2p, d2p); 21 | intsqr = polyint(dsqr); 22 | i = polyval(intsqr, T); 23 | end 24 | -------------------------------------------------------------------------------- /pp_obs_sep_none.m: -------------------------------------------------------------------------------- 1 | % dummy function that returns empty robot-obstacle halfspace constraints 2 | % for problems with no obstacles. 3 | % 4 | % inputs: 5 | % pps: {N} cell array of matlab ppform structs 6 | % obs_ellipsoid: [3] radii of ellipsoid for robot/obstacle collision model 7 | % 8 | % outputs: 9 | % polytopes: {N k-1} cell array of 0x4 matrices representing no constraints 10 | % 11 | function polytopes = pp_obs_sep_none(pps, obs_ellipsoid) 12 | 13 | N = length(pps); 14 | k = length(pps{1}.breaks); 15 | polytopes = repmat({zeros(0,4)}, N, k-1); 16 | end 17 | -------------------------------------------------------------------------------- /external/jsonlab/examples/example1.json: -------------------------------------------------------------------------------- 1 | { 2 | "firstName": "John", 3 | "lastName": "Smith", 4 | "age": 25, 5 | "address": 6 | { 7 | "streetAddress": "21 2nd Street", 8 | "city": "New York", 9 | "state": "NY", 10 | "postalCode": "10021" 11 | }, 12 | "phoneNumber": 13 | [ 14 | { 15 | "type": "home", 16 | "number": "212 555-1234" 17 | }, 18 | { 19 | "type": "fax", 20 | "number": "646 555-4567" 21 | } 22 | ] 23 | } 24 | -------------------------------------------------------------------------------- /utils/corners_to_box_vertices.m: -------------------------------------------------------------------------------- 1 | % given the lower and upper bounding corners of a box, 2 | % returns an array of all the box vertices. 3 | % 4 | % inputs: 5 | % x0: [3] lower corner of box. 6 | % x1: [3] upper corner of box. 7 | % 8 | % outputs: 9 | % verts: [8 3] all box corners. 10 | % 11 | function verts = corners_to_box_vertices(x0, x1) 12 | verts = [... 13 | x0(1) x0(2) x0(3); 14 | x0(1) x0(2) x1(3); 15 | x0(1) x1(2) x0(3); 16 | x0(1) x1(2) x1(3); 17 | x1(1) x0(2) x0(3); 18 | x1(1) x0(2) x1(3); 19 | x1(1) x1(2) x0(3); 20 | x1(1) x1(2) x1(3)]; 21 | end 22 | -------------------------------------------------------------------------------- /svm_sephyp_2point/make_csolve.m: -------------------------------------------------------------------------------- 1 | % Produced by CVXGEN, 2016-11-30 17:09:32 -0500. 2 | % CVXGEN is Copyright (C) 2006-2012 Jacob Mattingley, jem@cvxgen.com. 3 | % The code in this file is Copyright (C) 2006-2012 Jacob Mattingley. 4 | % CVXGEN, or solvers produced by CVXGEN, cannot be used for commercial 5 | % applications without prior written permission from Jacob Mattingley. 6 | 7 | % Filename: make_csolve.m. 8 | % Description: Calls mex to generate the csolve mex file. 9 | %mex -v csolve.c ldl.c matrix_support.c solver.c util.c 10 | mex csolve.c ldl.c matrix_support.c solver.c util.c 11 | -------------------------------------------------------------------------------- /svm_sephyp_32point/make_csolve.m: -------------------------------------------------------------------------------- 1 | % Produced by CVXGEN, 2016-11-30 16:59:57 -0500. 2 | % CVXGEN is Copyright (C) 2006-2012 Jacob Mattingley, jem@cvxgen.com. 3 | % The code in this file is Copyright (C) 2006-2012 Jacob Mattingley. 4 | % CVXGEN, or solvers produced by CVXGEN, cannot be used for commercial 5 | % applications without prior written permission from Jacob Mattingley. 6 | 7 | % Filename: make_csolve.m. 8 | % Description: Calls mex to generate the csolve mex file. 9 | %mex -v csolve.c ldl.c matrix_support.c solver.c util.c 10 | mex csolve.c ldl.c matrix_support.c solver.c util.c 11 | -------------------------------------------------------------------------------- /utils/polytope_erode_by_ellipsoid.m: -------------------------------------------------------------------------------- 1 | % A, b: half-space representation of polytope, {x : Ax <= b} 2 | % ellipsoid: diagonal matrix D of axis-aligned ellipsoid radii 3 | % 4 | % output: new right-hand-side b' for the same A matrix such that, 5 | % if Ax <= b', then the ellipsoid of given radii centered at x 6 | % lies inside the original polytope Ax <= b 7 | % 8 | function b = polytope_erode_by_ellipsoid(A, b, ellipsoid) 9 | [nr, nc] = size(A); 10 | for i=1:nr 11 | normal = A(i,:); 12 | if ~any(isnan(normal)) 13 | assert(abs(norm(normal) - 1) < 0.00001); 14 | b(i) = b(i) - norm(ellipsoid * normal'); 15 | end 16 | end 17 | end 18 | -------------------------------------------------------------------------------- /utils/read_schedule.m: -------------------------------------------------------------------------------- 1 | % read a schedule from Wolfgang's discrete planner 2 | % into a [3 x Kpts x Nrobots] waypoint array 3 | % 4 | function paths = read_schedule(fname) 5 | json = loadjson(fname); 6 | N = length(json.agents); 7 | k = 0; 8 | for i=1:N 9 | k = max(k, length(json.agents{i}.path)); 10 | end 11 | paths = nan(3,k,N); 12 | for i=1:N 13 | p = json.agents{i}.path; 14 | len = length(p); 15 | for j=1:len 16 | paths(1,j,i) = str2num(p{j}.x); 17 | paths(2,j,i) = str2num(p{j}.y); 18 | paths(3,j,i) = str2num(p{j}.z); 19 | end 20 | for j=(len+1):k 21 | paths(:,j,i) = paths(:,len,i); 22 | end 23 | end 24 | assert(~any(isnan(paths(:)))); 25 | end 26 | -------------------------------------------------------------------------------- /external/jsonlab/examples/example2.json: -------------------------------------------------------------------------------- 1 | { 2 | "glossary": { 3 | "title": "example glossary", 4 | "GlossDiv": { 5 | "title": "S", 6 | "GlossList": { 7 | "GlossEntry": { 8 | "ID": "SGML", 9 | "SortAs": "SGML", 10 | "GlossTerm": "Standard Generalized Markup Language", 11 | "Acronym": "SGML", 12 | "Abbrev": "ISO 8879:1986", 13 | "GlossDef": { 14 | "para": "A meta-markup language, used to create markup languages such as DocBook.", 15 | "GlossSeeAlso": ["GML", "XML"] 16 | }, 17 | "GlossSee": "markup" 18 | } 19 | } 20 | } 21 | } 22 | } 23 | -------------------------------------------------------------------------------- /external/jsonlab/examples/example4.json: -------------------------------------------------------------------------------- 1 | [ 2 | { 3 | "sample" : { 4 | "rho" : 1 5 | } 6 | }, 7 | { 8 | "sample" : { 9 | "rho" : 2 10 | } 11 | }, 12 | [ 13 | { 14 | "_ArrayType_" : "double", 15 | "_ArraySize_" : [1,2], 16 | "_ArrayData_" : [1,0] 17 | }, 18 | { 19 | "_ArrayType_" : "double", 20 | "_ArraySize_" : [1,2], 21 | "_ArrayData_" : [1,1] 22 | }, 23 | { 24 | "_ArrayType_" : "double", 25 | "_ArraySize_" : [1,2], 26 | "_ArrayData_" : [1,2] 27 | } 28 | ], 29 | [ 30 | "Paper", 31 | "Scissors", 32 | "Stone" 33 | ], 34 | ["a", "b\\", "c\"","d\\\"","e\"[","f\\\"[","g[\\","h[\\\""] 35 | ] 36 | -------------------------------------------------------------------------------- /svm_sephyp_2point/description.cvxgen: -------------------------------------------------------------------------------- 1 | # Produced by CVXGEN, 2016-11-30 17:09:31 -0500. 2 | # CVXGEN is Copyright (C) 2006-2012 Jacob Mattingley, jem@cvxgen.com. 3 | # The code in this file is Copyright (C) 2006-2012 Jacob Mattingley. 4 | # CVXGEN, or solvers produced by CVXGEN, cannot be used for commercial 5 | # applications without prior written permission from Jacob Mattingley. 6 | 7 | # Filename: description.cvxgen. 8 | # Description: A description of the CVXGEN problem. 9 | 10 | dimensions 11 | dim = 3 12 | npts = 4 13 | end 14 | 15 | parameters 16 | A(npts, dim+1) 17 | Q(dim+1, dim+1) diagonal psd 18 | end 19 | 20 | variables 21 | beta (dim + 1) 22 | end 23 | 24 | minimize 25 | quad(beta, Q) 26 | subject to 27 | A*beta <= -1 28 | end 29 | -------------------------------------------------------------------------------- /svm_sephyp_32point/description.cvxgen: -------------------------------------------------------------------------------- 1 | # Produced by CVXGEN, 2016-11-30 16:59:55 -0500. 2 | # CVXGEN is Copyright (C) 2006-2012 Jacob Mattingley, jem@cvxgen.com. 3 | # The code in this file is Copyright (C) 2006-2012 Jacob Mattingley. 4 | # CVXGEN, or solvers produced by CVXGEN, cannot be used for commercial 5 | # applications without prior written permission from Jacob Mattingley. 6 | 7 | # Filename: description.cvxgen. 8 | # Description: A description of the CVXGEN problem. 9 | 10 | dimensions 11 | dim = 3 12 | npts = 64 13 | end 14 | 15 | parameters 16 | A(npts, dim+1) 17 | Q(dim+1, dim+1) diagonal psd 18 | end 19 | 20 | variables 21 | beta (dim + 1) 22 | end 23 | 24 | minimize 25 | quad(beta, Q) 26 | subject to 27 | A*beta <= -1 28 | end 29 | -------------------------------------------------------------------------------- /external/jsonlab/examples/jsonlab_speedtest.m: -------------------------------------------------------------------------------- 1 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 2 | % Benchmarking processing speed of savejson and loadjson 3 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 4 | 5 | datalen=[1e3 1e4 1e5 1e6]; 6 | len=length(datalen); 7 | tsave=zeros(len,1); 8 | tload=zeros(len,1); 9 | for i=1:len 10 | tic; 11 | json=savejson('data',struct('d1',rand(datalen(i),3),'d2',rand(datalen(i),3)>0.5)); 12 | tsave(i)=toc; 13 | data=loadjson(json); 14 | tload(i)=toc-tsave(i); 15 | fprintf(1,'matrix size: %d\n',datalen(i)); 16 | end 17 | 18 | loglog(datalen,tsave,'o-',datalen,tload,'r*-'); 19 | legend('savejson runtime (s)','loadjson runtime (s)'); 20 | xlabel('array size'); 21 | ylabel('running time (s)'); 22 | -------------------------------------------------------------------------------- /utils/int_sqr_deriv_matrix.m: -------------------------------------------------------------------------------- 1 | % compute the matrix Q for a polynomial coefficent vector p 2 | % such that p' Q p = integral(square(d'th derivative of p)). 3 | % where coefs in p are given in descending (polyval) order. 4 | % 5 | % from "Polynomial trajectory planning for quadrotor flight", 6 | % C. Richter, A. Bry, N. Roy, ICRA 2013. 7 | % 8 | function Q = int_sqr_deriv_matrix(degree, deriv, T) 9 | r = deriv; 10 | order = degree+1; 11 | Q = nan(order); 12 | for i=0:degree 13 | for l=0:degree 14 | if i < r || l < r 15 | Q(i+1,l+1) = 0; 16 | else 17 | p = 1; 18 | for m=0:(r-1) 19 | p = p * (i - m) * (l - m); 20 | end 21 | x = 1 * p * T^(i + l - 2*r + 1) / (i + l - 2*r + 1); 22 | Q(i+1,l+1) = x; 23 | end 24 | end 25 | end 26 | Q = rot90(rot90(Q)); 27 | end 28 | -------------------------------------------------------------------------------- /utils/pp2csv.m: -------------------------------------------------------------------------------- 1 | % writes a matlab ppform struct to a CSV file. 2 | % TODO: document CSV format used. 3 | % 4 | function poly2csv(pp, filename) 5 | [breaks, coefs, npieces, order, dim] = unmkpp(pp); 6 | assert(dim == 3 || dim == 4); 7 | coefs = reshape(coefs, [], npieces, order); 8 | if dim == 3 9 | coefs = cat(1, coefs, zeros(1, npieces, order)); 10 | end 11 | 12 | fid = fopen(filename, 'w'); 13 | format long; 14 | vars = {'x' 'y' 'z' 'yaw'}; 15 | fprintf(fid, 'duration,'); 16 | for d=1:4 17 | for i=1:order 18 | fprintf(fid, '%s^%d,', vars{d}, i-1); 19 | end 20 | end 21 | fprintf(fid, '\n'); 22 | for piece=1:npieces 23 | duration = breaks(piece+1) - breaks(piece); 24 | fprintf(fid, '%f,', duration); 25 | for d=1:4 26 | fprintf(fid, '%f,', flipud(squeeze(coefs(d,piece,:)))); 27 | end 28 | fprintf(fid, '\n'); 29 | end 30 | end 31 | -------------------------------------------------------------------------------- /utils/path_linear_pps.m: -------------------------------------------------------------------------------- 1 | % given N sequences of waypoints, 2 | % creates N piecewise polynomial trajectories connecting the waypoints 3 | % with piecewise-linear constant velocity segments 4 | % and infinite acceleration at the waypoints. 5 | % 6 | % inputs: 7 | % paths: [dim K+1 N] waypoints 8 | % timescale: [1] the amount of time each segment should take 9 | % order: [1] the polynomial order 10 | % 11 | % output: {N} cell array of matlab ppform structs 12 | % 13 | function pps = path_linear_pps(paths, timescale, order) 14 | [dim, Kp1, N] = size(paths); 15 | K = Kp1 - 1; 16 | pps = cell(N, 1); 17 | breaks = timescale * (0:K); 18 | for i=1:N 19 | path = paths(:,:,i); 20 | coefs = zeros([dim K order]); 21 | slopes = diff(path, 1, 2) / timescale; 22 | coefs(:,:,end) = path(:,1:(end-1)); 23 | coefs(:,:,(end-1)) = slopes; 24 | pps{i} = mkpp(breaks, coefs, dim); 25 | end 26 | end 27 | -------------------------------------------------------------------------------- /external/jsonlab/mergestruct.m: -------------------------------------------------------------------------------- 1 | function s=mergestruct(s1,s2) 2 | % 3 | % s=mergestruct(s1,s2) 4 | % 5 | % merge two struct objects into one 6 | % 7 | % authors:Qianqian Fang (fangq nmr.mgh.harvard.edu) 8 | % date: 2012/12/22 9 | % 10 | % input: 11 | % s1,s2: a struct object, s1 and s2 can not be arrays 12 | % 13 | % output: 14 | % s: the merged struct object. fields in s1 and s2 will be combined in s. 15 | % 16 | % license: 17 | % BSD License, see LICENSE_BSD.txt files for details 18 | % 19 | % -- this function is part of jsonlab toolbox (http://iso2mesh.sf.net/cgi-bin/index.cgi?jsonlab) 20 | % 21 | 22 | if(~isstruct(s1) || ~isstruct(s2)) 23 | error('input parameters contain non-struct'); 24 | end 25 | if(length(s1)>1 || length(s2)>1) 26 | error('can not merge struct arrays'); 27 | end 28 | fn=fieldnames(s2); 29 | s=s1; 30 | for i=1:length(fn) 31 | s=setfield(s,fn{i},getfield(s2,fn{i})); 32 | end 33 | 34 | -------------------------------------------------------------------------------- /svm_sephyp_2point/cvxsolve.m: -------------------------------------------------------------------------------- 1 | % Produced by CVXGEN, 2016-11-30 17:09:31 -0500. 2 | % CVXGEN is Copyright (C) 2006-2012 Jacob Mattingley, jem@cvxgen.com. 3 | % The code in this file is Copyright (C) 2006-2012 Jacob Mattingley. 4 | % CVXGEN, or solvers produced by CVXGEN, cannot be used for commercial 5 | % applications without prior written permission from Jacob Mattingley. 6 | 7 | % Filename: cvxsolve.m. 8 | % Description: Solution file, via cvx, for use with sample.m. 9 | function [vars, status] = cvxsolve(params, settings) 10 | A = params.A; 11 | Q = params.Q; 12 | cvx_begin 13 | % Caution: automatically generated by cvxgen. May be incorrect. 14 | variable beta(4, 1); 15 | 16 | minimize(quad_form(beta, Q)); 17 | subject to 18 | A*beta <= -1; 19 | cvx_end 20 | vars.beta = beta; 21 | status.cvx_status = cvx_status; 22 | % Provide a drop-in replacement for csolve. 23 | status.optval = cvx_optval; 24 | status.converged = strcmp(cvx_status, 'Solved'); 25 | -------------------------------------------------------------------------------- /svm_sephyp_32point/cvxsolve.m: -------------------------------------------------------------------------------- 1 | % Produced by CVXGEN, 2016-11-30 16:59:55 -0500. 2 | % CVXGEN is Copyright (C) 2006-2012 Jacob Mattingley, jem@cvxgen.com. 3 | % The code in this file is Copyright (C) 2006-2012 Jacob Mattingley. 4 | % CVXGEN, or solvers produced by CVXGEN, cannot be used for commercial 5 | % applications without prior written permission from Jacob Mattingley. 6 | 7 | % Filename: cvxsolve.m. 8 | % Description: Solution file, via cvx, for use with sample.m. 9 | function [vars, status] = cvxsolve(params, settings) 10 | A = params.A; 11 | Q = params.Q; 12 | cvx_begin 13 | % Caution: automatically generated by cvxgen. May be incorrect. 14 | variable beta(4, 1); 15 | 16 | minimize(quad_form(beta, Q)); 17 | subject to 18 | A*beta <= -1; 19 | cvx_end 20 | vars.beta = beta; 21 | status.cvx_status = cvx_status; 22 | % Provide a drop-in replacement for csolve. 23 | status.optval = cvx_optval; 24 | status.converged = strcmp(cvx_status, 'Solved'); 25 | -------------------------------------------------------------------------------- /utils/read_octomap_bbox_mex.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | % input: string to octomap path 3 | % output: bbox 3x2 4 | */ 5 | 6 | #include "mex.h" 7 | 8 | #include 9 | #include 10 | 11 | void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) { 12 | if (nrhs != 1) { 13 | mexErrMsgTxt("need nrhs == 1\n"); 14 | } 15 | if (nlhs != 1) { 16 | mexErrMsgTxt("need nlhs == 1\n"); 17 | } 18 | 19 | /* input must be a string */ 20 | if (!mxIsChar(prhs[0])) { 21 | mexErrMsgTxt("Input must be a string.\n"); 22 | } 23 | 24 | char const *path = mxArrayToString(prhs[0]); 25 | 26 | if (path == NULL) { 27 | mexErrMsgTxt("Could not convert input to string.\n"); 28 | } 29 | 30 | /* set C-style string output_buf to MATLAB mexFunction output*/ 31 | plhs[0] = mxCreateDoubleMatrix(3, 2, mxREAL); 32 | double *b = mxGetPr(plhs[0]); 33 | 34 | octomap::OcTree input(path); 35 | input.getMetricMin(b[0], b[1], b[2]); 36 | input.getMetricMax(b[3], b[4], b[5]); 37 | } 38 | -------------------------------------------------------------------------------- /svm_sephyp_2point/Makefile: -------------------------------------------------------------------------------- 1 | # Produced by CVXGEN, 2016-11-30 17:09:31 -0500. 2 | # CVXGEN is Copyright (C) 2006-2012 Jacob Mattingley, jem@cvxgen.com. 3 | # The code in this file is Copyright (C) 2006-2012 Jacob Mattingley. 4 | # CVXGEN, or solvers produced by CVXGEN, cannot be used for commercial 5 | # applications without prior written permission from Jacob Mattingley. 6 | 7 | # Filename: Makefile. 8 | # Description: Basic Makefile. 9 | OPT = -Wall -Os 10 | # libmath is needed for sqrt, which is used only for reporting the gap. Can 11 | # remove if desired for production solvers.. 12 | LDLIBS = -lm 13 | CFLAGS = $(OPT) $(INCLUDES) 14 | CC = gcc 15 | .PHONY: all 16 | all: testsolver 17 | testsolver: solver.o matrix_support.o ldl.o testsolver.o util.o 18 | # Include util.o for random functions and easy matrix printing. 19 | #testsolver: solver.o matrix_support.o ldl.o util.o testsolver.o 20 | solver.o: solver.h 21 | matrix_support.o: solver.h 22 | ldl.o: solver.h 23 | util.o: solver.h 24 | testsolver.o: solver.h 25 | .PHONY : clean 26 | clean : 27 | -rm -f *.o testsolver 28 | -------------------------------------------------------------------------------- /svm_sephyp_32point/Makefile: -------------------------------------------------------------------------------- 1 | # Produced by CVXGEN, 2016-11-30 16:59:55 -0500. 2 | # CVXGEN is Copyright (C) 2006-2012 Jacob Mattingley, jem@cvxgen.com. 3 | # The code in this file is Copyright (C) 2006-2012 Jacob Mattingley. 4 | # CVXGEN, or solvers produced by CVXGEN, cannot be used for commercial 5 | # applications without prior written permission from Jacob Mattingley. 6 | 7 | # Filename: Makefile. 8 | # Description: Basic Makefile. 9 | OPT = -Wall -Os 10 | # libmath is needed for sqrt, which is used only for reporting the gap. Can 11 | # remove if desired for production solvers.. 12 | LDLIBS = -lm 13 | CFLAGS = $(OPT) $(INCLUDES) 14 | CC = gcc 15 | .PHONY: all 16 | all: testsolver 17 | testsolver: solver.o matrix_support.o ldl.o testsolver.o util.o 18 | # Include util.o for random functions and easy matrix printing. 19 | #testsolver: solver.o matrix_support.o ldl.o util.o testsolver.o 20 | solver.o: solver.h 21 | matrix_support.o: solver.h 22 | ldl.o: solver.h 23 | util.o: solver.h 24 | testsolver.o: solver.h 25 | .PHONY : clean 26 | clean : 27 | -rm -f *.o testsolver 28 | -------------------------------------------------------------------------------- /utils/analyze_schedule.m: -------------------------------------------------------------------------------- 1 | % input: [dim, nsteps, nrobots] discrete plan 2 | function analyze_schedule(paths) 3 | [dim, steps, nrobots] = size(paths); 4 | fprintf('%d robots, %d timesteps\n', nrobots, steps); 5 | waittot = 0; 6 | for r=1:nrobots 7 | delta = diff(paths(:,:,r), [], 2); 8 | stationary = all(delta == 0, 1); 9 | waittot = waittot + sum(stationary); 10 | end 11 | fprintf('%d / %d steps are waits\n', waittot, steps * nrobots); 12 | zmin = Inf; 13 | for s=1:steps 14 | xypos = squeeze(paths(1:2,s,:)); 15 | d = squareform(pdist(xypos')); 16 | d = d + diag(nan(1,nrobots)); 17 | [row, col] = find(d == 0); 18 | for i=1:length(row) 19 | zdist = abs(paths(3,s,row(i)) - paths(3,s,col(i))); 20 | zmin = min(zmin, zdist); 21 | end 22 | end 23 | fprintf('%d minimum stacked z distance\n', zmin); 24 | allxyz = reshape(paths, dim, []); 25 | lb = min(allxyz, [], 2); 26 | ub = max(allxyz, [], 2); 27 | fprintf('bounding box: \n\tmin = [%2.3f %2.3f %2.3f]\n\tmax = [%2.3f %2.3f %2.3f]\n', ... 28 | lb(1), lb(2), lb(3), ub(1), ub(2), ub(3)); 29 | end 30 | -------------------------------------------------------------------------------- /external/jsonlab/examples/jsonlab_selftest.m: -------------------------------------------------------------------------------- 1 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 2 | % Regression Test Unit of loadjson and savejson 3 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 4 | 5 | for i=1:4 6 | fname=sprintf('example%d.json',i); 7 | if(exist(fname,'file')==0) break; end 8 | fprintf(1,'===============================================\n>> %s\n',fname); 9 | json=savejson('data',loadjson(fname)); 10 | fprintf(1,'%s\n',json); 11 | fprintf(1,'%s\n',savejson('data',loadjson(fname),'Compact',1)); 12 | data=loadjson(json); 13 | savejson('data',data,'selftest.json'); 14 | data=loadjson('selftest.json'); 15 | end 16 | 17 | for i=1:4 18 | fname=sprintf('example%d.json',i); 19 | if(exist(fname,'file')==0) break; end 20 | fprintf(1,'===============================================\n>> %s\n',fname); 21 | json=saveubjson('data',loadjson(fname)); 22 | fprintf(1,'%s\n',json); 23 | data=loadubjson(json); 24 | savejson('',data); 25 | saveubjson('data',data,'selftest.ubj'); 26 | data=loadubjson('selftest.ubj'); 27 | end 28 | -------------------------------------------------------------------------------- /external/jsonlab/jsonopt.m: -------------------------------------------------------------------------------- 1 | function val=jsonopt(key,default,varargin) 2 | % 3 | % val=jsonopt(key,default,optstruct) 4 | % 5 | % setting options based on a struct. The struct can be produced 6 | % by varargin2struct from a list of 'param','value' pairs 7 | % 8 | % authors:Qianqian Fang (fangq nmr.mgh.harvard.edu) 9 | % 10 | % $Id: loadjson.m 371 2012-06-20 12:43:06Z fangq $ 11 | % 12 | % input: 13 | % key: a string with which one look up a value from a struct 14 | % default: if the key does not exist, return default 15 | % optstruct: a struct where each sub-field is a key 16 | % 17 | % output: 18 | % val: if key exists, val=optstruct.key; otherwise val=default 19 | % 20 | % license: 21 | % BSD License, see LICENSE_BSD.txt files for details 22 | % 23 | % -- this function is part of jsonlab toolbox (http://iso2mesh.sf.net/cgi-bin/index.cgi?jsonlab) 24 | % 25 | 26 | val=default; 27 | if(nargin<=2) return; end 28 | opt=varargin{1}; 29 | if(isstruct(opt)) 30 | if(isfield(opt,key)) 31 | val=getfield(opt,key); 32 | elseif(isfield(opt,lower(key))) 33 | val=getfield(opt,lower(key)); 34 | end 35 | end 36 | -------------------------------------------------------------------------------- /utils/box_cover.m: -------------------------------------------------------------------------------- 1 | % greedy box cover for voxel obstacles. output boxes can overlap. 2 | % input: 3xN list of obstacle voxel centers, assume on integers and 1-unit side 3 | % output: 3x2xK list of boxes - boxes(:,:,i) is [corner1 corner2] 4 | % 5 | function boxes = box_cover(cubes) 6 | 7 | if isempty(cubes) 8 | boxes = []; 9 | return; 10 | end 11 | 12 | % convert to voxels 13 | low = min(cubes, [], 2); 14 | cubes = bsxfun(@minus, cubes, low - 1); 15 | dims = max(cubes, [], 2)'; 16 | voxels = zeros(dims); 17 | for i=1:size(cubes,2) 18 | voxels(cubes(1,i), cubes(2,i), cubes(3,i)) = 1; 19 | end 20 | uncovered = voxels; 21 | 22 | boxes = []; 23 | 24 | while any(uncovered(:)) 25 | [x,y,z] = ind2sub(dims, find(uncovered, 1)); 26 | box = [x x; y y; z z]; 27 | ok = true; 28 | while ok 29 | ok = false; 30 | for d=1:3 31 | if box(d,2) < dims(d) 32 | box(d,2) = box(d,2) + 1; 33 | if all(voxels(box(1,1):box(1,2),box(2,1):box(2,2),box(3,1):box(3,2))) 34 | ok = true; 35 | else 36 | box(d,2) = box(d,2) - 1; 37 | end 38 | end 39 | end 40 | end 41 | boxes = cat(3, boxes, box); 42 | uncovered(box(1,1):box(1,2),box(2,1):box(2,2),box(3,1):box(3,2)) = 0; 43 | end 44 | 45 | boxes = bsxfun(@plus, boxes, low - 1); 46 | end 47 | -------------------------------------------------------------------------------- /external/stlread/stldemo.m: -------------------------------------------------------------------------------- 1 | %% 3D Model Demo 2 | % This is short demo that loads and renders a 3D model of a human femur. It 3 | % showcases some of MATLAB's advanced graphics features, including lighting and 4 | % specular reflectance. 5 | 6 | % Copyright 2011 The MathWorks, Inc. 7 | 8 | 9 | %% Load STL mesh 10 | % Stereolithography (STL) files are a common format for storing mesh data. STL 11 | % meshes are simply a collection of triangular faces. This type of model is very 12 | % suitable for use with MATLAB's PATCH graphics object. 13 | 14 | % Import an STL mesh, returning a PATCH-compatible face-vertex structure 15 | fv = stlread('femur.stl'); 16 | 17 | 18 | %% Render 19 | % The model is rendered with a PATCH graphics object. We also add some dynamic 20 | % lighting, and adjust the material properties to change the specular 21 | % highlighting. 22 | 23 | patch(fv,'FaceColor', [0.8 0.8 1.0], ... 24 | 'EdgeColor', 'none', ... 25 | 'FaceLighting', 'gouraud', ... 26 | 'AmbientStrength', 0.15); 27 | 28 | % Add a camera light, and tone down the specular highlighting 29 | camlight('headlight'); 30 | material('dull'); 31 | 32 | % Fix the axes scaling, and set a nice view angle 33 | axis('image'); 34 | view([-135 35]); -------------------------------------------------------------------------------- /pp_obs_sep_octomap.m: -------------------------------------------------------------------------------- 1 | % computes the robot-obstacle halfspace constraints 2 | % for the "octomap" obstacle model (using the Octomap C++ library) 3 | % 4 | % inputs: 5 | % pps: {N} cell array of matlab ppform structs 6 | % obs_ellipsoid: [3] radii of ellipsoid for robot/obstacle collision model 7 | % octomap_filepath: [string] file path of octomap, should have .bt extension. 8 | % 9 | % outputs: 10 | % polytopes: {N k-1} cell array of halfspaces 11 | % such that p = polytopes{irobot, istep} is a [4 nfaces] array 12 | % describing the linear system p(:,1:3)x <= p(:,4) 13 | % 14 | function polytopes = pp_obs_sep_octomap(pps, obs_ellipsoid, octomap_filepath) 15 | 16 | N = length(pps); 17 | k = length(pps{1}.breaks); 18 | 19 | polytopes = cell(N,k-1); 20 | 21 | % parfor 22 | parfor n=1:N 23 | pp_filepath = tempname(); 24 | hs_filepath = tempname(); 25 | write_pptrajs(pps(n), pp_filepath); 26 | cmd = sprintf('octomap_corridor/octomap_corridor %s %s %s', ... 27 | octomap_filepath, pp_filepath, hs_filepath); 28 | status = system(cmd); 29 | assert(status == 0); 30 | hs_slice = read_halfspaces(hs_filepath); 31 | assert(all(size(hs_slice) == [1 k-1])); 32 | polytopes(n,:) = hs_slice; 33 | delete(pp_filepath); 34 | delete(hs_filepath); 35 | end 36 | end 37 | -------------------------------------------------------------------------------- /external/jsonlab/varargin2struct.m: -------------------------------------------------------------------------------- 1 | function opt=varargin2struct(varargin) 2 | % 3 | % opt=varargin2struct('param1',value1,'param2',value2,...) 4 | % or 5 | % opt=varargin2struct(...,optstruct,...) 6 | % 7 | % convert a series of input parameters into a structure 8 | % 9 | % authors:Qianqian Fang (fangq nmr.mgh.harvard.edu) 10 | % date: 2012/12/22 11 | % 12 | % input: 13 | % 'param', value: the input parameters should be pairs of a string and a value 14 | % optstruct: if a parameter is a struct, the fields will be merged to the output struct 15 | % 16 | % output: 17 | % opt: a struct where opt.param1=value1, opt.param2=value2 ... 18 | % 19 | % license: 20 | % BSD License, see LICENSE_BSD.txt files for details 21 | % 22 | % -- this function is part of jsonlab toolbox (http://iso2mesh.sf.net/cgi-bin/index.cgi?jsonlab) 23 | % 24 | 25 | len=length(varargin); 26 | opt=struct; 27 | if(len==0) return; end 28 | i=1; 29 | while(i<=len) 30 | if(isstruct(varargin{i})) 31 | opt=mergestruct(opt,varargin{i}); 32 | elseif(ischar(varargin{i}) && i. All rights reserved. 2 | 3 | Redistribution and use in source and binary forms, with or without modification, are 4 | permitted provided that the following conditions are met: 5 | 6 | 1. Redistributions of source code must retain the above copyright notice, this list of 7 | conditions and the following disclaimer. 8 | 9 | 2. Redistributions in binary form must reproduce the above copyright notice, this list 10 | of conditions and the following disclaimer in the documentation and/or other materials 11 | provided with the distribution. 12 | 13 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS ''AS IS'' AND ANY EXPRESS OR IMPLIED 14 | WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND 15 | FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS 16 | OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 17 | CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 18 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON 19 | ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 20 | NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF 21 | ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 22 | 23 | The views and conclusions contained in the software and documentation are those of the 24 | authors and should not be interpreted as representing official policies, either expressed 25 | or implied, of the copyright holders. 26 | -------------------------------------------------------------------------------- /external/jsonlab/license.txt: -------------------------------------------------------------------------------- 1 | Copyright (c) 2015, Qianqian Fang 2 | Copyright (c) 2008, The MathWorks, Inc. 3 | Copyright (c) 2011, François Glineur 4 | Copyright (c) 2009, Nedialko 5 | All rights reserved. 6 | 7 | Redistribution and use in source and binary forms, with or without 8 | modification, are permitted provided that the following conditions are 9 | met: 10 | 11 | * Redistributions of source code must retain the above copyright 12 | notice, this list of conditions and the following disclaimer. 13 | * Redistributions in binary form must reproduce the above copyright 14 | notice, this list of conditions and the following disclaimer in 15 | the documentation and/or other materials provided with the distribution 16 | * Neither the name of the The MathWorks, Inc. nor the names 17 | of its contributors may be used to endorse or promote products derived 18 | from this software without specific prior written permission. 19 | 20 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 21 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 22 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 23 | ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 24 | LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 25 | CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 26 | SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 27 | INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 28 | CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 29 | ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 30 | POSSIBILITY OF SUCH DAMAGE. 31 | -------------------------------------------------------------------------------- /external/jsonlab/AUTHORS.txt: -------------------------------------------------------------------------------- 1 | The author of "jsonlab" toolbox is Qianqian Fang. Qianqian 2 | is currently an Assistant Professor in the Department of Bioengineering, 3 | Northeastern University. 4 | 5 | Address: Qianqian Fang 6 | Department of Bioengineering 7 | Northeastern University 8 | 212A Lake Hall 9 | 360 Huntington Ave, Boston, MA 02115, USA 10 | Office: 503 Holmes Hall 11 | Phone[O]: 617-373-3829 12 | URL: http://fanglab.org 13 | Email: and 14 | 15 | 16 | The script loadjson.m was built upon previous works by 17 | 18 | - Nedialko Krouchev: http://www.mathworks.com/matlabcentral/fileexchange/25713 19 | date: 2009/11/02 20 | - François Glineur: http://www.mathworks.com/matlabcentral/fileexchange/23393 21 | date: 2009/03/22 22 | - Joel Feenstra: http://www.mathworks.com/matlabcentral/fileexchange/20565 23 | date: 2008/07/03 24 | 25 | 26 | This toolbox contains patches submitted by the following contributors: 27 | 28 | - Blake Johnson 29 | part of revision 341 30 | 31 | - Niclas Borlin 32 | various fixes in revision 394, including 33 | - loadjson crashes for all-zero sparse matrix. 34 | - loadjson crashes for empty sparse matrix. 35 | - Non-zero size of 0-by-N and N-by-0 empty matrices is lost after savejson/loadjson. 36 | - loadjson crashes for sparse real column vector. 37 | - loadjson crashes for sparse complex column vector. 38 | - Data is corrupted by savejson for sparse real row vector. 39 | - savejson crashes for sparse complex row vector. 40 | 41 | - Yul Kang 42 | patches for svn revision 415. 43 | - savejson saves an empty cell array as [] instead of null 44 | - loadjson differentiates an empty struct from an empty array 45 | 46 | - Mykhailo Bratukha 47 | (Pull#14) Bug fix: File path is wrongly inerpreted as JSON string 48 | 49 | - Insik Kim 50 | (Pull#12) Bug fix: Resolving bug that cell type is converted to json with transposed data 51 | 52 | - Sertan Senturk 53 | (Pull#10,#11) Feature: Added matlab object saving to savejson and saveubjson 54 | -------------------------------------------------------------------------------- /svm_sephyp_2point/matrix_support.c: -------------------------------------------------------------------------------- 1 | /* Produced by CVXGEN, 2016-11-30 17:09:32 -0500. */ 2 | /* CVXGEN is Copyright (C) 2006-2012 Jacob Mattingley, jem@cvxgen.com. */ 3 | /* The code in this file is Copyright (C) 2006-2012 Jacob Mattingley. */ 4 | /* CVXGEN, or solvers produced by CVXGEN, cannot be used for commercial */ 5 | /* applications without prior written permission from Jacob Mattingley. */ 6 | 7 | /* Filename: matrix_support.c. */ 8 | /* Description: Support functions for matrix multiplication and vector filling. */ 9 | #include "solver.h" 10 | void multbymA(double *lhs, double *rhs) { 11 | } 12 | void multbymAT(double *lhs, double *rhs) { 13 | lhs[0] = 0; 14 | lhs[1] = 0; 15 | lhs[2] = 0; 16 | lhs[3] = 0; 17 | } 18 | void multbymG(double *lhs, double *rhs) { 19 | lhs[0] = -rhs[0]*(params.A[0])-rhs[1]*(params.A[4])-rhs[2]*(params.A[8])-rhs[3]*(params.A[12]); 20 | lhs[1] = -rhs[0]*(params.A[1])-rhs[1]*(params.A[5])-rhs[2]*(params.A[9])-rhs[3]*(params.A[13]); 21 | lhs[2] = -rhs[0]*(params.A[2])-rhs[1]*(params.A[6])-rhs[2]*(params.A[10])-rhs[3]*(params.A[14]); 22 | lhs[3] = -rhs[0]*(params.A[3])-rhs[1]*(params.A[7])-rhs[2]*(params.A[11])-rhs[3]*(params.A[15]); 23 | } 24 | void multbymGT(double *lhs, double *rhs) { 25 | lhs[0] = -rhs[0]*(params.A[0])-rhs[1]*(params.A[1])-rhs[2]*(params.A[2])-rhs[3]*(params.A[3]); 26 | lhs[1] = -rhs[0]*(params.A[4])-rhs[1]*(params.A[5])-rhs[2]*(params.A[6])-rhs[3]*(params.A[7]); 27 | lhs[2] = -rhs[0]*(params.A[8])-rhs[1]*(params.A[9])-rhs[2]*(params.A[10])-rhs[3]*(params.A[11]); 28 | lhs[3] = -rhs[0]*(params.A[12])-rhs[1]*(params.A[13])-rhs[2]*(params.A[14])-rhs[3]*(params.A[15]); 29 | } 30 | void multbyP(double *lhs, double *rhs) { 31 | /* TODO use the fact that P is symmetric? */ 32 | /* TODO check doubling / half factor etc. */ 33 | lhs[0] = rhs[0]*(2*params.Q[0]); 34 | lhs[1] = rhs[1]*(2*params.Q[1]); 35 | lhs[2] = rhs[2]*(2*params.Q[2]); 36 | lhs[3] = rhs[3]*(2*params.Q[3]); 37 | } 38 | void fillq(void) { 39 | work.q[0] = 0; 40 | work.q[1] = 0; 41 | work.q[2] = 0; 42 | work.q[3] = 0; 43 | } 44 | void fillh(void) { 45 | work.h[0] = -1; 46 | work.h[1] = -1; 47 | work.h[2] = -1; 48 | work.h[3] = -1; 49 | } 50 | void fillb(void) { 51 | } 52 | void pre_ops(void) { 53 | } 54 | -------------------------------------------------------------------------------- /svm_sephyp_2point/testsolver.c: -------------------------------------------------------------------------------- 1 | /* Produced by CVXGEN, 2016-11-30 17:09:32 -0500. */ 2 | /* CVXGEN is Copyright (C) 2006-2012 Jacob Mattingley, jem@cvxgen.com. */ 3 | /* The code in this file is Copyright (C) 2006-2012 Jacob Mattingley. */ 4 | /* CVXGEN, or solvers produced by CVXGEN, cannot be used for commercial */ 5 | /* applications without prior written permission from Jacob Mattingley. */ 6 | 7 | /* Filename: testsolver.c. */ 8 | /* Description: Basic test harness for solver.c. */ 9 | #include "solver.h" 10 | Vars vars; 11 | Params params; 12 | Workspace work; 13 | Settings settings; 14 | #define NUMTESTS 0 15 | int main(int argc, char **argv) { 16 | int num_iters; 17 | #if (NUMTESTS > 0) 18 | int i; 19 | double time; 20 | double time_per; 21 | #endif 22 | set_defaults(); 23 | setup_indexing(); 24 | load_default_data(); 25 | /* Solve problem instance for the record. */ 26 | settings.verbose = 1; 27 | num_iters = solve(); 28 | #ifndef ZERO_LIBRARY_MODE 29 | #if (NUMTESTS > 0) 30 | /* Now solve multiple problem instances for timing purposes. */ 31 | settings.verbose = 0; 32 | tic(); 33 | for (i = 0; i < NUMTESTS; i++) { 34 | solve(); 35 | } 36 | time = tocq(); 37 | printf("Timed %d solves over %.3f seconds.\n", NUMTESTS, time); 38 | time_per = time / NUMTESTS; 39 | if (time_per > 1) { 40 | printf("Actual time taken per solve: %.3g s.\n", time_per); 41 | } else if (time_per > 1e-3) { 42 | printf("Actual time taken per solve: %.3g ms.\n", 1e3*time_per); 43 | } else { 44 | printf("Actual time taken per solve: %.3g us.\n", 1e6*time_per); 45 | } 46 | #endif 47 | #endif 48 | return 0; 49 | } 50 | void load_default_data(void) { 51 | params.Q[0] = 1.5507979025745755; 52 | params.Q[1] = 1.7081478226181048; 53 | params.Q[2] = 1.2909047389129444; 54 | params.Q[3] = 1.510827605197663; 55 | params.A[0] = 1.5717878173906188; 56 | params.A[1] = 1.5851723557337523; 57 | params.A[2] = -1.497658758144655; 58 | params.A[3] = -1.171028487447253; 59 | params.A[4] = -1.7941311867966805; 60 | params.A[5] = -0.23676062539745413; 61 | params.A[6] = -1.8804951564857322; 62 | params.A[7] = -0.17266710242115568; 63 | params.A[8] = 0.596576190459043; 64 | params.A[9] = -0.8860508694080989; 65 | params.A[10] = 0.7050196079205251; 66 | params.A[11] = 0.3634512696654033; 67 | params.A[12] = -1.9040724704913385; 68 | params.A[13] = 0.23541635196352795; 69 | params.A[14] = -0.9629902123701384; 70 | params.A[15] = -0.3395952119597214; 71 | } 72 | -------------------------------------------------------------------------------- /examples/no_obstacles/main_no_obstacles.m: -------------------------------------------------------------------------------- 1 | % Load a real problem instance and solve. 2 | % --------------------------------------- 3 | function main_no_obstacles() 4 | 5 | % the duration of each step in the discrete plan, in seconds 6 | TIMESCALE = 0.25; 7 | 8 | % read input file 9 | discrete_plan_file = 'examples/no_obstacles/warehouse4.json'; 10 | s = read_schedule(discrete_plan_file); 11 | [~, ~, N] = size(s); 12 | 13 | % make a padded bounding box around the schedule 14 | all_pts = reshape(s, 3, []); 15 | bbox = zeros(3,2); 16 | bbox(:,1) = min(all_pts, [], 2) - 0.5; 17 | bbox(:,2) = max(all_pts, [], 2) + 0.5; 18 | 19 | % print some info about the discrete plan input 20 | analyze_schedule(s); 21 | 22 | % optional: clip the number of robots so it runs faster 23 | %N = 5; 24 | s = s(:,:,1:N); 25 | 26 | % add extra stationary steps at begin and end for smooth acceleration 27 | s = cat(2, s(:,1,:), s(:,1,:), s, s(:,end,:)); 28 | 29 | % polynomial degree 30 | deg = 7; 31 | 32 | % how many derivatives must be continuous 33 | cont = 4; 34 | 35 | % robot-robot collision ellipsoid 36 | ellipsoid = [0.12 0.12 0.3]; 37 | 38 | % robot-obstacle collision ellipsoid 39 | obs_ellipsoid = [0.15 0.15 0.15]; 40 | 41 | % number of iterations of refinement 42 | iters = 2; 43 | 44 | % null robot/obstacle separating hyperplane function - no obstacles 45 | pp_obs_sep_fun = @pp_obs_sep_none; 46 | 47 | % main routine 48 | [pps, costs, corridors] = smoothener(s, bbox, deg, cont, TIMESCALE, ellipsoid, obs_ellipsoid, iters, ... 49 | pp_obs_sep_fun, @corridor_trajectory_optimize); 50 | 51 | % Plot the results. 52 | % ----------------- 53 | 54 | % smoothener returns the piecewise polynomials for every iteration of refinement. 55 | % here, we plot only the results of the final iteration. 56 | pps = pps(end,:); 57 | 58 | % set up the figure. 59 | clf; hold on; axis equal; 60 | light('Position', [1 1 1]); 61 | light('Position', [1 1 -1]); 62 | campos([-32 45 21]); 63 | 64 | % render the output 65 | for i=1:N 66 | % plot the discrete plan 67 | h = plot3n(s(:,:,i)); 68 | 69 | % plot the continuous trajectory 70 | color = get(h, 'color'); 71 | duration = pps{i}.breaks(end); 72 | t = 0:0.1:duration; 73 | x = ppval(pps{i}, t); 74 | h = plot3n(x, 'color', color, 'LineWidth', 3); 75 | 76 | % plot the start and endpoints as markers 77 | lastPos = ppval(pps{i}, duration); 78 | scatter3(lastPos(1), lastPos(2), lastPos(3),50,'k','filled'); 79 | firstPos = ppval(pps{i}, 0); 80 | scatter3(firstPos(1), firstPos(2), firstPos(3),50,'k', 'square');%'g','filled'); 81 | end 82 | 83 | 84 | % Save the piecewise polynomial coefficients to a directory of CSV files. 85 | % ----------------------------------------------------------------------- 86 | SAVE_CSVS = true; 87 | if SAVE_CSVS 88 | DIR = 'examples/no_obstacles/no_obstacles_pps'; 89 | mkdir(DIR); 90 | for i=1:N 91 | filename = sprintf('%s/pp%d.csv', DIR, i); 92 | pp2csv(pps{i}, filename) 93 | end 94 | end 95 | 96 | end 97 | -------------------------------------------------------------------------------- /examples/octomap/main_octomap.m: -------------------------------------------------------------------------------- 1 | % Load a real problem instance and solve. 2 | % --------------------------------------- 3 | function main_octomap() 4 | 5 | % the duration of each step in the discrete plan, in seconds 6 | TIMESCALE = 0.25; 7 | 8 | % read input files 9 | EXAMPLE = 'examples/octomap/warehouse4'; 10 | discrete_plan_file = [EXAMPLE '.json']; 11 | octree_file = [EXAMPLE '.bt']; 12 | s = read_schedule(discrete_plan_file); 13 | [~, ~, N] = size(s); 14 | bbox = read_octomap_bbox_mex(octree_file); 15 | 16 | % print some info about the discrete plan input 17 | analyze_schedule(s); 18 | 19 | % optional: clip the number of robots so it runs faster 20 | % N = 5; 21 | s = s(:,:,1:N); 22 | 23 | % add extra stationary steps at begin and end for smooth acceleration 24 | s = cat(2, s(:,1,:), s(:,1,:), s, s(:,end,:)); 25 | 26 | % polynomial degree 27 | deg = 7; 28 | 29 | % how many derivatives must be continuous 30 | cont = 4; 31 | 32 | % robot-robot collision ellipsoid 33 | ellipsoid = [0.12 0.12 0.3]; 34 | 35 | % robot-obstacle collision ellipsoid 36 | obs_ellipsoid = [0.15 0.15 0.15]; 37 | 38 | % number of iterations of refinement 39 | iters = 2; 40 | 41 | % robot/obstacle separating hyperplane function 42 | pp_obs_sep_fun = @(pps, obs_ellipsoid) pp_obs_sep_octomap(pps, obs_ellipsoid, octree_file); 43 | 44 | % main routine 45 | [pps, costs, corridors] = smoothener(s, bbox, deg, cont, TIMESCALE, ellipsoid, obs_ellipsoid, iters, ... 46 | pp_obs_sep_fun, @corridor_trajectory_optimize); 47 | 48 | % Plot the results. 49 | % ----------------- 50 | 51 | % smoothener returns the piecewise polynomials for every iteration of refinement. 52 | % here, we plot only the results of the final iteration. 53 | pps = pps(end,:); 54 | 55 | % set up the figure. 56 | clf; hold on; axis equal; 57 | light('Position', [1 1 1]); 58 | light('Position', [1 1 -1]); 59 | campos([-32 45 21]); 60 | 61 | % read and render the STL file of the octree 62 | stl_file = [EXAMPLE '.stl']; 63 | fv = stlread(stl_file); 64 | patch(fv, ... 65 | 'FaceColor', [0.4 0.4 0.4], 'EdgeColor', 'none', ... 66 | 'SpecularStrength', 0.1, 'AmbientStrength', 0.5, 'facealpha', 0.5); 67 | 68 | % render the output 69 | for i=1:N 70 | % plot the discrete plan 71 | h = plot3n(s(:,:,i)); 72 | 73 | % plot the continuous trajectory 74 | color = get(h, 'color'); 75 | duration = pps{i}.breaks(end); 76 | t = 0:0.1:duration; 77 | x = ppval(pps{i}, t); 78 | h = plot3n(x, 'color', color, 'LineWidth', 3); 79 | 80 | % plot the start and endpoints as markers 81 | lastPos = ppval(pps{i}, duration); 82 | scatter3(lastPos(1), lastPos(2), lastPos(3),50,'k','filled'); 83 | firstPos = ppval(pps{i}, 0); 84 | scatter3(firstPos(1), firstPos(2), firstPos(3),50,'k', 'square');%'g','filled'); 85 | end 86 | 87 | 88 | % Save the piecewise polynomial coefficients to a directory of CSV files. 89 | % ----------------------------------------------------------------------- 90 | SAVE_CSVS = true; 91 | if SAVE_CSVS 92 | DIR = [EXAMPLE '_pps']; 93 | mkdir(DIR); 94 | for i=1:N 95 | filename = sprintf('%s/pp%d.csv', DIR, i); 96 | pp2csv(pps{i}, filename) 97 | end 98 | end 99 | 100 | end 101 | -------------------------------------------------------------------------------- /pp_obs_sep_grid.m: -------------------------------------------------------------------------------- 1 | % computes the robot-obstacle halfspace constraints 2 | % for the "list of boxes" obstacle model. 3 | % 4 | % inputs: 5 | % pps: {N} cell array of matlab ppform structs 6 | % obs_ellipsoid: [3] radii of ellipsoid for robot/obstacle collision model 7 | % boxes: [3 2 nObs] min/max corners of obstacle boxes 8 | % 9 | % outputs: 10 | % polytopes: {N k-1} cell array of halfspaces 11 | % such that p = polytopes{irobot, istep} is a [4 nfaces] array 12 | % describing the linear system p(:,1:3)x <= p(:,4) 13 | % 14 | function polytopes = pp_obs_sep_grid(pps, obs_ellipsoid, boxes) 15 | 16 | N = length(pps); 17 | k = length(pps{1}.breaks); 18 | [~, ~, nObs] = size(boxes); 19 | 20 | polytopes = cell(N,k-1); 21 | 22 | % parfor 23 | parfor irobot=1:N 24 | for istep = 1:(k-1) 25 | seg = pp_sample_piece(pps{irobot}, istep, 32)'; 26 | polytope = zeros(nObs, 4); 27 | for iobs = 1:nObs 28 | [aa, bb] = path_box_hyperplane(... 29 | seg, boxes(:,1,iobs), boxes(:,2,iobs), obs_ellipsoid); 30 | polytope(iobs,1:3) = aa; 31 | polytope(iobs,4) = bb; 32 | end 33 | polytopes{irobot,istep} = polytope; 34 | end 35 | end 36 | end 37 | 38 | % TODO: if more performance is needed, 39 | % rewrite the outer loop as a mex-function to avoid overhead 40 | % of setting up the many CVXGEN problems in Matlab interpreted code. 41 | 42 | % inputs: 43 | % path [2x3] or [32x3] path (segment or sampled pp) 44 | % x0[3] x1[3] box diagonally opposed corners 45 | % ell[3] ellipsoid radii 46 | % 47 | % outputs: [a, b] normalized hyperplane s.t. |a| = 1, a path < b, a cube > b 48 | % 49 | % note: this hyperplane is tight to the box and shifted by the ellipsoid 50 | % 51 | function [a, b] = path_box_hyperplane(path, x0, x1, ell) 52 | 53 | cube_pts = corners_to_box_vertices(x0, x1); 54 | cube_pts = repmat(cube_pts, 4, 1); 55 | 56 | if size(path, 1) == 2 57 | path = repmat(path, 16, 1); 58 | end 59 | 60 | assert(size(path, 1) == 32); 61 | 62 | [a, b] = separating_hyperplane(path, cube_pts, ell); 63 | 64 | % now we have a max-margin separating hyperplane shifted by ellipsoid 65 | % but we want a hyperplane tight to the cube, shifted by ellipsoid: 66 | min_dist = min(cube_pts * a - b); 67 | %ell_dist = norm(diag(ell) * a); 68 | b = b + min_dist;% - ell_dist; 69 | end 70 | 71 | % inputs: p1 [N * dim] path 1 72 | % p2 [M * dim] path 2 73 | % ell [3] ellipsoid radii 74 | % output: [a, b] normalized hyperplane s.t. |a| = 1, a p1 < b, a p2 > b 75 | % 76 | function [a, b] = separating_hyperplane(p1, p2, ell) 77 | 78 | [N, dim] = size(p1); 79 | [M, dim2] = size(p2); 80 | assert(dim == dim2); 81 | 82 | A = [ p1 ones(N, 1); ... 83 | -p2 -ones(M, 1) ]; 84 | Q = diag([ell.^2 0]); 85 | 86 | params.A = A; 87 | params.Q = Q; 88 | settings.verbose = 0; 89 | 90 | assert(dim == 3 && N == M && N == 32); 91 | [vars, status] = svm32_mex(params, settings); 92 | beta = vars.beta; 93 | assert(length(beta) == dim + 1); 94 | 95 | a = beta(1:dim); 96 | b = -beta(end); 97 | na = norm(a); 98 | a = a ./ na; 99 | b = b ./ na; 100 | end 101 | -------------------------------------------------------------------------------- /svm_sephyp_2point/util.c: -------------------------------------------------------------------------------- 1 | /* Produced by CVXGEN, 2016-11-30 17:09:32 -0500. */ 2 | /* CVXGEN is Copyright (C) 2006-2012 Jacob Mattingley, jem@cvxgen.com. */ 3 | /* The code in this file is Copyright (C) 2006-2012 Jacob Mattingley. */ 4 | /* CVXGEN, or solvers produced by CVXGEN, cannot be used for commercial */ 5 | /* applications without prior written permission from Jacob Mattingley. */ 6 | 7 | /* Filename: util.c. */ 8 | /* Description: Common utility file for all cvxgen code. */ 9 | #include "solver.h" 10 | #include 11 | #include 12 | #include 13 | long global_seed = 1; 14 | static clock_t tic_timestart; 15 | void tic(void) { 16 | tic_timestart = clock(); 17 | } 18 | float toc(void) { 19 | clock_t tic_timestop; 20 | tic_timestop = clock(); 21 | printf("time: %8.2f.\n", (float)(tic_timestop - tic_timestart) / CLOCKS_PER_SEC); 22 | return (float)(tic_timestop - tic_timestart) / CLOCKS_PER_SEC; 23 | } 24 | float tocq(void) { 25 | clock_t tic_timestop; 26 | tic_timestop = clock(); 27 | return (float)(tic_timestop - tic_timestart) / CLOCKS_PER_SEC; 28 | } 29 | void printmatrix(char *name, double *A, int m, int n, int sparse) { 30 | int i, j; 31 | printf("%s = [...\n", name); 32 | for (i = 0; i < m; i++) { 33 | for (j = 0; j < n; j++) 34 | if ((sparse == 1) && (A[i+j*m] == 0)) 35 | printf(" 0"); 36 | else 37 | printf(" % 9.4f", A[i+j*m]); 38 | printf(",\n"); 39 | } 40 | printf("];\n"); 41 | } 42 | double unif(double lower, double upper) { 43 | return lower + ((upper - lower)*rand())/RAND_MAX; 44 | } 45 | /* Next function is from numerical recipes in C. */ 46 | #define IA 16807 47 | #define IM 2147483647 48 | #define AM (1.0/IM) 49 | #define IQ 127773 50 | #define IR 2836 51 | #define NTAB 32 52 | #define NDIV (1+(IM-1)/NTAB) 53 | #define EPS 1.2e-7 54 | #define RNMX (1.0-EPS) 55 | float ran1(long*idum, int reset) { 56 | int j; 57 | long k; 58 | static long iy=0; 59 | static long iv[NTAB]; 60 | float temp; 61 | if (reset) { 62 | iy = 0; 63 | } 64 | if (*idum<=0||!iy) { 65 | if (-(*idum)<1)*idum=1; 66 | else *idum=-(*idum); 67 | for (j=NTAB+7; j>=0; j--) { 68 | k = (*idum)/IQ; 69 | *idum=IA*(*idum-k*IQ)-IR*k; 70 | if (*idum<0)*idum+=IM; 71 | if (j RNMX) return RNMX; 82 | else return temp; 83 | } 84 | /* Next function is from numerical recipes in C. */ 85 | float randn_internal(long *idum, int reset) { 86 | static int iset=0; 87 | static float gset; 88 | float fac, rsq, v1, v2; 89 | if (reset) { 90 | iset = 0; 91 | } 92 | if (iset==0) { 93 | do { 94 | v1 = 2.0*ran1(idum, reset)-1.0; 95 | v2 = 2.0*ran1(idum, reset)-1.0; 96 | rsq = v1*v1+v2*v2; 97 | } while(rsq >= 1.0 || rsq == 0.0); 98 | fac = sqrt(-2.0*log(rsq)/rsq); 99 | gset = v1*fac; 100 | iset = 1; 101 | return v2*fac; 102 | } else { 103 | iset = 0; 104 | return gset; 105 | } 106 | } 107 | double randn(void) { 108 | return randn_internal(&global_seed, 0); 109 | } 110 | void reset_rand(void) { 111 | srand(15); 112 | global_seed = 1; 113 | randn_internal(&global_seed, 1); 114 | } 115 | -------------------------------------------------------------------------------- /svm_sephyp_32point/util.c: -------------------------------------------------------------------------------- 1 | /* Produced by CVXGEN, 2016-11-30 16:59:57 -0500. */ 2 | /* CVXGEN is Copyright (C) 2006-2012 Jacob Mattingley, jem@cvxgen.com. */ 3 | /* The code in this file is Copyright (C) 2006-2012 Jacob Mattingley. */ 4 | /* CVXGEN, or solvers produced by CVXGEN, cannot be used for commercial */ 5 | /* applications without prior written permission from Jacob Mattingley. */ 6 | 7 | /* Filename: util.c. */ 8 | /* Description: Common utility file for all cvxgen code. */ 9 | #include "solver.h" 10 | #include 11 | #include 12 | #include 13 | long global_seed = 1; 14 | static clock_t tic_timestart; 15 | void tic(void) { 16 | tic_timestart = clock(); 17 | } 18 | float toc(void) { 19 | clock_t tic_timestop; 20 | tic_timestop = clock(); 21 | printf("time: %8.2f.\n", (float)(tic_timestop - tic_timestart) / CLOCKS_PER_SEC); 22 | return (float)(tic_timestop - tic_timestart) / CLOCKS_PER_SEC; 23 | } 24 | float tocq(void) { 25 | clock_t tic_timestop; 26 | tic_timestop = clock(); 27 | return (float)(tic_timestop - tic_timestart) / CLOCKS_PER_SEC; 28 | } 29 | void printmatrix(char *name, double *A, int m, int n, int sparse) { 30 | int i, j; 31 | printf("%s = [...\n", name); 32 | for (i = 0; i < m; i++) { 33 | for (j = 0; j < n; j++) 34 | if ((sparse == 1) && (A[i+j*m] == 0)) 35 | printf(" 0"); 36 | else 37 | printf(" % 9.4f", A[i+j*m]); 38 | printf(",\n"); 39 | } 40 | printf("];\n"); 41 | } 42 | double unif(double lower, double upper) { 43 | return lower + ((upper - lower)*rand())/RAND_MAX; 44 | } 45 | /* Next function is from numerical recipes in C. */ 46 | #define IA 16807 47 | #define IM 2147483647 48 | #define AM (1.0/IM) 49 | #define IQ 127773 50 | #define IR 2836 51 | #define NTAB 32 52 | #define NDIV (1+(IM-1)/NTAB) 53 | #define EPS 1.2e-7 54 | #define RNMX (1.0-EPS) 55 | float ran1(long*idum, int reset) { 56 | int j; 57 | long k; 58 | static long iy=0; 59 | static long iv[NTAB]; 60 | float temp; 61 | if (reset) { 62 | iy = 0; 63 | } 64 | if (*idum<=0||!iy) { 65 | if (-(*idum)<1)*idum=1; 66 | else *idum=-(*idum); 67 | for (j=NTAB+7; j>=0; j--) { 68 | k = (*idum)/IQ; 69 | *idum=IA*(*idum-k*IQ)-IR*k; 70 | if (*idum<0)*idum+=IM; 71 | if (j RNMX) return RNMX; 82 | else return temp; 83 | } 84 | /* Next function is from numerical recipes in C. */ 85 | float randn_internal(long *idum, int reset) { 86 | static int iset=0; 87 | static float gset; 88 | float fac, rsq, v1, v2; 89 | if (reset) { 90 | iset = 0; 91 | } 92 | if (iset==0) { 93 | do { 94 | v1 = 2.0*ran1(idum, reset)-1.0; 95 | v2 = 2.0*ran1(idum, reset)-1.0; 96 | rsq = v1*v1+v2*v2; 97 | } while(rsq >= 1.0 || rsq == 0.0); 98 | fac = sqrt(-2.0*log(rsq)/rsq); 99 | gset = v1*fac; 100 | iset = 1; 101 | return v2*fac; 102 | } else { 103 | iset = 0; 104 | return gset; 105 | } 106 | } 107 | double randn(void) { 108 | return randn_internal(&global_seed, 0); 109 | } 110 | void reset_rand(void) { 111 | srand(15); 112 | global_seed = 1; 113 | randn_internal(&global_seed, 1); 114 | } 115 | -------------------------------------------------------------------------------- /octomap_corridor/cvxgen/util.c: -------------------------------------------------------------------------------- 1 | /* Produced by CVXGEN, 2016-11-30 16:59:57 -0500. */ 2 | /* CVXGEN is Copyright (C) 2006-2012 Jacob Mattingley, jem@cvxgen.com. */ 3 | /* The code in this file is Copyright (C) 2006-2012 Jacob Mattingley. */ 4 | /* CVXGEN, or solvers produced by CVXGEN, cannot be used for commercial */ 5 | /* applications without prior written permission from Jacob Mattingley. */ 6 | 7 | /* Filename: util.c. */ 8 | /* Description: Common utility file for all cvxgen code. */ 9 | #include "solver.h" 10 | #include 11 | #include 12 | #include 13 | long global_seed = 1; 14 | static clock_t tic_timestart; 15 | void tic(void) { 16 | tic_timestart = clock(); 17 | } 18 | float toc(void) { 19 | clock_t tic_timestop; 20 | tic_timestop = clock(); 21 | printf("time: %8.2f.\n", (float)(tic_timestop - tic_timestart) / CLOCKS_PER_SEC); 22 | return (float)(tic_timestop - tic_timestart) / CLOCKS_PER_SEC; 23 | } 24 | float tocq(void) { 25 | clock_t tic_timestop; 26 | tic_timestop = clock(); 27 | return (float)(tic_timestop - tic_timestart) / CLOCKS_PER_SEC; 28 | } 29 | void printmatrix(char *name, double *A, int m, int n, int sparse) { 30 | int i, j; 31 | printf("%s = [...\n", name); 32 | for (i = 0; i < m; i++) { 33 | for (j = 0; j < n; j++) 34 | if ((sparse == 1) && (A[i+j*m] == 0)) 35 | printf(" 0"); 36 | else 37 | printf(" % 9.4f", A[i+j*m]); 38 | printf(",\n"); 39 | } 40 | printf("];\n"); 41 | } 42 | double unif(double lower, double upper) { 43 | return lower + ((upper - lower)*rand())/RAND_MAX; 44 | } 45 | /* Next function is from numerical recipes in C. */ 46 | #define IA 16807 47 | #define IM 2147483647 48 | #define AM (1.0/IM) 49 | #define IQ 127773 50 | #define IR 2836 51 | #define NTAB 32 52 | #define NDIV (1+(IM-1)/NTAB) 53 | #define EPS 1.2e-7 54 | #define RNMX (1.0-EPS) 55 | float ran1(long*idum, int reset) { 56 | int j; 57 | long k; 58 | static long iy=0; 59 | static long iv[NTAB]; 60 | float temp; 61 | if (reset) { 62 | iy = 0; 63 | } 64 | if (*idum<=0||!iy) { 65 | if (-(*idum)<1)*idum=1; 66 | else *idum=-(*idum); 67 | for (j=NTAB+7; j>=0; j--) { 68 | k = (*idum)/IQ; 69 | *idum=IA*(*idum-k*IQ)-IR*k; 70 | if (*idum<0)*idum+=IM; 71 | if (j RNMX) return RNMX; 82 | else return temp; 83 | } 84 | /* Next function is from numerical recipes in C. */ 85 | float randn_internal(long *idum, int reset) { 86 | static int iset=0; 87 | static float gset; 88 | float fac, rsq, v1, v2; 89 | if (reset) { 90 | iset = 0; 91 | } 92 | if (iset==0) { 93 | do { 94 | v1 = 2.0*ran1(idum, reset)-1.0; 95 | v2 = 2.0*ran1(idum, reset)-1.0; 96 | rsq = v1*v1+v2*v2; 97 | } while(rsq >= 1.0 || rsq == 0.0); 98 | fac = sqrt(-2.0*log(rsq)/rsq); 99 | gset = v1*fac; 100 | iset = 1; 101 | return v2*fac; 102 | } else { 103 | iset = 0; 104 | return gset; 105 | } 106 | } 107 | double randn(void) { 108 | return randn_internal(&global_seed, 0); 109 | } 110 | void reset_rand(void) { 111 | srand(15); 112 | global_seed = 1; 113 | randn_internal(&global_seed, 1); 114 | } 115 | -------------------------------------------------------------------------------- /external/jsonlab/struct2jdata.m: -------------------------------------------------------------------------------- 1 | function newdata=struct2jdata(data,varargin) 2 | % 3 | % newdata=struct2jdata(data,opt,...) 4 | % 5 | % convert a JData object (in the form of a struct array) into an array 6 | % 7 | % authors:Qianqian Fang (fangq nmr.mgh.harvard.edu) 8 | % 9 | % input: 10 | % data: a struct array. If data contains JData keywords in the first 11 | % level children, these fields are parsed and regrouped into a 12 | % data object (arrays, trees, graphs etc) based on JData 13 | % specification. The JData keywords are 14 | % "_ArrayType_", "_ArraySize_", "_ArrayData_" 15 | % "_ArrayIsSparse_", "_ArrayIsComplex_" 16 | % opt: (optional) a list of 'Param',value pairs for additional options 17 | % The supported options include 18 | % 'Recursive', if set to 1, will apply the conversion to 19 | % every child; 0 to disable 20 | % 21 | % output: 22 | % newdata: the covnerted data if the input data does contain a JData 23 | % structure; otherwise, the same as the input. 24 | % 25 | % examples: 26 | % obj=struct('_ArrayType_','double','_ArraySize_',[2 3], 27 | % '_ArrayIsSparse_',1 ,'_ArrayData_',null); 28 | % ubjdata=struct2jdata(obj); 29 | % 30 | % license: 31 | % BSD License, see LICENSE_BSD.txt files for details 32 | % 33 | % -- this function is part of JSONLab toolbox (http://iso2mesh.sf.net/cgi-bin/index.cgi?jsonlab) 34 | % 35 | 36 | fn=fieldnames(data); 37 | newdata=data; 38 | len=length(data); 39 | if(jsonopt('Recursive',0,varargin{:})==1) 40 | for i=1:length(fn) % depth-first 41 | for j=1:len 42 | if(isstruct(getfield(data(j),fn{i}))) 43 | newdata(j)=setfield(newdata(j),fn{i},jstruct2array(getfield(data(j),fn{i}))); 44 | end 45 | end 46 | end 47 | end 48 | if(~isempty(strmatch('x0x5F_ArrayType_',fn)) && ~isempty(strmatch('x0x5F_ArrayData_',fn))) 49 | newdata=cell(len,1); 50 | for j=1:len 51 | ndata=cast(data(j).x0x5F_ArrayData_,data(j).x0x5F_ArrayType_); 52 | iscpx=0; 53 | if(~isempty(strmatch('x0x5F_ArrayIsComplex_',fn))) 54 | if(data(j).x0x5F_ArrayIsComplex_) 55 | iscpx=1; 56 | end 57 | end 58 | if(~isempty(strmatch('x0x5F_ArrayIsSparse_',fn))) 59 | if(data(j).x0x5F_ArrayIsSparse_) 60 | if(~isempty(strmatch('x0x5F_ArraySize_',fn))) 61 | dim=double(data(j).x0x5F_ArraySize_); 62 | if(iscpx && size(ndata,2)==4-any(dim==1)) 63 | ndata(:,end-1)=complex(ndata(:,end-1),ndata(:,end)); 64 | end 65 | if isempty(ndata) 66 | % All-zeros sparse 67 | ndata=sparse(dim(1),prod(dim(2:end))); 68 | elseif dim(1)==1 69 | % Sparse row vector 70 | ndata=sparse(1,ndata(:,1),ndata(:,2),dim(1),prod(dim(2:end))); 71 | elseif dim(2)==1 72 | % Sparse column vector 73 | ndata=sparse(ndata(:,1),1,ndata(:,2),dim(1),prod(dim(2:end))); 74 | else 75 | % Generic sparse array. 76 | ndata=sparse(ndata(:,1),ndata(:,2),ndata(:,3),dim(1),prod(dim(2:end))); 77 | end 78 | else 79 | if(iscpx && size(ndata,2)==4) 80 | ndata(:,3)=complex(ndata(:,3),ndata(:,4)); 81 | end 82 | ndata=sparse(ndata(:,1),ndata(:,2),ndata(:,3)); 83 | end 84 | end 85 | elseif(~isempty(strmatch('x0x5F_ArraySize_',fn))) 86 | if(iscpx && size(ndata,2)==2) 87 | ndata=complex(ndata(:,1),ndata(:,2)); 88 | end 89 | ndata=reshape(ndata(:),data(j).x0x5F_ArraySize_); 90 | end 91 | newdata{j}=ndata; 92 | end 93 | if(len==1) 94 | newdata=newdata{1}; 95 | end 96 | end -------------------------------------------------------------------------------- /external/noredund.m: -------------------------------------------------------------------------------- 1 | function [An,bn] = noredund(A,b,c) 2 | % NOREDUND - Remove redundant linear inequalities from a set; i.e., 3 | % remove redundant linear constraints defining a feasible 4 | % region. Note that the feasible region satisfies A*x <= b, 5 | % where A is a fixed matrix, b is a fixed vector, and x 6 | % is the vector of coordinates in your space; i.e., all 7 | % values of x (or equivalently, all ordered n-tuples of 8 | % coordinate numbers) which satisfy the inequality A*x <= b 9 | % are inside the feasible region. 10 | % 11 | % [An,bn] = noredund(A,b) 12 | % 13 | % For n variables: 14 | % A = m x n matrix, where m >= n (m constraints) 15 | % b = m x 1 vector (m constraints) 16 | % An = mm x n matrix, where mm >= n (mm nonredundant constraints) 17 | % bn = mm x 1 vector (mm nonredundant constraints) 18 | % 19 | % NOTES: (1) Unbounded feasible regions are permitted. 20 | % (2) This program requires that the feasible region have some 21 | % finite extent in all dimensions. For example, the feasible 22 | % region cannot be a line segment in 2-D space, or a plane 23 | % in 3-D space. 24 | % (3) At least two dimensions are required. 25 | % (4) See function CON2VERT which is limited to bounded feasible 26 | % regions but also outputs vertices for the region. 27 | % (5) Written by Michael Kleder, June 2005. 28 | % 29 | % EXAMPLE (two figures produced): 30 | % 31 | % n=20; 32 | % A=rand(n,2)-.5; 33 | % b=rand(n,1); 34 | % figure('renderer','zbuffer') 35 | % hold on 36 | % [x,y]=ndgrid(-3:.01:3); 37 | % p=[x(:) y(:)]'; 38 | % p=(A*p <= repmat(b,[1 length(p)])); 39 | % p = double(all(p)); 40 | % p=reshape(p,size(x)); 41 | % h=pcolor(x,y,p); 42 | % set(h,'edgecolor','none') 43 | % set(h,'zdata',get(h,'zdata')-1) % keep in back 44 | % axis equal 45 | % set(gca,'color','none') 46 | % title(['Original feasible region, with ' num2str(size(A,1)) ' constraints.']) 47 | % [A,b]=noredund(A,b); 48 | % figure('renderer','zbuffer') 49 | % hold on 50 | % [x,y]=ndgrid(-3:.01:3); 51 | % p=[x(:) y(:)]'; 52 | % p=(A*p <= repmat(b,[1 length(p)])); 53 | % p = double(all(p)); 54 | % p=reshape(p,size(x)); 55 | % h=pcolor(x,y,p); 56 | % set(h,'edgecolor','none') 57 | % set(h,'zdata',get(h,'zdata')-1) % keep in back 58 | % axis equal 59 | % set(gca,'color','none') 60 | % title(['Final feasible region, with ' num2str(size(A,1)) ' constraints.']) 61 | 62 | if length(b) <= 2 63 | An = A; 64 | bn = b; 65 | return; 66 | end 67 | 68 | if nargin < 3 69 | % move polytope so that origin is included: 70 | % first, attempt to locate a feasible point: 71 | c = A\b; % least-squares soln, correct if no redundant constraints 72 | % find another solution if above isn't *inside* feasible region: 73 | end 74 | 75 | if ~all(A*c < b) % exclude exterior and also boundary points 76 | opts = optimset('display', 'off'); 77 | [c, ~, flag] = linprog(0*A(1,:), A, b, [], [], [], [], [], opts); 78 | assert(flag == 1); 79 | %{ 80 | [c,f,ef] = fminsearch(@obj,c,'params',{A,b}); 81 | if ef ~= 1 82 | warning('Unable to locate a point within the interior of a feasible region.') 83 | An = A; 84 | bn = b; 85 | return; 86 | end 87 | %} 88 | end 89 | 90 | % move polytope to contain origin 91 | bk = b; % preserve 92 | b = b - A*c; % polytope A*x <= b now includes the origin 93 | % obtain dual polytope vertices 94 | D = A ./ repmat(b,[1 size(A,2)]); 95 | try 96 | k = convhulln(D); 97 | % record which constraints generate points on the convex hull 98 | nr = unique(k(:)); 99 | An=A(nr,:); 100 | bn=bk(nr); 101 | catch 102 | % if convhull fails, just use the original 103 | An = A; 104 | bn = bk; 105 | end 106 | 107 | function d = obj(c,params) 108 | A=params{1}; 109 | b=params{2}; 110 | d = A*c-b; 111 | k=(d>=-1e-15); % exclude quasi-boundary points 112 | d(k)=d(k)+1; 113 | d = max([0;d]); 114 | return 115 | 116 | -------------------------------------------------------------------------------- /examples/grid/main_grid.m: -------------------------------------------------------------------------------- 1 | % Load a real problem instance and solve. 2 | % --------------------------------------- 3 | function main_grid() 4 | 5 | % scale of the discrete plan's integer grid in meters 6 | SCALE = 0.5; 7 | 8 | % the duration of each step in the discrete plan, in seconds 9 | TIMESCALE = 0.25; 10 | 11 | % read input files 12 | EXAMPLE = 'examples/grid/maze50'; 13 | map_file = [EXAMPLE '_map.json']; 14 | [dims, obstacles] = read_grid_map(map_file); 15 | % try to merge together neighboring obstacle cells into a smaller number of boxes 16 | boxes = box_cover(obstacles); 17 | % shift so that box centers are on integer coordinates 18 | boxes(:,1,:) = boxes(:,1,:) - 0.5; 19 | boxes(:,2,:) = boxes(:,2,:) + 0.5; 20 | 21 | discrete_plan_file = [EXAMPLE '.json']; 22 | s = read_schedule(discrete_plan_file); 23 | % refine_schedule deals with the "chasing" issue 24 | % described in "Downwash-aware trajectory planning for large quadcopter teams", 25 | % Preiss, Hoenig, Ayanian, and Sukhatme: https://arxiv.org/abs/1704.04852 26 | s = refine_schedule(s); 27 | [~, ~, N] = size(s); 28 | 29 | % apply scaling from integer grid into metric space 30 | s = SCALE * s; 31 | boxes = SCALE * boxes; 32 | bbox = SCALE * [[-0.5 -0.5 -0.5]', dims(:) - 0.5]; 33 | 34 | % print some info about the discrete plan input 35 | analyze_schedule(s); 36 | 37 | % optional: clip the number of robots so it runs faster 38 | %N = 2; 39 | %s = s(:,:,[1 20]); 40 | 41 | % add extra stationary steps at begin and end for smooth acceleration 42 | s = cat(2, s(:,1,:), s(:,1,:), s, s(:,end,:)); 43 | 44 | % polynomial degree 45 | deg = 7; 46 | 47 | % how many derivatives must be continuous 48 | cont = 4; 49 | 50 | % robot-robot collision ellipsoid 51 | ellipsoid = [0.12 0.12 0.3]; 52 | 53 | % robot-obstacle collision ellipsoid 54 | obs_ellipsoid = [0.15 0.15 0.15]; 55 | 56 | % number of iterations of refinement 57 | iters = 1; 58 | 59 | % robot/obstacle separating hyperplane function 60 | pp_obs_sep_fun = @(pps, obs_ellipsoid) pp_obs_sep_grid(pps, obs_ellipsoid, boxes); 61 | 62 | % main routine 63 | [pps, costs, corridors] = smoothener(s, bbox, deg, cont, TIMESCALE, ellipsoid, obs_ellipsoid, iters, ... 64 | pp_obs_sep_fun, @corridor_trajectory_optimize_sequence); 65 | 66 | % Plot the results. 67 | % ----------------- 68 | 69 | % smoothener returns the piecewise polynomials for every iteration of refinement. 70 | % here, we plot only the results of the final iteration. 71 | pps = pps(end,:); 72 | 73 | % set up the figure. 74 | clf; hold on; axis equal; 75 | light('Position', [1 1 1]); 76 | light('Position', [1 1 -1]); 77 | campos([-32 45 21]); 78 | 79 | % render obstacles 80 | for i=1:size(boxes,3) 81 | vtx = corners_to_box_vertices(boxes(:,1,i), boxes(:,2,i)); 82 | 83 | % trying to combat z-fighting 84 | shift = 0.01 * (rand(1,3) - 0.5); 85 | vtx = bsxfun(@plus, vtx, shift); 86 | 87 | tri = convhulln(vtx); 88 | color = 0.0 * rand(3,1) + 0.4; 89 | trisurf(tri, vtx(:,1), vtx(:,2), vtx(:,3), ... 90 | 'FaceColor', color, 'EdgeColor', 'none', ... 91 | 'SpecularStrength', 0.1, 'AmbientStrength', 0.5, 'facealpha',0.5); 92 | end 93 | 94 | % render the output 95 | for i=1:N 96 | % plot the discrete plan 97 | h = plot3n(s(:,:,i)); 98 | 99 | % plot the continuous trajectory 100 | color = get(h, 'color'); 101 | duration = pps{i}.breaks(end); 102 | t = 0:0.1:duration; 103 | x = ppval(pps{i}, t); 104 | h = plot3n(x, 'color', color, 'LineWidth', 3); 105 | 106 | % plot the start and endpoints as markers 107 | lastPos = ppval(pps{i}, duration); 108 | scatter3(lastPos(1), lastPos(2), lastPos(3),50,'k','filled'); 109 | firstPos = ppval(pps{i}, 0); 110 | scatter3(firstPos(1), firstPos(2), firstPos(3),50,'k', 'square');%'g','filled'); 111 | end 112 | 113 | 114 | % Save the piecewise polynomial coefficients to a directory of CSV files. 115 | % ----------------------------------------------------------------------- 116 | SAVE_CSVS = true; 117 | if SAVE_CSVS 118 | DIR = [EXAMPLE '_pps']; 119 | mkdir(DIR); 120 | for i=1:N 121 | filename = sprintf('%s/pp%d.csv', DIR, i); 122 | pp2csv(pps{i}, filename) 123 | end 124 | end 125 | 126 | end 127 | -------------------------------------------------------------------------------- /octomap_corridor/array2d.hpp: -------------------------------------------------------------------------------- 1 | #ifndef __ARRAY2D_HPP__ 2 | #define __ARRAY2D_HPP__ 3 | 4 | // 5 | // StaticArray2D has fixed size at compile-time, 6 | // DynamicArray2D has fixed size at construction. 7 | // 8 | // Memory is stored in a contiguous block. 9 | // begin() and end() "iterators" allow std::algorithm compatibility. 10 | // 11 | // Operator() is used for 2D indexing. 12 | // Oprerator[] is used for 1D indexing. 13 | // Arrays are row-major: arr(3, 1) and arr(3, 2) are consecutive in memory. 14 | // 15 | // 16 | // Copyright 2013 James Preiss. 17 | // Public Domain - Attribution is appreciated. 18 | // 19 | 20 | #include 21 | 22 | template 23 | class StaticArray2D 24 | { 25 | public: 26 | // default-construct all data 27 | void initialize() 28 | { 29 | for (size_t i = 0; i < Rows * Columns; ++i) 30 | { 31 | data_[i] = T(); 32 | } 33 | } 34 | 35 | size_t rows() const 36 | { 37 | return Rows; 38 | } 39 | 40 | size_t columns() const 41 | { 42 | return Columns; 43 | } 44 | 45 | size_t to1D(size_t row, size_t column) const 46 | { 47 | return row * Columns + column; 48 | } 49 | 50 | std::pair to2D(size_t index) const 51 | { 52 | return std::make_pair(index / Columns, index % Columns); 53 | } 54 | 55 | T const &operator[](size_t index) const 56 | { 57 | return data_[index]; 58 | } 59 | 60 | T &operator[](size_t index) 61 | { 62 | return data_[index]; 63 | } 64 | 65 | T const &operator()(size_t row, size_t column) const 66 | { 67 | return data_[to1D(row, column)]; 68 | } 69 | 70 | T &operator()(size_t row, size_t column) 71 | { 72 | return data_[to1D(row, column)]; 73 | } 74 | 75 | T const *data() const 76 | { 77 | return data_; 78 | } 79 | 80 | T *data() 81 | { 82 | return data_; 83 | } 84 | 85 | T const *begin() const 86 | { 87 | return data_; 88 | } 89 | 90 | T *begin() 91 | { 92 | return data_; 93 | } 94 | 95 | T const *end() const 96 | { 97 | return data_ + Rows * Columns; 98 | } 99 | 100 | T *end() 101 | { 102 | return data_ + Rows * Columns; 103 | } 104 | 105 | private: 106 | T data_[Rows * Columns]; 107 | }; 108 | 109 | 110 | template 111 | class Array2D 112 | { 113 | public: 114 | Array2D(size_t rows, size_t columns) : 115 | rows_(rows), 116 | columns_(columns), 117 | data_(new T[rows * columns]) 118 | { 119 | } 120 | 121 | Array2D(Array2D const &) = delete; 122 | 123 | Array2D(Array2D &&a) : 124 | rows_(a.rows_), 125 | columns_(a.columns_), 126 | data_(a.data_) 127 | { 128 | a.data_ = nullptr; 129 | a.rows_ = 0; 130 | a.columns_ = 0; 131 | } 132 | 133 | ~Array2D() 134 | { 135 | delete[] data_; 136 | } 137 | 138 | size_t rows() const 139 | { 140 | return rows_; 141 | } 142 | 143 | size_t columns() const 144 | { 145 | return columns_; 146 | } 147 | 148 | size_t to1D(size_t row, size_t column) const 149 | { 150 | return row * columns_ + column; 151 | } 152 | 153 | std::pair to2D(size_t index) const 154 | { 155 | return std::make_pair(index / columns_, index % columns_); 156 | } 157 | 158 | T const &operator[](size_t index) const 159 | { 160 | return data_[index]; 161 | } 162 | 163 | T &operator[](size_t index) 164 | { 165 | return data_[index]; 166 | } 167 | 168 | T const &operator()(size_t row, size_t column) const 169 | { 170 | return data_[to1D(row, column)]; 171 | } 172 | 173 | T &operator()(size_t row, size_t column) 174 | { 175 | return data_[to1D(row, column)]; 176 | } 177 | 178 | T const *data() const 179 | { 180 | return data_; 181 | } 182 | 183 | T *data() 184 | { 185 | return data_; 186 | } 187 | 188 | T const *begin() const 189 | { 190 | return data_; 191 | } 192 | 193 | T *begin() 194 | { 195 | return data_; 196 | } 197 | 198 | T const *end() const 199 | { 200 | return data_ + rows_ * columns_; 201 | } 202 | 203 | T *end() 204 | { 205 | return data_ + rows_ * columns_; 206 | } 207 | 208 | private: 209 | size_t rows_; 210 | size_t columns_; 211 | T *data_; 212 | }; 213 | #endif // __ARRAY2D_HPP__ 214 | -------------------------------------------------------------------------------- /svm_sephyp_32point/all_hyperplanes_pps_mex.c: -------------------------------------------------------------------------------- 1 | /* 2 | % inputs: paths: [DIM x PPVAL_PTS x NROBOTS] array of points 3 | % ellipsoid 4 | % 5 | % outputs: A: [DIM x NROBOTS x NROBOTS] array of 6 | % hyperplane normal vectors for each robot-robot interaction. 7 | % b: [NROBOTS x NROBOTS] array 8 | % distance from origin for hyperplanes. i.e. a(:,...)^T x <= b. 9 | % 10 | function [A, b] = all_hyperplanes(paths, ellipsoid) 11 | */ 12 | 13 | #include "mex.h" 14 | #include "solver.h" 15 | 16 | #define SQR(x) ((x) * (x)) 17 | #define SET_SZ 32 18 | 19 | Vars vars; 20 | Params params; 21 | Workspace work; 22 | Settings settings; 23 | 24 | void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) { 25 | if (nrhs != 2) { 26 | mexErrMsgTxt("need nrhs == 2\n"); 27 | } 28 | if (nlhs != 2) { 29 | mexErrMsgTxt("need nlhs == 2\n"); 30 | } 31 | mxArray const *pathsArr = prhs[0]; 32 | mxArray const *ellipsoidArr = prhs[1]; 33 | double const *paths = mxGetPr(prhs[0]); 34 | double const *ellipsoid = mxGetPr(prhs[1]); 35 | 36 | // paths: [DIM x PPVAL_PTS x NROBOTS] array of points 37 | mwSize ndim = mxGetNumberOfDimensions(pathsArr); 38 | if (ndim != 3) { 39 | mexErrMsgTxt("paths input must be 3d array"); 40 | } 41 | mwSize const *pathsDim = mxGetDimensions(pathsArr); 42 | if (pathsDim[0] != 3) { 43 | mexErrMsgTxt("first dimension of paths input must be 3"); 44 | } 45 | if (pathsDim[1] != SET_SZ) { 46 | mexErrMsgTxt("second dimension of paths input must be SET_SZ"); 47 | } 48 | mwSize const nrobots = pathsDim[2]; 49 | 50 | // ellipsoid: 1x3 or 3x1 51 | if (mxGetNumberOfElements(ellipsoidArr) != 3) { 52 | mexErrMsgTxt("ellipsoid input must 3x1 or 1x3"); 53 | } 54 | 55 | // A: [DIM x NROBOTS x NROBOTS] array of 56 | // b: distance from origin for hyperplanes. i.e. a(:,...)^T x <= b. 57 | mwSize Adims[3] = {3, nrobots, nrobots}; 58 | mwSize bdims[2] = {nrobots, nrobots}; 59 | plhs[0] = mxCreateNumericArray(3, Adims, mxDOUBLE_CLASS, 0); 60 | plhs[1] = mxCreateNumericArray(2, bdims, mxDOUBLE_CLASS, 0); 61 | 62 | double *A = mxGetPr(plhs[0]); 63 | double *b = mxGetPr(plhs[1]); 64 | 65 | set_defaults(); 66 | setup_indexing(); 67 | 68 | params.Q[0] = SQR(ellipsoid[0]); 69 | params.Q[1] = SQR(ellipsoid[1]); 70 | params.Q[2] = SQR(ellipsoid[2]); 71 | params.Q[3] = 0.0; 72 | 73 | // paths: [DIM x PPVAL_PTS x NROBOTS] array of points 74 | #define pathind(i1, i2, i3) paths[(i1) + 3*(i2) + SET_SZ*3*(i3)] 75 | // A: [DIM x NROBOTS x NROBOTS] array of ... 76 | #define Aind(i1, i2, i3) A[(i1) + 3*(i2) + 3*nrobots*(i3)] 77 | // b: distance from origin for hyperplanes. i.e. a(:,...)^T x <= b. 78 | #define bind(i1, i2) b[(i1) + nrobots*(i2)] 79 | 80 | #define paramsAind(i1, i2) params.A[(i1) + SET_SZ*2*(i2)] 81 | 82 | for (int i = 0; i < SET_SZ; ++i) { 83 | paramsAind(i, 3) = 1.0; 84 | } 85 | for (int i = SET_SZ; i < 2*SET_SZ; ++i) { 86 | paramsAind(i, 3) = -1.0; 87 | } 88 | 89 | for (int i = 0; i < nrobots; ++i) { 90 | Aind(0, i, i) = NAN; 91 | Aind(1, i, i) = NAN; 92 | Aind(2, i, i) = NAN; 93 | bind(i, i) = NAN; 94 | 95 | for (int dim = 0; dim < 3; ++dim) { 96 | for (int p = 0; p < SET_SZ; ++p) { 97 | paramsAind(p, dim) = pathind(dim, p, i); 98 | } 99 | } 100 | 101 | for (int j = i+1; j < nrobots; ++j) { 102 | for (int dim = 0; dim < 3; ++dim) { 103 | for (int p = 0; p < SET_SZ; ++p) { 104 | paramsAind(SET_SZ + p, dim) = -pathind(dim, p, j); 105 | } 106 | } 107 | 108 | set_defaults(); 109 | settings.verbose = 0; 110 | setup_indexing(); 111 | solve(); 112 | 113 | double b0 = vars.beta[0]; 114 | double b1 = vars.beta[1]; 115 | double b2 = vars.beta[2]; 116 | double b3 = vars.beta[3]; 117 | 118 | double norm = sqrt(SQR(b0) + SQR(b1) + SQR(b2)); 119 | 120 | // A: [DIM x NROBOTS x NROBOTS] array of ... 121 | Aind(0, i, j) = b0 / norm; 122 | Aind(1, i, j) = b1 / norm; 123 | Aind(2, i, j) = b2 / norm; 124 | bind(i, j) = -b3 / norm; 125 | 126 | Aind(0, j, i) = -b0 / norm; 127 | Aind(1, j, i) = -b1 / norm; 128 | Aind(2, j, i) = -b2 / norm; 129 | bind(j, i) = b3 / norm; 130 | } 131 | } 132 | } 133 | -------------------------------------------------------------------------------- /svm_sephyp_2point/all_hyperplanes_waypoints_mex.c: -------------------------------------------------------------------------------- 1 | /* 2 | % inputs: paths: [DIM x NPTS x NROBOTS] array of points 3 | % (TODO: input times?) 4 | % 5 | % outputs: A: [DIM x NROBOTS x NROBOTS x (NPTS - 1)] array of 6 | % hyperplane normal vectors for each robot-robot interaction 7 | % at each segment. 8 | % b: distance from origin for hyperplanes. i.e. a(:,...)^T x <= b. 9 | % 10 | function [A, b] = all_hyperplanes(paths, ellipsoid) 11 | */ 12 | 13 | #include "mex.h" 14 | #include "solver.h" 15 | 16 | #define SQR(x) ((x) * (x)) 17 | 18 | Vars vars; 19 | Params params; 20 | Workspace work; 21 | Settings settings; 22 | 23 | void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) { 24 | if (nrhs != 2) { 25 | mexErrMsgTxt("need nrhs == 2\n"); 26 | } 27 | if (nlhs != 2) { 28 | mexErrMsgTxt("need nlhs == 2\n"); 29 | } 30 | mxArray const *pathsArr = prhs[0]; 31 | mxArray const *ellipsoidArr = prhs[1]; 32 | double const *paths = mxGetPr(prhs[0]); 33 | double const *ellipsoid = mxGetPr(prhs[1]); 34 | 35 | // paths: [DIM x NPTS x NROBOTS] array of points 36 | mwSize ndim = mxGetNumberOfDimensions(pathsArr); 37 | if (ndim != 3) { 38 | mexErrMsgTxt("paths input must be 3d array"); 39 | } 40 | mwSize const *pathsDim = mxGetDimensions(pathsArr); 41 | if (pathsDim[0] != 3) { 42 | mexErrMsgTxt("first dimension of paths input must be 3"); 43 | } 44 | mwSize const npts = pathsDim[1]; 45 | mwSize const nrobots = pathsDim[2]; 46 | 47 | // ellipsoid: 1x3 or 3x1 48 | if (mxGetNumberOfElements(ellipsoidArr) != 3) { 49 | mexErrMsgTxt("ellipsoid input must 3x1 or 1x3"); 50 | } 51 | 52 | // A: [DIM x NROBOTS x NROBOTS x (NPTS - 1)] array of 53 | // b: distance from origin for hyperplanes. i.e. a(:,...)^T x <= b. 54 | mwSize Adims[4] = {3, nrobots, nrobots, npts-1}; 55 | mwSize bdims[3] = {nrobots, nrobots, npts-1}; 56 | plhs[0] = mxCreateNumericArray(4, Adims, mxDOUBLE_CLASS, 0); 57 | plhs[1] = mxCreateNumericArray(3, bdims, mxDOUBLE_CLASS, 0); 58 | 59 | double *A = mxGetPr(plhs[0]); 60 | double *b = mxGetPr(plhs[1]); 61 | 62 | set_defaults(); 63 | setup_indexing(); 64 | 65 | 66 | params.Q[0] = SQR(ellipsoid[0]); 67 | params.Q[1] = SQR(ellipsoid[1]); 68 | params.Q[2] = SQR(ellipsoid[2]); 69 | params.Q[3] = 0.0; 70 | 71 | params.A[12] = 1.0; 72 | params.A[13] = 1.0; 73 | params.A[14] = -1.0; 74 | params.A[15] = -1.0; 75 | 76 | // paths: [DIM x NPTS x NROBOTS] array of points 77 | #define pathind(i1, i2, i3) paths[(i1) + 3*(i2) + npts*3*(i3)] 78 | // A: [DIM x NROBOTS x NROBOTS x (NPTS - 1)] array of 79 | #define Aind(i1, i2, i3, i4) A[(i1) + 3*(i2) + 3*nrobots*(i3) + 3*nrobots*nrobots*(i4)] 80 | // b: distance from origin for hyperplanes. i.e. a(:,...)^T x <= b. 81 | #define bind(i1, i2, i3) b[(i1) + nrobots*(i2) + nrobots*nrobots*(i3)] 82 | 83 | for (int step = 0; step < (npts - 1); ++step) { 84 | for (int i = 0; i < nrobots; ++i) { 85 | Aind(0, i, i, step) = NAN; 86 | Aind(1, i, i, step) = NAN; 87 | Aind(2, i, i, step) = NAN; 88 | bind(i, i, step) = NAN; 89 | 90 | for (int j = i+1; j < nrobots; ++j) { 91 | params.A[0] = pathind(0, step, i); 92 | params.A[1] = pathind(0, step+1, i); 93 | params.A[2] = -pathind(0, step, j); 94 | params.A[3] = -pathind(0, step+1, j); 95 | 96 | params.A[4] = pathind(1, step, i); 97 | params.A[5] = pathind(1, step+1, i); 98 | params.A[6] = -pathind(1, step, j); 99 | params.A[7] = -pathind(1, step+1, j); 100 | 101 | params.A[8] = pathind(2, step, i); 102 | params.A[9] = pathind(2, step+1, i); 103 | params.A[10] = -pathind(2, step, j); 104 | params.A[11] = -pathind(2, step+1, j); 105 | 106 | set_defaults(); 107 | settings.verbose = 0; 108 | setup_indexing(); 109 | solve(); 110 | 111 | double b0 = vars.beta[0]; 112 | double b1 = vars.beta[1]; 113 | double b2 = vars.beta[2]; 114 | double b3 = vars.beta[3]; 115 | 116 | double norm = sqrt(SQR(b0) + SQR(b1) + SQR(b2)); 117 | 118 | Aind(0, i, j, step) = b0 / norm; 119 | Aind(1, i, j, step) = b1 / norm; 120 | Aind(2, i, j, step) = b2 / norm; 121 | bind(i, j, step) = -b3 / norm; 122 | 123 | Aind(0, j, i, step) = -b0 / norm; 124 | Aind(1, j, i, step) = -b1 / norm; 125 | Aind(2, j, i, step) = -b2 / norm; 126 | bind(j, i, step) = b3 / norm; 127 | } 128 | } 129 | } 130 | } 131 | -------------------------------------------------------------------------------- /svm_sephyp_2point/solver.h: -------------------------------------------------------------------------------- 1 | /* Produced by CVXGEN, 2016-11-30 17:09:32 -0500. */ 2 | /* CVXGEN is Copyright (C) 2006-2012 Jacob Mattingley, jem@cvxgen.com. */ 3 | /* The code in this file is Copyright (C) 2006-2012 Jacob Mattingley. */ 4 | /* CVXGEN, or solvers produced by CVXGEN, cannot be used for commercial */ 5 | /* applications without prior written permission from Jacob Mattingley. */ 6 | 7 | /* Filename: solver.h. */ 8 | /* Description: Header file with relevant definitions. */ 9 | #ifndef SOLVER_H 10 | #define SOLVER_H 11 | /* Uncomment the next line to remove all library dependencies. */ 12 | /*#define ZERO_LIBRARY_MODE */ 13 | #ifdef MATLAB_MEX_FILE 14 | /* Matlab functions. MATLAB_MEX_FILE will be defined by the mex compiler. */ 15 | /* If you are not using the mex compiler, this functionality will not intrude, */ 16 | /* as it will be completely disabled at compile-time. */ 17 | #include "mex.h" 18 | #else 19 | #ifndef ZERO_LIBRARY_MODE 20 | #include 21 | #endif 22 | #endif 23 | /* Space must be allocated somewhere (testsolver.c, csolve.c or your own */ 24 | /* program) for the global variables vars, params, work and settings. */ 25 | /* At the bottom of this file, they are externed. */ 26 | #ifndef ZERO_LIBRARY_MODE 27 | #include 28 | #define pm(A, m, n) printmatrix(#A, A, m, n, 1) 29 | #endif 30 | typedef struct Params_t { 31 | double Q[4]; 32 | double A[16]; 33 | } Params; 34 | typedef struct Vars_t { 35 | double *beta; /* 4 rows. */ 36 | } Vars; 37 | typedef struct Workspace_t { 38 | double h[4]; 39 | double s_inv[4]; 40 | double s_inv_z[4]; 41 | double *b; 42 | double q[4]; 43 | double rhs[12]; 44 | double x[12]; 45 | double *s; 46 | double *z; 47 | double *y; 48 | double lhs_aff[12]; 49 | double lhs_cc[12]; 50 | double buffer[12]; 51 | double buffer2[12]; 52 | double KKT[32]; 53 | double L[26]; 54 | double d[12]; 55 | double v[12]; 56 | double d_inv[12]; 57 | double gap; 58 | double optval; 59 | double ineq_resid_squared; 60 | double eq_resid_squared; 61 | double block_33[1]; 62 | /* Pre-op symbols. */ 63 | int converged; 64 | } Workspace; 65 | typedef struct Settings_t { 66 | double resid_tol; 67 | double eps; 68 | int max_iters; 69 | int refine_steps; 70 | int better_start; 71 | /* Better start obviates the need for s_init and z_init. */ 72 | double s_init; 73 | double z_init; 74 | int verbose; 75 | /* Show extra details of the iterative refinement steps. */ 76 | int verbose_refinement; 77 | int debug; 78 | /* For regularization. Minimum value of abs(D_ii) in the kkt D factor. */ 79 | double kkt_reg; 80 | } Settings; 81 | extern Vars vars; 82 | extern Params params; 83 | extern Workspace work; 84 | extern Settings settings; 85 | /* Function definitions in ldl.c: */ 86 | void ldl_solve(double *target, double *var); 87 | void ldl_factor(void); 88 | double check_factorization(void); 89 | void matrix_multiply(double *result, double *source); 90 | double check_residual(double *target, double *multiplicand); 91 | void fill_KKT(void); 92 | 93 | /* Function definitions in matrix_support.c: */ 94 | void multbymA(double *lhs, double *rhs); 95 | void multbymAT(double *lhs, double *rhs); 96 | void multbymG(double *lhs, double *rhs); 97 | void multbymGT(double *lhs, double *rhs); 98 | void multbyP(double *lhs, double *rhs); 99 | void fillq(void); 100 | void fillh(void); 101 | void fillb(void); 102 | void pre_ops(void); 103 | 104 | /* Function definitions in solver.c: */ 105 | double eval_gap(void); 106 | void set_defaults(void); 107 | void setup_pointers(void); 108 | void setup_indexing(void); 109 | void set_start(void); 110 | double eval_objv(void); 111 | void fillrhs_aff(void); 112 | void fillrhs_cc(void); 113 | void refine(double *target, double *var); 114 | double calc_ineq_resid_squared(void); 115 | double calc_eq_resid_squared(void); 116 | void better_start(void); 117 | void fillrhs_start(void); 118 | long solve(void); 119 | 120 | /* Function definitions in testsolver.c: */ 121 | int main(int argc, char **argv); 122 | void load_default_data(void); 123 | 124 | /* Function definitions in util.c: */ 125 | void tic(void); 126 | float toc(void); 127 | float tocq(void); 128 | void printmatrix(char *name, double *A, int m, int n, int sparse); 129 | double unif(double lower, double upper); 130 | float ran1(long*idum, int reset); 131 | float randn_internal(long *idum, int reset); 132 | double randn(void); 133 | void reset_rand(void); 134 | 135 | #endif 136 | -------------------------------------------------------------------------------- /svm_sephyp_32point/solver.h: -------------------------------------------------------------------------------- 1 | /* Produced by CVXGEN, 2016-11-30 16:59:57 -0500. */ 2 | /* CVXGEN is Copyright (C) 2006-2012 Jacob Mattingley, jem@cvxgen.com. */ 3 | /* The code in this file is Copyright (C) 2006-2012 Jacob Mattingley. */ 4 | /* CVXGEN, or solvers produced by CVXGEN, cannot be used for commercial */ 5 | /* applications without prior written permission from Jacob Mattingley. */ 6 | 7 | /* Filename: solver.h. */ 8 | /* Description: Header file with relevant definitions. */ 9 | #ifndef SOLVER_H 10 | #define SOLVER_H 11 | /* Uncomment the next line to remove all library dependencies. */ 12 | /*#define ZERO_LIBRARY_MODE */ 13 | #ifdef MATLAB_MEX_FILE 14 | /* Matlab functions. MATLAB_MEX_FILE will be defined by the mex compiler. */ 15 | /* If you are not using the mex compiler, this functionality will not intrude, */ 16 | /* as it will be completely disabled at compile-time. */ 17 | #include "mex.h" 18 | #else 19 | #ifndef ZERO_LIBRARY_MODE 20 | #include 21 | #endif 22 | #endif 23 | /* Space must be allocated somewhere (testsolver.c, csolve.c or your own */ 24 | /* program) for the global variables vars, params, work and settings. */ 25 | /* At the bottom of this file, they are externed. */ 26 | #ifndef ZERO_LIBRARY_MODE 27 | #include 28 | #define pm(A, m, n) printmatrix(#A, A, m, n, 1) 29 | #endif 30 | typedef struct Params_t { 31 | double Q[4]; 32 | double A[256]; 33 | } Params; 34 | typedef struct Vars_t { 35 | double *beta; /* 4 rows. */ 36 | } Vars; 37 | typedef struct Workspace_t { 38 | double h[64]; 39 | double s_inv[64]; 40 | double s_inv_z[64]; 41 | double *b; 42 | double q[4]; 43 | double rhs[132]; 44 | double x[132]; 45 | double *s; 46 | double *z; 47 | double *y; 48 | double lhs_aff[132]; 49 | double lhs_cc[132]; 50 | double buffer[132]; 51 | double buffer2[132]; 52 | double KKT[452]; 53 | double L[326]; 54 | double d[132]; 55 | double v[132]; 56 | double d_inv[132]; 57 | double gap; 58 | double optval; 59 | double ineq_resid_squared; 60 | double eq_resid_squared; 61 | double block_33[1]; 62 | /* Pre-op symbols. */ 63 | int converged; 64 | } Workspace; 65 | typedef struct Settings_t { 66 | double resid_tol; 67 | double eps; 68 | int max_iters; 69 | int refine_steps; 70 | int better_start; 71 | /* Better start obviates the need for s_init and z_init. */ 72 | double s_init; 73 | double z_init; 74 | int verbose; 75 | /* Show extra details of the iterative refinement steps. */ 76 | int verbose_refinement; 77 | int debug; 78 | /* For regularization. Minimum value of abs(D_ii) in the kkt D factor. */ 79 | double kkt_reg; 80 | } Settings; 81 | extern Vars vars; 82 | extern Params params; 83 | extern Workspace work; 84 | extern Settings settings; 85 | /* Function definitions in ldl.c: */ 86 | void ldl_solve(double *target, double *var); 87 | void ldl_factor(void); 88 | double check_factorization(void); 89 | void matrix_multiply(double *result, double *source); 90 | double check_residual(double *target, double *multiplicand); 91 | void fill_KKT(void); 92 | 93 | /* Function definitions in matrix_support.c: */ 94 | void multbymA(double *lhs, double *rhs); 95 | void multbymAT(double *lhs, double *rhs); 96 | void multbymG(double *lhs, double *rhs); 97 | void multbymGT(double *lhs, double *rhs); 98 | void multbyP(double *lhs, double *rhs); 99 | void fillq(void); 100 | void fillh(void); 101 | void fillb(void); 102 | void pre_ops(void); 103 | 104 | /* Function definitions in solver.c: */ 105 | double eval_gap(void); 106 | void set_defaults(void); 107 | void setup_pointers(void); 108 | void setup_indexing(void); 109 | void set_start(void); 110 | double eval_objv(void); 111 | void fillrhs_aff(void); 112 | void fillrhs_cc(void); 113 | void refine(double *target, double *var); 114 | double calc_ineq_resid_squared(void); 115 | double calc_eq_resid_squared(void); 116 | void better_start(void); 117 | void fillrhs_start(void); 118 | long solve(void); 119 | 120 | /* Function definitions in testsolver.c: */ 121 | int main(int argc, char **argv); 122 | void load_default_data(void); 123 | 124 | /* Function definitions in util.c: */ 125 | void tic(void); 126 | float toc(void); 127 | float tocq(void); 128 | void printmatrix(char *name, double *A, int m, int n, int sparse); 129 | double unif(double lower, double upper); 130 | float ran1(long*idum, int reset); 131 | float randn_internal(long *idum, int reset); 132 | double randn(void); 133 | void reset_rand(void); 134 | 135 | #endif 136 | -------------------------------------------------------------------------------- /octomap_corridor/cvxgen/solver.h: -------------------------------------------------------------------------------- 1 | /* Produced by CVXGEN, 2016-11-30 16:59:57 -0500. */ 2 | /* CVXGEN is Copyright (C) 2006-2012 Jacob Mattingley, jem@cvxgen.com. */ 3 | /* The code in this file is Copyright (C) 2006-2012 Jacob Mattingley. */ 4 | /* CVXGEN, or solvers produced by CVXGEN, cannot be used for commercial */ 5 | /* applications without prior written permission from Jacob Mattingley. */ 6 | 7 | /* Filename: solver.h. */ 8 | /* Description: Header file with relevant definitions. */ 9 | #ifndef SOLVER_H 10 | #define SOLVER_H 11 | /* Uncomment the next line to remove all library dependencies. */ 12 | /*#define ZERO_LIBRARY_MODE */ 13 | #ifdef MATLAB_MEX_FILE 14 | /* Matlab functions. MATLAB_MEX_FILE will be defined by the mex compiler. */ 15 | /* If you are not using the mex compiler, this functionality will not intrude, */ 16 | /* as it will be completely disabled at compile-time. */ 17 | #include "mex.h" 18 | #else 19 | #ifndef ZERO_LIBRARY_MODE 20 | #include 21 | #endif 22 | #endif 23 | /* Space must be allocated somewhere (testsolver.c, csolve.c or your own */ 24 | /* program) for the global variables vars, params, work and settings. */ 25 | /* At the bottom of this file, they are externed. */ 26 | #ifndef ZERO_LIBRARY_MODE 27 | #include 28 | #define pm(A, m, n) printmatrix(#A, A, m, n, 1) 29 | #endif 30 | 31 | #ifdef __cplusplus 32 | extern "C" { 33 | #endif 34 | 35 | typedef struct Params_t { 36 | double Q[4]; 37 | double A[256]; 38 | } Params; 39 | typedef struct Vars_t { 40 | double *beta; /* 4 rows. */ 41 | } Vars; 42 | typedef struct Workspace_t { 43 | double h[64]; 44 | double s_inv[64]; 45 | double s_inv_z[64]; 46 | double *b; 47 | double q[4]; 48 | double rhs[132]; 49 | double x[132]; 50 | double *s; 51 | double *z; 52 | double *y; 53 | double lhs_aff[132]; 54 | double lhs_cc[132]; 55 | double buffer[132]; 56 | double buffer2[132]; 57 | double KKT[452]; 58 | double L[326]; 59 | double d[132]; 60 | double v[132]; 61 | double d_inv[132]; 62 | double gap; 63 | double optval; 64 | double ineq_resid_squared; 65 | double eq_resid_squared; 66 | double block_33[1]; 67 | /* Pre-op symbols. */ 68 | int converged; 69 | } Workspace; 70 | typedef struct Settings_t { 71 | double resid_tol; 72 | double eps; 73 | int max_iters; 74 | int refine_steps; 75 | int better_start; 76 | /* Better start obviates the need for s_init and z_init. */ 77 | double s_init; 78 | double z_init; 79 | int verbose; 80 | /* Show extra details of the iterative refinement steps. */ 81 | int verbose_refinement; 82 | int debug; 83 | /* For regularization. Minimum value of abs(D_ii) in the kkt D factor. */ 84 | double kkt_reg; 85 | } Settings; 86 | extern Vars vars; 87 | extern Params params; 88 | extern Workspace work; 89 | extern Settings settings; 90 | /* Function definitions in ldl.c: */ 91 | void ldl_solve(double *target, double *var); 92 | void ldl_factor(void); 93 | double check_factorization(void); 94 | void matrix_multiply(double *result, double *source); 95 | double check_residual(double *target, double *multiplicand); 96 | void fill_KKT(void); 97 | 98 | /* Function definitions in matrix_support.c: */ 99 | void multbymA(double *lhs, double *rhs); 100 | void multbymAT(double *lhs, double *rhs); 101 | void multbymG(double *lhs, double *rhs); 102 | void multbymGT(double *lhs, double *rhs); 103 | void multbyP(double *lhs, double *rhs); 104 | void fillq(void); 105 | void fillh(void); 106 | void fillb(void); 107 | void pre_ops(void); 108 | 109 | /* Function definitions in solver.c: */ 110 | double eval_gap(void); 111 | void set_defaults(void); 112 | void setup_pointers(void); 113 | void setup_indexing(void); 114 | void set_start(void); 115 | double eval_objv(void); 116 | void fillrhs_aff(void); 117 | void fillrhs_cc(void); 118 | void refine(double *target, double *var); 119 | double calc_ineq_resid_squared(void); 120 | double calc_eq_resid_squared(void); 121 | void better_start(void); 122 | void fillrhs_start(void); 123 | long solve(void); 124 | 125 | /* Function definitions in testsolver.c: */ 126 | int main(int argc, char **argv); 127 | void load_default_data(void); 128 | 129 | /* Function definitions in util.c: */ 130 | void tic(void); 131 | float toc(void); 132 | float tocq(void); 133 | void printmatrix(char *name, double *A, int m, int n, int sparse); 134 | double unif(double lower, double upper); 135 | float ran1(long*idum, int reset); 136 | float randn_internal(long *idum, int reset); 137 | double randn(void); 138 | void reset_rand(void); 139 | 140 | #ifdef __cplusplus 141 | } 142 | #endif 143 | 144 | 145 | #endif 146 | -------------------------------------------------------------------------------- /external/stlread/stlread.m: -------------------------------------------------------------------------------- 1 | function varargout = stlread(file) 2 | % STLREAD imports geometry from an STL file into MATLAB. 3 | % FV = STLREAD(FILENAME) imports triangular faces from the ASCII or binary 4 | % STL file idicated by FILENAME, and returns the patch struct FV, with fields 5 | % 'faces' and 'vertices'. 6 | % 7 | % [F,V] = STLREAD(FILENAME) returns the faces F and vertices V separately. 8 | % 9 | % [F,V,N] = STLREAD(FILENAME) also returns the face normal vectors. 10 | % 11 | % The faces and vertices are arranged in the format used by the PATCH plot 12 | % object. 13 | 14 | % Copyright 2011 The MathWorks, Inc. 15 | 16 | if ~exist(file,'file') 17 | error(['File ''%s'' not found. If the file is not on MATLAB''s path' ... 18 | ', be sure to specify the full path to the file.'], file); 19 | end 20 | 21 | fid = fopen(file,'r'); 22 | if ~isempty(ferror(fid)) 23 | error(lasterror); %#ok 24 | end 25 | 26 | M = fread(fid,inf,'uint8=>uint8'); 27 | fclose(fid); 28 | 29 | [f,v,n] = stlbinary(M); 30 | %if( isbinary(M) ) % This may not be a reliable test 31 | % [f,v,n] = stlbinary(M); 32 | %else 33 | % [f,v,n] = stlascii(M); 34 | %end 35 | 36 | varargout = cell(1,nargout); 37 | switch nargout 38 | case 2 39 | varargout{1} = f; 40 | varargout{2} = v; 41 | case 3 42 | varargout{1} = f; 43 | varargout{2} = v; 44 | varargout{3} = n; 45 | otherwise 46 | varargout{1} = struct('faces',f,'vertices',v); 47 | end 48 | 49 | end 50 | 51 | 52 | function [F,V,N] = stlbinary(M) 53 | 54 | F = []; 55 | V = []; 56 | N = []; 57 | 58 | if length(M) < 84 59 | error('MATLAB:stlread:incorrectFormat', ... 60 | 'Incomplete header information in binary STL file.'); 61 | end 62 | 63 | % Bytes 81-84 are an unsigned 32-bit integer specifying the number of faces 64 | % that follow. 65 | numFaces = typecast(M(81:84),'uint32'); 66 | %numFaces = double(numFaces); 67 | if numFaces == 0 68 | warning('MATLAB:stlread:nodata','No data in STL file.'); 69 | return 70 | end 71 | 72 | T = M(85:end); 73 | F = NaN(numFaces,3); 74 | V = NaN(3*numFaces,3); 75 | N = NaN(numFaces,3); 76 | 77 | numRead = 0; 78 | while numRead < numFaces 79 | % Each facet is 50 bytes 80 | % - Three single precision values specifying the face normal vector 81 | % - Three single precision values specifying the first vertex (XYZ) 82 | % - Three single precision values specifying the second vertex (XYZ) 83 | % - Three single precision values specifying the third vertex (XYZ) 84 | % - Two unused bytes 85 | i1 = 50 * numRead + 1; 86 | i2 = i1 + 50 - 1; 87 | facet = T(i1:i2)'; 88 | 89 | n = typecast(facet(1:12),'single'); 90 | v1 = typecast(facet(13:24),'single'); 91 | v2 = typecast(facet(25:36),'single'); 92 | v3 = typecast(facet(37:48),'single'); 93 | 94 | n = double(n); 95 | v = double([v1; v2; v3]); 96 | 97 | % Figure out where to fit these new vertices, and the face, in the 98 | % larger F and V collections. 99 | fInd = numRead + 1; 100 | vInd1 = 3 * (fInd - 1) + 1; 101 | vInd2 = vInd1 + 3 - 1; 102 | 103 | V(vInd1:vInd2,:) = v; 104 | F(fInd,:) = vInd1:vInd2; 105 | N(fInd,:) = n; 106 | 107 | numRead = numRead + 1; 108 | end 109 | 110 | end 111 | 112 | 113 | function [F,V,N] = stlascii(M) 114 | warning('MATLAB:stlread:ascii','ASCII STL files currently not supported.'); 115 | F = []; 116 | V = []; 117 | N = []; 118 | end 119 | 120 | % TODO: Change the testing criteria! Some binary STL files still begin with 121 | % 'solid'. 122 | function tf = isbinary(A) 123 | % ISBINARY uses the first line of an STL file to identify its format. 124 | if isempty(A) || length(A) < 5 125 | error('MATLAB:stlread:incorrectFormat', ... 126 | 'File does not appear to be an ASCII or binary STL file.'); 127 | end 128 | if strcmpi('solid',char(A(1:5)')) 129 | tf = false; % ASCII 130 | else 131 | tf = true; % Binary 132 | end 133 | end 134 | 135 | 136 | -------------------------------------------------------------------------------- /smoothener.m: -------------------------------------------------------------------------------- 1 | % main function of Smoothener package. 2 | % given a discrete multi-robot motion plan as a synchronized waypoint list, 3 | % and a description of obstacles in the environment [1], 4 | % computes smooth piecewise polynomial trajectories for each robot 5 | % that locally minimize a weighted integral-squared-derivative cost 6 | % while ensuring robot/robot and robot/obstacle collision avoidance. 7 | % 8 | % it is an implementation of the continuous portion of: 9 | % Downwash-Aware Trajectory Planning for Large Quadcopter Teams 10 | % James A. Preiss, Wolfgang H\"onig, Nora Ayanian, and Gaurav S. Sukhatme 11 | % to appear in IEEE IROS 2017, preprint at arxiv.org/abs/1704.04852. 12 | % Octomap extension currently under review. 13 | % 14 | % [1] "list of boxes" and "octomap" are currently supported obstacle models. 15 | % Adding another obstacle model requires implementing a single function 16 | % with the pp_obs_sep_fun signature described below. 17 | % 18 | % 19 | % inputs: 20 | % paths: [dim k N] discrete planner waypoints 21 | % bbox: [3 2] bounding box around environment 22 | % deg: [1] polynomial degree 23 | % cont: [1] derivative continuity (e.g. 2 == cts accel) 24 | % timescale: [1] duration in seconds of step in discrete plan 25 | % ellipsoid: [3 1] radii of robot/robot collision ellipsoid 26 | % obs_ellipsoid: [3 1] radii of robot/obstacle collision ellipsoid 27 | % iters: [1] number of iterations of refinement (>= 1) 28 | % pp_obs_sep_fun: function handle with signature described below, 29 | % that computes robot/obstacle separating hyperplanes 30 | % 31 | % function polytopes = pp_obs_sep_fun(pps, obs_ellipsoid) 32 | % pps: {N} cell array of matlab ppform structs 33 | % obs_ellipsoid: as above 34 | % polytopes: {N k-1} cell array of halfspaces 35 | % such that p = polytopes{i} is a [4 nfaces] array 36 | % describing the linear system p(:,1:3)x <= p(:,4) 37 | % 38 | % output: cell array of [iters, N] ppform piecewise polynomials 39 | % array of [iters, N] costs 40 | % 41 | function [all_pps, all_costs, all_corridors] = smoothener(... 42 | paths, bbox, ... 43 | deg, cont, timescale, ... 44 | ellipsoid, obs_ellipsoid, ... 45 | iters, ... 46 | pp_obs_sep_fun, ... 47 | corridor_fun) 48 | 49 | [dim, k, N] = size(paths); 50 | 51 | lb = bbox(:,1) + obs_ellipsoid(:); 52 | ub = bbox(:,2) - obs_ellipsoid(:); 53 | 54 | % for a reasonable problem, cost should converge after ~5 iterations. 55 | assert(iters >= 1); 56 | assert(iters <= 20); 57 | 58 | % outputs 59 | all_pps = cell(iters, N); 60 | all_costs = zeros(iters, N); 61 | all_corridors = cell(iters, N); 62 | 63 | % piecewise linear (physically impossible) pps of path 64 | % for input to pp-vs-octree obstacle hyperplane function 65 | pps = path_linear_pps(paths, timescale, deg + 1); 66 | 67 | for iter=1:iters 68 | fprintf('iteration %d of %d...\n', iter, iters); 69 | tic; 70 | if iter==1 71 | % first iteration: decompose by segments 72 | [A, b] = all_hyperplanes_waypoints_mex(paths, ellipsoid); 73 | else 74 | % continuing iteration: decompose by pps 75 | [A, b] = all_hyperplanes_pps(pps, ellipsoid); 76 | end 77 | 78 | if iter > 1 79 | for irobot=1:N 80 | paths(:,:,irobot) = ppval(pps{irobot}, pps{irobot}.breaks); 81 | end 82 | end 83 | 84 | hs = pp_obs_sep_fun(pps, obs_ellipsoid); 85 | 86 | t_hyperplanes = toc; 87 | fprintf('hyperplanes: %f sec\n', t_hyperplanes); 88 | 89 | % solve the independent spline trajectory optimization problems. 90 | tic; 91 | pps = cell(1,N); 92 | iter_costs = zeros(1,N); 93 | % parfor 94 | parfor j=1:N 95 | hs_slice = squeeze(hs(j,:)); 96 | step_n_faces = cellfun(@(a) size(a, 1), hs_slice); 97 | assert(length(step_n_faces) == (k-1)); 98 | max_n_faces = max(step_n_faces(:)); 99 | 100 | Aobs = nan(dim, max_n_faces, k-1); 101 | bobs = nan(max_n_faces, k-1); 102 | Arobots = squeeze(A(:,j,:,:)); 103 | brobots = squeeze(b(j,:,:)); 104 | for i=1:(k-1) 105 | n_faces = step_n_faces(i); 106 | hs_slice_step = hs_slice{i}; 107 | assert(size(hs_slice_step, 2) == 4); 108 | Aobs(:,1:n_faces,i) = hs_slice_step(:,1:3)'; 109 | bobs(1:n_faces,i) = hs_slice_step(:,4); 110 | end 111 | [pps{j}, iter_costs(j)] = corridor_fun(... 112 | Arobots, brobots, ... 113 | Aobs, bobs, ... 114 | lb, ub,... 115 | paths(:,:,j), deg, cont, timescale, ellipsoid, obs_ellipsoid); 116 | 117 | s = []; 118 | s.Arobots = Arobots; 119 | s.brobots = brobots; 120 | s.Aobs = Aobs; 121 | s.bobs = bobs; 122 | all_corridors{iter,j} = s; 123 | end 124 | t_splines = toc; 125 | fprintf('splines: %f sec\n', t_splines); 126 | fprintf('total: %f sec\n', t_hyperplanes + t_splines); 127 | fprintf('cost: %f\n', sum(iter_costs)); 128 | all_costs(iter,:) = iter_costs; 129 | all_pps(iter,:) = pps; 130 | end 131 | end 132 | -------------------------------------------------------------------------------- /svm_sephyp_32point/all_hyperplanes_robot_obstacle_mex.c: -------------------------------------------------------------------------------- 1 | /* 2 | % inputs: paths: [DIM x PPVAL_PTS x NROBOTS] array of points 3 | % obs: [DIM x PPVAL_PTS x NOBS] array of points 4 | % ellipsoid 5 | % 6 | % outputs: A: [DIM x NROBOTS x NOBS] array of 7 | % hyperplane normal vectors for each robot-robot interaction. 8 | % b: [NROBOTS x NOBS] array 9 | % distance from origin for hyperplanes. i.e. a(:,...)^T x <= b. 10 | % 11 | */ 12 | 13 | #include "mex.h" 14 | #include "solver.h" 15 | 16 | #define SQR(x) ((x) * (x)) 17 | #define SET_SZ 32 18 | 19 | Vars vars; 20 | Params params; 21 | Workspace work; 22 | Settings settings; 23 | 24 | void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) { 25 | if (nrhs != 3) { 26 | mexErrMsgTxt("need nrhs == 3\n"); 27 | } 28 | if (nlhs != 2) { 29 | mexErrMsgTxt("need nlhs == 2\n"); 30 | } 31 | mxArray const *pathsArr = prhs[0]; 32 | mxArray const *obsArr = prhs[1]; 33 | mxArray const *ellipsoidArr = prhs[2]; 34 | double const *paths = mxGetPr(pathsArr); 35 | double const *obs = mxGetPr(obsArr); 36 | double const *ellipsoid = mxGetPr(ellipsoidArr); 37 | 38 | // paths: [DIM x PPVAL_PTS x NROBOTS] array of points 39 | mwSize pathsNdim = mxGetNumberOfDimensions(pathsArr); 40 | if (pathsNdim != 3) { 41 | mexErrMsgTxt("paths input must be 3d array"); 42 | } 43 | mwSize const *pathsDim = mxGetDimensions(pathsArr); 44 | if (pathsDim[0] != 3) { 45 | mexErrMsgTxt("first dimension of paths input must be 3"); 46 | } 47 | if (pathsDim[1] != SET_SZ) { 48 | mexErrMsgTxt("second dimension of paths input must be SET_SZ"); 49 | } 50 | mwSize const nRobots = pathsDim[2]; 51 | 52 | mwSize obsNdim = mxGetNumberOfDimensions(obsArr); 53 | if (obsNdim != 3) { 54 | mexErrMsgTxt("obs input must be 3d array"); 55 | } 56 | mwSize const *obsDim = mxGetDimensions(obsArr); 57 | if (obsDim[0] != 3) { 58 | mexErrMsgTxt("first dimension of paths input must be 3"); 59 | } 60 | if (obsDim[1] != SET_SZ) { 61 | mexErrMsgTxt("second dimension of paths input must be SET_SZ"); 62 | } 63 | mwSize const nObs = obsDim[2]; 64 | 65 | // ellipsoid: 1x3 or 3x1 66 | if (mxGetNumberOfElements(ellipsoidArr) != 3) { 67 | mexErrMsgTxt("ellipsoid input must 3x1 or 1x3"); 68 | } 69 | 70 | // A: [DIM x NROBOTS x NROBOTS] array of 71 | // b: distance from origin for hyperplanes. i.e. a(:,...)^T x <= b. 72 | mwSize Adims[3] = {3, nRobots, nObs}; 73 | mwSize bdims[2] = {nRobots, nObs}; 74 | plhs[0] = mxCreateNumericArray(3, Adims, mxDOUBLE_CLASS, 0); 75 | plhs[1] = mxCreateNumericArray(2, bdims, mxDOUBLE_CLASS, 0); 76 | 77 | double *A = mxGetPr(plhs[0]); 78 | double *b = mxGetPr(plhs[1]); 79 | 80 | set_defaults(); 81 | setup_indexing(); 82 | 83 | params.Q[0] = SQR(ellipsoid[0]); 84 | params.Q[1] = SQR(ellipsoid[1]); 85 | params.Q[2] = SQR(ellipsoid[2]); 86 | params.Q[3] = 0.0; 87 | 88 | // paths: [DIM x PPVAL_PTS x NROBOTS] array of points 89 | #define pathind(i1, i2, i3) paths[(i1) + 3*(i2) + SET_SZ*3*(i3)] 90 | // obs: [DIM x PPVAL_PTS x NOBS] array of points 91 | #define obsind(i1, i2, i3) obs[(i1) + 3*(i2) + SET_SZ*3*(i3)] 92 | // A: [DIM x NROBOTS x NOBS] array of ... 93 | #define Aind(i1, i2, i3) A[(i1) + 3*(i2) + 3*nRobots*(i3)] 94 | // b: distance from origin for hyperplanes. i.e. a(:,...)^T x <= b. 95 | #define bind(i1, i2) b[(i1) + nRobots*(i2)] 96 | 97 | #define paramsAind(i1, i2) params.A[(i1) + SET_SZ*2*(i2)] 98 | 99 | for (int i = 0; i < SET_SZ; ++i) { 100 | paramsAind(i, 3) = 1.0; 101 | } 102 | for (int i = SET_SZ; i < 2*SET_SZ; ++i) { 103 | paramsAind(i, 3) = -1.0; 104 | } 105 | 106 | for (int i = 0; i < nRobots; ++i) { 107 | for (int dim = 0; dim < 3; ++dim) { 108 | for (int p = 0; p < SET_SZ; ++p) { 109 | paramsAind(p, dim) = pathind(dim, p, i); 110 | } 111 | } 112 | 113 | for (int j = 0; j < nObs; ++j) { 114 | for (int dim = 0; dim < 3; ++dim) { 115 | for (int p = 0; p < SET_SZ; ++p) { 116 | paramsAind(SET_SZ + p, dim) = -obsind(dim, p, j); 117 | } 118 | } 119 | 120 | set_defaults(); 121 | settings.verbose = 0; 122 | setup_indexing(); 123 | solve(); 124 | 125 | double b0 = vars.beta[0]; 126 | double b1 = vars.beta[1]; 127 | double b2 = vars.beta[2]; 128 | double b3 = vars.beta[3]; 129 | 130 | double invNorm = 1.0 / sqrt(SQR(b0) + SQR(b1) + SQR(b2)); 131 | double a0 = invNorm * b0; 132 | double a1 = invNorm * b1; 133 | double a2 = invNorm * b2; 134 | double bb = -invNorm * b3; 135 | 136 | // make it a supporting hyperplane 137 | double minDist = 1000000000; 138 | for (int boxVtx = 0; boxVtx < 8; ++boxVtx) { 139 | double mul = a0 * obsind(0, boxVtx, j) 140 | + a1 * obsind(1, boxVtx, j) 141 | + a2 * obsind(2, boxVtx, j); 142 | minDist = fmin(minDist, mul - bb); 143 | } 144 | bb = bb + minDist; 145 | 146 | // A: [DIM x NROBOTS x NOBS] array of ... 147 | Aind(0, i, j) = a0; 148 | Aind(1, i, j) = a1; 149 | Aind(2, i, j) = a2; 150 | bind(i, j) = bb; 151 | } 152 | } 153 | } 154 | -------------------------------------------------------------------------------- /external/jsonlab/examples/jsonlab_selftest.matlab: -------------------------------------------------------------------------------- 1 | 2 | < M A T L A B (R) > 3 | Copyright 1984-2010 The MathWorks, Inc. 4 | Version 7.11.0.584 (R2010b) 64-bit (glnxa64) 5 | August 16, 2010 6 | 7 | 8 | To get started, type one of these: helpwin, helpdesk, or demo. 9 | For product information, visit www.mathworks.com. 10 | 11 | >> >> >> >> >> =============================================== 12 | >> example1.json 13 | { 14 | "data": { 15 | "firstName": "John", 16 | "lastName": "Smith", 17 | "age": 25, 18 | "address": { 19 | "streetAddress": "21 2nd Street", 20 | "city": "New York", 21 | "state": "NY", 22 | "postalCode": "10021" 23 | }, 24 | "phoneNumber": [ 25 | { 26 | "type": "home", 27 | "number": "212 555-1234" 28 | }, 29 | { 30 | "type": "fax", 31 | "number": "646 555-4567" 32 | } 33 | ] 34 | } 35 | } 36 | 37 | {"data": {"firstName": "John","lastName": "Smith","age": 25,"address": {"streetAddress": "21 2nd Street","city": "New York","state": "NY","postalCode": "10021"},"phoneNumber": [{"type": "home","number": "212 555-1234"},{"type": "fax","number": "646 555-4567"}]}} 38 | 39 | =============================================== 40 | >> example2.json 41 | { 42 | "data": { 43 | "glossary": { 44 | "title": "example glossary", 45 | "GlossDiv": { 46 | "title": "S", 47 | "GlossList": { 48 | "GlossEntry": { 49 | "ID": "SGML", 50 | "SortAs": "SGML", 51 | "GlossTerm": "Standard Generalized Markup Language", 52 | "Acronym": "SGML", 53 | "Abbrev": "ISO 8879:1986", 54 | "GlossDef": { 55 | "para": "A meta-markup language, used to create markup languages such as DocBook.", 56 | "GlossSeeAlso": [ 57 | "GML", 58 | "XML" 59 | ] 60 | }, 61 | "GlossSee": "markup" 62 | } 63 | } 64 | } 65 | } 66 | } 67 | } 68 | 69 | {"data": {"glossary": {"title": "example glossary","GlossDiv": {"title": "S","GlossList": {"GlossEntry": {"ID": "SGML","SortAs": "SGML","GlossTerm": "Standard Generalized Markup Language","Acronym": "SGML","Abbrev": "ISO 8879:1986","GlossDef": {"para": "A meta-markup language, used to create markup languages such as DocBook.","GlossSeeAlso": ["GML","XML"]},"GlossSee": "markup"}}}}}} 70 | 71 | =============================================== 72 | >> example3.json 73 | { 74 | "data": { 75 | "menu": { 76 | "id": "file", 77 | "value": "_&File", 78 | "popup": { 79 | "menuitem": [ 80 | { 81 | "value": "_&New", 82 | "onclick": "CreateNewDoc(\"'\\\"Untitled\\\"'\")" 83 | }, 84 | { 85 | "value": "_&Open", 86 | "onclick": "OpenDoc()" 87 | }, 88 | { 89 | "value": "_&Close", 90 | "onclick": "CloseDoc()" 91 | } 92 | ] 93 | } 94 | } 95 | } 96 | } 97 | 98 | {"data": {"menu": {"id": "file","value": "_&File","popup": {"menuitem": [{"value": "_&New","onclick": "CreateNewDoc(\"'\\\"Untitled\\\"'\")"},{"value": "_&Open","onclick": "OpenDoc()"},{"value": "_&Close","onclick": "CloseDoc()"}]}}}} 99 | 100 | =============================================== 101 | >> example4.json 102 | { 103 | "data": [ 104 | { 105 | "sample": { 106 | "rho": 1 107 | } 108 | }, 109 | { 110 | "sample": { 111 | "rho": 2 112 | } 113 | }, 114 | [ 115 | [1,0], 116 | [1,1], 117 | [1,2] 118 | ], 119 | [ 120 | "Paper", 121 | "Scissors", 122 | "Stone" 123 | ], 124 | [ 125 | "a", 126 | "b\\", 127 | "c\"", 128 | "d\\\"", 129 | "e\"[", 130 | "f\\\"[", 131 | "g[\\", 132 | "h[\\\"" 133 | ] 134 | ] 135 | } 136 | 137 | {"data": [{"sample": {"rho": 1}},{"sample": {"rho": 2}},[[1,0],[1,1],[1,2]],["Paper","Scissors","Stone"],["a","b\\","c\"","d\\\"","e\"[","f\\\"[","g[\\","h[\\\""]]} 138 | 139 | >> >> =============================================== 140 | >> example1.json 141 | {Udata{U firstNameSUJohnUlastNameSUSmithUageiUaddress{U streetAddressSU 21 2nd StreetUcitySUNew YorkUstateSUNYU 142 | postalCodeSU10021}U phoneNumber[{UtypeSUhomeUnumberSU 212 555-1234}{UtypeSUfaxUnumberSU 646 555-4567}]}} 143 | =============================================== 144 | >> example2.json 145 | {Udata{Uglossary{UtitleSUexample glossaryUGlossDiv{UtitleCSU GlossList{U 146 | GlossEntry{UIDSUSGMLUSortAsSUSGMLU GlossTermSU$Standard Generalized Markup LanguageUAcronymSUSGMLUAbbrevSU ISO 8879:1986UGlossDef{UparaSUHA meta-markup language, used to create markup languages such as DocBook.U GlossSeeAlso[SUGMLSUXML]}UGlossSeeSUmarkup}}}}}} 147 | =============================================== 148 | >> example3.json 149 | {Udata{Umenu{UidSUfileUvalueSU_&FileUpopup{Umenuitem[{UvalueSU_&NewUonclickSUCreateNewDoc("'\"Untitled\"'")}{UvalueSU_&OpenUonclickSU OpenDoc()}{UvalueSU_&CloseUonclickSU 150 | CloseDoc()}]}}}} 151 | =============================================== 152 | >> example4.json 153 | {Udata[{Usample{Urhoi}}{Usample{Urhoi}}[[$i#U[$i#U[$i#U][SUPaperSUScissorsSUStone][CaSUb\SUc"SUd\"SUe"[SUf\"[SUg[\SUh[\"]]} 154 | >> -------------------------------------------------------------------------------- /external/jsonlab/ChangeLog.txt: -------------------------------------------------------------------------------- 1 | ============================================================================ 2 | 3 | JSONlab - a toolbox to encode/decode JSON/UBJSON files in MATLAB/Octave 4 | 5 | ---------------------------------------------------------------------------- 6 | 7 | JSONlab ChangeLog (key features marked by *): 8 | 9 | == JSONlab 1.2 (codename: Optimus - Update 2), FangQ == 10 | 11 | 2015/12/16 replacing string concatenation by str cells to gain 2x speed in savejson (Issue#17) 12 | 2015/12/11 fix FileName option case bug (SVN rev#495) 13 | 2015/12/11 add SingletCell option, add SingletArray to replace NoRowBracket (Issue#15,#8) 14 | 2015/11/10 fix bug for inerpreting file names as JSON string - by Mykhailo Bratukha (Pull#14) 15 | 2015/10/16 fix bug for cell with transposed data - by Insik Kim (Pull#12) 16 | 2015/09/25 support exporting matlab object to JSON - by Sertan Senturk (Pull#10, #11) 17 | 18 | == JSONlab 1.1 (codename: Optimus - Update 1), FangQ == 19 | 20 | 2015/05/05 *massively accelerating loadjson for parsing large collection of unstructured small objects 21 | 2015/05/05 force array bracket in 1x1 struct to maintain depth (Issue#1) 22 | 2015/05/05 parse logicals in loadjson 23 | 2015/05/05 make options case insensitive 24 | 2015/05/01 reading unicode encoded json files (thanks to Sertan Senturk,Issue#3) 25 | 2015/04/30 allow \uXXXX to represent a unicode in a string (Issue#2) 26 | 2015/03/30 save a 0x0 solid real empty array as null and handel empty struct array 27 | 2015/03/30 properly handle escape characters in a string 28 | 2015/01/24 *implement the UBJSON Draft12 new name format 29 | 2015/01/13 correct cell array indentation inconsistency 30 | 31 | == JSONlab 1.0 (codename: Optimus - Final), FangQ == 32 | 33 | 2015/01/02 polish help info for all major functions, update examples, finalize 1.0 34 | 2014/12/19 fix a bug to strictly respect NoRowBracket in savejson 35 | 36 | == JSONlab 1.0.0-RC2 (codename: Optimus - RC2), FangQ == 37 | 38 | 2014/11/22 show progress bar in loadjson ('ShowProgress') 39 | 2014/11/17 *add Compact option in savejson to output compact JSON format ('Compact') 40 | 2014/11/17 add FastArrayParser in loadjson to specify fast parser applicable levels 41 | 2014/09/18 *start official github mirror: https://github.com/fangq/jsonlab 42 | 43 | == JSONlab 1.0.0-RC1 (codename: Optimus - RC1), FangQ == 44 | 45 | 2014/09/17 fix several compatibility issues when running on octave versions 3.2-3.8 46 | 2014/09/17 *support 2D cell and struct arrays in both savejson and saveubjson 47 | 2014/08/04 escape special characters in a JSON string 48 | 2014/02/16 fix a bug when saving ubjson files 49 | 50 | == JSONlab 0.9.9 (codename: Optimus - beta), FangQ == 51 | 52 | 2014/01/22 use binary read and write in saveubjson and loadubjson 53 | 54 | == JSONlab 0.9.8-1 (codename: Optimus - alpha update 1), FangQ == 55 | 56 | 2013/10/07 better round-trip conservation for empty arrays and structs (patch submitted by Yul Kang) 57 | 58 | == JSONlab 0.9.8 (codename: Optimus - alpha), FangQ == 59 | 2013/08/23 *universal Binary JSON (UBJSON) support, including both saveubjson and loadubjson 60 | 61 | == JSONlab 0.9.1 (codename: Rodimus, update 1), FangQ == 62 | 2012/12/18 *handling of various empty and sparse matrices (fixes submitted by Niclas Borlin) 63 | 64 | == JSONlab 0.9.0 (codename: Rodimus), FangQ == 65 | 66 | 2012/06/17 *new format for an invalid leading char, unpacking hex code in savejson 67 | 2012/06/01 support JSONP in savejson 68 | 2012/05/25 fix the empty cell bug (reported by Cyril Davin) 69 | 2012/04/05 savejson can save to a file (suggested by Patrick Rapin) 70 | 71 | == JSONlab 0.8.1 (codename: Sentiel, Update 1), FangQ == 72 | 73 | 2012/02/28 loadjson quotation mark escape bug, see http://bit.ly/yyk1nS 74 | 2012/01/25 patch to handle root-less objects, contributed by Blake Johnson 75 | 76 | == JSONlab 0.8.0 (codename: Sentiel), FangQ == 77 | 78 | 2012/01/13 *speed up loadjson by 20 fold when parsing large data arrays in matlab 79 | 2012/01/11 remove row bracket if an array has 1 element, suggested by Mykel Kochenderfer 80 | 2011/12/22 *accept sequence of 'param',value input in savejson and loadjson 81 | 2011/11/18 fix struct array bug reported by Mykel Kochenderfer 82 | 83 | == JSONlab 0.5.1 (codename: Nexus Update 1), FangQ == 84 | 85 | 2011/10/21 fix a bug in loadjson, previous code does not use any of the acceleration 86 | 2011/10/20 loadjson supports JSON collections - concatenated JSON objects 87 | 88 | == JSONlab 0.5.0 (codename: Nexus), FangQ == 89 | 90 | 2011/10/16 package and release jsonlab 0.5.0 91 | 2011/10/15 *add json demo and regression test, support cpx numbers, fix double quote bug 92 | 2011/10/11 *speed up readjson dramatically, interpret _Array* tags, show data in root level 93 | 2011/10/10 create jsonlab project, start jsonlab website, add online documentation 94 | 2011/10/07 *speed up savejson by 25x using sprintf instead of mat2str, add options support 95 | 2011/10/06 *savejson works for structs, cells and arrays 96 | 2011/09/09 derive loadjson from JSON parser from MATLAB Central, draft savejson.m 97 | -------------------------------------------------------------------------------- /corridor_trajectory_optimize_sequence.m: -------------------------------------------------------------------------------- 1 | % solves the corridor-constrained trajectory optimization problem for one robot. 2 | % uses "sequence of points" parameterization instead of Bezier polynomial basis. 3 | % if the discrete plan is high-resolution (meaning there is a very small 4 | % amount of space and/or time per step) then the high-order Bezier curves 5 | % are more fine-grained control than we actually need. But the Bezier formulation 6 | % requires one polynomial piece per polytope no matter what. 7 | % By switching to a sequence-of-points representation, we lose the exact 8 | % collision avoidance guarantee of the Bezier convex hull property, 9 | % but we can reduce the number of variables a lot, 10 | % making optimization much faster. 11 | 12 | % inputs: 13 | % Arobots, brobots: [DIM x NROBOTS x NSTEPS] and [NROBOTS x NSTEPS] 14 | % hyperplanes separating this robot from the other robots at each step 15 | % Aobs, bobs: [DIM x x NSTEPS] and [ x NSTEPS] 16 | % hyperplanes separating this robot from obstacles at each step. 17 | % second dimension is large enough for the polytope with the most faces, 18 | % so some rows are allowed to be NaN for polytopes with fewer faces 19 | % lb, ub: [3] and [3] lower/upper bound of environment box 20 | % path: [3 NSTEPS+1] the discrete plan 21 | % npts: [1] number of points to use per polytope 22 | % cont: [1] derivative continuity (here, indicates number of 0 derivatives at start) 23 | % timescale: [1] duration in seconds of step in discrete plan 24 | % ellipsoid: [3 1] radii of robot/robot collision ellipsoid 25 | % obs_ellipsoid: [3 1] radii of robot/obstacle collision ellipsoid 26 | % 27 | % outputs: 28 | % pp: a matlab ppform struct containing the trajectory 29 | % cost: the cost value of the quadratic program 30 | % 31 | function [pp, cost] = corridor_trajectory_optimize(... 32 | Arobots, brobots, Aobs, bobs, lb, ub, ... 33 | path, npts, cont, timescale, ellipsoid, obs_ellipsoid) 34 | 35 | [dim, ~, steps] = size(Arobots); 36 | assert(size(path, 2) == steps + 1); 37 | init = path(:,1); 38 | goal = path(:,end); 39 | 40 | ellipsoid = diag(ellipsoid); 41 | obs_ellipsoid = diag(obs_ellipsoid); 42 | 43 | % TODO move this outside 44 | me = find(isnan(brobots(:,1))); 45 | assert(length(me) == 1); 46 | brobots(me,:) = []; 47 | Arobots(:,me,:) = []; 48 | 49 | % number of decision variables 50 | nvars = dim * npts * steps; 51 | lb = repmat(lb, steps * npts, 1); 52 | ub = repmat(ub, steps * npts, 1); 53 | 54 | Aineq = {}; 55 | bineq = []; 56 | Aeq = {}; 57 | beq = []; 58 | 59 | dt = timescale / npts; 60 | diff1_mtx = diff_matrix(dim, steps * npts, dt, 1); 61 | diff2_mtx = diff_matrix(dim, steps * npts, dt, 2); 62 | diff3_mtx = diff_matrix(dim, steps * npts, dt, 3); 63 | diff4_mtx = diff_matrix(dim, steps * npts, dt, 4); 64 | diff_mtxs = {diff1_mtx diff2_mtx diff3_mtx diff4_mtx}; 65 | 66 | for step=1:steps 67 | dim_select = 1:steps == step; 68 | 69 | % offset the corridor bounding polyhedra by the ellipsoid 70 | Astep = [Arobots(:,:,step)'; Aobs(:,:,step)']; 71 | bstep = [polytope_erode_by_ellipsoid(Arobots(:,:,step)', brobots(:,step), ellipsoid); ... 72 | polytope_erode_by_ellipsoid(Aobs(:,:,step)', bobs(:,step), obs_ellipsoid)]; 73 | 74 | % delete NaN inputs coming from "ragged" Aobs, bobs 75 | nan_rows = isnan(bstep); 76 | Astep(nan_rows,:) = []; 77 | bstep(nan_rows) = []; 78 | 79 | % try to eliminate redundant half-space constraints 80 | interior_pt = (path(:,step) + path(:,step+1)) ./ 2; 81 | [Astep,bstep] = noredund(Astep,bstep,interior_pt); 82 | 83 | % add bounding polyhedron constraints on trajectory points 84 | Aineq = [Aineq; kron(dim_select, kron(eye(npts), Astep))]; 85 | bineq = [bineq; repmat(bstep, npts, 1)]; 86 | 87 | if step == 1 88 | % initial position and 0 derivatives 89 | Aeq = [Aeq; kron(dim_select, kron(onehot(1, npts), eye(dim)))]; 90 | beq = [beq; init]; 91 | 92 | for d=1:cont 93 | D = diff_mtxs{d}; 94 | Aeq = [Aeq; D(1:dim,:)]; 95 | beq = [beq; zeros(dim,1)]; 96 | end 97 | end 98 | 99 | if step == steps 100 | % final position and 0 derivatives 101 | Aeq = [Aeq; kron(dim_select, kron(onehot(npts, npts), eye(dim)))]; 102 | beq = [beq; goal]; 103 | 104 | for d=1:cont 105 | D = diff_mtxs{d}; 106 | Aeq = [Aeq; D((end-dim+1):end,:)]; 107 | beq = [beq; zeros(dim,1)]; 108 | end 109 | end 110 | end 111 | 112 | Aineq = cat(1, Aineq{:}); 113 | Aeq = cat(1, Aeq{:}); 114 | assert(size(Aineq, 2) == nvars); 115 | assert(size(Aineq, 1) == length(bineq)); 116 | assert(size(Aeq, 2) == nvars); 117 | assert(size(Aeq, 1) == length(beq)); 118 | 119 | Q = ... 120 | 1 * (diff2_mtx' * dt * diff2_mtx) + ... 121 | 5e-3 * (diff4_mtx' * dt * diff4_mtx); 122 | 123 | options = optimoptions('quadprog', 'Display', 'off'); 124 | 125 | if exist('cplexqp') 126 | [x, cost] = cplexqp(Q, zeros(1,nvars), Aineq, bineq, Aeq, beq, lb, ub); 127 | else 128 | [x, cost] = quadprog(sparse(Q), zeros(1,nvars), ... 129 | sparse(Aineq), sparse(bineq), sparse(Aeq), sparse(beq), ... 130 | lb, ub, [], options); 131 | end 132 | 133 | x = reshape(x, [dim, npts * steps]); 134 | t = 0:(npts*steps-1); 135 | t = ((steps * timescale) / (length(t) - 1)) .* t; 136 | 137 | knots = linspace(t(1), t(end), steps + 1); 138 | augknots = augknt(knots, cont); 139 | bspline = spap2(augknots, cont, t, x); 140 | pp = fn2fm(bspline, 'pp'); 141 | end 142 | 143 | function x = onehot(i, n) 144 | x = zeros(1, n); 145 | x(i) = 1; 146 | end 147 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # smoothener 2 | Convert multi-robot waypoint sequences into smooth piecewise polynomial trajectories. 3 | 4 | This repository contains a Matlab implementation of the continuous trajectory optimization stage 5 | of the algorithm described in: 6 | 7 | > *Downwash-Aware Trajectory Planning for Large Quadcopter Teams* 8 | > 9 | > James A. Preiss, Wolfgang Hönig, Nora Ayanian, Gaurav S. Sukhatme 10 | > 11 | > Accepted at IEEE IROS 2017, 12 | > Preprint at [https://arxiv.org/abs/1704.04852](https://arxiv.org/abs/1704.04852) 13 | 14 | 15 | ## overview 16 | 17 | The purpose of this program is to convert a waypoint sequence for multiple robots 18 | into a set of smooth trajectories. 19 | It is assumed that the waypoint sequence comes from a planner (typically graph-based) 20 | that models robots moving on straight lines between waypoints. 21 | It is impossible to execute such a plan in real life with nonzero velocity at the waypoints, 22 | because it would require infinite acceleration. 23 | Therefore, we want to "smoothen" the trajectories while maintaining 24 | the collision avoidance guarantee. 25 | 26 | We model robots as axis-aligned ellipsoids to deal with the downwash effect of quadrotors. 27 | The output format is piecewise polynomial trajectories with user-specified smoothness and degree. 28 | Output is given as Matlab's [ppform](https://www.mathworks.com/help/curvefit/the-ppform.html) structs. 29 | 30 | 31 | ## setup instructions 32 | 1. Make sure your Matlab MEX compiler is set up correctly, and uses at least -O2 optimization. 33 | 2. Run `make`. 34 | 3. From the `smoothener` root directory , open a Matlab session and run `main_grid`. 35 | Computation should take several seconds, and you should see a 3D plot when it is done. 36 | 37 | ### extra setup to use Octomap: 38 | 4. run `git submodule init && git submodule update`. 39 | 5. `cd` into `octomap_corridor/octomap` and follow the "to only compile the library" instructions in [Octomap's `README.md`](https://github.com/OctoMap/octomap). 40 | 6. `cd` back into the `smoothener` root directory and run `make octomap`. 41 | 7. From the `smoothener` root directory , open a Matlab session and run `main_octomap`. 42 | Computation should take several seconds, and you should see a 3D plot when it is done. 43 | 44 | 45 | ## environment obstacles 46 | 47 | Currently supported environment models are: 48 | 49 | * [Octomap](https://octomap.github.io/)s, given as binary files on the disk 50 | * sets of axis-aligned boxes 51 | 52 | This part of the code is modularized, making it easy to add support for other environment representations. 53 | 54 | 55 | ## project structure 56 | 57 | item | description 58 | ---- | ----------- 59 | `` | main routines, obstacle model implementations, and makefile. The main entry point is `smoothener.m`. 60 | `examples/` | example problems for both "octomap" and "list of boxes" environment models, and an obstacle-free problem. 61 | `external/` | third-party code from the Matlab file exchange. 62 | `octomap_corridor/` | a standalone program that computes the safe corridor for a single robot in an octomap. A separate process is used instead of a mex-function because the auto-generated SVM solver C code from CVXGEN uses global variables, making multithreading impossible. 63 | `svm_sephyp_*point/` | generated C code from the CVXGEN package to solve ellipsoid-weighted hard margin SVM problems for separating hyperplanes in 3D. Used for both robot-robot and robot-obstacle separation. Also contains mex-functions implementing the outer loop for all robots. 64 | `tests/` | very few unit tests, need to write some more... 65 | `utils/` | low-level simple subroutines. 66 | 67 | 68 | ## project status 69 | 70 | ### short-horizon TODOs: 71 | - Support 2D problems. Most "difficult" parts of the code are dimension-generic, but some places assume 3D, 72 | particularly the CVXGEN generated code for separating hyperplane problems. 73 | In the meantime, use a discrete plan with all zeroes for the z-axis, 74 | and set the z bounding box to something like +/- 0.001. 75 | - Support varying time allocation to each step in the discrete plan (each step is same duration for all robots, but not all steps in sequence need to be same duration) 76 | - Fix the hard-coded ellipsoid dimensions in `octomap_corridor.cpp`. 77 | - Write more unit tests. 78 | 79 | ### long-horizon TODOs: 80 | - Port to Python, Julia, ... (**outside contributors welcomed!**) 81 | - Port to C++ 82 | 83 | ## implementation notes 84 | 85 | The implementation uses generated code from 86 | [CVXGEN](https://cvxgen.com/docs/index.html) to solve many small separating-hyperplane problems. 87 | This is necessary to avoid excessive overhead of interpreting Matlab code. 88 | The makefile compiles all C/C++ code. 89 | 90 | The implementation has been profiled and optimized. 91 | There is no known low-hanging fruit. 92 | For medium-sized problems (20-80 robots), the bottleneck is solving the large quadratic programs 93 | for each robot's corridor-constrained trajectory optimization problem. 94 | If [CPLEX](https://www-01.ibm.com/software/commerce/optimization/cplex-optimizer/) is installed, 95 | it will be used to solve these QPs. 96 | 97 | Wherever possible, computational bottlenecks are parallelized. 98 | Multiple CPU cores up to the number of robots will show a large benefit. 99 | 100 | 101 | ## acknowledgements 102 | 103 | This project is developed under the supervision of [Gaurav S. Sukhatme](http://www-robotics.usc.edu/~gaurav/) 104 | as a part of the [Robotic Embedded Systems Lab (RESL)](http://robotics.usc.edu/resl/). 105 | 106 | This work originated as a project in [Nora Ayanian](http://www-bcf.usc.edu/~ayanian/)'s course *Coordinated Mobile Robotics* at USC in Fall 2016. 107 | 108 | Ongoing development has been a collaboration with [Wolfgang Hönig](https://github.com/whoenig) who provided the discrete graph planning front end and contributed to debugging and experiments. 109 | 110 | The method builds upon [Sarah Tang](http://www.seas.upenn.edu/~sytang/)'s IROS 2016 [paper](http://www.seas.upenn.edu/~sytang/docs/2016IROS.pdf). 111 | 112 | The project name is from [Anna Lukina](http://logic-cs.at/phd/students/anna-lukina/). 113 | -------------------------------------------------------------------------------- /corridor_trajectory_optimize.m: -------------------------------------------------------------------------------- 1 | % solves the corridor-constrained piecewise polynomial trajectory optimization problem for one robot. 2 | % 3 | % inputs: 4 | % Arobots, brobots: [DIM x NROBOTS x NSTEPS] and [NROBOTS x NSTEPS] 5 | % hyperplanes separating this robot from the other robots at each step 6 | % Aobs, bobs: [DIM x x NSTEPS] and [ x NSTEPS] 7 | % hyperplanes separating this robot from obstacles at each step. 8 | % second dimension is large enough for the polytope with the most faces, 9 | % so some rows are allowed to be NaN for polytopes with fewer faces 10 | % lb, ub: [3] and [3] lower/upper bound of environment box 11 | % path: [3 NSTEPS+1] the discrete plan 12 | % deg: [1] polynomial degree 13 | % cont: [1] derivative continuity (e.g. 2 == cts accel) 14 | % timescale: [1] duration in seconds of step in discrete plan 15 | % ellipsoid: [3 1] radii of robot/robot collision ellipsoid 16 | % obs_ellipsoid: [3 1] radii of robot/obstacle collision ellipsoid 17 | % 18 | % outputs: 19 | % pp: a matlab ppform struct containing the trajectory 20 | % cost: the cost value of the quadratic program 21 | % 22 | function [pp, cost] = corridor_trajectory_optimize(... 23 | Arobots, brobots, Aobs, bobs, lb, ub, ... 24 | path, deg, cont, timescale, ellipsoid, obs_ellipsoid) 25 | 26 | [dim, ~, steps] = size(Arobots); 27 | assert(size(path, 2) == steps + 1); 28 | init = path(:,1); 29 | goal = path(:,end); 30 | order = deg + 1; 31 | 32 | ellipsoid = diag(ellipsoid); 33 | obs_ellipsoid = diag(obs_ellipsoid); 34 | 35 | % hack - so we can use 7th degree 36 | ends_zeroderivs = min(3,cont); 37 | 38 | % TODO move this outside 39 | me = find(isnan(brobots(:,1))); 40 | assert(length(me) == 1); 41 | brobots(me,:) = []; 42 | Arobots(:,me,:) = []; 43 | 44 | % construct the Bernstein polynomials and derivatives 45 | bern = bernstein(deg); 46 | for i=1:size(bern,2) 47 | bern(i,:) = polystretchtime(bern(i,:), timescale); 48 | end 49 | 50 | bernderivs = repmat(bern, 1, 1, cont + 1); 51 | for d=2:(cont+1) 52 | for r=1:order 53 | p = polyder(bernderivs(r,:,(d-1))); 54 | p = [zeros(1, order - length(p)), p]; 55 | bernderivs(r,:,d) = p; 56 | end 57 | end 58 | bernderivs = bernderivs(:,:,2:end); 59 | assert(size(bernderivs, 3) == cont); 60 | 61 | % number of decision variables 62 | nvars = dim * order * steps; 63 | lb = repmat(lb, steps * order, 1); 64 | ub = repmat(ub, steps * order, 1); 65 | 66 | Aineq = {}; 67 | bineq = []; 68 | Aeq = {}; 69 | beq = []; 70 | 71 | % permutation matrix to convert [xyz xyz xyz]' into [xxx yyy zzz]' 72 | dim_collect_one_step = dim_collect_matrix(dim, order); 73 | 74 | % time vectors at start and end 75 | t0 = 0 .^ (deg:(-1):0); 76 | t1 = timescale .^ (deg:(-1):0); 77 | 78 | for step=1:steps 79 | dim_select = 1:steps == step; 80 | dim_collect = kron(dim_select, dim_collect_one_step); 81 | dim_collect_prev = kron((1:steps == (step-1)), dim_collect_one_step); 82 | 83 | % offset the corridor bounding polyhedra by the ellipsoid 84 | Astep = [Arobots(:,:,step)'; Aobs(:,:,step)']; 85 | bstep = [polytope_erode_by_ellipsoid(Arobots(:,:,step)', brobots(:,step), ellipsoid); ... 86 | polytope_erode_by_ellipsoid(Aobs(:,:,step)', bobs(:,step), obs_ellipsoid)]; 87 | 88 | % delete NaN inputs coming from "ragged" Aobs, bobs 89 | nan_rows = isnan(bstep); 90 | Astep(nan_rows,:) = []; 91 | bstep(nan_rows) = []; 92 | 93 | % try to eliminate redundant half-space constraints 94 | interior_pt = (path(:,step) + path(:,step+1)) ./ 2; 95 | [Astep,bstep] = noredund(Astep,bstep,interior_pt); 96 | 97 | % add bounding polyhedron constraints on control points 98 | Aineq = [Aineq; kron(dim_select, kron(eye(order), Astep))]; 99 | bineq = [bineq; repmat(bstep, order, 1)]; 100 | 101 | if step == 1 102 | % initial position and 0 derivatives 103 | Aeq = [Aeq; kron(eye(dim), t0 * bern') * dim_collect]; 104 | beq = [beq; init]; 105 | for d=1:ends_zeroderivs 106 | Aeq = [Aeq; kron(eye(dim), t0 * bernderivs(:,:,d)') * dim_collect]; 107 | beq = [beq; zeros(dim,1)]; 108 | end 109 | else 110 | % continuity with previous 111 | A_sub = kron(eye(dim), t0 * bern') * dim_collect - ... 112 | kron(eye(dim), t1 * bern') * dim_collect_prev; 113 | Aeq = [Aeq; A_sub]; 114 | beq = [beq; zeros(dim,1)]; 115 | for d=1:cont 116 | A_sub = kron(eye(dim), t0 * bernderivs(:,:,d)') * dim_collect - ... 117 | kron(eye(dim), t1 * bernderivs(:,:,d)') * dim_collect_prev; 118 | Aeq = [Aeq; A_sub]; 119 | beq = [beq; zeros(dim,1)]; 120 | end 121 | end 122 | 123 | if step == steps 124 | % goal position and 0 derivatives 125 | Aeq = [Aeq; kron(eye(dim), t1 * bern') * dim_collect]; 126 | beq = [beq; goal]; 127 | for d=1:ends_zeroderivs 128 | Aeq = [Aeq; kron(eye(dim), t1 * bernderivs(:,:,d)') * dim_collect]; 129 | beq = [beq; zeros(dim,1)]; 130 | end 131 | end 132 | end 133 | 134 | Aineq = cat(1, Aineq{:}); 135 | Aeq = cat(1, Aeq{:}); 136 | assert(size(Aineq, 2) == nvars); 137 | assert(size(Aineq, 1) == length(bineq)); 138 | assert(size(Aeq, 2) == nvars); 139 | assert(size(Aeq, 1) == length(beq)); 140 | 141 | coef_cost = ... 142 | 1 * int_sqr_deriv_matrix(deg, 2, timescale) + ... 143 | 0 * int_sqr_deriv_matrix(deg, 3, timescale) + ... 144 | 5e-3 * int_sqr_deriv_matrix(deg, 4, timescale); 145 | piece_to_coefs = kron(eye(dim), bern') * dim_collect_one_step; 146 | piece_cost = piece_to_coefs' * kron(eye(dim), coef_cost) * piece_to_coefs; 147 | Q = kron(eye(steps), piece_cost); 148 | Q = Q + Q'; % matlab complains, but error is small. TODO track down source. 149 | 150 | options = optimoptions('quadprog', 'Display', 'off'); 151 | options.TolCon = options.TolCon / 10; 152 | 153 | % DEBUGGING INFEASIBLE 154 | %x_ineq = linprog(zeros(1,nvars), Aineq, bineq); 155 | %x_ineq_err = bineq - Aineq*x_ineq; 156 | %x_eq = Aeq\beq; 157 | %x_all = linprog(zeros(1,nvars), Aineq, bineq, Aeq, beq, lb, ub); 158 | 159 | if exist('cplexqp') 160 | [x, cost] = cplexqp(Q, zeros(1,nvars), Aineq, bineq, Aeq, beq, lb, ub); 161 | else 162 | [x, cost] = quadprog(sparse(Q), zeros(1,nvars), ... 163 | sparse(Aineq), sparse(bineq), sparse(Aeq), sparse(beq), ... 164 | lb, ub, [], options); 165 | end 166 | 167 | % x is [dim, ctrlpoint, piece] - want [dim, piece, degree] 168 | x = reshape(x, [dim*order, steps]); 169 | coefs = []; 170 | for piece=1:steps 171 | xx = reshape(dim_collect_one_step * x(:,piece), [order dim]); 172 | piece_coefs = bern' * xx; % [order dim] 173 | coefs = cat(2, coefs, reshape(piece_coefs', [dim 1 order])); 174 | end 175 | 176 | breaks = timescale * (0:steps); 177 | pp = mkpp(breaks, coefs, dim); 178 | end 179 | -------------------------------------------------------------------------------- /svm_sephyp_2point/csolve.c: -------------------------------------------------------------------------------- 1 | /* Produced by CVXGEN, 2016-11-30 17:09:31 -0500. */ 2 | /* CVXGEN is Copyright (C) 2006-2012 Jacob Mattingley, jem@cvxgen.com. */ 3 | /* The code in this file is Copyright (C) 2006-2012 Jacob Mattingley. */ 4 | /* CVXGEN, or solvers produced by CVXGEN, cannot be used for commercial */ 5 | /* applications without prior written permission from Jacob Mattingley. */ 6 | 7 | /* Filename: csolve.c. */ 8 | /* Description: mex-able file for running cvxgen solver. */ 9 | #include "mex.h" 10 | #include "solver.h" 11 | Vars vars; 12 | Params params; 13 | Workspace work; 14 | Settings settings; 15 | void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) { 16 | int i, j; 17 | mxArray *xm, *cell, *xm_cell; 18 | double *src; 19 | double *dest; 20 | double *dest_cell; 21 | int valid_vars; 22 | int steps; 23 | int this_var_errors; 24 | int warned_diags; 25 | int prepare_for_c = 0; 26 | int extra_solves; 27 | const char *status_names[] = {"optval", "gap", "steps", "converged"}; 28 | mwSize dims1x1of1[1] = {1}; 29 | mwSize dims[1]; 30 | const char *var_names[] = {"beta"}; 31 | const int num_var_names = 1; 32 | /* Avoid compiler warnings of unused variables by using a dummy assignment. */ 33 | warned_diags = j = 0; 34 | extra_solves = 0; 35 | set_defaults(); 36 | /* Check we got the right number of arguments. */ 37 | if (nrhs == 0) 38 | mexErrMsgTxt("Not enough arguments: You need to specify at least the parameters.\n"); 39 | if (nrhs > 1) { 40 | /* Assume that the second argument is the settings. */ 41 | if (mxGetField(prhs[1], 0, "eps") != NULL) 42 | settings.eps = *mxGetPr(mxGetField(prhs[1], 0, "eps")); 43 | if (mxGetField(prhs[1], 0, "max_iters") != NULL) 44 | settings.max_iters = *mxGetPr(mxGetField(prhs[1], 0, "max_iters")); 45 | if (mxGetField(prhs[1], 0, "refine_steps") != NULL) 46 | settings.refine_steps = *mxGetPr(mxGetField(prhs[1], 0, "refine_steps")); 47 | if (mxGetField(prhs[1], 0, "verbose") != NULL) 48 | settings.verbose = *mxGetPr(mxGetField(prhs[1], 0, "verbose")); 49 | if (mxGetField(prhs[1], 0, "better_start") != NULL) 50 | settings.better_start = *mxGetPr(mxGetField(prhs[1], 0, "better_start")); 51 | if (mxGetField(prhs[1], 0, "verbose_refinement") != NULL) 52 | settings.verbose_refinement = *mxGetPr(mxGetField(prhs[1], 0, 53 | "verbose_refinement")); 54 | if (mxGetField(prhs[1], 0, "debug") != NULL) 55 | settings.debug = *mxGetPr(mxGetField(prhs[1], 0, "debug")); 56 | if (mxGetField(prhs[1], 0, "kkt_reg") != NULL) 57 | settings.kkt_reg = *mxGetPr(mxGetField(prhs[1], 0, "kkt_reg")); 58 | if (mxGetField(prhs[1], 0, "s_init") != NULL) 59 | settings.s_init = *mxGetPr(mxGetField(prhs[1], 0, "s_init")); 60 | if (mxGetField(prhs[1], 0, "z_init") != NULL) 61 | settings.z_init = *mxGetPr(mxGetField(prhs[1], 0, "z_init")); 62 | if (mxGetField(prhs[1], 0, "resid_tol") != NULL) 63 | settings.resid_tol = *mxGetPr(mxGetField(prhs[1], 0, "resid_tol")); 64 | if (mxGetField(prhs[1], 0, "extra_solves") != NULL) 65 | extra_solves = *mxGetPr(mxGetField(prhs[1], 0, "extra_solves")); 66 | else 67 | extra_solves = 0; 68 | if (mxGetField(prhs[1], 0, "prepare_for_c") != NULL) 69 | prepare_for_c = *mxGetPr(mxGetField(prhs[1], 0, "prepare_for_c")); 70 | } 71 | valid_vars = 0; 72 | this_var_errors = 0; 73 | xm = mxGetField(prhs[0], 0, "A"); 74 | if (xm == NULL) { 75 | printf("could not find params.A.\n"); 76 | } else { 77 | if (!((mxGetM(xm) == 4) && (mxGetN(xm) == 4))) { 78 | printf("A must be size (4,4), not (%d,%d).\n", mxGetM(xm), mxGetN(xm)); 79 | this_var_errors++; 80 | } 81 | if (mxIsComplex(xm)) { 82 | printf("parameter A must be real.\n"); 83 | this_var_errors++; 84 | } 85 | if (!mxIsClass(xm, "double")) { 86 | printf("parameter A must be a full matrix of doubles.\n"); 87 | this_var_errors++; 88 | } 89 | if (mxIsSparse(xm)) { 90 | printf("parameter A must be a full matrix.\n"); 91 | this_var_errors++; 92 | } 93 | if (this_var_errors == 0) { 94 | dest = params.A; 95 | src = mxGetPr(xm); 96 | for (i = 0; i < 16; i++) 97 | *dest++ = *src++; 98 | valid_vars++; 99 | } 100 | } 101 | this_var_errors = 0; 102 | xm = mxGetField(prhs[0], 0, "Q"); 103 | if (xm == NULL) { 104 | printf("could not find params.Q.\n"); 105 | } else { 106 | if (!((mxGetM(xm) == 4) && (mxGetN(xm) == 4))) { 107 | printf("Q must be size (4,4), not (%d,%d).\n", mxGetM(xm), mxGetN(xm)); 108 | this_var_errors++; 109 | } 110 | if (mxIsComplex(xm)) { 111 | printf("parameter Q must be real.\n"); 112 | this_var_errors++; 113 | } 114 | if (!mxIsClass(xm, "double")) { 115 | printf("parameter Q must be a full matrix of doubles.\n"); 116 | this_var_errors++; 117 | } 118 | if (mxIsSparse(xm)) { 119 | printf("parameter Q must be a full matrix.\n"); 120 | this_var_errors++; 121 | } 122 | if (this_var_errors == 0) { 123 | dest = params.Q; 124 | src = mxGetPr(xm); 125 | warned_diags = 0; 126 | for (i = 0; i < 4; i++) { 127 | for (j = 0; j < 4; j++) { 128 | if (i == j) { 129 | *dest++ = *src; 130 | } else if (!warned_diags && (*src != 0)) { 131 | printf("\n!!! Warning: ignoring off-diagonal elements in Q !!!\n\n"); 132 | warned_diags = 1; 133 | } 134 | src++; 135 | } 136 | } 137 | valid_vars++; 138 | } 139 | } 140 | if (valid_vars != 2) { 141 | printf("Error: %d parameters are invalid.\n", 2 - valid_vars); 142 | mexErrMsgTxt("invalid parameters found."); 143 | } 144 | if (prepare_for_c) { 145 | printf("settings.prepare_for_c == 1. thus, outputting for C.\n"); 146 | for (i = 0; i < 4; i++) 147 | printf(" params.Q[%d] = %.6g;\n", i, params.Q[i]); 148 | for (i = 0; i < 16; i++) 149 | printf(" params.A[%d] = %.6g;\n", i, params.A[i]); 150 | } 151 | /* Perform the actual solve in here. */ 152 | steps = solve(); 153 | /* For profiling purposes, allow extra silent solves if desired. */ 154 | settings.verbose = 0; 155 | for (i = 0; i < extra_solves; i++) 156 | solve(); 157 | /* Update the status variables. */ 158 | plhs[1] = mxCreateStructArray(1, dims1x1of1, 4, status_names); 159 | xm = mxCreateDoubleMatrix(1, 1, mxREAL); 160 | mxSetField(plhs[1], 0, "optval", xm); 161 | *mxGetPr(xm) = work.optval; 162 | xm = mxCreateDoubleMatrix(1, 1, mxREAL); 163 | mxSetField(plhs[1], 0, "gap", xm); 164 | *mxGetPr(xm) = work.gap; 165 | xm = mxCreateDoubleMatrix(1, 1, mxREAL); 166 | mxSetField(plhs[1], 0, "steps", xm); 167 | *mxGetPr(xm) = steps; 168 | xm = mxCreateDoubleMatrix(1, 1, mxREAL); 169 | mxSetField(plhs[1], 0, "converged", xm); 170 | *mxGetPr(xm) = work.converged; 171 | /* Extract variable values. */ 172 | plhs[0] = mxCreateStructArray(1, dims1x1of1, num_var_names, var_names); 173 | xm = mxCreateDoubleMatrix(4, 1, mxREAL); 174 | mxSetField(plhs[0], 0, "beta", xm); 175 | dest = mxGetPr(xm); 176 | src = vars.beta; 177 | for (i = 0; i < 4; i++) { 178 | *dest++ = *src++; 179 | } 180 | } 181 | -------------------------------------------------------------------------------- /external/jsonlab/examples/demo_jsonlab_basic.m: -------------------------------------------------------------------------------- 1 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 2 | % Demonstration of Basic Utilities of JSONlab 3 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 4 | 5 | rngstate = rand ('state'); 6 | randseed=hex2dec('623F9A9E'); 7 | clear data2json json2data 8 | 9 | fprintf(1,'\n%%=================================================\n') 10 | fprintf(1,'%% a simple scalar value \n') 11 | fprintf(1,'%%=================================================\n\n') 12 | 13 | data2json=pi 14 | savejson('',data2json) 15 | json2data=loadjson(ans) 16 | 17 | fprintf(1,'\n%%=================================================\n') 18 | fprintf(1,'%% a complex number\n') 19 | fprintf(1,'%%=================================================\n\n') 20 | 21 | clear i; 22 | data2json=1+2*i 23 | savejson('',data2json) 24 | json2data=loadjson(ans) 25 | 26 | fprintf(1,'\n%%=================================================\n') 27 | fprintf(1,'%% a complex matrix\n') 28 | fprintf(1,'%%=================================================\n\n') 29 | 30 | data2json=magic(6); 31 | data2json=data2json(:,1:3)+data2json(:,4:6)*i 32 | savejson('',data2json) 33 | json2data=loadjson(ans) 34 | 35 | fprintf(1,'\n%%=================================================\n') 36 | fprintf(1,'%% MATLAB special constants\n') 37 | fprintf(1,'%%=================================================\n\n') 38 | 39 | data2json=[NaN Inf -Inf] 40 | savejson('specials',data2json) 41 | json2data=loadjson(ans) 42 | 43 | fprintf(1,'\n%%=================================================\n') 44 | fprintf(1,'%% a real sparse matrix\n') 45 | fprintf(1,'%%=================================================\n\n') 46 | 47 | data2json=sprand(10,10,0.1) 48 | savejson('sparse',data2json) 49 | json2data=loadjson(ans) 50 | 51 | fprintf(1,'\n%%=================================================\n') 52 | fprintf(1,'%% a complex sparse matrix\n') 53 | fprintf(1,'%%=================================================\n\n') 54 | 55 | data2json=data2json-data2json*i 56 | savejson('complex_sparse',data2json) 57 | json2data=loadjson(ans) 58 | 59 | fprintf(1,'\n%%=================================================\n') 60 | fprintf(1,'%% an all-zero sparse matrix\n') 61 | fprintf(1,'%%=================================================\n\n') 62 | 63 | data2json=sparse(2,3); 64 | savejson('all_zero_sparse',data2json) 65 | json2data=loadjson(ans) 66 | 67 | fprintf(1,'\n%%=================================================\n') 68 | fprintf(1,'%% an empty sparse matrix\n') 69 | fprintf(1,'%%=================================================\n\n') 70 | 71 | data2json=sparse([]); 72 | savejson('empty_sparse',data2json) 73 | json2data=loadjson(ans) 74 | 75 | fprintf(1,'\n%%=================================================\n') 76 | fprintf(1,'%% an empty 0-by-0 real matrix\n') 77 | fprintf(1,'%%=================================================\n\n') 78 | 79 | data2json=[]; 80 | savejson('empty_0by0_real',data2json) 81 | json2data=loadjson(ans) 82 | 83 | fprintf(1,'\n%%=================================================\n') 84 | fprintf(1,'%% an empty 0-by-3 real matrix\n') 85 | fprintf(1,'%%=================================================\n\n') 86 | 87 | data2json=zeros(0,3); 88 | savejson('empty_0by3_real',data2json) 89 | json2data=loadjson(ans) 90 | 91 | fprintf(1,'\n%%=================================================\n') 92 | fprintf(1,'%% a sparse real column vector\n') 93 | fprintf(1,'%%=================================================\n\n') 94 | 95 | data2json=sparse([0,3,0,1,4]'); 96 | savejson('sparse_column_vector',data2json) 97 | json2data=loadjson(ans) 98 | 99 | fprintf(1,'\n%%=================================================\n') 100 | fprintf(1,'%% a sparse complex column vector\n') 101 | fprintf(1,'%%=================================================\n\n') 102 | 103 | data2json=data2json-1i*data2json; 104 | savejson('complex_sparse_column_vector',data2json) 105 | json2data=loadjson(ans) 106 | 107 | fprintf(1,'\n%%=================================================\n') 108 | fprintf(1,'%% a sparse real row vector\n') 109 | fprintf(1,'%%=================================================\n\n') 110 | 111 | data2json=sparse([0,3,0,1,4]); 112 | savejson('sparse_row_vector',data2json) 113 | json2data=loadjson(ans) 114 | 115 | fprintf(1,'\n%%=================================================\n') 116 | fprintf(1,'%% a sparse complex row vector\n') 117 | fprintf(1,'%%=================================================\n\n') 118 | 119 | data2json=data2json-1i*data2json; 120 | savejson('complex_sparse_row_vector',data2json) 121 | json2data=loadjson(ans) 122 | 123 | fprintf(1,'\n%%=================================================\n') 124 | fprintf(1,'%% a structure\n') 125 | fprintf(1,'%%=================================================\n\n') 126 | 127 | data2json=struct('name','Think Different','year',1997,'magic',magic(3),... 128 | 'misfits',[Inf,NaN],'embedded',struct('left',true,'right',false)) 129 | savejson('astruct',data2json,struct('ParseLogical',1)) 130 | json2data=loadjson(ans) 131 | class(json2data.astruct.embedded.left) 132 | 133 | fprintf(1,'\n%%=================================================\n') 134 | fprintf(1,'%% a structure array\n') 135 | fprintf(1,'%%=================================================\n\n') 136 | 137 | data2json=struct('name','Nexus Prime','rank',9); 138 | data2json(2)=struct('name','Sentinel Prime','rank',9); 139 | data2json(3)=struct('name','Optimus Prime','rank',9); 140 | savejson('Supreme Commander',data2json) 141 | json2data=loadjson(ans) 142 | 143 | fprintf(1,'\n%%=================================================\n') 144 | fprintf(1,'%% a cell array\n') 145 | fprintf(1,'%%=================================================\n\n') 146 | 147 | data2json=cell(3,1); 148 | data2json{1}=struct('buzz',1.1,'rex',1.2,'bo',1.3,'hamm',2.0,'slink',2.1,'potato',2.2,... 149 | 'woody',3.0,'sarge',3.1,'etch',4.0,'lenny',5.0,'squeeze',6.0,'wheezy',7.0); 150 | data2json{2}=struct('Ubuntu',['Kubuntu';'Xubuntu';'Lubuntu']); 151 | data2json{3}=[10.04,10.10,11.04,11.10] 152 | savejson('debian',data2json,struct('FloatFormat','%.2f')) 153 | json2data=loadjson(ans) 154 | 155 | fprintf(1,'\n%%=================================================\n') 156 | fprintf(1,'%% invalid field-name handling\n') 157 | fprintf(1,'%%=================================================\n\n') 158 | 159 | json2data=loadjson('{"ValidName":1, "_InvalidName":2, ":Field:":3, "项目":"绝密"}') 160 | 161 | fprintf(1,'\n%%=================================================\n') 162 | fprintf(1,'%% a 2D cell array\n') 163 | fprintf(1,'%%=================================================\n\n') 164 | 165 | data2json={{1,{2,3}},{4,5},{6};{7},{8,9},{10}}; 166 | savejson('data2json',data2json) 167 | json2data=loadjson(ans) % only savejson works for cell arrays, loadjson has issues 168 | 169 | fprintf(1,'\n%%=================================================\n') 170 | fprintf(1,'%% a 2D struct array\n') 171 | fprintf(1,'%%=================================================\n\n') 172 | 173 | data2json=repmat(struct('idx',0,'data','structs'),[2,3]) 174 | for i=1:6 175 | data2json(i).idx=i; 176 | end 177 | savejson('data2json',data2json) 178 | json2data=loadjson(ans) 179 | 180 | rand ('state',rngstate); 181 | 182 | -------------------------------------------------------------------------------- /svm_sephyp_32point/svm32_mex.c: -------------------------------------------------------------------------------- 1 | /* Produced by CVXGEN, 2016-11-30 16:59:55 -0500. */ 2 | /* CVXGEN is Copyright (C) 2006-2012 Jacob Mattingley, jem@cvxgen.com. */ 3 | /* The code in this file is Copyright (C) 2006-2012 Jacob Mattingley. */ 4 | /* CVXGEN, or solvers produced by CVXGEN, cannot be used for commercial */ 5 | /* applications without prior written permission from Jacob Mattingley. */ 6 | 7 | /* Filename: csolve.c. */ 8 | /* Description: mex-able file for running cvxgen solver. */ 9 | #include "mex.h" 10 | #include "solver.h" 11 | Vars vars; 12 | Params params; 13 | Workspace work; 14 | Settings settings; 15 | void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) { 16 | int i, j; 17 | mxArray *xm, *cell, *xm_cell; 18 | double *src; 19 | double *dest; 20 | double *dest_cell; 21 | int valid_vars; 22 | int steps; 23 | int this_var_errors; 24 | int warned_diags; 25 | int prepare_for_c = 0; 26 | int extra_solves; 27 | const char *status_names[] = {"optval", "gap", "steps", "converged"}; 28 | mwSize dims1x1of1[1] = {1}; 29 | mwSize dims[1]; 30 | const char *var_names[] = {"beta"}; 31 | const int num_var_names = 1; 32 | /* Avoid compiler warnings of unused variables by using a dummy assignment. */ 33 | warned_diags = j = 0; 34 | extra_solves = 0; 35 | set_defaults(); 36 | /* Check we got the right number of arguments. */ 37 | if (nrhs == 0) 38 | mexErrMsgTxt("Not enough arguments: You need to specify at least the parameters.\n"); 39 | if (nrhs > 1) { 40 | /* Assume that the second argument is the settings. */ 41 | if (mxGetField(prhs[1], 0, "eps") != NULL) 42 | settings.eps = *mxGetPr(mxGetField(prhs[1], 0, "eps")); 43 | if (mxGetField(prhs[1], 0, "max_iters") != NULL) 44 | settings.max_iters = *mxGetPr(mxGetField(prhs[1], 0, "max_iters")); 45 | if (mxGetField(prhs[1], 0, "refine_steps") != NULL) 46 | settings.refine_steps = *mxGetPr(mxGetField(prhs[1], 0, "refine_steps")); 47 | if (mxGetField(prhs[1], 0, "verbose") != NULL) 48 | settings.verbose = *mxGetPr(mxGetField(prhs[1], 0, "verbose")); 49 | if (mxGetField(prhs[1], 0, "better_start") != NULL) 50 | settings.better_start = *mxGetPr(mxGetField(prhs[1], 0, "better_start")); 51 | if (mxGetField(prhs[1], 0, "verbose_refinement") != NULL) 52 | settings.verbose_refinement = *mxGetPr(mxGetField(prhs[1], 0, 53 | "verbose_refinement")); 54 | if (mxGetField(prhs[1], 0, "debug") != NULL) 55 | settings.debug = *mxGetPr(mxGetField(prhs[1], 0, "debug")); 56 | if (mxGetField(prhs[1], 0, "kkt_reg") != NULL) 57 | settings.kkt_reg = *mxGetPr(mxGetField(prhs[1], 0, "kkt_reg")); 58 | if (mxGetField(prhs[1], 0, "s_init") != NULL) 59 | settings.s_init = *mxGetPr(mxGetField(prhs[1], 0, "s_init")); 60 | if (mxGetField(prhs[1], 0, "z_init") != NULL) 61 | settings.z_init = *mxGetPr(mxGetField(prhs[1], 0, "z_init")); 62 | if (mxGetField(prhs[1], 0, "resid_tol") != NULL) 63 | settings.resid_tol = *mxGetPr(mxGetField(prhs[1], 0, "resid_tol")); 64 | if (mxGetField(prhs[1], 0, "extra_solves") != NULL) 65 | extra_solves = *mxGetPr(mxGetField(prhs[1], 0, "extra_solves")); 66 | else 67 | extra_solves = 0; 68 | if (mxGetField(prhs[1], 0, "prepare_for_c") != NULL) 69 | prepare_for_c = *mxGetPr(mxGetField(prhs[1], 0, "prepare_for_c")); 70 | } 71 | valid_vars = 0; 72 | this_var_errors = 0; 73 | xm = mxGetField(prhs[0], 0, "A"); 74 | if (xm == NULL) { 75 | printf("could not find params.A.\n"); 76 | } else { 77 | if (!((mxGetM(xm) == 64) && (mxGetN(xm) == 4))) { 78 | printf("A must be size (64,4), not (%d,%d).\n", mxGetM(xm), mxGetN(xm)); 79 | this_var_errors++; 80 | } 81 | if (mxIsComplex(xm)) { 82 | printf("parameter A must be real.\n"); 83 | this_var_errors++; 84 | } 85 | if (!mxIsClass(xm, "double")) { 86 | printf("parameter A must be a full matrix of doubles.\n"); 87 | this_var_errors++; 88 | } 89 | if (mxIsSparse(xm)) { 90 | printf("parameter A must be a full matrix.\n"); 91 | this_var_errors++; 92 | } 93 | if (this_var_errors == 0) { 94 | dest = params.A; 95 | src = mxGetPr(xm); 96 | for (i = 0; i < 256; i++) 97 | *dest++ = *src++; 98 | valid_vars++; 99 | } 100 | } 101 | this_var_errors = 0; 102 | xm = mxGetField(prhs[0], 0, "Q"); 103 | if (xm == NULL) { 104 | printf("could not find params.Q.\n"); 105 | } else { 106 | if (!((mxGetM(xm) == 4) && (mxGetN(xm) == 4))) { 107 | printf("Q must be size (4,4), not (%d,%d).\n", mxGetM(xm), mxGetN(xm)); 108 | this_var_errors++; 109 | } 110 | if (mxIsComplex(xm)) { 111 | printf("parameter Q must be real.\n"); 112 | this_var_errors++; 113 | } 114 | if (!mxIsClass(xm, "double")) { 115 | printf("parameter Q must be a full matrix of doubles.\n"); 116 | this_var_errors++; 117 | } 118 | if (mxIsSparse(xm)) { 119 | printf("parameter Q must be a full matrix.\n"); 120 | this_var_errors++; 121 | } 122 | if (this_var_errors == 0) { 123 | dest = params.Q; 124 | src = mxGetPr(xm); 125 | warned_diags = 0; 126 | for (i = 0; i < 4; i++) { 127 | for (j = 0; j < 4; j++) { 128 | if (i == j) { 129 | *dest++ = *src; 130 | } else if (!warned_diags && (*src != 0)) { 131 | printf("\n!!! Warning: ignoring off-diagonal elements in Q !!!\n\n"); 132 | warned_diags = 1; 133 | } 134 | src++; 135 | } 136 | } 137 | valid_vars++; 138 | } 139 | } 140 | if (valid_vars != 2) { 141 | printf("Error: %d parameters are invalid.\n", 2 - valid_vars); 142 | mexErrMsgTxt("invalid parameters found."); 143 | } 144 | if (prepare_for_c) { 145 | printf("settings.prepare_for_c == 1. thus, outputting for C.\n"); 146 | for (i = 0; i < 4; i++) 147 | printf(" params.Q[%d] = %.6g;\n", i, params.Q[i]); 148 | for (i = 0; i < 256; i++) 149 | printf(" params.A[%d] = %.6g;\n", i, params.A[i]); 150 | } 151 | /* Perform the actual solve in here. */ 152 | steps = solve(); 153 | /* For profiling purposes, allow extra silent solves if desired. */ 154 | settings.verbose = 0; 155 | for (i = 0; i < extra_solves; i++) 156 | solve(); 157 | /* Update the status variables. */ 158 | plhs[1] = mxCreateStructArray(1, dims1x1of1, 4, status_names); 159 | xm = mxCreateDoubleMatrix(1, 1, mxREAL); 160 | mxSetField(plhs[1], 0, "optval", xm); 161 | *mxGetPr(xm) = work.optval; 162 | xm = mxCreateDoubleMatrix(1, 1, mxREAL); 163 | mxSetField(plhs[1], 0, "gap", xm); 164 | *mxGetPr(xm) = work.gap; 165 | xm = mxCreateDoubleMatrix(1, 1, mxREAL); 166 | mxSetField(plhs[1], 0, "steps", xm); 167 | *mxGetPr(xm) = steps; 168 | xm = mxCreateDoubleMatrix(1, 1, mxREAL); 169 | mxSetField(plhs[1], 0, "converged", xm); 170 | *mxGetPr(xm) = work.converged; 171 | /* Extract variable values. */ 172 | plhs[0] = mxCreateStructArray(1, dims1x1of1, num_var_names, var_names); 173 | xm = mxCreateDoubleMatrix(4, 1, mxREAL); 174 | mxSetField(plhs[0], 0, "beta", xm); 175 | dest = mxGetPr(xm); 176 | src = vars.beta; 177 | for (i = 0; i < 4; i++) { 178 | *dest++ = *src++; 179 | } 180 | } 181 | -------------------------------------------------------------------------------- /external/jsonlab/examples/demo_ubjson_basic.m: -------------------------------------------------------------------------------- 1 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 2 | % Demonstration of Basic Utilities of JSONlab 3 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 4 | 5 | rngstate = rand ('state'); 6 | randseed=hex2dec('623F9A9E'); 7 | clear data2json json2data 8 | 9 | fprintf(1,'\n%%=================================================\n') 10 | fprintf(1,'%% a simple scalar value \n') 11 | fprintf(1,'%%=================================================\n\n') 12 | 13 | data2json=pi 14 | saveubjson('',data2json) 15 | json2data=loadubjson(ans) 16 | 17 | fprintf(1,'\n%%=================================================\n') 18 | fprintf(1,'%% a complex number\n') 19 | fprintf(1,'%%=================================================\n\n') 20 | 21 | clear i; 22 | data2json=1+2*i 23 | saveubjson('',data2json) 24 | json2data=loadubjson(ans) 25 | 26 | fprintf(1,'\n%%=================================================\n') 27 | fprintf(1,'%% a complex matrix\n') 28 | fprintf(1,'%%=================================================\n\n') 29 | 30 | data2json=magic(6); 31 | data2json=data2json(:,1:3)+data2json(:,4:6)*i 32 | saveubjson('',data2json) 33 | json2data=loadubjson(ans) 34 | 35 | fprintf(1,'\n%%=================================================\n') 36 | fprintf(1,'%% MATLAB special constants\n') 37 | fprintf(1,'%%=================================================\n\n') 38 | 39 | data2json=[NaN Inf -Inf] 40 | saveubjson('specials',data2json) 41 | json2data=loadubjson(ans) 42 | 43 | fprintf(1,'\n%%=================================================\n') 44 | fprintf(1,'%% a real sparse matrix\n') 45 | fprintf(1,'%%=================================================\n\n') 46 | 47 | data2json=sprand(10,10,0.1) 48 | saveubjson('sparse',data2json) 49 | json2data=loadubjson(ans) 50 | 51 | fprintf(1,'\n%%=================================================\n') 52 | fprintf(1,'%% a complex sparse matrix\n') 53 | fprintf(1,'%%=================================================\n\n') 54 | 55 | data2json=data2json-data2json*i 56 | saveubjson('complex_sparse',data2json) 57 | json2data=loadubjson(ans) 58 | 59 | fprintf(1,'\n%%=================================================\n') 60 | fprintf(1,'%% an all-zero sparse matrix\n') 61 | fprintf(1,'%%=================================================\n\n') 62 | 63 | data2json=sparse(2,3); 64 | saveubjson('all_zero_sparse',data2json) 65 | json2data=loadubjson(ans) 66 | 67 | fprintf(1,'\n%%=================================================\n') 68 | fprintf(1,'%% an empty sparse matrix\n') 69 | fprintf(1,'%%=================================================\n\n') 70 | 71 | data2json=sparse([]); 72 | saveubjson('empty_sparse',data2json) 73 | json2data=loadubjson(ans) 74 | 75 | fprintf(1,'\n%%=================================================\n') 76 | fprintf(1,'%% an empty 0-by-0 real matrix\n') 77 | fprintf(1,'%%=================================================\n\n') 78 | 79 | data2json=[]; 80 | saveubjson('empty_0by0_real',data2json) 81 | json2data=loadubjson(ans) 82 | 83 | fprintf(1,'\n%%=================================================\n') 84 | fprintf(1,'%% an empty 0-by-3 real matrix\n') 85 | fprintf(1,'%%=================================================\n\n') 86 | 87 | data2json=zeros(0,3); 88 | saveubjson('empty_0by3_real',data2json) 89 | json2data=loadubjson(ans) 90 | 91 | fprintf(1,'\n%%=================================================\n') 92 | fprintf(1,'%% a sparse real column vector\n') 93 | fprintf(1,'%%=================================================\n\n') 94 | 95 | data2json=sparse([0,3,0,1,4]'); 96 | saveubjson('sparse_column_vector',data2json) 97 | json2data=loadubjson(ans) 98 | 99 | fprintf(1,'\n%%=================================================\n') 100 | fprintf(1,'%% a sparse complex column vector\n') 101 | fprintf(1,'%%=================================================\n\n') 102 | 103 | data2json=data2json-1i*data2json; 104 | saveubjson('complex_sparse_column_vector',data2json) 105 | json2data=loadubjson(ans) 106 | 107 | fprintf(1,'\n%%=================================================\n') 108 | fprintf(1,'%% a sparse real row vector\n') 109 | fprintf(1,'%%=================================================\n\n') 110 | 111 | data2json=sparse([0,3,0,1,4]); 112 | saveubjson('sparse_row_vector',data2json) 113 | json2data=loadubjson(ans) 114 | 115 | fprintf(1,'\n%%=================================================\n') 116 | fprintf(1,'%% a sparse complex row vector\n') 117 | fprintf(1,'%%=================================================\n\n') 118 | 119 | data2json=data2json-1i*data2json; 120 | saveubjson('complex_sparse_row_vector',data2json) 121 | json2data=loadubjson(ans) 122 | 123 | fprintf(1,'\n%%=================================================\n') 124 | fprintf(1,'%% a structure\n') 125 | fprintf(1,'%%=================================================\n\n') 126 | 127 | data2json=struct('name','Think Different','year',1997,'magic',magic(3),... 128 | 'misfits',[Inf,NaN],'embedded',struct('left',true,'right',false)) 129 | saveubjson('astruct',data2json,struct('ParseLogical',1)) 130 | json2data=loadubjson(ans) 131 | 132 | fprintf(1,'\n%%=================================================\n') 133 | fprintf(1,'%% a structure array\n') 134 | fprintf(1,'%%=================================================\n\n') 135 | 136 | data2json=struct('name','Nexus Prime','rank',9); 137 | data2json(2)=struct('name','Sentinel Prime','rank',9); 138 | data2json(3)=struct('name','Optimus Prime','rank',9); 139 | saveubjson('Supreme Commander',data2json) 140 | json2data=loadubjson(ans) 141 | 142 | fprintf(1,'\n%%=================================================\n') 143 | fprintf(1,'%% a cell array\n') 144 | fprintf(1,'%%=================================================\n\n') 145 | 146 | data2json=cell(3,1); 147 | data2json{1}=struct('buzz',1.1,'rex',1.2,'bo',1.3,'hamm',2.0,'slink',2.1,'potato',2.2,... 148 | 'woody',3.0,'sarge',3.1,'etch',4.0,'lenny',5.0,'squeeze',6.0,'wheezy',7.0); 149 | data2json{2}=struct('Ubuntu',['Kubuntu';'Xubuntu';'Lubuntu']); 150 | data2json{3}=[10.04,10.10,11.04,11.10] 151 | saveubjson('debian',data2json,struct('FloatFormat','%.2f')) 152 | json2data=loadubjson(ans) 153 | 154 | fprintf(1,'\n%%=================================================\n') 155 | fprintf(1,'%% invalid field-name handling\n') 156 | fprintf(1,'%%=================================================\n\n') 157 | 158 | json2data=loadubjson(saveubjson('',loadjson('{"ValidName":1, "_InvalidName":2, ":Field:":3, "项目":"绝密"}'))) 159 | 160 | fprintf(1,'\n%%=================================================\n') 161 | fprintf(1,'%% a 2D cell array\n') 162 | fprintf(1,'%%=================================================\n\n') 163 | 164 | data2json={{1,{2,3}},{4,5},{6};{7},{8,9},{10}}; 165 | saveubjson('data2json',data2json) 166 | json2data=loadubjson(ans) % only savejson works for cell arrays, loadjson has issues 167 | 168 | fprintf(1,'\n%%=================================================\n') 169 | fprintf(1,'%% a 2D struct array\n') 170 | fprintf(1,'%%=================================================\n\n') 171 | 172 | data2json=repmat(struct('idx',0,'data','structs'),[2,3]) 173 | for i=1:6 174 | data2json(i).idx=i; 175 | end 176 | saveubjson('data2json',data2json) 177 | json2data=loadubjson(ans) 178 | 179 | rand ('state',rngstate); 180 | 181 | -------------------------------------------------------------------------------- /octomap_corridor/octomap_corridor.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | #include "octomap/OcTree.h" 9 | 10 | #include "array2d.hpp" 11 | 12 | 13 | // cvxgen requires that we define these globals :( 14 | #include "cvxgen/solver.h" 15 | Vars vars; 16 | Params params; 17 | Workspace work; 18 | Settings settings; 19 | 20 | 21 | // TODO: make it an input, maybe 22 | // static float const OCCUPANCY_THRESHOLD = 0.95; 23 | 24 | // TODO: these are fixed to avoid needing 4d array class, but should be variable 25 | static int const DIM = 3; 26 | static int const PP_DEGREE = 7; 27 | static int const PP_SIZE = PP_DEGREE + 1; 28 | static int const SAMPLES_PER_PIECE = 32; 29 | 30 | // ax <= b 31 | struct halfspace 32 | { 33 | float a[3]; 34 | float b; 35 | }; 36 | 37 | using poly_piece = std::array, DIM>; 38 | using poly_sample = std::array, DIM>; 39 | using polyhedron = std::vector; 40 | 41 | template 42 | T sqr(T const &x) 43 | { 44 | return x * x; 45 | } 46 | 47 | using p3d = octomath::Vector3; 48 | 49 | p3d min(p3d const &a, p3d const &b) 50 | { 51 | return p3d( 52 | std::min(a.x(), b.x()), 53 | std::min(a.y(), b.y()), 54 | std::min(a.z(), b.z())); 55 | } 56 | 57 | p3d max(p3d const &a, p3d const &b) 58 | { 59 | return p3d( 60 | std::max(a.x(), b.x()), 61 | std::max(a.y(), b.y()), 62 | std::max(a.z(), b.z())); 63 | } 64 | 65 | 66 | bool overlapping(const p3d& b1min, const p3d& b1max, const p3d& b2min, const p3d& b2max) 67 | { 68 | return (b1max.x() >= b2min.x() && b2max.x() >= b1min.x()) 69 | && (b1max.y() >= b2min.y() && b2max.y() >= b1min.y()) 70 | && (b1max.z() >= b2min.z() && b2max.z() >= b1min.z()); 71 | } 72 | 73 | // inner function for separating one octomap cell from one robot path segment 74 | halfspace supporting_hyperplane( 75 | octomap::OcTree::iterator_base const &it, 76 | poly_sample const &sample, 77 | float const ellipsoid[3]) 78 | { 79 | // cvxgen 80 | set_defaults(); 81 | setup_indexing(); 82 | 83 | params.Q[0] = sqr(ellipsoid[0]); 84 | params.Q[1] = sqr(ellipsoid[1]); 85 | params.Q[2] = sqr(ellipsoid[2]); 86 | params.Q[3] = 0.0; 87 | 88 | // convert octomap cell into its 8 vertices 89 | float cx = it.getX(); 90 | float cy = it.getY(); 91 | float cz = it.getZ(); 92 | float hsz = it.getSize() / 2.0; 93 | int const BOX_N_VTX = 8; 94 | float cube_pts[BOX_N_VTX][3] = { 95 | { cx + hsz, cy + hsz, cz + hsz }, 96 | { cx + hsz, cy + hsz, cz - hsz }, 97 | { cx + hsz, cy - hsz, cz + hsz }, 98 | { cx + hsz, cy - hsz, cz - hsz }, 99 | { cx - hsz, cy + hsz, cz + hsz }, 100 | { cx - hsz, cy + hsz, cz - hsz }, 101 | { cx - hsz, cy - hsz, cz + hsz }, 102 | { cx - hsz, cy - hsz, cz - hsz }, 103 | }; 104 | 105 | #define params_a_ind(i1, i2) params.A[(i1) + SAMPLES_PER_PIECE*2*(i2)] 106 | 107 | // linear constraints for "in" points (robot path) 108 | for (int dim = 0; dim < 3; ++dim) { 109 | for (int p = 0; p < SAMPLES_PER_PIECE; ++p) { 110 | params_a_ind(p, dim) = sample[dim][p]; 111 | } 112 | } 113 | for (int i = 0; i < SAMPLES_PER_PIECE; ++i) { 114 | params_a_ind(i, 3) = 1.0; 115 | } 116 | 117 | // linear constraints for "out" points (obstacle vertices) 118 | // repeat cube vertices 4 times to get 32 vs. 32 pt sep hyp 119 | int const VTX_REP = 4; 120 | static_assert(VTX_REP * BOX_N_VTX == SAMPLES_PER_PIECE, "oops"); 121 | for (int dim = 0; dim < 3; ++dim) { 122 | for (int rep = 0; rep < VTX_REP; ++rep) { 123 | for (int p = 0; p < BOX_N_VTX; ++p) { 124 | int row = SAMPLES_PER_PIECE + BOX_N_VTX * rep + p; 125 | assert(row < 2 * SAMPLES_PER_PIECE); 126 | params_a_ind(row, dim) = -cube_pts[p][dim]; 127 | } 128 | } 129 | } 130 | for (int i = SAMPLES_PER_PIECE; i < 2*SAMPLES_PER_PIECE; ++i) { 131 | params_a_ind(i, 3) = -1.0; 132 | } 133 | 134 | set_defaults(); 135 | settings.verbose = 0; 136 | setup_indexing(); 137 | solve(); 138 | 139 | double b0 = vars.beta[0]; 140 | double b1 = vars.beta[1]; 141 | double b2 = vars.beta[2]; 142 | double b3 = vars.beta[3]; 143 | 144 | double inv_norm = 1.0 / sqrt(sqr(b0) + sqr(b1) + sqr(b2)); 145 | double a0 = inv_norm * b0; 146 | double a1 = inv_norm * b1; 147 | double a2 = inv_norm * b2; 148 | double bb = -inv_norm * b3; 149 | 150 | // SVM gives us separating hyperplane - 151 | // make it a supporting hyperplane 152 | double min_dist = 1000000000; 153 | for (int i = 0; i < BOX_N_VTX; ++i) { 154 | double mul = a0 * cube_pts[i][0] 155 | + a1 * cube_pts[i][1] 156 | + a2 * cube_pts[i][2]; 157 | min_dist = std::min(min_dist, mul - bb); 158 | } 159 | bb = bb + min_dist; 160 | 161 | halfspace h; 162 | h.a[0] = a0; 163 | h.a[1] = a1; 164 | h.a[2] = a2; 165 | h.b = bb; 166 | 167 | return h; 168 | } 169 | 170 | polyhedron partition(octomap::OcTree const &ot, poly_sample const &s) 171 | { 172 | // TODO input 173 | float const ellipsoid[3] = {0.15, 0.15, 0.15}; 174 | 175 | // compute padded bounding box around traj sample 176 | auto pt = [&s](int i) { return p3d(s[0][i], s[1][i], s[2][i]); }; 177 | p3d bbox_min = pt(0); 178 | p3d bbox_max = pt(0); 179 | for (int i = 1; i < SAMPLES_PER_PIECE; ++i) { 180 | p3d p = pt(i); 181 | bbox_min = min(bbox_min, p); 182 | bbox_max = min(bbox_max, p); 183 | } 184 | // TODO compute the pad amount intelligently 185 | float const BBOX_PAD = 1.0; // meters 186 | bbox_min -= p3d(1, 1, 1) * BBOX_PAD; 187 | bbox_max += p3d(1, 1, 1) * BBOX_PAD; 188 | 189 | // output 190 | std::vector part; 191 | 192 | // compute supporting hyperplanes for each occupied cell in bbox 193 | auto it = ot.begin_leafs_bbx(bbox_min, bbox_max); 194 | auto end = ot.end_leafs_bbx(); 195 | for (; it != end; ++it) { 196 | p3d minLeaf = it.getCoordinate() - p3d(it.getSize() / 2, it.getSize() / 2, it.getSize() / 2); 197 | p3d maxLeaf = it.getCoordinate() + p3d(it.getSize() / 2, it.getSize() / 2, it.getSize() / 2); 198 | 199 | if (overlapping(minLeaf, maxLeaf, bbox_min, bbox_max) && ot.isNodeOccupied(*it)) { 200 | // double occ = it->getOccupancy(); 201 | // if (occ > OCCUPANCY_THRESHOLD) { 202 | // it's an occupied cell 203 | halfspace h = supporting_hyperplane(it, s, ellipsoid); 204 | part.push_back(h); 205 | } 206 | } 207 | 208 | // add half-spaces for the query bbox 209 | // TODO: can we use octomap data to remove some of these? 210 | halfspace h_zero; 211 | h_zero.a[0] = h_zero.a[1] = h_zero.a[2] = 0; 212 | h_zero.b = 0; 213 | for (int d = 0; d < 3; ++d) { 214 | halfspace hmax = h_zero; 215 | hmax.a[d] = 1; 216 | hmax.b = bbox_max(d); 217 | part.push_back(hmax); 218 | 219 | halfspace hmin = h_zero; 220 | hmin.a[d] = -1; 221 | hmin.b = -bbox_min(d); 222 | part.push_back(hmin); 223 | } 224 | 225 | return part; 226 | } 227 | 228 | // evaluate a polynomial using horner's rule. 229 | float polyval(float const p[PP_SIZE], float t) 230 | { 231 | float x = 0.0; 232 | for (int i = PP_DEGREE; i >= 0; --i) { 233 | x = x * t + p[i]; 234 | } 235 | return x; 236 | } 237 | 238 | template 239 | T read(std::istream &s) 240 | { 241 | T x; 242 | s.read((char *)&x, sizeof(T)); 243 | return x; 244 | } 245 | 246 | template 247 | void write(std::ostream &s, T const &val) 248 | { 249 | s.write((char const *)&val, sizeof(T)); 250 | } 251 | 252 | 253 | // argv[1] : octomap file 254 | // argv[2] : .pptrajs file 255 | // argv[3] : output .halfspaces file 256 | int main(int argc, char *argv[]) 257 | { 258 | /* 259 | .pptrajs format: 260 | Header: 261 | N - int32 - number of robots 262 | k - int32 - number of timesteps 263 | dim - int32 - dimension 264 | order - int32 - polynomial degree + 1 265 | Breaks: 266 | t - float32[k + 1] - piece break times 267 | Coefs: 268 | c - float32[order, dim, k, N] - first dimension contiguous - 269 | ascending order (constant term first) 270 | Sentinel - 0xFFFFFFFF - for sanity check 271 | */ 272 | 273 | if (argc < 4) { 274 | std::cerr << "usage: pptraj " << std::endl; 275 | return 1; 276 | } 277 | 278 | // TODO: verify throws on error! 279 | octomap::OcTree ot(argv[1]); 280 | 281 | std::ifstream ppstream(argv[2], std::ios::in | std::ios::binary); 282 | if (!ppstream.good()) { 283 | std::cerr << "error loading trajectories from " << argv[2] << std::endl; 284 | return 1; 285 | } 286 | std::ofstream outstream(argv[3], std::ios::out | std::ios::binary); 287 | if (!outstream.good()) { 288 | std::cerr << "error opening output file for writing: " << argv[3] << std::endl; 289 | return 1; 290 | } 291 | 292 | // read the header 293 | int32_t const N = read(ppstream); 294 | int32_t const K = read(ppstream); 295 | int32_t const dim = read(ppstream); 296 | int32_t const order = read(ppstream); 297 | 298 | assert(dim == 3); 299 | assert(order == PP_SIZE); 300 | 301 | // read the break times 302 | std::vector breaks(K + 1); 303 | for (int i = 0; i < (K + 1); ++i) { 304 | breaks[i] = read(ppstream); 305 | } 306 | 307 | // read polynomial coefficients and sample trajectory points 308 | Array2D coefs(N, K); 309 | Array2D samples(N, K); 310 | for (int n = 0; n < N; ++n) { 311 | for (int k = 0; k < K; ++k) { 312 | for (int d = 0; d < dim; ++d) { 313 | // collect the coefficients 314 | for (int i = 0; i < order; ++i) { 315 | float val = read(ppstream); 316 | coefs(n,k)[d][i] = val; 317 | } 318 | float const *coef_block = &coefs(n,k)[d][0]; 319 | 320 | // sample the piece 321 | float const T_piece = breaks[k + 1] - breaks[k]; 322 | float const dt = T_piece / (SAMPLES_PER_PIECE - 1); 323 | for (int i = 0; i < SAMPLES_PER_PIECE; ++i) { 324 | float t = dt * i; 325 | float val = polyval(coef_block, t); 326 | samples(n,k)[d][i] = val; 327 | } 328 | } 329 | } 330 | } 331 | 332 | // check that we got through the whole file 333 | uint32_t sentinel = read(ppstream); 334 | assert(sentinel == 0xFFFFFFFFu); 335 | ppstream.close(); 336 | 337 | // compute all corridors 338 | Array2D corridors(N, K); 339 | std::transform(samples.begin(), samples.end(), corridors.begin(), 340 | [&ot](poly_sample const &s) { return partition(ot, s); }); 341 | 342 | // write the corridors to the file 343 | /* 344 | .halfspaces format: 345 | Header: 346 | N - int32 - number of robots 347 | k - int32 - number of timesteps 348 | dim - int32 - dimension 349 | for N robots: 350 | for K timesteps: 351 | M - int32 - number of halfspaces 352 | for M halfspaces: 353 | a - float32[dim] for ax <= b 354 | b - float32 355 | Sentinel - 0xFFFFFFFF - for sanity check 356 | 357 | note: M can be different for each robot-timestep pair! 358 | */ 359 | write(outstream, N); 360 | write(outstream, K); 361 | write(outstream, dim); 362 | for (int n = 0; n < N; ++n) { 363 | for (int k = 0; k < K; ++k) { 364 | auto const &p = corridors(n, k); 365 | int32_t m = (int32_t)p.size(); 366 | write(outstream, m); 367 | for (int i = 0; i < m; ++i) { 368 | halfspace const &h = p[i]; 369 | write(outstream, h.a[0]); 370 | write(outstream, h.a[1]); 371 | write(outstream, h.a[2]); 372 | write(outstream, h.b); 373 | } 374 | } 375 | } 376 | write(outstream, 0xFFFFFFFF); 377 | outstream.close(); 378 | } 379 | -------------------------------------------------------------------------------- /svm_sephyp_2point/solver.c: -------------------------------------------------------------------------------- 1 | /* Produced by CVXGEN, 2016-11-30 17:09:32 -0500. */ 2 | /* CVXGEN is Copyright (C) 2006-2012 Jacob Mattingley, jem@cvxgen.com. */ 3 | /* The code in this file is Copyright (C) 2006-2012 Jacob Mattingley. */ 4 | /* CVXGEN, or solvers produced by CVXGEN, cannot be used for commercial */ 5 | /* applications without prior written permission from Jacob Mattingley. */ 6 | 7 | /* Filename: solver.c. */ 8 | /* Description: Main solver file. */ 9 | #include "solver.h" 10 | double eval_gap(void) { 11 | int i; 12 | double gap; 13 | gap = 0; 14 | for (i = 0; i < 4; i++) 15 | gap += work.z[i]*work.s[i]; 16 | return gap; 17 | } 18 | void set_defaults(void) { 19 | settings.resid_tol = 1e-6; 20 | settings.eps = 1e-4; 21 | settings.max_iters = 25; 22 | settings.refine_steps = 1; 23 | settings.s_init = 1; 24 | settings.z_init = 1; 25 | settings.debug = 0; 26 | settings.verbose = 1; 27 | settings.verbose_refinement = 0; 28 | settings.better_start = 1; 29 | settings.kkt_reg = 1e-7; 30 | } 31 | void setup_pointers(void) { 32 | work.y = work.x + 4; 33 | work.s = work.x + 4; 34 | work.z = work.x + 8; 35 | vars.beta = work.x + 0; 36 | } 37 | void setup_indexing(void) { 38 | setup_pointers(); 39 | } 40 | void set_start(void) { 41 | int i; 42 | for (i = 0; i < 4; i++) 43 | work.x[i] = 0; 44 | for (i = 0; i < 0; i++) 45 | work.y[i] = 0; 46 | for (i = 0; i < 4; i++) 47 | work.s[i] = (work.h[i] > 0) ? work.h[i] : settings.s_init; 48 | for (i = 0; i < 4; i++) 49 | work.z[i] = settings.z_init; 50 | } 51 | double eval_objv(void) { 52 | int i; 53 | double objv; 54 | /* Borrow space in work.rhs. */ 55 | multbyP(work.rhs, work.x); 56 | objv = 0; 57 | for (i = 0; i < 4; i++) 58 | objv += work.x[i]*work.rhs[i]; 59 | objv *= 0.5; 60 | for (i = 0; i < 4; i++) 61 | objv += work.q[i]*work.x[i]; 62 | objv += 0; 63 | return objv; 64 | } 65 | void fillrhs_aff(void) { 66 | int i; 67 | double *r1, *r2, *r3, *r4; 68 | r1 = work.rhs; 69 | r2 = work.rhs + 4; 70 | r3 = work.rhs + 8; 71 | r4 = work.rhs + 12; 72 | /* r1 = -A^Ty - G^Tz - Px - q. */ 73 | multbymAT(r1, work.y); 74 | multbymGT(work.buffer, work.z); 75 | for (i = 0; i < 4; i++) 76 | r1[i] += work.buffer[i]; 77 | multbyP(work.buffer, work.x); 78 | for (i = 0; i < 4; i++) 79 | r1[i] -= work.buffer[i] + work.q[i]; 80 | /* r2 = -z. */ 81 | for (i = 0; i < 4; i++) 82 | r2[i] = -work.z[i]; 83 | /* r3 = -Gx - s + h. */ 84 | multbymG(r3, work.x); 85 | for (i = 0; i < 4; i++) 86 | r3[i] += -work.s[i] + work.h[i]; 87 | /* r4 = -Ax + b. */ 88 | multbymA(r4, work.x); 89 | for (i = 0; i < 0; i++) 90 | r4[i] += work.b[i]; 91 | } 92 | void fillrhs_cc(void) { 93 | int i; 94 | double *r2; 95 | double *ds_aff, *dz_aff; 96 | double mu; 97 | double alpha; 98 | double sigma; 99 | double smu; 100 | double minval; 101 | r2 = work.rhs + 4; 102 | ds_aff = work.lhs_aff + 4; 103 | dz_aff = work.lhs_aff + 8; 104 | mu = 0; 105 | for (i = 0; i < 4; i++) 106 | mu += work.s[i]*work.z[i]; 107 | /* Don't finish calculating mu quite yet. */ 108 | /* Find min(min(ds./s), min(dz./z)). */ 109 | minval = 0; 110 | for (i = 0; i < 4; i++) 111 | if (ds_aff[i] < minval*work.s[i]) 112 | minval = ds_aff[i]/work.s[i]; 113 | for (i = 0; i < 4; i++) 114 | if (dz_aff[i] < minval*work.z[i]) 115 | minval = dz_aff[i]/work.z[i]; 116 | /* Find alpha. */ 117 | if (-1 < minval) 118 | alpha = 1; 119 | else 120 | alpha = -1/minval; 121 | sigma = 0; 122 | for (i = 0; i < 4; i++) 123 | sigma += (work.s[i] + alpha*ds_aff[i])* 124 | (work.z[i] + alpha*dz_aff[i]); 125 | sigma /= mu; 126 | sigma = sigma*sigma*sigma; 127 | /* Finish calculating mu now. */ 128 | mu *= 0.25; 129 | smu = sigma*mu; 130 | /* Fill-in the rhs. */ 131 | for (i = 0; i < 4; i++) 132 | work.rhs[i] = 0; 133 | for (i = 8; i < 12; i++) 134 | work.rhs[i] = 0; 135 | for (i = 0; i < 4; i++) 136 | r2[i] = work.s_inv[i]*(smu - ds_aff[i]*dz_aff[i]); 137 | } 138 | void refine(double *target, double *var) { 139 | int i, j; 140 | double *residual = work.buffer; 141 | double norm2; 142 | double *new_var = work.buffer2; 143 | for (j = 0; j < settings.refine_steps; j++) { 144 | norm2 = 0; 145 | matrix_multiply(residual, var); 146 | for (i = 0; i < 12; i++) { 147 | residual[i] = residual[i] - target[i]; 148 | norm2 += residual[i]*residual[i]; 149 | } 150 | #ifndef ZERO_LIBRARY_MODE 151 | if (settings.verbose_refinement) { 152 | if (j == 0) 153 | printf("Initial residual before refinement has norm squared %.6g.\n", norm2); 154 | else 155 | printf("After refinement we get squared norm %.6g.\n", norm2); 156 | } 157 | #endif 158 | /* Solve to find new_var = KKT \ (target - A*var). */ 159 | ldl_solve(residual, new_var); 160 | /* Update var += new_var, or var += KKT \ (target - A*var). */ 161 | for (i = 0; i < 12; i++) { 162 | var[i] -= new_var[i]; 163 | } 164 | } 165 | #ifndef ZERO_LIBRARY_MODE 166 | if (settings.verbose_refinement) { 167 | /* Check the residual once more, but only if we're reporting it, since */ 168 | /* it's expensive. */ 169 | norm2 = 0; 170 | matrix_multiply(residual, var); 171 | for (i = 0; i < 12; i++) { 172 | residual[i] = residual[i] - target[i]; 173 | norm2 += residual[i]*residual[i]; 174 | } 175 | if (j == 0) 176 | printf("Initial residual before refinement has norm squared %.6g.\n", norm2); 177 | else 178 | printf("After refinement we get squared norm %.6g.\n", norm2); 179 | } 180 | #endif 181 | } 182 | double calc_ineq_resid_squared(void) { 183 | /* Calculates the norm ||-Gx - s + h||. */ 184 | double norm2_squared; 185 | int i; 186 | /* Find -Gx. */ 187 | multbymG(work.buffer, work.x); 188 | /* Add -s + h. */ 189 | for (i = 0; i < 4; i++) 190 | work.buffer[i] += -work.s[i] + work.h[i]; 191 | /* Now find the squared norm. */ 192 | norm2_squared = 0; 193 | for (i = 0; i < 4; i++) 194 | norm2_squared += work.buffer[i]*work.buffer[i]; 195 | return norm2_squared; 196 | } 197 | double calc_eq_resid_squared(void) { 198 | /* Calculates the norm ||-Ax + b||. */ 199 | double norm2_squared; 200 | int i; 201 | /* Find -Ax. */ 202 | multbymA(work.buffer, work.x); 203 | /* Add +b. */ 204 | for (i = 0; i < 0; i++) 205 | work.buffer[i] += work.b[i]; 206 | /* Now find the squared norm. */ 207 | norm2_squared = 0; 208 | for (i = 0; i < 0; i++) 209 | norm2_squared += work.buffer[i]*work.buffer[i]; 210 | return norm2_squared; 211 | } 212 | void better_start(void) { 213 | /* Calculates a better starting point, using a similar approach to CVXOPT. */ 214 | /* Not yet speed optimized. */ 215 | int i; 216 | double *x, *s, *z, *y; 217 | double alpha; 218 | work.block_33[0] = -1; 219 | /* Make sure sinvz is 1 to make hijacked KKT system ok. */ 220 | for (i = 0; i < 4; i++) 221 | work.s_inv_z[i] = 1; 222 | fill_KKT(); 223 | ldl_factor(); 224 | fillrhs_start(); 225 | /* Borrow work.lhs_aff for the solution. */ 226 | ldl_solve(work.rhs, work.lhs_aff); 227 | /* Don't do any refinement for now. Precision doesn't matter too much. */ 228 | x = work.lhs_aff; 229 | s = work.lhs_aff + 4; 230 | z = work.lhs_aff + 8; 231 | y = work.lhs_aff + 12; 232 | /* Just set x and y as is. */ 233 | for (i = 0; i < 4; i++) 234 | work.x[i] = x[i]; 235 | for (i = 0; i < 0; i++) 236 | work.y[i] = y[i]; 237 | /* Now complete the initialization. Start with s. */ 238 | /* Must have alpha > max(z). */ 239 | alpha = -1e99; 240 | for (i = 0; i < 4; i++) 241 | if (alpha < z[i]) 242 | alpha = z[i]; 243 | if (alpha < 0) { 244 | for (i = 0; i < 4; i++) 245 | work.s[i] = -z[i]; 246 | } else { 247 | alpha += 1; 248 | for (i = 0; i < 4; i++) 249 | work.s[i] = -z[i] + alpha; 250 | } 251 | /* Now initialize z. */ 252 | /* Now must have alpha > max(-z). */ 253 | alpha = -1e99; 254 | for (i = 0; i < 4; i++) 255 | if (alpha < -z[i]) 256 | alpha = -z[i]; 257 | if (alpha < 0) { 258 | for (i = 0; i < 4; i++) 259 | work.z[i] = z[i]; 260 | } else { 261 | alpha += 1; 262 | for (i = 0; i < 4; i++) 263 | work.z[i] = z[i] + alpha; 264 | } 265 | } 266 | void fillrhs_start(void) { 267 | /* Fill rhs with (-q, 0, h, b). */ 268 | int i; 269 | double *r1, *r2, *r3, *r4; 270 | r1 = work.rhs; 271 | r2 = work.rhs + 4; 272 | r3 = work.rhs + 8; 273 | r4 = work.rhs + 12; 274 | for (i = 0; i < 4; i++) 275 | r1[i] = -work.q[i]; 276 | for (i = 0; i < 4; i++) 277 | r2[i] = 0; 278 | for (i = 0; i < 4; i++) 279 | r3[i] = work.h[i]; 280 | for (i = 0; i < 0; i++) 281 | r4[i] = work.b[i]; 282 | } 283 | long solve(void) { 284 | int i; 285 | int iter; 286 | double *dx, *ds, *dy, *dz; 287 | double minval; 288 | double alpha; 289 | work.converged = 0; 290 | setup_pointers(); 291 | pre_ops(); 292 | #ifndef ZERO_LIBRARY_MODE 293 | if (settings.verbose) 294 | printf("iter objv gap |Ax-b| |Gx+s-h| step\n"); 295 | #endif 296 | fillq(); 297 | fillh(); 298 | fillb(); 299 | if (settings.better_start) 300 | better_start(); 301 | else 302 | set_start(); 303 | for (iter = 0; iter < settings.max_iters; iter++) { 304 | for (i = 0; i < 4; i++) { 305 | work.s_inv[i] = 1.0 / work.s[i]; 306 | work.s_inv_z[i] = work.s_inv[i]*work.z[i]; 307 | } 308 | work.block_33[0] = 0; 309 | fill_KKT(); 310 | ldl_factor(); 311 | /* Affine scaling directions. */ 312 | fillrhs_aff(); 313 | ldl_solve(work.rhs, work.lhs_aff); 314 | refine(work.rhs, work.lhs_aff); 315 | /* Centering plus corrector directions. */ 316 | fillrhs_cc(); 317 | ldl_solve(work.rhs, work.lhs_cc); 318 | refine(work.rhs, work.lhs_cc); 319 | /* Add the two together and store in aff. */ 320 | for (i = 0; i < 12; i++) 321 | work.lhs_aff[i] += work.lhs_cc[i]; 322 | /* Rename aff to reflect its new meaning. */ 323 | dx = work.lhs_aff; 324 | ds = work.lhs_aff + 4; 325 | dz = work.lhs_aff + 8; 326 | dy = work.lhs_aff + 12; 327 | /* Find min(min(ds./s), min(dz./z)). */ 328 | minval = 0; 329 | for (i = 0; i < 4; i++) 330 | if (ds[i] < minval*work.s[i]) 331 | minval = ds[i]/work.s[i]; 332 | for (i = 0; i < 4; i++) 333 | if (dz[i] < minval*work.z[i]) 334 | minval = dz[i]/work.z[i]; 335 | /* Find alpha. */ 336 | if (-0.99 < minval) 337 | alpha = 1; 338 | else 339 | alpha = -0.99/minval; 340 | /* Update the primal and dual variables. */ 341 | for (i = 0; i < 4; i++) 342 | work.x[i] += alpha*dx[i]; 343 | for (i = 0; i < 4; i++) 344 | work.s[i] += alpha*ds[i]; 345 | for (i = 0; i < 4; i++) 346 | work.z[i] += alpha*dz[i]; 347 | for (i = 0; i < 0; i++) 348 | work.y[i] += alpha*dy[i]; 349 | work.gap = eval_gap(); 350 | work.eq_resid_squared = calc_eq_resid_squared(); 351 | work.ineq_resid_squared = calc_ineq_resid_squared(); 352 | #ifndef ZERO_LIBRARY_MODE 353 | if (settings.verbose) { 354 | work.optval = eval_objv(); 355 | printf("%3d %10.3e %9.2e %9.2e %9.2e % 6.4f\n", 356 | iter+1, work.optval, work.gap, sqrt(work.eq_resid_squared), 357 | sqrt(work.ineq_resid_squared), alpha); 358 | } 359 | #endif 360 | /* Test termination conditions. Requires optimality, and satisfied */ 361 | /* constraints. */ 362 | if ( (work.gap < settings.eps) 363 | && (work.eq_resid_squared <= settings.resid_tol*settings.resid_tol) 364 | && (work.ineq_resid_squared <= settings.resid_tol*settings.resid_tol) 365 | ) { 366 | work.converged = 1; 367 | work.optval = eval_objv(); 368 | return iter+1; 369 | } 370 | } 371 | return iter; 372 | } 373 | -------------------------------------------------------------------------------- /svm_sephyp_32point/solver.c: -------------------------------------------------------------------------------- 1 | /* Produced by CVXGEN, 2016-11-30 16:59:57 -0500. */ 2 | /* CVXGEN is Copyright (C) 2006-2012 Jacob Mattingley, jem@cvxgen.com. */ 3 | /* The code in this file is Copyright (C) 2006-2012 Jacob Mattingley. */ 4 | /* CVXGEN, or solvers produced by CVXGEN, cannot be used for commercial */ 5 | /* applications without prior written permission from Jacob Mattingley. */ 6 | 7 | /* Filename: solver.c. */ 8 | /* Description: Main solver file. */ 9 | #include "solver.h" 10 | double eval_gap(void) { 11 | int i; 12 | double gap; 13 | gap = 0; 14 | for (i = 0; i < 64; i++) 15 | gap += work.z[i]*work.s[i]; 16 | return gap; 17 | } 18 | void set_defaults(void) { 19 | settings.resid_tol = 1e-6; 20 | settings.eps = 1e-4; 21 | settings.max_iters = 25; 22 | settings.refine_steps = 1; 23 | settings.s_init = 1; 24 | settings.z_init = 1; 25 | settings.debug = 0; 26 | settings.verbose = 1; 27 | settings.verbose_refinement = 0; 28 | settings.better_start = 1; 29 | settings.kkt_reg = 1e-7; 30 | } 31 | void setup_pointers(void) { 32 | work.y = work.x + 4; 33 | work.s = work.x + 4; 34 | work.z = work.x + 68; 35 | vars.beta = work.x + 0; 36 | } 37 | void setup_indexing(void) { 38 | setup_pointers(); 39 | } 40 | void set_start(void) { 41 | int i; 42 | for (i = 0; i < 4; i++) 43 | work.x[i] = 0; 44 | for (i = 0; i < 0; i++) 45 | work.y[i] = 0; 46 | for (i = 0; i < 64; i++) 47 | work.s[i] = (work.h[i] > 0) ? work.h[i] : settings.s_init; 48 | for (i = 0; i < 64; i++) 49 | work.z[i] = settings.z_init; 50 | } 51 | double eval_objv(void) { 52 | int i; 53 | double objv; 54 | /* Borrow space in work.rhs. */ 55 | multbyP(work.rhs, work.x); 56 | objv = 0; 57 | for (i = 0; i < 4; i++) 58 | objv += work.x[i]*work.rhs[i]; 59 | objv *= 0.5; 60 | for (i = 0; i < 4; i++) 61 | objv += work.q[i]*work.x[i]; 62 | objv += 0; 63 | return objv; 64 | } 65 | void fillrhs_aff(void) { 66 | int i; 67 | double *r1, *r2, *r3, *r4; 68 | r1 = work.rhs; 69 | r2 = work.rhs + 4; 70 | r3 = work.rhs + 68; 71 | r4 = work.rhs + 132; 72 | /* r1 = -A^Ty - G^Tz - Px - q. */ 73 | multbymAT(r1, work.y); 74 | multbymGT(work.buffer, work.z); 75 | for (i = 0; i < 4; i++) 76 | r1[i] += work.buffer[i]; 77 | multbyP(work.buffer, work.x); 78 | for (i = 0; i < 4; i++) 79 | r1[i] -= work.buffer[i] + work.q[i]; 80 | /* r2 = -z. */ 81 | for (i = 0; i < 64; i++) 82 | r2[i] = -work.z[i]; 83 | /* r3 = -Gx - s + h. */ 84 | multbymG(r3, work.x); 85 | for (i = 0; i < 64; i++) 86 | r3[i] += -work.s[i] + work.h[i]; 87 | /* r4 = -Ax + b. */ 88 | multbymA(r4, work.x); 89 | for (i = 0; i < 0; i++) 90 | r4[i] += work.b[i]; 91 | } 92 | void fillrhs_cc(void) { 93 | int i; 94 | double *r2; 95 | double *ds_aff, *dz_aff; 96 | double mu; 97 | double alpha; 98 | double sigma; 99 | double smu; 100 | double minval; 101 | r2 = work.rhs + 4; 102 | ds_aff = work.lhs_aff + 4; 103 | dz_aff = work.lhs_aff + 68; 104 | mu = 0; 105 | for (i = 0; i < 64; i++) 106 | mu += work.s[i]*work.z[i]; 107 | /* Don't finish calculating mu quite yet. */ 108 | /* Find min(min(ds./s), min(dz./z)). */ 109 | minval = 0; 110 | for (i = 0; i < 64; i++) 111 | if (ds_aff[i] < minval*work.s[i]) 112 | minval = ds_aff[i]/work.s[i]; 113 | for (i = 0; i < 64; i++) 114 | if (dz_aff[i] < minval*work.z[i]) 115 | minval = dz_aff[i]/work.z[i]; 116 | /* Find alpha. */ 117 | if (-1 < minval) 118 | alpha = 1; 119 | else 120 | alpha = -1/minval; 121 | sigma = 0; 122 | for (i = 0; i < 64; i++) 123 | sigma += (work.s[i] + alpha*ds_aff[i])* 124 | (work.z[i] + alpha*dz_aff[i]); 125 | sigma /= mu; 126 | sigma = sigma*sigma*sigma; 127 | /* Finish calculating mu now. */ 128 | mu *= 0.015625; 129 | smu = sigma*mu; 130 | /* Fill-in the rhs. */ 131 | for (i = 0; i < 4; i++) 132 | work.rhs[i] = 0; 133 | for (i = 68; i < 132; i++) 134 | work.rhs[i] = 0; 135 | for (i = 0; i < 64; i++) 136 | r2[i] = work.s_inv[i]*(smu - ds_aff[i]*dz_aff[i]); 137 | } 138 | void refine(double *target, double *var) { 139 | int i, j; 140 | double *residual = work.buffer; 141 | double norm2; 142 | double *new_var = work.buffer2; 143 | for (j = 0; j < settings.refine_steps; j++) { 144 | norm2 = 0; 145 | matrix_multiply(residual, var); 146 | for (i = 0; i < 132; i++) { 147 | residual[i] = residual[i] - target[i]; 148 | norm2 += residual[i]*residual[i]; 149 | } 150 | #ifndef ZERO_LIBRARY_MODE 151 | if (settings.verbose_refinement) { 152 | if (j == 0) 153 | printf("Initial residual before refinement has norm squared %.6g.\n", norm2); 154 | else 155 | printf("After refinement we get squared norm %.6g.\n", norm2); 156 | } 157 | #endif 158 | /* Solve to find new_var = KKT \ (target - A*var). */ 159 | ldl_solve(residual, new_var); 160 | /* Update var += new_var, or var += KKT \ (target - A*var). */ 161 | for (i = 0; i < 132; i++) { 162 | var[i] -= new_var[i]; 163 | } 164 | } 165 | #ifndef ZERO_LIBRARY_MODE 166 | if (settings.verbose_refinement) { 167 | /* Check the residual once more, but only if we're reporting it, since */ 168 | /* it's expensive. */ 169 | norm2 = 0; 170 | matrix_multiply(residual, var); 171 | for (i = 0; i < 132; i++) { 172 | residual[i] = residual[i] - target[i]; 173 | norm2 += residual[i]*residual[i]; 174 | } 175 | if (j == 0) 176 | printf("Initial residual before refinement has norm squared %.6g.\n", norm2); 177 | else 178 | printf("After refinement we get squared norm %.6g.\n", norm2); 179 | } 180 | #endif 181 | } 182 | double calc_ineq_resid_squared(void) { 183 | /* Calculates the norm ||-Gx - s + h||. */ 184 | double norm2_squared; 185 | int i; 186 | /* Find -Gx. */ 187 | multbymG(work.buffer, work.x); 188 | /* Add -s + h. */ 189 | for (i = 0; i < 64; i++) 190 | work.buffer[i] += -work.s[i] + work.h[i]; 191 | /* Now find the squared norm. */ 192 | norm2_squared = 0; 193 | for (i = 0; i < 64; i++) 194 | norm2_squared += work.buffer[i]*work.buffer[i]; 195 | return norm2_squared; 196 | } 197 | double calc_eq_resid_squared(void) { 198 | /* Calculates the norm ||-Ax + b||. */ 199 | double norm2_squared; 200 | int i; 201 | /* Find -Ax. */ 202 | multbymA(work.buffer, work.x); 203 | /* Add +b. */ 204 | for (i = 0; i < 0; i++) 205 | work.buffer[i] += work.b[i]; 206 | /* Now find the squared norm. */ 207 | norm2_squared = 0; 208 | for (i = 0; i < 0; i++) 209 | norm2_squared += work.buffer[i]*work.buffer[i]; 210 | return norm2_squared; 211 | } 212 | void better_start(void) { 213 | /* Calculates a better starting point, using a similar approach to CVXOPT. */ 214 | /* Not yet speed optimized. */ 215 | int i; 216 | double *x, *s, *z, *y; 217 | double alpha; 218 | work.block_33[0] = -1; 219 | /* Make sure sinvz is 1 to make hijacked KKT system ok. */ 220 | for (i = 0; i < 64; i++) 221 | work.s_inv_z[i] = 1; 222 | fill_KKT(); 223 | ldl_factor(); 224 | fillrhs_start(); 225 | /* Borrow work.lhs_aff for the solution. */ 226 | ldl_solve(work.rhs, work.lhs_aff); 227 | /* Don't do any refinement for now. Precision doesn't matter too much. */ 228 | x = work.lhs_aff; 229 | s = work.lhs_aff + 4; 230 | z = work.lhs_aff + 68; 231 | y = work.lhs_aff + 132; 232 | /* Just set x and y as is. */ 233 | for (i = 0; i < 4; i++) 234 | work.x[i] = x[i]; 235 | for (i = 0; i < 0; i++) 236 | work.y[i] = y[i]; 237 | /* Now complete the initialization. Start with s. */ 238 | /* Must have alpha > max(z). */ 239 | alpha = -1e99; 240 | for (i = 0; i < 64; i++) 241 | if (alpha < z[i]) 242 | alpha = z[i]; 243 | if (alpha < 0) { 244 | for (i = 0; i < 64; i++) 245 | work.s[i] = -z[i]; 246 | } else { 247 | alpha += 1; 248 | for (i = 0; i < 64; i++) 249 | work.s[i] = -z[i] + alpha; 250 | } 251 | /* Now initialize z. */ 252 | /* Now must have alpha > max(-z). */ 253 | alpha = -1e99; 254 | for (i = 0; i < 64; i++) 255 | if (alpha < -z[i]) 256 | alpha = -z[i]; 257 | if (alpha < 0) { 258 | for (i = 0; i < 64; i++) 259 | work.z[i] = z[i]; 260 | } else { 261 | alpha += 1; 262 | for (i = 0; i < 64; i++) 263 | work.z[i] = z[i] + alpha; 264 | } 265 | } 266 | void fillrhs_start(void) { 267 | /* Fill rhs with (-q, 0, h, b). */ 268 | int i; 269 | double *r1, *r2, *r3, *r4; 270 | r1 = work.rhs; 271 | r2 = work.rhs + 4; 272 | r3 = work.rhs + 68; 273 | r4 = work.rhs + 132; 274 | for (i = 0; i < 4; i++) 275 | r1[i] = -work.q[i]; 276 | for (i = 0; i < 64; i++) 277 | r2[i] = 0; 278 | for (i = 0; i < 64; i++) 279 | r3[i] = work.h[i]; 280 | for (i = 0; i < 0; i++) 281 | r4[i] = work.b[i]; 282 | } 283 | long solve(void) { 284 | int i; 285 | int iter; 286 | double *dx, *ds, *dy, *dz; 287 | double minval; 288 | double alpha; 289 | work.converged = 0; 290 | setup_pointers(); 291 | pre_ops(); 292 | #ifndef ZERO_LIBRARY_MODE 293 | if (settings.verbose) 294 | printf("iter objv gap |Ax-b| |Gx+s-h| step\n"); 295 | #endif 296 | fillq(); 297 | fillh(); 298 | fillb(); 299 | if (settings.better_start) 300 | better_start(); 301 | else 302 | set_start(); 303 | for (iter = 0; iter < settings.max_iters; iter++) { 304 | for (i = 0; i < 64; i++) { 305 | work.s_inv[i] = 1.0 / work.s[i]; 306 | work.s_inv_z[i] = work.s_inv[i]*work.z[i]; 307 | } 308 | work.block_33[0] = 0; 309 | fill_KKT(); 310 | ldl_factor(); 311 | /* Affine scaling directions. */ 312 | fillrhs_aff(); 313 | ldl_solve(work.rhs, work.lhs_aff); 314 | refine(work.rhs, work.lhs_aff); 315 | /* Centering plus corrector directions. */ 316 | fillrhs_cc(); 317 | ldl_solve(work.rhs, work.lhs_cc); 318 | refine(work.rhs, work.lhs_cc); 319 | /* Add the two together and store in aff. */ 320 | for (i = 0; i < 132; i++) 321 | work.lhs_aff[i] += work.lhs_cc[i]; 322 | /* Rename aff to reflect its new meaning. */ 323 | dx = work.lhs_aff; 324 | ds = work.lhs_aff + 4; 325 | dz = work.lhs_aff + 68; 326 | dy = work.lhs_aff + 132; 327 | /* Find min(min(ds./s), min(dz./z)). */ 328 | minval = 0; 329 | for (i = 0; i < 64; i++) 330 | if (ds[i] < minval*work.s[i]) 331 | minval = ds[i]/work.s[i]; 332 | for (i = 0; i < 64; i++) 333 | if (dz[i] < minval*work.z[i]) 334 | minval = dz[i]/work.z[i]; 335 | /* Find alpha. */ 336 | if (-0.99 < minval) 337 | alpha = 1; 338 | else 339 | alpha = -0.99/minval; 340 | /* Update the primal and dual variables. */ 341 | for (i = 0; i < 4; i++) 342 | work.x[i] += alpha*dx[i]; 343 | for (i = 0; i < 64; i++) 344 | work.s[i] += alpha*ds[i]; 345 | for (i = 0; i < 64; i++) 346 | work.z[i] += alpha*dz[i]; 347 | for (i = 0; i < 0; i++) 348 | work.y[i] += alpha*dy[i]; 349 | work.gap = eval_gap(); 350 | work.eq_resid_squared = calc_eq_resid_squared(); 351 | work.ineq_resid_squared = calc_ineq_resid_squared(); 352 | #ifndef ZERO_LIBRARY_MODE 353 | if (settings.verbose) { 354 | work.optval = eval_objv(); 355 | printf("%3d %10.3e %9.2e %9.2e %9.2e % 6.4f\n", 356 | iter+1, work.optval, work.gap, sqrt(work.eq_resid_squared), 357 | sqrt(work.ineq_resid_squared), alpha); 358 | } 359 | #endif 360 | /* Test termination conditions. Requires optimality, and satisfied */ 361 | /* constraints. */ 362 | if ( (work.gap < settings.eps) 363 | && (work.eq_resid_squared <= settings.resid_tol*settings.resid_tol) 364 | && (work.ineq_resid_squared <= settings.resid_tol*settings.resid_tol) 365 | ) { 366 | work.converged = 1; 367 | work.optval = eval_objv(); 368 | return iter+1; 369 | } 370 | } 371 | return iter; 372 | } 373 | --------------------------------------------------------------------------------