├── .gitignore ├── README.md ├── project1 ├── .vscode │ └── settings.json ├── CMakeLists.txt ├── ReadMe.assets │ ├── Figure_1.png │ ├── image-20220716101638403.png │ └── image-20220716103849041.png ├── ReadMe.md ├── common │ ├── CMakeLists.txt │ ├── config_flags.cc │ ├── config_flags.hpp │ ├── logger.cc │ └── logger.hpp ├── main.cpp ├── modules │ ├── base │ │ ├── CMakeLists.txt │ │ └── cost_fuction.hpp │ └── unconstrained_optimization │ │ ├── CMakeLists.txt │ │ └── steepest_gradient_descent.hpp ├── problem.hpp ├── results │ └── recorder.txt ├── run.sh ├── unit_testing │ ├── CMakeLists.txt │ └── opt_sgd_test.cpp └── viewer │ ├── Figure_1.png │ └── plot_data.py └── project2 ├── .catkin_workspace └── gcopter ├── CMakeLists.txt ├── ReadMe.assets ├── image-20220731145839800.png ├── image-20220731145955498.png ├── image-20220731150208152.png ├── image-20220731150823094.png └── image-20220731151006783.png ├── ReadMe.md ├── 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 /.gitignore: -------------------------------------------------------------------------------- 1 | # Prerequisites 2 | log/ 3 | build/ 4 | project1/build/ 5 | project1/log/ 6 | *.d 7 | 8 | # Compiled Object files 9 | *.slo 10 | *.lo 11 | *.o 12 | *.obj 13 | 14 | # Precompiled Headers 15 | *.gch 16 | *.pch 17 | 18 | # Compiled Dynamic libraries 19 | *.so 20 | *.dylib 21 | *.dll 22 | 23 | # Fortran module files 24 | *.mod 25 | *.smod 26 | 27 | # Compiled Static libraries 28 | *.lai 29 | *.la 30 | *.a 31 | *.lib 32 | 33 | # Executables 34 | *.exe 35 | *.out 36 | *.app 37 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Numerical_Optimization 2 | 深蓝学院《机器人中的数值优化》第一期 3 | -------------------------------------------------------------------------------- /project1/.vscode/settings.json: -------------------------------------------------------------------------------- 1 | { 2 | "files.associations": { 3 | "iosfwd": "cpp", 4 | "fstream": "cpp" 5 | } 6 | } 7 | -------------------------------------------------------------------------------- /project1/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.10.2) 2 | project(AMao LANGUAGES CXX VERSION 0.0.1) 3 | 4 | # -fPIC：生成动态库，-fopenmp 开启多线程，-O3 对代码进行优化，-g 打印调试信息，-Wall 打印所有警告信息, pthread 支持多线程 5 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fPIC -std=c++17 -g -O3 -fopenmp -pthread") 6 | 7 | # 不生成警告信息: debug阶段尽量不要使用 8 | add_definitions(-w) 9 | 10 | option(BUILD_TEST "Build all tests." ON) 11 | if(BUILD_TEST) 12 | find_package(GTest REQUIRED) 13 | enable_testing() 14 | endif() 15 | 16 | find_package(Boost REQUIRED COMPONENTS thread filesystem program_options system) 17 | 18 | link_directories( 19 | ${Boost_INCLUDE_DIRS} 20 | ${PROJECT_SOURCE_DIR}/common 21 | ${PROJECT_SOURCE_DIR}/modules/base 22 | ${PROJECT_SOURCE_DIR}/modules/unconstrained_optimization 23 | ) 24 | 25 | add_subdirectory(common) 26 | add_subdirectory(modules/base) 27 | add_subdirectory(modules/unconstrained_optimization) 28 | 29 | 30 | 31 | if(BUILD_TEST) 32 | add_subdirectory(unit_testing) 33 | endif() 34 | 35 | add_executable(${PROJECT_NAME} main.cpp) 36 | target_compile_features(${PROJECT_NAME} PRIVATE cxx_std_17) 37 | target_link_libraries(${PROJECT_NAME} PRIVATE 38 | # dw 39 | common 40 | base 41 | unconstrained_optimization 42 | ${Boost_LIBRARIES} 43 | ) 44 | -------------------------------------------------------------------------------- /project1/ReadMe.assets/Figure_1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ChenJiahao031008/Numerical_Optimization/b3e66365546fda3e44cd7935e022aea713f43ea2/project1/ReadMe.assets/Figure_1.png -------------------------------------------------------------------------------- /project1/ReadMe.assets/image-20220716101638403.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ChenJiahao031008/Numerical_Optimization/b3e66365546fda3e44cd7935e022aea713f43ea2/project1/ReadMe.assets/image-20220716101638403.png -------------------------------------------------------------------------------- /project1/ReadMe.assets/image-20220716103849041.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ChenJiahao031008/Numerical_Optimization/b3e66365546fda3e44cd7935e022aea713f43ea2/project1/ReadMe.assets/image-20220716103849041.png -------------------------------------------------------------------------------- /project1/ReadMe.md: -------------------------------------------------------------------------------- 1 | ## Project 1: Linear-search Steepest Gradient Descent 2 | 3 | From: https://github.com/ChenJiahao031008/Numerical_Optimization 4 | 5 | ### 1 Dependency 6 | 7 | ``` 8 | gcc >= 9 9 | Eigen 3.2.7 10 | glog 11 | gtest 12 | boost 13 | ``` 14 | 15 | ### 2 How to use 16 | 17 | ```bash 18 | bash run.sh build # build project 19 | bash run.sh build_and_run # build project and run program 20 | bash run.sh build_and_test # build project and run gtest 21 | bash run.sh run # run program 22 | bash run.sh test # run gtest 23 | bash run.sh clean # clean up all cache and 'build' folder 24 | ``` 25 | 26 | ### 3 System Structure 27 | 28 | ```shell 29 | common: provide logging module, parameter server and other tools. 30 | modules: provide algorithm core 31 | viewer: provide visualization of results 32 | unit_testing: provide gtest as unit test 33 | ``` 34 | 35 | ### 4 Workflow 36 | 37 | if you want to use `Linear-search Steepest Gradient Descent` algorithm, please define cost function, which is inherited from class `CostFunction`. And rewrite the `ComputeFunction` and `ComputeJacobian` function. For example, generate a polynomial function: 38 | 39 | ```c++ 40 | class Example : public CostFunction 41 | { 42 | 43 | public: 44 | Example(Eigen::VectorXd ¶m) : CostFunction(param){}; 45 | 46 | ~Example() = default; 47 | 48 | double ComputeFunction(const Eigen::VectorXd &x) override 49 | { 50 | double result = 0; 51 | result = x[0] * x[0] + 2 * x[1] * x[1] - 2 * x[0] * x[1] - 2 * x[1]; 52 | return result; 53 | } 54 | 55 | Eigen::VectorXd ComputeJacobian(const Eigen::VectorXd &x) override 56 | { 57 | Eigen::VectorXd jacobian(x.rows()); 58 | jacobian(0, 0) = 2 * x[0] - 2 * x[1]; 59 | jacobian(1, 0) = 4 * x[1] - 2 * x[0] - 2; 60 | return jacobian; 61 | } 62 | }; 63 | ``` 64 | 65 | and instantiate in the main program: 66 | 67 | ```c++ 68 | Eigen::VectorXd x(2); 69 | x << 0.0, 0.0; 70 | Example ex(x); 71 | ``` 72 | 73 | There are two ways to instantiate an SGD solver and set relevant parameters: 74 | 75 | ```c++ 76 | // CASE 1 77 | GradientDescent gd; 78 | gd.SetCostFunction(&ex); 79 | gd.SetEpsilon(1e-6); 80 | gd.SetC(0.5); 81 | gd.SetTau(1.0); 82 | gd.SetMaxIters(100); 83 | gd.SetVerbose(false); // whether the process is printed 84 | // CASE 2 85 | GradientDescent gd(1.0, 0.5, 1e-6, 100, false); 86 | gd.SetCostFunction(&ex); 87 | ``` 88 | 89 | then, use `Solve` function to solve problem: 90 | 91 | ```c++ 92 | auto res = gd.Solve(); 93 | ``` 94 | 95 | if you want to record the iteration process, as follows: 96 | 97 | ```c++ 98 | #if GCC_VERSION >= 90400 99 | namespace fs = std::filesystem; 100 | #else 101 | namespace fs = boost::filesystem; 102 | #endif 103 | const std::string recorder_path 104 | = fs::current_path().string()+ "/" + common::FLAGS_iter_recorder_file; 105 | fs::path recoder_file(recorder_path); 106 | AINFO << "Recorder File Path is : " << recoder_file.string(); 107 | if (!fs::exists(recoder_file)) 108 | fs::create_directories(recoder_file.parent_path()); 109 | std::ofstream outfile(recorder_path, std::ios::out); 110 | auto res = gd.Solve(outfile); 111 | ``` 112 | 113 | ### 5 Results 114 | 115 | 1. Multi-dimensional Rosenbrock Function 116 | 117 | ![](ReadMe.assets/image-20220716101638403.png) 118 | 119 | 2. Two-dimensional Rosenbrock Function 120 | 121 | ![](ReadMe.assets/image-20220716103849041.png) 122 | 123 | 3. Two-dimensional Rosenbrock Function Visualization 124 | 125 | ![](ReadMe.assets/Figure_1.png) 126 | 127 | -------------------------------------------------------------------------------- /project1/common/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | project (common) 2 | 3 | find_package(Glog) 4 | 5 | file(GLOB _SRCS "*.cc" "*.h" "*.hpp") 6 | add_library(${PROJECT_NAME} OBJECT ${_SRCS}) 7 | 8 | target_compile_features(${PROJECT_NAME} PRIVATE cxx_std_17) 9 | target_include_directories(${PROJECT_NAME} PRIVATE 10 | PUBLIC ${PROJECT_SOURCE_DIR} 11 | ) 12 | target_link_libraries(${PROJECT_NAME} PRIVATE 13 | gflags 14 | glog 15 | ) 16 | 17 | -------------------------------------------------------------------------------- /project1/common/config_flags.cc: -------------------------------------------------------------------------------- 1 | #include "config_flags.hpp" 2 | namespace common { 3 | 4 | DEFINE_string(iter_recorder_file, 5 | "../results/recorder.txt", 6 | "The file to save the iteration records."); 7 | 8 | 9 | } // namespace common 10 | -------------------------------------------------------------------------------- /project1/common/config_flags.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | 4 | namespace common { 5 | 6 | DECLARE_string(iter_recorder_file); 7 | 8 | } // namespace common 9 | 10 | -------------------------------------------------------------------------------- /project1/common/logger.cc: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include "logger.hpp" 4 | 5 | // #define LOG_CORE_DUMP_CAPTURE 6 | namespace common { 7 | 8 | Logger::Logger(int &argsize, char**& program) { 9 | /* 如果需要调整日志配置，请修改下面代码 */ 10 | google::ParseCommandLineFlags(&argsize, &program, false); 11 | google::InitGoogleLogging(program[0]); 12 | 13 | FLAGS_colorlogtostderr = true; // 设置输出到屏幕的日志显示对应颜色 14 | FLAGS_logbufsecs = 0; // 缓冲日志立即输出 15 | FLAGS_max_log_size = 100; // 最大日志大小为 100MB 16 | FLAGS_stop_logging_if_full_disk = true; // 当磁盘被写满时，停止日志输出 17 | FLAGS_alsologtostderr = true; // 输出到屏幕的日志也输出到文件 18 | 19 | google::SetStderrLogging(google::INFO); // 设置级别高于INFO 的日志同一时候输出到屏幕 20 | 21 | #ifdef LOG_CORE_DUMP_CAPTURE 22 | google::InstallFailureSignalHandler(); // 捕捉 core dumped 23 | #endif 24 | /* 如果需要调整文件位置，请修改下面代码 */ 25 | #if GCC_VERSION >= 90400 26 | auto curr_path = std::filesystem::current_path(); // 获取当前路径 27 | auto log_path = curr_path.parent_path() / "log"; // 日志文件存放路径,支持/重载 28 | if (!std::filesystem::exists(log_path)) { 29 | std::filesystem::create_directory(log_path); 30 | AINFO << "Create Log Folder: " << log_path.string(); 31 | } 32 | #else 33 | auto curr_path = boost::filesystem::current_path(); // 获取当前路径 34 | auto log_path = curr_path.parent_path() / "log"; // 日志文件存放路径,支持/重载 35 | if (!boost::filesystem::exists(log_path)){ 36 | boost::filesystem::create_directory(log_path); //文件夹不存在则创建一个 37 | AINFO << "Create Log Folder: " << log_path.string(); 38 | } 39 | #endif 40 | 41 | std::string module_path = log_path.string() + "/" + MODULE_NAME; 42 | 43 | /* 判断是否需要ROS节点作为模块名称 */ 44 | #ifdef ROS 45 | module_path = LOGDIR + ros::this_node::getName(); 46 | #endif 47 | google::SetLogDestination(google::INFO, (module_path + "_INFO_").c_str()); 48 | google::SetLogDestination(google::WARNING, (module_path + "_WARNING_").c_str()); 49 | google::SetLogDestination(google::ERROR, (module_path + "_ERROR_").c_str()); 50 | google::SetLogDestination(google::FATAL, (module_path + "_FATAL_").c_str()); 51 | } 52 | 53 | Logger::~Logger() 54 | { 55 | google::ShutdownGoogleLogging(); 56 | std::cout << "[LOGGER WARNING] Logger IS FINISHED!" << std::endl; 57 | } 58 | 59 | } // namespace common 60 | -------------------------------------------------------------------------------- /project1/common/logger.hpp: -------------------------------------------------------------------------------- 1 | // LOG LINE PREFIX FORMAT 2 | // 3 | // Log lines have this form: 4 | // 5 | // Lmmdd hh:mm:ss.uuuuuu threadid file:line] msg... 6 | // 7 | // where the fields are defined as follows: 8 | // 9 | // L A single character, representing the log level 10 | // (eg 'I' for INFO) 11 | // mm The month (zero padded; ie May is '05') 12 | // dd The day (zero padded) 13 | // hh:mm:ss.uuuuuu Time in hours, minutes and fractional seconds 14 | // threadid The space-padded thread ID as returned by GetTID() 15 | // (this matches the PID on Linux) 16 | // file The file name 17 | // line The line number 18 | // msg The user-supplied message 19 | // 20 | 21 | #pragma once 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include "config_flags.hpp" 28 | 29 | #define MODULE_NAME "MAIN_MODULE" 30 | 31 | #define GCC_VERSION (__GNUC__ * 10000 + __GNUC_MINOR__ * 100 + __GNUC_PATCHLEVEL_) 32 | #if GCC_VERSION >= 90400 33 | #include 34 | #else 35 | #include 36 | #endif 37 | 38 | /* 当存在ROS环境时，可以用ROS模块的节点名称代替默认模块名称 */ 39 | // #define ROS 40 | #ifdef ROS 41 | #include 42 | #define MODULE_NAME ros::this_node::getName().c_str() 43 | #endif 44 | 45 | /* 调整glog默认配置 */ 46 | #define DEFAULT_CONFIG 47 | #ifdef DEFAULT_CONFIG 48 | namespace common 49 | { 50 | class Logger 51 | { 52 | public: 53 | Logger(int &argsize, char **&program); 54 | ~Logger(); 55 | }; // namespace Logger 56 | } // namespace common 57 | #endif 58 | 59 | #define LEFT_BRACKET "[" 60 | #define RIGHT_BRACKET "]" 61 | 62 | 63 | #define ADEBUG_MODULE(module) \ 64 | VLOG(4) << LEFT_BRACKET << module << RIGHT_BRACKET << "[DEBUG] " 65 | #define ADEBUG ADEBUG_MODULE(MODULE_NAME) 66 | #define AINFO ALOG_MODULE(MODULE_NAME, INFO) 67 | #define AWARN ALOG_MODULE(MODULE_NAME, WARN) 68 | #define AERROR ALOG_MODULE(MODULE_NAME, ERROR) 69 | #define AFATAL ALOG_MODULE(MODULE_NAME, FATAL) 70 | 71 | #ifndef ALOG_MODULE_STREAM 72 | #define ALOG_MODULE_STREAM(log_severity) ALOG_MODULE_STREAM_##log_severity 73 | #endif 74 | 75 | #ifndef ALOG_MODULE 76 | #define ALOG_MODULE(module, log_severity) \ 77 | ALOG_MODULE_STREAM(log_severity) \ 78 | (module) 79 | #endif 80 | 81 | #define ALOG_MODULE_STREAM_INFO(module) \ 82 | google::LogMessage(__FILE__, __LINE__, google::INFO).stream() \ 83 | << LEFT_BRACKET << module << RIGHT_BRACKET 84 | 85 | #define ALOG_MODULE_STREAM_WARN(module) \ 86 | google::LogMessage(__FILE__, __LINE__, google::WARNING).stream() \ 87 | << LEFT_BRACKET << module << RIGHT_BRACKET 88 | 89 | #define ALOG_MODULE_STREAM_ERROR(module) \ 90 | google::LogMessage(__FILE__, __LINE__, google::ERROR).stream() \ 91 | << LEFT_BRACKET << module << RIGHT_BRACKET 92 | 93 | #define ALOG_MODULE_STREAM_FATAL(module) \ 94 | google::LogMessage(__FILE__, __LINE__, google::FATAL).stream() \ 95 | << LEFT_BRACKET << module << RIGHT_BRACKET 96 | 97 | #define AINFO_IF(cond) ALOG_IF(INFO, cond, MODULE_NAME) 98 | #define AWARN_IF(cond) ALOG_IF(WARN, cond, MODULE_NAME) 99 | #define AERROR_IF(cond) ALOG_IF(ERROR, cond, MODULE_NAME) 100 | #define AFATAL_IF(cond) ALOG_IF(FATAL, cond, MODULE_NAME) 101 | #define ALOG_IF(severity, cond, module) \ 102 | !(cond) ? (void)0 \ 103 | : google::LogMessageVoidify() & ALOG_MODULE(module, severity) 104 | 105 | #define ACHECK(cond) CHECK(cond) << LEFT_BRACKET << MODULE_NAME << RIGHT_BRACKET 106 | 107 | #define AINFO_EVERY(freq) \ 108 | LOG_EVERY_N(INFO, freq) << LEFT_BRACKET << MODULE_NAME << RIGHT_BRACKET 109 | #define AWARN_EVERY(freq) \ 110 | LOG_EVERY_N(WARNING, freq) << LEFT_BRACKET << MODULE_NAME << RIGHT_BRACKET 111 | #define AERROR_EVERY(freq) \ 112 | LOG_EVERY_N(ERROR, freq) << LEFT_BRACKET << MODULE_NAME << RIGHT_BRACKET 113 | 114 | #if !defined(RETURN_IF_NULL) 115 | #define RETURN_IF_NULL(ptr) \ 116 | if (ptr == nullptr) \ 117 | { \ 118 | AWARN << #ptr << " is nullptr."; \ 119 | return; \ 120 | } 121 | #endif 122 | 123 | #if !defined(RETURN_VAL_IF_NULL) 124 | #define RETURN_VAL_IF_NULL(ptr, val) \ 125 | if (ptr == nullptr) \ 126 | { \ 127 | AWARN << #ptr << " is nullptr."; \ 128 | return val; \ 129 | } 130 | #endif 131 | 132 | #if !defined(RETURN_IF) 133 | #define RETURN_IF(condition) \ 134 | if (condition) \ 135 | { \ 136 | AWARN << #condition << " is met."; \ 137 | return; \ 138 | } 139 | #endif 140 | 141 | #if !defined(RETURN_VAL_IF) 142 | #define RETURN_VAL_IF(condition, val) \ 143 | if (condition) \ 144 | { \ 145 | AWARN << #condition << " is met."; \ 146 | return val; \ 147 | } 148 | #endif 149 | 150 | #if !defined(_RETURN_VAL_IF_NULL2__) 151 | #define _RETURN_VAL_IF_NULL2__ 152 | #define RETURN_VAL_IF_NULL2(ptr, val) \ 153 | if (ptr == nullptr) \ 154 | { \ 155 | return (val); \ 156 | } 157 | #endif 158 | 159 | #if !defined(_RETURN_VAL_IF2__) 160 | #define _RETURN_VAL_IF2__ 161 | #define RETURN_VAL_IF2(condition, val) \ 162 | if (condition) \ 163 | { \ 164 | return (val); \ 165 | } 166 | #endif 167 | 168 | #if !defined(_RETURN_IF2__) 169 | #define _RETURN_IF2__ 170 | #define RETURN_IF2(condition) \ 171 | if (condition) \ 172 | { \ 173 | return; \ 174 | } 175 | #endif 176 | -------------------------------------------------------------------------------- /project1/main.cpp: -------------------------------------------------------------------------------- 1 | #include "problem.hpp" 2 | // #define BACKWARD_HAS_DW 1 3 | // #include "backward.hpp" 4 | // namespace backward 5 | // { 6 | // backward::SignalHandling sh; 7 | // } // namespace backward 8 | 9 | int main(int argc, char** argv) 10 | { 11 | common::Logger logger(argc, argv); 12 | 13 | Eigen::Vector2d x(0, 0); 14 | 15 | #if GCC_VERSION >= 90400 16 | namespace fs = std::filesystem; 17 | #else 18 | namespace fs = boost::filesystem; 19 | #endif 20 | const std::string recorder_path 21 | = fs::current_path().string()+ "/" + common::FLAGS_iter_recorder_file; 22 | fs::path recoder_file(recorder_path); 23 | AINFO << "Recorder File Path is : " << recoder_file.string(); 24 | if (!fs::exists(recoder_file)) 25 | fs::create_directories(recoder_file.parent_path()); 26 | std::ofstream outfile(recorder_path, std::ios::out); 27 | 28 | GradientDescent gd; 29 | gd.SetEpsilon(1e-3); 30 | gd.SetC(0.1); 31 | gd.SetTau(1.0); 32 | gd.SetVerbose(true); 33 | 34 | Rosenbrock2dExample rosenbrock(x); 35 | auto res = gd.Solve(rosenbrock); 36 | AINFO << "result = [" << res.transpose() << "]"; 37 | AINFO << "f = " << rosenbrock.ComputeFunction(res); 38 | 39 | return 0; 40 | } 41 | -------------------------------------------------------------------------------- /project1/modules/base/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | project (base) 2 | 3 | file(GLOB _SRCS "*.cc" "*.h" "*.hpp") 4 | add_library(${PROJECT_NAME} OBJECT ${_SRCS}) 5 | 6 | target_compile_features(${PROJECT_NAME} PRIVATE cxx_std_17) 7 | target_include_directories(${PROJECT_NAME} PRIVATE 8 | PUBLIC ${PROJECT_SOURCE_DIR} 9 | ) 10 | target_link_libraries(${PROJECT_NAME} PRIVATE 11 | common 12 | ) 13 | 14 | -------------------------------------------------------------------------------- /project1/modules/base/cost_fuction.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include "../../common/logger.hpp" 6 | 7 | namespace modules::optimization{ 8 | 9 | template 10 | class CostFunction 11 | { 12 | public: 13 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 14 | const int N; 15 | Eigen::Matrix<_Scalar, _Rows, 1> x; 16 | 17 | public: 18 | CostFunction(const Eigen::Matrix<_Scalar, _Rows, 1> ¶m) : N(param.rows()), x(param){}; 19 | 20 | virtual ~CostFunction()=default; 21 | 22 | virtual double ComputeFunction(const Eigen::Matrix<_Scalar, _Rows, 1> &x) = 0; 23 | 24 | virtual Eigen::Matrix<_Scalar, _Rows, 1> ComputeJacobian(const Eigen::Matrix<_Scalar, _Rows, 1> &x) = 0; 25 | 26 | Eigen::Matrix<_Scalar, _Rows, 1> GetInitParam() { return x; }; 27 | 28 | int GetParamSize() { return N; }; 29 | }; 30 | 31 | } // namespace modules::optimization 32 | -------------------------------------------------------------------------------- /project1/modules/unconstrained_optimization/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | project (unconstrained_optimization) 2 | 3 | file(GLOB _SRCS "*.cc" "*.h" "*.hpp") 4 | add_library(${PROJECT_NAME} OBJECT ${_SRCS}) 5 | 6 | target_compile_features(${PROJECT_NAME} PRIVATE cxx_std_17) 7 | target_include_directories(${PROJECT_NAME} PRIVATE 8 | PUBLIC ${PROJECT_SOURCE_DIR} 9 | ) 10 | target_link_libraries(${PROJECT_NAME} PRIVATE 11 | common 12 | base 13 | ) 14 | 15 | -------------------------------------------------------------------------------- /project1/modules/unconstrained_optimization/steepest_gradient_descent.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include "../../common/logger.hpp" 6 | #include "../base/cost_fuction.hpp" 7 | 8 | namespace modules::optimization{ 9 | 10 | 11 | class GradientDescent 12 | { 13 | public: 14 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 15 | 16 | private: 17 | double tau, init_tau; 18 | double c; 19 | double epsilon; 20 | int max_iters; 21 | bool verbose; 22 | 23 | public: 24 | GradientDescent() : init_tau(tau), c(0.5), epsilon(1e-6), max_iters(100000), verbose(true){}; 25 | 26 | GradientDescent(double tau_, double c_, double epsilon_, int max_iters_, bool verbose_) 27 | : init_tau(tau_), c(c_), epsilon(epsilon_), 28 | max_iters(max_iters_), verbose(verbose_) {}; 29 | 30 | ~GradientDescent(){}; 31 | 32 | inline void SetTau(double tau_) { init_tau = tau_; }; 33 | inline void SetC(double c_) { c = c_; }; 34 | inline void SetEpsilon(double epsilon_) { epsilon = epsilon_; }; 35 | inline void SetMaxIters(int max_iters_) { max_iters = max_iters_; }; 36 | inline void SetVerbose(bool verbose_) { verbose = verbose_; }; 37 | 38 | // 智能指针不支持抽象类(?) 39 | template 40 | Eigen::Matrix<_Scalar, _Rows, 1> Solve(CostFunction<_Scalar, _Rows> &cost_function) 41 | { 42 | auto x = cost_function.GetInitParam(); 43 | if (verbose) 44 | AINFO << "Iter Count 0: Init vector is [" << x.transpose() << "]"; 45 | 46 | double delta = cost_function.ComputeJacobian(x).norm(); 47 | int iter_count = 0; 48 | while (delta >= epsilon && iter_count < max_iters) 49 | { 50 | tau = init_tau; 51 | while (ArmijoCondition<_Scalar, _Rows>(x, cost_function)) 52 | { 53 | tau = tau * 0.5; 54 | } 55 | auto cur_d = cost_function.ComputeJacobian(x); 56 | x = x - tau * cur_d; 57 | delta = cur_d.norm(); 58 | iter_count++; 59 | if (verbose) 60 | { 61 | AINFO << "Iter-->[" << iter_count << "/" << max_iters << "] delta: " << delta; 62 | AINFO << "Current Param: " << x.transpose(); 63 | } 64 | } 65 | return x; 66 | }; 67 | 68 | template 69 | Eigen::Matrix<_Scalar, _Rows, 1> Solve(CostFunction<_Scalar, _Rows> &cost_function, std::ofstream& output) 70 | { 71 | auto x = cost_function.GetInitParam(); 72 | if (verbose) 73 | AINFO << "Iter Count 0: Init vector is [" << x.transpose() << "]"; 74 | output << std::fixed << std::setprecision(6); 75 | 76 | double delta = cost_function.ComputeJacobian(x).norm(); 77 | int iter_count = 0; 78 | while (delta >= epsilon && iter_count < max_iters) 79 | { 80 | tau = init_tau; 81 | while (ArmijoCondition<_Scalar, _Rows>(x, cost_function)) 82 | { 83 | tau = tau * 0.5; 84 | } 85 | auto cur_d = cost_function.ComputeJacobian(x); 86 | x = x - tau * cur_d; 87 | delta = cur_d.norm(); 88 | iter_count++; 89 | double cur_f = cost_function.ComputeFunction(x); 90 | output << iter_count << " " << x.transpose() << " " << cur_f << std::endl; 91 | 92 | if (verbose) 93 | { 94 | AINFO << "Iter-->[" << iter_count << "/" << max_iters << "] delta: " << delta; 95 | AINFO << "Current Param: " << x.transpose(); 96 | } 97 | 98 | } 99 | return x; 100 | }; 101 | 102 | private: 103 | template 104 | bool ArmijoCondition(Eigen::Matrix<_Scalar, _Rows, 1> &x, CostFunction<_Scalar, _Rows> &cost_function) 105 | { 106 | Eigen::Matrix<_Scalar, _Rows, 1> d = cost_function.ComputeJacobian(x); 107 | double x_k0 = cost_function.ComputeFunction(x); 108 | double x_k1 = cost_function.ComputeFunction(x - d * tau); 109 | double left = x_k1 - x_k0; 110 | double right = -(c * d.transpose() * d * tau)[0]; 111 | if (left >= right) 112 | return true; 113 | else 114 | return false; 115 | }; 116 | }; 117 | 118 | 119 | 120 | 121 | } 122 | -------------------------------------------------------------------------------- /project1/problem.hpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include "common/logger.hpp" 4 | #include "modules/base/cost_fuction.hpp" 5 | #include "modules/unconstrained_optimization/steepest_gradient_descent.hpp" 6 | 7 | using namespace modules::optimization; 8 | 9 | /** 10 | * @brief : 11 | * 待优化求解的Rosenbrock函数, 继承自CostFunction基类, 来自wiki公式4 12 | * @param : 13 | * double* param: 初值; 14 | */ 15 | template 16 | class Rosenbrock : public CostFunction 17 | { 18 | public: 19 | Rosenbrock(const Eigen::Matrix ¶m) : CostFunction(param){}; 20 | 21 | ~Rosenbrock() = default; 22 | 23 | double ComputeFunction(const Eigen::Matrix &x) override 24 | { 25 | double result = 0; 26 | for (size_t i = 0; i < x.rows() - 1; ++i) 27 | { 28 | double part_1 = x[i + 1] - x[i] * x[i]; 29 | double part_2 = 1 - x[i]; 30 | result += 100.0 * part_1 * part_1 + part_2 * part_2; 31 | } 32 | return result; 33 | } 34 | 35 | Eigen::Matrix ComputeJacobian(const Eigen::Matrix &x) override 36 | { 37 | Eigen::Matrix jacobian(x.rows()); 38 | for (size_t i = 0; i < x.rows(); ++i) 39 | { 40 | if (i == 0) 41 | { 42 | jacobian(i, 0) = -400 * x[i] * (x[i + 1] - x[i] * x[i]) - 2 * (1 - x[i]); 43 | } 44 | else if (i == x.rows() - 1) 45 | { 46 | jacobian(i, 0) = 200 * (x[i] - x[i - 1] * x[i - 1]); 47 | } 48 | else 49 | { 50 | jacobian(i, 0) = -400 * x[i] * (x[i + 1] - x[i] * x[i]) - 2 * (1 - x[i]) + 200 * (x[i] - x[i - 1] * x[i - 1]); 51 | } 52 | } 53 | return jacobian; 54 | } 55 | }; 56 | 57 | /** 58 | * @brief : 59 | * 待优化求解的Rosenbrock函数, 继承自CostFunction基类, 来自wiki公式3 60 | * @param : 61 | * double* param: 初值; 62 | */ 63 | template 64 | class Rosenbrock2 : public CostFunction 65 | { 66 | public: 67 | Rosenbrock2(const Eigen::Matrix ¶m) : CostFunction(param){}; 68 | 69 | ~Rosenbrock2() = default; 70 | 71 | double ComputeFunction(const Eigen::Matrix &x) override 72 | { 73 | double result = 0; 74 | for (size_t i = 0; i < x.rows() / 2; ++i) 75 | { 76 | double part_1 = x[2 * i] * x[2 * i] - x[2 * i + 1]; 77 | double part_2 = x[2 * i] - 1; 78 | result += 100.0 * part_1 * part_1 + part_2 * part_2; 79 | } 80 | return result; 81 | } 82 | 83 | Eigen::Matrix ComputeJacobian(const Eigen::Matrix &x) override 84 | { 85 | Eigen::Matrix jacobian(x.rows()); 86 | jacobian.setZero(); 87 | for (size_t i = 0; i < x.rows() / 2; i++) 88 | { 89 | jacobian(2 * i, 0) = 400 * x[2 * i] * (x[2 * i] * x[2 * i] - x[2 * i + 1]) + 2 * (x[2 * i] - 1); 90 | jacobian(2 * i + 1, 0) = -200 * (x[2 * i] * x[2 * i] - x[2 * i + 1]); 91 | } 92 | return jacobian; 93 | } 94 | }; 95 | 96 | /** 97 | * @brief : 98 | * 待优化求解的多项式函数, 继承自CostFunction基类 99 | * @param : 100 | * double* param: 初值; 101 | */ 102 | class Example : public CostFunction 103 | { 104 | 105 | public: 106 | Example(Eigen::Vector2d ¶m) : CostFunction(param){}; 107 | 108 | ~Example() = default; 109 | 110 | double ComputeFunction(const Eigen::Vector2d &x) override 111 | { 112 | double result = 0; 113 | result = x[0] * x[0] + 2 * x[1] * x[1] - 2 * x[0] * x[1] - 2 * x[1]; 114 | return result; 115 | } 116 | 117 | Eigen::Vector2d ComputeJacobian(const Eigen::Vector2d &x) override 118 | { 119 | Eigen::Vector2d jacobian(x.rows()); 120 | jacobian(0, 0) = 2 * x[0] - 2 * x[1]; 121 | jacobian(1, 0) = 4 * x[1] - 2 * x[0] - 2; 122 | return jacobian; 123 | } 124 | }; 125 | 126 | /** 127 | * @brief : 128 | * 待优化求解的Rosenbrock函数, 继承自CostFunction基类，仅有两维便于可视化 129 | * @param : 130 | * double* param: 初值; 131 | */ 132 | class Rosenbrock2dExample : public CostFunction 133 | { 134 | public: 135 | Rosenbrock2dExample(Eigen::Vector2d ¶m) : CostFunction(param){}; 136 | 137 | ~Rosenbrock2dExample()=default; 138 | 139 | double ComputeFunction(const Eigen::Vector2d &x) override 140 | { 141 | double result = 0; 142 | double part_1 = x[0] * x[0] - x[1]; 143 | double part_2 = x[0] - 1; 144 | result = 100.0 * part_1 * part_1 + part_2 * part_2; 145 | return result; 146 | } 147 | 148 | Eigen::Vector2d ComputeJacobian(const Eigen::Vector2d &x) override 149 | { 150 | Eigen::Vector2d jacobian(x.rows()); 151 | jacobian(0, 0) = -2 * (1 - x[0]) - 400 * x[0] * ( x[1] - x[0] * x[0] ); 152 | jacobian(1, 0) = 200 * (x[1] - x[0] * x[0]); 153 | return jacobian; 154 | } 155 | }; 156 | -------------------------------------------------------------------------------- /project1/results/recorder.txt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ChenJiahao031008/Numerical_Optimization/b3e66365546fda3e44cd7935e022aea713f43ea2/project1/results/recorder.txt -------------------------------------------------------------------------------- /project1/run.sh: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env bash 2 | 3 | function build_project(){ 4 | mkdir -p build && cd build 5 | cmake .. 6 | make -j4 7 | cd ../ 8 | } 9 | 10 | function run_project(){ 11 | cd build/ 12 | ./AMao 13 | cd ../ 14 | } 15 | 16 | function test_project(){ 17 | cd build/unit_testing/ 18 | ./opt_sgd_test 19 | cd ../ 20 | } 21 | 22 | function clean_project(){ 23 | rm -rf log/ 24 | rm -rf build/* 25 | } 26 | 27 | 28 | function main() { 29 | local cmd="$1" 30 | shift 31 | case "${cmd}" in 32 | build) 33 | build_project 34 | ;; 35 | test) 36 | test_project 37 | ;; 38 | run) 39 | run_project 40 | ;; 41 | build_and_test) 42 | build_project && test_project 43 | ;; 44 | build_and_run) 45 | build_project && run_project 46 | ;; 47 | clean) 48 | clean_project 49 | ;; 50 | 51 | esac 52 | } 53 | 54 | main "$@" 55 | -------------------------------------------------------------------------------- /project1/unit_testing/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | project(unit_testing) 2 | 3 | # 输出可执行文件到指定目录 4 | # set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/../bin) 5 | 6 | add_executable(opt_sgd_test opt_sgd_test.cpp) 7 | target_compile_features(opt_sgd_test PRIVATE cxx_std_17) 8 | target_link_libraries(opt_sgd_test 9 | gtest gtest_main 10 | common 11 | unconstrained_optimization 12 | ${Boost_LIBRARIES} 13 | ) 14 | add_test(NAME opt_sgd_test COMMAND opt_sgd_test) 15 | -------------------------------------------------------------------------------- /project1/unit_testing/opt_sgd_test.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | #include 5 | #include "../common/logger.hpp" 6 | #include "../modules/base/cost_fuction.hpp" 7 | #include "../modules/unconstrained_optimization/steepest_gradient_descent.hpp" 8 | #include "../problem.hpp" 9 | 10 | TEST(TestUFreeOpt, TestSGDRosenbrock) 11 | { 12 | Eigen::VectorXd x(5); 13 | x.setZero(); 14 | Rosenbrock<5> rosenbrock(x); 15 | GradientDescent gd; 16 | gd.SetEpsilon(1e-6); 17 | gd.SetC(0.5); 18 | gd.SetTau(1.0); 19 | gd.SetMaxIters(10000); 20 | gd.SetVerbose(false); // 过程不打印 21 | auto res = gd.Solve(rosenbrock); 22 | Eigen::Matrix true_result; 23 | true_result.setOnes(); 24 | EXPECT_NEAR((res - true_result).norm(), 0, 1e-3); 25 | } 26 | 27 | TEST(TestUFreeOpt, TestSGDRosenbrock2d) 28 | { 29 | Eigen::Vector2d x(0.0, 0.0); 30 | Rosenbrock2dExample rosenbrock(x); 31 | GradientDescent gd; 32 | gd.SetEpsilon(1e-6); 33 | gd.SetC(0.5); 34 | gd.SetTau(1.0); 35 | gd.SetMaxIters(10000); 36 | gd.SetVerbose(false); // 过程不打印 37 | auto res = gd.Solve(rosenbrock); 38 | Eigen::Vector2d true_result = {1.0, 1.0}; 39 | EXPECT_NEAR ((res -true_result).norm(), 0, 1e-3); 40 | } 41 | 42 | TEST(TestUFreeOpt, TestSGDPolynomial) 43 | { 44 | Eigen::Vector2d x(0.0, 0.0); 45 | Example polynomial(x); 46 | GradientDescent gd; 47 | gd.SetEpsilon(1e-6); 48 | gd.SetC(0.5); 49 | gd.SetTau(1.0); 50 | gd.SetMaxIters(100); 51 | gd.SetVerbose(false); // 过程不打印 52 | auto res = gd.Solve(polynomial); 53 | Eigen::Vector2d true_result = {1.0, 1.0}; 54 | EXPECT_NEAR((res - true_result).norm(), 0, 1e-3); 55 | } 56 | 57 | -------------------------------------------------------------------------------- /project1/viewer/Figure_1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ChenJiahao031008/Numerical_Optimization/b3e66365546fda3e44cd7935e022aea713f43ea2/project1/viewer/Figure_1.png -------------------------------------------------------------------------------- /project1/viewer/plot_data.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import os 3 | import matplotlib as mlp 4 | import matplotlib.pyplot as plt 5 | import matplotlib.cm as cm 6 | import matplotlib.mlab as mlab 7 | import matplotlib.pyplot as plt 8 | ############################################## 9 | # 代码功能：读取的文件，保留数据进行绘图 10 | ############################################## 11 | 12 | ## 需要读取的文件，如果更改请修改此处 13 | file_name = "../results/recorder.txt" 14 | 15 | ## ________________数据生成________________ ## 16 | def function(x1, x2): 17 | z = (np.square(1 - x1) + 100 * np.square(x2 - np.square(x1))) 18 | return z 19 | 20 | ## ________________读取文件________________ ## 21 | 22 | x1 = [] 23 | x2 = [] 24 | f = [] 25 | 26 | file = open(file_name, 'r') 27 | for line in file.readlines(): 28 | line = line.strip('\n') # 除去换行 29 | line = line.split(' ') # 文件以“ ”分隔 30 | if "" in line: # 解决每行结尾有空格的问题 31 | line.remove("") 32 | x1.append(float(line[1])) 33 | x2.append(float(line[2])) 34 | f.append(float(line[3])) 35 | 36 | ## ________________绘制图片________________ ## 37 | num = min(len(x1), len(x2)) 38 | idx = [i for i in range(num)] 39 | 40 | points = np.arange(-0.1, 1.1, 0.01) 41 | xs, ys = np.meshgrid(points, points) 42 | z = function(xs, ys) 43 | 44 | plt.figure() 45 | plt.contourf(xs, ys, z, 50, alpha = 0.8, cmap="rainbow") 46 | plt.contour(xs, ys, z, 50, colors='black') 47 | plt.scatter(x1, x2, s=60, c="r", marker="x", alpha=1) 48 | plt.plot(x1, x2, color="black", linewidth=1, linestyle="-", label="iter", alpha=0.5) 49 | 50 | plt.show() 51 | -------------------------------------------------------------------------------- /project2/.catkin_workspace: -------------------------------------------------------------------------------- 1 | # This file currently only serves to mark the location of a catkin workspace for tool integration 2 | -------------------------------------------------------------------------------- /project2/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 | add_definitions(-w) 9 | 10 | find_package(Eigen3 REQUIRED) 11 | find_package(ompl REQUIRED) 12 | 13 | find_package(catkin REQUIRED COMPONENTS 14 | roscpp 15 | std_msgs 16 | geometry_msgs 17 | sensor_msgs 18 | visualization_msgs 19 | ) 20 | 21 | include_directories( 22 | ${catkin_INCLUDE_DIRS} 23 | ${OMPL_INCLUDE_DIRS} 24 | ${EIGEN3_INCLUDE_DIRS} 25 | include 26 | ) 27 | 28 | catkin_package() 29 | 30 | add_executable(curve_gen src/curve_gen.cpp) 31 | 32 | target_link_libraries(curve_gen 33 | ${OMPL_LIBRARIES} 34 | ${catkin_LIBRARIES} 35 | ) 36 | -------------------------------------------------------------------------------- /project2/gcopter/ReadMe.assets/image-20220731145839800.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ChenJiahao031008/Numerical_Optimization/b3e66365546fda3e44cd7935e022aea713f43ea2/project2/gcopter/ReadMe.assets/image-20220731145839800.png -------------------------------------------------------------------------------- /project2/gcopter/ReadMe.assets/image-20220731145955498.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ChenJiahao031008/Numerical_Optimization/b3e66365546fda3e44cd7935e022aea713f43ea2/project2/gcopter/ReadMe.assets/image-20220731145955498.png -------------------------------------------------------------------------------- /project2/gcopter/ReadMe.assets/image-20220731150208152.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ChenJiahao031008/Numerical_Optimization/b3e66365546fda3e44cd7935e022aea713f43ea2/project2/gcopter/ReadMe.assets/image-20220731150208152.png -------------------------------------------------------------------------------- /project2/gcopter/ReadMe.assets/image-20220731150823094.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ChenJiahao031008/Numerical_Optimization/b3e66365546fda3e44cd7935e022aea713f43ea2/project2/gcopter/ReadMe.assets/image-20220731150823094.png -------------------------------------------------------------------------------- /project2/gcopter/ReadMe.assets/image-20220731151006783.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ChenJiahao031008/Numerical_Optimization/b3e66365546fda3e44cd7935e022aea713f43ea2/project2/gcopter/ReadMe.assets/image-20220731151006783.png -------------------------------------------------------------------------------- /project2/gcopter/ReadMe.md: -------------------------------------------------------------------------------- 1 | ## Project 2: Smooth Navigation Path Generation 2 | 3 | From: https://github.com/ChenJiahao031008/Numerical_Optimization 4 | 5 | ### 1 Workflow 6 | 7 | 1. Cubic Spline: 8 | $$ 9 | \begin{aligned} 10 | \rho(s) &= a_i + b_is_i + c_is_i^2 + d_is_i^3,\ s_i\in(0,1)\\ 11 | \rho(s)^{'} &=b_i + 2c_is_i + 3d_is_i^2,\\ 12 | \rho(s)^{''} &=2c_i + 6d_is_i 13 | \end{aligned} 14 | $$ 15 | and natural spline is: $\rho(s_0)^{''} = \rho(s_N)^{''} = 0$. Ref: [Zhihu](https://zhuanlan.zhihu.com/p/62860859) 16 | 17 | 2. CostFunction: 18 | $$ 19 | \begin{aligned} 20 | \text{cost}&=\text{Energy}(x_1,...,x_{N-1}) + \text{Potential}(x_1,...,x_{N-1}) \\ 21 | &= \sum_{i=0}^{N}(4c_i^2 + 12c_id_i + 12d_i^2)\ 22 | +\ 1000\sum_{i=1}^{N-1}\sum_{j=1}^{M}\left(\max(r_j - \|x_i - o_i\|,0)\right) \\ 23 | \text{where},&\\ 24 | x_i & = [\bar x_i, \bar y_i]_{1\times2}\\ 25 | a_i &= x_i\\ 26 | b_i &= D_i\\ 27 | c_i &=-3(x_i - x_{i+1}) -2D_i - D_{i+1}\\ 28 | d_i &= 2(x_i - x_{i+1}) + D_i + D_{i+1}\\ 29 | \text{and, here:}&\\ 30 | &\begin{bmatrix} D_1\\ D_2\\ ...\\D_{N-1} \end{bmatrix}_{(N-1)\times 2} 31 | = \begin{bmatrix} 32 | 4 &1 \\ 1 & 4 & 1 \\& ...& ...& ...\\& &1 &4 &1 \\ &&&1 &4 33 | \end{bmatrix}^{-1}_{(N-1)\times(N-1)} 34 | \begin{bmatrix} 3(x_2-x_0)\\ 3(x_2-x_0)\\ ...\\3(x_N-x_{N-2}) \end{bmatrix}_{(N-1)\times 2} 35 | \end{aligned} 36 | $$ 37 | 38 | + **About Band Matrix**: lower and upper bandwidth are both 1. Ref: [Zhihu](https://zhuanlan.zhihu.com/p/400460201) 39 | 40 | + **Discussion: Why do we use cubic spline interpolation for two dimensions?** 41 | 42 | Although track optimization is time-independent, coordinates are an expression of time t in subsequent work. Therefore, two-dimensional optimization is employed for convenience. 43 | 44 | + **Core Code**: 45 | 46 | ```c++ 47 | inline void setInnerPoints(const Eigen::Ref &inPs) 48 | { 49 | A.reset(); 50 | b.setZero(); 51 | for (int i = 0; i < N - 1; ++i) 52 | { 53 | if (i == 0){ 54 | A(0, 0) = 4; A(0, 1) = 1; 55 | }else if (i == N - 2){ 56 | A(N - 2, N - 3) = 1; A(N - 2, N - 2) = 4; 57 | }else{ 58 | A(i, i - 1) = 1; A(i, i) = 4; A(i, i + 1) = 1; 59 | } 60 | } 61 | 62 | b.row(0) = 3 * (inPs.col(1).transpose() - headP.transpose()); 63 | for (int i = 1; i < N - 2; ++i){ 64 | b.row(i) = 3 * (inPs.col(i + 1).transpose() - inPs.col(i - 1).transpose()); 65 | } 66 | b.row(N - 2) = 3 * (tailP.transpose() - inPs.col(N - 3).transpose()); 67 | 68 | A.factorizeLU(); 69 | A.solve(b); 70 | 71 | ...... 72 | 73 | // 将b重新塑性为系数矩阵 74 | b.resize(4 * N, 2); 75 | b = coeff.transpose(); 76 | } 77 | 78 | inline void getStretchEnergy(double &energy) const 79 | { 80 | // An example for you to finish the other function 81 | energy = 0.0; 82 | for (int i = 0; i < N; ++i) 83 | { 84 | energy += 4.0 * b.row(4 * i + 2).squaredNorm() + 85 | 12.0 * b.row(4 * i + 2).dot(b.row(4 * i + 3)) + 86 | 12.0 * b.row(4 * i + 3).squaredNorm(); 87 | } 88 | } 89 | ``` 90 | 91 | 3. Gradient: 92 | 93 | + Energy Gradient 94 | 95 | + **Formula**: 96 | $$ 97 | \begin{aligned} 98 | \frac{\partial E}{\partial x}|_{x_1,..,x_{N-1}}&= 99 | \sum_{i=0}^{N-1}[ 100 | (8c_i + 12d_i)\frac{\partial c_i}{\partial x} 101 | + (12c_i + 24d_i)\frac{\partial d_i}{\partial x} ]\\ 102 | 103 | \text{where},&\\ 104 | \frac{\partial c_i}{\partial x}|_{x_1,..,x_{N-1}}&= 105 | -3\frac{\partial (x_i - x_{i+1})}{\partial x} 106 | -2\frac{\partial D_i}{\partial x} - \frac{\partial D_{i+1}}{\partial x}\\ 107 | 108 | \frac{\partial d_i}{\partial x}|_{x_1,..,x_{N-1}}&= 109 | 2\frac{\partial (x_i - x_{i+1})}{\partial x} 110 | +\frac{\partial D_i}{\partial x} + \frac{\partial D_{i+1}}{\partial x}\\ 111 | 112 | \frac{\partial}{\partial x}\begin{bmatrix} x_0 - x_1\\ 113 | x_1 -x_2 \\...\\ x_N - x_{N-1}\end{bmatrix}&= 114 | \begin{bmatrix} 115 | -1 \\ 1 & -1 \\& ...& ...\\& &1 &-1 \\ &&&1 &-1 116 | \end{bmatrix}_{(N-1)\times(N-1)} 117 | 118 | \end{aligned} 119 | \\ 120 | \frac{\partial D_i}{\partial x} 121 | = \begin{bmatrix} 122 | 4 &1 \\ 1 & 4 & 1 \\& ...& ...& ...\\& &1 &4 &1 \\ &&&1 &4 123 | \end{bmatrix}^{-1}_{(N-1)\times(N-1)} 124 | \begin{bmatrix} 125 | 0 &3 \\ -3 & 0 & 3 \\& ...& ...& ...\\& &-3 &0 &3 \\ &&&-3 &0 126 | \end{bmatrix}_{(N-1)\times(N-1)} 127 | $$ 128 | 129 | + **Core Code**: 130 | 131 | ```c++ 132 | inline void getGrad(Eigen::Ref gradByPoints) const 133 | { 134 | // * step1：获取 \partial_x(D), x is [x_1, ... , x_n-1] 135 | ...... 136 | 137 | // * step2：获取 \partial_x(x_i - x_{i+1}), x is [x_1, ... , x_n-1] 138 | ...... 139 | 140 | // * step3: 获取\partial_x(c)和\partial_x(d), x is [x_1, ... , x_n-1] 141 | Eigen::MatrixXd partial_c; partial_c.resize(N, N - 1); 142 | partial_c.setZero(); 143 | Eigen::MatrixXd partial_d; partial_d.resize(N, N - 1); 144 | partial_d.setZero(); 145 | 146 | partial_c.row(0) = -3 * partial_diff_x.row(0) - partial_D.row(0); 147 | partial_d.row(0) = 2 * partial_diff_x.row(0) + partial_D.row(0); 148 | for (size_t i = 1; i < N - 1; ++i) 149 | { 150 | partial_c.row(i) = -3 * partial_diff_x.row(i) - 2 * partial_D.row(i - 1) - partial_D.row(i); 151 | partial_d.row(i) = 2 * partial_diff_x.row(i) + partial_D.row(i - 1) + partial_D.row(i); 152 | } 153 | partial_c.row(N - 1) = -3 * partial_diff_x.row(N - 1) - 2 * partial_D.row(N - 2); 154 | partial_d.row(N - 1) = 2 * partial_diff_x.row(N - 1) + partial_D.row(N - 2); 155 | 156 | 157 | // * step4: 填入gradByPoints 158 | gradByPoints.setZero(); 159 | for (size_t i = 0; i < N; ++i) 160 | { 161 | Eigen::Vector2d c_i = coeff.col(4 * i + 2); 162 | Eigen::Vector2d d_i = coeff.col(4 * i + 3); 163 | // (2 * 1) x (1 * N - 1) = 2 x N - 1 164 | gradByPoints += (24 * d_i + 12 * c_i) * partial_d.row(i) + (12 * d_i + 8 * c_i) * partial_c.row(i); 165 | } 166 | } 167 | ``` 168 | 169 | + Potential Gradient 170 | 171 | + **Formula**: 172 | $$ 173 | \begin{aligned} 174 | \frac{\partial P}{\partial \bar x}|_{\bar x_1,..,\bar x_{N-1}} &= 175 | -1000\sum_{j=1}^M\frac{\bar x_i - a_j}{\sqrt{(\bar x_i- a_j)^2+(\bar y_i -b_j)^2}}\\ 176 | \frac{\partial P}{\partial \bar y}|_{\bar y_1,..,\bar y_{N-1}} &= 177 | -1000\sum_{j=1}^M\frac{\bar y_i - a_j}{\sqrt{(\bar x_i- a_j)^2+(\bar y_i -b_j)^2}} 178 | \end{aligned} 179 | $$ 180 | 181 | + **Core Code**: 182 | 183 | ```c++ 184 | static inline double costFunction(void *ptr, 185 | const Eigen::VectorXd &x, 186 | Eigen::VectorXd &g){ 187 | double obstacles = 0.0; 188 | Eigen::Matrix2Xd potential_grad; 189 | potential_grad.resize(2, points_nums); 190 | potential_grad.setZero(); 191 | for (int i = 0; i < points_nums; ++i){ 192 | for (int j = 0; j < instance->diskObstacles.cols(); ++j){ 193 | Eigen::Vector2d diff = inPs.col(i) - instance->diskObstacles.col(j).head(2); 194 | double distance = diff.norm(); 195 | double delta = instance->diskObstacles(2, j) - distance; 196 | 197 | if (delta > 0.0){ 198 | obstacles += instance->penaltyWeight * delta; 199 | potential_grad.col(i) += instance->penaltyWeight * ( -diff / distance); 200 | } 201 | } 202 | } 203 | } 204 | ``` 205 | 206 | 4. Optimization 207 | 208 | + **Theory**: 209 | 210 | image-20220731150823094 211 | 212 | + **Core Code**: 213 | 214 | ```c++ 215 | inline int line_search_lewisoverton(Eigen::VectorXd &x, 216 | double &f, 217 | Eigen::VectorXd &g, 218 | double &stp, 219 | const Eigen::VectorXd &d, 220 | const Eigen::VectorXd &xp, 221 | const Eigen::VectorXd &gp, 222 | const double stpmin, 223 | const double stpmax, 224 | const callback_data_t &cd, 225 | const lbfgs_parameter_t ¶m){ 226 | int iter_nums = 0; 227 | /* 对照补充：Check the input parameters for errors. */ 228 | bool touched = false; 229 | if (!(stp > 0.0)) 230 | return LBFGSERR_INVALIDPARAMETERS; 231 | 232 | double f_k0 = f; 233 | double f_k1 = f; 234 | auto S_alpha_right = param.f_dec_coeff * gp.dot(d); 235 | auto C_alpha_right = param.s_curv_coeff * gp.dot(d); 236 | 237 | double left = 0.0, right = stpmax; 238 | while (true) 239 | { 240 | x = xp + stp * d; 241 | f_k1 = cd.proc_evaluate(cd.instance, x, g); 242 | iter_nums++; 243 | 244 | /* 对照补充：Test for errors. */ 245 | if (std::isinf(f_k1) || std::isnan(f_k1)) 246 | return LBFGSERR_INVALID_FUNCVAL; 247 | 248 | // Armijo condition fails 249 | if (f_k0 - f_k1 < stp * S_alpha_right) right = stp; 250 | // weak wolfe condition fails 251 | else if (g.dot(d) < C_alpha_right) left = stp; 252 | // all success 253 | else{ 254 | f = f_k1; 255 | return iter_nums; 256 | } 257 | 258 | // 对照补充： —————————————————————— 259 | bool brackt = right < stpmax ? true : false; 260 | if (param.max_linesearch <= iter_nums) 261 | return LBFGSERR_MAXIMUMLINESEARCH; 262 | if (brackt && (right - left) < param.machine_prec * right) 263 | return LBFGSERR_WIDTHTOOSMALL; 264 | // 对照补充 end —————————————————————— 265 | 266 | if (right < stpmax) stp = 0.5 * (left + right); 267 | else stp *= 2.0; 268 | 269 | // 对照补充：—————————————————————— 270 | if (stp < stpmin) return LBFGSERR_MINIMUMSTEP; 271 | if (stp > stpmax){ 272 | if (touched) return LBFGSERR_MAXIMUMSTEP; 273 | else{ 274 | touched = true; 275 | stp = stpmax; 276 | } 277 | } 278 | // 对照补充 end —————————————————————— 279 | } 280 | } 281 | ``` 282 | 283 | ### 2 Results 284 | 285 | 1. Limited-BFGS for Rosenbrock Function 286 | 287 | ![image-20220731151006783](ReadMe.assets/image-20220731151006783.png) 288 | 289 | 2. trajectory 290 | 291 | image-20220731145839800image-20220731145955498image-20220731150208152 292 | 293 | -------------------------------------------------------------------------------- /project2/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 | -------------------------------------------------------------------------------- /project2/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 | -------------------------------------------------------------------------------- /project2/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 | -------------------------------------------------------------------------------- /project2/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 | -------------------------------------------------------------------------------- /project2/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 (size_t 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 (size_t 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 (size_t 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 (size_t 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 | -------------------------------------------------------------------------------- /project2/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 | -------------------------------------------------------------------------------- /project2/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 | -------------------------------------------------------------------------------- /project2/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 | quanti = (rV.col(i) / res).array(); 114 | if (filter.find(quanti) == filter.end()) 115 | { 116 | filter.insert(quanti); 117 | fV.col(offset) = rV.col(i); 118 | offset++; 119 | } 120 | } 121 | fV = fV.leftCols(offset).eval(); 122 | return; 123 | } 124 | 125 | // Each row of hPoly is defined by h0, h1, h2, h3 as 126 | // h0*x + h1*y + h2*z + h3 <= 0 127 | // proposed epsilon is 1.0e-6 128 | inline void enumerateVs(const Eigen::MatrixX4d &hPoly, 129 | const Eigen::Vector3d &inner, 130 | Eigen::Matrix3Xd &vPoly, 131 | const double epsilon = 1.0e-6) 132 | { 133 | const Eigen::VectorXd b = -hPoly.rightCols<1>() - hPoly.leftCols<3>() * inner; 134 | const Eigen::Matrix A = 135 | (hPoly.leftCols<3>().array().colwise() / b.array()).transpose(); 136 | 137 | quickhull::QuickHull qh; 138 | const double qhullEps = std::min(epsilon, quickhull::defaultEps()); 139 | // CCW is false because the normal in quickhull towards interior 140 | const auto cvxHull = qh.getConvexHull(A.data(), A.cols(), false, true, qhullEps); 141 | const auto &idBuffer = cvxHull.getIndexBuffer(); 142 | const int hNum = idBuffer.size() / 3; 143 | Eigen::Matrix3Xd rV(3, hNum); 144 | Eigen::Vector3d normal, point, edge0, edge1; 145 | for (int i = 0; i < hNum; i++) 146 | { 147 | point = A.col(idBuffer[3 * i + 1]); 148 | edge0 = point - A.col(idBuffer[3 * i]); 149 | edge1 = A.col(idBuffer[3 * i + 2]) - point; 150 | normal = edge0.cross(edge1); //cross in CW gives an outter normal 151 | rV.col(i) = normal / normal.dot(point); 152 | } 153 | filterVs(rV, epsilon, vPoly); 154 | vPoly = (vPoly.array().colwise() + inner.array()).eval(); 155 | return; 156 | } 157 | 158 | // Each row of hPoly is defined by h0, h1, h2, h3 as 159 | // h0*x + h1*y + h2*z + h3 <= 0 160 | // proposed epsilon is 1.0e-6 161 | inline bool enumerateVs(const Eigen::MatrixX4d &hPoly, 162 | Eigen::Matrix3Xd &vPoly, 163 | const double epsilon = 1.0e-6) 164 | { 165 | Eigen::Vector3d inner; 166 | if (findInterior(hPoly, inner)) 167 | { 168 | enumerateVs(hPoly, inner, vPoly, epsilon); 169 | return true; 170 | } 171 | else 172 | { 173 | return false; 174 | } 175 | } 176 | 177 | } // namespace geo_utils 178 | 179 | #endif 180 | -------------------------------------------------------------------------------- /project2/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 | -------------------------------------------------------------------------------- /project2/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 | -------------------------------------------------------------------------------- /project2/gcopter/include/gcopter/trajectory.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 TRAJECTORY_HPP 26 | #define TRAJECTORY_HPP 27 | 28 | #include "gcopter/root_finder.hpp" 29 | 30 | #include 31 | 32 | #include 33 | #include 34 | #include 35 | #include 36 | 37 | template 38 | class Piece 39 | { 40 | public: 41 | typedef Eigen::Matrix CoefficientMat; 42 | typedef Eigen::Matrix VelCoefficientMat; 43 | typedef Eigen::Matrix AccCoefficientMat; 44 | 45 | private: 46 | double duration; 47 | CoefficientMat coeffMat; 48 | 49 | public: 50 | Piece() = default; 51 | 52 | Piece(double dur, const CoefficientMat &cMat) 53 | : duration(dur), coeffMat(cMat) {} 54 | 55 | inline int getDim() const 56 | { 57 | return 3; 58 | } 59 | 60 | inline int getDegree() const 61 | { 62 | return D; 63 | } 64 | 65 | inline double getDuration() const 66 | { 67 | return duration; 68 | } 69 | 70 | inline const CoefficientMat &getCoeffMat() const 71 | { 72 | return coeffMat; 73 | } 74 | 75 | inline Eigen::Vector3d getPos(const double &t) const 76 | { 77 | Eigen::Vector3d pos(0.0, 0.0, 0.0); 78 | double tn = 1.0; 79 | for (int i = D; i >= 0; i--) 80 | { 81 | pos += tn * coeffMat.col(i); 82 | tn *= t; 83 | } 84 | return pos; 85 | } 86 | 87 | inline Eigen::Vector3d getVel(const double &t) const 88 | { 89 | Eigen::Vector3d vel(0.0, 0.0, 0.0); 90 | double tn = 1.0; 91 | int n = 1; 92 | for (int i = D - 1; i >= 0; i--) 93 | { 94 | vel += n * tn * coeffMat.col(i); 95 | tn *= t; 96 | n++; 97 | } 98 | return vel; 99 | } 100 | 101 | inline Eigen::Vector3d getAcc(const double &t) const 102 | { 103 | Eigen::Vector3d acc(0.0, 0.0, 0.0); 104 | double tn = 1.0; 105 | int m = 1; 106 | int n = 2; 107 | for (int i = D - 2; i >= 0; i--) 108 | { 109 | acc += m * n * tn * coeffMat.col(i); 110 | tn *= t; 111 | m++; 112 | n++; 113 | } 114 | return acc; 115 | } 116 | 117 | inline Eigen::Vector3d getJer(const double &t) const 118 | { 119 | Eigen::Vector3d jer(0.0, 0.0, 0.0); 120 | double tn = 1.0; 121 | int l = 1; 122 | int m = 2; 123 | int n = 3; 124 | for (int i = D - 3; i >= 0; i--) 125 | { 126 | jer += l * m * n * tn * coeffMat.col(i); 127 | tn *= t; 128 | l++; 129 | m++; 130 | n++; 131 | } 132 | return jer; 133 | } 134 | 135 | inline CoefficientMat normalizePosCoeffMat() const 136 | { 137 | CoefficientMat nPosCoeffsMat; 138 | double t = 1.0; 139 | for (int i = D; i >= 0; i--) 140 | { 141 | nPosCoeffsMat.col(i) = coeffMat.col(i) * t; 142 | t *= duration; 143 | } 144 | return nPosCoeffsMat; 145 | } 146 | 147 | inline VelCoefficientMat normalizeVelCoeffMat() const 148 | { 149 | VelCoefficientMat nVelCoeffMat; 150 | int n = 1; 151 | double t = duration; 152 | for (int i = D - 1; i >= 0; i--) 153 | { 154 | nVelCoeffMat.col(i) = n * coeffMat.col(i) * t; 155 | t *= duration; 156 | n++; 157 | } 158 | return nVelCoeffMat; 159 | } 160 | 161 | inline AccCoefficientMat normalizeAccCoeffMat() const 162 | { 163 | AccCoefficientMat nAccCoeffMat; 164 | int n = 2; 165 | int m = 1; 166 | double t = duration * duration; 167 | for (int i = D - 2; i >= 0; i--) 168 | { 169 | nAccCoeffMat.col(i) = n * m * coeffMat.col(i) * t; 170 | n++; 171 | m++; 172 | t *= duration; 173 | } 174 | return nAccCoeffMat; 175 | } 176 | 177 | inline double getMaxVelRate() const 178 | { 179 | VelCoefficientMat nVelCoeffMat = normalizeVelCoeffMat(); 180 | Eigen::VectorXd coeff = RootFinder::polySqr(nVelCoeffMat.row(0)) + 181 | RootFinder::polySqr(nVelCoeffMat.row(1)) + 182 | RootFinder::polySqr(nVelCoeffMat.row(2)); 183 | int N = coeff.size(); 184 | int n = N - 1; 185 | for (int i = 0; i < N; i++) 186 | { 187 | coeff(i) *= n; 188 | n--; 189 | } 190 | if (coeff.head(N - 1).squaredNorm() < DBL_EPSILON) 191 | { 192 | return getVel(0.0).norm(); 193 | } 194 | else 195 | { 196 | double l = -0.0625; 197 | double r = 1.0625; 198 | while (fabs(RootFinder::polyVal(coeff.head(N - 1), l)) < DBL_EPSILON) 199 | { 200 | l = 0.5 * l; 201 | } 202 | while (fabs(RootFinder::polyVal(coeff.head(N - 1), r)) < DBL_EPSILON) 203 | { 204 | r = 0.5 * (r + 1.0); 205 | } 206 | std::set candidates = RootFinder::solvePolynomial(coeff.head(N - 1), l, r, 207 | FLT_EPSILON / duration); 208 | candidates.insert(0.0); 209 | candidates.insert(1.0); 210 | double maxVelRateSqr = -INFINITY; 211 | double tempNormSqr; 212 | for (std::set::const_iterator it = candidates.begin(); 213 | it != candidates.end(); 214 | it++) 215 | { 216 | if (0.0 <= *it && 1.0 >= *it) 217 | { 218 | tempNormSqr = getVel((*it) * duration).squaredNorm(); 219 | maxVelRateSqr = maxVelRateSqr < tempNormSqr ? tempNormSqr : maxVelRateSqr; 220 | } 221 | } 222 | return sqrt(maxVelRateSqr); 223 | } 224 | } 225 | 226 | inline double getMaxAccRate() const 227 | { 228 | AccCoefficientMat nAccCoeffMat = normalizeAccCoeffMat(); 229 | Eigen::VectorXd coeff = RootFinder::polySqr(nAccCoeffMat.row(0)) + 230 | RootFinder::polySqr(nAccCoeffMat.row(1)) + 231 | RootFinder::polySqr(nAccCoeffMat.row(2)); 232 | int N = coeff.size(); 233 | int n = N - 1; 234 | for (int i = 0; i < N; i++) 235 | { 236 | coeff(i) *= n; 237 | n--; 238 | } 239 | if (coeff.head(N - 1).squaredNorm() < DBL_EPSILON) 240 | { 241 | return getAcc(0.0).norm(); 242 | } 243 | else 244 | { 245 | double l = -0.0625; 246 | double r = 1.0625; 247 | while (fabs(RootFinder::polyVal(coeff.head(N - 1), l)) < DBL_EPSILON) 248 | { 249 | l = 0.5 * l; 250 | } 251 | while (fabs(RootFinder::polyVal(coeff.head(N - 1), r)) < DBL_EPSILON) 252 | { 253 | r = 0.5 * (r + 1.0); 254 | } 255 | std::set candidates = RootFinder::solvePolynomial(coeff.head(N - 1), l, r, 256 | FLT_EPSILON / duration); 257 | candidates.insert(0.0); 258 | candidates.insert(1.0); 259 | double maxAccRateSqr = -INFINITY; 260 | double tempNormSqr; 261 | for (std::set::const_iterator it = candidates.begin(); 262 | it != candidates.end(); 263 | it++) 264 | { 265 | if (0.0 <= *it && 1.0 >= *it) 266 | { 267 | tempNormSqr = getAcc((*it) * duration).squaredNorm(); 268 | maxAccRateSqr = maxAccRateSqr < tempNormSqr ? tempNormSqr : maxAccRateSqr; 269 | } 270 | } 271 | return sqrt(maxAccRateSqr); 272 | } 273 | } 274 | 275 | inline bool checkMaxVelRate(const double &maxVelRate) const 276 | { 277 | double sqrMaxVelRate = maxVelRate * maxVelRate; 278 | if (getVel(0.0).squaredNorm() >= sqrMaxVelRate || 279 | getVel(duration).squaredNorm() >= sqrMaxVelRate) 280 | { 281 | return false; 282 | } 283 | else 284 | { 285 | VelCoefficientMat nVelCoeffMat = normalizeVelCoeffMat(); 286 | Eigen::VectorXd coeff = RootFinder::polySqr(nVelCoeffMat.row(0)) + 287 | RootFinder::polySqr(nVelCoeffMat.row(1)) + 288 | RootFinder::polySqr(nVelCoeffMat.row(2)); 289 | double t2 = duration * duration; 290 | coeff.tail<1>()(0) -= sqrMaxVelRate * t2; 291 | return RootFinder::countRoots(coeff, 0.0, 1.0) == 0; 292 | } 293 | } 294 | 295 | inline bool checkMaxAccRate(const double &maxAccRate) const 296 | { 297 | double sqrMaxAccRate = maxAccRate * maxAccRate; 298 | if (getAcc(0.0).squaredNorm() >= sqrMaxAccRate || 299 | getAcc(duration).squaredNorm() >= sqrMaxAccRate) 300 | { 301 | return false; 302 | } 303 | else 304 | { 305 | AccCoefficientMat nAccCoeffMat = normalizeAccCoeffMat(); 306 | Eigen::VectorXd coeff = RootFinder::polySqr(nAccCoeffMat.row(0)) + 307 | RootFinder::polySqr(nAccCoeffMat.row(1)) + 308 | RootFinder::polySqr(nAccCoeffMat.row(2)); 309 | double t2 = duration * duration; 310 | double t4 = t2 * t2; 311 | coeff.tail<1>()(0) -= sqrMaxAccRate * t4; 312 | return RootFinder::countRoots(coeff, 0.0, 1.0) == 0; 313 | } 314 | } 315 | }; 316 | 317 | template 318 | class Trajectory 319 | { 320 | private: 321 | typedef std::vector> Pieces; 322 | Pieces pieces; 323 | 324 | public: 325 | Trajectory() = default; 326 | 327 | Trajectory(const std::vector &durs, 328 | const std::vector::CoefficientMat> &cMats) 329 | { 330 | int N = std::min(durs.size(), cMats.size()); 331 | pieces.reserve(N); 332 | for (int i = 0; i < N; i++) 333 | { 334 | pieces.emplace_back(durs[i], cMats[i]); 335 | } 336 | } 337 | 338 | inline int getPieceNum() const 339 | { 340 | return pieces.size(); 341 | } 342 | 343 | inline Eigen::VectorXd getDurations() const 344 | { 345 | int N = getPieceNum(); 346 | Eigen::VectorXd durations(N); 347 | for (int i = 0; i < N; i++) 348 | { 349 | durations(i) = pieces[i].getDuration(); 350 | } 351 | return durations; 352 | } 353 | 354 | inline double getTotalDuration() const 355 | { 356 | int N = getPieceNum(); 357 | double totalDuration = 0.0; 358 | for (int i = 0; i < N; i++) 359 | { 360 | totalDuration += pieces[i].getDuration(); 361 | } 362 | return totalDuration; 363 | } 364 | 365 | inline Eigen::Matrix3Xd getPositions() const 366 | { 367 | int N = getPieceNum(); 368 | Eigen::Matrix3Xd positions(3, N + 1); 369 | for (int i = 0; i < N; i++) 370 | { 371 | positions.col(i) = pieces[i].getCoeffMat().col(D); 372 | } 373 | positions.col(N) = pieces[N - 1].getPos(pieces[N - 1].getDuration()); 374 | return positions; 375 | } 376 | 377 | inline const Piece &operator[](int i) const 378 | { 379 | return pieces[i]; 380 | } 381 | 382 | inline Piece &operator[](int i) 383 | { 384 | return pieces[i]; 385 | } 386 | 387 | inline void clear(void) 388 | { 389 | pieces.clear(); 390 | return; 391 | } 392 | 393 | inline typename Pieces::const_iterator begin() const 394 | { 395 | return pieces.begin(); 396 | } 397 | 398 | inline typename Pieces::const_iterator end() const 399 | { 400 | return pieces.end(); 401 | } 402 | 403 | inline typename Pieces::iterator begin() 404 | { 405 | return pieces.begin(); 406 | } 407 | 408 | inline typename Pieces::iterator end() 409 | { 410 | return pieces.end(); 411 | } 412 | 413 | inline void reserve(const int &n) 414 | { 415 | pieces.reserve(n); 416 | return; 417 | } 418 | 419 | inline void emplace_back(const Piece &piece) 420 | { 421 | pieces.emplace_back(piece); 422 | return; 423 | } 424 | 425 | inline void emplace_back(const double &dur, 426 | const typename Piece::CoefficientMat &cMat) 427 | { 428 | pieces.emplace_back(dur, cMat); 429 | return; 430 | } 431 | 432 | inline void append(const Trajectory &traj) 433 | { 434 | pieces.insert(pieces.end(), traj.begin(), traj.end()); 435 | return; 436 | } 437 | 438 | inline int locatePieceIdx(double &t) const 439 | { 440 | int N = getPieceNum(); 441 | int idx; 442 | double dur; 443 | for (idx = 0; 444 | idx < N && 445 | t > (dur = pieces[idx].getDuration()); 446 | idx++) 447 | { 448 | t -= dur; 449 | } 450 | if (idx == N) 451 | { 452 | idx--; 453 | t += pieces[idx].getDuration(); 454 | } 455 | return idx; 456 | } 457 | 458 | inline Eigen::Vector3d getPos(double t) const 459 | { 460 | int pieceIdx = locatePieceIdx(t); 461 | return pieces[pieceIdx].getPos(t); 462 | } 463 | 464 | inline Eigen::Vector3d getVel(double t) const 465 | { 466 | int pieceIdx = locatePieceIdx(t); 467 | return pieces[pieceIdx].getVel(t); 468 | } 469 | 470 | inline Eigen::Vector3d getAcc(double t) const 471 | { 472 | int pieceIdx = locatePieceIdx(t); 473 | return pieces[pieceIdx].getAcc(t); 474 | } 475 | 476 | inline Eigen::Vector3d getJer(double t) const 477 | { 478 | int pieceIdx = locatePieceIdx(t); 479 | return pieces[pieceIdx].getJer(t); 480 | } 481 | 482 | inline Eigen::Vector3d getJuncPos(int juncIdx) const 483 | { 484 | if (juncIdx != getPieceNum()) 485 | { 486 | return pieces[juncIdx].getCoeffMat().col(D); 487 | } 488 | else 489 | { 490 | return pieces[juncIdx - 1].getPos(pieces[juncIdx - 1].getDuration()); 491 | } 492 | } 493 | 494 | inline Eigen::Vector3d getJuncVel(int juncIdx) const 495 | { 496 | if (juncIdx != getPieceNum()) 497 | { 498 | return pieces[juncIdx].getCoeffMat().col(D - 1); 499 | } 500 | else 501 | { 502 | return pieces[juncIdx - 1].getVel(pieces[juncIdx - 1].getDuration()); 503 | } 504 | } 505 | 506 | inline Eigen::Vector3d getJuncAcc(int juncIdx) const 507 | { 508 | if (juncIdx != getPieceNum()) 509 | { 510 | return pieces[juncIdx].getCoeffMat().col(D - 2) * 2.0; 511 | } 512 | else 513 | { 514 | return pieces[juncIdx - 1].getAcc(pieces[juncIdx - 1].getDuration()); 515 | } 516 | } 517 | 518 | inline double getMaxVelRate() const 519 | { 520 | int N = getPieceNum(); 521 | double maxVelRate = -INFINITY; 522 | double tempNorm; 523 | for (int i = 0; i < N; i++) 524 | { 525 | tempNorm = pieces[i].getMaxVelRate(); 526 | maxVelRate = maxVelRate < tempNorm ? tempNorm : maxVelRate; 527 | } 528 | return maxVelRate; 529 | } 530 | 531 | inline double getMaxAccRate() const 532 | { 533 | int N = getPieceNum(); 534 | double maxAccRate = -INFINITY; 535 | double tempNorm; 536 | for (int i = 0; i < N; i++) 537 | { 538 | tempNorm = pieces[i].getMaxAccRate(); 539 | maxAccRate = maxAccRate < tempNorm ? tempNorm : maxAccRate; 540 | } 541 | return maxAccRate; 542 | } 543 | 544 | inline bool checkMaxVelRate(const double &maxVelRate) const 545 | { 546 | int N = getPieceNum(); 547 | bool feasible = true; 548 | for (int i = 0; i < N && feasible; i++) 549 | { 550 | feasible = feasible && pieces[i].checkMaxVelRate(maxVelRate); 551 | } 552 | return feasible; 553 | } 554 | 555 | inline bool checkMaxAccRate(const double &maxAccRate) const 556 | { 557 | int N = getPieceNum(); 558 | bool feasible = true; 559 | for (int i = 0; i < N && feasible; i++) 560 | { 561 | feasible = feasible && pieces[i].checkMaxAccRate(maxAccRate); 562 | } 563 | return feasible; 564 | } 565 | }; 566 | 567 | #endif -------------------------------------------------------------------------------- /project2/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 | -------------------------------------------------------------------------------- /project2/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 | -------------------------------------------------------------------------------- /project2/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 -------------------------------------------------------------------------------- /project2/gcopter/launch/curve_gen.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | -------------------------------------------------------------------------------- /project2/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 | -------------------------------------------------------------------------------- /project2/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 | -------------------------------------------------------------------------------- /project2/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 | -------------------------------------------------------------------------------- /project2/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 | --------------------------------------------------------------------------------