├── BFGS ├── Readme.md ├── bfgs.m ├── quasi_newton.m └── weak_wolfe.m ├── L-BFGS ├── CMakeLists.txt ├── Readme.md ├── include │ └── lbfgs.hpp └── src │ └── lbfgs_example.cpp ├── Low-Dimensional QP ├── CMakeLists.txt ├── Readme.md ├── include │ └── sdqp │ │ └── sdqp.hpp └── src │ └── sdqp_example.cpp ├── README.md ├── Seidel's LP ├── CMakeLists.txt ├── Readme.md ├── include │ └── sdlp │ │ └── sdlp.hpp └── src │ └── sdlp_example.cpp ├── Steepest Gradient Descent ├── Readme.md ├── armijo.m └── speedest_gradient.m └── Traj_gen by L-BFGS ├── Readme.md ├── images ├── 1.png └── 2.png └── src ├── LICENSE ├── README.md ├── gcopter ├── CMakeLists.txt ├── config │ ├── curve_gen.yaml │ ├── global_planning.rviz │ └── global_planning.yaml ├── include │ ├── gcopter │ │ ├── cubic_curve.hpp │ │ ├── cubic_spline.hpp │ │ ├── firi.hpp │ │ ├── flatness.hpp │ │ ├── gcopter.hpp │ │ ├── geo_utils.hpp │ │ ├── lbfgs.hpp │ │ ├── minco.hpp │ │ ├── path_smoother.hpp │ │ ├── quickhull.hpp │ │ ├── root_finder.hpp │ │ ├── sdlp.hpp │ │ ├── sfc_gen.hpp │ │ ├── trajectory.hpp │ │ ├── voxel_dilater.hpp │ │ └── voxel_map.hpp │ └── misc │ │ └── visualizer.hpp ├── launch │ ├── curve_gen.launch │ └── global_planning.launch ├── package.xml └── src │ ├── curve_gen.cpp │ └── global_planning.cpp └── map_gen └── mockamap ├── CMakeLists.txt ├── README.md ├── config └── rviz.rviz ├── include ├── maps.hpp └── perlinnoise.hpp ├── launch ├── maze2d.launch ├── maze3d.launch ├── mockamap.launch ├── perlin3d.launch └── post2d.launch ├── package.xml └── src ├── ces_randommap.cpp ├── maps.cpp ├── mockamap.cpp └── perlinnoise.cpp /BFGS/Readme.md: -------------------------------------------------------------------------------- 1 | ### weak_wolfe condition: 2 | 3 | $f({x^k}) - f({x^k} + \alpha d) \ge - c_1 \cdot \alpha {d^T}\nabla f({x^k})$ 4 | 5 | ${d^T}\nabla f({x^k + \alpha d}) \ge c_2 \cdot {d^T}\nabla f({x^k})$ 6 | 7 | ### strong_wolfe condition: 8 | 9 | $f({x^k}) - f({x^k} + \alpha d) \ge - c_1 \cdot \alpha {d^T}\nabla f({x^k})$ 10 | 11 | $\lvert {d^T}\nabla f({x^k + \alpha d}) \rvert \le \lvert c_2 \cdot {d^T}\nabla f({x^k}) \rvert$ 12 | 13 | ### BFGS更新准则 14 | 15 | $B^{k+1}=\left(I-\frac{\Delta x \Delta g^T}{\Delta g^T \Delta x}\right) B^k\left(I-\frac{\Delta g \Delta x^T}{\Delta g^T \Delta x}\right)+\frac{\Delta x \Delta x^T}{\Delta g^T \Delta x}$ 16 | 17 | $B^0=I, \Delta x=x^{k+1}-x^k, \Delta g=\nabla f\left(x^{k+1}\right)-\nabla f\left(x^k\right)$ 18 | 19 | ### Cautious-BFGS 20 | 21 | $if$ 22 | 23 | ${ {\rm{ }}\Delta {g^T}\Delta x > \varepsilon \left\| {{g_k}} \right\|\Delta {x^T}\Delta x,\varepsilon = {{10}^{ - 6}}}$ 24 | 25 | $then,$ 26 | 27 | $B^{k+1}= {\left( {I - \frac{{\Delta x\Delta {g^T}}}{{\Delta {g^T}\Delta x}}} \right){B^k}\left( {I - \frac{{\Delta g\Delta {x^T}}}{{\Delta {g^T}\Delta x}}} \right) + \frac{{\Delta x\Delta {x^T}}}{{\Delta {g^T}\Delta x}}}$ 28 | 29 | $else$ 30 | 31 | $B^{k+1}=B^{k}$ 32 | -------------------------------------------------------------------------------- /BFGS/bfgs.m: -------------------------------------------------------------------------------- 1 | function [Bnew] = bfgs(B,gnew,g0,xnew,x0) 2 | A = eye(2); 3 | deltag = gnew-g0; 4 | deltax = xnew-x0; 5 | Bnew = (A - deltax*deltag'/(deltag'*deltax))*B*(A - deltag*deltax'/(deltag'*deltax)) ... 6 | +deltax*deltax'/(deltag'*deltax); 7 | end -------------------------------------------------------------------------------- /BFGS/quasi_newton.m: -------------------------------------------------------------------------------- 1 | clear; 2 | clc; 3 | 4 | syms x1 x2; 5 | f = (1-x1)^2+(x2-x1^2)^2; 6 | f = matlabFunction(f); 7 | 8 | figure(1); 9 | clf; 10 | fcontour(f,[-1 1.5 -1 1.5]); 11 | axis equal; 12 | hold on 13 | 14 | f_sym = sym(f); 15 | F_td = matlabFunction(gradient(f_sym)); 16 | x0 = [-0.75;-0.5]; 17 | eps = 1e-5; 18 | k = 0; 19 | 20 | B = eye(2); 21 | g0 = F_td(x0(1),x0(2)); 22 | while norm(g0) > eps 23 | d = -B*g0; 24 | [tao_wolfe] = weak_wolfe(x0,d,f,F_td); 25 | xnew = x0 + tao_wolfe*d; 26 | gnew = F_td(xnew(1),xnew(2)); 27 | [Bnew] = bfgs(B,gnew,g0,xnew,x0); 28 | plot([x0(1) xnew(1)],[x0(2) xnew(2)], 'ko-'); 29 | refresh 30 | B = Bnew; 31 | x0 = xnew; 32 | g0 = F_td(x0(1),x0(2)); 33 | k = k+1; 34 | end 35 | x = x0 36 | result = f(x(1),x(2)) -------------------------------------------------------------------------------- /BFGS/weak_wolfe.m: -------------------------------------------------------------------------------- 1 | function [tao_wolfe] = weak_wolfe(x0,d,f,F_td) 2 | tao = 1; %初始值设为1 3 | c1 = 1e-4; 4 | c2 = 0.9; 5 | sigma = 0.5; 6 | j = 1; 7 | while(j>0) 8 | x_new = x0 + tao.*d; 9 | if f(x_new(1),x_new(2)) <= f(x0(1),x0(2)) + c1*tao.*F_td(x0(1),x0(2))'*d ... 10 | && d'*F_td(x_new(1),x_new(2)) >= c2.*d'*F_td(x0(1),x0(2)) 11 | j = 0; 12 | tao_wolfe = tao; 13 | else 14 | tao = sigma*tao; 15 | end 16 | end -------------------------------------------------------------------------------- /L-BFGS/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | 3 | project(lbfgs) 4 | 5 | set(CMAKE_CXX_FLAGS "-std=c++11") 6 | set(CMAKE_BUILD_TYPE "Release") 7 | set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall -fPIC") 8 | 9 | find_package(Eigen3 REQUIRED) 10 | 11 | include_directories( 12 | include 13 | ${EIGEN3_INCLUDE_DIRS} 14 | ) 15 | 16 | add_executable(${PROJECT_NAME} src/lbfgs_example.cpp) 17 | -------------------------------------------------------------------------------- /L-BFGS/Readme.md: -------------------------------------------------------------------------------- 1 | ## 代码内容 2 | 3 | 实现了L-BFGS算法，可以针对nonconvex、nonsmooth函数进行优化 4 | 5 | ### 更多细节 6 | https://github.com/ZJU-FAST-Lab/LBFGS-Lite 7 | 8 | ## 实现流程 9 | 10 | BFGS Update + Cautious Update + Limited Memory Two-Loop Recursion + Lewis-Overton Line Search 11 | -------------------------------------------------------------------------------- /L-BFGS/src/lbfgs_example.cpp: -------------------------------------------------------------------------------- 1 | #include "lbfgs.hpp" 2 | #include 3 | #include 4 | #include 5 | 6 | class MinimizationExample 7 | { 8 | public: 9 | int run(const int N) 10 | { 11 | double finalCost; 12 | Eigen::VectorXd x(N); 13 | 14 | /* Set the initial guess */ 15 | // 设定初值 16 | for (int i = 0; i < N; i += 2) 17 | { 18 | x(i) = -1.2; 19 | x(i + 1) = 1.0; 20 | } 21 | 22 | /* Set the minimization parameters */ 23 | lbfgs::lbfgs_parameter_t params; 24 | params.g_epsilon = 1.0e-8; 25 | params.past = 3; 26 | params.delta = 1.0e-8; 27 | 28 | /* Start minimization */ 29 | int ret = lbfgs::lbfgs_optimize(x, 30 | finalCost, 31 | costFunction, 32 | monitorProgress, 33 | this, 34 | params); 35 | 36 | /* Report the result. */ 37 | std::cout << std::setprecision(4) 38 | << "================================" << std::endl 39 | << "L-BFGS Optimization Returned: " << ret << std::endl 40 | << "Minimized Cost: " << finalCost << std::endl 41 | << "Optimal Variables: " << std::endl 42 | << x.transpose() << std::endl; 43 | 44 | return ret; 45 | } 46 | 47 | private: 48 | static double costFunction(void *instance, 49 | const Eigen::VectorXd &x, 50 | Eigen::VectorXd &g) 51 | { 52 | const int n = x.size(); 53 | double fx = 0.0; 54 | for (int i = 0; i < n; i += 2) 55 | { 56 | const double t1 = 1.0 - x(i); 57 | const double t2 = 10.0 * (x(i + 1) - x(i) * x(i)); 58 | g(i + 1) = 20.0 * t2; 59 | g(i) = -2.0 * (x(i) * g(i + 1) + t1); 60 | fx += t1 * t1 + t2 * t2; 61 | } 62 | return fx; 63 | } 64 | 65 | static int monitorProgress(void *instance, 66 | const Eigen::VectorXd &x, 67 | const Eigen::VectorXd &g, 68 | const double fx, 69 | const double step, 70 | const int k, 71 | const int ls) 72 | { 73 | std::cout << std::setprecision(4) 74 | << "================================" << std::endl 75 | << "Iteration: " << k << std::endl 76 | << "Function Value: " << fx << std::endl 77 | << "Gradient Inf Norm: " << g.cwiseAbs().maxCoeff() << std::endl 78 | << "Variables: " << std::endl 79 | << x.transpose() << std::endl; 80 | return 0; 81 | } 82 | }; 83 | 84 | int main(int argc, char **argv) 85 | { 86 | MinimizationExample example; 87 | return example.run(200); 88 | } 89 | -------------------------------------------------------------------------------- /Low-Dimensional QP/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | 3 | project(sdqp) 4 | 5 | set(CMAKE_CXX_FLAGS "-std=c++11") 6 | set(CMAKE_BUILD_TYPE "Release") 7 | set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall -fPIC") 8 | 9 | find_package(Eigen3 REQUIRED) 10 | 11 | include_directories( 12 | include 13 | ${EIGEN3_INCLUDE_DIRS} 14 | ) 15 | 16 | add_executable(${PROJECT_NAME}_example src/sdqp_example.cpp) 17 | -------------------------------------------------------------------------------- /Low-Dimensional QP/Readme.md: -------------------------------------------------------------------------------- 1 | ## More details 2 | 3 | https://github.com/ZJU-FAST-Lab/SDQP 4 | -------------------------------------------------------------------------------- /Low-Dimensional QP/include/sdqp/sdqp.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | MIT License 3 | 4 | Copyright (c) 2022 Zhepei Wang (wangzhepei@live.com) 5 | 6 | Permission is hereby granted, free of charge, to any person obtaining a copy 7 | of this software and associated documentation files (the "Software"), to deal 8 | in the Software without restriction, including without limitation the rights 9 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | copies of the Software, and to permit persons to whom the Software is 11 | furnished to do so, subject to the following conditions: 12 | 13 | The above copyright notice and this permission notice shall be included in all 14 | copies or substantial portions of the Software. 15 | 16 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | SOFTWARE. 23 | */ 24 | 25 | #ifndef SDQP_HPP 26 | #define SDQP_HPP 27 | 28 | #include 29 | #include 30 | #include 31 | 32 | namespace sdqp 33 | { 34 | constexpr double eps = 1.0e-12; 35 | 36 | enum 37 | { 38 | MINIMUM = 0, 39 | INFEASIBLE, 40 | }; 41 | 42 | template 43 | inline void set_zero(double *x) 44 | { 45 | for (int i = 0; i < d; ++i) 46 | { 47 | x[i] = 0.0; 48 | } 49 | return; 50 | } 51 | 52 | template 53 | inline double dot(const double *x, 54 | const double *y) 55 | { 56 | double s = 0.0; 57 | for (int i = 0; i < d; ++i) 58 | { 59 | s += x[i] * y[i]; 60 | } 61 | return s; 62 | } 63 | 64 | template 65 | inline double sqr_norm(const double *x) 66 | { 67 | double s = 0.0; 68 | for (int i = 0; i < d; ++i) 69 | { 70 | s += x[i] * x[i]; 71 | } 72 | return s; 73 | } 74 | 75 | template 76 | inline void mul(const double *x, 77 | const double s, 78 | double *y) 79 | { 80 | for (int i = 0; i < d; ++i) 81 | { 82 | y[i] = x[i] * s; 83 | } 84 | return; 85 | } 86 | 87 | template 88 | inline int max_abs(const double *x) 89 | { 90 | int id = 0; 91 | double mag = std::fabs(x[0]); 92 | for (int i = 1; i < d; ++i) 93 | { 94 | const double s = std::fabs(x[i]); 95 | if (s > mag) 96 | { 97 | id = i; 98 | mag = s; 99 | } 100 | } 101 | return id; 102 | } 103 | 104 | template 105 | inline void cpy(const double *x, 106 | double *y) 107 | { 108 | for (int i = 0; i < d; ++i) 109 | { 110 | y[i] = x[i]; 111 | } 112 | return; 113 | } 114 | 115 | inline int move_to_front(const int i, 116 | int *next, 117 | int *prev) 118 | { 119 | if (i == 0 || i == next[0]) 120 | { 121 | return i; 122 | } 123 | const int previ = prev[i]; 124 | next[prev[i]] = next[i]; 125 | prev[next[i]] = prev[i]; 126 | next[i] = next[0]; 127 | prev[i] = 0; 128 | prev[next[i]] = i; 129 | next[0] = i; 130 | return previ; 131 | } 132 | 133 | template 134 | inline int min_norm(const double *halves, 135 | const int n, 136 | const int m, 137 | double *opt, 138 | double *work, 139 | int *next, 140 | int *prev) 141 | { 142 | int status = MINIMUM; 143 | set_zero(opt); 144 | if (m <= 0) 145 | { 146 | return status; 147 | } 148 | 149 | double *reflx = work; 150 | double *new_opt = reflx + d; 151 | double *new_halves = new_opt + (d - 1); 152 | double *new_work = new_halves + n * d; 153 | double new_origin[d] = {0.0}; 154 | 155 | for (int i = 0; i != m; i = next[i]) 156 | { 157 | const double *plane_i = halves + (d + 1) * i; 158 | 159 | if (dot(opt, plane_i) + plane_i[d] > (d + 1) * eps) 160 | { 161 | const double s = sqr_norm(plane_i); 162 | 163 | if (s < (d + 1) * eps * eps) 164 | { 165 | return INFEASIBLE; 166 | } 167 | 168 | mul(plane_i, -plane_i[d] / s, new_origin); 169 | 170 | if (i == 0) 171 | { 172 | continue; 173 | } 174 | const int id = max_abs(new_origin); 175 | //////////////////////////////// HOMEWORK START //////////////////////////////// 176 | // 177 | // MISSION TO BE ACCOMPLISHED: 178 | // 179 | // now we know the best solution "opt" violates the i-th halfspace. Therefore, 180 | // please project all previous i halfspaces (from the 0-th to the (i-1)-th one) 181 | // onto the boundary of the i-th halfspace, then store all projected halfspaces 182 | // in the double-type-c-array "new_halves". 183 | // If you successfully complete the mission, the sdqp_example should prints 184 | // optimal sol: 4.11111 9.15556 4.50022 185 | // optimal obj: 201.14 186 | // cons precision: *.********e-16 187 | // This means you obtained the correct exact solution (precision near DBL_EPSILON) 188 | // 189 | // VARIABLES YOU NEED TO ACCESS: 190 | // 191 | // opt is a d-dimensional double-type-c-array 192 | // opt contains an optimal solution that meets all linear constraints from the 0-th 193 | // to the (i-1)-th one, but it is known to violate the i-th halfspace here 194 | // 195 | // new_origin is also a d-dimensional double-type-c-array 196 | // new_origin contains the minimum norm point on the boundary of the i-th plane 197 | // 198 | // you should calculate the vector 'u' of Householder (you can review this concept in 199 | // the course) with the i_d th natural normal Orthogonal basis and store it in the 200 | // Array reflx 201 | 202 | // you can read all previous halfspaces via the iteration below 203 | // 204 | // for (int j = 0; j != i; j = next[j]) 205 | // { 206 | // const double *halfspace = halves + (d + 1) * j; 207 | // // thus the j-th halfspace is the inequality below 208 | // // halfspace[0] * x1 + halfspace[1] * x2 + ... + halfspace[d-1] * xd + halfspace[d] <= 0 209 | // } 210 | // 211 | // you can write or store all your projected halfspaces via the iteration below 212 | // 213 | // for (int j = 0; j != i; j = next[j]) 214 | // { 215 | // double *proj_halfspace = new_halves + d * j; 216 | // // thus the j-th projected halfspace is the inequality below 217 | // // proj_halfspace[0] * y1 + proj_halfspace[1] * y2 + ... + proj_halfspace[d-2] * y(d-1) + proj_halfspace[d-1] <= 0 218 | // // y1 to y(d-1) is the new coordinate constructed on the boundary of the i-th halfspace 219 | // } 220 | // 221 | 222 | // TODO 223 | const double g_norm = std::sqrt(sqr_norm(new_origin)); 224 | // u = g + sgn(g_i)*||g||*e_i 225 | cpy(new_origin, reflx); 226 | if(new_origin[id] < 0.0){ 227 | reflx[id] += -g_norm; 228 | }else{ 229 | reflx[id] += g_norm; 230 | } 231 | const double c = -2.0 / sqr_norm(reflx); 232 | for(int j = 0; j != i; j =next[j]){ 233 | double* new_plane = new_halves + j * d; 234 | const double* old_plane = halves + j * (d + 1); 235 | 236 | const double A_new = c * dot(old_plane, reflx); 237 | for(int k = 0; k < d; k++){ 238 | if(k <= id){ 239 | new_plane[k] = old_plane[k] + reflx[k] * A_new; 240 | }else{ 241 | new_plane[k - 1] = old_plane[k] + reflx[k] * A_new; 242 | } 243 | } 244 | new_plane[d - 1] = dot(new_origin, old_plane) + old_plane[d]; 245 | } 246 | 247 | //////////////////////////////// HOMEWORK END //////////////////////////////// 248 | 249 | status = min_norm(new_halves, n, i, new_opt, new_work, next, prev); 250 | 251 | if (status == INFEASIBLE) 252 | { 253 | return INFEASIBLE; 254 | } 255 | 256 | double coeff = 0.0; 257 | for (int j = 0; j < d; ++j) 258 | { 259 | const int k = j < id ? j : j - 1; 260 | coeff += j != id ? reflx[j] * new_opt[k] : 0.0; 261 | } 262 | coeff *= -2.0 / sqr_norm(reflx); 263 | for (int j = 0; j < d; ++j) 264 | { 265 | const int k = j < id ? j : j - 1; 266 | opt[j] = new_origin[j] += j != id ? new_opt[k] + reflx[j] * coeff : reflx[j] * coeff; 267 | } 268 | 269 | i = move_to_front(i, next, prev); 270 | } 271 | } 272 | 273 | return status; 274 | } 275 | 276 | template <> 277 | inline int min_norm<1>(const double *halves, 278 | const int n, 279 | const int m, 280 | double *opt, 281 | double *work, 282 | int *next, 283 | int *prev) 284 | { 285 | opt[0] = 0.0; 286 | bool l = false; 287 | bool r = false; 288 | 289 | for (int i = 0; i != m; i = next[i]) 290 | { 291 | const double a = halves[2 * i]; 292 | const double b = halves[2 * i + 1]; 293 | if (a * opt[0] + b > 2.0 * eps) 294 | { 295 | if (std::fabs(a) < 2.0 * eps) 296 | { 297 | return INFEASIBLE; 298 | } 299 | 300 | l = l || a < 0.0; 301 | r = r || a > 0.0; 302 | 303 | if (l && r) 304 | { 305 | return INFEASIBLE; 306 | } 307 | 308 | opt[0] = -b / a; 309 | } 310 | } 311 | 312 | return MINIMUM; 313 | } 314 | 315 | inline void rand_permutation(const int n, 316 | int *p) 317 | { 318 | typedef std::uniform_int_distribution rand_int; 319 | typedef rand_int::param_type rand_range; 320 | static std::mt19937_64 gen; 321 | static rand_int rdi(0, 1); 322 | int j, k; 323 | for (int i = 0; i < n; ++i) 324 | { 325 | p[i] = i; 326 | } 327 | for (int i = 0; i < n; ++i) 328 | { 329 | rdi.param(rand_range(0, n - i - 1)); 330 | j = rdi(gen) + i; 331 | k = p[j]; 332 | p[j] = p[i]; 333 | p[i] = k; 334 | } 335 | } 336 | 337 | template 338 | inline double sdmn(const Eigen::Matrix &A, 339 | const Eigen::Matrix &b, 340 | Eigen::Matrix &x) 341 | { 342 | x.setZero(); 343 | const int n = b.size(); 344 | if (n < 1) 345 | { 346 | return 0.0; 347 | } 348 | 349 | Eigen::VectorXi perm(n - 1); 350 | Eigen::VectorXi next(n); 351 | Eigen::VectorXi prev(n + 1); 352 | if (n > 1) 353 | { 354 | rand_permutation(n - 1, perm.data()); 355 | prev(0) = 0; 356 | next(0) = perm(0) + 1; 357 | prev(perm(0) + 1) = 0; 358 | for (int i = 0; i < n - 2; ++i) 359 | { 360 | next(perm(i) + 1) = perm(i + 1) + 1; 361 | prev(perm(i + 1) + 1) = perm(i) + 1; 362 | } 363 | next(perm(n - 2) + 1) = n; 364 | } 365 | else 366 | { 367 | prev(0) = 0; 368 | next(0) = 1; 369 | next(1) = 1; 370 | } 371 | 372 | Eigen::Matrix halves(d + 1, n); 373 | Eigen::VectorXd work((n + 2) * (d + 2) * (d - 1) / 2 + 1 - d); 374 | 375 | const Eigen::VectorXd scale = A.rowwise().norm(); 376 | halves.template topRows() = (A.array().colwise() / scale.array()).transpose(); 377 | halves.template bottomRows<1>() = (-b.array() / scale.array()).transpose(); 378 | 379 | const int status = min_norm(halves.data(), n, n, 380 | x.data(), work.data(), 381 | next.data(), prev.data()); 382 | 383 | double minimum = INFINITY; 384 | if (status != INFEASIBLE) 385 | { 386 | minimum = x.norm(); 387 | } 388 | 389 | return minimum; 390 | } 391 | 392 | /** 393 | * minimize 0.5 x' Q x + c' x 394 | * subject to A x <= b 395 | * Q must be positive definite 396 | **/ 397 | 398 | template 399 | inline double sdqp(const Eigen::Matrix &Q, 400 | const Eigen::Matrix &c, 401 | const Eigen::Matrix &A, 402 | const Eigen::Matrix &b, 403 | Eigen::Matrix &x) 404 | { 405 | Eigen::LLT> llt(Q); 406 | if (llt.info() != Eigen::Success) 407 | { 408 | return INFINITY; 409 | } 410 | 411 | const Eigen::Matrix As = llt.matrixU().template solve(A); 412 | const Eigen::Matrix v = llt.solve(c); 413 | const Eigen::Matrix bs = A * v + b; 414 | 415 | double minimum = sdmn(As, bs, x); 416 | if (!std::isinf(minimum)) 417 | { 418 | llt.matrixU().template solveInPlace(x); 419 | x -= v; 420 | minimum = 0.5 * (Q * x).dot(x) + c.dot(x); 421 | } 422 | 423 | return minimum; 424 | } 425 | 426 | } // namespace sdqp 427 | 428 | #endif 429 | -------------------------------------------------------------------------------- /Low-Dimensional QP/src/sdqp_example.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include "sdqp/sdqp.hpp" 4 | 5 | using namespace std; 6 | using namespace Eigen; 7 | 8 | int main(int argc, char **argv) 9 | { 10 | int m = 7; 11 | Eigen::Matrix Q; 12 | Eigen::Matrix c; 13 | Eigen::Matrix x; // decision variables 14 | Eigen::Matrix A(m, 3); // constraint matrix 15 | Eigen::VectorXd b(m); // constraint bound 16 | 17 | Q << 2.0, 1.0, 1.0, 1.0, 2.0, 1.0, 1.0, 1.0, 2.0; 18 | c << 1.2, 2.5, -10.0; 19 | 20 | A << 1.0, 0.0, 0.0, 21 | 0.0, 1.0, 0.0, 22 | 0.0, 0.0, 1.0, 23 | -0.7, 0.5, 0.0, 24 | 0.5, -1.0, 0.0, 25 | 0.0, 0.13, -1.0, 26 | 0.1, -3.0, -1.3; 27 | b << 10.0, 10.0, 10.0, 1.7, -7.1, -3.31, 2.59; 28 | 29 | double minobj = sdqp::sdqp<3>(Q, c, A, b, x); 30 | 31 | std::cout << "optimal sol: " << x.transpose() << std::endl; 32 | std::cout << "optimal obj: " << minobj << std::endl; 33 | std::cout << "cons precision: " << (A * x - b).maxCoeff() << std::endl; 34 | 35 | return 0; 36 | } 37 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Numerical-Optimization-in-Robotics -------------------------------------------------------------------------------- /Seidel's LP/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | 3 | project(sdlp) 4 | 5 | set(CMAKE_CXX_FLAGS "-std=c++11") 6 | set(CMAKE_BUILD_TYPE "Release") 7 | set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall -fPIC") 8 | 9 | find_package(Eigen3 REQUIRED) 10 | 11 | include_directories( 12 | include 13 | ${EIGEN3_INCLUDE_DIRS} 14 | ) 15 | 16 | add_executable(${PROJECT_NAME}_example src/sdlp_example.cpp) 17 | -------------------------------------------------------------------------------- /Seidel's LP/Readme.md: -------------------------------------------------------------------------------- 1 | ## Seidel's LP Algorithm 2 | 3 | 通过迭代的思想，把多维问题不停地降维直至一维（选用高斯消元法）。 4 | 5 | 选择降维对象时，选择系数绝对值最大的那一项，这样可以尽可能减少因为降维带来的信息损失（类似LU分解）。 6 | 7 | ## More details 8 | 9 | https://github.com/ZJU-FAST-Lab/SDLP 10 | -------------------------------------------------------------------------------- /Seidel's LP/src/sdlp_example.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include "sdlp/sdlp.hpp" 4 | 5 | using namespace std; 6 | using namespace Eigen; 7 | 8 | int main(int argc, char **argv) 9 | { 10 | int m = 2 * 7; 11 | Eigen::Matrix x; // decision variables 12 | Eigen::Matrix c; // objective coefficients 13 | Eigen::Matrix A(m, 7); // constraint matrix 14 | Eigen::VectorXd b(m); // constraint bound 15 | 16 | c << 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0; 17 | A << 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 18 | 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 19 | 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 20 | 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 21 | 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 22 | 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 23 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 24 | -1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 25 | 0.0, -1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 26 | 0.0, 0.0, -1.0, 0.0, 0.0, 0.0, 0.0, 27 | 0.0, 0.0, 0.0, -1.0, 0.0, 0.0, 0.0, 28 | 0.0, 0.0, 0.0, 0.0, -1.0, 0.0, 0.0, 29 | 0.0, 0.0, 0.0, 0.0, 0.0, -1.0, 0.0, 30 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -1.0; 31 | b << 6.0, 5.0, 4.0, 3.0, 2.0, 1.0, 0.0, 1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0; 32 | 33 | double minobj = sdlp::linprog<7>(c, A, b, x); 34 | 35 | std::cout << "prob:\n" 36 | << std::endl; 37 | std::cout << " min x1 + ... + x7," << std::endl; 38 | std::cout << " s.t. x1 <= 6, x2 <= 5, ..., x7 <= 0," << std::endl; 39 | std::cout << " x1 >= -1, x2 >= -2, ..., x7 >= -7.\n" 40 | << std::endl; 41 | std::cout << "optimal sol: " << x.transpose() << std::endl; 42 | std::cout << "optimal obj: " << minobj << std::endl; 43 | 44 | return 0; 45 | } -------------------------------------------------------------------------------- /Steepest Gradient Descent/Readme.md: -------------------------------------------------------------------------------- 1 | 使用armijo condition实现最速下降 2 | 3 | 相比于常规的最速下降，运行的迭代次数会增加，但是每次迭代所需的时间是会大大降低的 4 | 5 | 通常的梯度下降法中，步长更新算法为： 6 | 7 | $\tau = \mathop {\arg \min }\limits_\alpha f({x^k} + \alpha d)$ 8 | 9 | 使用armijo condition的最速梯度下降法，步长更新算法为： 10 | 11 | $\tau \in \lbrace\{ {\alpha |f({x^k}) - f({x^k} + \alpha d) \ge - c \cdot \alpha {d^T}\nabla f({x^k})} \rbrace\}$ 12 | -------------------------------------------------------------------------------- /Steepest Gradient Descent/armijo.m: -------------------------------------------------------------------------------- 1 | function [tao_armijo] = armijo(x0,d,f,F_td) 2 | tao = 1; %初始值设为1 3 | c = 0.4; 4 | sigma = 0.5; 5 | j = 1; 6 | while(j>0) 7 | x_new = x0 + tao.*d; 8 | if f(x_new(1),x_new(2)) <= f(x0(1),x0(2)) + c*tao.*F_td(x0(1),x0(2))'*d 9 | j = 0; 10 | tao_armijo = tao; 11 | else 12 | tao = sigma*tao; 13 | end 14 | end 15 | 16 | -------------------------------------------------------------------------------- /Steepest Gradient Descent/speedest_gradient.m: -------------------------------------------------------------------------------- 1 | clear; 2 | clc; 3 | 4 | syms x1 x2; 5 | f = 100*(x2-x1^2)^2+(1-x1)^2; 6 | f = matlabFunction(f); 7 | 8 | figure(1); 9 | clf; 10 | fcontour(f,[-2 2 -2 2]); 11 | axis equal; 12 | hold on 13 | 14 | f_sym = sym(f); 15 | F_td = matlabFunction(gradient(f_sym)); 16 | x0 = [-0.75;1]; 17 | eps = 1e-5; 18 | k = 0; 19 | 20 | f_td = F_td(x0(1),x0(2)); 21 | while norm(f_td) > eps 22 | d = -f_td; 23 | [tao_armijo] = armijo(x0,d,f,F_td); 24 | xnew = x0 + tao_armijo*d; 25 | plot([x0(1) xnew(1)],[x0(2) xnew(2)], 'ko-'); 26 | % refresh 27 | x0 = xnew; 28 | f_td = F_td(x0(1),x0(2)); 29 | k = k+1; 30 | end 31 | 32 | x = x0 33 | result = f(x(1),x(2)) 34 | -------------------------------------------------------------------------------- /Traj_gen by L-BFGS/Readme.md: -------------------------------------------------------------------------------- 1 | # 基于L-BFGS的路径生成 2 | 3 | ## 主要代码 4 | 5 | gcopter/include/gcopter/cubic_spline.hpp 6 | 7 | gcopter/include/gcopter/path_smoother.hpp 8 | 9 | ## 生成结果 10 | 11 | ![image](https://github.com/Rao-Kai/Numerical-Optimization-in-Robotics/blob/main/Traj_gen%20by%20L-BFGS/images/1.png) 12 | ![image](https://github.com/Rao-Kai/Numerical-Optimization-in-Robotics/blob/main/Traj_gen%20by%20L-BFGS/images/2.png) 13 | -------------------------------------------------------------------------------- /Traj_gen by L-BFGS/images/1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Rao-Kai/Numerical-Optimization-in-Robotics/639e67181bd1a11f637d3e63e5a8206e653d857b/Traj_gen by L-BFGS/images/1.png -------------------------------------------------------------------------------- /Traj_gen by L-BFGS/images/2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Rao-Kai/Numerical-Optimization-in-Robotics/639e67181bd1a11f637d3e63e5a8206e653d857b/Traj_gen by L-BFGS/images/2.png -------------------------------------------------------------------------------- /Traj_gen by L-BFGS/src/LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright(c) 2021 Zhepei Wang (wangzhepei@live.com) and Fei Gao (fgaoaa@zju.edu.cn) 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /Traj_gen by L-BFGS/src/README.md: -------------------------------------------------------------------------------- 1 | # GCOPTER 2 | 3 | __GCOPTER__ is an efficient and versatile multicopter trajectory optimizer built upon a novel sparse trajectory representation named [__MINCO__](https://arxiv.org/pdf/2103.00190.pdf). __User-defined state-input constraints__ for dynamics involving [__nonlinear drag effects__](https://github.com/ZJU-FAST-Lab/GCOPTER/blob/main/misc/flatness.pdf) are supported. 4 | 5 | ## Updates 6 | 7 | * **Mar 11, 2022** - A minimal but non-trivial example for global kinodynamic planning is released. Modules for trajectory optimization, quadcopter dynamics with nonlinear drags, [fast iterative region inflation for corridor generation](https://github.com/ZJU-FAST-Lab/GCOPTER/blob/main/gcopter/include/gcopter/firi.hpp), non-uniform MINCO (s=3), etc., are released. 8 | 9 | * **Mar 15, 2022** - Released non-uniform MINCO for s=2 and s=4. 10 | 11 | * **May 19, 2022** - Released a doc to detail [differential flatness for multicopters under nonlinear drag effects](https://github.com/ZJU-FAST-Lab/GCOPTER/blob/main/misc/flatness.pdf). Add code links for all projects powered by MINCO. 12 | 13 | * **Plan** - __More examples are on the way__, including uniform MINCO (s=2,3,4), trajectory generation for tube-shaped and sphere-shaped corridors, local replanner, whole-body SE(3) planner, interfaces for external constraints, augmented Lagrangian, and so on. 14 | 15 | ## About 16 | 17 | If our repo helps your academic projects, please cite our paper. Thank you! 18 | 19 | __Author__: [Zhepei Wang](https://zhepeiwang.github.io) and [Fei Gao](https://scholar.google.com/citations?hl=en&user=4RObDv0AAAAJ) from [ZJU FAST Lab](http://zju-fast.com). 20 | 21 | __Paper__: [Geometrically Constrained Trajectory Optimization for Multicopters](https://arxiv.org/abs/2103.00190), Zhepei Wang, Xin Zhou, Chao Xu, and Fei Gao, [IEEE Transactions on Robotics](https://doi.org/10.1109/TRO.2022.3160022) (__T-RO__), Regular Paper. 22 | ``` 23 | @article{WANG2022GCOPTER, 24 | title={Geometrically Constrained Trajectory Optimization for Multicopters}, 25 | author={Wang, Zhepei and Zhou, Xin and Xu, Chao and Gao, Fei}, 26 | journal={IEEE Transactions on Robotics}, 27 | year={2022}, 28 | volume={}, 29 | number={}, 30 | pages={1-20}, 31 | doi={10.1109/TRO.2022.3160022} 32 | } 33 | ``` 34 | 35 | ## Applications 36 | 37 | ### Example 1: Global Trajectory Planning 38 | 39 | This is a minimal yet non-trivial example of our trajectory optimizer for real-time high-quality corridor and global trajectory generation subject to dynamic constraints. For installation, the following terminal commands are helpful. 40 | 41 | sudo apt update 42 | sudo apt install cpufrequtils 43 | sudo apt install libompl-dev 44 | sudo cpufreq-set -g performance 45 | mkdir ROS; cd ROS; mkdir src; cd src 46 | git clone https://github.com/ZJU-FAST-Lab/GCOPTER.git 47 | cd .. 48 | catkin_make 49 | source devel/setup.bash 50 | roslaunch gcopter global_planning.launch 51 | 52 | After conduct the command, you will see the windows for rviz and rqt_plot. Please follow the gif below for global trajectory planning in a random map. 53 |

54 | 55 |

56 | The angle between the arrow of 2D Nav Goal and positive x-axis (red axis) decides the relative height. You can repeat choosing the start and goal to trigger the global planning. The solution trajectory considers spatial-temporal optimality and vehicle dynamics with drag effects. Some states for trajectories, like net thrust, tilt angle, body rate are all available. The magnitudes for some of them are shown in the rqt_plot. Corridor and trajectory generation are computed in real-time. Physical parameters in standard units are all modifiable in a config file. If you only wants a point-mass model to achieve a faster computing, please modify the penalty-functional-relevant code. 57 | 58 | ## Projects Supported by GCOPTER or MINCO 59 | 60 | - Robust Real-Time SE(3) Planning: [youtube](https://www.youtube.com/watch?v=pQ4oSf1rdBU) or [bilibili](https://www.bilibili.com/video/BV1bb4y1X7VE/). (__Reported by [IEEE Spectrum Website](https://spectrum.ieee.org/)!__) 61 | 62 |

63 | 64 |

65 | 66 | 67 | - High-Speed FPV Flight Planning: [youtube](https://www.youtube.com/watch?v=QQS0AM3iOmc) or [bilibili](https://www.bilibili.com/video/BV1pq4y1z7Jp). 68 | 69 |

70 | 71 |

72 | 73 | 74 | - Multicopter Swarms Planning: [youtube](https://www.youtube.com/watch?v=w5GDMpjAoVQ) or [bilibili](https://www.bilibili.com/video/BV1gK4y1g7F7). (__Reported by [IEEE Spectrum Website](https://spectrum.ieee.org/)!__) [Code](https://github.com/ZJU-FAST-Lab/EGO-Planner-v2) 75 | 76 |

77 | 78 |

79 | 80 | 81 | - Long-Distance Drone Racing Planning: [youtube](https://www.youtube.com/watch?v=oIqtN3zWIhM) or [bilibili](https://www.bilibili.com/video/BV1sq4y1779e). (__Published in [IEEE RA-L](https://ieeexplore.ieee.org/document/9543598)__) [Code](https://github.com/ZJU-FAST-Lab/Fast-Racing) 82 | 83 |

84 | 85 |

86 | 87 | 88 | - Gaze Teleoperation Planning: [youtube](https://www.youtube.com/watch?v=WYujLePQwB8) or [bilibili](https://www.bilibili.com/video/BV1Yf4y1P74v). (__Published in [IEEE RA-L](https://doi.org/10.1109/LRA.2022.3153898)__) [Code](https://github.com/ZJU-FAST-Lab/GPA-Teleoperation) 89 | 90 |

91 | 92 |

93 | 94 | 95 | - Formation Keeping Planning: [youtube](https://www.youtube.com/watch?v=lFumt0rJci4) or [bilibili](https://www.bilibili.com/video/BV1qv41137Si). (IEEE ICRA) [Code](https://github.com/ZJU-FAST-Lab/Swarm-Formation) 96 | 97 |

98 | 99 |

100 | 101 | 102 | - A variety of applications powered by __GCOPTER__ or __MINCO__ are not listed here, such as [visibility-aware aerial tracking](https://arxiv.org/abs/2109.07111), and [planning with nonlinear drag effects](https://arxiv.org/abs/2109.08403), etc. 103 | 104 | ## Powerful Submodules 105 | - [SDLP: Seidel's Algorithm](https://github.com/ZJU-FAST-Lab/SDLP) on Linear-Complexity Linear Programming for Computational Geometry. 106 | - [VertexEnumeration3D](https://github.com/ZJU-FAST-Lab/VertexEnumeration3D): Highly Efficient Vertex Enumeration for 3D Convex Polytopes (Outperforms [cddlib](https://github.com/cddlib/cddlib) in 3D). 107 | - [LBFGS-Lite](https://github.com/ZJU-FAST-Lab/LBFGS-Lite): An Easy-to-Use Header-Only L-BFGS Solver. 108 | -------------------------------------------------------------------------------- /Traj_gen by L-BFGS/src/gcopter/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | 3 | project(gcopter) 4 | 5 | set(CMAKE_CXX_FLAGS "-std=c++14") 6 | set(CMAKE_BUILD_TYPE "Release") 7 | set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall -fPIC") 8 | 9 | find_package(Eigen3 REQUIRED) 10 | find_package(ompl REQUIRED) 11 | 12 | find_package(catkin REQUIRED COMPONENTS 13 | roscpp 14 | std_msgs 15 | geometry_msgs 16 | sensor_msgs 17 | visualization_msgs 18 | ) 19 | 20 | include_directories( 21 | ${catkin_INCLUDE_DIRS} 22 | ${OMPL_INCLUDE_DIRS} 23 | ${EIGEN3_INCLUDE_DIRS} 24 | include 25 | ) 26 | 27 | catkin_package() 28 | 29 | add_executable(curve_gen src/curve_gen.cpp) 30 | 31 | target_link_libraries(curve_gen 32 | ${OMPL_LIBRARIES} 33 | ${catkin_LIBRARIES} 34 | ) 35 | -------------------------------------------------------------------------------- /Traj_gen by L-BFGS/src/gcopter/config/curve_gen.yaml: -------------------------------------------------------------------------------- 1 | TargetTopic: '/move_base_simple/goal' 2 | 3 | PenaltyWeight: 100.0 4 | 5 | CircleObs: [ 0.0, 0.0, 4.0, 6 | 1.0, 8.0, 3.0, 7 | -8.0, 1.0, 2.0, 8 | -5.0, 16.0, 5.0, 9 | 13.0, 2.0, 6.0, 10 | -15.0, -12.0, 5.0, 11 | -18.0, 6.0, 8.0, 12 | 12.0, -15.0, 4.0, 13 | 20.0, 19.0, 4.5, 14 | -3.0, -18.0, 6.5] 15 | 16 | PieceLength: 0.5 17 | 18 | RelCostTol: 1.0e-6 19 | -------------------------------------------------------------------------------- /Traj_gen by L-BFGS/src/gcopter/config/global_planning.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 0 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /GlobalMap1/GlobalMap1 8 | - /GlobalMap1/GlobalMap1/Autocompute Value Bounds1 9 | Splitter Ratio: 0.5472221970558167 10 | Tree Height: 1005 11 | - Class: rviz/Selection 12 | Name: Selection 13 | - Class: rviz/Tool Properties 14 | Expanded: 15 | - /Interact1 16 | - /2D Pose Estimate1 17 | - /2D Nav Goal1 18 | - /Publish Point1 19 | Name: Tool Properties 20 | Splitter Ratio: 0.5886790156364441 21 | - Class: rviz/Views 22 | Expanded: 23 | - /Current View1 24 | Name: Views 25 | Splitter Ratio: 0.5 26 | - Class: rviz/Time 27 | Experimental: false 28 | Name: Time 29 | SyncMode: 0 30 | SyncSource: "" 31 | - Class: rviz/Displays 32 | Help Height: 78 33 | Name: Displays 34 | Property Tree Widget: 35 | Expanded: 36 | - /GlobalMap1 37 | - /GlobalMap1/GlobalMap1 38 | - /GlobalMap1/GlobalMap1/Autocompute Value Bounds1 39 | Splitter Ratio: 0.5 40 | Tree Height: 363 41 | - Class: rviz/Tool Properties 42 | Expanded: ~ 43 | Name: Tool Properties 44 | Splitter Ratio: 0.6555560231208801 45 | - Class: rviz/Views 46 | Expanded: 47 | - /Current View1 48 | Name: Views 49 | Splitter Ratio: 0.5 50 | - Class: rviz/Tool Properties 51 | Expanded: ~ 52 | Name: Tool Properties 53 | Splitter Ratio: 0.5 54 | - Class: rviz/Views 55 | Expanded: 56 | - /Current View1 57 | Name: Views 58 | Splitter Ratio: 0.5 59 | Preferences: 60 | PromptSaveOnExit: true 61 | Toolbars: 62 | toolButtonStyle: 2 63 | Visualization Manager: 64 | Class: "" 65 | Displays: 66 | - Alpha: 1 67 | Cell Size: 1 68 | Class: rviz/Grid 69 | Color: 160; 160; 164 70 | Enabled: true 71 | Line Style: 72 | Line Width: 0.029999999329447746 73 | Value: Lines 74 | Name: Grid 75 | Normal Cell Count: 0 76 | Offset: 77 | X: 0 78 | Y: 0 79 | Z: 0 80 | Plane: XY 81 | Plane Cell Count: 50 82 | Reference Frame: 83 | Value: true 84 | - Alpha: 1 85 | Class: rviz/Axes 86 | Enabled: true 87 | Length: 3 88 | Name: Axes 89 | Radius: 0.30000001192092896 90 | Reference Frame: 91 | Value: true 92 | - Class: rviz/Group 93 | Displays: 94 | - Alpha: 1 95 | Autocompute Intensity Bounds: true 96 | Autocompute Value Bounds: 97 | Max Value: 4.75 98 | Min Value: 0 99 | Value: true 100 | Axis: Z 101 | Channel Name: intensity 102 | Class: rviz/PointCloud2 103 | Color: 255; 255; 255 104 | Color Transformer: AxisColor 105 | Decay Time: 0 106 | Enabled: true 107 | Invert Rainbow: false 108 | Max Color: 255; 255; 255 109 | Min Color: 0; 0; 0 110 | Name: GlobalMap 111 | Position Transformer: XYZ 112 | Queue Size: 10 113 | Selectable: false 114 | Size (Pixels): 3 115 | Size (m): 0.25 116 | Style: Boxes 117 | Topic: /voxel_map 118 | Unreliable: true 119 | Use Fixed Frame: true 120 | Use rainbow: true 121 | Value: true 122 | Enabled: true 123 | Name: GlobalMap 124 | - Class: rviz/Marker 125 | Enabled: true 126 | Marker Topic: /visualizer/waypoints 127 | Name: Marker 128 | Namespaces: 129 | {} 130 | Queue Size: 100 131 | Value: true 132 | - Class: rviz/Marker 133 | Enabled: true 134 | Marker Topic: /visualizer/route 135 | Name: Marker 136 | Namespaces: 137 | {} 138 | Queue Size: 100 139 | Value: true 140 | - Class: rviz/Marker 141 | Enabled: true 142 | Marker Topic: /visualizer/mesh 143 | Name: Marker 144 | Namespaces: 145 | {} 146 | Queue Size: 100 147 | Value: true 148 | - Class: rviz/Marker 149 | Enabled: true 150 | Marker Topic: /visualizer/edge 151 | Name: Marker 152 | Namespaces: 153 | {} 154 | Queue Size: 100 155 | Value: true 156 | - Class: rviz/Marker 157 | Enabled: true 158 | Marker Topic: /visualizer/spheres 159 | Name: Marker 160 | Namespaces: 161 | {} 162 | Queue Size: 100 163 | Value: true 164 | - Class: rviz/Marker 165 | Enabled: true 166 | Marker Topic: /visualizer/trajectory 167 | Name: Marker 168 | Namespaces: 169 | {} 170 | Queue Size: 100 171 | Value: true 172 | - Class: rviz/MarkerArray 173 | Enabled: true 174 | Marker Topic: /visualizer/disks 175 | Name: MarkerArray 176 | Namespaces: 177 | {} 178 | Queue Size: 100 179 | Value: true 180 | Enabled: true 181 | Global Options: 182 | Background Color: 235; 235; 235 183 | Default Light: true 184 | Fixed Frame: odom 185 | Frame Rate: 60 186 | Name: root 187 | Tools: 188 | - Class: rviz/Interact 189 | Hide Inactive Objects: true 190 | - Class: rviz/MoveCamera 191 | - Class: rviz/Select 192 | - Class: rviz/FocusCamera 193 | - Class: rviz/Measure 194 | - Class: rviz/SetInitialPose 195 | Theta std deviation: 0.2617993950843811 196 | Topic: /initialpose 197 | X std deviation: 0.5 198 | Y std deviation: 0.5 199 | - Class: rviz/SetGoal 200 | Topic: /move_base_simple/goal 201 | - Class: rviz/PublishPoint 202 | Single click: true 203 | Topic: /clicked_point 204 | Value: true 205 | Views: 206 | Current: 207 | Class: rviz/Orbit 208 | Distance: 62.165191650390625 209 | Enable Stereo Rendering: 210 | Stereo Eye Separation: 0.05999999865889549 211 | Stereo Focal Distance: 1 212 | Swap Stereo Eyes: false 213 | Value: false 214 | Field of View: 0.7853981852531433 215 | Focal Point: 216 | X: -2.1948978900909424 217 | Y: 10.723564147949219 218 | Z: 1.8768043518066406 219 | Focal Shape Fixed Size: false 220 | Focal Shape Size: 0.05000000074505806 221 | Invert Z Axis: false 222 | Name: Current View 223 | Near Clip Distance: 0.009999999776482582 224 | Pitch: 1.2297966480255127 225 | Target Frame: drone1 226 | Yaw: 2.5316662788391113 227 | Saved: 228 | - Class: rviz/ThirdPersonFollower 229 | Distance: 4.048277378082275 230 | Enable Stereo Rendering: 231 | Stereo Eye Separation: 0.05999999865889549 232 | Stereo Focal Distance: 1 233 | Swap Stereo Eyes: false 234 | Value: false 235 | Field of View: 0.7853981852531433 236 | Focal Point: 237 | X: 3.297011613845825 238 | Y: -0.13499683141708374 239 | Z: -1.0989770889282227 240 | Focal Shape Fixed Size: true 241 | Focal Shape Size: 0.05000000074505806 242 | Invert Z Axis: false 243 | Name: ThirdPersonFollower 244 | Near Clip Distance: 0.009999999776482582 245 | Pitch: 0.15354639291763306 246 | Target Frame: drone1 247 | Yaw: 3.1068692207336426 248 | - Class: rviz/Orbit 249 | Distance: 14.031325340270996 250 | Enable Stereo Rendering: 251 | Stereo Eye Separation: 0.05999999865889549 252 | Stereo Focal Distance: 1 253 | Swap Stereo Eyes: false 254 | Value: false 255 | Field of View: 0.7853981852531433 256 | Focal Point: 257 | X: -7.736832141876221 258 | Y: -7.348899841308594 259 | Z: 0.8977007865905762 260 | Focal Shape Fixed Size: false 261 | Focal Shape Size: 0.05000000074505806 262 | Invert Z Axis: false 263 | Name: Orbit 264 | Near Clip Distance: 0.009999999776482582 265 | Pitch: 1.0697968006134033 266 | Target Frame: map 267 | Yaw: 4.538923740386963 268 | - Class: rviz/Orbit 269 | Distance: 13.27771282196045 270 | Enable Stereo Rendering: 271 | Stereo Eye Separation: 0.05999999865889549 272 | Stereo Focal Distance: 1 273 | Swap Stereo Eyes: false 274 | Value: false 275 | Field of View: 0.7853981852531433 276 | Focal Point: 277 | X: -9.282596588134766 278 | Y: -8.473143577575684 279 | Z: 1.7744790315628052 280 | Focal Shape Fixed Size: true 281 | Focal Shape Size: 0.05000000074505806 282 | Invert Z Axis: false 283 | Name: Orbit 284 | Near Clip Distance: 0.009999999776482582 285 | Pitch: 1.1547964811325073 286 | Target Frame: map 287 | Yaw: 6.054520606994629 288 | - Class: rviz/ThirdPersonFollower 289 | Distance: 10.391033172607422 290 | Enable Stereo Rendering: 291 | Stereo Eye Separation: 0.05999999865889549 292 | Stereo Focal Distance: 1 293 | Swap Stereo Eyes: false 294 | Value: false 295 | Field of View: 0.7853981852531433 296 | Focal Point: 297 | X: 1.0356316566467285 298 | Y: -0.4885826110839844 299 | Z: -0.00026488304138183594 300 | Focal Shape Fixed Size: true 301 | Focal Shape Size: 0.05000000074505806 302 | Invert Z Axis: false 303 | Name: ThirdPersonFollower 304 | Near Clip Distance: 0.009999999776482582 305 | Pitch: 0.6947967410087585 306 | Target Frame: drone1 307 | Yaw: 2.7186548709869385 308 | - Class: rviz/ThirdPersonFollower 309 | Distance: 22.49183464050293 310 | Enable Stereo Rendering: 311 | Stereo Eye Separation: 0.05999999865889549 312 | Stereo Focal Distance: 1 313 | Swap Stereo Eyes: false 314 | Value: false 315 | Field of View: 0.7853981852531433 316 | Focal Point: 317 | X: 0.4420492649078369 318 | Y: 1.1208577156066895 319 | Z: 1.5976747818058357e-06 320 | Focal Shape Fixed Size: true 321 | Focal Shape Size: 0.05000000074505806 322 | Invert Z Axis: false 323 | Name: ThirdPersonFollower 324 | Near Clip Distance: 0.009999999776482582 325 | Pitch: 1.0247968435287476 326 | Target Frame: drone1 327 | Yaw: 3.1827449798583984 328 | - Class: rviz/ThirdPersonFollower 329 | Distance: 22.4423770904541 330 | Enable Stereo Rendering: 331 | Stereo Eye Separation: 0.05999999865889549 332 | Stereo Focal Distance: 1 333 | Swap Stereo Eyes: false 334 | Value: false 335 | Field of View: 0.7853981852531433 336 | Focal Point: 337 | X: 3.6197400093078613 338 | Y: 0.5623701810836792 339 | Z: -3.2186508178710938e-06 340 | Focal Shape Fixed Size: true 341 | Focal Shape Size: 0.05000000074505806 342 | Invert Z Axis: false 343 | Name: ThirdPersonFollower 344 | Near Clip Distance: 0.009999999776482582 345 | Pitch: 0.8597970008850098 346 | Target Frame: drone1 347 | Yaw: 3.265840530395508 348 | Window Geometry: 349 | Displays: 350 | collapsed: false 351 | Height: 1156 352 | Hide Left Dock: false 353 | Hide Right Dock: false 354 | QMainWindow State: 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 355 | Selection: 356 | collapsed: false 357 | Time: 358 | collapsed: false 359 | Tool Properties: 360 | collapsed: false 361 | Views: 362 | collapsed: false 363 | Width: 1902 364 | X: 433 365 | Y: 83 366 | -------------------------------------------------------------------------------- /Traj_gen by L-BFGS/src/gcopter/config/global_planning.yaml: -------------------------------------------------------------------------------- 1 | MapTopic: '/voxel_map' 2 | 3 | TargetTopic: '/move_base_simple/goal' 4 | 5 | DilateRadius: 0.5 6 | 7 | VoxelWidth: 0.25 8 | 9 | MapBound: [-25.0, 25.0, -25.0, 25.0, 0.0, 5.0] 10 | 11 | TimeoutRRT: 0.02 12 | 13 | MaxVelMag: 4.0 14 | 15 | MaxBdrMag: 2.1 16 | 17 | MaxTiltAngle: 1.05 18 | 19 | MinThrust: 2.0 20 | 21 | MaxThrust: 12.0 22 | 23 | VehicleMass: 0.61 24 | 25 | GravAcc: 9.8 26 | 27 | HorizDrag: 0.70 28 | 29 | VertDrag: 0.80 30 | 31 | ParasDrag: 0.01 32 | 33 | SpeedEps: 0.0001 34 | 35 | WeightT: 20.0 36 | 37 | ChiVec: [1.0e+4, 1.0e+4, 1.0e+4, 1.0e+4, 1.0e+5] 38 | 39 | SmoothingEps: 1.0e-2 40 | 41 | IntegralIntervs: 16 42 | 43 | RelCostTol: 1.0e-5 44 | -------------------------------------------------------------------------------- /Traj_gen by L-BFGS/src/gcopter/include/gcopter/cubic_curve.hpp: -------------------------------------------------------------------------------- 1 | #ifndef CUBIC_CURVE_HPP 2 | #define CUBIC_CURVE_HPP 3 | 4 | #include 5 | 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | class CubicPolynomial 12 | { 13 | private: 14 | double duration; 15 | Eigen::Matrix coeffMat; 16 | 17 | public: 18 | CubicPolynomial() = default; 19 | 20 | CubicPolynomial(double dur, const Eigen::Matrix &cMat) 21 | : duration(dur), coeffMat(cMat) {} 22 | 23 | inline int getDim() const 24 | { 25 | return 2; 26 | } 27 | 28 | inline int getDegree() const 29 | { 30 | return 3; 31 | } 32 | 33 | inline double getDuration() const 34 | { 35 | return duration; 36 | } 37 | 38 | inline const Eigen::Matrix &getCoeffMat() const 39 | { 40 | return coeffMat; 41 | } 42 | 43 | inline Eigen::Vector2d getPos(const double &t) const 44 | { 45 | return coeffMat.col(3) + t * (coeffMat.col(2) + t * (coeffMat.col(1) + t * coeffMat.col(0))); 46 | } 47 | }; 48 | 49 | class CubicCurve 50 | { 51 | private: 52 | typedef std::vector Pieces; 53 | Pieces pieces; 54 | 55 | public: 56 | CubicCurve() = default; 57 | 58 | CubicCurve(const std::vector &durs, 59 | const std::vector> &cMats) 60 | { 61 | const int N = std::min(durs.size(), cMats.size()); 62 | pieces.reserve(N); 63 | for (int i = 0; i < N; ++i) 64 | { 65 | pieces.emplace_back(durs[i], cMats[i]); 66 | } 67 | } 68 | 69 | inline int getPieceNum() const 70 | { 71 | return pieces.size(); 72 | } 73 | 74 | inline Eigen::VectorXd getDurations() const 75 | { 76 | const int N = getPieceNum(); 77 | Eigen::VectorXd durations(N); 78 | for (int i = 0; i < N; ++i) 79 | { 80 | durations(i) = pieces[i].getDuration(); 81 | } 82 | return durations; 83 | } 84 | 85 | inline double getTotalDuration() const 86 | { 87 | const int N = getPieceNum(); 88 | double totalDuration = 0.0; 89 | for (int i = 0; i < N; ++i) 90 | { 91 | totalDuration += pieces[i].getDuration(); 92 | } 93 | return totalDuration; 94 | } 95 | 96 | inline Eigen::Matrix2Xd getPositions() const 97 | { 98 | const int N = getPieceNum(); 99 | Eigen::Matrix2Xd positions(2, N + 1); 100 | for (int i = 0; i < N; ++i) 101 | { 102 | positions.col(i) = pieces[i].getCoeffMat().col(3); 103 | } 104 | positions.col(N) = pieces[N - 1].getPos(pieces[N - 1].getDuration()); 105 | return positions; 106 | } 107 | 108 | inline const CubicPolynomial &operator[](int i) const 109 | { 110 | return pieces[i]; 111 | } 112 | 113 | inline CubicPolynomial &operator[](int i) 114 | { 115 | return pieces[i]; 116 | } 117 | 118 | inline void clear(void) 119 | { 120 | pieces.clear(); 121 | return; 122 | } 123 | 124 | inline Pieces::const_iterator begin() const 125 | { 126 | return pieces.begin(); 127 | } 128 | 129 | inline Pieces::const_iterator end() const 130 | { 131 | return pieces.end(); 132 | } 133 | 134 | inline Pieces::iterator begin() 135 | { 136 | return pieces.begin(); 137 | } 138 | 139 | inline Pieces::iterator end() 140 | { 141 | return pieces.end(); 142 | } 143 | 144 | inline void reserve(const int &n) 145 | { 146 | pieces.reserve(n); 147 | return; 148 | } 149 | 150 | inline void emplace_back(const CubicPolynomial &piece) 151 | { 152 | pieces.emplace_back(piece); 153 | return; 154 | } 155 | 156 | inline void emplace_back(const double &dur, 157 | const Eigen::Matrix &cMat) 158 | { 159 | pieces.emplace_back(dur, cMat); 160 | return; 161 | } 162 | 163 | inline void append(const CubicCurve &traj) 164 | { 165 | pieces.insert(pieces.end(), traj.begin(), traj.end()); 166 | return; 167 | } 168 | 169 | inline int locatePieceIdx(double &t) const 170 | { 171 | const int N = getPieceNum(); 172 | int idx; 173 | double dur; 174 | for (idx = 0; 175 | idx < N && 176 | t > (dur = pieces[idx].getDuration()); 177 | idx++) 178 | { 179 | t -= dur; 180 | } 181 | if (idx == N) 182 | { 183 | idx--; 184 | t += pieces[idx].getDuration(); 185 | } 186 | return idx; 187 | } 188 | 189 | inline Eigen::Vector2d getPos(double t) const 190 | { 191 | const int pieceIdx = locatePieceIdx(t); 192 | return pieces[pieceIdx].getPos(t); 193 | } 194 | 195 | inline Eigen::Vector2d getJuncPos(const int juncIdx) const 196 | { 197 | if (juncIdx != getPieceNum()) 198 | { 199 | return pieces[juncIdx].getCoeffMat().col(3); 200 | } 201 | else 202 | { 203 | return pieces[juncIdx - 1].getPos(pieces[juncIdx - 1].getDuration()); 204 | } 205 | } 206 | }; 207 | 208 | #endif 209 | -------------------------------------------------------------------------------- /Traj_gen by L-BFGS/src/gcopter/include/gcopter/cubic_spline.hpp: -------------------------------------------------------------------------------- 1 | #ifndef CUBIC_SPLINE_HPP 2 | #define CUBIC_SPLINE_HPP 3 | 4 | #include "cubic_curve.hpp" 5 | 6 | #include 7 | 8 | #include 9 | #include 10 | 11 | namespace cubic_spline 12 | { 13 | 14 | // The banded system class is used for solving 15 | // banded linear system Ax=b efficiently. 16 | // A is an N*N band matrix with lower band width lowerBw 17 | // and upper band width upperBw. 18 | // Banded LU factorization has O(N) time complexity. 19 | class BandedSystem 20 | { 21 | public: 22 | // The size of A, as well as the lower/upper 23 | // banded width p/q are needed 24 | inline void create(const int &n, const int &p, const int &q) 25 | { 26 | // In case of re-creating before destroying 27 | destroy(); 28 | N = n; 29 | lowerBw = p; 30 | upperBw = q; 31 | int actualSize = N * (lowerBw + upperBw + 1); 32 | ptrData = new double[actualSize]; 33 | std::fill_n(ptrData, actualSize, 0.0); 34 | return; 35 | } 36 | 37 | inline void destroy() 38 | { 39 | if (ptrData != nullptr) 40 | { 41 | delete[] ptrData; 42 | ptrData = nullptr; 43 | } 44 | return; 45 | } 46 | 47 | private: 48 | int N; 49 | int lowerBw; 50 | int upperBw; 51 | // Compulsory nullptr initialization here 52 | double *ptrData = nullptr; 53 | 54 | public: 55 | // Reset the matrix to zero 56 | inline void reset(void) 57 | { 58 | std::fill_n(ptrData, N * (lowerBw + upperBw + 1), 0.0); 59 | return; 60 | } 61 | 62 | // The band matrix is stored as suggested in "Matrix Computation" 63 | inline const double &operator()(const int &i, const int &j) const 64 | { 65 | return ptrData[(i - j + upperBw) * N + j]; 66 | } 67 | 68 | inline double &operator()(const int &i, const int &j) 69 | { 70 | return ptrData[(i - j + upperBw) * N + j]; 71 | } 72 | 73 | // This function conducts banded LU factorization in place 74 | // Note that NO PIVOT is applied on the matrix "A" for efficiency!!! 75 | inline void factorizeLU() 76 | { 77 | int iM, jM; 78 | double cVl; 79 | for (int k = 0; k <= N - 2; ++k) 80 | { 81 | iM = std::min(k + lowerBw, N - 1); 82 | cVl = operator()(k, k); 83 | for (int i = k + 1; i <= iM; ++i) 84 | { 85 | if (operator()(i, k) != 0.0) 86 | { 87 | operator()(i, k) /= cVl; 88 | } 89 | } 90 | jM = std::min(k + upperBw, N - 1); 91 | for (int j = k + 1; j <= jM; ++j) 92 | { 93 | cVl = operator()(k, j); 94 | if (cVl != 0.0) 95 | { 96 | for (int i = k + 1; i <= iM; ++i) 97 | { 98 | if (operator()(i, k) != 0.0) 99 | { 100 | operator()(i, j) -= operator()(i, k) * cVl; 101 | } 102 | } 103 | } 104 | } 105 | } 106 | return; 107 | } 108 | 109 | // This function solves Ax=b, then stores x in b 110 | // The input b is required to be N*m, i.e., 111 | // m vectors to be solved. 112 | template 113 | inline void solve(EIGENMAT &b) const 114 | { 115 | int iM; 116 | for (int j = 0; j <= N - 1; ++j) 117 | { 118 | iM = std::min(j + lowerBw, N - 1); 119 | for (int i = j + 1; i <= iM; ++i) 120 | { 121 | if (operator()(i, j) != 0.0) 122 | { 123 | b.row(i) -= operator()(i, j) * b.row(j); 124 | } 125 | } 126 | } 127 | for (int j = N - 1; j >= 0; --j) 128 | { 129 | b.row(j) /= operator()(j, j); 130 | iM = std::max(0, j - upperBw); 131 | for (int i = iM; i <= j - 1; ++i) 132 | { 133 | if (operator()(i, j) != 0.0) 134 | { 135 | b.row(i) -= operator()(i, j) * b.row(j); 136 | } 137 | } 138 | } 139 | return; 140 | } 141 | 142 | // This function solves ATx=b, then stores x in b 143 | // The input b is required to be N*m, i.e., 144 | // m vectors to be solved. 145 | template 146 | inline void solveAdj(EIGENMAT &b) const 147 | { 148 | int iM; 149 | for (int j = 0; j <= N - 1; ++j) 150 | { 151 | b.row(j) /= operator()(j, j); 152 | iM = std::min(j + upperBw, N - 1); 153 | for (int i = j + 1; i <= iM; ++i) 154 | { 155 | if (operator()(j, i) != 0.0) 156 | { 157 | b.row(i) -= operator()(j, i) * b.row(j); 158 | } 159 | } 160 | } 161 | for (int j = N - 1; j >= 0; --j) 162 | { 163 | iM = std::max(0, j - lowerBw); 164 | for (int i = iM; i <= j - 1; ++i) 165 | { 166 | if (operator()(j, i) != 0.0) 167 | { 168 | b.row(i) -= operator()(j, i) * b.row(j); 169 | } 170 | } 171 | } 172 | } 173 | }; 174 | 175 | class CubicSpline 176 | { 177 | public: 178 | CubicSpline() = default; 179 | ~CubicSpline() { A.destroy(); } 180 | 181 | private: 182 | int N; 183 | Eigen::Vector2d headP; 184 | Eigen::Vector2d tailP; 185 | BandedSystem A; 186 | Eigen::MatrixX2d b; 187 | Eigen::Matrix2Xd coeff; 188 | 189 | public: 190 | inline void setConditions(const Eigen::Vector2d &headPos, 191 | const Eigen::Vector2d &tailPos, 192 | const int &pieceNum) 193 | { 194 | // This is a function to set the coefficients matrix A (AD=x represent the curve) 195 | // which has no 'return'. 196 | N = pieceNum; 197 | headP = headPos; 198 | tailP = tailPos; 199 | 200 | // TODO 201 | // 带状矩阵的存储：https://zhuanlan.zhihu.com/p/400460201 202 | A.create(N - 1, 3, 3); 203 | b.resize(N - 1, 2); 204 | 205 | return; 206 | } 207 | 208 | inline void setInnerPoints(const Eigen::Ref &inPs) 209 | { 210 | // This is a function to set the inner point x (AD=x) of the curve which 211 | // has no return. 212 | // TODO 213 | A.reset(); 214 | b.setZero(); 215 | for (int i = 0; i < N - 1; ++i) 216 | { 217 | if (i == 0) 218 | { 219 | A(0, 0) = 4; 220 | A(0, 1) = 1; 221 | } 222 | else if (i == N - 2) 223 | { 224 | A(N - 2, N - 3) = 1; 225 | A(N - 2, N - 2) = 4; 226 | } 227 | else 228 | { 229 | A(i, i - 1) = 1; 230 | A(i, i) = 4; 231 | A(i, i + 1) = 1; 232 | } 233 | } 234 | 235 | b.row(0) = 3 * (inPs.col(1).transpose() - headP.transpose()); 236 | for (int i = 1; i < N - 2; ++i) 237 | { 238 | b.row(i) = 3 * (inPs.col(i + 1).transpose() - inPs.col(i - 1).transpose()); 239 | } 240 | b.row(N - 2) = 3 * (tailP.transpose() - inPs.col(N - 3).transpose()); 241 | 242 | A.factorizeLU(); 243 | A.solve(b); 244 | 245 | // b[0]~b[n-2] 对应 D1~D_{N-1}, D0 = D_{N} = 0 246 | std::vector D(N + 1, Eigen::Vector2d::Zero()); 247 | for (int i = 0; i < N - 1; ++i) 248 | D[i + 1] = b.row(i).transpose(); 249 | 250 | // 系数矩阵coeff 251 | coeff.resize(2, 4 * N); 252 | coeff.setZero(); 253 | for (int i = 0; i < N; ++i) 254 | { 255 | if ( i == 0){ 256 | coeff.col(0) = headP; 257 | coeff.col(1) = Eigen::Vector2d::Zero(); 258 | coeff.col(2) = 3 * (inPs.col(0) - headP) - D[1]; 259 | coeff.col(3) = 2 * (headP - inPs.col(0)) + D[1]; 260 | }else if( i == N - 1){ 261 | coeff.col(4 * (N - 1) + 0) = inPs.col(N - 2); 262 | coeff.col(4 * (N - 1) + 1) = D[N - 1]; 263 | coeff.col(4 * (N - 1) + 2) = 3 * (tailP - inPs.col(N - 2)) - 2 * D[N - 1]; 264 | coeff.col(4 * (N - 1) + 3) = 2 * (inPs.col(N - 2) - tailP) + 1 * D[N - 1]; 265 | }else{ 266 | coeff.col(4 * i + 0) = inPs.col(i - 1); 267 | coeff.col(4 * i + 1) = D[i]; 268 | coeff.col(4 * i + 2) = 3 * (inPs.col(i) - inPs.col(i - 1)) - 2 * D[i] - D[i + 1]; 269 | coeff.col(4 * i + 3) = 2 * (inPs.col(i - 1) - inPs.col(i)) + 1 * D[i] + D[i + 1]; 270 | } 271 | 272 | } 273 | 274 | // 将b重新塑性为系数矩阵 275 | b.resize(4 * N, 2); 276 | b = coeff.transpose(); 277 | 278 | return; 279 | } 280 | 281 | inline void getCurve(CubicCurve &curve) const 282 | { 283 | // Not TODO 284 | curve.clear(); 285 | curve.reserve(N); 286 | for (int i = 0; i < N; ++i) 287 | { 288 | curve.emplace_back(1.0, 289 | b.block<4, 2>(4 * i, 0) 290 | .transpose() 291 | .rowwise() 292 | .reverse()); 293 | } 294 | return; 295 | } 296 | 297 | inline void getStretchEnergy(double &energy) const 298 | { 299 | // An example for you to finish the other function 300 | energy = 0.0; 301 | for (int i = 0; i < N; ++i) 302 | { 303 | energy += 4.0 * b.row(4 * i + 2).squaredNorm() + 304 | 12.0 * b.row(4 * i + 2).dot(b.row(4 * i + 3)) + 305 | 12.0 * b.row(4 * i + 3).squaredNorm(); 306 | } 307 | return; 308 | } 309 | 310 | inline const Eigen::MatrixX2d &getCoeffs(void) const 311 | { 312 | return b; 313 | } 314 | 315 | inline void getGrad(Eigen::Ref gradByPoints) const 316 | { 317 | // This is a function to get the Grad 318 | // TODO 319 | 320 | // * step1：获取 \partial_x(D), x is [x_1, ... , x_n-1] 321 | Eigen::MatrixXd A_tmp; 322 | A_tmp.resize(N - 1, N - 1); 323 | Eigen::MatrixXd partial_B; 324 | partial_B.resize(N - 1, N - 1); 325 | A_tmp.setZero(); 326 | partial_B.setZero(); 327 | for (int i = 0; i < N - 1; ++i) 328 | { 329 | if (i == 0) 330 | { 331 | A_tmp(0, 0) = 4; 332 | A_tmp(0, 1) = 1; 333 | partial_B(0, 1) = 3; 334 | } 335 | else if (i == N - 2) 336 | { 337 | A_tmp(N - 2, N - 3) = 1; 338 | A_tmp(N - 2, N - 2) = 4; 339 | partial_B(N - 2, N - 3) = -3; 340 | } 341 | else 342 | { 343 | A_tmp(i, i - 1) = 1; 344 | A_tmp(i, i) = 4; 345 | A_tmp(i, i + 1) = 1; 346 | partial_B(i, i - 1) = -3; 347 | partial_B(i, i + 1) = 3; 348 | } 349 | } 350 | Eigen::MatrixXd A_inv = A_tmp.inverse(); 351 | Eigen::MatrixXd partial_D = A_inv * partial_B; 352 | // std::cout << "partial_D : " << partial_D.rows() << " x " << partial_D.cols() << std::endl; 353 | 354 | 355 | // * step2：获取 \partial_x(x_i - x_{i+1}), x is [x_1, ... , x_n-1] 356 | Eigen::MatrixXd partial_diff_x; 357 | partial_diff_x.resize(N, N - 1); 358 | partial_diff_x.setZero(); 359 | partial_diff_x(0, 0) = -1; 360 | partial_diff_x(N - 1, N - 2) = 1; 361 | for (int i = 1; i < N - 1; ++i) 362 | { 363 | partial_diff_x(i, i) = -1; 364 | partial_diff_x(i, i - 1) = 1; 365 | } 366 | // std::cout << "partial_diff_x : " << partial_diff_x.rows() << " x " << partial_diff_x.cols() << std::endl; 367 | 368 | // * step3: 获取\partial_x(c)和\partial_x(d), x is [x_1, ... , x_n-1] 369 | Eigen::MatrixXd partial_c; 370 | partial_c.resize(N, N - 1); 371 | partial_c.setZero(); 372 | 373 | Eigen::MatrixXd partial_d; 374 | partial_d.resize(N, N - 1); 375 | partial_d.setZero(); 376 | 377 | partial_c.row(0) = -3 * partial_diff_x.row(0) - partial_D.row(0); 378 | partial_d.row(0) = 2 * partial_diff_x.row(0) + partial_D.row(0); 379 | for (int i = 1; i < N - 1; ++i) 380 | { 381 | partial_c.row(i) = -3 * partial_diff_x.row(i) - 2 * partial_D.row(i - 1) - partial_D.row(i); 382 | partial_d.row(i) = 2 * partial_diff_x.row(i) + partial_D.row(i - 1) + partial_D.row(i); 383 | } 384 | partial_c.row(N - 1) = -3 * partial_diff_x.row(N - 1) - 2 * partial_D.row(N - 2); 385 | partial_d.row(N - 1) = 2 * partial_diff_x.row(N - 1) + partial_D.row(N - 2); 386 | 387 | // std::cout << "partial_c : " << partial_c.rows() << " x " << partial_c.cols() << std::endl; 388 | // std::cout << "partial_d : " << partial_d.rows() << " x " << partial_d.cols() << std::endl; 389 | 390 | // * 填入gradByPoints 391 | gradByPoints.setZero(); 392 | 393 | for (int i = 0; i < N; ++i) 394 | { 395 | Eigen::Vector2d c_i = coeff.col(4 * i + 2); 396 | Eigen::Vector2d d_i = coeff.col(4 * i + 3); 397 | // (2 * 1) x (1 * N - 1) = 2 x N - 1 398 | gradByPoints += (24 * d_i + 12 * c_i) * partial_d.row(i) + (12 * d_i + 8 * c_i) * partial_c.row(i); 399 | } 400 | // std::cout << "gradByPoints : " << gradByPoints.rows() << " x " << gradByPoints.cols() << std::endl; 401 | } 402 | }; 403 | } 404 | 405 | #endif 406 | -------------------------------------------------------------------------------- /Traj_gen by L-BFGS/src/gcopter/include/gcopter/firi.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | MIT License 3 | 4 | Copyright (c) 2021 Zhepei Wang (wangzhepei@live.com) 5 | 6 | Permission is hereby granted, free of charge, to any person obtaining a copy 7 | of this software and associated documentation files (the "Software"), to deal 8 | in the Software without restriction, including without limitation the rights 9 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | copies of the Software, and to permit persons to whom the Software is 11 | furnished to do so, subject to the following conditions: 12 | 13 | The above copyright notice and this permission notice shall be included in all 14 | copies or substantial portions of the Software. 15 | 16 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | SOFTWARE. 23 | */ 24 | 25 | /* This is an old version of FIRI for temporary usage here. */ 26 | 27 | #ifndef FIRI_HPP 28 | #define FIRI_HPP 29 | 30 | #include "lbfgs.hpp" 31 | #include "sdlp.hpp" 32 | 33 | #include 34 | 35 | #include 36 | #include 37 | #include 38 | #include 39 | #include 40 | #include 41 | 42 | namespace firi 43 | { 44 | 45 | inline void chol3d(const Eigen::Matrix3d &A, 46 | Eigen::Matrix3d &L) 47 | { 48 | L(0, 0) = sqrt(A(0, 0)); 49 | L(0, 1) = 0.0; 50 | L(0, 2) = 0.0; 51 | L(1, 0) = 0.5 * (A(0, 1) + A(1, 0)) / L(0, 0); 52 | L(1, 1) = sqrt(A(1, 1) - L(1, 0) * L(1, 0)); 53 | L(1, 2) = 0.0; 54 | L(2, 0) = 0.5 * (A(0, 2) + A(2, 0)) / L(0, 0); 55 | L(2, 1) = (0.5 * (A(1, 2) + A(2, 1)) - L(2, 0) * L(1, 0)) / L(1, 1); 56 | L(2, 2) = sqrt(A(2, 2) - L(2, 0) * L(2, 0) - L(2, 1) * L(2, 1)); 57 | return; 58 | } 59 | 60 | inline bool smoothedL1(const double &mu, 61 | const double &x, 62 | double &f, 63 | double &df) 64 | { 65 | if (x < 0.0) 66 | { 67 | return false; 68 | } 69 | else if (x > mu) 70 | { 71 | f = x - 0.5 * mu; 72 | df = 1.0; 73 | return true; 74 | } 75 | else 76 | { 77 | const double xdmu = x / mu; 78 | const double sqrxdmu = xdmu * xdmu; 79 | const double mumxd2 = mu - 0.5 * x; 80 | f = mumxd2 * sqrxdmu * xdmu; 81 | df = sqrxdmu * ((-0.5) * xdmu + 3.0 * mumxd2 / mu); 82 | return true; 83 | } 84 | } 85 | 86 | inline double costMVIE(void *data, 87 | const Eigen::VectorXd &x, 88 | Eigen::VectorXd &grad) 89 | { 90 | const int *pM = (int *)data; 91 | const double *pSmoothEps = (double *)(pM + 1); 92 | const double *pPenaltyWt = pSmoothEps + 1; 93 | const double *pA = pPenaltyWt + 1; 94 | 95 | const int M = *pM; 96 | const double smoothEps = *pSmoothEps; 97 | const double penaltyWt = *pPenaltyWt; 98 | Eigen::Map A(pA, M, 3); 99 | Eigen::Map p(x.data()); 100 | Eigen::Map rtd(x.data() + 3); 101 | Eigen::Map cde(x.data() + 6); 102 | Eigen::Map gdp(grad.data()); 103 | Eigen::Map gdrtd(grad.data() + 3); 104 | Eigen::Map gdcde(grad.data() + 6); 105 | 106 | double cost = 0; 107 | gdp.setZero(); 108 | gdrtd.setZero(); 109 | gdcde.setZero(); 110 | 111 | Eigen::Matrix3d L; 112 | L(0, 0) = rtd(0) * rtd(0) + DBL_EPSILON; 113 | L(0, 1) = 0.0; 114 | L(0, 2) = 0.0; 115 | L(1, 0) = cde(0); 116 | L(1, 1) = rtd(1) * rtd(1) + DBL_EPSILON; 117 | L(1, 2) = 0.0; 118 | L(2, 0) = cde(2); 119 | L(2, 1) = cde(1); 120 | L(2, 2) = rtd(2) * rtd(2) + DBL_EPSILON; 121 | 122 | const Eigen::MatrixX3d AL = A * L; 123 | const Eigen::VectorXd normAL = AL.rowwise().norm(); 124 | const Eigen::Matrix3Xd adjNormAL = (AL.array().colwise() / normAL.array()).transpose(); 125 | const Eigen::VectorXd consViola = (normAL + A * p).array() - 1.0; 126 | 127 | double c, dc; 128 | Eigen::Vector3d vec; 129 | for (int i = 0; i < M; ++i) 130 | { 131 | if (smoothedL1(smoothEps, consViola(i), c, dc)) 132 | { 133 | cost += c; 134 | vec = dc * A.row(i).transpose(); 135 | gdp += vec; 136 | gdrtd += adjNormAL.col(i).cwiseProduct(vec); 137 | gdcde(0) += adjNormAL(0, i) * vec(1); 138 | gdcde(1) += adjNormAL(1, i) * vec(2); 139 | gdcde(2) += adjNormAL(0, i) * vec(2); 140 | } 141 | } 142 | cost *= penaltyWt; 143 | gdp *= penaltyWt; 144 | gdrtd *= penaltyWt; 145 | gdcde *= penaltyWt; 146 | 147 | cost -= log(L(0, 0)) + log(L(1, 1)) + log(L(2, 2)); 148 | gdrtd(0) -= 1.0 / L(0, 0); 149 | gdrtd(1) -= 1.0 / L(1, 1); 150 | gdrtd(2) -= 1.0 / L(2, 2); 151 | 152 | gdrtd(0) *= 2.0 * rtd(0); 153 | gdrtd(1) *= 2.0 * rtd(1); 154 | gdrtd(2) *= 2.0 * rtd(2); 155 | 156 | return cost; 157 | } 158 | 159 | // Each row of hPoly is defined by h0, h1, h2, h3 as 160 | // h0*x + h1*y + h2*z + h3 <= 0 161 | // R, p, r are ALWAYS taken as the initial guess 162 | // R is also assumed to be a rotation matrix 163 | inline bool maxVolInsEllipsoid(const Eigen::MatrixX4d &hPoly, 164 | Eigen::Matrix3d &R, 165 | Eigen::Vector3d &p, 166 | Eigen::Vector3d &r) 167 | { 168 | // Find the deepest interior point 169 | const int M = hPoly.rows(); 170 | Eigen::MatrixX4d Alp(M, 4); 171 | Eigen::VectorXd blp(M); 172 | Eigen::Vector4d clp, xlp; 173 | const Eigen::ArrayXd hNorm = hPoly.leftCols<3>().rowwise().norm(); 174 | Alp.leftCols<3>() = hPoly.leftCols<3>().array().colwise() / hNorm; 175 | Alp.rightCols<1>().setConstant(1.0); 176 | blp = -hPoly.rightCols<1>().array() / hNorm; 177 | clp.setZero(); 178 | clp(3) = -1.0; 179 | const double maxdepth = -sdlp::linprog<4>(clp, Alp, blp, xlp); 180 | if (!(maxdepth > 0.0) || std::isinf(maxdepth)) 181 | { 182 | return false; 183 | } 184 | const Eigen::Vector3d interior = xlp.head<3>(); 185 | 186 | // Prepare the data for MVIE optimization 187 | uint8_t *optData = new uint8_t[sizeof(int) + (2 + 3 * M) * sizeof(double)]; 188 | int *pM = (int *)optData; 189 | double *pSmoothEps = (double *)(pM + 1); 190 | double *pPenaltyWt = pSmoothEps + 1; 191 | double *pA = pPenaltyWt + 1; 192 | 193 | *pM = M; 194 | Eigen::Map A(pA, M, 3); 195 | A = Alp.leftCols<3>().array().colwise() / 196 | (blp - Alp.leftCols<3>() * interior).array(); 197 | 198 | Eigen::VectorXd x(9); 199 | const Eigen::Matrix3d Q = R * (r.cwiseProduct(r)).asDiagonal() * R.transpose(); 200 | Eigen::Matrix3d L; 201 | chol3d(Q, L); 202 | 203 | x.head<3>() = p - interior; 204 | x(3) = sqrt(L(0, 0)); 205 | x(4) = sqrt(L(1, 1)); 206 | x(5) = sqrt(L(2, 2)); 207 | x(6) = L(1, 0); 208 | x(7) = L(2, 1); 209 | x(8) = L(2, 0); 210 | 211 | double minCost; 212 | lbfgs::lbfgs_parameter_t paramsMVIE; 213 | paramsMVIE.mem_size = 18; 214 | paramsMVIE.g_epsilon = 0.0; 215 | paramsMVIE.min_step = 1.0e-32; 216 | paramsMVIE.past = 3; 217 | paramsMVIE.delta = 1.0e-7; 218 | *pSmoothEps = 1.0e-2; 219 | *pPenaltyWt = 1.0e+3; 220 | 221 | int ret = lbfgs::lbfgs_optimize(x, 222 | minCost, 223 | &costMVIE, 224 | nullptr, 225 | nullptr, 226 | optData, 227 | paramsMVIE); 228 | 229 | if (ret < 0) 230 | { 231 | printf("FIRI WARNING: %s\n", lbfgs::lbfgs_strerror(ret)); 232 | } 233 | 234 | p = x.head<3>() + interior; 235 | L(0, 0) = x(3) * x(3); 236 | L(0, 1) = 0.0; 237 | L(0, 2) = 0.0; 238 | L(1, 0) = x(6); 239 | L(1, 1) = x(4) * x(4); 240 | L(1, 2) = 0.0; 241 | L(2, 0) = x(8); 242 | L(2, 1) = x(7); 243 | L(2, 2) = x(5) * x(5); 244 | Eigen::JacobiSVD svd(L, Eigen::ComputeFullU); 245 | const Eigen::Matrix3d U = svd.matrixU(); 246 | const Eigen::Vector3d S = svd.singularValues(); 247 | if (U.determinant() < 0.0) 248 | { 249 | R.col(0) = U.col(1); 250 | R.col(1) = U.col(0); 251 | R.col(2) = U.col(2); 252 | r(0) = S(1); 253 | r(1) = S(0); 254 | r(2) = S(2); 255 | } 256 | else 257 | { 258 | R = U; 259 | r = S; 260 | } 261 | 262 | delete[] optData; 263 | 264 | return ret >= 0; 265 | } 266 | 267 | inline bool firi(const Eigen::MatrixX4d &bd, 268 | const Eigen::Matrix3Xd &pc, 269 | const Eigen::Vector3d &a, 270 | const Eigen::Vector3d &b, 271 | Eigen::MatrixX4d &hPoly, 272 | const int iterations = 4, 273 | const double epsilon = 1.0e-6) 274 | { 275 | const Eigen::Vector4d ah(a(0), a(1), a(2), 1.0); 276 | const Eigen::Vector4d bh(b(0), b(1), b(2), 1.0); 277 | 278 | if ((bd * ah).maxCoeff() > 0.0 || 279 | (bd * bh).maxCoeff() > 0.0) 280 | { 281 | return false; 282 | } 283 | 284 | const int M = bd.rows(); 285 | const int N = pc.cols(); 286 | 287 | Eigen::Matrix3d R = Eigen::Matrix3d::Identity(); 288 | Eigen::Vector3d p = 0.5 * (a + b); 289 | Eigen::Vector3d r = Eigen::Vector3d::Ones(); 290 | Eigen::MatrixX4d forwardH(M + N, 4); 291 | int nH = 0; 292 | 293 | for (int loop = 0; loop < iterations; ++loop) 294 | { 295 | const Eigen::Matrix3d forward = r.cwiseInverse().asDiagonal() * R.transpose(); 296 | const Eigen::Matrix3d backward = R * r.asDiagonal(); 297 | const Eigen::MatrixX3d forwardB = bd.leftCols<3>() * backward; 298 | const Eigen::VectorXd forwardD = bd.rightCols<1>() + bd.leftCols<3>() * p; 299 | const Eigen::Matrix3Xd forwardPC = forward * (pc.colwise() - p); 300 | const Eigen::Vector3d fwd_a = forward * (a - p); 301 | const Eigen::Vector3d fwd_b = forward * (b - p); 302 | 303 | const Eigen::VectorXd distDs = forwardD.cwiseAbs().cwiseQuotient(forwardB.rowwise().norm()); 304 | Eigen::MatrixX4d tangents(N, 4); 305 | Eigen::VectorXd distRs(N); 306 | 307 | for (int i = 0; i < N; i++) 308 | { 309 | distRs(i) = forwardPC.col(i).norm(); 310 | tangents(i, 3) = -distRs(i); 311 | tangents.block<1, 3>(i, 0) = forwardPC.col(i).transpose() / distRs(i); 312 | if (tangents.block<1, 3>(i, 0).dot(fwd_a) + tangents(i, 3) > epsilon) 313 | { 314 | const Eigen::Vector3d delta = forwardPC.col(i) - fwd_a; 315 | tangents.block<1, 3>(i, 0) = fwd_a - (delta.dot(fwd_a) / delta.squaredNorm()) * delta; 316 | distRs(i) = tangents.block<1, 3>(i, 0).norm(); 317 | tangents(i, 3) = -distRs(i); 318 | tangents.block<1, 3>(i, 0) /= distRs(i); 319 | } 320 | if (tangents.block<1, 3>(i, 0).dot(fwd_b) + tangents(i, 3) > epsilon) 321 | { 322 | const Eigen::Vector3d delta = forwardPC.col(i) - fwd_b; 323 | tangents.block<1, 3>(i, 0) = fwd_b - (delta.dot(fwd_b) / delta.squaredNorm()) * delta; 324 | distRs(i) = tangents.block<1, 3>(i, 0).norm(); 325 | tangents(i, 3) = -distRs(i); 326 | tangents.block<1, 3>(i, 0) /= distRs(i); 327 | } 328 | if (tangents.block<1, 3>(i, 0).dot(fwd_a) + tangents(i, 3) > epsilon) 329 | { 330 | tangents.block<1, 3>(i, 0) = (fwd_a - forwardPC.col(i)).cross(fwd_b - forwardPC.col(i)).normalized(); 331 | tangents(i, 3) = -tangents.block<1, 3>(i, 0).dot(fwd_a); 332 | tangents.row(i) *= tangents(i, 3) > 0.0 ? -1.0 : 1.0; 333 | } 334 | } 335 | 336 | Eigen::Matrix bdFlags = Eigen::Matrix::Constant(M, 1); 337 | Eigen::Matrix pcFlags = Eigen::Matrix::Constant(N, 1); 338 | 339 | nH = 0; 340 | 341 | bool completed = false; 342 | int bdMinId = 0, pcMinId = 0; 343 | double minSqrD = distDs.minCoeff(&bdMinId); 344 | double minSqrR = INFINITY; 345 | if (distRs.size() != 0) 346 | { 347 | minSqrR = distRs.minCoeff(&pcMinId); 348 | } 349 | for (int i = 0; !completed && i < (M + N); ++i) 350 | { 351 | if (minSqrD < minSqrR) 352 | { 353 | forwardH.block<1, 3>(nH, 0) = forwardB.row(bdMinId); 354 | forwardH(nH, 3) = forwardD(bdMinId); 355 | bdFlags(bdMinId) = 0; 356 | } 357 | else 358 | { 359 | forwardH.row(nH) = tangents.row(pcMinId); 360 | pcFlags(pcMinId) = 0; 361 | } 362 | 363 | completed = true; 364 | minSqrD = INFINITY; 365 | for (int j = 0; j < M; ++j) 366 | { 367 | if (bdFlags(j)) 368 | { 369 | completed = false; 370 | if (minSqrD > distDs(j)) 371 | { 372 | bdMinId = j; 373 | minSqrD = distDs(j); 374 | } 375 | } 376 | } 377 | minSqrR = INFINITY; 378 | for (int j = 0; j < N; ++j) 379 | { 380 | if (pcFlags(j)) 381 | { 382 | if (forwardH.block<1, 3>(nH, 0).dot(forwardPC.col(j)) + forwardH(nH, 3) > -epsilon) 383 | { 384 | pcFlags(j) = 0; 385 | } 386 | else 387 | { 388 | completed = false; 389 | if (minSqrR > distRs(j)) 390 | { 391 | pcMinId = j; 392 | minSqrR = distRs(j); 393 | } 394 | } 395 | } 396 | } 397 | ++nH; 398 | } 399 | 400 | hPoly.resize(nH, 4); 401 | for (int i = 0; i < nH; ++i) 402 | { 403 | hPoly.block<1, 3>(i, 0) = forwardH.block<1, 3>(i, 0) * forward; 404 | hPoly(i, 3) = forwardH(i, 3) - hPoly.block<1, 3>(i, 0).dot(p); 405 | } 406 | 407 | if (loop == iterations - 1) 408 | { 409 | break; 410 | } 411 | 412 | maxVolInsEllipsoid(hPoly, R, p, r); 413 | } 414 | 415 | return true; 416 | } 417 | 418 | } 419 | 420 | #endif 421 | -------------------------------------------------------------------------------- /Traj_gen by L-BFGS/src/gcopter/include/gcopter/flatness.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | MIT License 3 | 4 | Copyright (c) 2021 Zhepei Wang (wangzhepei@live.com) 5 | 6 | Permission is hereby granted, free of charge, to any person obtaining a copy 7 | of this software and associated documentation files (the "Software"), to deal 8 | in the Software without restriction, including without limitation the rights 9 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | copies of the Software, and to permit persons to whom the Software is 11 | furnished to do so, subject to the following conditions: 12 | 13 | The above copyright notice and this permission notice shall be included in all 14 | copies or substantial portions of the Software. 15 | 16 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | SOFTWARE. 23 | */ 24 | 25 | #ifndef FLATNESS_HPP 26 | #define FLATNESS_HPP 27 | 28 | #include 29 | 30 | #include 31 | 32 | namespace flatness 33 | { 34 | class FlatnessMap // See https://github.com/ZJU-FAST-Lab/GCOPTER/blob/main/misc/flatness.pdf 35 | { 36 | public: 37 | inline void reset(const double &vehicle_mass, 38 | const double &gravitational_acceleration, 39 | const double &horitonral_drag_coeff, 40 | const double &vertical_drag_coeff, 41 | const double ¶sitic_drag_coeff, 42 | const double &speed_smooth_factor) 43 | { 44 | mass = vehicle_mass; 45 | grav = gravitational_acceleration; 46 | dh = horitonral_drag_coeff; 47 | dv = vertical_drag_coeff; 48 | cp = parasitic_drag_coeff; 49 | veps = speed_smooth_factor; 50 | 51 | return; 52 | } 53 | 54 | inline void forward(const Eigen::Vector3d &vel, 55 | const Eigen::Vector3d &acc, 56 | const Eigen::Vector3d &jer, 57 | const double &psi, 58 | const double &dpsi, 59 | double &thr, 60 | Eigen::Vector4d &quat, 61 | Eigen::Vector3d &omg) 62 | { 63 | double w0, w1, w2, dw0, dw1, dw2; 64 | 65 | v0 = vel(0); 66 | v1 = vel(1); 67 | v2 = vel(2); 68 | a0 = acc(0); 69 | a1 = acc(1); 70 | a2 = acc(2); 71 | cp_term = sqrt(v0 * v0 + v1 * v1 + v2 * v2 + veps); 72 | w_term = 1.0 + cp * cp_term; 73 | w0 = w_term * v0; 74 | w1 = w_term * v1; 75 | w2 = w_term * v2; 76 | dh_over_m = dh / mass; 77 | zu0 = a0 + dh_over_m * w0; 78 | zu1 = a1 + dh_over_m * w1; 79 | zu2 = a2 + dh_over_m * w2 + grav; 80 | zu_sqr0 = zu0 * zu0; 81 | zu_sqr1 = zu1 * zu1; 82 | zu_sqr2 = zu2 * zu2; 83 | zu01 = zu0 * zu1; 84 | zu12 = zu1 * zu2; 85 | zu02 = zu0 * zu2; 86 | zu_sqr_norm = zu_sqr0 + zu_sqr1 + zu_sqr2; 87 | zu_norm = sqrt(zu_sqr_norm); 88 | z0 = zu0 / zu_norm; 89 | z1 = zu1 / zu_norm; 90 | z2 = zu2 / zu_norm; 91 | ng_den = zu_sqr_norm * zu_norm; 92 | ng00 = (zu_sqr1 + zu_sqr2) / ng_den; 93 | ng01 = -zu01 / ng_den; 94 | ng02 = -zu02 / ng_den; 95 | ng11 = (zu_sqr0 + zu_sqr2) / ng_den; 96 | ng12 = -zu12 / ng_den; 97 | ng22 = (zu_sqr0 + zu_sqr1) / ng_den; 98 | v_dot_a = v0 * a0 + v1 * a1 + v2 * a2; 99 | dw_term = cp * v_dot_a / cp_term; 100 | dw0 = w_term * a0 + dw_term * v0; 101 | dw1 = w_term * a1 + dw_term * v1; 102 | dw2 = w_term * a2 + dw_term * v2; 103 | dz_term0 = jer(0) + dh_over_m * dw0; 104 | dz_term1 = jer(1) + dh_over_m * dw1; 105 | dz_term2 = jer(2) + dh_over_m * dw2; 106 | dz0 = ng00 * dz_term0 + ng01 * dz_term1 + ng02 * dz_term2; 107 | dz1 = ng01 * dz_term0 + ng11 * dz_term1 + ng12 * dz_term2; 108 | dz2 = ng02 * dz_term0 + ng12 * dz_term1 + ng22 * dz_term2; 109 | f_term0 = mass * a0 + dv * w0; 110 | f_term1 = mass * a1 + dv * w1; 111 | f_term2 = mass * (a2 + grav) + dv * w2; 112 | thr = z0 * f_term0 + z1 * f_term1 + z2 * f_term2; 113 | tilt_den = sqrt(2.0 * (1.0 + z2)); 114 | tilt0 = 0.5 * tilt_den; 115 | tilt1 = -z1 / tilt_den; 116 | tilt2 = z0 / tilt_den; 117 | c_half_psi = cos(0.5 * psi); 118 | s_half_psi = sin(0.5 * psi); 119 | quat(0) = tilt0 * c_half_psi; 120 | quat(1) = tilt1 * c_half_psi + tilt2 * s_half_psi; 121 | quat(2) = tilt2 * c_half_psi - tilt1 * s_half_psi; 122 | quat(3) = tilt0 * s_half_psi; 123 | c_psi = cos(psi); 124 | s_psi = sin(psi); 125 | omg_den = z2 + 1.0; 126 | omg_term = dz2 / omg_den; 127 | omg(0) = dz0 * s_psi - dz1 * c_psi - 128 | (z0 * s_psi - z1 * c_psi) * omg_term; 129 | omg(1) = dz0 * c_psi + dz1 * s_psi - 130 | (z0 * c_psi + z1 * s_psi) * omg_term; 131 | omg(2) = (z1 * dz0 - z0 * dz1) / omg_den + dpsi; 132 | 133 | return; 134 | } 135 | 136 | inline void backward(const Eigen::Vector3d &pos_grad, 137 | const Eigen::Vector3d &vel_grad, 138 | const double &thr_grad, 139 | const Eigen::Vector4d &quat_grad, 140 | const Eigen::Vector3d &omg_grad, 141 | Eigen::Vector3d &pos_total_grad, 142 | Eigen::Vector3d &vel_total_grad, 143 | Eigen::Vector3d &acc_total_grad, 144 | Eigen::Vector3d &jer_total_grad, 145 | double &psi_total_grad, 146 | double &dpsi_total_grad) const 147 | { 148 | double w0b, w1b, w2b, dw0b, dw1b, dw2b; 149 | double z0b, z1b, z2b, dz0b, dz1b, dz2b; 150 | double v_sqr_normb, cp_termb, w_termb; 151 | double zu_sqr_normb, zu_normb, zu0b, zu1b, zu2b; 152 | double zu_sqr0b, zu_sqr1b, zu_sqr2b, zu01b, zu12b, zu02b; 153 | double ng00b, ng01b, ng02b, ng11b, ng12b, ng22b, ng_denb; 154 | double dz_term0b, dz_term1b, dz_term2b, f_term0b, f_term1b, f_term2b; 155 | double tilt_denb, tilt0b, tilt1b, tilt2b, head0b, head3b; 156 | double cpsib, spsib, omg_denb, omg_termb; 157 | double tempb, tilt_den_sqr; 158 | 159 | tilt0b = s_half_psi * (quat_grad(3)) + c_half_psi * (quat_grad(0)); 160 | head3b = tilt0 * (quat_grad(3)) + tilt2 * (quat_grad(1)) - tilt1 * (quat_grad(2)); 161 | tilt2b = c_half_psi * (quat_grad(2)) + s_half_psi * (quat_grad(1)); 162 | head0b = tilt2 * (quat_grad(2)) + tilt1 * (quat_grad(1)) + tilt0 * (quat_grad(0)); 163 | tilt1b = c_half_psi * (quat_grad(1)) - s_half_psi * (quat_grad(2)); 164 | tilt_den_sqr = tilt_den * tilt_den; 165 | tilt_denb = (z1 * tilt1b - z0 * tilt2b) / tilt_den_sqr + 0.5 * tilt0b; 166 | omg_termb = -((z0 * c_psi + z1 * s_psi) * (omg_grad(1))) - 167 | (z0 * s_psi - z1 * c_psi) * (omg_grad(0)); 168 | tempb = omg_grad(2) / omg_den; 169 | dpsi_total_grad = omg_grad(2); 170 | z1b = dz0 * tempb; 171 | dz0b = z1 * tempb + c_psi * (omg_grad(1)) + s_psi * (omg_grad(0)); 172 | z0b = -(dz1 * tempb); 173 | dz1b = s_psi * (omg_grad(1)) - z0 * tempb - c_psi * (omg_grad(0)); 174 | omg_denb = -((z1 * dz0 - z0 * dz1) * tempb / omg_den) - 175 | dz2 * omg_termb / (omg_den * omg_den); 176 | tempb = -(omg_term * (omg_grad(1))); 177 | cpsib = dz0 * (omg_grad(1)) + z0 * tempb; 178 | spsib = dz1 * (omg_grad(1)) + z1 * tempb; 179 | z0b += c_psi * tempb; 180 | z1b += s_psi * tempb; 181 | tempb = -(omg_term * (omg_grad(0))); 182 | spsib += dz0 * (omg_grad(0)) + z0 * tempb; 183 | cpsib += -dz1 * (omg_grad(0)) - z1 * tempb; 184 | z0b += s_psi * tempb + tilt2b / tilt_den + f_term0 * (thr_grad); 185 | z1b += -c_psi * tempb - tilt1b / tilt_den + f_term1 * (thr_grad); 186 | dz2b = omg_termb / omg_den; 187 | z2b = omg_denb + tilt_denb / tilt_den + f_term2 * (thr_grad); 188 | psi_total_grad = c_psi * spsib + 0.5 * c_half_psi * head3b - 189 | s_psi * cpsib - 0.5 * s_half_psi * head0b; 190 | f_term0b = z0 * (thr_grad); 191 | f_term1b = z1 * (thr_grad); 192 | f_term2b = z2 * (thr_grad); 193 | ng02b = dz_term0 * dz2b + dz_term2 * dz0b; 194 | dz_term0b = ng02 * dz2b + ng01 * dz1b + ng00 * dz0b; 195 | ng12b = dz_term1 * dz2b + dz_term2 * dz1b; 196 | dz_term1b = ng12 * dz2b + ng11 * dz1b + ng01 * dz0b; 197 | ng22b = dz_term2 * dz2b; 198 | dz_term2b = ng22 * dz2b + ng12 * dz1b + ng02 * dz0b; 199 | ng01b = dz_term0 * dz1b + dz_term1 * dz0b; 200 | ng11b = dz_term1 * dz1b; 201 | ng00b = dz_term0 * dz0b; 202 | jer_total_grad(2) = dz_term2b; 203 | dw2b = dh_over_m * dz_term2b; 204 | jer_total_grad(1) = dz_term1b; 205 | dw1b = dh_over_m * dz_term1b; 206 | jer_total_grad(0) = dz_term0b; 207 | dw0b = dh_over_m * dz_term0b; 208 | tempb = cp * (v2 * dw2b + v1 * dw1b + v0 * dw0b) / cp_term; 209 | acc_total_grad(2) = mass * f_term2b + w_term * dw2b + v2 * tempb; 210 | acc_total_grad(1) = mass * f_term1b + w_term * dw1b + v1 * tempb; 211 | acc_total_grad(0) = mass * f_term0b + w_term * dw0b + v0 * tempb; 212 | vel_total_grad(2) = dw_term * dw2b + a2 * tempb; 213 | vel_total_grad(1) = dw_term * dw1b + a1 * tempb; 214 | vel_total_grad(0) = dw_term * dw0b + a0 * tempb; 215 | cp_termb = -(v_dot_a * tempb / cp_term); 216 | tempb = ng22b / ng_den; 217 | zu_sqr0b = tempb; 218 | zu_sqr1b = tempb; 219 | ng_denb = -((zu_sqr0 + zu_sqr1) * tempb / ng_den); 220 | zu12b = -(ng12b / ng_den); 221 | tempb = ng11b / ng_den; 222 | ng_denb += zu12 * ng12b / (ng_den * ng_den) - 223 | (zu_sqr0 + zu_sqr2) * tempb / ng_den; 224 | zu_sqr0b += tempb; 225 | zu_sqr2b = tempb; 226 | zu02b = -(ng02b / ng_den); 227 | zu01b = -(ng01b / ng_den); 228 | tempb = ng00b / ng_den; 229 | ng_denb += zu02 * ng02b / (ng_den * ng_den) + 230 | zu01 * ng01b / (ng_den * ng_den) - 231 | (zu_sqr1 + zu_sqr2) * tempb / ng_den; 232 | zu_normb = zu_sqr_norm * ng_denb - 233 | (zu2 * z2b + zu1 * z1b + zu0 * z0b) / zu_sqr_norm; 234 | zu_sqr_normb = zu_norm * ng_denb + zu_normb / (2.0 * zu_norm); 235 | tempb += zu_sqr_normb; 236 | zu_sqr1b += tempb; 237 | zu_sqr2b += tempb; 238 | zu2b = z2b / zu_norm + zu0 * zu02b + zu1 * zu12b + 2 * zu2 * zu_sqr2b; 239 | w2b = dv * f_term2b + dh_over_m * zu2b; 240 | zu1b = z1b / zu_norm + zu2 * zu12b + zu0 * zu01b + 2 * zu1 * zu_sqr1b; 241 | w1b = dv * f_term1b + dh_over_m * zu1b; 242 | zu_sqr0b += zu_sqr_normb; 243 | zu0b = z0b / zu_norm + zu2 * zu02b + zu1 * zu01b + 2 * zu0 * zu_sqr0b; 244 | w0b = dv * f_term0b + dh_over_m * zu0b; 245 | w_termb = a2 * dw2b + a1 * dw1b + a0 * dw0b + 246 | v2 * w2b + v1 * w1b + v0 * w0b; 247 | acc_total_grad(2) += zu2b; 248 | acc_total_grad(1) += zu1b; 249 | acc_total_grad(0) += zu0b; 250 | cp_termb += cp * w_termb; 251 | v_sqr_normb = cp_termb / (2.0 * cp_term); 252 | vel_total_grad(2) += w_term * w2b + 2 * v2 * v_sqr_normb + vel_grad(2); 253 | vel_total_grad(1) += w_term * w1b + 2 * v1 * v_sqr_normb + vel_grad(1); 254 | vel_total_grad(0) += w_term * w0b + 2 * v0 * v_sqr_normb + vel_grad(0); 255 | pos_total_grad(2) = pos_grad(2); 256 | pos_total_grad(1) = pos_grad(1); 257 | pos_total_grad(0) = pos_grad(0); 258 | 259 | return; 260 | } 261 | 262 | private: 263 | double mass, grav, dh, dv, cp, veps; 264 | 265 | double v0, v1, v2, a0, a1, a2, v_dot_a; 266 | double z0, z1, z2, dz0, dz1, dz2; 267 | double cp_term, w_term, dh_over_m; 268 | double zu_sqr_norm, zu_norm, zu0, zu1, zu2; 269 | double zu_sqr0, zu_sqr1, zu_sqr2, zu01, zu12, zu02; 270 | double ng00, ng01, ng02, ng11, ng12, ng22, ng_den; 271 | double dw_term, dz_term0, dz_term1, dz_term2, f_term0, f_term1, f_term2; 272 | double tilt_den, tilt0, tilt1, tilt2, c_half_psi, s_half_psi; 273 | double c_psi, s_psi, omg_den, omg_term; 274 | }; 275 | } 276 | 277 | #endif 278 | -------------------------------------------------------------------------------- /Traj_gen by L-BFGS/src/gcopter/include/gcopter/geo_utils.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | MIT License 3 | 4 | Copyright (c) 2021 Zhepei Wang (wangzhepei@live.com) 5 | 6 | Permission is hereby granted, free of charge, to any person obtaining a copy 7 | of this software and associated documentation files (the "Software"), to deal 8 | in the Software without restriction, including without limitation the rights 9 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | copies of the Software, and to permit persons to whom the Software is 11 | furnished to do so, subject to the following conditions: 12 | 13 | The above copyright notice and this permission notice shall be included in all 14 | copies or substantial portions of the Software. 15 | 16 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | SOFTWARE. 23 | */ 24 | 25 | #ifndef GEO_UTILS_HPP 26 | #define GEO_UTILS_HPP 27 | 28 | #include "quickhull.hpp" 29 | #include "sdlp.hpp" 30 | 31 | #include 32 | 33 | #include 34 | #include 35 | #include 36 | #include 37 | 38 | namespace geo_utils 39 | { 40 | 41 | // Each row of hPoly is defined by h0, h1, h2, h3 as 42 | // h0*x + h1*y + h2*z + h3 <= 0 43 | inline bool findInterior(const Eigen::MatrixX4d &hPoly, 44 | Eigen::Vector3d &interior) 45 | { 46 | const int m = hPoly.rows(); 47 | 48 | Eigen::MatrixX4d A(m, 4); 49 | Eigen::VectorXd b(m); 50 | Eigen::Vector4d c, x; 51 | const Eigen::ArrayXd hNorm = hPoly.leftCols<3>().rowwise().norm(); 52 | A.leftCols<3>() = hPoly.leftCols<3>().array().colwise() / hNorm; 53 | A.rightCols<1>().setConstant(1.0); 54 | b = -hPoly.rightCols<1>().array() / hNorm; 55 | c.setZero(); 56 | c(3) = -1.0; 57 | 58 | const double minmaxsd = sdlp::linprog<4>(c, A, b, x); 59 | interior = x.head<3>(); 60 | 61 | return minmaxsd < 0.0 && !std::isinf(minmaxsd); 62 | } 63 | 64 | inline bool overlap(const Eigen::MatrixX4d &hPoly0, 65 | const Eigen::MatrixX4d &hPoly1, 66 | const double eps = 1.0e-6) 67 | 68 | { 69 | const int m = hPoly0.rows(); 70 | const int n = hPoly1.rows(); 71 | Eigen::MatrixX4d A(m + n, 4); 72 | Eigen::Vector4d c, x; 73 | Eigen::VectorXd b(m + n); 74 | A.leftCols<3>().topRows(m) = hPoly0.leftCols<3>(); 75 | A.leftCols<3>().bottomRows(n) = hPoly1.leftCols<3>(); 76 | A.rightCols<1>().setConstant(1.0); 77 | b.topRows(m) = -hPoly0.rightCols<1>(); 78 | b.bottomRows(n) = -hPoly1.rightCols<1>(); 79 | c.setZero(); 80 | c(3) = -1.0; 81 | 82 | const double minmaxsd = sdlp::linprog<4>(c, A, b, x); 83 | 84 | return minmaxsd < -eps && !std::isinf(minmaxsd); 85 | } 86 | 87 | struct filterLess 88 | { 89 | inline bool operator()(const Eigen::Vector3d &l, 90 | const Eigen::Vector3d &r) 91 | { 92 | return l(0) < r(0) || 93 | (l(0) == r(0) && 94 | (l(1) < r(1) || 95 | (l(1) == r(1) && 96 | l(2) < r(2)))); 97 | } 98 | }; 99 | 100 | inline void filterVs(const Eigen::Matrix3Xd &rV, 101 | const double &epsilon, 102 | Eigen::Matrix3Xd &fV) 103 | { 104 | const double mag = std::max(fabs(rV.maxCoeff()), fabs(rV.minCoeff())); 105 | const double res = mag * std::max(fabs(epsilon) / mag, DBL_EPSILON); 106 | std::set filter; 107 | fV = rV; 108 | int offset = 0; 109 | Eigen::Vector3d quanti; 110 | for (int i = 0; i < rV.cols(); i++) 111 | { 112 | quanti = (rV.col(i) / res).array().round(); 113 | if (filter.find(quanti) == filter.end()) 114 | { 115 | filter.insert(quanti); 116 | fV.col(offset) = rV.col(i); 117 | offset++; 118 | } 119 | } 120 | fV = fV.leftCols(offset).eval(); 121 | return; 122 | } 123 | 124 | // Each row of hPoly is defined by h0, h1, h2, h3 as 125 | // h0*x + h1*y + h2*z + h3 <= 0 126 | // proposed epsilon is 1.0e-6 127 | inline void enumerateVs(const Eigen::MatrixX4d &hPoly, 128 | const Eigen::Vector3d &inner, 129 | Eigen::Matrix3Xd &vPoly, 130 | const double epsilon = 1.0e-6) 131 | { 132 | const Eigen::VectorXd b = -hPoly.rightCols<1>() - hPoly.leftCols<3>() * inner; 133 | const Eigen::Matrix A = 134 | (hPoly.leftCols<3>().array().colwise() / b.array()).transpose(); 135 | 136 | quickhull::QuickHull qh; 137 | const double qhullEps = std::min(epsilon, quickhull::defaultEps()); 138 | // CCW is false because the normal in quickhull towards interior 139 | const auto cvxHull = qh.getConvexHull(A.data(), A.cols(), false, true, qhullEps); 140 | const auto &idBuffer = cvxHull.getIndexBuffer(); 141 | const int hNum = idBuffer.size() / 3; 142 | Eigen::Matrix3Xd rV(3, hNum); 143 | Eigen::Vector3d normal, point, edge0, edge1; 144 | for (int i = 0; i < hNum; i++) 145 | { 146 | point = A.col(idBuffer[3 * i + 1]); 147 | edge0 = point - A.col(idBuffer[3 * i]); 148 | edge1 = A.col(idBuffer[3 * i + 2]) - point; 149 | normal = edge0.cross(edge1); //cross in CW gives an outter normal 150 | rV.col(i) = normal / normal.dot(point); 151 | } 152 | filterVs(rV, epsilon, vPoly); 153 | vPoly = (vPoly.array().colwise() + inner.array()).eval(); 154 | return; 155 | } 156 | 157 | // Each row of hPoly is defined by h0, h1, h2, h3 as 158 | // h0*x + h1*y + h2*z + h3 <= 0 159 | // proposed epsilon is 1.0e-6 160 | inline bool enumerateVs(const Eigen::MatrixX4d &hPoly, 161 | Eigen::Matrix3Xd &vPoly, 162 | const double epsilon = 1.0e-6) 163 | { 164 | Eigen::Vector3d inner; 165 | if (findInterior(hPoly, inner)) 166 | { 167 | enumerateVs(hPoly, inner, vPoly, epsilon); 168 | return true; 169 | } 170 | else 171 | { 172 | return false; 173 | } 174 | } 175 | 176 | } // namespace geo_utils 177 | 178 | #endif 179 | -------------------------------------------------------------------------------- /Traj_gen by L-BFGS/src/gcopter/include/gcopter/path_smoother.hpp: -------------------------------------------------------------------------------- 1 | #ifndef PATH_SMOOTHER_HPP 2 | #define PATH_SMOOTHER_HPP 3 | 4 | #include "cubic_spline.hpp" 5 | #include "lbfgs.hpp" 6 | 7 | #include 8 | 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | 15 | namespace path_smoother 16 | { 17 | 18 | class PathSmoother 19 | { 20 | private: 21 | cubic_spline::CubicSpline cubSpline; 22 | 23 | int pieceN; 24 | Eigen::Matrix3Xd diskObstacles; 25 | double penaltyWeight; 26 | Eigen::Vector2d headP; 27 | Eigen::Vector2d tailP; 28 | Eigen::Matrix2Xd points; 29 | Eigen::Matrix2Xd gradByPoints; 30 | 31 | lbfgs::lbfgs_parameter_t lbfgs_params; 32 | 33 | private: 34 | static inline double costFunction(void *ptr, 35 | const Eigen::VectorXd &x, 36 | Eigen::VectorXd &g) 37 | { 38 | // Hints:This is a function which define the costfunction, you can use 39 | // the content which *ptr point to initialize a quote of Class: 40 | // PathSmoother, the you can use the quote to visit the cubSpline 41 | // TODO 42 | auto instance = reinterpret_cast(ptr); 43 | const int points_nums = instance->pieceN - 1; 44 | Eigen::Matrix2Xd grad; 45 | grad.resize(2, points_nums); 46 | grad.setZero(); 47 | double cost = 0.0; 48 | 49 | // * step1: 送入目标点，计算曲线系数 50 | Eigen::Matrix2Xd inPs; 51 | inPs.resize(2, points_nums); 52 | inPs.row(0) = x.head(points_nums); 53 | inPs.row(1) = x.tail(points_nums); 54 | instance->cubSpline.setInnerPoints(inPs); 55 | 56 | // * step2: 计算能量函数值和梯度 57 | double energy = 0.0; 58 | Eigen::Matrix2Xd energy_grad; 59 | energy_grad.resize(2, points_nums); 60 | energy_grad.setZero(); 61 | instance->cubSpline.getStretchEnergy(energy); 62 | instance->cubSpline.getGrad(energy_grad); 63 | 64 | cost += energy; 65 | grad += energy_grad; 66 | 67 | // * step3: 计算碰撞函数和代价 68 | double obstacles = 0.0; 69 | Eigen::Matrix2Xd potential_grad; 70 | potential_grad.resize(2, points_nums); 71 | potential_grad.setZero(); 72 | for (int i = 0; i < points_nums; ++i){ 73 | for (int j = 0; j < instance->diskObstacles.cols(); ++j){ 74 | Eigen::Vector2d diff = inPs.col(i) - instance->diskObstacles.col(j).head(2); 75 | double distance = diff.norm(); 76 | double delta = instance->diskObstacles(2, j) - distance; 77 | 78 | if (delta > 0.0){ 79 | obstacles += instance->penaltyWeight * delta; 80 | potential_grad.col(i) += instance->penaltyWeight * ( -diff / distance); 81 | } 82 | } 83 | } 84 | cost += obstacles; 85 | grad += potential_grad; 86 | 87 | // * step4: 得到结果 88 | g.setZero(); 89 | g.head(points_nums) = grad.row(0).transpose(); 90 | g.tail(points_nums) = grad.row(1).transpose(); 91 | 92 | 93 | std::cout << std::setprecision(10) 94 | << "================================" << std::endl 95 | << "points_nums: " << points_nums << std::endl 96 | << "Function Value: " << cost << std::endl 97 | << "Gradient Inf Norm: " << g.cwiseAbs().maxCoeff() << std::endl 98 | << "================================" << std::endl; 99 | 100 | return cost; 101 | } 102 | 103 | public: 104 | inline bool setup(const Eigen::Vector2d &initialP, 105 | const Eigen::Vector2d &terminalP, 106 | const int &pieceNum, 107 | const Eigen::Matrix3Xd &diskObs, 108 | const double penaWeight) 109 | { 110 | pieceN = pieceNum; 111 | diskObstacles = diskObs; 112 | penaltyWeight = penaWeight; 113 | headP = initialP; 114 | tailP = terminalP; 115 | 116 | cubSpline.setConditions(headP, tailP, pieceN); 117 | 118 | points.resize(2, pieceN - 1); 119 | gradByPoints.resize(2, pieceN - 1); 120 | 121 | return true; 122 | } 123 | 124 | 125 | inline double optimize(CubicCurve &curve, 126 | const Eigen::Matrix2Xd &iniInPs, 127 | const double &relCostTol) 128 | { 129 | // NOT TODO 130 | Eigen::VectorXd x(pieceN * 2 - 2); 131 | Eigen::Map innerP(x.data(), 2, pieceN - 1); 132 | innerP = iniInPs; 133 | 134 | double minCost = 0.0; 135 | lbfgs_params.mem_size = 64; 136 | lbfgs_params.past = 3; 137 | lbfgs_params.min_step = 1.0e-32; 138 | lbfgs_params.g_epsilon = 1.0e-6; 139 | lbfgs_params.delta = relCostTol; 140 | 141 | int ret = lbfgs::lbfgs_optimize(x, 142 | minCost, 143 | &PathSmoother::costFunction, 144 | nullptr, 145 | this, 146 | lbfgs_params); 147 | 148 | if (ret >= 0) 149 | { 150 | // cubSpline.setInnerPoints(innerP); 151 | cubSpline.getCurve(curve); 152 | } 153 | else 154 | { 155 | curve.clear(); 156 | minCost = INFINITY; 157 | std::cout << "Optimization Failed: " 158 | << lbfgs::lbfgs_strerror(ret) 159 | << std::endl; 160 | } 161 | 162 | return minCost; 163 | } 164 | }; 165 | 166 | } 167 | 168 | #endif 169 | -------------------------------------------------------------------------------- /Traj_gen by L-BFGS/src/gcopter/include/gcopter/sfc_gen.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | MIT License 3 | 4 | Copyright (c) 2021 Zhepei Wang (wangzhepei@live.com) 5 | 6 | Permission is hereby granted, free of charge, to any person obtaining a copy 7 | of this software and associated documentation files (the "Software"), to deal 8 | in the Software without restriction, including without limitation the rights 9 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | copies of the Software, and to permit persons to whom the Software is 11 | furnished to do so, subject to the following conditions: 12 | 13 | The above copyright notice and this permission notice shall be included in all 14 | copies or substantial portions of the Software. 15 | 16 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | SOFTWARE. 23 | */ 24 | 25 | #ifndef SFC_GEN_HPP 26 | #define SFC_GEN_HPP 27 | 28 | #include "geo_utils.hpp" 29 | #include "firi.hpp" 30 | 31 | #include 32 | #include 33 | #include 34 | #include 35 | #include 36 | #include 37 | 38 | #include 39 | #include 40 | #include 41 | 42 | namespace sfc_gen 43 | { 44 | 45 | template 46 | inline double planPath(const Eigen::Vector3d &s, 47 | const Eigen::Vector3d &g, 48 | const Eigen::Vector3d &lb, 49 | const Eigen::Vector3d &hb, 50 | const Map *mapPtr, 51 | const double &timeout, 52 | std::vector &p) 53 | { 54 | auto space(std::make_shared(3)); 55 | 56 | ompl::base::RealVectorBounds bounds(3); 57 | bounds.setLow(0, 0.0); 58 | bounds.setHigh(0, hb(0) - lb(0)); 59 | bounds.setLow(1, 0.0); 60 | bounds.setHigh(1, hb(1) - lb(1)); 61 | bounds.setLow(2, 0.0); 62 | bounds.setHigh(2, hb(2) - lb(2)); 63 | space->setBounds(bounds); 64 | 65 | auto si(std::make_shared(space)); 66 | 67 | si->setStateValidityChecker( 68 | [&](const ompl::base::State *state) 69 | { 70 | const auto *pos = state->as(); 71 | const Eigen::Vector3d position(lb(0) + (*pos)[0], 72 | lb(1) + (*pos)[1], 73 | lb(2) + (*pos)[2]); 74 | return mapPtr->query(position) == 0; 75 | }); 76 | si->setup(); 77 | 78 | ompl::msg::setLogLevel(ompl::msg::LOG_NONE); 79 | 80 | ompl::base::ScopedState<> start(space), goal(space); 81 | start[0] = s(0) - lb(0); 82 | start[1] = s(1) - lb(1); 83 | start[2] = s(2) - lb(2); 84 | goal[0] = g(0) - lb(0); 85 | goal[1] = g(1) - lb(1); 86 | goal[2] = g(2) - lb(2); 87 | 88 | auto pdef(std::make_shared(si)); 89 | pdef->setStartAndGoalStates(start, goal); 90 | pdef->setOptimizationObjective(std::make_shared(si)); 91 | auto planner(std::make_shared(si)); 92 | planner->setProblemDefinition(pdef); 93 | planner->setup(); 94 | 95 | ompl::base::PlannerStatus solved; 96 | solved = planner->ompl::base::Planner::solve(timeout); 97 | 98 | double cost = INFINITY; 99 | if (solved) 100 | { 101 | p.clear(); 102 | const ompl::geometric::PathGeometric path_ = 103 | ompl::geometric::PathGeometric( 104 | dynamic_cast(*pdef->getSolutionPath())); 105 | for (size_t i = 0; i < path_.getStateCount(); i++) 106 | { 107 | const auto state = path_.getState(i)->as()->values; 108 | p.emplace_back(lb(0) + state[0], lb(1) + state[1], lb(2) + state[2]); 109 | } 110 | cost = pdef->getSolutionPath()->cost(pdef->getOptimizationObjective()).value(); 111 | } 112 | 113 | return cost; 114 | } 115 | 116 | inline void convexCover(const std::vector &path, 117 | const std::vector &points, 118 | const Eigen::Vector3d &lowCorner, 119 | const Eigen::Vector3d &highCorner, 120 | const double &progress, 121 | const double &range, 122 | std::vector &hpolys, 123 | const double eps = 1.0e-6) 124 | { 125 | hpolys.clear(); 126 | const int n = path.size(); 127 | Eigen::Matrix bd = Eigen::Matrix::Zero(); 128 | bd(0, 0) = 1.0; 129 | bd(1, 0) = -1.0; 130 | bd(2, 1) = 1.0; 131 | bd(3, 1) = -1.0; 132 | bd(4, 2) = 1.0; 133 | bd(5, 2) = -1.0; 134 | 135 | Eigen::MatrixX4d hp, gap; 136 | Eigen::Vector3d a, b = path[0]; 137 | std::vector valid_pc; 138 | std::vector bs; 139 | valid_pc.reserve(points.size()); 140 | for (int i = 1; i < n;) 141 | { 142 | a = b; 143 | if ((a - path[i]).norm() > progress) 144 | { 145 | b = (path[i] - a).normalized() * progress + a; 146 | } 147 | else 148 | { 149 | b = path[i]; 150 | i++; 151 | } 152 | bs.emplace_back(b); 153 | 154 | bd(0, 3) = -std::min(std::max(a(0), b(0)) + range, highCorner(0)); 155 | bd(1, 3) = +std::max(std::min(a(0), b(0)) - range, lowCorner(0)); 156 | bd(2, 3) = -std::min(std::max(a(1), b(1)) + range, highCorner(1)); 157 | bd(3, 3) = +std::max(std::min(a(1), b(1)) - range, lowCorner(1)); 158 | bd(4, 3) = -std::min(std::max(a(2), b(2)) + range, highCorner(2)); 159 | bd(5, 3) = +std::max(std::min(a(2), b(2)) - range, lowCorner(2)); 160 | 161 | valid_pc.clear(); 162 | for (const Eigen::Vector3d &p : points) 163 | { 164 | if ((bd.leftCols<3>() * p + bd.rightCols<1>()).maxCoeff() < 0.0) 165 | { 166 | valid_pc.emplace_back(p); 167 | } 168 | } 169 | Eigen::Map> pc(valid_pc[0].data(), 3, valid_pc.size()); 170 | 171 | firi::firi(bd, pc, a, b, hp); 172 | 173 | if (hpolys.size() != 0) 174 | { 175 | const Eigen::Vector4d ah(a(0), a(1), a(2), 1.0); 176 | if (3 <= ((hp * ah).array() > -eps).cast().sum() + 177 | ((hpolys.back() * ah).array() > -eps).cast().sum()) 178 | { 179 | firi::firi(bd, pc, a, a, gap, 1); 180 | hpolys.emplace_back(gap); 181 | } 182 | } 183 | 184 | hpolys.emplace_back(hp); 185 | } 186 | } 187 | 188 | inline void shortCut(std::vector &hpolys) 189 | { 190 | std::vector htemp = hpolys; 191 | if (htemp.size() == 1) 192 | { 193 | Eigen::MatrixX4d headPoly = htemp.front(); 194 | htemp.insert(htemp.begin(), headPoly); 195 | } 196 | hpolys.clear(); 197 | 198 | int M = htemp.size(); 199 | Eigen::MatrixX4d hPoly; 200 | bool overlap; 201 | std::deque idices; 202 | idices.push_front(M - 1); 203 | for (int i = M - 1; i >= 0; i--) 204 | { 205 | for (int j = 0; j < i; j++) 206 | { 207 | if (j < i - 1) 208 | { 209 | overlap = geo_utils::overlap(htemp[i], htemp[j], 0.01); 210 | } 211 | else 212 | { 213 | overlap = true; 214 | } 215 | if (overlap) 216 | { 217 | idices.push_front(j); 218 | i = j + 1; 219 | break; 220 | } 221 | } 222 | } 223 | for (const auto &ele : idices) 224 | { 225 | hpolys.push_back(htemp[ele]); 226 | } 227 | } 228 | 229 | } 230 | 231 | #endif 232 | -------------------------------------------------------------------------------- /Traj_gen by L-BFGS/src/gcopter/include/gcopter/voxel_dilater.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | MIT License 3 | 4 | Copyright (c) 2021 Zhepei Wang (wangzhepei@live.com) 5 | 6 | Permission is hereby granted, free of charge, to any person obtaining a copy 7 | of this software and associated documentation files (the "Software"), to deal 8 | in the Software without restriction, including without limitation the rights 9 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | copies of the Software, and to permit persons to whom the Software is 11 | furnished to do so, subject to the following conditions: 12 | 13 | The above copyright notice and this permission notice shall be included in all 14 | copies or substantial portions of the Software. 15 | 16 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | SOFTWARE. 23 | */ 24 | 25 | #ifndef VOXEL_DILATER 26 | #define VOXEL_DILATER(i, j, k, x, y, z, sy, sz, bx, by, bz, ck, ogm, ofst, val, fdl) \ 27 | (ck) = (x) == 0 || (x) == (bx) || (y) == 0 || (y) == (by) || (z) == 0 || (z) == (bz); \ 28 | (i) = (x) - 1; (j) = (y) - (sy); (k) = (z) - (sz); (ofst) = (i) + (j) + (k); if ((!(ck) || ((ck) && (i) >= 0 && (j) >= 0 && (k) >= 0 )) && (ogm)[(ofst)] == 0) { (ogm)[(ofst)] = (val); (fdl).emplace_back((i), (j), (k)); } \ 29 | (i) = (x) - 1; (j) = (y) - (sy); (k) = (z); (ofst) = (i) + (j) + (k); if ((!(ck) || ((ck) && (i) >= 0 && (j) >= 0 )) && (ogm)[(ofst)] == 0) { (ogm)[(ofst)] = (val); (fdl).emplace_back((i), (j), (k)); } \ 30 | (i) = (x) - 1; (j) = (y) - (sy); (k) = (z) + (sz); (ofst) = (i) + (j) + (k); if ((!(ck) || ((ck) && (i) >= 0 && (j) >= 0 && (k) <= (bz))) && (ogm)[(ofst)] == 0) { (ogm)[(ofst)] = (val); (fdl).emplace_back((i), (j), (k)); } \ 31 | (i) = (x) - 1; (j) = (y); (k) = (z) - (sz); (ofst) = (i) + (j) + (k); if ((!(ck) || ((ck) && (i) >= 0 && (k) >= 0 )) && (ogm)[(ofst)] == 0) { (ogm)[(ofst)] = (val); (fdl).emplace_back((i), (j), (k)); } \ 32 | (i) = (x) - 1; (j) = (y); (k) = (z); (ofst) = (i) + (j) + (k); if ((!(ck) || ((ck) && (i) >= 0 )) && (ogm)[(ofst)] == 0) { (ogm)[(ofst)] = (val); (fdl).emplace_back((i), (j), (k)); } \ 33 | (i) = (x) - 1; (j) = (y); (k) = (z) + (sz); (ofst) = (i) + (j) + (k); if ((!(ck) || ((ck) && (i) >= 0 && (k) <= (bz))) && (ogm)[(ofst)] == 0) { (ogm)[(ofst)] = (val); (fdl).emplace_back((i), (j), (k)); } \ 34 | (i) = (x) - 1; (j) = (y) + (sy); (k) = (z) - (sz); (ofst) = (i) + (j) + (k); if ((!(ck) || ((ck) && (i) >= 0 && (j) <= (by) && (k) >= 0 )) && (ogm)[(ofst)] == 0) { (ogm)[(ofst)] = (val); (fdl).emplace_back((i), (j), (k)); } \ 35 | (i) = (x) - 1; (j) = (y) + (sy); (k) = (z); (ofst) = (i) + (j) + (k); if ((!(ck) || ((ck) && (i) >= 0 && (j) <= (by) )) && (ogm)[(ofst)] == 0) { (ogm)[(ofst)] = (val); (fdl).emplace_back((i), (j), (k)); } \ 36 | (i) = (x) - 1; (j) = (y) + (sy); (k) = (z) + (sz); (ofst) = (i) + (j) + (k); if ((!(ck) || ((ck) && (i) >= 0 && (j) <= (by) && (k) <= (bz))) && (ogm)[(ofst)] == 0) { (ogm)[(ofst)] = (val); (fdl).emplace_back((i), (j), (k)); } \ 37 | (i) = (x); (j) = (y) - (sy); (k) = (z) - (sz); (ofst) = (i) + (j) + (k); if ((!(ck) || ((ck) && (j) >= 0 && (k) >= 0 )) && (ogm)[(ofst)] == 0) { (ogm)[(ofst)] = (val); (fdl).emplace_back((i), (j), (k)); } \ 38 | (i) = (x); (j) = (y) - (sy); (k) = (z); (ofst) = (i) + (j) + (k); if ((!(ck) || ((ck) && (j) >= 0 )) && (ogm)[(ofst)] == 0) { (ogm)[(ofst)] = (val); (fdl).emplace_back((i), (j), (k)); } \ 39 | (i) = (x); (j) = (y) - (sy); (k) = (z) + (sz); (ofst) = (i) + (j) + (k); if ((!(ck) || ((ck) && (j) >= 0 && (k) <= (bz))) && (ogm)[(ofst)] == 0) { (ogm)[(ofst)] = (val); (fdl).emplace_back((i), (j), (k)); } \ 40 | (i) = (x); (j) = (y); (k) = (z) - (sz); (ofst) = (i) + (j) + (k); if ((!(ck) || ((ck) && (k) >= 0 )) && (ogm)[(ofst)] == 0) { (ogm)[(ofst)] = (val); (fdl).emplace_back((i), (j), (k)); } \ 41 | (i) = (x); (j) = (y); (k) = (z) + (sz); (ofst) = (i) + (j) + (k); if ((!(ck) || ((ck) && (k) <= (bz))) && (ogm)[(ofst)] == 0) { (ogm)[(ofst)] = (val); (fdl).emplace_back((i), (j), (k)); } \ 42 | (i) = (x); (j) = (y) + (sy); (k) = (z) - (sz); (ofst) = (i) + (j) + (k); if ((!(ck) || ((ck) && (j) <= (by) && (k) >= 0 )) && (ogm)[(ofst)] == 0) { (ogm)[(ofst)] = (val); (fdl).emplace_back((i), (j), (k)); } \ 43 | (i) = (x); (j) = (y) + (sy); (k) = (z); (ofst) = (i) + (j) + (k); if ((!(ck) || ((ck) && (j) <= (by) )) && (ogm)[(ofst)] == 0) { (ogm)[(ofst)] = (val); (fdl).emplace_back((i), (j), (k)); } \ 44 | (i) = (x); (j) = (y) + (sy); (k) = (z) + (sz); (ofst) = (i) + (j) + (k); if ((!(ck) || ((ck) && (j) <= (by) && (k) <= (bz))) && (ogm)[(ofst)] == 0) { (ogm)[(ofst)] = (val); (fdl).emplace_back((i), (j), (k)); } \ 45 | (i) = (x) + 1; (j) = (y) - (sy); (k) = (z) - (sz); (ofst) = (i) + (j) + (k); if ((!(ck) || ((ck) && (i) <= (bx) && (j) >= 0 && (k) >= 0 )) && (ogm)[(ofst)] == 0) { (ogm)[(ofst)] = (val); (fdl).emplace_back((i), (j), (k)); } \ 46 | (i) = (x) + 1; (j) = (y) - (sy); (k) = (z); (ofst) = (i) + (j) + (k); if ((!(ck) || ((ck) && (i) <= (bx) && (j) >= 0 )) && (ogm)[(ofst)] == 0) { (ogm)[(ofst)] = (val); (fdl).emplace_back((i), (j), (k)); } \ 47 | (i) = (x) + 1; (j) = (y) - (sy); (k) = (z) + (sz); (ofst) = (i) + (j) + (k); if ((!(ck) || ((ck) && (i) <= (bx) && (j) >= 0 && (k) <= (bz))) && (ogm)[(ofst)] == 0) { (ogm)[(ofst)] = (val); (fdl).emplace_back((i), (j), (k)); } \ 48 | (i) = (x) + 1; (j) = (y); (k) = (z) - (sz); (ofst) = (i) + (j) + (k); if ((!(ck) || ((ck) && (i) <= (bx) && (k) >= 0 )) && (ogm)[(ofst)] == 0) { (ogm)[(ofst)] = (val); (fdl).emplace_back((i), (j), (k)); } \ 49 | (i) = (x) + 1; (j) = (y); (k) = (z); (ofst) = (i) + (j) + (k); if ((!(ck) || ((ck) && (i) <= (bx) )) && (ogm)[(ofst)] == 0) { (ogm)[(ofst)] = (val); (fdl).emplace_back((i), (j), (k)); } \ 50 | (i) = (x) + 1; (j) = (y); (k) = (z) + (sz); (ofst) = (i) + (j) + (k); if ((!(ck) || ((ck) && (i) <= (bx) && (k) <= (bz))) && (ogm)[(ofst)] == 0) { (ogm)[(ofst)] = (val); (fdl).emplace_back((i), (j), (k)); } \ 51 | (i) = (x) + 1; (j) = (y) + (sy); (k) = (z) - (sz); (ofst) = (i) + (j) + (k); if ((!(ck) || ((ck) && (i) <= (bx) && (j) <= (by) && (k) >= 0 )) && (ogm)[(ofst)] == 0) { (ogm)[(ofst)] = (val); (fdl).emplace_back((i), (j), (k)); } \ 52 | (i) = (x) + 1; (j) = (y) + (sy); (k) = (z); (ofst) = (i) + (j) + (k); if ((!(ck) || ((ck) && (i) <= (bx) && (j) <= (by) )) && (ogm)[(ofst)] == 0) { (ogm)[(ofst)] = (val); (fdl).emplace_back((i), (j), (k)); } \ 53 | (i) = (x) + 1; (j) = (y) + (sy); (k) = (z) + (sz); (ofst) = (i) + (j) + (k); if ((!(ck) || ((ck) && (i) <= (bx) && (j) <= (by) && (k) <= (bz))) && (ogm)[(ofst)] == 0) { (ogm)[(ofst)] = (val); (fdl).emplace_back((i), (j), (k)); } 54 | #endif 55 | -------------------------------------------------------------------------------- /Traj_gen by L-BFGS/src/gcopter/include/gcopter/voxel_map.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | MIT License 3 | 4 | Copyright (c) 2021 Zhepei Wang (wangzhepei@live.com) 5 | 6 | Permission is hereby granted, free of charge, to any person obtaining a copy 7 | of this software and associated documentation files (the "Software"), to deal 8 | in the Software without restriction, including without limitation the rights 9 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | copies of the Software, and to permit persons to whom the Software is 11 | furnished to do so, subject to the following conditions: 12 | 13 | The above copyright notice and this permission notice shall be included in all 14 | copies or substantial portions of the Software. 15 | 16 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | SOFTWARE. 23 | */ 24 | 25 | #ifndef VOXEL_MAP_HPP 26 | #define VOXEL_MAP_HPP 27 | 28 | #include "voxel_dilater.hpp" 29 | #include 30 | #include 31 | #include 32 | 33 | namespace voxel_map 34 | { 35 | 36 | constexpr uint8_t Unoccupied = 0; 37 | constexpr uint8_t Occupied = 1; 38 | constexpr uint8_t Dilated = 2; 39 | 40 | class VoxelMap 41 | { 42 | 43 | public: 44 | VoxelMap() = default; 45 | VoxelMap(const Eigen::Vector3i &size, 46 | const Eigen::Vector3d &origin, 47 | const double &voxScale) 48 | : mapSize(size), 49 | o(origin), 50 | scale(voxScale), 51 | voxNum(mapSize.prod()), 52 | step(1, mapSize(0), mapSize(1) * mapSize(0)), 53 | oc(o + Eigen::Vector3d::Constant(0.5 * scale)), 54 | bounds((mapSize.array() - 1) * step.array()), 55 | stepScale(step.cast().cwiseInverse() * scale), 56 | voxels(voxNum, Unoccupied) {} 57 | 58 | private: 59 | Eigen::Vector3i mapSize; 60 | Eigen::Vector3d o; 61 | double scale; 62 | int voxNum; 63 | Eigen::Vector3i step; 64 | Eigen::Vector3d oc; 65 | Eigen::Vector3i bounds; 66 | Eigen::Vector3d stepScale; 67 | std::vector voxels; 68 | std::vector surf; 69 | 70 | public: 71 | inline Eigen::Vector3i getSize(void) const 72 | { 73 | return mapSize; 74 | } 75 | 76 | inline double getScale(void) const 77 | { 78 | return scale; 79 | } 80 | 81 | inline Eigen::Vector3d getOrigin(void) const 82 | { 83 | return o; 84 | } 85 | 86 | inline Eigen::Vector3d getCorner(void) const 87 | { 88 | return mapSize.cast() * scale + o; 89 | } 90 | 91 | inline const std::vector &getVoxels(void) const 92 | { 93 | return voxels; 94 | } 95 | 96 | inline void setOccupied(const Eigen::Vector3d &pos) 97 | { 98 | const Eigen::Vector3i id = ((pos - o) / scale).cast(); 99 | if (id(0) >= 0 && id(1) >= 0 && id(2) >= 0 && 100 | id(0) < mapSize(0) && id(1) < mapSize(1) && id(2) < mapSize(2)) 101 | { 102 | voxels[id.dot(step)] = Occupied; 103 | } 104 | } 105 | 106 | inline void setOccupied(const Eigen::Vector3i &id) 107 | { 108 | if (id(0) >= 0 && id(1) >= 0 && id(2) >= 0 && 109 | id(0) < mapSize(0) && id(1) < mapSize(1) && id(2) < mapSize(2)) 110 | { 111 | voxels[id.dot(step)] = Occupied; 112 | } 113 | } 114 | 115 | inline void dilate(const int &r) 116 | { 117 | if (r <= 0) 118 | { 119 | return; 120 | } 121 | else 122 | { 123 | std::vector lvec, cvec; 124 | lvec.reserve(voxNum); 125 | cvec.reserve(voxNum); 126 | int i, j, k, idx; 127 | bool check; 128 | for (int x = 0; x <= bounds(0); x++) 129 | { 130 | for (int y = 0; y <= bounds(1); y += step(1)) 131 | { 132 | for (int z = 0; z <= bounds(2); z += step(2)) 133 | { 134 | if (voxels[x + y + z] == Occupied) 135 | { 136 | VOXEL_DILATER(i, j, k, 137 | x, y, z, 138 | step(1), step(2), 139 | bounds(0), bounds(1), bounds(2), 140 | check, voxels, idx, Dilated, cvec) 141 | } 142 | } 143 | } 144 | } 145 | 146 | for (int loop = 1; loop < r; loop++) 147 | { 148 | std::swap(cvec, lvec); 149 | for (const Eigen::Vector3i &id : lvec) 150 | { 151 | VOXEL_DILATER(i, j, k, 152 | id(0), id(1), id(2), 153 | step(1), step(2), 154 | bounds(0), bounds(1), bounds(2), 155 | check, voxels, idx, Dilated, cvec) 156 | } 157 | lvec.clear(); 158 | } 159 | 160 | surf = cvec; 161 | } 162 | } 163 | 164 | inline void getSurfInBox(const Eigen::Vector3i ¢er, 165 | const int &halfWidth, 166 | std::vector &points) const 167 | { 168 | for (const Eigen::Vector3i &id : surf) 169 | { 170 | if (std::abs(id(0) - center(0)) <= halfWidth && 171 | std::abs(id(1) / step(1) - center(1)) <= halfWidth && 172 | std::abs(id(2) / step(2) - center(2)) <= halfWidth) 173 | { 174 | points.push_back(id.cast().cwiseProduct(stepScale) + oc); 175 | } 176 | } 177 | 178 | return; 179 | } 180 | 181 | inline void getSurf(std::vector &points) const 182 | { 183 | points.reserve(surf.size()); 184 | for (const Eigen::Vector3i &id : surf) 185 | { 186 | points.push_back(id.cast().cwiseProduct(stepScale) + oc); 187 | } 188 | return; 189 | } 190 | 191 | inline bool query(const Eigen::Vector3d &pos) const 192 | { 193 | const Eigen::Vector3i id = ((pos - o) / scale).cast(); 194 | if (id(0) >= 0 && id(1) >= 0 && id(2) >= 0 && 195 | id(0) < mapSize(0) && id(1) < mapSize(1) && id(2) < mapSize(2)) 196 | { 197 | return voxels[id.dot(step)]; 198 | } 199 | else 200 | { 201 | return true; 202 | } 203 | } 204 | 205 | inline bool query(const Eigen::Vector3i &id) const 206 | { 207 | if (id(0) >= 0 && id(1) >= 0 && id(2) >= 0 && 208 | id(0) < mapSize(0) && id(1) < mapSize(1) && id(2) < mapSize(2)) 209 | { 210 | return voxels[id.dot(step)]; 211 | } 212 | else 213 | { 214 | return true; 215 | } 216 | } 217 | 218 | inline Eigen::Vector3d posI2D(const Eigen::Vector3i &id) const 219 | { 220 | return id.cast() * scale + oc; 221 | } 222 | 223 | inline Eigen::Vector3i posD2I(const Eigen::Vector3d &pos) const 224 | { 225 | return ((pos - o) / scale).cast(); 226 | } 227 | }; 228 | } 229 | 230 | #endif 231 | -------------------------------------------------------------------------------- /Traj_gen by L-BFGS/src/gcopter/include/misc/visualizer.hpp: -------------------------------------------------------------------------------- 1 | #ifndef VISUALIZER_HPP 2 | #define VISUALIZER_HPP 3 | 4 | #include "gcopter/trajectory.hpp" 5 | #include "gcopter/quickhull.hpp" 6 | #include "gcopter/geo_utils.hpp" 7 | #include "gcopter/cubic_curve.hpp" 8 | 9 | #include 10 | #include 11 | #include 12 | #include 13 | 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | 21 | // Visualizer for the planner 22 | class Visualizer 23 | { 24 | private: 25 | // config contains the scale for some markers 26 | ros::NodeHandle nh; 27 | 28 | // These are publishers for path, waypoints on the trajectory, 29 | // the entire trajectory, the mesh of free-space polytopes, 30 | // the edge of free-space polytopes, and spheres for safety radius 31 | ros::Publisher routePub; 32 | ros::Publisher wayPointsPub; 33 | ros::Publisher trajectoryPub; 34 | ros::Publisher meshPub; 35 | ros::Publisher edgePub; 36 | ros::Publisher spherePub; 37 | ros::Publisher diskPub; 38 | 39 | public: 40 | ros::Publisher speedPub; 41 | ros::Publisher thrPub; 42 | ros::Publisher tiltPub; 43 | ros::Publisher bdrPub; 44 | 45 | public: 46 | Visualizer(ros::NodeHandle &nh_) 47 | : nh(nh_) 48 | { 49 | routePub = nh.advertise("/visualizer/route", 10); 50 | wayPointsPub = nh.advertise("/visualizer/waypoints", 10); 51 | trajectoryPub = nh.advertise("/visualizer/trajectory", 10); 52 | meshPub = nh.advertise("/visualizer/mesh", 1000); 53 | edgePub = nh.advertise("/visualizer/edge", 1000); 54 | spherePub = nh.advertise("/visualizer/spheres", 1000); 55 | diskPub = nh.advertise("/visualizer/disks", 1000); 56 | speedPub = nh.advertise("/visualizer/speed", 1000); 57 | thrPub = nh.advertise("/visualizer/total_thrust", 1000); 58 | tiltPub = nh.advertise("/visualizer/tilt_angle", 1000); 59 | bdrPub = nh.advertise("/visualizer/body_rate", 1000); 60 | } 61 | 62 | // Visualize the trajectory and its front-end path 63 | template 64 | inline void visualize(const Trajectory &traj, 65 | const std::vector &route) 66 | { 67 | visualization_msgs::Marker routeMarker, wayPointsMarker, trajMarker; 68 | 69 | routeMarker.id = 0; 70 | routeMarker.type = visualization_msgs::Marker::LINE_LIST; 71 | routeMarker.header.stamp = ros::Time::now(); 72 | routeMarker.header.frame_id = "odom"; 73 | routeMarker.pose.orientation.w = 1.00; 74 | routeMarker.action = visualization_msgs::Marker::ADD; 75 | routeMarker.ns = "route"; 76 | routeMarker.color.r = 1.00; 77 | routeMarker.color.g = 0.00; 78 | routeMarker.color.b = 0.00; 79 | routeMarker.color.a = 1.00; 80 | routeMarker.scale.x = 0.1; 81 | 82 | wayPointsMarker = routeMarker; 83 | wayPointsMarker.id = -wayPointsMarker.id - 1; 84 | wayPointsMarker.type = visualization_msgs::Marker::SPHERE_LIST; 85 | wayPointsMarker.ns = "waypoints"; 86 | wayPointsMarker.color.r = 1.00; 87 | wayPointsMarker.color.g = 0.00; 88 | wayPointsMarker.color.b = 0.00; 89 | wayPointsMarker.scale.x = 0.35; 90 | wayPointsMarker.scale.y = 0.35; 91 | wayPointsMarker.scale.z = 0.35; 92 | 93 | trajMarker = routeMarker; 94 | trajMarker.header.frame_id = "odom"; 95 | trajMarker.id = 0; 96 | trajMarker.ns = "trajectory"; 97 | trajMarker.color.r = 0.00; 98 | trajMarker.color.g = 0.50; 99 | trajMarker.color.b = 1.00; 100 | trajMarker.scale.x = 0.30; 101 | 102 | if (route.size() > 0) 103 | { 104 | bool first = true; 105 | Eigen::Vector3d last; 106 | for (auto it : route) 107 | { 108 | if (first) 109 | { 110 | first = false; 111 | last = it; 112 | continue; 113 | } 114 | geometry_msgs::Point point; 115 | 116 | point.x = last(0); 117 | point.y = last(1); 118 | point.z = last(2); 119 | routeMarker.points.push_back(point); 120 | point.x = it(0); 121 | point.y = it(1); 122 | point.z = it(2); 123 | routeMarker.points.push_back(point); 124 | last = it; 125 | } 126 | 127 | routePub.publish(routeMarker); 128 | } 129 | 130 | if (traj.getPieceNum() > 0) 131 | { 132 | Eigen::MatrixXd wps = traj.getPositions(); 133 | for (int i = 0; i < wps.cols(); i++) 134 | { 135 | geometry_msgs::Point point; 136 | point.x = wps.col(i)(0); 137 | point.y = wps.col(i)(1); 138 | point.z = wps.col(i)(2); 139 | wayPointsMarker.points.push_back(point); 140 | } 141 | 142 | wayPointsPub.publish(wayPointsMarker); 143 | } 144 | 145 | if (traj.getPieceNum() > 0) 146 | { 147 | double T = 0.01; 148 | Eigen::Vector3d lastX = traj.getPos(0.0); 149 | for (double t = T; t < traj.getTotalDuration(); t += T) 150 | { 151 | geometry_msgs::Point point; 152 | Eigen::Vector3d X = traj.getPos(t); 153 | point.x = lastX(0); 154 | point.y = lastX(1); 155 | point.z = lastX(2); 156 | trajMarker.points.push_back(point); 157 | point.x = X(0); 158 | point.y = X(1); 159 | point.z = X(2); 160 | trajMarker.points.push_back(point); 161 | lastX = X; 162 | } 163 | trajectoryPub.publish(trajMarker); 164 | } 165 | } 166 | 167 | inline void visualize(const CubicCurve &curve) 168 | { 169 | visualization_msgs::Marker routeMarker, wayPointsMarker, trajMarker; 170 | 171 | routeMarker.id = 0; 172 | routeMarker.type = visualization_msgs::Marker::LINE_LIST; 173 | routeMarker.header.stamp = ros::Time::now(); 174 | routeMarker.header.frame_id = "odom"; 175 | routeMarker.pose.orientation.w = 1.00; 176 | routeMarker.action = visualization_msgs::Marker::ADD; 177 | routeMarker.ns = "route"; 178 | routeMarker.color.r = 1.00; 179 | routeMarker.color.g = 0.00; 180 | routeMarker.color.b = 0.00; 181 | routeMarker.color.a = 1.00; 182 | routeMarker.scale.x = 0.1; 183 | 184 | wayPointsMarker = routeMarker; 185 | wayPointsMarker.id = -wayPointsMarker.id - 1; 186 | wayPointsMarker.type = visualization_msgs::Marker::SPHERE_LIST; 187 | wayPointsMarker.ns = "waypoints"; 188 | wayPointsMarker.color.r = 1.00; 189 | wayPointsMarker.color.g = 0.00; 190 | wayPointsMarker.color.b = 0.00; 191 | wayPointsMarker.scale.x = 0.35; 192 | wayPointsMarker.scale.y = 0.35; 193 | wayPointsMarker.scale.z = 0.35; 194 | 195 | trajMarker = routeMarker; 196 | trajMarker.header.frame_id = "odom"; 197 | trajMarker.id = 0; 198 | trajMarker.ns = "trajectory"; 199 | trajMarker.color.r = 0.00; 200 | trajMarker.color.g = 0.50; 201 | trajMarker.color.b = 1.00; 202 | trajMarker.scale.x = 0.20; 203 | 204 | if (curve.getPieceNum() > 0) 205 | { 206 | Eigen::MatrixXd wps = curve.getPositions(); 207 | for (int i = 0; i < wps.cols(); i++) 208 | { 209 | geometry_msgs::Point point; 210 | point.x = wps.col(i)(0); 211 | point.y = wps.col(i)(1); 212 | point.z = 0.0; 213 | wayPointsMarker.points.push_back(point); 214 | } 215 | 216 | wayPointsPub.publish(wayPointsMarker); 217 | } 218 | 219 | if (curve.getPieceNum() > 0) 220 | { 221 | double T = 0.01; 222 | Eigen::Vector2d lastX = curve.getPos(0.0); 223 | for (double t = T; t < curve.getTotalDuration(); t += T) 224 | { 225 | geometry_msgs::Point point; 226 | Eigen::Vector2d X = curve.getPos(t); 227 | point.x = lastX(0); 228 | point.y = lastX(1); 229 | point.z = 0.0; 230 | trajMarker.points.push_back(point); 231 | point.x = X(0); 232 | point.y = X(1); 233 | point.z = 0.0; 234 | trajMarker.points.push_back(point); 235 | lastX = X; 236 | } 237 | trajectoryPub.publish(trajMarker); 238 | } 239 | } 240 | 241 | inline void visualizeDisks(const Eigen::Matrix3Xd &disks) 242 | { 243 | visualization_msgs::Marker diskMarker; 244 | visualization_msgs::MarkerArray diskMarkers; 245 | 246 | diskMarker.type = visualization_msgs::Marker::CYLINDER; 247 | diskMarker.header.stamp = ros::Time::now(); 248 | diskMarker.header.frame_id = "odom"; 249 | diskMarker.pose.orientation.w = 1.00; 250 | diskMarker.action = visualization_msgs::Marker::ADD; 251 | diskMarker.ns = "disks"; 252 | diskMarker.color.r = 1.00; 253 | diskMarker.color.g = 0.00; 254 | diskMarker.color.b = 0.00; 255 | diskMarker.color.a = 1.00; 256 | 257 | for (int i = 0; i < disks.cols(); ++i) 258 | { 259 | diskMarker.id = i; 260 | diskMarker.pose.position.x = disks(0, i); 261 | diskMarker.pose.position.y = disks(1, i); 262 | diskMarker.pose.position.z = 0.5; 263 | diskMarker.scale.x = disks(2, i) * 2.0; 264 | diskMarker.scale.y = disks(2, i) * 2.0; 265 | diskMarker.scale.z = 1.0; 266 | diskMarkers.markers.push_back(diskMarker); 267 | } 268 | 269 | diskPub.publish(diskMarkers); 270 | } 271 | 272 | // Visualize some polytopes in H-representation 273 | inline void visualizePolytope(const std::vector &hPolys) 274 | { 275 | 276 | // Due to the fact that H-representation cannot be directly visualized 277 | // We first conduct vertex enumeration of them, then apply quickhull 278 | // to obtain triangle meshs of polyhedra 279 | Eigen::Matrix3Xd mesh(3, 0), curTris(3, 0), oldTris(3, 0); 280 | for (size_t id = 0; id < hPolys.size(); id++) 281 | { 282 | oldTris = mesh; 283 | Eigen::Matrix vPoly; 284 | geo_utils::enumerateVs(hPolys[id], vPoly); 285 | 286 | quickhull::QuickHull tinyQH; 287 | const auto polyHull = tinyQH.getConvexHull(vPoly.data(), vPoly.cols(), false, true); 288 | const auto &idxBuffer = polyHull.getIndexBuffer(); 289 | int hNum = idxBuffer.size() / 3; 290 | 291 | curTris.resize(3, hNum * 3); 292 | for (int i = 0; i < hNum * 3; i++) 293 | { 294 | curTris.col(i) = vPoly.col(idxBuffer[i]); 295 | } 296 | mesh.resize(3, oldTris.cols() + curTris.cols()); 297 | mesh.leftCols(oldTris.cols()) = oldTris; 298 | mesh.rightCols(curTris.cols()) = curTris; 299 | } 300 | 301 | // RVIZ support tris for visualization 302 | visualization_msgs::Marker meshMarker, edgeMarker; 303 | 304 | meshMarker.id = 0; 305 | meshMarker.header.stamp = ros::Time::now(); 306 | meshMarker.header.frame_id = "odom"; 307 | meshMarker.pose.orientation.w = 1.00; 308 | meshMarker.action = visualization_msgs::Marker::ADD; 309 | meshMarker.type = visualization_msgs::Marker::TRIANGLE_LIST; 310 | meshMarker.ns = "mesh"; 311 | meshMarker.color.r = 0.00; 312 | meshMarker.color.g = 0.00; 313 | meshMarker.color.b = 1.00; 314 | meshMarker.color.a = 0.15; 315 | meshMarker.scale.x = 1.0; 316 | meshMarker.scale.y = 1.0; 317 | meshMarker.scale.z = 1.0; 318 | 319 | edgeMarker = meshMarker; 320 | edgeMarker.type = visualization_msgs::Marker::LINE_LIST; 321 | edgeMarker.ns = "edge"; 322 | edgeMarker.color.r = 0.00; 323 | edgeMarker.color.g = 1.00; 324 | edgeMarker.color.b = 1.00; 325 | edgeMarker.color.a = 1.00; 326 | edgeMarker.scale.x = 0.02; 327 | 328 | geometry_msgs::Point point; 329 | 330 | int ptnum = mesh.cols(); 331 | 332 | for (int i = 0; i < ptnum; i++) 333 | { 334 | point.x = mesh(0, i); 335 | point.y = mesh(1, i); 336 | point.z = mesh(2, i); 337 | meshMarker.points.push_back(point); 338 | } 339 | 340 | for (int i = 0; i < ptnum / 3; i++) 341 | { 342 | for (int j = 0; j < 3; j++) 343 | { 344 | point.x = mesh(0, 3 * i + j); 345 | point.y = mesh(1, 3 * i + j); 346 | point.z = mesh(2, 3 * i + j); 347 | edgeMarker.points.push_back(point); 348 | point.x = mesh(0, 3 * i + (j + 1) % 3); 349 | point.y = mesh(1, 3 * i + (j + 1) % 3); 350 | point.z = mesh(2, 3 * i + (j + 1) % 3); 351 | edgeMarker.points.push_back(point); 352 | } 353 | } 354 | 355 | meshPub.publish(meshMarker); 356 | edgePub.publish(edgeMarker); 357 | 358 | return; 359 | } 360 | 361 | // Visualize all spheres with centers sphs and the same radius 362 | inline void visualizeSphere(const Eigen::Vector3d ¢er, 363 | const double &radius) 364 | { 365 | visualization_msgs::Marker sphereMarkers, sphereDeleter; 366 | 367 | sphereMarkers.id = 0; 368 | sphereMarkers.type = visualization_msgs::Marker::SPHERE_LIST; 369 | sphereMarkers.header.stamp = ros::Time::now(); 370 | sphereMarkers.header.frame_id = "odom"; 371 | sphereMarkers.pose.orientation.w = 1.00; 372 | sphereMarkers.action = visualization_msgs::Marker::ADD; 373 | sphereMarkers.ns = "spheres"; 374 | sphereMarkers.color.r = 0.00; 375 | sphereMarkers.color.g = 0.00; 376 | sphereMarkers.color.b = 1.00; 377 | sphereMarkers.color.a = 1.00; 378 | sphereMarkers.scale.x = radius * 2.0; 379 | sphereMarkers.scale.y = radius * 2.0; 380 | sphereMarkers.scale.z = radius * 2.0; 381 | 382 | sphereDeleter = sphereMarkers; 383 | sphereDeleter.action = visualization_msgs::Marker::DELETE; 384 | 385 | geometry_msgs::Point point; 386 | point.x = center(0); 387 | point.y = center(1); 388 | point.z = center(2); 389 | sphereMarkers.points.push_back(point); 390 | 391 | spherePub.publish(sphereDeleter); 392 | spherePub.publish(sphereMarkers); 393 | } 394 | 395 | inline void visualizeStartGoal(const Eigen::Vector3d ¢er, 396 | const double &radius, 397 | const int sg) 398 | { 399 | visualization_msgs::Marker sphereMarkers, sphereDeleter; 400 | 401 | sphereMarkers.id = sg; 402 | sphereMarkers.type = visualization_msgs::Marker::SPHERE_LIST; 403 | sphereMarkers.header.stamp = ros::Time::now(); 404 | sphereMarkers.header.frame_id = "odom"; 405 | sphereMarkers.pose.orientation.w = 1.00; 406 | sphereMarkers.action = visualization_msgs::Marker::ADD; 407 | sphereMarkers.ns = "StartGoal"; 408 | sphereMarkers.color.r = 1.00; 409 | sphereMarkers.color.g = 0.00; 410 | sphereMarkers.color.b = 0.00; 411 | sphereMarkers.color.a = 1.00; 412 | sphereMarkers.scale.x = radius * 2.0; 413 | sphereMarkers.scale.y = radius * 2.0; 414 | sphereMarkers.scale.z = radius * 2.0; 415 | 416 | sphereDeleter = sphereMarkers; 417 | sphereDeleter.action = visualization_msgs::Marker::DELETEALL; 418 | 419 | geometry_msgs::Point point; 420 | point.x = center(0); 421 | point.y = center(1); 422 | point.z = center(2); 423 | sphereMarkers.points.push_back(point); 424 | 425 | if (sg == 0) 426 | { 427 | spherePub.publish(sphereDeleter); 428 | ros::Duration(1.0e-9).sleep(); 429 | sphereMarkers.header.stamp = ros::Time::now(); 430 | } 431 | spherePub.publish(sphereMarkers); 432 | } 433 | }; 434 | 435 | #endif -------------------------------------------------------------------------------- /Traj_gen by L-BFGS/src/gcopter/launch/curve_gen.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | -------------------------------------------------------------------------------- /Traj_gen by L-BFGS/src/gcopter/launch/global_planning.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 6 | 7 | 8 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | -------------------------------------------------------------------------------- /Traj_gen by L-BFGS/src/gcopter/package.xml: -------------------------------------------------------------------------------- 1 | 2 | gcopter 3 | Zhepei Wang 4 | Zhepei Wang 5 | 0.1.0 6 | 7 | the gcopter package 8 | 9 | BSD 10 | 11 | catkin 12 | 13 | roscpp 14 | std_msgs 15 | geometry_msgs 16 | sensor_msgs 17 | visualization_msgs 18 | roscpp 19 | std_msgs 20 | geometry_msgs 21 | sensor_msgs 22 | visualization_msgs 23 | 24 | 25 | -------------------------------------------------------------------------------- /Traj_gen by L-BFGS/src/gcopter/src/curve_gen.cpp: -------------------------------------------------------------------------------- 1 | #include "misc/visualizer.hpp" 2 | #include "gcopter/cubic_curve.hpp" 3 | #include "gcopter/cubic_spline.hpp" 4 | #include "gcopter/path_smoother.hpp" 5 | 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | 19 | struct Config 20 | { 21 | std::string targetTopic; 22 | double penaltyWeight; 23 | Eigen::Matrix3Xd circleObs; 24 | double pieceLength; 25 | double relCostTol; 26 | 27 | Config(const ros::NodeHandle &nh_priv) 28 | { 29 | std::vector circleObsVec; 30 | 31 | nh_priv.getParam("TargetTopic", targetTopic); 32 | nh_priv.getParam("PenaltyWeight", penaltyWeight); 33 | nh_priv.getParam("CircleObs", circleObsVec); 34 | nh_priv.getParam("PieceLength", pieceLength); 35 | nh_priv.getParam("RelCostTol", relCostTol); 36 | 37 | circleObs = Eigen::Map>( 38 | circleObsVec.data(), 3, circleObsVec.size() / 3); 39 | } 40 | }; 41 | 42 | class CurveGen 43 | { 44 | private: 45 | Config config; 46 | ros::NodeHandle nh; 47 | ros::Subscriber targetSub; 48 | 49 | Visualizer visualizer; 50 | std::vector startGoal; 51 | 52 | CubicCurve curve; 53 | 54 | public: 55 | CurveGen(ros::NodeHandle &nh_) 56 | : config(ros::NodeHandle("~")), 57 | nh(nh_), 58 | visualizer(nh) 59 | { 60 | targetSub = nh.subscribe(config.targetTopic, 1, &CurveGen::targetCallBack, this, 61 | ros::TransportHints().tcpNoDelay()); 62 | } 63 | 64 | inline void vizObs() 65 | { 66 | visualizer.visualizeDisks(config.circleObs); 67 | } 68 | 69 | inline void plan() 70 | { 71 | if (startGoal.size() == 2) 72 | { 73 | const int N = (startGoal.back() - startGoal.front()).norm() / config.pieceLength; 74 | Eigen::Matrix2Xd innerPoints(2, N - 1); 75 | for (int i = 0; i < N - 1; ++i) 76 | { 77 | innerPoints.col(i) = (startGoal.back() - startGoal.front()) * (i + 1.0) / N + startGoal.front(); 78 | } 79 | 80 | path_smoother::PathSmoother pathSmoother; 81 | pathSmoother.setup(startGoal.front(), startGoal.back(), N, config.circleObs, config.penaltyWeight); 82 | CubicCurve curve; 83 | 84 | if (std::isinf(pathSmoother.optimize(curve, innerPoints, config.relCostTol))) 85 | { 86 | return; 87 | } 88 | 89 | if (curve.getPieceNum() > 0) 90 | { 91 | visualizer.visualize(curve); 92 | } 93 | } 94 | } 95 | 96 | inline void targetCallBack(const geometry_msgs::PoseStamped::ConstPtr &msg) 97 | { 98 | 99 | if (startGoal.size() >= 2) 100 | { 101 | startGoal.clear(); 102 | } 103 | 104 | startGoal.emplace_back(msg->pose.position.x, msg->pose.position.y); 105 | 106 | plan(); 107 | 108 | return; 109 | } 110 | }; 111 | 112 | int main(int argc, char **argv) 113 | { 114 | ros::init(argc, argv, "curve_gen_node"); 115 | ros::NodeHandle nh_; 116 | 117 | CurveGen curveGen(nh_); 118 | 119 | ros::Duration(2.0).sleep(); 120 | 121 | curveGen.vizObs(); 122 | 123 | ros::spin(); 124 | 125 | return 0; 126 | } 127 | -------------------------------------------------------------------------------- /Traj_gen by L-BFGS/src/gcopter/src/global_planning.cpp: -------------------------------------------------------------------------------- 1 | #include "misc/visualizer.hpp" 2 | #include "gcopter/trajectory.hpp" 3 | #include "gcopter/gcopter.hpp" 4 | #include "gcopter/firi.hpp" 5 | #include "gcopter/flatness.hpp" 6 | #include "gcopter/voxel_map.hpp" 7 | #include "gcopter/sfc_gen.hpp" 8 | 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | 23 | struct Config 24 | { 25 | std::string mapTopic; 26 | std::string targetTopic; 27 | double dilateRadius; 28 | double voxelWidth; 29 | std::vector mapBound; 30 | double timeoutRRT; 31 | double maxVelMag; 32 | double maxBdrMag; 33 | double maxTiltAngle; 34 | double minThrust; 35 | double maxThrust; 36 | double vehicleMass; 37 | double gravAcc; 38 | double horizDrag; 39 | double vertDrag; 40 | double parasDrag; 41 | double speedEps; 42 | double weightT; 43 | std::vector chiVec; 44 | double smoothingEps; 45 | int integralIntervs; 46 | double relCostTol; 47 | 48 | Config(const ros::NodeHandle &nh_priv) 49 | { 50 | nh_priv.getParam("MapTopic", mapTopic); 51 | nh_priv.getParam("TargetTopic", targetTopic); 52 | nh_priv.getParam("DilateRadius", dilateRadius); 53 | nh_priv.getParam("VoxelWidth", voxelWidth); 54 | nh_priv.getParam("MapBound", mapBound); 55 | nh_priv.getParam("TimeoutRRT", timeoutRRT); 56 | nh_priv.getParam("MaxVelMag", maxVelMag); 57 | nh_priv.getParam("MaxBdrMag", maxBdrMag); 58 | nh_priv.getParam("MaxTiltAngle", maxTiltAngle); 59 | nh_priv.getParam("MinThrust", minThrust); 60 | nh_priv.getParam("MaxThrust", maxThrust); 61 | nh_priv.getParam("VehicleMass", vehicleMass); 62 | nh_priv.getParam("GravAcc", gravAcc); 63 | nh_priv.getParam("HorizDrag", horizDrag); 64 | nh_priv.getParam("VertDrag", vertDrag); 65 | nh_priv.getParam("ParasDrag", parasDrag); 66 | nh_priv.getParam("SpeedEps", speedEps); 67 | nh_priv.getParam("WeightT", weightT); 68 | nh_priv.getParam("ChiVec", chiVec); 69 | nh_priv.getParam("SmoothingEps", smoothingEps); 70 | nh_priv.getParam("IntegralIntervs", integralIntervs); 71 | nh_priv.getParam("RelCostTol", relCostTol); 72 | } 73 | }; 74 | 75 | class GlobalPlanner 76 | { 77 | private: 78 | Config config; 79 | 80 | ros::NodeHandle nh; 81 | ros::Subscriber mapSub; 82 | ros::Subscriber targetSub; 83 | 84 | bool mapInitialized; 85 | voxel_map::VoxelMap voxelMap; 86 | Visualizer visualizer; 87 | std::vector startGoal; 88 | 89 | Trajectory<5> traj; 90 | double trajStamp; 91 | 92 | public: 93 | GlobalPlanner(const Config &conf, 94 | ros::NodeHandle &nh_) 95 | : config(conf), 96 | nh(nh_), 97 | mapInitialized(false), 98 | visualizer(nh) 99 | { 100 | const Eigen::Vector3i xyz((config.mapBound[1] - config.mapBound[0]) / config.voxelWidth, 101 | (config.mapBound[3] - config.mapBound[2]) / config.voxelWidth, 102 | (config.mapBound[5] - config.mapBound[4]) / config.voxelWidth); 103 | 104 | const Eigen::Vector3d offset(config.mapBound[0], config.mapBound[2], config.mapBound[4]); 105 | 106 | voxelMap = voxel_map::VoxelMap(xyz, offset, config.voxelWidth); 107 | 108 | mapSub = nh.subscribe(config.mapTopic, 1, &GlobalPlanner::mapCallBack, this, 109 | ros::TransportHints().tcpNoDelay()); 110 | 111 | targetSub = nh.subscribe(config.targetTopic, 1, &GlobalPlanner::targetCallBack, this, 112 | ros::TransportHints().tcpNoDelay()); 113 | } 114 | 115 | inline void mapCallBack(const sensor_msgs::PointCloud2::ConstPtr &msg) 116 | { 117 | if (!mapInitialized) 118 | { 119 | size_t cur = 0; 120 | const size_t total = msg->data.size() / msg->point_step; 121 | float *fdata = (float *)(&msg->data[0]); 122 | for (size_t i = 0; i < total; i++) 123 | { 124 | cur = msg->point_step / sizeof(float) * i; 125 | 126 | if (std::isnan(fdata[cur + 0]) || std::isinf(fdata[cur + 0]) || 127 | std::isnan(fdata[cur + 1]) || std::isinf(fdata[cur + 1]) || 128 | std::isnan(fdata[cur + 2]) || std::isinf(fdata[cur + 2])) 129 | { 130 | continue; 131 | } 132 | voxelMap.setOccupied(Eigen::Vector3d(fdata[cur + 0], 133 | fdata[cur + 1], 134 | fdata[cur + 2])); 135 | } 136 | 137 | voxelMap.dilate(std::ceil(config.dilateRadius / voxelMap.getScale())); 138 | 139 | mapInitialized = true; 140 | } 141 | } 142 | 143 | inline void plan() 144 | { 145 | if (startGoal.size() == 2) 146 | { 147 | std::vector route; 148 | sfc_gen::planPath(startGoal[0], 149 | startGoal[1], 150 | voxelMap.getOrigin(), 151 | voxelMap.getCorner(), 152 | &voxelMap, 0.01, 153 | route); 154 | std::vector hPolys; 155 | std::vector pc; 156 | voxelMap.getSurf(pc); 157 | 158 | sfc_gen::convexCover(route, 159 | pc, 160 | voxelMap.getOrigin(), 161 | voxelMap.getCorner(), 162 | 7.0, 163 | 3.0, 164 | hPolys); 165 | sfc_gen::shortCut(hPolys); 166 | 167 | if (route.size() > 1) 168 | { 169 | visualizer.visualizePolytope(hPolys); 170 | 171 | Eigen::Matrix3d iniState; 172 | Eigen::Matrix3d finState; 173 | iniState << route.front(), Eigen::Vector3d::Zero(), Eigen::Vector3d::Zero(); 174 | finState << route.back(), Eigen::Vector3d::Zero(), Eigen::Vector3d::Zero(); 175 | 176 | gcopter::GCOPTER_PolytopeSFC gcopter; 177 | 178 | // magnitudeBounds = [v_max, omg_max, theta_max, thrust_min, thrust_max]^T 179 | // penaltyWeights = [pos_weight, vel_weight, omg_weight, theta_weight, thrust_weight]^T 180 | // physicalParams = [vehicle_mass, gravitational_acceleration, horitonral_drag_coeff, 181 | // vertical_drag_coeff, parasitic_drag_coeff, speed_smooth_factor]^T 182 | // initialize some constraint parameters 183 | Eigen::VectorXd magnitudeBounds(5); 184 | Eigen::VectorXd penaltyWeights(5); 185 | Eigen::VectorXd physicalParams(6); 186 | magnitudeBounds(0) = config.maxVelMag; 187 | magnitudeBounds(1) = config.maxBdrMag; 188 | magnitudeBounds(2) = config.maxTiltAngle; 189 | magnitudeBounds(3) = config.minThrust; 190 | magnitudeBounds(4) = config.maxThrust; 191 | penaltyWeights(0) = (config.chiVec)[0]; 192 | penaltyWeights(1) = (config.chiVec)[1]; 193 | penaltyWeights(2) = (config.chiVec)[2]; 194 | penaltyWeights(3) = (config.chiVec)[3]; 195 | penaltyWeights(4) = (config.chiVec)[4]; 196 | physicalParams(0) = config.vehicleMass; 197 | physicalParams(1) = config.gravAcc; 198 | physicalParams(2) = config.horizDrag; 199 | physicalParams(3) = config.vertDrag; 200 | physicalParams(4) = config.parasDrag; 201 | physicalParams(5) = config.speedEps; 202 | const int quadratureRes = config.integralIntervs; 203 | 204 | traj.clear(); 205 | 206 | if (!gcopter.setup(config.weightT, 207 | iniState, finState, 208 | hPolys, INFINITY, 209 | config.smoothingEps, 210 | quadratureRes, 211 | magnitudeBounds, 212 | penaltyWeights, 213 | physicalParams)) 214 | { 215 | return; 216 | } 217 | 218 | if (std::isinf(gcopter.optimize(traj, config.relCostTol))) 219 | { 220 | return; 221 | } 222 | 223 | if (traj.getPieceNum() > 0) 224 | { 225 | trajStamp = ros::Time::now().toSec(); 226 | visualizer.visualize(traj, route); 227 | } 228 | } 229 | } 230 | } 231 | 232 | inline void targetCallBack(const geometry_msgs::PoseStamped::ConstPtr &msg) 233 | { 234 | if (mapInitialized) 235 | { 236 | if (startGoal.size() >= 2) 237 | { 238 | startGoal.clear(); 239 | } 240 | const double zGoal = config.mapBound[4] + config.dilateRadius + 241 | fabs(msg->pose.orientation.z) * 242 | (config.mapBound[5] - config.mapBound[4] - 2 * config.dilateRadius); 243 | const Eigen::Vector3d goal(msg->pose.position.x, msg->pose.position.y, zGoal); 244 | if (voxelMap.query(goal) == 0) 245 | { 246 | visualizer.visualizeStartGoal(goal, 0.5, startGoal.size()); 247 | startGoal.emplace_back(goal); 248 | } 249 | else 250 | { 251 | ROS_WARN("Infeasible Position Selected !!!\n"); 252 | } 253 | 254 | plan(); 255 | } 256 | return; 257 | } 258 | 259 | inline void process() 260 | { 261 | Eigen::VectorXd physicalParams(6); 262 | physicalParams(0) = config.vehicleMass; 263 | physicalParams(1) = config.gravAcc; 264 | physicalParams(2) = config.horizDrag; 265 | physicalParams(3) = config.vertDrag; 266 | physicalParams(4) = config.parasDrag; 267 | physicalParams(5) = config.speedEps; 268 | 269 | flatness::FlatnessMap flatmap; 270 | flatmap.reset(physicalParams(0), physicalParams(1), physicalParams(2), 271 | physicalParams(3), physicalParams(4), physicalParams(5)); 272 | 273 | if (traj.getPieceNum() > 0) 274 | { 275 | const double delta = ros::Time::now().toSec() - trajStamp; 276 | if (delta > 0.0 && delta < traj.getTotalDuration()) 277 | { 278 | double thr; 279 | Eigen::Vector4d quat; 280 | Eigen::Vector3d omg; 281 | 282 | flatmap.forward(traj.getVel(delta), 283 | traj.getAcc(delta), 284 | traj.getJer(delta), 285 | 0.0, 0.0, 286 | thr, quat, omg); 287 | double speed = traj.getVel(delta).norm(); 288 | double bodyratemag = omg.norm(); 289 | double tiltangle = acos(1.0 - 2.0 * (quat(1) * quat(1) + quat(2) * quat(2))); 290 | std_msgs::Float64 speedMsg, thrMsg, tiltMsg, bdrMsg; 291 | speedMsg.data = speed; 292 | thrMsg.data = thr; 293 | tiltMsg.data = tiltangle; 294 | bdrMsg.data = bodyratemag; 295 | visualizer.speedPub.publish(speedMsg); 296 | visualizer.thrPub.publish(thrMsg); 297 | visualizer.tiltPub.publish(tiltMsg); 298 | visualizer.bdrPub.publish(bdrMsg); 299 | 300 | visualizer.visualizeSphere(traj.getPos(delta), 301 | config.dilateRadius); 302 | } 303 | } 304 | } 305 | }; 306 | 307 | int main(int argc, char **argv) 308 | { 309 | ros::init(argc, argv, "global_planning_node"); 310 | ros::NodeHandle nh_; 311 | 312 | GlobalPlanner global_planner(Config(ros::NodeHandle("~")), nh_); 313 | 314 | ros::Rate lr(1000); 315 | while (ros::ok()) 316 | { 317 | global_planner.process(); 318 | ros::spinOnce(); 319 | lr.sleep(); 320 | } 321 | 322 | return 0; 323 | } 324 | -------------------------------------------------------------------------------- /Traj_gen by L-BFGS/src/map_gen/mockamap/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(mockamap) 3 | 4 | ## Compile as C++14, supported in ROS Kinetic and newer 5 | add_compile_options(-std=c++14) 6 | 7 | ## Find catkin macros and libraries 8 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 9 | ## is used, also find other catkin packages 10 | find_package(catkin REQUIRED COMPONENTS 11 | roscpp 12 | pcl_ros 13 | pcl_conversions 14 | ) 15 | 16 | ## System dependencies are found with CMake's conventions 17 | # find_package(Boost REQUIRED COMPONENTS system) 18 | 19 | 20 | ## Uncomment this if the package has a setup.py. This macro ensures 21 | ## modules and global scripts declared therein get installed 22 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 23 | # catkin_python_setup() 24 | 25 | ################################################ 26 | ## Declare ROS messages, services and actions ## 27 | ################################################ 28 | 29 | ## To declare and build messages, services or actions from within this 30 | ## package, follow these steps: 31 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 32 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 33 | ## * In the file package.xml: 34 | ## * add a build_depend tag for "message_generation" 35 | ## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET 36 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 37 | ## but can be declared for certainty nonetheless: 38 | ## * add a run_depend tag for "message_runtime" 39 | ## * In this file (CMakeLists.txt): 40 | ## * add "message_generation" and every package in MSG_DEP_SET to 41 | ## find_package(catkin REQUIRED COMPONENTS ...) 42 | ## * add "message_runtime" and every package in MSG_DEP_SET to 43 | ## catkin_package(CATKIN_DEPENDS ...) 44 | ## * uncomment the add_*_files sections below as needed 45 | ## and list every .msg/.srv/.action file to be processed 46 | ## * uncomment the generate_messages entry below 47 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 48 | 49 | ## Generate messages in the 'msg' folder 50 | # add_message_files( 51 | # FILES 52 | # Message1.msg 53 | # Message2.msg 54 | # ) 55 | 56 | ## Generate services in the 'srv' folder 57 | # add_service_files( 58 | # FILES 59 | # Service1.srv 60 | # Service2.srv 61 | # ) 62 | 63 | ## Generate actions in the 'action' folder 64 | # add_action_files( 65 | # FILES 66 | # Action1.action 67 | # Action2.action 68 | # ) 69 | 70 | ## Generate added messages and services with any dependencies listed here 71 | # generate_messages( 72 | # DEPENDENCIES 73 | # std_msgs # Or other packages containing msgs 74 | # ) 75 | 76 | ################################################ 77 | ## Declare ROS dynamic reconfigure parameters ## 78 | ################################################ 79 | 80 | ## To declare and build dynamic reconfigure parameters within this 81 | ## package, follow these steps: 82 | ## * In the file package.xml: 83 | ## * add a build_depend and a run_depend tag for "dynamic_reconfigure" 84 | ## * In this file (CMakeLists.txt): 85 | ## * add "dynamic_reconfigure" to 86 | ## find_package(catkin REQUIRED COMPONENTS ...) 87 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 88 | ## and list every .cfg file to be processed 89 | 90 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 91 | # generate_dynamic_reconfigure_options( 92 | # cfg/DynReconf1.cfg 93 | # cfg/DynReconf2.cfg 94 | # ) 95 | 96 | ################################### 97 | ## catkin specific configuration ## 98 | ################################### 99 | ## The catkin_package macro generates cmake config files for your package 100 | ## Declare things to be passed to dependent projects 101 | ## INCLUDE_DIRS: uncomment this if you package contains header files 102 | ## LIBRARIES: libraries you create in this project that dependent projects also need 103 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 104 | ## DEPENDS: system dependencies of this project that dependent projects also need 105 | catkin_package( 106 | INCLUDE_DIRS include 107 | # LIBRARIES mockamap 108 | CATKIN_DEPENDS roscpp pcl_ros pcl_conversions 109 | # DEPENDS system_lib 110 | ) 111 | 112 | ########### 113 | ## Build ## 114 | ########### 115 | 116 | ## Specify additional locations of header files 117 | ## Your package locations should be listed before other locations 118 | include_directories( 119 | include 120 | ${catkin_INCLUDE_DIRS} 121 | ) 122 | 123 | ## Declare a C++ library 124 | # add_library(${PROJECT_NAME} 125 | # src/${PROJECT_NAME}/mockamap.cpp 126 | # ) 127 | 128 | ## Add cmake target dependencies of the library 129 | ## as an example, code may need to be generated before libraries 130 | ## either from message generation or dynamic reconfigure 131 | # add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 132 | 133 | ## Declare a C++ executable 134 | ## With catkin_make all packages are built within a single CMake context 135 | ## The recommended prefix ensures that target names across packages don't collide 136 | file(GLOB ${PROJECT_NAME}_SRCS src/*.cpp) 137 | 138 | add_executable(${PROJECT_NAME}_node 139 | ${${PROJECT_NAME}_SRCS}) 140 | 141 | ## Rename C++ executable without prefix 142 | ## The above recommended prefix causes long target names, the following renames the 143 | ## target back to the shorter version for ease of user use 144 | ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" 145 | # set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") 146 | 147 | ## Add cmake target dependencies of the executable 148 | ## same as for the library above 149 | # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 150 | 151 | ## Specify libraries to link a library or executable target against 152 | target_link_libraries(${PROJECT_NAME}_node 153 | ${catkin_LIBRARIES} 154 | ) 155 | 156 | ############# 157 | ## Install ## 158 | ############# 159 | 160 | # all install targets should use catkin DESTINATION variables 161 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 162 | 163 | ## Mark executable scripts (Python etc.) for installation 164 | ## in contrast to setup.py, you can choose the destination 165 | # install(PROGRAMS 166 | # scripts/my_python_script 167 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 168 | # ) 169 | 170 | ## Mark executables and/or libraries for installation 171 | # install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node 172 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 173 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 174 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 175 | # ) 176 | 177 | ## Mark cpp header files for installation 178 | # install(DIRECTORY include/${PROJECT_NAME}/ 179 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 180 | # FILES_MATCHING PATTERN "*.h" 181 | # PATTERN ".svn" EXCLUDE 182 | # ) 183 | 184 | ## Mark other files for installation (e.g. launch and bag files, etc.) 185 | # install(FILES 186 | # # myfile1 187 | # # myfile2 188 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 189 | # ) 190 | 191 | ############# 192 | ## Testing ## 193 | ############# 194 | 195 | ## Add gtest based cpp test target and link libraries 196 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_mockamap.cpp) 197 | # if(TARGET ${PROJECT_NAME}-test) 198 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 199 | # endif() 200 | 201 | ## Add folders to be run by python nosetests 202 | # catkin_add_nosetests(test) 203 | -------------------------------------------------------------------------------- /Traj_gen by L-BFGS/src/map_gen/mockamap/README.md: -------------------------------------------------------------------------------- 1 | # mockamap 2 | a simple map generator based on ROS 3 | 4 | demo cases: 5 | 6 | * perlin3d map 7 | 8 | ![Alt text](https://github.com/HKUST-Aerial-Robotics/mockamap/blob/master/images/perlin3d.png) 9 | 10 | * post2d map 11 | 12 | ![Alt text](https://github.com/HKUST-Aerial-Robotics/mockamap/blob/master/images/post2d.png) 13 | 14 | @misc{mockamap, 15 | author = "William.Wu", 16 | title = "mockamap", 17 | howpublished = "\url{https://github.com/HKUST-Aerial-Robotics/mockamap }", 18 | } 19 | -------------------------------------------------------------------------------- /Traj_gen by L-BFGS/src/map_gen/mockamap/config/rviz.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | - /Path1 10 | - /PointCloud22 11 | Splitter Ratio: 0.5 12 | Tree Height: 489 13 | - Class: rviz/Selection 14 | Name: Selection 15 | - Class: rviz/Tool Properties 16 | Expanded: 17 | - /2D Pose Estimate1 18 | - /2D Nav Goal1 19 | - /Publish Point1 20 | - /Game Like Input1 21 | Name: Tool Properties 22 | Splitter Ratio: 0.588679 23 | - Class: rviz/Views 24 | Expanded: 25 | - /Current View1 26 | Name: Views 27 | Splitter Ratio: 0.5 28 | - Class: rviz/Time 29 | Experimental: false 30 | Name: Time 31 | SyncMode: 0 32 | SyncSource: PointCloud2 33 | - Class: mockamap/PCDView 34 | Input: /mock_map 35 | Name: PCDView 36 | Output: /mock_map/cut 37 | Visualization Manager: 38 | Class: "" 39 | Displays: 40 | - Alpha: 0.5 41 | Cell Size: 1 42 | Class: rviz/Grid 43 | Color: 160; 160; 164 44 | Enabled: true 45 | Line Style: 46 | Line Width: 0.03 47 | Value: Lines 48 | Name: Grid 49 | Normal Cell Count: 0 50 | Offset: 51 | X: 0 52 | Y: 0 53 | Z: 0 54 | Plane: XY 55 | Plane Cell Count: 10 56 | Reference Frame: 57 | Value: true 58 | - Alpha: 1 59 | Autocompute Intensity Bounds: true 60 | Autocompute Value Bounds: 61 | Max Value: 1.9 62 | Min Value: 0 63 | Value: true 64 | Axis: Z 65 | Channel Name: intensity 66 | Class: rviz/PointCloud2 67 | Color: 255; 255; 255 68 | Color Transformer: AxisColor 69 | Decay Time: 0 70 | Enabled: false 71 | Invert Rainbow: false 72 | Max Color: 255; 255; 255 73 | Max Intensity: 4096 74 | Min Color: 0; 0; 0 75 | Min Intensity: 0 76 | Name: PointCloud2 77 | Position Transformer: XYZ 78 | Queue Size: 10 79 | Selectable: true 80 | Size (Pixels): 3 81 | Size (m): 0.1 82 | Style: Boxes 83 | Topic: /mock_map 84 | Unreliable: false 85 | Use Fixed Frame: true 86 | Use rainbow: true 87 | Value: false 88 | - Alpha: 1 89 | Buffer Length: 1 90 | Class: rviz/Path 91 | Color: 255; 85; 255 92 | Enabled: true 93 | Head Diameter: 0.3 94 | Head Length: 0.2 95 | Length: 0.3 96 | Line Style: Lines 97 | Line Width: 0.03 98 | Name: Path 99 | Offset: 100 | X: 0 101 | Y: 0 102 | Z: 0 103 | Pose Style: None 104 | Radius: 0.03 105 | Shaft Diameter: 0.1 106 | Shaft Length: 0.1 107 | Topic: /theta_star_node/output 108 | Unreliable: false 109 | Value: true 110 | - Alpha: 1 111 | Autocompute Intensity Bounds: true 112 | Autocompute Value Bounds: 113 | Max Value: 1.9 114 | Min Value: 0 115 | Value: true 116 | Axis: Z 117 | Channel Name: intensity 118 | Class: rviz/PointCloud2 119 | Color: 255; 255; 255 120 | Color Transformer: AxisColor 121 | Decay Time: 0 122 | Enabled: true 123 | Invert Rainbow: false 124 | Max Color: 255; 255; 255 125 | Max Intensity: 4096 126 | Min Color: 0; 0; 0 127 | Min Intensity: 0 128 | Name: PointCloud2 129 | Position Transformer: XYZ 130 | Queue Size: 10 131 | Selectable: true 132 | Size (Pixels): 3 133 | Size (m): 0.1 134 | Style: Boxes 135 | Topic: /mock_map/cut 136 | Unreliable: false 137 | Use Fixed Frame: true 138 | Use rainbow: true 139 | Value: true 140 | Enabled: true 141 | Global Options: 142 | Background Color: 48; 48; 48 143 | Fixed Frame: map 144 | Frame Rate: 30 145 | Name: root 146 | Tools: 147 | - Class: rviz/Interact 148 | Hide Inactive Objects: true 149 | - Class: rviz/MoveCamera 150 | - Class: rviz/Select 151 | - Class: rviz/FocusCamera 152 | - Class: rviz/Measure 153 | - Class: rviz/SetInitialPose 154 | Topic: /initialpose 155 | - Class: rviz/SetGoal 156 | Topic: /move_base_simple/goal 157 | - Class: rviz/PublishPoint 158 | Single click: true 159 | Topic: /clicked_point 160 | - Boost Property: 0.5 161 | Class: rviz/FPSMotionTool 162 | Fallback Tool: rviz/Interact 163 | Fallback ViewController: rviz/Orbit 164 | Fly Mode: false 165 | Left Hand Mode: false 166 | Step Length: 0.1 167 | - Class: rviz_plugins/GameLikeInput 168 | RangeX_max: 1000 169 | RangeX_min: -1000 170 | RangeY_max: 1000 171 | RangeY_min: -1000 172 | RangeZ_max: 1000 173 | RangeZ_min: -1000 174 | TopicPoint: point_list 175 | TopicSelect: select_list 176 | TopicSwarm: swarm 177 | Value: true 178 | Views: 179 | Current: 180 | Class: rviz/Orbit 181 | Distance: 9.57022 182 | Enable Stereo Rendering: 183 | Stereo Eye Separation: 0.06 184 | Stereo Focal Distance: 1 185 | Swap Stereo Eyes: false 186 | Value: false 187 | Focal Point: 188 | X: -2.02708 189 | Y: -4.23441 190 | Z: 0.814007 191 | Name: Current View 192 | Near Clip Distance: 0.01 193 | Pitch: 0.525203 194 | Target Frame: 195 | Value: Orbit (rviz) 196 | Yaw: 1.88225 197 | Saved: ~ 198 | Window Geometry: 199 | Displays: 200 | collapsed: false 201 | Height: 1056 202 | Hide Left Dock: false 203 | Hide Right Dock: false 204 | PCDView: 205 | collapsed: false 206 | QMainWindow State: 000000ff00000000fd00000004000000000000027000000396fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afc0000002800000296000000fb0100001dfa000000010100000002fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000000ffffffff0000009800fffffffb000000100044006900730070006c00610079007301000000000000016a0000016a00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000c00540065006c0065006f0070000000024d0000009e0000000000000000fb0000000e005000430044005600690065007701000002c4000000fa000000cd00ffffff000000010000010f00000396fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000002800000396000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073f0000003efc0100000002fb0000000800540069006d006501000000000000073f000002f600fffffffb0000000800540069006d00650100000000000004500000000000000000000003b40000039600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 207 | Selection: 208 | collapsed: false 209 | Time: 210 | collapsed: false 211 | Tool Properties: 212 | collapsed: false 213 | Views: 214 | collapsed: false 215 | Width: 1855 216 | X: 65 217 | Y: 24 218 | -------------------------------------------------------------------------------- /Traj_gen by L-BFGS/src/map_gen/mockamap/include/maps.hpp: -------------------------------------------------------------------------------- 1 | #ifndef MAPS_HPP 2 | #define MAPS_HPP 3 | 4 | #include 5 | 6 | #include 7 | #include 8 | #include 9 | 10 | namespace mocka { 11 | 12 | class Maps { 13 | public: 14 | typedef struct BasicInfo { 15 | ros::NodeHandle *nh_private; 16 | int sizeX; 17 | int sizeY; 18 | int sizeZ; 19 | int seed; 20 | double scale; 21 | sensor_msgs::PointCloud2 *output; 22 | pcl::PointCloud *cloud; 23 | } BasicInfo; 24 | 25 | BasicInfo getInfo() const; 26 | void setInfo(const BasicInfo &value); 27 | 28 | public: 29 | Maps(); 30 | 31 | public: 32 | void generate(int type); 33 | 34 | private: 35 | BasicInfo info; 36 | 37 | private: 38 | void pcl2ros(); 39 | 40 | void perlin3D(); 41 | void maze2D(); 42 | void randomMapGenerate(); 43 | void Maze3DGen(); 44 | void recursiveDivision(int xl, int xh, int yl, int yh, Eigen::MatrixXi &maze); 45 | void recursizeDivisionMaze(Eigen::MatrixXi &maze); 46 | void optimizeMap(); 47 | }; 48 | 49 | class MazePoint { 50 | private: 51 | pcl::PointXYZ point; 52 | double dist1; 53 | double dist2; 54 | int point1; 55 | int point2; 56 | bool isdoor; 57 | 58 | public: 59 | pcl::PointXYZ getPoint(); 60 | int getPoint1(); 61 | int getPoint2(); 62 | double getDist1(); 63 | double getDist2(); 64 | void setPoint(pcl::PointXYZ p); 65 | void setPoint1(int p); 66 | void setPoint2(int p); 67 | void setDist1(double set); 68 | void setDist2(double set); 69 | }; 70 | 71 | } // namespace mocka 72 | 73 | #endif // MAPS_HPP 74 | -------------------------------------------------------------------------------- /Traj_gen by L-BFGS/src/map_gen/mockamap/include/perlinnoise.hpp: -------------------------------------------------------------------------------- 1 | #ifndef PERLINNOISE_HPP 2 | #define PERLINNOISE_HPP 3 | 4 | #include 5 | 6 | // THIS CLASS IS A TRANSLATION TO C++14 FROM THE REFERENCE 7 | // JAVA IMPLEMENTATION OF THE IMPROVED PERLIN FUNCTION (see 8 | // http://mrl.nyu.edu/~perlin/noise/) 9 | // THE ORIGINAL JAVA IMPLEMENTATION IS COPYRIGHT 2002 KEN PERLIN 10 | 11 | // I ADDED AN EXTRA METHOD THAT GENERATES A NEW PERMUTATION VECTOR (THIS IS NOT 12 | // PRESENT IN THE ORIGINAL IMPLEMENTATION) 13 | 14 | class PerlinNoise 15 | { 16 | // The permutation vector 17 | std::vector p; 18 | 19 | public: 20 | // Initialize with the reference values for the permutation vector 21 | PerlinNoise(); 22 | // Generate a new permutation vector based on the value of seed 23 | PerlinNoise(unsigned int seed); 24 | // Get a noise value, for 2D images z can have any value 25 | double noise(double x, double y, double z); 26 | 27 | private: 28 | double fade(double t); 29 | double lerp(double t, double a, double b); 30 | double grad(int hash, double x, double y, double z); 31 | }; 32 | 33 | #endif // PERLINNOISE_HPP 34 | -------------------------------------------------------------------------------- /Traj_gen by L-BFGS/src/map_gen/mockamap/launch/maze2d.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 26 | 27 | -------------------------------------------------------------------------------- /Traj_gen by L-BFGS/src/map_gen/mockamap/launch/maze3d.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 26 | 27 | -------------------------------------------------------------------------------- /Traj_gen by L-BFGS/src/map_gen/mockamap/launch/mockamap.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 22 | 24 | 25 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 56 | 57 | -------------------------------------------------------------------------------- /Traj_gen by L-BFGS/src/map_gen/mockamap/launch/perlin3d.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 19 | 21 | 22 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | -------------------------------------------------------------------------------- /Traj_gen by L-BFGS/src/map_gen/mockamap/launch/post2d.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 24 | 25 | -------------------------------------------------------------------------------- /Traj_gen by L-BFGS/src/map_gen/mockamap/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | mockamap 4 | 0.1.0 5 | The mockamap package 6 | 7 | 8 | 9 | 10 | William.Wu 11 | 12 | 13 | 14 | 15 | 16 | GPLv3 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | William.Wu 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | catkin 44 | roscpp 45 | pcl_ros 46 | pcl_conversions 47 | roscpp 48 | pcl_ros 49 | pcl_conversions 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | -------------------------------------------------------------------------------- /Traj_gen by L-BFGS/src/map_gen/mockamap/src/ces_randommap.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | 22 | #include 23 | #include 24 | #include 25 | 26 | #include 27 | #include 28 | #include 29 | 30 | #include "visualization_msgs/Marker.h" 31 | #include "visualization_msgs/MarkerArray.h" 32 | 33 | //! @todo historical above 34 | #include "maps.hpp" 35 | 36 | using namespace std; 37 | using namespace mocka; 38 | 39 | #if MAP_OR_WORLD 40 | const string kFrameIdNs_ = "/map"; 41 | #else 42 | const string kFrameIdNs_ = "/world"; 43 | #endif 44 | 45 | pcl::search::KdTree kdtreeLocalMap; 46 | vector pointIdxRadiusSearch; 47 | vector pointRadiusSquaredDistance; 48 | 49 | ros::Publisher _local_map_pub; 50 | ros::Publisher _local_map_inflate_pub; 51 | ros::Publisher _global_map_pub; 52 | 53 | ros::Subscriber _map_sub; 54 | ros::Subscriber _odom_sub; 55 | 56 | deque _odom_queue; 57 | vector _state; 58 | const size_t _odom_queue_size = 200; 59 | nav_msgs::Odometry _odom; 60 | 61 | double z_limit; 62 | double _SenseRate; 63 | double _sensing_range; 64 | 65 | // ros::Timer vis_map; 66 | bool map_ok = false; 67 | bool _has_odom = false; 68 | 69 | sensor_msgs::PointCloud2 globalMap_pcd; 70 | sensor_msgs::PointCloud2 localMap_pcd; 71 | pcl::PointCloud cloudMap; 72 | ros::Time begin_time = ros::TIME_MAX; 73 | 74 | typedef Eigen::Vector3d ObsPos; 75 | typedef Eigen::Vector3d ObsSize; // x, y, height --- z 76 | typedef pair Obstacle; 77 | std::vector obstacle_list; 78 | 79 | void 80 | fixedMapGenerate() 81 | { 82 | double _resolution; 83 | 84 | cloudMap.points.clear(); 85 | obstacle_list.push_back( 86 | make_pair(ObsPos(-7.0, 1.0, 0.0), ObsSize(1.0, 3.0, 5.0))); 87 | obstacle_list.push_back( 88 | make_pair(ObsPos(-1.0, 1.0, 0.0), ObsSize(1.0, 3.0, 5.0))); 89 | obstacle_list.push_back( 90 | make_pair(ObsPos(10.0, 1.0, 0.0), ObsSize(1.0, 3.0, 5.0))); 91 | obstacle_list.push_back( 92 | make_pair(ObsPos(16.0, 1.0, 0.0), ObsSize(1.0, 3.0, 5.0))); 93 | obstacle_list.push_back( 94 | make_pair(ObsPos(-4.0, -1.0, 0.0), ObsSize(1.0, 3.0, 5.0))); 95 | obstacle_list.push_back( 96 | make_pair(ObsPos(13.0, -1.0, 0.0), ObsSize(1.0, 3.0, 5.0))); 97 | 98 | obstacle_list.push_back( 99 | make_pair(ObsPos(5.0, 2.5, 0.0), ObsSize(30.0, 1.0, 5.0))); 100 | obstacle_list.push_back( 101 | make_pair(ObsPos(5.0, -2.5, 0.0), ObsSize(30.0, 1.0, 5.0))); 102 | 103 | int num_total_obs = obstacle_list.size(); 104 | pcl::PointXYZ pt_insert; 105 | 106 | for (int i = 0; i < num_total_obs; i++) 107 | { 108 | double x, y, z; 109 | double lx, ly, lz; 110 | x = (obstacle_list[i].first)[0]; 111 | y = (obstacle_list[i].first)[1]; 112 | z = (obstacle_list[i].first)[2]; 113 | lx = (obstacle_list[i].second)[0]; 114 | ly = (obstacle_list[i].second)[1]; 115 | lz = (obstacle_list[i].second)[2]; 116 | 117 | int num_mesh_x = ceil(lx / _resolution); 118 | int num_mesh_y = ceil(ly / _resolution); 119 | int num_mesh_z = ceil(lz / _resolution); 120 | 121 | int left_x, right_x, left_y, right_y, left_z, right_z; 122 | left_x = -num_mesh_x / 2; 123 | right_x = num_mesh_x / 2; 124 | left_y = -num_mesh_y / 2; 125 | right_y = num_mesh_y / 2; 126 | left_z = 0; 127 | right_z = num_mesh_z; 128 | 129 | for (int r = left_x; r < right_x; r++) 130 | for (int s = left_y; s < right_y; s++) 131 | { 132 | for (int t = left_z; t < right_z; t++) 133 | { 134 | if ((r - left_x) * (r - right_x + 1) * (s - left_y) * 135 | (s - right_y + 1) * (t - left_z) * (t - right_z + 1) == 136 | 0) 137 | { 138 | pt_insert.x = x + r * _resolution; 139 | pt_insert.y = y + s * _resolution; 140 | pt_insert.z = z + t * _resolution; 141 | cloudMap.points.push_back(pt_insert); 142 | } 143 | } 144 | } 145 | } 146 | 147 | cloudMap.width = cloudMap.points.size(); 148 | cloudMap.height = 1; 149 | cloudMap.is_dense = true; 150 | 151 | ROS_WARN("Finished generate random map "); 152 | cout << cloudMap.size() << endl; 153 | kdtreeLocalMap.setInputCloud(cloudMap.makeShared()); 154 | map_ok = true; 155 | } 156 | 157 | void 158 | rcvOdometryCallbck(const nav_msgs::Odometry odom) 159 | { 160 | if (odom.child_frame_id == "X" || odom.child_frame_id == "O") 161 | return; 162 | _odom = odom; 163 | _has_odom = true; 164 | 165 | _state = { _odom.pose.pose.position.x, 166 | _odom.pose.pose.position.y, 167 | _odom.pose.pose.position.z, 168 | _odom.twist.twist.linear.x, 169 | _odom.twist.twist.linear.y, 170 | _odom.twist.twist.linear.z, 171 | 0.0, 172 | 0.0, 173 | 0.0 }; 174 | 175 | _odom_queue.push_back(odom); 176 | while (_odom_queue.size() > _odom_queue_size) 177 | _odom_queue.pop_front(); 178 | } 179 | 180 | int frequence_division_global = 40; 181 | 182 | void 183 | publishAllPoints() 184 | { 185 | if (!map_ok) 186 | return; 187 | 188 | if ((ros::Time::now() - begin_time).toSec() > 7.0) 189 | return; 190 | 191 | frequence_division_global--; 192 | if (frequence_division_global == 0) 193 | { 194 | pcl::toROSMsg(cloudMap, globalMap_pcd); 195 | globalMap_pcd.header.frame_id = kFrameIdNs_; 196 | _global_map_pub.publish(globalMap_pcd); 197 | frequence_division_global = 40; 198 | ROS_ERROR("[SERVER]Publish one global map"); 199 | } 200 | } 201 | 202 | void 203 | pubSensedPoints() 204 | { 205 | if (!map_ok || !_has_odom) 206 | return; 207 | 208 | ros::Time time_bef_sensing = ros::Time::now(); 209 | 210 | pcl::PointCloud localMap; 211 | 212 | pcl::PointXYZ searchPoint(_state[0], _state[1], _state[2]); 213 | pointIdxRadiusSearch.clear(); 214 | pointRadiusSquaredDistance.clear(); 215 | 216 | pcl::PointXYZ ptInNoflation; 217 | 218 | if (kdtreeLocalMap.radiusSearch(searchPoint, _sensing_range, 219 | pointIdxRadiusSearch, 220 | pointRadiusSquaredDistance) > 0) 221 | { 222 | for (size_t i = 0; i < pointIdxRadiusSearch.size(); ++i) 223 | { 224 | ptInNoflation = cloudMap.points[pointIdxRadiusSearch[i]]; 225 | localMap.points.push_back(ptInNoflation); 226 | } 227 | } 228 | else 229 | { 230 | // ROS_ERROR("[Map server] No obstacles ."); 231 | // cout< 5.0) 253 | return; 254 | 255 | frequence_division_global--; 256 | if (frequence_division_global == 0) 257 | { 258 | pcl::toROSMsg(cloudMap, globalMap_pcd); 259 | globalMap_pcd.header.frame_id = kFrameIdNs_; 260 | _global_map_pub.publish(globalMap_pcd); 261 | frequence_division_global = 40; 262 | ROS_INFO("[SERVER]Publish one global map"); 263 | } 264 | } 265 | -------------------------------------------------------------------------------- /Traj_gen by L-BFGS/src/map_gen/mockamap/src/mockamap.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | #include 9 | #include 10 | #include 11 | 12 | #include "maps.hpp" 13 | 14 | void 15 | optimizeMap(mocka::Maps::BasicInfo& in) 16 | { 17 | std::vector* temp = new std::vector; 18 | 19 | pcl::KdTreeFLANN kdtree; 20 | pcl::PointCloud::Ptr cloud(new pcl::PointCloud); 21 | 22 | cloud->width = in.cloud->width; 23 | cloud->height = in.cloud->height; 24 | cloud->points.resize(cloud->width * cloud->height); 25 | 26 | for (int i = 0; i < cloud->width; i++) 27 | { 28 | cloud->points[i].x = in.cloud->points[i].x; 29 | cloud->points[i].y = in.cloud->points[i].y; 30 | cloud->points[i].z = in.cloud->points[i].z; 31 | } 32 | 33 | kdtree.setInputCloud(cloud); 34 | double radius = 1.75 / in.scale; // 1.75 is the rounded up value of sqrt(3) 35 | 36 | for (int i = 0; i < cloud->width; i++) 37 | { 38 | std::vector pointIdxRadiusSearch; 39 | std::vector pointRadiusSquaredDistance; 40 | 41 | if (kdtree.radiusSearch(cloud->points[i], radius, pointIdxRadiusSearch, 42 | pointRadiusSquaredDistance) >= 27) 43 | { 44 | temp->push_back(i); 45 | } 46 | } 47 | for (int i = temp->size() - 1; i >= 0; i--) 48 | { 49 | in.cloud->points.erase(in.cloud->points.begin() + 50 | temp->at(i)); // erasing the enclosed points 51 | } 52 | in.cloud->width -= temp->size(); 53 | 54 | pcl::toROSMsg(*in.cloud, *in.output); 55 | in.output->header.frame_id = "odom"; 56 | ROS_INFO("finish: number of points after optimization %d", in.cloud->width); 57 | delete temp; 58 | return; 59 | } 60 | 61 | int 62 | main(int argc, char** argv) 63 | { 64 | ros::init(argc, argv, "mockamap"); 65 | ros::NodeHandle nh; 66 | ros::NodeHandle nh_private("~"); 67 | 68 | ros::Publisher pcl_pub = 69 | nh.advertise("mock_map", 1); 70 | pcl::PointCloud cloud; 71 | sensor_msgs::PointCloud2 output; 72 | // Fill in the cloud data 73 | 74 | int seed; 75 | 76 | int sizeX; 77 | int sizeY; 78 | int sizeZ; 79 | 80 | double scale; 81 | double update_freq; 82 | 83 | int type; 84 | 85 | nh_private.param("seed", seed, 4546); 86 | nh_private.param("update_freq", update_freq, 1.0); 87 | nh_private.param("resolution", scale, 0.38); 88 | nh_private.param("x_length", sizeX, 100); 89 | nh_private.param("y_length", sizeY, 100); 90 | nh_private.param("z_length", sizeZ, 10); 91 | 92 | nh_private.param("type", type, 1); 93 | 94 | scale = 1 / scale; 95 | sizeX = sizeX * scale; 96 | sizeY = sizeY * scale; 97 | sizeZ = sizeZ * scale; 98 | 99 | mocka::Maps::BasicInfo info; 100 | info.nh_private = &nh_private; 101 | info.sizeX = sizeX; 102 | info.sizeY = sizeY; 103 | info.sizeZ = sizeZ; 104 | info.seed = seed; 105 | info.scale = scale; 106 | info.output = &output; 107 | info.cloud = &cloud; 108 | 109 | mocka::Maps map; 110 | map.setInfo(info); 111 | map.generate(type); 112 | 113 | // optimizeMap(info); 114 | 115 | //! @note publish loop 116 | ros::Rate loop_rate(update_freq); 117 | while (ros::ok()) 118 | { 119 | pcl_pub.publish(output); 120 | ros::spinOnce(); 121 | loop_rate.sleep(); 122 | } 123 | return 0; 124 | } 125 | -------------------------------------------------------------------------------- /Traj_gen by L-BFGS/src/map_gen/mockamap/src/perlinnoise.cpp: -------------------------------------------------------------------------------- 1 | #include "perlinnoise.hpp" 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | // THIS IS A DIRECT TRANSLATION TO C++14 FROM THE REFERENCE 9 | // JAVA IMPLEMENTATION OF THE IMPROVED PERLIN FUNCTION (see 10 | // http://mrl.nyu.edu/~perlin/noise/) 11 | // THE ORIGINAL JAVA IMPLEMENTATION IS COPYRIGHT 2002 KEN PERLIN 12 | 13 | // I ADDED AN EXTRA METHOD THAT GENERATES A NEW PERMUTATION VECTOR (THIS IS NOT 14 | // PRESENT IN THE ORIGINAL IMPLEMENTATION) 15 | 16 | // Initialize with the reference values for the permutation vector 17 | PerlinNoise::PerlinNoise() 18 | { 19 | 20 | // Initialize the permutation vector with the reference values 21 | p = { 151, 160, 137, 91, 90, 15, 131, 13, 201, 95, 96, 53, 194, 233, 22 | 7, 225, 140, 36, 103, 30, 69, 142, 8, 99, 37, 240, 21, 10, 23 | 23, 190, 6, 148, 247, 120, 234, 75, 0, 26, 197, 62, 94, 252, 24 | 219, 203, 117, 35, 11, 32, 57, 177, 33, 88, 237, 149, 56, 87, 25 | 174, 20, 125, 136, 171, 168, 68, 175, 74, 165, 71, 134, 139, 48, 26 | 27, 166, 77, 146, 158, 231, 83, 111, 229, 122, 60, 211, 133, 230, 27 | 220, 105, 92, 41, 55, 46, 245, 40, 244, 102, 143, 54, 65, 25, 28 | 63, 161, 1, 216, 80, 73, 209, 76, 132, 187, 208, 89, 18, 169, 29 | 200, 196, 135, 130, 116, 188, 159, 86, 164, 100, 109, 198, 173, 186, 30 | 3, 64, 52, 217, 226, 250, 124, 123, 5, 202, 38, 147, 118, 126, 31 | 255, 82, 85, 212, 207, 206, 59, 227, 47, 16, 58, 17, 182, 189, 32 | 28, 42, 223, 183, 170, 213, 119, 248, 152, 2, 44, 154, 163, 70, 33 | 221, 153, 101, 155, 167, 43, 172, 9, 129, 22, 39, 253, 19, 98, 34 | 108, 110, 79, 113, 224, 232, 178, 185, 112, 104, 218, 246, 97, 228, 35 | 251, 34, 242, 193, 238, 210, 144, 12, 191, 179, 162, 241, 81, 51, 36 | 145, 235, 249, 14, 239, 107, 49, 192, 214, 31, 181, 199, 106, 157, 37 | 184, 84, 204, 176, 115, 121, 50, 45, 127, 4, 150, 254, 138, 236, 38 | 205, 93, 222, 114, 67, 29, 24, 72, 243, 141, 128, 195, 78, 66, 39 | 215, 61, 156, 180 }; 40 | // Duplicate the permutation vector 41 | p.insert(p.end(), p.begin(), p.end()); 42 | } 43 | 44 | // Generate a new permutation vector based on the value of seed 45 | PerlinNoise::PerlinNoise(unsigned int seed) 46 | { 47 | p.resize(256); 48 | 49 | // Fill p with values from 0 to 255 50 | std::iota(p.begin(), p.end(), 0); 51 | 52 | // Initialize a random engine with seed 53 | std::default_random_engine engine(seed); 54 | 55 | // Suffle using the above random engine 56 | std::shuffle(p.begin(), p.end(), engine); 57 | 58 | // Duplicate the permutation vector 59 | p.insert(p.end(), p.begin(), p.end()); 60 | } 61 | 62 | double 63 | PerlinNoise::noise(double x, double y, double z) 64 | { 65 | // Find the unit cube that contains the point 66 | int X = (int)floor(x) & 255; 67 | int Y = (int)floor(y) & 255; 68 | int Z = (int)floor(z) & 255; 69 | 70 | // Find relative x, y,z of point in cube 71 | x -= floor(x); 72 | y -= floor(y); 73 | z -= floor(z); 74 | 75 | // Compute fade curves for each of x, y, z 76 | double u = fade(x); 77 | double v = fade(y); 78 | double w = fade(z); 79 | 80 | // Hash coordinates of the 8 cube corners 81 | int A = p[X] + Y; 82 | int AA = p[A] + Z; 83 | int AB = p[A + 1] + Z; 84 | int B = p[X + 1] + Y; 85 | int BA = p[B] + Z; 86 | int BB = p[B + 1] + Z; 87 | 88 | // Add blended results from 8 corners of cube 89 | double res = 90 | lerp(w, // 91 | lerp(v, // 92 | lerp(u, grad(p[AA], x, y, z), grad(p[BA], x - 1, y, z)), 93 | lerp(u, grad(p[AB], x, y - 1, z), grad(p[BB], x - 1, y - 1, z))), 94 | lerp(v, // 95 | lerp(u, // 96 | grad(p[AA + 1], x, y, z - 1), // 97 | grad(p[BA + 1], x - 1, y, z - 1)), 98 | lerp(u, // 99 | grad(p[AB + 1], x, y - 1, z - 1), 100 | grad(p[BB + 1], x - 1, y - 1, z - 1)))); 101 | return (res + 1.0) / 2.0; 102 | } 103 | 104 | double 105 | PerlinNoise::fade(double t) 106 | { 107 | return t * t * t * (t * (t * 6 - 15) + 10); 108 | } 109 | 110 | double 111 | PerlinNoise::lerp(double t, double a, double b) 112 | { 113 | return a + t * (b - a); 114 | } 115 | 116 | double 117 | PerlinNoise::grad(int hash, double x, double y, double z) 118 | { 119 | int h = hash & 15; 120 | // Convert lower 4 bits of hash into 12 gradient directions 121 | double u = h < 8 ? x : y, v = h < 4 ? y : h == 12 || h == 14 ? x : z; 122 | return ((h & 1) == 0 ? u : -u) + ((h & 2) == 0 ? v : -v); 123 | } 124 | --------------------------------------------------------------------------------