├── .catkin_workspace ├── .vscode ├── c_cpp_properties.json └── settings.json ├── LICENSE ├── ReadMe.md ├── results ├── v2_01.png ├── v2_02.png ├── v2_03.png ├── v2_04.png ├── v2_05.png ├── v2_06.png ├── v2_07.png ├── v2_08.png ├── v2_09.png ├── v2_10.png ├── v2_11.png ├── v2_12.png ├── v2_13.png ├── v2_14.png ├── v2_15.png ├── v2_16.png ├── v2_17.png ├── v2_18.png ├── v2_19.png └── v2_19 │ ├── dtt1_a.txt │ ├── dtt1_b.txt │ ├── dtt1_c.txt │ ├── dtt1_d.txt │ ├── dtt1_q.txt │ ├── dtt1_qa.txt │ ├── dtt1_qv.txt │ ├── dtt1_s.txt │ ├── process.m │ └── processed.svg ├── src └── sl_hw5 │ ├── CMakeLists.txt │ ├── include │ ├── lbfgs │ │ ├── lbfgs.hpp │ │ ├── lbfgs_bak.hpp │ │ └── lbfgs_raw.hpp │ ├── ros1_disp │ │ ├── rviz_dis.hpp │ │ ├── scene_dis.hpp │ │ ├── spline_opt_dis.hpp │ │ └── topp_dis.hpp │ ├── scene │ │ ├── pavel_valtr.hpp │ │ ├── scene_astar.hpp │ │ ├── scene_base.hpp │ │ ├── scene_obstacles.hpp │ │ └── scene_obstacles_new.hpp │ ├── sdqp │ │ ├── sdqp.hpp │ │ └── sdqp_bak.hpp │ └── traj_optim │ │ ├── conic_alm_topp.hpp │ │ └── spline_opt.hpp │ ├── launch │ ├── main.launch │ ├── rviz_config │ │ └── rviz_test0.rviz │ └── test0.launch │ ├── package.xml │ └── src │ ├── main.cpp │ └── test0.cpp └── tkalpha-第5次作业.pdf /.catkin_workspace: -------------------------------------------------------------------------------- 1 | # This file currently only serves to mark the location of a catkin workspace for tool integration 2 | -------------------------------------------------------------------------------- /.vscode/c_cpp_properties.json: -------------------------------------------------------------------------------- 1 | { 2 | "configurations": [ 3 | { 4 | "name": "Linux", 5 | "includePath": [ 6 | "${workspaceFolder}/**", 7 | "/usr/include/**", 8 | "/opt/ros/noetic/include/**", 9 | "/opt/ros/noetic/include", 10 | "${workspaceFolder}/src/sl_hw5/include" 11 | ], 12 | "defines": [], 13 | "compilerPath": "/usr/bin/gcc", 14 | "cStandard": "gnu17", 15 | "cppStandard": "gnu++17", 16 | "intelliSenseMode": "linux-gcc-x64", 17 | "configurationProvider": "ms-vscode.cmake-tools" 18 | } 19 | ], 20 | "version": 4 21 | } -------------------------------------------------------------------------------- /.vscode/settings.json: -------------------------------------------------------------------------------- 1 | { 2 | "files.associations": { 3 | "geometry": "cpp", 4 | "random": "cpp", 5 | "cmath": "cpp", 6 | "cctype": "cpp", 7 | "clocale": "cpp", 8 | "cstdarg": "cpp", 9 | "cstddef": "cpp", 10 | "cstdio": "cpp", 11 | "cstdlib": "cpp", 12 | "cstring": "cpp", 13 | "ctime": "cpp", 14 | "cwchar": "cpp", 15 | "cwctype": "cpp", 16 | "array": "cpp", 17 | "atomic": "cpp", 18 | "strstream": "cpp", 19 | "*.tcc": "cpp", 20 | "bitset": "cpp", 21 | "chrono": "cpp", 22 | "complex": "cpp", 23 | "condition_variable": "cpp", 24 | "cstdint": "cpp", 25 | "deque": "cpp", 26 | "list": "cpp", 27 | "unordered_map": "cpp", 28 | "unordered_set": "cpp", 29 | "vector": "cpp", 30 | "exception": "cpp", 31 | "algorithm": "cpp", 32 | "functional": "cpp", 33 | "iterator": "cpp", 34 | "map": "cpp", 35 | "memory": "cpp", 36 | "memory_resource": "cpp", 37 | "numeric": "cpp", 38 | "optional": "cpp", 39 | "ratio": "cpp", 40 | "set": "cpp", 41 | "string": "cpp", 42 | "string_view": "cpp", 43 | "system_error": "cpp", 44 | "tuple": "cpp", 45 | "type_traits": "cpp", 46 | "utility": "cpp", 47 | "hash_map": "cpp", 48 | "hash_set": "cpp", 49 | "fstream": "cpp", 50 | "initializer_list": "cpp", 51 | "iomanip": "cpp", 52 | "iosfwd": "cpp", 53 | "iostream": "cpp", 54 | "istream": "cpp", 55 | "limits": "cpp", 56 | "mutex": "cpp", 57 | "new": "cpp", 58 | "ostream": "cpp", 59 | "shared_mutex": "cpp", 60 | "sstream": "cpp", 61 | "stdexcept": "cpp", 62 | "streambuf": "cpp", 63 | "thread": "cpp", 64 | "cfenv": "cpp", 65 | "cinttypes": "cpp", 66 | "typeindex": "cpp", 67 | "typeinfo": "cpp", 68 | "variant": "cpp", 69 | "bit": "cpp", 70 | "dense": "cpp", 71 | "*.cuh": "cpp", 72 | "csignal": "cpp", 73 | "core": "cpp", 74 | "*.txx": "cpp", 75 | "valarray": "cpp", 76 | "*.ipp": "cpp", 77 | "sparsecholesky": "cpp", 78 | "sparsecore": "cpp", 79 | "sparselu": "cpp", 80 | "queue": "cpp", 81 | "eigen": "cpp", 82 | "forward_list": "cpp" 83 | }, 84 | "python.autoComplete.extraPaths": [ 85 | "/opt/ros/noetic/lib/python3/dist-packages" 86 | ], 87 | "editor.formatOnSave": false, 88 | "C_Cpp.clang_format_style": "{ BasedOnStyle: Google, IndentWidth: 2, ColumnLimit: 0}", 89 | "python.analysis.extraPaths": [ 90 | "/opt/ros/noetic/lib/python3/dist-packages" 91 | ] 92 | } -------------------------------------------------------------------------------- /ReadMe.md: -------------------------------------------------------------------------------- 1 | # Numerical Optimization in Robotics - Homework5 2 | ### by tkalpha, 2022.08.31 V0.2.1 3 | 4 | Course Link ： [深蓝学院_机器人中的数值优化](https://www.shenlanxueyuan.com/course/573) 5 | 6 | ## 作业目标以及完成思路 7 | 1. 点离凸多边形的最短距离求解 8 | - 采用Pavel Valtr算法生成随机的凸多边形。 9 | - 采用随机采样方法生成场景中的凸多边形与圆形障碍物。 10 | - 在进行最短距离与距离梯度计算前判断点是否在多边形内。 11 | - 如果点在凸多边形内，直接取点到N边直线的最短距离即可。 12 | - 如果点在凸多边形外，构建二次规划问题，然后使用SDQP算法求解最短距离点。 13 | 2. 无碰撞的轨迹优化生成 14 | - 在场景中构建搜索栅格，然后使用Astar算法搜索初始最短可行轨迹 15 | - 在初始轨迹上等距离取点得到三次样条曲线的中间点，进而将初始轨迹拟合为三次样条曲线 16 | - 将无碰撞平滑轨迹生成任务构建无约束优化问题 17 | - 路径的平滑优化目标取三次样条曲线的Stretch Energy 18 | - 路径的无碰撞目标为路径点在场景中离障碍物的最近距离的指数函数 19 | - 采用L-BFGS算法求解无约束的优化问题 20 | 3. 构建SOCP形式的TOPP问题并进行求解 21 | - 按照课堂讲解内容构建SOCP形式的TOPP问题。 22 | - 设计优化问题目标函数及其导数的计算方式，其中对二阶锥约束，等式约束与不等式约束分别进行了增广拉格朗日项的设计。 23 | - 自行设计conic PHR-ALM算法流程求解优化目标。 24 | - 采用L-BFGS算法作为内层优化器求解conic PHR-ALM算法中的无约束优化问题。 25 | 26 | ## 作业程序的运行 27 | ### 编程环境 28 | Ubuntu MATE 20.04.4 LTS / GCC 9.4.0 / C17标准 / CMake 3.16.3 / ROS Noetic / Vscode 1.70.0 / Eigen 3.4.0 29 | ### 作业工程的编译与运行方式 30 | 31 | ``` 32 | catkin_make 33 | source devel/setup.sh 34 | roslaunch sl_hw5 main.launch 35 | ``` 36 | ## 效果展示 37 | 1. 图中的白色格点为Astar算法的搜索空间。 38 | 2. 淡绿色的实心多边形为随机生成的多边形或者圆形障碍物。两个正绿色圆点为随机生成的路径起始点与终止点。 39 | 3. 深绿色路径为Astar算法搜索得到的初始路径。 40 | 4. 从红色到黄色渐变的曲线为经过L-BFGS无约束优化与conic ALM TOPP问题求解以后得到的最终路径。曲线颜色越接近浅黄色代表此时轨迹的速度越高；曲线的颜色越接近深红色代表此时的速度越低。 41 | 5. 可以发现L-BFGS无约束优化方法生成的路径无碰撞且较为平滑，采用新的势场目标函数以后生成路径不再紧贴障碍物而是会在条件允许的情况下与障碍物保持一定的距离。 42 | 43 | 以下仅给出部分程序运行结果作为示例。附件提供了18份不同的程序运行结果，详见作业压缩包的results文件夹。 44 | 例1 45 | ![](results/v2_18.png) 46 | 例2 47 | ![](results/v2_13.png) 48 | 例3 49 | ![](results/v2_03.png) 50 | 例4 51 | ![](results/v2_19.png) 52 | 进一步绘制例4的线速度、线加速度曲线以及速度约束的曲线如下图所示。可以发现TOPP规划的结果符合速度约束要求：在轨迹曲率较大的地方速度较低而在轨迹曲率较小的地方速度较高。具体计算分析过程详见 results/v2_19/process.m 53 | ![](results/v2_19/processed.svg) 54 | 因此可以认为作业程序实现了期望的功能。 55 | 56 | ## sl_hw5包中的主要程序文件简介 57 | - include 文件夹 58 | - lbfgs 文件夹 59 | - lbfgs.hpp 作业2中使用的基于Eigen的L-BFGS算法程序 60 | - ros1_disp 文件夹 61 | - rviz_dis.hpp 对rviz1显示功能的封装，提供障碍物与路径曲线绘制等基本功能 62 | - scene_dis.hpp 运动规划场景以及Astar图搜索结果的绘制显示 63 | - spline_opt_dis.hpp 三次样条曲线插值与无约束优化结果的显示 64 | - topp_dis 最终生成轨迹的显示，以及规划结果的文件记录 65 | - scene文件夹 66 | - pavel_valtr.hpp Pavel Valtr算法，用于在给定参数下随机生成多边形 67 | - scene_astar.hpp 运动规划场景地图构建，障碍物与起止点随机生成，Astar初始路径规划 68 | - scene_base.hpp 提供scene_astar.hpp中sceneAstar类的基类，是对人工距离势场规划问题的抽象，用于后续功能拓展 69 | - scene_obstacles.hpp 提供不同种类障碍物的生成方法。提供点是否处于障碍物内的判断方法以及点到不同种类障碍物最近距离的计算方法。 70 | - sdqp文件夹 71 | - sdqp.hpp Low-dimentional QP问题求解算法，曾用于作业3与作业4 72 | - traj_optim 文件夹 73 | - conic_alm_topp.hpp TOPP问题的conic PHR-ALM求解算法，相对作业4的通用conicALM算法采用了不同的内层优化方法，并针对TOPP问题进行了优化简化了计算 74 | - spline_opt.hpp 对初始规划轨迹的三次样条曲线插值方法。对三次样条曲线基于障碍物距离场的无约束优化方法 75 | - launch 文件夹 76 | - rviz_config 文件夹，包含工程采用的rviz配置文件 77 | - main.launch 工程主项目的ROS launch配置文件 78 | - src 文件夹 79 | - main.cpp 主程序运行入口，提供以上所有方法的运行示例 80 | -------------------------------------------------------------------------------- /results/v2_01.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tk166/Shenlan_Numerical-Optimization_in_Robotics/89129bd22531a55a481c60c3aa8da2a7fc51a1c0/results/v2_01.png -------------------------------------------------------------------------------- /results/v2_02.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tk166/Shenlan_Numerical-Optimization_in_Robotics/89129bd22531a55a481c60c3aa8da2a7fc51a1c0/results/v2_02.png -------------------------------------------------------------------------------- /results/v2_03.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tk166/Shenlan_Numerical-Optimization_in_Robotics/89129bd22531a55a481c60c3aa8da2a7fc51a1c0/results/v2_03.png -------------------------------------------------------------------------------- /results/v2_04.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tk166/Shenlan_Numerical-Optimization_in_Robotics/89129bd22531a55a481c60c3aa8da2a7fc51a1c0/results/v2_04.png -------------------------------------------------------------------------------- /results/v2_05.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tk166/Shenlan_Numerical-Optimization_in_Robotics/89129bd22531a55a481c60c3aa8da2a7fc51a1c0/results/v2_05.png -------------------------------------------------------------------------------- /results/v2_06.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tk166/Shenlan_Numerical-Optimization_in_Robotics/89129bd22531a55a481c60c3aa8da2a7fc51a1c0/results/v2_06.png -------------------------------------------------------------------------------- /results/v2_07.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tk166/Shenlan_Numerical-Optimization_in_Robotics/89129bd22531a55a481c60c3aa8da2a7fc51a1c0/results/v2_07.png -------------------------------------------------------------------------------- /results/v2_08.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tk166/Shenlan_Numerical-Optimization_in_Robotics/89129bd22531a55a481c60c3aa8da2a7fc51a1c0/results/v2_08.png -------------------------------------------------------------------------------- /results/v2_09.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tk166/Shenlan_Numerical-Optimization_in_Robotics/89129bd22531a55a481c60c3aa8da2a7fc51a1c0/results/v2_09.png -------------------------------------------------------------------------------- /results/v2_10.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tk166/Shenlan_Numerical-Optimization_in_Robotics/89129bd22531a55a481c60c3aa8da2a7fc51a1c0/results/v2_10.png -------------------------------------------------------------------------------- /results/v2_11.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tk166/Shenlan_Numerical-Optimization_in_Robotics/89129bd22531a55a481c60c3aa8da2a7fc51a1c0/results/v2_11.png -------------------------------------------------------------------------------- /results/v2_12.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tk166/Shenlan_Numerical-Optimization_in_Robotics/89129bd22531a55a481c60c3aa8da2a7fc51a1c0/results/v2_12.png -------------------------------------------------------------------------------- /results/v2_13.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tk166/Shenlan_Numerical-Optimization_in_Robotics/89129bd22531a55a481c60c3aa8da2a7fc51a1c0/results/v2_13.png -------------------------------------------------------------------------------- /results/v2_14.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tk166/Shenlan_Numerical-Optimization_in_Robotics/89129bd22531a55a481c60c3aa8da2a7fc51a1c0/results/v2_14.png -------------------------------------------------------------------------------- /results/v2_15.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tk166/Shenlan_Numerical-Optimization_in_Robotics/89129bd22531a55a481c60c3aa8da2a7fc51a1c0/results/v2_15.png -------------------------------------------------------------------------------- /results/v2_16.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tk166/Shenlan_Numerical-Optimization_in_Robotics/89129bd22531a55a481c60c3aa8da2a7fc51a1c0/results/v2_16.png -------------------------------------------------------------------------------- /results/v2_17.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tk166/Shenlan_Numerical-Optimization_in_Robotics/89129bd22531a55a481c60c3aa8da2a7fc51a1c0/results/v2_17.png -------------------------------------------------------------------------------- /results/v2_18.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tk166/Shenlan_Numerical-Optimization_in_Robotics/89129bd22531a55a481c60c3aa8da2a7fc51a1c0/results/v2_18.png -------------------------------------------------------------------------------- /results/v2_19.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tk166/Shenlan_Numerical-Optimization_in_Robotics/89129bd22531a55a481c60c3aa8da2a7fc51a1c0/results/v2_19.png -------------------------------------------------------------------------------- /results/v2_19/dtt1_a.txt: -------------------------------------------------------------------------------- 1 | 1.05232 2 | 1.04783 3 | 1.04201 4 | 1.03592 5 | 1.02875 6 | 1.02056 7 | 1.01201 8 | 1.00337 9 | 0.994572 10 | 0.985978 11 | 0.979161 12 | 0.974317 13 | 0.970685 14 | 0.968683 15 | 0.969277 16 | 0.972594 17 | 0.978956 18 | 0.988086 19 | 0.999563 20 | 1.00961 21 | 1.01707 22 | 1.02184 23 | 1.02493 24 | 1.02704 25 | 1.02781 26 | 1.02729 27 | 1.02662 28 | 1.02613 29 | 1.02404 30 | 1.02014 31 | 1.01659 32 | 1.0136 33 | 1.00884 34 | 1.0021 35 | 0.995967 36 | 0.990744 37 | 0.985633 38 | 0.980586 39 | 0.97479 40 | 0.9698 41 | 0.861123 42 | 0.832893 43 | 0.829779 44 | 0.834484 45 | 0.845708 46 | 0.865411 47 | 0.881049 48 | 0.901077 49 | 0.884859 50 | 0.651831 51 | -0.981931 52 | -0.997731 53 | -0.994637 54 | -0.989252 55 | -0.98144 56 | -0.969985 57 | -0.95531 58 | -0.937911 59 | -0.917263 60 | -0.893328 61 | -0.868405 62 | -0.843371 63 | -0.686863 64 | -0.416945 65 | -0.183783 66 | 0.0258902 67 | 0.109282 68 | 0.115465 69 | 0.131448 70 | 0.14987 71 | 0.160078 72 | 0.169701 73 | 0.227204 74 | 0.320249 75 | 0.377157 76 | 0.418507 77 | 0.472781 78 | 0.540091 79 | 0.608607 80 | 0.680125 81 | 0.741597 82 | 0.792005 83 | 0.809976 84 | 0.797553 85 | 0.783756 86 | 0.769067 87 | 0.765105 88 | 0.771768 89 | 0.788434 90 | 0.816168 91 | 0.852362 92 | 0.732926 93 | -0.727302 94 | -0.880132 95 | -0.932943 96 | -0.98162 97 | -1.04539 98 | -1.11857 99 | -1.19511 100 | -1.27319 101 | -1.35131 102 | -1.31778 103 | -1.21681 104 | -1.16655 105 | -1.12981 106 | -1.10731 107 | -1.0859 108 | -1.0643 109 | -1.03192 110 | -0.989951 111 | -0.950527 112 | -0.911458 113 | -0.868408 114 | -0.823673 115 | -0.765618 116 | -0.69166 117 | -0.610484 118 | -0.520669 119 | -0.431156 120 | -0.338413 121 | -0.246857 122 | -0.156218 123 | -0.0686391 124 | 0.0205247 125 | 0.133842 126 | 0.287291 127 | 0.464907 128 | 0.673847 129 | 0.808783 130 | 0.838953 131 | 0.867686 132 | 0.894237 133 | 0.917274 134 | 0.937208 135 | 0.955318 136 | 0.97092 137 | 0.983531 138 | 0.993149 139 | 0.999945 140 | 1.00448 141 | 1.00733 142 | 1.00907 143 | 1.0092 144 | 1.00831 145 | 1.00645 146 | 1.00368 147 | 1.00078 148 | 0.998141 149 | 0.99579 150 | 0.993705 151 | 0.991381 152 | 0.989523 153 | 0.989004 154 | 0.990226 155 | 0.992496 156 | -0.719272 157 | -0.202883 158 | -0.380071 159 | -0.944352 160 | -0.973439 161 | -0.962058 162 | -0.948452 163 | -0.934333 164 | -0.920955 165 | -0.905812 166 | -0.888458 167 | -0.870188 168 | -0.85092 169 | -0.781396 170 | -0.656595 171 | -0.461834 172 | -0.279619 173 | -0.774601 174 | -0.783677 175 | -0.788437 176 | -0.798983 177 | -0.813795 178 | -0.831859 179 | -0.850934 180 | -0.870831 181 | -0.891379 182 | -0.912221 183 | -0.932761 184 | -0.95273 185 | -0.976591 186 | -1.00278 187 | -1.03226 188 | -1.06259 189 | -1.10896 190 | -1.15903 191 | -1.26058 192 | -0.819368 193 | -0.988241 194 | -1.14669 195 | -1.07731 196 | -1.03699 197 | -1.01679 198 | -1.00678 199 | -1.00204 200 | -1.00023 201 | -------------------------------------------------------------------------------- /results/v2_19/dtt1_b.txt: -------------------------------------------------------------------------------- 1 | 0.000192028 2 | 0.029702 3 | 0.116079 4 | 0.256074 5 | 0.446809 6 | 0.685362 7 | 0.968915 8 | 1.29488 9 | 1.66095 10 | 2.06488 11 | 2.50455 12 | 2.9788 13 | 3.48719 14 | 4.02922 15 | 4.60466 16 | 5.21411 17 | 5.85856 18 | 6.53948 19 | 7.25827 20 | 8.01628 21 | 8.8121 22 | 9.64311 23 | 10.5062 24 | 11.3987 25 | 12.3188 26 | 13.2637 27 | 14.231 28 | 15.2189 29 | 16.2263 30 | 17.2501 31 | 18.2869 32 | 19.3356 33 | 20.3953 34 | 21.4629 35 | 22.5352 36 | 23.6118 37 | 24.6926 38 | 25.7768 39 | 26.8634 40 | 27.9508 41 | 29.0388 42 | 30.0095 43 | 30.9523 44 | 31.8948 45 | 32.8451 46 | 33.8099 47 | 34.7982 48 | 35.8046 49 | 36.8332 50 | 37.8416 51 | 38.5825 52 | 37.4702 53 | 36.3451 54 | 35.2297 55 | 34.1274 56 | 33.0419 57 | 31.9774 58 | 30.938 59 | 29.9264 60 | 28.946 61 | 28.0001 62 | 27.0894 63 | 26.2137 64 | 25.5077 65 | 25.0833 66 | 24.8979 67 | 24.9238 68 | 25.032 69 | 25.1454 70 | 25.2732 71 | 25.4177 72 | 25.5705 73 | 25.7311 74 | 25.9443 75 | 26.2421 76 | 26.5896 77 | 26.9718 78 | 27.4001 79 | 27.8853 80 | 28.4275 81 | 29.0285 82 | 29.6785 83 | 30.3671 84 | 31.0655 85 | 31.7479 86 | 32.4137 87 | 33.0627 88 | 33.7043 89 | 34.3476 90 | 35.0007 91 | 35.6728 92 | 36.3703 93 | 36.9665 94 | 36.3783 95 | 35.6702 96 | 34.9231 97 | 34.1401 98 | 33.3093 99 | 32.4231 100 | 31.4786 101 | 30.4747 102 | 29.4113 103 | 28.376 104 | 27.4214 105 | 26.5077 106 | 25.6246 107 | 24.7608 108 | 23.9154 109 | 23.0885 110 | 22.289 111 | 21.524 112 | 20.7914 113 | 20.0913 114 | 19.4267 115 | 18.7985 116 | 18.2165 117 | 17.6923 118 | 17.231 119 | 16.8385 120 | 16.5142 121 | 16.2601 122 | 16.075 123 | 15.9579 124 | 15.9065 125 | 15.9219 126 | 16.0221 127 | 16.2372 128 | 16.5852 129 | 17.0899 130 | 17.6962 131 | 18.3266 132 | 18.9808 133 | 19.6582 134 | 20.3569 135 | 21.0754 136 | 21.8128 137 | 22.5675 138 | 23.3378 139 | 24.1212 140 | 24.9157 141 | 25.719 142 | 26.5293 143 | 27.3451 144 | 28.1644 145 | 28.9861 146 | 29.8089 147 | 30.6318 148 | 31.4545 149 | 32.2769 150 | 33.0988 151 | 33.9203 152 | 34.7411 153 | 35.5616 154 | 36.3831 155 | 37.2073 156 | 38.0354 157 | 37.4337 158 | 37.2635 159 | 36.9437 160 | 36.1461 161 | 35.321 162 | 34.5023 163 | 33.6917 164 | 32.8894 165 | 32.0949 166 | 31.3096 167 | 30.5351 168 | 29.772 169 | 29.0209 170 | 28.3262 171 | 27.738 172 | 27.3206 173 | 27.0655 174 | 26.3514 175 | 25.6205 176 | 24.8761 177 | 24.1119 178 | 23.323 179 | 22.5055 180 | 21.6575 181 | 20.7773 182 | 19.8628 183 | 18.9126 184 | 17.9255 185 | 16.9007 186 | 15.8329 187 | 14.7181 188 | 13.5516 189 | 12.331 190 | 11.036 191 | 9.6613 192 | 8.144 193 | 7.14376 194 | 5.92481 195 | 4.53472 196 | 3.29777 197 | 2.21779 198 | 1.31221 199 | 0.613336 200 | 0.16128 201 | 0.000297341 202 | -------------------------------------------------------------------------------- /results/v2_19/dtt1_c.txt: -------------------------------------------------------------------------------- 1 | 0.0169735 2 | 0.172326 3 | 0.340706 4 | 0.506039 5 | 0.668438 6 | 0.827867 7 | 0.984336 8 | 1.13793 9 | 1.28878 10 | 1.43697 11 | 1.58258 12 | 1.72592 13 | 1.8674 14 | 2.00729 15 | 2.14585 16 | 2.28344 17 | 2.42045 18 | 2.55725 19 | 2.69412 20 | 2.8313 21 | 2.96852 22 | 3.10533 23 | 3.24132 24 | 3.3762 25 | 3.50981 26 | 3.64195 27 | 3.7724 28 | 3.90115 29 | 4.02819 30 | 4.15332 31 | 4.27632 32 | 4.39723 33 | 4.51612 34 | 4.63281 35 | 4.74713 36 | 4.8592 37 | 4.96918 38 | 5.07708 39 | 5.183 40 | 5.28685 41 | 5.38877 42 | 5.4781 43 | 5.56348 44 | 5.64754 45 | 5.73106 46 | 5.81462 47 | 5.899 48 | 5.98369 49 | 6.06904 50 | 6.15155 51 | 6.21148 52 | 6.12129 53 | 6.02869 54 | 5.93546 55 | 5.84187 56 | 5.74821 57 | 5.65486 58 | 5.56218 59 | 5.47051 60 | 5.38015 61 | 5.29151 62 | 5.20475 63 | 5.11992 64 | 5.05051 65 | 5.00832 66 | 4.98978 67 | 4.99238 68 | 5.0032 69 | 5.01452 70 | 5.02725 71 | 5.04159 72 | 5.05673 73 | 5.07259 74 | 5.09355 75 | 5.1227 76 | 5.15651 77 | 5.19344 78 | 5.23451 79 | 5.28065 80 | 5.33174 81 | 5.38781 82 | 5.4478 83 | 5.51063 84 | 5.57365 85 | 5.63453 86 | 5.6933 87 | 5.75001 88 | 5.80554 89 | 5.86068 90 | 5.91614 91 | 5.97267 92 | 6.03078 93 | 6.08001 94 | 6.03144 95 | 5.97245 96 | 5.90957 97 | 5.84294 98 | 5.77142 99 | 5.69414 100 | 5.61059 101 | 5.5204 102 | 5.42323 103 | 5.32691 104 | 5.23653 105 | 5.14855 106 | 5.06208 107 | 4.97602 108 | 4.89033 109 | 4.80505 110 | 4.72112 111 | 4.63939 112 | 4.55976 113 | 4.48233 114 | 4.40757 115 | 4.33572 116 | 4.26808 117 | 4.20622 118 | 4.15102 119 | 4.10348 120 | 4.06377 121 | 4.03238 122 | 4.00936 123 | 3.99474 124 | 3.9883 125 | 3.99022 126 | 4.00276 127 | 4.02954 128 | 4.0725 129 | 4.13399 130 | 4.20669 131 | 4.28095 132 | 4.35669 133 | 4.43375 134 | 4.51187 135 | 4.59079 136 | 4.67041 137 | 4.75053 138 | 4.83092 139 | 4.91134 140 | 4.99156 141 | 5.0714 142 | 5.15066 143 | 5.22925 144 | 5.30701 145 | 5.38387 146 | 5.45975 147 | 5.5346 148 | 5.60843 149 | 5.68127 150 | 5.75316 151 | 5.82411 152 | 5.89416 153 | 5.96336 154 | 6.03184 155 | 6.09978 156 | 6.16728 157 | 6.11831 158 | 6.10438 159 | 6.07813 160 | 6.01217 161 | 5.94316 162 | 5.87386 163 | 5.80445 164 | 5.73493 165 | 5.66523 166 | 5.59549 167 | 5.52585 168 | 5.45637 169 | 5.3871 170 | 5.32224 171 | 5.26669 172 | 5.22692 173 | 5.20246 174 | 5.13336 175 | 5.06167 176 | 4.9876 177 | 4.91039 178 | 4.82939 179 | 4.74399 180 | 4.65376 181 | 4.55821 182 | 4.45677 183 | 4.34886 184 | 4.23385 185 | 4.11105 186 | 3.97905 187 | 3.83642 188 | 3.68125 189 | 3.51155 190 | 3.32205 191 | 3.10826 192 | 2.85377 193 | 2.67278 194 | 2.43409 195 | 2.12949 196 | 1.81598 197 | 1.48922 198 | 1.14552 199 | 0.783156 200 | 0.401599 201 | 0.0210374 202 | -------------------------------------------------------------------------------- /results/v2_19/dtt1_d.txt: -------------------------------------------------------------------------------- 1 | 5.28253 2 | 1.94921 3 | 1.18098 4 | 0.851442 5 | 0.668312 6 | 0.551813 7 | 0.471194 8 | 0.412081 9 | 0.366872 10 | 0.331175 11 | 0.302252 12 | 0.278294 13 | 0.258085 14 | 0.240782 15 | 0.22577 16 | 0.21259 17 | 0.200896 18 | 0.190427 19 | 0.180982 20 | 0.172419 21 | 0.16464 22 | 0.157563 23 | 0.151114 24 | 0.145222 25 | 0.139826 26 | 0.134874 27 | 0.130318 28 | 0.126114 29 | 0.122227 30 | 0.118629 31 | 0.115293 32 | 0.112191 33 | 0.109302 34 | 0.106611 35 | 0.104098 36 | 0.101746 37 | 0.0995395 38 | 0.0974652 39 | 0.0955123 40 | 0.0936713 41 | 0.0920229 42 | 0.0905668 43 | 0.0891979 44 | 0.0878842 45 | 0.0866124 46 | 0.0853707 47 | 0.084156 48 | 0.0829688 49 | 0.0818291 50 | 0.0808863 51 | 0.0810848 52 | 0.0823047 53 | 0.0835831 54 | 0.0849089 55 | 0.0862807 56 | 0.0876957 57 | 0.08915 58 | 0.0906397 59 | 0.0921603 60 | 0.0937061 61 | 0.095272 62 | 0.0968554 63 | 0.0983242 64 | 0.0994151 65 | 0.100019 66 | 0.100179 67 | 0.100044 68 | 0.0998231 69 | 0.0995841 70 | 0.0993163 71 | 0.0990263 72 | 0.0987233 73 | 0.0983657 74 | 0.0978833 75 | 0.0972838 76 | 0.0966189 77 | 0.0958961 78 | 0.0951008 79 | 0.0942295 80 | 0.0932875 81 | 0.0922883 82 | 0.0912539 83 | 0.0902179 84 | 0.0892206 85 | 0.0882781 86 | 0.0873872 87 | 0.0865385 88 | 0.0857176 89 | 0.0849126 90 | 0.0841127 91 | 0.0833094 92 | 0.082571 93 | 0.0825665 94 | 0.0833063 95 | 0.0841608 96 | 0.0850882 97 | 0.0861003 98 | 0.0872177 99 | 0.0884585 100 | 0.0898393 101 | 0.0913774 102 | 0.0930221 103 | 0.0946662 104 | 0.096292 105 | 0.097937 106 | 0.0996204 107 | 0.101355 108 | 0.103142 109 | 0.104974 110 | 0.106832 111 | 0.108706 112 | 0.110594 113 | 0.112487 114 | 0.114373 115 | 0.116228 116 | 0.118004 117 | 0.119657 118 | 0.121146 119 | 0.12244 120 | 0.123516 121 | 0.124351 122 | 0.124936 123 | 0.125266 124 | 0.125337 125 | 0.12511 126 | 0.124497 127 | 0.123426 128 | 0.121855 129 | 0.119894 130 | 0.117818 131 | 0.115772 132 | 0.11376 133 | 0.111787 134 | 0.109858 135 | 0.107977 136 | 0.106147 137 | 0.104368 138 | 0.102646 139 | 0.100981 140 | 0.0993744 141 | 0.0978277 142 | 0.0963399 143 | 0.0949103 144 | 0.0935376 145 | 0.0922201 146 | 0.0909558 147 | 0.0897422 148 | 0.0885762 149 | 0.0874552 150 | 0.0863762 151 | 0.0853368 152 | 0.0843347 153 | 0.0833667 154 | 0.0824293 155 | 0.0815191 156 | 0.0813962 157 | 0.0818151 158 | 0.0820849 159 | 0.082711 160 | 0.0836447 161 | 0.0846237 162 | 0.0856288 163 | 0.0866598 164 | 0.087718 165 | 0.0888042 166 | 0.0899172 167 | 0.0910563 168 | 0.0922214 169 | 0.0933765 170 | 0.0944383 171 | 0.0952962 172 | 0.0958831 173 | 0.0967509 174 | 0.0980869 175 | 0.0995097 176 | 0.101031 177 | 0.102672 178 | 0.104456 179 | 0.106408 180 | 0.108554 181 | 0.110927 182 | 0.113564 183 | 0.116513 184 | 0.119834 185 | 0.123608 186 | 0.127951 187 | 0.13302 188 | 0.139028 189 | 0.146336 190 | 0.155513 191 | 0.167728 192 | 0.180945 193 | 0.195815 194 | 0.219126 195 | 0.253456 196 | 0.302553 197 | 0.379544 198 | 0.518491 199 | 0.844056 200 | 2.36607 201 | -------------------------------------------------------------------------------- /results/v2_19/dtt1_q.txt: -------------------------------------------------------------------------------- 1 | -42, -17.5 2 | -41.9867, -17.5044 3 | -41.9474, -17.5169 4 | -41.8833, -17.5368 5 | -41.7951, -17.5632 6 | -41.6837, -17.5953 7 | -41.5498, -17.6323 8 | -41.394, -17.6731 9 | -41.2168, -17.7168 10 | -41.0189, -17.7622 11 | -40.8008, -17.8083 12 | -40.5629, -17.8538 13 | -40.3057, -17.8976 14 | -40.0296, -17.9387 15 | -39.7349, -17.9759 16 | -39.4222, -18.0078 17 | -39.0918, -18.0335 18 | -38.7445, -18.0516 19 | -38.381, -18.0612 20 | -38.0018, -18.0615 21 | -37.6078, -18.053 22 | -37.1996, -18.0361 23 | -36.778, -18.0116 24 | -36.3437, -17.9803 25 | -35.8974, -17.9427 26 | -35.4397, -17.8996 27 | -34.9714, -17.8515 28 | -34.4931, -17.799 29 | -34.0055, -17.7425 30 | -33.5092, -17.6826 31 | -33.0049, -17.6199 32 | -32.4932, -17.5551 33 | -31.9747, -17.4885 34 | -31.45, -17.4208 35 | -30.9193, -17.3526 36 | -30.3831, -17.2847 37 | -29.8418, -17.2175 38 | -29.2958, -17.1518 39 | -28.7454, -17.088 40 | -28.191, -17.0267 41 | -27.6331, -16.9686 42 | -27.0721, -16.9143 43 | -26.5083, -16.8649 44 | -25.9421, -16.8211 45 | -25.3739, -16.784 46 | -24.8043, -16.7543 47 | -24.2337, -16.7328 48 | -23.6627, -16.7194 49 | -23.092, -16.7118 50 | -22.5222, -16.7075 51 | -21.9538, -16.7046 52 | -21.3875, -16.7016 53 | -20.8236, -16.6971 54 | -20.263, -16.6898 55 | -19.706, -16.6787 56 | -19.1532, -16.6624 57 | -18.605, -16.6401 58 | -18.0617, -16.6106 59 | -17.5237, -16.5732 60 | -16.9913, -16.5271 61 | -16.4648, -16.4715 62 | -15.9446, -16.406 63 | -15.431, -16.3303 64 | -14.9244, -16.2441 65 | -14.4247, -16.1477 66 | -13.9317, -16.0414 67 | -13.4455, -15.9259 68 | -12.9662, -15.8018 69 | -12.4936, -15.6693 70 | -12.028, -15.5289 71 | -11.5693, -15.3809 72 | -11.1178, -15.2257 73 | -10.6733, -15.0636 74 | -10.2357, -14.8948 75 | -9.8049, -14.72 76 | -9.38096, -14.5396 77 | -8.9636, -14.3542 78 | -8.55256, -14.1641 79 | -8.14766, -13.9696 80 | -7.74873, -13.7714 81 | -7.35555, -13.5699 82 | -6.96789, -13.3654 83 | -6.58558, -13.1585 84 | -6.20849, -12.9495 85 | -5.83634, -12.7385 86 | -5.46888, -12.5255 87 | -5.10584, -12.3105 88 | -4.747, -12.0936 89 | -4.39225, -11.8749 90 | -4.0415, -11.6546 91 | -3.69466, -11.4327 92 | -3.35163, -11.2097 93 | -3.01225, -10.9856 94 | -2.67627, -10.7606 95 | -2.34332, -10.5349 96 | -2.01298, -10.3085 97 | -1.68493, -10.0817 98 | -1.35886, -9.85467 99 | -1.0343, -9.62753 100 | -0.710818, -9.4006 101 | -0.388005, -9.17425 102 | -0.0654699, -8.9489 103 | 0.257265, -8.72496 104 | 0.580612, -8.50284 105 | 0.904644, -8.2829 106 | 1.22937, -8.06546 107 | 1.55494, -7.85068 108 | 1.88146, -7.6387 109 | 2.20893, -7.4298 110 | 2.53733, -7.22424 111 | 2.86676, -7.02232 112 | 3.19733, -6.82432 113 | 3.529, -6.63064 114 | 3.86175, -6.44167 115 | 4.19574, -6.25769 116 | 4.53121, -6.079 117 | 4.86833, -5.906 118 | 5.20732, -5.73914 119 | 5.54841, -5.57884 120 | 5.89187, -5.42553 121 | 6.23783, -5.27966 122 | 6.58644, -5.14163 123 | 6.93781, -5.01178 124 | 7.29204, -4.89042 125 | 7.64908, -4.77781 126 | 8.0088, -4.67421 127 | 8.37103, -4.57986 128 | 8.73561, -4.49499 129 | 9.10242, -4.41967 130 | 9.47141, -4.35382 131 | 9.84275, -4.29698 132 | 10.2166, -4.24858 133 | 10.5932, -4.20806 134 | 10.9726, -4.17485 135 | 11.355, -4.14849 136 | 11.7404, -4.12853 137 | 12.1288, -4.11443 138 | 12.5202, -4.1056 139 | 12.9146, -4.10146 140 | 13.3119, -4.1014 141 | 13.7117, -4.10488 142 | 14.1138, -4.11139 143 | 14.518, -4.12055 144 | 14.9237, -4.13195 145 | 15.331, -4.14514 146 | 15.7395, -4.15961 147 | 16.1491, -4.1749 148 | 16.5599, -4.19055 149 | 16.9715, -4.20614 150 | 17.3839, -4.22131 151 | 17.7971, -4.23569 152 | 18.2108, -4.24892 153 | 18.6252, -4.26057 154 | 19.0404, -4.2702 155 | 19.4565, -4.27738 156 | 19.8737, -4.28171 157 | 20.292, -4.28302 158 | 20.7114, -4.28113 159 | 21.1321, -4.27577 160 | 21.5543, -4.26662 161 | 21.9779, -4.25333 162 | 22.403, -4.23549 163 | 22.8297, -4.2126 164 | 23.2581, -4.18418 165 | 23.6881, -4.14991 166 | 24.1197, -4.1095 167 | 24.553, -4.06252 168 | 24.9881, -4.00851 169 | 25.4252, -3.94701 170 | 25.8642, -3.87756 171 | 26.3053, -3.79977 172 | 26.7488, -3.7133 173 | 27.1949, -3.61795 174 | 27.6439, -3.51359 175 | 28.0962, -3.40009 176 | 28.552, -3.27738 177 | 29.0117, -3.14547 178 | 29.4754, -3.00445 179 | 29.9434, -2.85454 180 | 30.4157, -2.69596 181 | 30.8927, -2.52888 182 | 31.3747, -2.35345 183 | 31.8621, -2.16979 184 | 32.3553, -1.97803 185 | 32.8546, -1.77829 186 | 33.3604, -1.57074 187 | 33.873, -1.35587 188 | 34.3928, -1.13436 189 | 34.9203, -0.907186 190 | 35.4564, -0.675946 191 | 36.0023, -0.444181 192 | 36.5601, -0.218218 193 | 37.135, -0.0135578 194 | 37.7282, 0.155047 195 | 38.3201, 0.285001 196 | 38.8866, 0.377789 197 | 39.4037, 0.439061 198 | 39.8475, 0.475462 199 | 40.1941, 0.493448 200 | 40.4195, 0.499453 201 | 40.4785, 0.503419 202 | -------------------------------------------------------------------------------- /results/v2_19/dtt1_qa.txt: -------------------------------------------------------------------------------- 1 | 0.124169, 0.378852 2 | 0.062085, 0.194412 3 | 0.0399714, 0.129056 4 | 0.0298674, 0.0997192 5 | 0.0259286, 0.0898607 6 | 0.0236668, 0.0857602 7 | 0.0217069, 0.0828607 8 | 0.0199288, 0.0808437 9 | 0.0184686, 0.080408 10 | 0.0171012, 0.0809709 11 | 0.0152875, 0.0799153 12 | 0.0132775, 0.0778888 13 | 0.011443, 0.0769 14 | 0.00966481, 0.0766812 15 | 0.00776841, 0.0759956 16 | 0.00581907, 0.0750444 17 | 0.00382292, 0.0733077 18 | 0.00187842, 0.0711342 19 | 6.26419e-05, 0.0641654 20 | -0.00116752, 0.0537037 21 | -0.00183708, 0.0443421 22 | -0.00208502, 0.0358887 23 | -0.00211939, 0.0293966 24 | -0.00205495, 0.0244456 25 | -0.00188925, 0.0200425 26 | -0.00165279, 0.0160834 27 | -0.00144014, 0.0131192 28 | -0.00127001, 0.0109741 29 | -0.00102734, 0.00851205 30 | -0.000716036, 0.00575793 31 | -0.000466952, 0.00368309 32 | -0.000281664, 0.00219467 33 | -3.02173e-05, 0.000234169 34 | 0.000279335, -0.00217374 35 | 0.000525002, -0.00414269 36 | 0.000708907, -0.00571764 37 | 0.000867519, -0.00720766 38 | 0.000999844, -0.00862708 39 | 0.00113364, -0.010253 40 | 0.00126018, -0.0120931 41 | 0.00138495, -0.0143215 42 | 0.00148595, -0.0169312 43 | 0.00151186, -0.0195542 44 | 0.0014522, -0.0222225 45 | 0.00126043, -0.0242521 46 | 0.000966977, -0.0256567 47 | 0.000521297, -0.0221368 48 | 0.000182254, -0.0136851 49 | 5.28172e-05, -0.00695725 50 | 9.67518e-06, -0.00191011 51 | -1.41497e-05, 0.00265748 52 | -5.40644e-05, 0.00677228 53 | -0.000139519, 0.0107864 54 | -0.00029529, 0.014722 55 | -0.000548278, 0.0186853 56 | -0.000924399, 0.0226852 57 | -0.00143304, 0.0264193 58 | -0.00207706, 0.0298581 59 | -0.00287923, 0.0332222 60 | -0.00385016, 0.0365076 61 | -0.00494226, 0.0392614 62 | -0.00611696, 0.0414627 63 | -0.00723837, 0.0425385 64 | -0.00816177, 0.0423012 65 | -0.00892591, 0.041411 66 | -0.00948426, 0.0399242 67 | -0.0101061, 0.0390224 68 | -0.0108469, 0.0387017 69 | -0.0115358, 0.0382595 70 | -0.0121787, 0.0377437 71 | -0.0128188, 0.0372918 72 | -0.0134353, 0.0368312 73 | -0.0137617, 0.0356837 74 | -0.0137567, 0.0338944 75 | -0.0137913, 0.0324252 76 | -0.013868, 0.0312162 77 | -0.0137473, 0.0297158 78 | -0.0134272, 0.0279615 79 | -0.0129922, 0.0261469 80 | -0.0124339, 0.024256 81 | -0.0118807, 0.0225299 82 | -0.0113592, 0.0209899 83 | -0.0110807, 0.0199917 84 | -0.0110669, 0.0195165 85 | -0.0110737, 0.0191043 86 | -0.0111085, 0.0187603 87 | -0.0110397, 0.0182637 88 | -0.010876, 0.0176406 89 | -0.0106072, 0.0168832 90 | -0.010228, 0.0159928 91 | -0.00974809, 0.014991 92 | -0.0091557, 0.0138655 93 | -0.00839045, 0.0125302 94 | -0.00743172, 0.0109625 95 | -0.00645452, 0.00942037 96 | -0.00547648, 0.00792123 97 | -0.00416338, 0.00597836 98 | -0.00250858, 0.00358445 99 | -0.000666025, 0.000949414 100 | 0.00134948, -0.00192463 101 | 0.00353483, -0.00505921 102 | 0.00586875, -0.00845778 103 | 0.00752906, -0.0109603 104 | 0.0085361, -0.0125761 105 | 0.00938045, -0.0140089 106 | 0.0100463, -0.015228 107 | 0.0107159, -0.0165068 108 | 0.0114061, -0.0178797 109 | 0.012267, -0.0195978 110 | 0.013273, -0.0216541 111 | 0.014236, -0.0237683 112 | 0.0151774, -0.0259911 113 | 0.016145, -0.028428 114 | 0.0170868, -0.031019 115 | 0.0181367, -0.0340487 116 | 0.0192739, -0.0375603 117 | 0.0203208, -0.0412817 118 | 0.0212338, -0.0451826 119 | 0.0218494, -0.0489503 120 | 0.0221759, -0.0525942 121 | 0.0221378, -0.0559109 122 | 0.0217436, -0.0588386 123 | 0.020992, -0.0612713 124 | 0.0199509, -0.0632545 125 | 0.0187589, -0.0651328 126 | 0.0174327, -0.0669335 127 | 0.015848, -0.0680792 128 | 0.0140602, -0.0684673 129 | 0.0119299, -0.0668483 130 | 0.00964769, -0.0630278 131 | 0.00761249, -0.0588105 132 | 0.0058433, -0.0542992 133 | 0.00438683, -0.0501201 134 | 0.00319467, -0.0463487 135 | 0.00218228, -0.0421358 136 | 0.00136421, -0.0375645 137 | 0.000742617, -0.0329477 138 | 0.000297415, -0.0283197 139 | 3.57469e-06, -0.0241082 140 | -0.000176906, -0.0203308 141 | -0.000279109, -0.0172334 142 | -0.00033467, -0.0147763 143 | -0.00033801, -0.0120261 144 | -0.000290271, -0.0089651 145 | -0.000213443, -0.00602337 146 | -0.000118825, -0.00318376 147 | -2.44135e-05, -0.00064094 148 | 6.12988e-05, 0.00161791 149 | 0.00013759, 0.00374174 150 | 0.000199675, 0.00573613 151 | 0.000256575, 0.00802299 152 | 0.000297914, 0.0105955 153 | 0.000303843, 0.0131013 154 | 0.000267544, 0.0155218 155 | 0.000177144, 0.0170515 156 | 5.53726e-05, 0.0177212 157 | -8.47874e-05, 0.0188387 158 | -0.000259496, 0.0203696 159 | -0.000477343, 0.0220301 160 | -0.000747919, 0.0238333 161 | -0.00109159, 0.0260154 162 | -0.00153053, 0.0285364 163 | -0.00200804, 0.0302654 164 | -0.00249091, 0.0312561 165 | -0.00305653, 0.0326426 166 | -0.00372493, 0.0343544 167 | -0.00446679, 0.0359889 168 | -0.00528348, 0.0375451 169 | -0.00611618, 0.0386618 170 | -0.00693479, 0.0393264 171 | -0.00765007, 0.0392341 172 | -0.00820982, 0.0384118 173 | -0.00868017, 0.0373456 174 | -0.00904905, 0.0360578 175 | -0.009258, 0.0343885 176 | -0.00930205, 0.0324155 177 | -0.00918272, 0.0301989 178 | -0.00890994, 0.0278137 179 | -0.00859007, 0.0255859 180 | -0.00822406, 0.0234789 181 | -0.00781389, 0.0214686 182 | -0.007369, 0.0195558 183 | -0.00690831, 0.0177666 184 | -0.00644016, 0.0160984 185 | -0.00571178, 0.0139199 186 | -0.00474872, 0.0113295 187 | -0.00342359, 0.00803316 188 | -0.00177491, 0.00412179 189 | 0.00148177, -0.00343536 190 | 0.00605775, -0.014268 191 | 0.0174203, -0.0430007 192 | 0.0317012, -0.0890571 193 | 0.029042, -0.102163 194 | 0.0202291, -0.0921456 195 | 0.0138983, -0.0848504 196 | 0.00900699, -0.0760089 197 | 0.00600999, -0.0732732 198 | 0.0041003, -0.079013 199 | 0.00271312, -0.101875 200 | 0.00117349, -0.172661 201 | 0.00117349, -0.172661 202 | -------------------------------------------------------------------------------- /results/v2_19/dtt1_qv.txt: -------------------------------------------------------------------------------- 1 | 0.950261, -0.311449 2 | 0.952601, -0.304211 3 | 0.955229, -0.295854 4 | 0.95795, -0.286921 5 | 0.960798, -0.277231 6 | 0.963961, -0.266019 7 | 0.967349, -0.253416 8 | 0.970925, -0.239343 9 | 0.974611, -0.223854 10 | 0.978403, -0.20664 11 | 0.982174, -0.187887 12 | 0.985762, -0.168041 13 | 0.98909, -0.14718 14 | 0.992129, -0.125047 15 | 0.994792, -0.10169 16 | 0.996981, -0.0773076 17 | 0.998616, -0.0520768 18 | 0.999624, -0.0263968 19 | 0.999975, -0.000976231 20 | 0.999745, 0.0217345 21 | 0.999129, 0.0413937 22 | 0.998307, 0.0579985 23 | 0.997404, 0.0719092 24 | 0.99648, 0.0837663 25 | 0.995583, 0.0938458 26 | 0.994759, 0.102225 27 | 0.994027, 0.109118 28 | 0.993369, 0.114961 29 | 0.992795, 0.119822 30 | 0.992356, 0.123406 31 | 0.992059, 0.125776 32 | 0.991865, 0.127296 33 | 0.991777, 0.12798 34 | 0.991844, 0.127457 35 | 0.992065, 0.125724 36 | 0.992401, 0.123044 37 | 0.992834, 0.119498 38 | 0.99335, 0.115125 39 | 0.993942, 0.109897 40 | 0.994612, 0.103646 41 | 0.995354, 0.0962548 42 | 0.996167, 0.0874276 43 | 0.997019, 0.077086 44 | 0.997865, 0.0652085 45 | 0.998644, 0.0519015 46 | 0.999282, 0.037662 47 | 0.999716, 0.0235422 48 | 0.999909, 0.0133164 49 | 0.999971, 0.00759145 50 | 0.999987, 0.00506519 51 | 0.999986, 0.00532441 52 | 0.999968, 0.00798293 53 | 0.999915, 0.0129337 54 | 0.999796, 0.0200536 55 | 0.999565, 0.02933 56 | 0.999164, 0.0407149 57 | 0.998523, 0.0541622 58 | 0.997578, 0.0693959 59 | 0.996252, 0.0863411 60 | 0.994469, 0.104879 61 | 0.992152, 0.124893 62 | 0.989273, 0.145946 63 | 0.98581, 0.167746 64 | 0.981871, 0.189446 65 | 0.977531, 0.210701 66 | 0.972907, 0.231121 67 | 0.968046, 0.250707 68 | 0.962881, 0.269867 69 | 0.957411, 0.288674 70 | 0.95167, 0.307074 71 | 0.945675, 0.325068 72 | 0.939434, 0.342688 73 | 0.933007, 0.359821 74 | 0.926579, 0.376069 75 | 0.920212, 0.391392 76 | 0.913866, 0.405992 77 | 0.907576, 0.419866 78 | 0.901444, 0.432876 79 | 0.895531, 0.444982 80 | 0.889888, 0.456166 81 | 0.884544, 0.466447 82 | 0.879469, 0.475946 83 | 0.874633, 0.484778 84 | 0.869875, 0.493264 85 | 0.86516, 0.501488 86 | 0.860464, 0.509505 87 | 0.855801, 0.5173 88 | 0.851219, 0.524805 89 | 0.84675, 0.531986 90 | 0.842446, 0.538775 91 | 0.838341, 0.545142 92 | 0.834484, 0.551029 93 | 0.830916, 0.556394 94 | 0.827724, 0.561134 95 | 0.824939, 0.565221 96 | 0.822553, 0.568687 97 | 0.820614, 0.571482 98 | 0.819289, 0.573381 99 | 0.81865, 0.574293 100 | 0.818784, 0.574102 101 | 0.819735, 0.572742 102 | 0.821584, 0.570087 103 | 0.824256, 0.566215 104 | 0.827404, 0.561604 105 | 0.830919, 0.55639 106 | 0.834712, 0.550683 107 | 0.838756, 0.544503 108 | 0.843057, 0.537819 109 | 0.847637, 0.53057 110 | 0.852579, 0.522591 111 | 0.857887, 0.51383 112 | 0.863545, 0.504262 113 | 0.869546, 0.493839 114 | 0.875895, 0.482486 115 | 0.882589, 0.470127 116 | 0.88969, 0.456541 117 | 0.897181, 0.441634 118 | 0.905026, 0.425321 119 | 0.913146, 0.40759 120 | 0.921423, 0.388511 121 | 0.929751, 0.368132 122 | 0.937978, 0.346627 123 | 0.945995, 0.324105 124 | 0.953663, 0.300792 125 | 0.960913, 0.276752 126 | 0.96769, 0.252032 127 | 0.973931, 0.226719 128 | 0.979531, 0.201153 129 | 0.98442, 0.175682 130 | 0.988463, 0.151304 131 | 0.991706, 0.128367 132 | 0.994242, 0.106993 133 | 0.996176, 0.0871917 134 | 0.99762, 0.0687627 135 | 0.99865, 0.0517216 136 | 0.999332, 0.0362922 137 | 0.999739, 0.0225334 138 | 0.99994, 0.0105014 139 | 0.999996, 0.000148277 140 | 0.999959, -0.00870101 141 | 0.999867, -0.0161937 142 | 0.999742, -0.0226432 143 | 0.999604, -0.0280952 144 | 0.999476, -0.0323609 145 | 0.999372, -0.0354137 146 | 0.999304, -0.0372963 147 | 0.999275, -0.0380625 148 | 0.999283, -0.0378605 149 | 0.999325, -0.0367469 150 | 0.999394, -0.034789 151 | 0.999489, -0.0319636 152 | 0.999604, -0.028106 153 | 0.99973, -0.0231855 154 | 0.99985, -0.0172341 155 | 0.999944, -0.0103881 156 | 0.999993, -0.00312464 157 | 0.999987, 0.00450066 158 | 0.999916, 0.0127383 159 | 0.999762, 0.0216626 160 | 0.999504, 0.0313657 161 | 0.999116, 0.0419222 162 | 0.998559, 0.0535568 163 | 0.997799, 0.0662018 164 | 0.996832, 0.0794411 165 | 0.995636, 0.0932278 166 | 0.994164, 0.107794 167 | 0.992375, 0.123169 168 | 0.990232, 0.139349 169 | 0.987704, 0.156252 170 | 0.984793, 0.173658 171 | 0.981502, 0.191379 172 | 0.9779, 0.209008 173 | 0.974023, 0.22639 174 | 0.969911, 0.243409 175 | 0.965608, 0.259959 176 | 0.961196, 0.275827 177 | 0.956737, 0.290919 178 | 0.952321, 0.30507 179 | 0.947991, 0.318273 180 | 0.943771, 0.330579 181 | 0.939688, 0.342016 182 | 0.935764, 0.352614 183 | 0.932017, 0.362402 184 | 0.928457, 0.37143 185 | 0.925141, 0.379615 186 | 0.922261, 0.386563 187 | 0.919938, 0.392061 188 | 0.918463, 0.395505 189 | 0.918225, 0.396058 190 | 0.92047, 0.390803 191 | 0.926801, 0.375462 192 | 0.941961, 0.335305 193 | 0.961717, 0.273388 194 | 0.976606, 0.214398 195 | 0.986748, 0.161627 196 | 0.992986, 0.117668 197 | 0.996608, 0.0817434 198 | 0.998624, 0.0518226 199 | 0.999623, 0.0266217 200 | 0.999965, 0.0067963 201 | 0.999965, 0.0067963 202 | -------------------------------------------------------------------------------- /results/v2_19/dtt1_s.txt: -------------------------------------------------------------------------------- 1 | 0 2 | 0.014021 3 | 0.0552338 4 | 0.122406 5 | 0.214465 6 | 0.330405 7 | 0.469323 8 | 0.630371 9 | 0.812794 10 | 1.01587 11 | 1.23883 12 | 1.48101 13 | 1.74191 14 | 2.02111 15 | 2.31813 16 | 2.63252 17 | 2.96382 18 | 3.3116 19 | 3.67533 20 | 4.0545 21 | 4.44863 22 | 4.85716 23 | 5.27948 24 | 5.7149 25 | 6.16279 26 | 6.62249 27 | 7.09329 28 | 7.57445 29 | 8.0653 30 | 8.56519 31 | 9.07338 32 | 9.58916 33 | 10.1119 34 | 10.641 35 | 11.176 36 | 11.7165 37 | 12.262 38 | 12.8119 39 | 13.366 40 | 13.9238 41 | 14.4847 42 | 15.0484 43 | 15.6143 44 | 16.1822 45 | 16.7516 46 | 17.322 47 | 17.893 48 | 18.4642 49 | 19.0349 50 | 19.6047 51 | 20.1731 52 | 20.7395 53 | 21.3033 54 | 21.864 55 | 22.4211 56 | 22.9742 57 | 23.5229 58 | 24.0669 59 | 24.6062 60 | 25.1406 61 | 25.67 62 | 26.1944 63 | 26.7135 64 | 27.2274 65 | 27.7364 66 | 28.2407 67 | 28.7404 68 | 29.2356 69 | 29.7263 70 | 30.2127 71 | 30.6946 72 | 31.1721 73 | 31.6453 74 | 32.1143 75 | 32.5792 76 | 33.0399 77 | 33.4966 78 | 33.9495 79 | 34.3987 80 | 34.8442 81 | 35.286 82 | 35.7243 83 | 36.1589 84 | 36.5901 85 | 37.0179 86 | 37.4426 87 | 37.8646 88 | 38.2839 89 | 38.7006 90 | 39.1149 91 | 39.5266 92 | 39.9357 93 | 40.3424 94 | 40.7468 95 | 41.149 96 | 41.5495 97 | 41.9483 98 | 42.3456 99 | 42.7418 100 | 43.1369 101 | 43.5312 102 | 43.9246 103 | 44.3175 104 | 44.7098 105 | 45.1014 106 | 45.4922 107 | 45.8822 108 | 46.2715 109 | 46.6599 110 | 47.0474 111 | 47.4338 112 | 47.8191 113 | 48.2032 114 | 48.5858 115 | 48.9672 116 | 49.3473 117 | 49.7262 118 | 50.104 119 | 50.4809 120 | 50.857 121 | 51.2325 122 | 51.6074 123 | 51.982 124 | 52.3565 125 | 52.7309 126 | 53.1052 127 | 53.4796 128 | 53.8539 129 | 54.2284 130 | 54.6032 131 | 54.9789 132 | 55.3559 133 | 55.7346 134 | 56.1155 135 | 56.4988 136 | 56.8847 137 | 57.2734 138 | 57.6649 139 | 58.0593 140 | 58.4566 141 | 58.8564 142 | 59.2586 143 | 59.6628 144 | 60.0688 145 | 60.4762 146 | 60.885 147 | 61.2949 148 | 61.706 149 | 62.1179 150 | 62.5306 151 | 62.944 152 | 63.3579 153 | 63.7725 154 | 64.1878 155 | 64.604 156 | 65.0212 157 | 65.4395 158 | 65.8589 159 | 66.2797 160 | 66.7019 161 | 67.1258 162 | 67.5513 163 | 67.9786 164 | 68.4079 165 | 68.8393 166 | 69.2728 167 | 69.7086 168 | 70.1471 169 | 70.5884 170 | 71.0329 171 | 71.4809 172 | 71.9327 173 | 72.3889 174 | 72.8499 175 | 73.3161 176 | 73.7882 177 | 74.2664 178 | 74.7512 179 | 75.2426 180 | 75.7408 181 | 76.2462 182 | 76.7592 183 | 77.28 184 | 77.8091 185 | 78.3469 186 | 78.8937 187 | 79.4495 188 | 80.0145 189 | 80.5889 190 | 81.1727 191 | 81.7658 192 | 82.3676 193 | 82.978 194 | 83.5947 195 | 84.2008 196 | 84.7749 197 | 85.2956 198 | 85.7409 199 | 86.088 200 | 86.3136 201 | 86.394 202 | -------------------------------------------------------------------------------- /results/v2_19/process.m: -------------------------------------------------------------------------------- 1 | % SL_HW5 V2_19示例数据处理 2 | % V0.0.1 20220831 tkalpha 3 | clear;clc 4 | 5 | a = readmatrix("dtt1_a.txt"); 6 | b = readmatrix("dtt1_b.txt"); 7 | c = readmatrix("dtt1_c.txt"); 8 | d = readmatrix("dtt1_d.txt"); 9 | s = readmatrix("dtt1_s.txt"); 10 | q = readmatrix("dtt1_q.txt"); 11 | qv = readmatrix("dtt1_qv.txt"); 12 | qa = readmatrix("dtt1_qa.txt"); 13 | 14 | % curvature velocity limit 15 | curve = zeros(length(q), 1); 16 | for i = 1 : length(q) 17 | j = max(min(i, length(q)-1),2); 18 | curve_dist = [-norm(q(j,:)-q(j-1,:));0;norm(q(j,:)-q(j+1,:))]; 19 | curve_mat = [ones(3,1), curve_dist, curve_dist.^2]; 20 | curve_x = [q(j-1,1); q(j,1); q(j+1,1)]; 21 | curve_y = [q(j-1,2); q(j,2); q(j+1,2)]; 22 | curve_px = curve_mat \ curve_x; 23 | curve_py = curve_mat \ curve_y; 24 | curve(i) = -2.0*(curve_px(3)*curve_py(2)-curve_py(3)*curve_px(2))... 25 | /(curve_px(2)^2+curve_py(2)^2)^(3/2); 26 | end 27 | acc_max = 1.0; 28 | v_curve_max = sqrt(acc_max./abs(curve)); 29 | 30 | % origin method velocity limit 31 | v_origin_max = ones(length(q), 1)*1e20; 32 | v_origin_min = zeros(length(q), 1); 33 | for i = 1:length(q)-1 34 | if(qa(i,1) > 0) 35 | v_origin_max(i) = min(v_origin_max(i), sqrt(max((acc_max-qv(i,1)*a(i))/qa(i,1),0))); 36 | else 37 | v_origin_min(i) = max(v_origin_min(i), sqrt(max((acc_max-qv(i,1)*a(i))/qa(i,1),0))); 38 | end 39 | if(qa(i,2) > 0) 40 | v_origin_max(i) = min(v_origin_max(i), sqrt(max((acc_max-qv(i,2)*a(i))/qa(i,2),0))); 41 | else 42 | v_origin_min(i) = max(v_origin_min(i), sqrt(max((acc_max-qv(i,2)*a(i))/qa(i,2),0))); 43 | end 44 | end 45 | 46 | 47 | figure(220831001); clf(220831001); 48 | plot(q(:,1), q(:,2), '-b'); 49 | axis equal; grid on; xlabel('X (m)'); ylabel('Y (m)'); 50 | title('global trajectory'); 51 | 52 | figure(220831002); clf(220831002); 53 | plot(s, v_origin_max, '-', 'LineWidth', 4, 'Color', [0.5,0.5,0.5]); 54 | hold on; 55 | plot(s, v_origin_min, ':', 'LineWidth', 3.5, 'Color', [0.5,0.5,0.5]); 56 | plot(s, sqrt(b), '-b', 'LineWidth', 2.0); 57 | grid on; xlabel('trajectory length (m)'); ylabel('line speed (m/s)'); 58 | ylim([0, 7]); 59 | yyaxis right; 60 | plot(s(1:end-1), a, '-r', 'LineWidth', 1.5); 61 | ylabel('line acceleration (m/s^2)'); 62 | legend({'Upper speed bound','Lower speed bound','Planned speed', 'Planned acceleration'}); -------------------------------------------------------------------------------- /src/sl_hw5/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(sl_hw5) 3 | 4 | ## Compile as C++11, supported in ROS Kinetic and newer 5 | # add_compile_options(-std=c++11) 6 | 7 | set(CMAKE_BUILD_TYPE "Release") 8 | set(CMAKE_CXX_FLAGS "-std=c++17") 9 | set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall -fPIC") 10 | 11 | ## Find catkin macros and libraries 12 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 13 | ## is used, also find other catkin packages 14 | find_package(catkin REQUIRED COMPONENTS 15 | geometry_msgs 16 | roscpp 17 | rospy 18 | roslaunch 19 | std_msgs 20 | ) 21 | 22 | ## System dependencies are found with CMake's conventions 23 | # find_package(Boost REQUIRED COMPONENTS system) 24 | find_package(Eigen3 REQUIRED) 25 | 26 | 27 | ## Uncomment this if the package has a setup.py. This macro ensures 28 | ## modules and global scripts declared therein get installed 29 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 30 | # catkin_python_setup() 31 | 32 | ################################################ 33 | ## Declare ROS messages, services and actions ## 34 | ################################################ 35 | 36 | ## To declare and build messages, services or actions from within this 37 | ## package, follow these steps: 38 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 39 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 40 | ## * In the file package.xml: 41 | ## * add a build_depend tag for "message_generation" 42 | ## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET 43 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 44 | ## but can be declared for certainty nonetheless: 45 | ## * add a exec_depend tag for "message_runtime" 46 | ## * In this file (CMakeLists.txt): 47 | ## * add "message_generation" and every package in MSG_DEP_SET to 48 | ## find_package(catkin REQUIRED COMPONENTS ...) 49 | ## * add "message_runtime" and every package in MSG_DEP_SET to 50 | ## catkin_package(CATKIN_DEPENDS ...) 51 | ## * uncomment the add_*_files sections below as needed 52 | ## and list every .msg/.srv/.action file to be processed 53 | ## * uncomment the generate_messages entry below 54 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 55 | 56 | ## Generate messages in the 'msg' folder 57 | # add_message_files( 58 | # FILES 59 | # Message1.msg 60 | # Message2.msg 61 | # ) 62 | 63 | ## Generate services in the 'srv' folder 64 | # add_service_files( 65 | # FILES 66 | # Service1.srv 67 | # Service2.srv 68 | # ) 69 | 70 | ## Generate actions in the 'action' folder 71 | # add_action_files( 72 | # FILES 73 | # Action1.action 74 | # Action2.action 75 | # ) 76 | 77 | ## Generate added messages and services with any dependencies listed here 78 | # generate_messages( 79 | # DEPENDENCIES 80 | # geometry_msgs# std_msgs 81 | # ) 82 | 83 | ################################################ 84 | ## Declare ROS dynamic reconfigure parameters ## 85 | ################################################ 86 | 87 | ## To declare and build dynamic reconfigure parameters within this 88 | ## package, follow these steps: 89 | ## * In the file package.xml: 90 | ## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" 91 | ## * In this file (CMakeLists.txt): 92 | ## * add "dynamic_reconfigure" to 93 | ## find_package(catkin REQUIRED COMPONENTS ...) 94 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 95 | ## and list every .cfg file to be processed 96 | 97 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 98 | # generate_dynamic_reconfigure_options( 99 | # cfg/DynReconf1.cfg 100 | # cfg/DynReconf2.cfg 101 | # ) 102 | 103 | ################################### 104 | ## catkin specific configuration ## 105 | ################################### 106 | ## The catkin_package macro generates cmake config files for your package 107 | ## Declare things to be passed to dependent projects 108 | ## INCLUDE_DIRS: uncomment this if your package contains header files 109 | ## LIBRARIES: libraries you create in this project that dependent projects also need 110 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 111 | ## DEPENDS: system dependencies of this project that dependent projects also need 112 | catkin_package( 113 | INCLUDE_DIRS include ${EIGEN3_INCLUDE_DIR} 114 | # LIBRARIES sl_hw5 115 | CATKIN_DEPENDS geometry_msgs roscpp rospy std_msgs 116 | DEPENDS system_lib EIGEN3 117 | ) 118 | 119 | ########### 120 | ## Build ## 121 | ########### 122 | 123 | ## Specify additional locations of header files 124 | ## Your package locations should be listed before other locations 125 | include_directories( 126 | include 127 | ${EIGEN3_INCLUDE_DIR} 128 | ${catkin_INCLUDE_DIRS} 129 | ) 130 | 131 | ## Declare a C++ library 132 | # add_library(${PROJECT_NAME} 133 | # src/${PROJECT_NAME}/sl_hw5.cpp 134 | # ) 135 | 136 | ## Add cmake target dependencies of the library 137 | ## as an example, code may need to be generated before libraries 138 | ## either from message generation or dynamic reconfigure 139 | # add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 140 | 141 | ## Declare a C++ executable 142 | ## With catkin_make all packages are built within a single CMake context 143 | ## The recommended prefix ensures that target names across packages don't collide 144 | # add_executable(${PROJECT_NAME}_node src/sl_hw5_node.cpp) 145 | ## Rename C++ executable without prefix 146 | ## The above recommended prefix causes long target names, the following renames the 147 | ## target back to the shorter version for ease of user use 148 | ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" 149 | # set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") 150 | 151 | ## Add cmake target dependencies of the executable 152 | ## same as for the library above 153 | # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 154 | 155 | ## Specify libraries to link a library or executable target against 156 | # target_link_libraries(${PROJECT_NAME}_node 157 | # ${catkin_LIBRARIES} 158 | # ) 159 | add_executable(test0_node src/test0.cpp) 160 | target_link_libraries(test0_node ${catkin_LIBRARIES}) 161 | add_executable(main_node src/main.cpp) 162 | target_link_libraries(main_node ${catkin_LIBRARIES}) 163 | 164 | ############# 165 | ## Install ## 166 | ############# 167 | 168 | # all install targets should use catkin DESTINATION variables 169 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 170 | 171 | ## Mark executable scripts (Python etc.) for installation 172 | ## in contrast to setup.py, you can choose the destination 173 | # catkin_install_python(PROGRAMS 174 | # scripts/my_python_script 175 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 176 | # ) 177 | 178 | ## Mark executables for installation 179 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html 180 | # install(TARGETS ${PROJECT_NAME}_node 181 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 182 | # ) 183 | 184 | ## Mark libraries for installation 185 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html 186 | # install(TARGETS ${PROJECT_NAME} 187 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 188 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 189 | # RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} 190 | # ) 191 | 192 | ## Mark cpp header files for installation 193 | # install(DIRECTORY include/${PROJECT_NAME}/ 194 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 195 | # FILES_MATCHING PATTERN "*.h" 196 | # PATTERN ".svn" EXCLUDE 197 | # ) 198 | 199 | ## Mark other files for installation (e.g. launch and bag files, etc.) 200 | # install(FILES 201 | # # myfile1 202 | # # myfile2 203 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 204 | # ) 205 | roslaunch_add_file_check(launch) 206 | 207 | ############# 208 | ## Testing ## 209 | ############# 210 | 211 | ## Add gtest based cpp test target and link libraries 212 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_sl_hw5.cpp) 213 | # if(TARGET ${PROJECT_NAME}-test) 214 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 215 | # endif() 216 | 217 | ## Add folders to be run by python nosetests 218 | # catkin_add_nosetests(test) 219 | -------------------------------------------------------------------------------- /src/sl_hw5/include/lbfgs/lbfgs.hpp: -------------------------------------------------------------------------------- 1 | // [Homework 2 - Part 1] LBFGS algorithm design 2 | // V0.0.1 20220721 tkalpha 3 | 4 | #ifndef LBFGS_HPP 5 | #define LBFGS_HPP 6 | 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | namespace lbfgs { 13 | // ----------------------- Data Type Part ----------------------- 14 | 15 | /** 16 | * L-BFGS optimization parameters. 17 | */ 18 | struct lbfgs_parameter_t { 19 | /** 20 | * The number of corrections to approximate the inverse hessian matrix. 21 | * The L-BFGS routine stores the computation results of previous m 22 | * iterations to approximate the inverse hessian matrix of the current 23 | * iteration. This parameter controls the size of the limited memories 24 | * (corrections). The default value is 8. Values less than 3 are 25 | * not recommended. Large values will result in excessive computing time. 26 | */ 27 | int mem_size = 8; 28 | 29 | /** 30 | * Epsilon for grad convergence test. DO NOT USE IT in nonsmooth cases! 31 | * Set it to 0.0 and use past-delta-based test for nonsmooth functions. 32 | * This parameter determines the accuracy with which the solution is to 33 | * be found. A minimization terminates when 34 | * ||g(x)||_inf / max(1, ||x||_inf) < g_epsilon, 35 | * where ||.||_inf is the infinity norm. The default value is 0.0. 36 | * This should be greater than 1.0e-6 in practice because L-BFGS does 37 | * not directly reduce first-order residual. It still needs the function 38 | * value which can be corrupted by machine_prec when ||g|| is small. 39 | */ 40 | double g_epsilon = 0.0; 41 | 42 | /** 43 | * Distance for delta-based convergence test. 44 | * This parameter determines the distance, in iterations, to compute 45 | * the rate of decrease of the cost function. If the value of this 46 | * parameter is zero, the library does not perform the delta-based 47 | * convergence test. The default value is 3. 48 | */ 49 | int past = 3; 50 | 51 | /** 52 | * Delta for convergence test. 53 | * This parameter determines the minimum rate of decrease of the 54 | * cost function. The library stops iterations when the following 55 | * condition is met: 56 | * |f' - f| / max(1, |f|) < delta, 57 | * where f' is the cost value of past iterations ago, and f is 58 | * the cost value of the current iteration. 59 | * The default value is 1.0e-6. 60 | */ 61 | double delta = 1.0e-6; 62 | 63 | /** 64 | * The maximum number of iterations. 65 | * The lbfgs_optimize() function terminates an minimization process with 66 | * ::LBFGSERR_MAXIMUMITERATION status code when the iteration count 67 | * exceedes this parameter. Setting this parameter to zero continues an 68 | * minimization process until a convergence or error. The default value 69 | * is 0. 70 | */ 71 | int max_iterations = 0; 72 | 73 | /** 74 | * The maximum number of trials for the line search. 75 | * This parameter controls the number of function and gradients evaluations 76 | * per iteration for the line search routine. The default value is 64. 77 | */ 78 | int max_linesearch = 64; 79 | 80 | /** 81 | * The minimum step of the line search routine. 82 | * The default value is 1.0e-20. This value need not be modified unless 83 | * the exponents are too large for the machine being used, or unless the 84 | * problem is extremely badly scaled (in which case the exponents should 85 | * be increased). 86 | */ 87 | double min_step = 1.0e-20; 88 | 89 | /** 90 | * The maximum step of the line search. 91 | * The default value is 1.0e+20. This value need not be modified unless 92 | * the exponents are too large for the machine being used, or unless the 93 | * problem is extremely badly scaled (in which case the exponents should 94 | * be increased). 95 | */ 96 | double max_step = 1.0e+20; 97 | 98 | /** 99 | * A parameter to control the accuracy of the line search routine. 100 | * The default value is 1.0e-4. This parameter should be greater 101 | * than zero and smaller than 1.0. 102 | */ 103 | double f_dec_coeff = 1.0e-4; 104 | 105 | /** 106 | * A parameter to control the accuracy of the line search routine. 107 | * The default value is 0.9. If the function and gradient 108 | * evaluations are inexpensive with respect to the cost of the 109 | * iteration (which is sometimes the case when solving very large 110 | * problems) it may be advantageous to set this parameter to a small 111 | * value. A typical small value is 0.1. This parameter should be 112 | * greater than the f_dec_coeff parameter and smaller than 1.0. 113 | */ 114 | double s_curv_coeff = 0.9; 115 | 116 | /** 117 | * A parameter to ensure the global convergence for nonconvex functions. 118 | * The default value is 1.0e-6. The parameter performs the so called 119 | * cautious update for L-BFGS, especially when the convergence is 120 | * not sufficient. The parameter must be positive but might as well 121 | * be less than 1.0e-3 in practice. 122 | */ 123 | double cautious_factor = 1.0e-6; 124 | 125 | /** 126 | * The machine precision for floating-point values. The default is 1.0e-16. 127 | * This parameter must be a positive value set by a client program to 128 | * estimate the machine precision. 129 | */ 130 | double machine_prec = 1.0e-16; 131 | }; 132 | 133 | /** 134 | * Return values of lbfgs_optimize(). 135 | * Roughly speaking, a negative value indicates an error. 136 | */ 137 | enum { 138 | /** L-BFGS reaches convergence. */ 139 | LBFGS_CONVERGENCE = 0, 140 | /** L-BFGS satisfies stopping criteria. */ 141 | LBFGS_STOP, 142 | /** The iteration has been canceled by the monitor callback. */ 143 | LBFGS_CANCELED, 144 | 145 | /** Unknown error. */ 146 | LBFGSERR_UNKNOWNERROR = -1024, 147 | /** Invalid number of variables specified. */ 148 | LBFGSERR_INVALID_N, 149 | /** Invalid parameter lbfgs_parameter_t::mem_size specified. */ 150 | LBFGSERR_INVALID_MEMSIZE, 151 | /** Invalid parameter lbfgs_parameter_t::g_epsilon specified. */ 152 | LBFGSERR_INVALID_GEPSILON, 153 | /** Invalid parameter lbfgs_parameter_t::past specified. */ 154 | LBFGSERR_INVALID_TESTPERIOD, 155 | /** Invalid parameter lbfgs_parameter_t::delta specified. */ 156 | LBFGSERR_INVALID_DELTA, 157 | /** Invalid parameter lbfgs_parameter_t::min_step specified. */ 158 | LBFGSERR_INVALID_MINSTEP, 159 | /** Invalid parameter lbfgs_parameter_t::max_step specified. */ 160 | LBFGSERR_INVALID_MAXSTEP, 161 | /** Invalid parameter lbfgs_parameter_t::f_dec_coeff specified. */ 162 | LBFGSERR_INVALID_FDECCOEFF, 163 | /** Invalid parameter lbfgs_parameter_t::s_curv_coeff specified. */ 164 | LBFGSERR_INVALID_SCURVCOEFF, 165 | /** Invalid parameter lbfgs_parameter_t::machine_prec specified. */ 166 | LBFGSERR_INVALID_MACHINEPREC, 167 | /** Invalid parameter lbfgs_parameter_t::max_linesearch specified. */ 168 | LBFGSERR_INVALID_MAXLINESEARCH, 169 | /** The function value became NaN or Inf. */ 170 | LBFGSERR_INVALID_FUNCVAL, 171 | /** The line-search step became smaller than lbfgs_parameter_t::min_step. */ 172 | LBFGSERR_MINIMUMSTEP, 173 | /** The line-search step became larger than lbfgs_parameter_t::max_step. */ 174 | LBFGSERR_MAXIMUMSTEP, 175 | /** Line search reaches the maximum, assumptions not satisfied or precision 176 | not achievable.*/ 177 | LBFGSERR_MAXIMUMLINESEARCH, 178 | /** The algorithm routine reaches the maximum number of iterations. */ 179 | LBFGSERR_MAXIMUMITERATION, 180 | /** Relative search interval width is at least 181 | lbfgs_parameter_t::machine_prec. */ 182 | LBFGSERR_WIDTHTOOSMALL, 183 | /** A logic error (negative line-search step) occurred. */ 184 | LBFGSERR_INVALIDPARAMETERS, 185 | /** The current search direction increases the cost function value. */ 186 | LBFGSERR_INCREASEGRADIENT, 187 | }; 188 | 189 | /** 190 | * Callback interface to provide cost function and gradient evaluations. 191 | * 192 | * The lbfgs_optimize() function call this function to obtain the values of 193 | * cost function and its gradients when needed. A client program must implement 194 | * this function to evaluate the values of the cost function and its 195 | * gradients, given current values of variables. 196 | * 197 | * @param instance The user data sent for lbfgs_optimize() function by the 198 | * client. 199 | * @param x The current values of variables. 200 | * @param g The gradient vector. The callback function must compute 201 | * the gradient values for the current variables. 202 | * @retval double The value of the cost function for the current 203 | * variables. 204 | */ 205 | typedef double (*lbfgs_evaluate_t)(void *instance, const Eigen::VectorXd &x, 206 | Eigen::VectorXd &g); 207 | 208 | /** 209 | * Callback interface to monitor the progress of the minimization process. 210 | * 211 | * The lbfgs_optimize() function call this function for each iteration. 212 | * Implementing this function, a client program can store or display the current 213 | * progress of the minimization process. If it is not used, just set it nullptr. 214 | * 215 | * @param instance The user data sent for lbfgs_optimize() function by the 216 | * client. 217 | * @param x The current values of variables. 218 | * @param g The current gradient values of variables. 219 | * @param fx The current value of the cost function. 220 | * @param step The line-search step used for this iteration. 221 | * @param k The iteration count. 222 | * @param ls The number of evaluations called for this iteration. 223 | * @retval int Zero to continue the minimization process. Returning a 224 | * non-zero value will cancel the minimization process. 225 | */ 226 | typedef int (*lbfgs_progress_t)(void *instance, const Eigen::VectorXd &x, 227 | const Eigen::VectorXd &g, const double fx, 228 | const double step, const int k, const int ls); 229 | 230 | /** 231 | * Callback data struct 232 | */ 233 | struct callback_data_t { 234 | void *instance = nullptr; 235 | lbfgs_evaluate_t proc_evaluate = nullptr; 236 | lbfgs_progress_t proc_progress = nullptr; 237 | }; 238 | 239 | // ----------------------- L-BFGS Part ----------------------- 240 | 241 | /** 242 | * Line search method for smooth or nonsmooth functions. 243 | * This function performs line search to find a point that satisfy 244 | * both the Armijo condition and the weak Wolfe condition. It is 245 | * as robust as the backtracking line search but further applies 246 | * to continuous and piecewise smooth functions where the strong 247 | * Wolfe condition usually does not hold. 248 | * 249 | * @see 250 | * Adrian S. Lewis and Michael L. Overton. Nonsmooth optimization 251 | * via quasi-Newton methods. Mathematical Programming, Vol 141, 252 | * No 1, pp. 135-163, 2013. 253 | */ 254 | inline int line_search_lewisoverton( 255 | Eigen::VectorXd &x, double &f, Eigen::VectorXd &g, double &stp, 256 | const Eigen::VectorXd &s, const Eigen::VectorXd &xp, 257 | const Eigen::VectorXd &gp, const double stpmin, const double stpmax, 258 | const callback_data_t &cd, const lbfgs_parameter_t ¶m) { 259 | // x is the decision variable vector 260 | // f is function value at x 261 | // g is the gradient value at x 262 | // stp is the initial stepsize for line search 263 | // s is the search direction vector 264 | // xp is the decision variable vector at the current iteration 265 | // gp is the gradient vector at the current iteration 266 | // stpmin is the minimum allowable stepsize 267 | // stpmax is the maximum allowable stepsize 268 | // the struct param contains all necessary parameters 269 | // the cd contains all necessary callback function 270 | 271 | // eg. x = xp; f = cd.proc_evaluate(cd.instance, x, g); 272 | // the above line assigns x with xp and computes the function and grad at x 273 | 274 | // note the output x, f and g which satisfy the weak wolfe condition when the 275 | // function returns 276 | 277 | //////////////////////////// HOMEWORK 1 START //////////////////////////// 278 | 279 | // PUT YOUR CODE FOR Lewis-Overton line search here 280 | int alpha_found = -1; 281 | double l = stpmin; 282 | double u = stpmax; 283 | 284 | Eigen::VectorXd gp0(gp.size()); 285 | double fp = cd.proc_evaluate(cd.instance, xp, gp0); 286 | int count = 0; 287 | while (count < param.max_linesearch) { 288 | x = xp + stp * s; 289 | f = cd.proc_evaluate(cd.instance, x, g); 290 | double s_alpha = fp - f + param.f_dec_coeff * stp * s.dot(gp); 291 | if (s_alpha < 0) { 292 | u = stp; 293 | } else { 294 | double c_alpha = s.dot(g) - param.s_curv_coeff * s.dot(gp); 295 | if (c_alpha < 0) { 296 | l = stp; 297 | } else { 298 | alpha_found = 0; 299 | break; 300 | } 301 | } 302 | if (u < 1e8) { 303 | stp = 0.5 * (l + u); 304 | } else { 305 | stp = 2.1 * l; 306 | } 307 | ++count; 308 | } 309 | 310 | // std::cout << " 0.0 && param.f_dec_coeff < 1.0)) { 401 | return LBFGSERR_INVALID_FDECCOEFF; 402 | } 403 | if (!(param.s_curv_coeff < 1.0 && param.s_curv_coeff > param.f_dec_coeff)) { 404 | return LBFGSERR_INVALID_SCURVCOEFF; 405 | } 406 | if (!(param.machine_prec > 0.0)) { 407 | return LBFGSERR_INVALID_MACHINEPREC; 408 | } 409 | if (param.max_linesearch <= 0) { 410 | return LBFGSERR_INVALID_MAXLINESEARCH; 411 | } 412 | 413 | /* Prepare intermediate variables. */ 414 | Eigen::VectorXd xp(n); 415 | Eigen::VectorXd g(n); 416 | Eigen::VectorXd gp(n); 417 | Eigen::VectorXd d(n); 418 | Eigen::VectorXd pf(std::max(1, param.past)); 419 | 420 | /* Initialize the limited memory. */ 421 | Eigen::VectorXd lm_alpha = Eigen::VectorXd::Zero(m); 422 | Eigen::MatrixXd lm_s = Eigen::MatrixXd::Zero(n, m); 423 | Eigen::MatrixXd lm_y = Eigen::MatrixXd::Zero(n, m); 424 | Eigen::VectorXd lm_ys = Eigen::VectorXd::Zero(m); 425 | 426 | /* Construct a callback data. */ 427 | callback_data_t cd; 428 | cd.instance = instance; 429 | cd.proc_evaluate = proc_evaluate; 430 | cd.proc_progress = proc_progress; 431 | 432 | /* Evaluate the function value and its gradient. */ 433 | fx = cd.proc_evaluate(cd.instance, x, g); 434 | 435 | /* Store the initial value of the cost function. */ 436 | pf(0) = fx; 437 | 438 | /* 439 | Compute the direction; 440 | we assume the initial hessian matrix H_0 as the identity matrix. 441 | */ 442 | d = -g; 443 | 444 | /* 445 | Make sure that the initial variables are not a stationary point. 446 | */ 447 | gnorm_inf = g.cwiseAbs().maxCoeff(); 448 | xnorm_inf = x.cwiseAbs().maxCoeff(); 449 | 450 | if (gnorm_inf / std::max(1.0, xnorm_inf) < param.g_epsilon) { 451 | /* The initial guess is already a stationary point. */ 452 | ret = LBFGS_CONVERGENCE; 453 | } else { 454 | /* 455 | Compute the initial step: 456 | */ 457 | step = 1.0 / d.norm(); 458 | 459 | k = 1; 460 | end = 0; 461 | bound = 0; 462 | 463 | while (true) { 464 | /* Store the current position and gradient vectors. */ 465 | xp = x; 466 | gp = g; 467 | 468 | /* If the step bound can be provied dynamically, then apply it. */ 469 | step_min = param.min_step; 470 | step_max = param.max_step; 471 | 472 | /* Search for an optimal step. */ 473 | ls = line_search_lewisoverton(x, fx, g, step, d, xp, gp, step_min, 474 | step_max, cd, param); 475 | 476 | if (ls < 0) { 477 | /* Revert to the previous point. */ 478 | x = xp; 479 | g = gp; 480 | ret = ls; 481 | break; 482 | } 483 | 484 | /* Report the progress. */ 485 | if (cd.proc_progress) { 486 | if (cd.proc_progress(cd.instance, x, g, fx, step, k, ls)) { 487 | ret = LBFGS_CANCELED; 488 | break; 489 | } 490 | } 491 | 492 | /* 493 | Convergence test. 494 | The criterion is given by the following formula: 495 | ||g(x)||_inf / max(1, ||x||_inf) < g_epsilon 496 | */ 497 | gnorm_inf = g.cwiseAbs().maxCoeff(); 498 | xnorm_inf = x.cwiseAbs().maxCoeff(); 499 | if (gnorm_inf / std::max(1.0, xnorm_inf) < param.g_epsilon) { 500 | /* Convergence. */ 501 | ret = LBFGS_CONVERGENCE; 502 | break; 503 | } 504 | 505 | /* 506 | Test for stopping criterion. 507 | The criterion is given by the following formula: 508 | |f(past_x) - f(x)| / max(1, |f(x)|) < \delta. 509 | */ 510 | if (0 < param.past) { 511 | /* We don't test the stopping criterion while k < past. */ 512 | if (param.past <= k) { 513 | /* The stopping criterion. */ 514 | rate = 515 | std::fabs(pf(k % param.past) - fx) / std::max(1.0, std::fabs(fx)); 516 | 517 | if (rate < param.delta) { 518 | ret = LBFGS_STOP; 519 | break; 520 | } 521 | } 522 | 523 | /* Store the current value of the cost function. */ 524 | pf(k % param.past) = fx; 525 | } 526 | 527 | if (param.max_iterations != 0 && param.max_iterations <= k) { 528 | /* Maximum number of iterations. */ 529 | ret = LBFGSERR_MAXIMUMITERATION; 530 | break; 531 | } 532 | 533 | /* Count the iteration number. */ 534 | ++k; 535 | 536 | /* 537 | Update vectors s and y: 538 | s_{k+1} = x_{k+1} - x_{k} = \step * d_{k}. 539 | y_{k+1} = g_{k+1} - g_{k}. 540 | */ 541 | lm_s.col(end) = x - xp; 542 | lm_y.col(end) = g - gp; 543 | 544 | /* 545 | Compute scalars ys and yy: 546 | ys = y^t \cdot s = 1 / \rho. 547 | yy = y^t \cdot y. 548 | Notice that yy is used for scaling the hessian matrix H_0 (Cholesky 549 | factor). 550 | */ 551 | ys = lm_y.col(end).dot(lm_s.col(end)); 552 | yy = lm_y.col(end).squaredNorm(); 553 | lm_ys(end) = ys; 554 | 555 | /* Compute the negative of gradients. */ 556 | d = -g; 557 | 558 | /* 559 | Only cautious update is performed here as long as 560 | (y^t \cdot s) / ||s_{k+1}||^2 > \epsilon * ||g_{k}||^\alpha, 561 | where \epsilon is the cautious factor and a proposed value 562 | for \alpha is 1. 563 | This is not for enforcing the PD of the approxomated Hessian 564 | since ys > 0 is already ensured by the weak Wolfe condition. 565 | This is to ensure the global convergence as described in: 566 | Dong-Hui Li and Masao Fukushima. On the global convergence of 567 | the BFGS method for nonconvex unconstrained optimization problems. 568 | SIAM Journal on Optimization, Vol 11, No 4, pp. 1054-1064, 2011. 569 | */ 570 | cau = lm_s.col(end).squaredNorm() * gp.norm() * param.cautious_factor; 571 | 572 | if (ys > cau) { 573 | /* 574 | Recursive formula to compute dir = -(H \cdot g). 575 | This is described in page 779 of: 576 | Jorge Nocedal. 577 | Updating Quasi-Newton Matrices with Limited Storage. 578 | Mathematics of Computation, Vol. 35, No. 151, 579 | pp. 773--782, 1980. 580 | */ 581 | ++bound; 582 | bound = m < bound ? m : bound; 583 | end = (end + 1) % m; 584 | 585 | j = end; 586 | for (i = 0; i < bound; ++i) { 587 | j = (j + m - 1) % m; /* if (--j == -1) j = m-1; */ 588 | /* \alpha_{j} = \rho_{j} s^{t}_{j} \cdot q_{k+1}. */ 589 | lm_alpha(j) = lm_s.col(j).dot(d) / lm_ys(j); 590 | /* q_{i} = q_{i+1} - \alpha_{i} y_{i}. */ 591 | d += (-lm_alpha(j)) * lm_y.col(j); 592 | } 593 | 594 | d *= ys / yy; 595 | 596 | for (i = 0; i < bound; ++i) { 597 | /* \beta_{j} = \rho_{j} y^t_{j} \cdot \gamm_{i}. */ 598 | beta = lm_y.col(j).dot(d) / lm_ys(j); 599 | /* \gamm_{i+1} = \gamm_{i} + (\alpha_{j} - \beta_{j}) s_{j}. */ 600 | d += (lm_alpha(j) - beta) * lm_s.col(j); 601 | j = (j + 1) % m; /* if (++j == m) j = 0; */ 602 | } 603 | } 604 | 605 | /* The search direction d is ready. We try step = 1 first. */ 606 | step = 1.0; 607 | } 608 | } 609 | 610 | /* Return the final value of the cost function. */ 611 | f = fx; 612 | 613 | return ret; 614 | } 615 | 616 | /** 617 | * Get string description of an lbfgs_optimize() return code. 618 | * 619 | * @param err A value returned by lbfgs_optimize(). 620 | */ 621 | inline const char *lbfgs_strerror(const int err) { 622 | switch (err) { 623 | case LBFGS_CONVERGENCE: 624 | return "Success: reached convergence (g_epsilon)."; 625 | 626 | case LBFGS_STOP: 627 | return "Success: met stopping criteria (past f decrease less than " 628 | "delta)."; 629 | 630 | case LBFGS_CANCELED: 631 | return "The iteration has been canceled by the monitor callback."; 632 | 633 | case LBFGSERR_UNKNOWNERROR: 634 | return "Unknown error."; 635 | 636 | case LBFGSERR_INVALID_N: 637 | return "Invalid number of variables specified."; 638 | 639 | case LBFGSERR_INVALID_MEMSIZE: 640 | return "Invalid parameter lbfgs_parameter_t::mem_size specified."; 641 | 642 | case LBFGSERR_INVALID_GEPSILON: 643 | return "Invalid parameter lbfgs_parameter_t::g_epsilon specified."; 644 | 645 | case LBFGSERR_INVALID_TESTPERIOD: 646 | return "Invalid parameter lbfgs_parameter_t::past specified."; 647 | 648 | case LBFGSERR_INVALID_DELTA: 649 | return "Invalid parameter lbfgs_parameter_t::delta specified."; 650 | 651 | case LBFGSERR_INVALID_MINSTEP: 652 | return "Invalid parameter lbfgs_parameter_t::min_step specified."; 653 | 654 | case LBFGSERR_INVALID_MAXSTEP: 655 | return "Invalid parameter lbfgs_parameter_t::max_step specified."; 656 | 657 | case LBFGSERR_INVALID_FDECCOEFF: 658 | return "Invalid parameter lbfgs_parameter_t::f_dec_coeff specified."; 659 | 660 | case LBFGSERR_INVALID_SCURVCOEFF: 661 | return "Invalid parameter lbfgs_parameter_t::s_curv_coeff specified."; 662 | 663 | case LBFGSERR_INVALID_MACHINEPREC: 664 | return "Invalid parameter lbfgs_parameter_t::machine_prec specified."; 665 | 666 | case LBFGSERR_INVALID_MAXLINESEARCH: 667 | return "Invalid parameter lbfgs_parameter_t::max_linesearch specified."; 668 | 669 | case LBFGSERR_INVALID_FUNCVAL: 670 | return "The function value became NaN or Inf."; 671 | 672 | case LBFGSERR_MINIMUMSTEP: 673 | return "The line-search step became smaller than " 674 | "lbfgs_parameter_t::min_step."; 675 | 676 | case LBFGSERR_MAXIMUMSTEP: 677 | return "The line-search step became larger than " 678 | "lbfgs_parameter_t::max_step."; 679 | 680 | case LBFGSERR_MAXIMUMLINESEARCH: 681 | return "Line search reaches the maximum try number, assumptions not " 682 | "satisfied or precision not achievable."; 683 | 684 | case LBFGSERR_MAXIMUMITERATION: 685 | return "The algorithm routine reaches the maximum number of iterations."; 686 | 687 | case LBFGSERR_WIDTHTOOSMALL: 688 | return "Relative search interval width is at least " 689 | "lbfgs_parameter_t::machine_prec."; 690 | 691 | case LBFGSERR_INVALIDPARAMETERS: 692 | return "A logic error (negative line-search step) occurred."; 693 | 694 | case LBFGSERR_INCREASEGRADIENT: 695 | return "The current search direction increases the cost function value."; 696 | 697 | default: 698 | return "(unknown)"; 699 | } 700 | } 701 | 702 | } // namespace lbfgs 703 | 704 | #endif 705 | -------------------------------------------------------------------------------- /src/sl_hw5/include/lbfgs/lbfgs_bak.hpp: -------------------------------------------------------------------------------- 1 | // [Homework 2 - Part 1] LBFGS algorithm design 2 | // V0.0.1 20220721 tkalpha 3 | 4 | #ifndef LBFGS_HPP 5 | #define LBFGS_HPP 6 | 7 | // #include 8 | 9 | #include 10 | #include 11 | #include 12 | 13 | namespace lbfgs { 14 | // ----------------------- Data Type Part ----------------------- 15 | 16 | /** 17 | * L-BFGS optimization parameters. 18 | */ 19 | struct lbfgs_parameter_t { 20 | /** 21 | * The number of corrections to approximate the inverse hessian matrix. 22 | * The L-BFGS routine stores the computation results of previous m 23 | * iterations to approximate the inverse hessian matrix of the current 24 | * iteration. This parameter controls the size of the limited memories 25 | * (corrections). The default value is 8. Values less than 3 are 26 | * not recommended. Large values will result in excessive computing time. 27 | */ 28 | int mem_size = 8; 29 | 30 | /** 31 | * Epsilon for grad convergence test. DO NOT USE IT in nonsmooth cases! 32 | * Set it to 0.0 and use past-delta-based test for nonsmooth functions. 33 | * This parameter determines the accuracy with which the solution is to 34 | * be found. A minimization terminates when 35 | * ||g(x)||_inf / max(1, ||x||_inf) < g_epsilon, 36 | * where ||.||_inf is the infinity norm. The default value is 0.0. 37 | * This should be greater than 1.0e-6 in practice because L-BFGS does 38 | * not directly reduce first-order residual. It still needs the function 39 | * value which can be corrupted by machine_prec when ||g|| is small. 40 | */ 41 | double g_epsilon = 0.0; 42 | 43 | /** 44 | * Distance for delta-based convergence test. 45 | * This parameter determines the distance, in iterations, to compute 46 | * the rate of decrease of the cost function. If the value of this 47 | * parameter is zero, the library does not perform the delta-based 48 | * convergence test. The default value is 3. 49 | */ 50 | int past = 3; 51 | 52 | /** 53 | * Delta for convergence test. 54 | * This parameter determines the minimum rate of decrease of the 55 | * cost function. The library stops iterations when the following 56 | * condition is met: 57 | * |f' - f| / max(1, |f|) < delta, 58 | * where f' is the cost value of past iterations ago, and f is 59 | * the cost value of the current iteration. 60 | * The default value is 1.0e-6. 61 | */ 62 | double delta = 1.0e-6; 63 | 64 | /** 65 | * The maximum number of iterations. 66 | * The lbfgs_optimize() function terminates an minimization process with 67 | * ::LBFGSERR_MAXIMUMITERATION status code when the iteration count 68 | * exceedes this parameter. Setting this parameter to zero continues an 69 | * minimization process until a convergence or error. The default value 70 | * is 0. 71 | */ 72 | int max_iterations = 0; 73 | 74 | /** 75 | * The maximum number of trials for the line search. 76 | * This parameter controls the number of function and gradients evaluations 77 | * per iteration for the line search routine. The default value is 64. 78 | */ 79 | int max_linesearch = 64; 80 | 81 | /** 82 | * The minimum step of the line search routine. 83 | * The default value is 1.0e-20. This value need not be modified unless 84 | * the exponents are too large for the machine being used, or unless the 85 | * problem is extremely badly scaled (in which case the exponents should 86 | * be increased). 87 | */ 88 | double min_step = 1.0e-20; 89 | 90 | /** 91 | * The maximum step of the line search. 92 | * The default value is 1.0e+20. This value need not be modified unless 93 | * the exponents are too large for the machine being used, or unless the 94 | * problem is extremely badly scaled (in which case the exponents should 95 | * be increased). 96 | */ 97 | double max_step = 1.0e+20; 98 | 99 | /** 100 | * A parameter to control the accuracy of the line search routine. 101 | * The default value is 1.0e-4. This parameter should be greater 102 | * than zero and smaller than 1.0. 103 | */ 104 | double f_dec_coeff = 1.0e-4; 105 | 106 | /** 107 | * A parameter to control the accuracy of the line search routine. 108 | * The default value is 0.9. If the function and gradient 109 | * evaluations are inexpensive with respect to the cost of the 110 | * iteration (which is sometimes the case when solving very large 111 | * problems) it may be advantageous to set this parameter to a small 112 | * value. A typical small value is 0.1. This parameter should be 113 | * greater than the f_dec_coeff parameter and smaller than 1.0. 114 | */ 115 | double s_curv_coeff = 0.9; 116 | 117 | /** 118 | * A parameter to ensure the global convergence for nonconvex functions. 119 | * The default value is 1.0e-6. The parameter performs the so called 120 | * cautious update for L-BFGS, especially when the convergence is 121 | * not sufficient. The parameter must be positive but might as well 122 | * be less than 1.0e-3 in practice. 123 | */ 124 | double cautious_factor = 1.0e-6; 125 | 126 | /** 127 | * The machine precision for floating-point values. The default is 1.0e-16. 128 | * This parameter must be a positive value set by a client program to 129 | * estimate the machine precision. 130 | */ 131 | double machine_prec = 1.0e-16; 132 | }; 133 | 134 | /** 135 | * Return values of lbfgs_optimize(). 136 | * Roughly speaking, a negative value indicates an error. 137 | */ 138 | enum { 139 | /** L-BFGS reaches convergence. */ 140 | LBFGS_CONVERGENCE = 0, 141 | /** L-BFGS satisfies stopping criteria. */ 142 | LBFGS_STOP, 143 | /** The iteration has been canceled by the monitor callback. */ 144 | LBFGS_CANCELED, 145 | 146 | /** Unknown error. */ 147 | LBFGSERR_UNKNOWNERROR = -1024, 148 | /** Invalid number of variables specified. */ 149 | LBFGSERR_INVALID_N, 150 | /** Invalid parameter lbfgs_parameter_t::mem_size specified. */ 151 | LBFGSERR_INVALID_MEMSIZE, 152 | /** Invalid parameter lbfgs_parameter_t::g_epsilon specified. */ 153 | LBFGSERR_INVALID_GEPSILON, 154 | /** Invalid parameter lbfgs_parameter_t::past specified. */ 155 | LBFGSERR_INVALID_TESTPERIOD, 156 | /** Invalid parameter lbfgs_parameter_t::delta specified. */ 157 | LBFGSERR_INVALID_DELTA, 158 | /** Invalid parameter lbfgs_parameter_t::min_step specified. */ 159 | LBFGSERR_INVALID_MINSTEP, 160 | /** Invalid parameter lbfgs_parameter_t::max_step specified. */ 161 | LBFGSERR_INVALID_MAXSTEP, 162 | /** Invalid parameter lbfgs_parameter_t::f_dec_coeff specified. */ 163 | LBFGSERR_INVALID_FDECCOEFF, 164 | /** Invalid parameter lbfgs_parameter_t::s_curv_coeff specified. */ 165 | LBFGSERR_INVALID_SCURVCOEFF, 166 | /** Invalid parameter lbfgs_parameter_t::machine_prec specified. */ 167 | LBFGSERR_INVALID_MACHINEPREC, 168 | /** Invalid parameter lbfgs_parameter_t::max_linesearch specified. */ 169 | LBFGSERR_INVALID_MAXLINESEARCH, 170 | /** The function value became NaN or Inf. */ 171 | LBFGSERR_INVALID_FUNCVAL, 172 | /** The line-search step became smaller than lbfgs_parameter_t::min_step. */ 173 | LBFGSERR_MINIMUMSTEP, 174 | /** The line-search step became larger than lbfgs_parameter_t::max_step. */ 175 | LBFGSERR_MAXIMUMSTEP, 176 | /** Line search reaches the maximum, assumptions not satisfied or precision 177 | not achievable.*/ 178 | LBFGSERR_MAXIMUMLINESEARCH, 179 | /** The algorithm routine reaches the maximum number of iterations. */ 180 | LBFGSERR_MAXIMUMITERATION, 181 | /** Relative search interval width is at least 182 | lbfgs_parameter_t::machine_prec. */ 183 | LBFGSERR_WIDTHTOOSMALL, 184 | /** A logic error (negative line-search step) occurred. */ 185 | LBFGSERR_INVALIDPARAMETERS, 186 | /** The current search direction increases the cost function value. */ 187 | LBFGSERR_INCREASEGRADIENT, 188 | }; 189 | 190 | /** 191 | * Callback interface to provide cost function and gradient evaluations. 192 | * 193 | * The lbfgs_optimize() function call this function to obtain the values of 194 | * cost function and its gradients when needed. A client program must implement 195 | * this function to evaluate the values of the cost function and its 196 | * gradients, given current values of variables. 197 | * 198 | * @param instance The user data sent for lbfgs_optimize() function by the 199 | * client. 200 | * @param x The current values of variables. 201 | * @param g The gradient vector. The callback function must compute 202 | * the gradient values for the current variables. 203 | * @retval double The value of the cost function for the current 204 | * variables. 205 | */ 206 | typedef double (*lbfgs_evaluate_t)(void *instance, const Eigen::VectorXd &x, 207 | Eigen::VectorXd &g); 208 | 209 | /** 210 | * Callback interface to monitor the progress of the minimization process. 211 | * 212 | * The lbfgs_optimize() function call this function for each iteration. 213 | * Implementing this function, a client program can store or display the current 214 | * progress of the minimization process. If it is not used, just set it nullptr. 215 | * 216 | * @param instance The user data sent for lbfgs_optimize() function by the 217 | * client. 218 | * @param x The current values of variables. 219 | * @param g The current gradient values of variables. 220 | * @param fx The current value of the cost function. 221 | * @param step The line-search step used for this iteration. 222 | * @param k The iteration count. 223 | * @param ls The number of evaluations called for this iteration. 224 | * @retval int Zero to continue the minimization process. Returning a 225 | * non-zero value will cancel the minimization process. 226 | */ 227 | typedef int (*lbfgs_progress_t)(void *instance, const Eigen::VectorXd &x, 228 | const Eigen::VectorXd &g, const double fx, 229 | const double step, const int k, const int ls); 230 | 231 | /** 232 | * Callback data struct 233 | */ 234 | struct callback_data_t { 235 | void *instance = nullptr; 236 | lbfgs_evaluate_t proc_evaluate = nullptr; 237 | lbfgs_progress_t proc_progress = nullptr; 238 | }; 239 | 240 | // ----------------------- L-BFGS Part ----------------------- 241 | 242 | /** 243 | * Line search method for smooth or nonsmooth functions. 244 | * This function performs line search to find a point that satisfy 245 | * both the Armijo condition and the weak Wolfe condition. It is 246 | * as robust as the backtracking line search but further applies 247 | * to continuous and piecewise smooth functions where the strong 248 | * Wolfe condition usually does not hold. 249 | * 250 | * @see 251 | * Adrian S. Lewis and Michael L. Overton. Nonsmooth optimization 252 | * via quasi-Newton methods. Mathematical Programming, Vol 141, 253 | * No 1, pp. 135-163, 2013. 254 | */ 255 | inline int line_search_lewisoverton( 256 | Eigen::VectorXd &x, double &f, Eigen::VectorXd &g, double &stp, 257 | const Eigen::VectorXd &s, const Eigen::VectorXd &xp, 258 | const Eigen::VectorXd &gp, const double stpmin, const double stpmax, 259 | const callback_data_t &cd, const lbfgs_parameter_t ¶m) { 260 | // x is the decision variable vector 261 | // f is function value at x 262 | // g is the gradient value at x 263 | // stp is the initial stepsize for line search 264 | // s is the search direction vector 265 | // xp is the decision variable vector at the current iteration 266 | // gp is the gradient vector at the current iteration 267 | // stpmin is the minimum allowable stepsize 268 | // stpmax is the maximum allowable stepsize 269 | // the struct param contains all necessary parameters 270 | // the cd contains all necessary callback function 271 | 272 | // eg. x = xp; f = cd.proc_evaluate(cd.instance, x, g); 273 | // the above line assigns x with xp and computes the function and grad at x 274 | 275 | // note the output x, f and g which satisfy the weak wolfe condition when the 276 | // function returns 277 | 278 | //////////////////////////// HOMEWORK 1 START //////////////////////////// 279 | 280 | // PUT YOUR CODE FOR Lewis-Overton line search here 281 | int alpha_found = -1; 282 | double l = stpmin; 283 | double u = stpmax; 284 | 285 | Eigen::VectorXd gp0(gp.size()); 286 | double fp = cd.proc_evaluate(cd.instance, xp, gp0); 287 | int count = 0, step_state; 288 | while (count < param.max_linesearch) { 289 | step_state = 0; 290 | x = xp + stp * s; 291 | f = cd.proc_evaluate(cd.instance, x, g); 292 | double s_alpha = fp - f + param.f_dec_coeff * stp * s.dot(gp); 293 | if (s_alpha < 0) { 294 | step_state = 1; 295 | u = stp; 296 | } else { 297 | double c_alpha = s.dot(g) - param.s_curv_coeff * s.dot(gp); 298 | if (c_alpha < 0) { 299 | step_state = 2; 300 | l = stp; 301 | } else { 302 | alpha_found = 0; 303 | break; 304 | } 305 | } 306 | if (u < 1e8) { 307 | stp = 0.5 * (l + u); 308 | } else { 309 | stp = 2.1 * l; 310 | } 311 | ++count; 312 | 313 | std::cout << "{{{ step2: stp: "<< stp <<" count: " < 0.0 && param.f_dec_coeff < 1.0)) { 404 | return LBFGSERR_INVALID_FDECCOEFF; 405 | } 406 | if (!(param.s_curv_coeff < 1.0 && param.s_curv_coeff > param.f_dec_coeff)) { 407 | return LBFGSERR_INVALID_SCURVCOEFF; 408 | } 409 | if (!(param.machine_prec > 0.0)) { 410 | return LBFGSERR_INVALID_MACHINEPREC; 411 | } 412 | if (param.max_linesearch <= 0) { 413 | return LBFGSERR_INVALID_MAXLINESEARCH; 414 | } 415 | 416 | /* Prepare intermediate variables. */ 417 | Eigen::VectorXd xp(n); 418 | Eigen::VectorXd g(n); 419 | Eigen::VectorXd gp(n); 420 | Eigen::VectorXd d(n); 421 | Eigen::VectorXd pf(std::max(1, param.past)); 422 | 423 | /* Initialize the limited memory. */ 424 | Eigen::VectorXd lm_alpha = Eigen::VectorXd::Zero(m); 425 | Eigen::MatrixXd lm_s = Eigen::MatrixXd::Zero(n, m); 426 | Eigen::MatrixXd lm_y = Eigen::MatrixXd::Zero(n, m); 427 | Eigen::VectorXd lm_ys = Eigen::VectorXd::Zero(m); 428 | 429 | /* Construct a callback data. */ 430 | callback_data_t cd; 431 | cd.instance = instance; 432 | cd.proc_evaluate = proc_evaluate; 433 | cd.proc_progress = proc_progress; 434 | 435 | /* Evaluate the function value and its gradient. */ 436 | fx = cd.proc_evaluate(cd.instance, x, g); 437 | 438 | /* Store the initial value of the cost function. */ 439 | pf(0) = fx; 440 | 441 | /* 442 | Compute the direction; 443 | we assume the initial hessian matrix H_0 as the identity matrix. 444 | */ 445 | d = -g; 446 | 447 | /* 448 | Make sure that the initial variables are not a stationary point. 449 | */ 450 | gnorm_inf = g.cwiseAbs().maxCoeff(); 451 | xnorm_inf = x.cwiseAbs().maxCoeff(); 452 | 453 | if (gnorm_inf / std::max(1.0, xnorm_inf) < param.g_epsilon) { 454 | /* The initial guess is already a stationary point. */ 455 | ret = LBFGS_CONVERGENCE; 456 | } else { 457 | /* 458 | Compute the initial step: 459 | */ 460 | step = 1.0 / d.norm(); 461 | 462 | k = 1; 463 | end = 0; 464 | bound = 0; 465 | 466 | while (true) { 467 | /* Store the current position and gradient vectors. */ 468 | xp = x; 469 | gp = g; 470 | 471 | /* If the step bound can be provied dynamically, then apply it. */ 472 | step_min = param.min_step; 473 | step_max = param.max_step; 474 | 475 | /* Search for an optimal step. */ 476 | ls = line_search_lewisoverton(x, fx, g, step, d, xp, gp, step_min, 477 | step_max, cd, param); 478 | 479 | if (ls < 0) { 480 | /* Revert to the previous point. */ 481 | x = xp; 482 | g = gp; 483 | ret = ls; 484 | break; 485 | } 486 | 487 | /* Report the progress. */ 488 | if (cd.proc_progress) { 489 | if (cd.proc_progress(cd.instance, x, g, fx, step, k, ls)) { 490 | ret = LBFGS_CANCELED; 491 | break; 492 | } 493 | } 494 | 495 | /* 496 | Convergence test. 497 | The criterion is given by the following formula: 498 | ||g(x)||_inf / max(1, ||x||_inf) < g_epsilon 499 | */ 500 | gnorm_inf = g.cwiseAbs().maxCoeff(); 501 | xnorm_inf = x.cwiseAbs().maxCoeff(); 502 | if (gnorm_inf / std::max(1.0, xnorm_inf) < param.g_epsilon) { 503 | /* Convergence. */ 504 | ret = LBFGS_CONVERGENCE; 505 | break; 506 | } 507 | 508 | /* 509 | Test for stopping criterion. 510 | The criterion is given by the following formula: 511 | |f(past_x) - f(x)| / max(1, |f(x)|) < \delta. 512 | */ 513 | if (0 < param.past) { 514 | /* We don't test the stopping criterion while k < past. */ 515 | if (param.past <= k) { 516 | /* The stopping criterion. */ 517 | rate = 518 | std::fabs(pf(k % param.past) - fx) / std::max(1.0, std::fabs(fx)); 519 | 520 | if (rate < param.delta) { 521 | ret = LBFGS_STOP; 522 | break; 523 | } 524 | } 525 | 526 | /* Store the current value of the cost function. */ 527 | pf(k % param.past) = fx; 528 | } 529 | 530 | if (param.max_iterations != 0 && param.max_iterations <= k) { 531 | /* Maximum number of iterations. */ 532 | ret = LBFGSERR_MAXIMUMITERATION; 533 | break; 534 | } 535 | 536 | /* Count the iteration number. */ 537 | ++k; 538 | 539 | /* 540 | Update vectors s and y: 541 | s_{k+1} = x_{k+1} - x_{k} = \step * d_{k}. 542 | y_{k+1} = g_{k+1} - g_{k}. 543 | */ 544 | lm_s.col(end) = x - xp; 545 | lm_y.col(end) = g - gp; 546 | 547 | /* 548 | Compute scalars ys and yy: 549 | ys = y^t \cdot s = 1 / \rho. 550 | yy = y^t \cdot y. 551 | Notice that yy is used for scaling the hessian matrix H_0 (Cholesky 552 | factor). 553 | */ 554 | ys = lm_y.col(end).dot(lm_s.col(end)); 555 | yy = lm_y.col(end).squaredNorm(); 556 | lm_ys(end) = ys; 557 | 558 | /* Compute the negative of gradients. */ 559 | d = -g; 560 | 561 | /* 562 | Only cautious update is performed here as long as 563 | (y^t \cdot s) / ||s_{k+1}||^2 > \epsilon * ||g_{k}||^\alpha, 564 | where \epsilon is the cautious factor and a proposed value 565 | for \alpha is 1. 566 | This is not for enforcing the PD of the approxomated Hessian 567 | since ys > 0 is already ensured by the weak Wolfe condition. 568 | This is to ensure the global convergence as described in: 569 | Dong-Hui Li and Masao Fukushima. On the global convergence of 570 | the BFGS method for nonconvex unconstrained optimization problems. 571 | SIAM Journal on Optimization, Vol 11, No 4, pp. 1054-1064, 2011. 572 | */ 573 | cau = lm_s.col(end).squaredNorm() * gp.norm() * param.cautious_factor; 574 | 575 | if (ys > cau) { 576 | /* 577 | Recursive formula to compute dir = -(H \cdot g). 578 | This is described in page 779 of: 579 | Jorge Nocedal. 580 | Updating Quasi-Newton Matrices with Limited Storage. 581 | Mathematics of Computation, Vol. 35, No. 151, 582 | pp. 773--782, 1980. 583 | */ 584 | ++bound; 585 | bound = m < bound ? m : bound; 586 | end = (end + 1) % m; 587 | 588 | j = end; 589 | for (i = 0; i < bound; ++i) { 590 | j = (j + m - 1) % m; /* if (--j == -1) j = m-1; */ 591 | /* \alpha_{j} = \rho_{j} s^{t}_{j} \cdot q_{k+1}. */ 592 | lm_alpha(j) = lm_s.col(j).dot(d) / lm_ys(j); 593 | /* q_{i} = q_{i+1} - \alpha_{i} y_{i}. */ 594 | d += (-lm_alpha(j)) * lm_y.col(j); 595 | } 596 | 597 | d *= ys / yy; 598 | 599 | for (i = 0; i < bound; ++i) { 600 | /* \beta_{j} = \rho_{j} y^t_{j} \cdot \gamm_{i}. */ 601 | beta = lm_y.col(j).dot(d) / lm_ys(j); 602 | /* \gamm_{i+1} = \gamm_{i} + (\alpha_{j} - \beta_{j}) s_{j}. */ 603 | d += (lm_alpha(j) - beta) * lm_s.col(j); 604 | j = (j + 1) % m; /* if (++j == m) j = 0; */ 605 | } 606 | } 607 | 608 | /* The search direction d is ready. We try step = 1 first. */ 609 | step = 1.0; 610 | } 611 | } 612 | 613 | /* Return the final value of the cost function. */ 614 | f = fx; 615 | 616 | return ret; 617 | } 618 | 619 | /** 620 | * Get string description of an lbfgs_optimize() return code. 621 | * 622 | * @param err A value returned by lbfgs_optimize(). 623 | */ 624 | inline const char *lbfgs_strerror(const int err) { 625 | switch (err) { 626 | case LBFGS_CONVERGENCE: 627 | return "Success: reached convergence (g_epsilon)."; 628 | 629 | case LBFGS_STOP: 630 | return "Success: met stopping criteria (past f decrease less than " 631 | "delta)."; 632 | 633 | case LBFGS_CANCELED: 634 | return "The iteration has been canceled by the monitor callback."; 635 | 636 | case LBFGSERR_UNKNOWNERROR: 637 | return "Unknown error."; 638 | 639 | case LBFGSERR_INVALID_N: 640 | return "Invalid number of variables specified."; 641 | 642 | case LBFGSERR_INVALID_MEMSIZE: 643 | return "Invalid parameter lbfgs_parameter_t::mem_size specified."; 644 | 645 | case LBFGSERR_INVALID_GEPSILON: 646 | return "Invalid parameter lbfgs_parameter_t::g_epsilon specified."; 647 | 648 | case LBFGSERR_INVALID_TESTPERIOD: 649 | return "Invalid parameter lbfgs_parameter_t::past specified."; 650 | 651 | case LBFGSERR_INVALID_DELTA: 652 | return "Invalid parameter lbfgs_parameter_t::delta specified."; 653 | 654 | case LBFGSERR_INVALID_MINSTEP: 655 | return "Invalid parameter lbfgs_parameter_t::min_step specified."; 656 | 657 | case LBFGSERR_INVALID_MAXSTEP: 658 | return "Invalid parameter lbfgs_parameter_t::max_step specified."; 659 | 660 | case LBFGSERR_INVALID_FDECCOEFF: 661 | return "Invalid parameter lbfgs_parameter_t::f_dec_coeff specified."; 662 | 663 | case LBFGSERR_INVALID_SCURVCOEFF: 664 | return "Invalid parameter lbfgs_parameter_t::s_curv_coeff specified."; 665 | 666 | case LBFGSERR_INVALID_MACHINEPREC: 667 | return "Invalid parameter lbfgs_parameter_t::machine_prec specified."; 668 | 669 | case LBFGSERR_INVALID_MAXLINESEARCH: 670 | return "Invalid parameter lbfgs_parameter_t::max_linesearch specified."; 671 | 672 | case LBFGSERR_INVALID_FUNCVAL: 673 | return "The function value became NaN or Inf."; 674 | 675 | case LBFGSERR_MINIMUMSTEP: 676 | return "The line-search step became smaller than " 677 | "lbfgs_parameter_t::min_step."; 678 | 679 | case LBFGSERR_MAXIMUMSTEP: 680 | return "The line-search step became larger than " 681 | "lbfgs_parameter_t::max_step."; 682 | 683 | case LBFGSERR_MAXIMUMLINESEARCH: 684 | return "Line search reaches the maximum try number, assumptions not " 685 | "satisfied or precision not achievable."; 686 | 687 | case LBFGSERR_MAXIMUMITERATION: 688 | return "The algorithm routine reaches the maximum number of iterations."; 689 | 690 | case LBFGSERR_WIDTHTOOSMALL: 691 | return "Relative search interval width is at least " 692 | "lbfgs_parameter_t::machine_prec."; 693 | 694 | case LBFGSERR_INVALIDPARAMETERS: 695 | return "A logic error (negative line-search step) occurred."; 696 | 697 | case LBFGSERR_INCREASEGRADIENT: 698 | return "The current search direction increases the cost function value."; 699 | 700 | default: 701 | return "(unknown)"; 702 | } 703 | } 704 | 705 | } // namespace lbfgs 706 | 707 | #endif 708 | -------------------------------------------------------------------------------- /src/sl_hw5/include/ros1_disp/rviz_dis.hpp: -------------------------------------------------------------------------------- 1 | // class to draw in rviz 1 2 | // V0.0.1 20220825 tkalpha 3 | #ifndef RVIZ_DIS_01 4 | #define RVIZ_DIS_01 5 | #include 6 | #include 7 | 8 | #include 9 | #include 10 | #include 11 | 12 | typedef Eigen::MatrixXd Mat; 13 | typedef Eigen::VectorXd Vec; 14 | 15 | struct colorSimp { 16 | double r; 17 | double g; 18 | double b; 19 | double a; 20 | colorSimp(double r0 = 0.5, double g0 = 0.6, double b0 = 0.5, double a0 = 1.0) : r(r0), g(g0), b(b0), a(a0) {} 21 | ~colorSimp() {} 22 | }; 23 | 24 | struct rviz1DisSimp { 25 | ros::NodeHandle n; 26 | ros::Publisher pub; 27 | ros::Rate r; 28 | std::string p_name; 29 | std::string f_name; 30 | std::string ns; 31 | visualization_msgs::MarkerArray ma; 32 | int id0; 33 | rviz1DisSimp(ros::NodeHandle& nh, const std::string publisher_name, const std::string frame_name, const std::string name_space) 34 | : n(nh), r(30), p_name(publisher_name), f_name(frame_name), ns(name_space), id0(0) { 35 | pub = n.advertise(publisher_name, 10); 36 | while (pub.getNumSubscribers() == 0) { 37 | r.sleep(); 38 | } 39 | } 40 | virtual ~rviz1DisSimp() {} 41 | 42 | visualization_msgs::Marker get_default_marker(int id) { 43 | visualization_msgs::Marker m; 44 | m.header.frame_id = f_name; 45 | m.header.stamp = ros::Time::now(); 46 | m.ns = ns; 47 | m.action = visualization_msgs::Marker::ADD; 48 | m.id = id; 49 | m.type = visualization_msgs::Marker::LINE_STRIP; 50 | m.pose.orientation.w = 1.0; 51 | m.scale.x = 0.2; 52 | m.color.r = m.color.g = m.color.b = m.color.a = 1.0; 53 | return m; 54 | } 55 | 56 | int add_convex_poly_flat(int id, Mat& vertex, double height, colorSimp c = colorSimp()) { 57 | visualization_msgs::Marker m = get_default_marker(id); 58 | m.type = visualization_msgs::Marker::TRIANGLE_LIST; 59 | m.scale.x = m.scale.y = m.scale.z = 1.0; 60 | geometry_msgs::Point pl; 61 | std_msgs::ColorRGBA c1; 62 | c1.r = c.r; 63 | c1.g = c.g; 64 | c1.b = c.b; 65 | c1.a = c.a; 66 | m.points.clear(); 67 | m.colors.clear(); 68 | int num = vertex.rows(); 69 | // add the display of top and bottom surfaces 70 | for (int k = 1; k <= num - 2; ++k) { 71 | pl.z = 0.0; 72 | pl.x = vertex(0, 0); 73 | pl.y = vertex(0, 1); 74 | m.points.push_back(pl); 75 | pl.x = vertex(k, 0); 76 | pl.y = vertex(k, 1); 77 | m.points.push_back(pl); 78 | pl.x = vertex(k + 1, 0); 79 | pl.y = vertex(k + 1, 1); 80 | m.points.push_back(pl); 81 | m.colors.push_back(c1); 82 | pl.z = height; 83 | pl.x = vertex(0, 0); 84 | pl.y = vertex(0, 1); 85 | m.points.push_back(pl); 86 | pl.x = vertex(k, 0); 87 | pl.y = vertex(k, 1); 88 | m.points.push_back(pl); 89 | pl.x = vertex(k + 1, 0); 90 | pl.y = vertex(k + 1, 1); 91 | m.points.push_back(pl); 92 | m.colors.push_back(c1); 93 | } 94 | std_msgs::ColorRGBA c2; 95 | c2.r = c.r * 0.8; 96 | c2.g = c.g * 0.8; 97 | c2.b = c.b * 0.8; 98 | c2.a = c.a; 99 | // add the display of surfaces on the side 100 | int j = num - 1; 101 | for (int k = 0; k <= num - 1; ++k) { 102 | pl.z = height; 103 | pl.x = vertex(k, 0); 104 | pl.y = vertex(k, 1); 105 | m.points.push_back(pl); 106 | pl.z = 0.0; 107 | m.points.push_back(pl); 108 | pl.x = vertex(j, 0); 109 | pl.y = vertex(j, 1); 110 | m.points.push_back(pl); 111 | m.colors.push_back(c2); 112 | m.points.push_back(pl); 113 | pl.z = height; 114 | m.points.push_back(pl); 115 | pl.x = vertex(k, 0); 116 | pl.y = vertex(k, 1); 117 | m.points.push_back(pl); 118 | m.colors.push_back(c2); 119 | j = k; 120 | } 121 | ma.markers.push_back(m); 122 | return 0; 123 | } 124 | 125 | int add_cylinder(int id, double x0, double y0, double r0, double height, colorSimp c = colorSimp()) { 126 | visualization_msgs::Marker m = get_default_marker(id); 127 | m.type = visualization_msgs::Marker::CYLINDER; 128 | m.color.r = c.r; 129 | m.color.g = c.g; 130 | m.color.b = c.b; 131 | m.color.a = c.a; 132 | m.pose.position.x = x0; 133 | m.pose.position.y = y0; 134 | m.scale.x = m.scale.y = r0 * 2; 135 | m.scale.z = height; 136 | ma.markers.push_back(m); 137 | return 0; 138 | } 139 | 140 | int add_path_strip2d(int id, Mat& path, double width, double height, colorSimp c = colorSimp()) { 141 | visualization_msgs::Marker m = get_default_marker(id); 142 | m.type = visualization_msgs::Marker::LINE_STRIP; 143 | m.color.r = c.r; 144 | m.color.g = c.g; 145 | m.color.b = c.b; 146 | m.color.a = c.a; 147 | m.points.clear(); 148 | m.scale.x = width; 149 | geometry_msgs::Point pl; 150 | pl.z = height; 151 | for (int k = 0; k < path.rows(); ++k) { 152 | pl.x = path(k, 0); 153 | pl.y = path(k, 1); 154 | m.points.push_back(pl); 155 | } 156 | ma.markers.push_back(m); 157 | return 0; 158 | } 159 | 160 | int add_scattered2d(int id, Mat& points, double diameter, double height, colorSimp c = colorSimp()) { 161 | visualization_msgs::Marker m = get_default_marker(id); 162 | m.type = visualization_msgs::Marker::SPHERE_LIST; 163 | m.color.r = c.r; 164 | m.color.g = c.g; 165 | m.color.b = c.b; 166 | m.color.a = c.a; 167 | m.points.clear(); 168 | m.scale.x = m.scale.y = m.scale.z = diameter; 169 | geometry_msgs::Point pl; 170 | pl.z = height; 171 | for (int k = 0; k < points.rows(); ++k) { 172 | pl.x = points(k, 0); 173 | pl.y = points(k, 1); 174 | m.points.push_back(pl); 175 | } 176 | ma.markers.push_back(m); 177 | return 0; 178 | } 179 | 180 | int add_colored_path_strip2d(int id, Mat& path, Vec& val, double val_min, double val_max, double width, 181 | double height, colorSimp c_min = colorSimp(0.8, 0.2, 0.2, 1.0), colorSimp c_max = colorSimp(1.0, 1.0, 0.2, 1.0)) { 182 | visualization_msgs::Marker m = get_default_marker(id); 183 | m.type = visualization_msgs::Marker::LINE_STRIP; 184 | m.color.r = 1.0; 185 | m.color.g = 1.0; 186 | m.color.b = 1.0; 187 | m.color.a = 1.0; 188 | m.points.clear(); 189 | m.scale.x = width; 190 | geometry_msgs::Point pl; 191 | std_msgs::ColorRGBA cl; 192 | pl.z = height; 193 | for (int k = 0; k < path.rows() - 1; ++k) { 194 | pl.x = path(k, 0); 195 | pl.y = path(k, 1); 196 | double cl_pos = std::min(1.0, std::max(0.0, (val(k) - val_min) / (val_max - val_min))); 197 | cl.r = c_min.r + (c_max.r - c_min.r) * cl_pos; 198 | cl.g = c_min.g + (c_max.g - c_min.g) * cl_pos; 199 | cl.b = c_min.b + (c_max.b - c_min.b) * cl_pos; 200 | cl.a = 1.0; 201 | m.points.push_back(pl); 202 | m.colors.push_back(cl); 203 | } 204 | ma.markers.push_back(m); 205 | return 0; 206 | } 207 | 208 | int add_arrow(int id, double x0, double y0, double z0, double x1, double y1, double z1, double size, colorSimp c = colorSimp(1.0, 0, 0, 1.0)) { 209 | visualization_msgs::Marker m = get_default_marker(id); 210 | m.type = visualization_msgs::Marker::ARROW; 211 | m.color.r = c.r; 212 | m.color.g = c.g; 213 | m.color.b = c.b; 214 | m.color.a = c.a; 215 | m.scale.x = size * 0.5; 216 | m.scale.y = size; 217 | m.scale.z = size; 218 | geometry_msgs::Point pl; 219 | pl.x = x0; 220 | pl.y = y0; 221 | pl.z = z0; 222 | m.points.push_back(pl); 223 | pl.x = x1; 224 | pl.y = y1; 225 | pl.z = z1; 226 | m.points.push_back(pl); 227 | ma.markers.push_back(m); 228 | return 0; 229 | } 230 | 231 | int clear() { 232 | ma.markers.clear(); 233 | id0 = 0; 234 | return 0; 235 | } 236 | int send() { 237 | if (ros::ok()) { 238 | pub.publish(ma); 239 | pub.publish(ma); 240 | } else { 241 | std::cout << "ros NOT ok." << std::endl; 242 | } 243 | return 0; 244 | } 245 | }; 246 | 247 | #endif // RVIZ_DIS_01 -------------------------------------------------------------------------------- /src/sl_hw5/include/ros1_disp/scene_dis.hpp: -------------------------------------------------------------------------------- 1 | #ifndef SCENE_DISP_01 2 | #define SCENE_DISP_01 3 | 4 | #include "rviz_dis.hpp" 5 | #include "scene/scene_astar.hpp" 6 | 7 | struct sceneAstarDisp { 8 | sceneAstar& sa; 9 | rviz1DisSimp& r1ds; 10 | sceneAstarDisp(rviz1DisSimp&_r1ds, sceneAstar& _sa) : 11 | sa(_sa), r1ds(_r1ds){} 12 | int clear(){ 13 | return r1ds.clear(); 14 | } 15 | int send(){ 16 | return r1ds.send(); 17 | } 18 | int add_scene_grid(){ 19 | Mat points(sa.num_x*sa.num_y, 2); 20 | for(size_t i = 0; itype == 0){ 41 | auto ob_ptr1 = (cylinderObstacle*) ob_ptr; 42 | r1ds.add_cylinder(r1ds.id0++, ob_ptr1->x, ob_ptr1->y, ob_ptr1->r, 0.2); 43 | } else { 44 | auto ob_ptr1 = (polyObstacle*) ob_ptr; 45 | r1ds.add_convex_poly_flat(r1ds.id0++, ob_ptr1->poly, 0.2); 46 | } 47 | } 48 | return 0; 49 | } 50 | 51 | int add_astar_path(){ 52 | return r1ds.add_path_strip2d(r1ds.id0++, sa.astar_path, 0.1, 0.1, {0.0,0.7,0.0,1.0}); 53 | } 54 | }; 55 | 56 | #endif // SCENE_DISP_01 -------------------------------------------------------------------------------- /src/sl_hw5/include/ros1_disp/spline_opt_dis.hpp: -------------------------------------------------------------------------------- 1 | #ifndef SPLINE_OPT_DIS_01 2 | #define SPLINE_OPT_DIS_01 3 | 4 | #include "rviz_dis.hpp" 5 | #include "traj_optim/spline_opt.hpp" 6 | 7 | struct splineOptDis{ 8 | cubicSplineOpt& cso; 9 | rviz1DisSimp& r1ds; 10 | splineOptDis(rviz1DisSimp&_r1ds, cubicSplineOpt& _cso) : 11 | cso(_cso), r1ds(_r1ds){} 12 | 13 | int add_lbfgs_path_points(){ 14 | return r1ds.add_scattered2d(r1ds.id0++, cso.points1, 0.3, 0.0, {0.0,1.0,1.0,1.0}); 15 | } 16 | 17 | int add_lbfgs_path(){ 18 | return r1ds.add_path_strip2d(r1ds.id0++, cso.points1, 0.1, 0.1, {0.0,1.0,0.0,1.0}); 19 | } 20 | }; 21 | 22 | #endif // SPLINE_OPT_DIS_01 23 | -------------------------------------------------------------------------------- /src/sl_hw5/include/ros1_disp/topp_dis.hpp: -------------------------------------------------------------------------------- 1 | #ifndef TOPP_DIS_01 2 | #define TOPP_DIS_01 3 | 4 | #include 5 | 6 | #include "rviz_dis.hpp" 7 | #include "traj_optim/conic_alm_topp.hpp" 8 | 9 | struct conicAlmToppDis { 10 | conicALMTOPP2& topp2; 11 | rviz1DisSimp& r1ds; 12 | conicAlmToppDis(rviz1DisSimp& _r1ds, conicALMTOPP2& _topp2) : topp2(_topp2), r1ds(_r1ds) {} 13 | 14 | int add_topp_trajectory_points(double v_min, double v_max) { 15 | Vec a, b, c, d; 16 | topp2.get_result(a, b, c, d); 17 | Vec v = b.cwiseSqrt(); 18 | std::ofstream file; 19 | file.open("/home/tku/1/dtt1_q.txt"); 20 | for(int k = 0; k < (int)topp2.q.rows(); ++k){ 21 | file << topp2.q(k,0)<<", "< 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | 11 | typedef Eigen::MatrixXd Mat; 12 | typedef Eigen::VectorXd Vec; 13 | 14 | struct randPoly { 15 | int RND_MAX = 655369; 16 | std::random_device dev; 17 | std::mt19937 rng; 18 | std::uniform_int_distribution random_numer; 19 | std::uniform_int_distribution random_logic; 20 | 21 | randPoly() : rng(dev()), random_numer(0, RND_MAX), random_logic(0, 1) {} 22 | virtual ~randPoly() {} 23 | 24 | int generate(const int n, const double r0, Mat& poly) { 25 | auto gen = [&]() { return random_numer(rng); }; 26 | // initialize random samples and sort them 27 | // int m = n / 2; 28 | std::vector x(n), y(n), vx(n), vy(n), idx(n); 29 | std::vector a(n); 30 | std::generate(x.begin(), x.end(), gen); 31 | std::generate(y.begin(), y.end(), gen); 32 | std::iota(idx.begin(), idx.end(), 0); 33 | std::sort(x.begin(), x.end()); 34 | std::sort(y.begin(), y.end()); 35 | // divide samples and get vector component 36 | int x0 = x[0], x1 = x0; 37 | for (int k = 1; k < n - 1; ++k) { 38 | if (random_logic(rng)) { 39 | vx[k - 1] = x[k] - x0; 40 | x0 = x[k]; 41 | } else { 42 | vx[k - 1] = x1 - x[k]; 43 | x1 = x[k]; 44 | } 45 | } 46 | vx[n - 2] = x[n - 1] - x0; 47 | vx[n - 1] = x1 - x[n - 1]; 48 | int y0 = y[0], y1 = y0; 49 | for (int k = 1; k < n - 1; ++k) { 50 | if (random_logic(rng)) { 51 | vy[k - 1] = y[k] - y0; 52 | y0 = y[k]; 53 | } else { 54 | vy[k - 1] = y1 - y[k]; 55 | y1 = y[k]; 56 | } 57 | } 58 | vy[n - 2] = y[n - 1] - y0; 59 | vy[n - 1] = y1 - y[n - 1]; 60 | // random pair up vector components and sort by angle 61 | std::shuffle(vy.begin(), vy.end(), rng); 62 | for (int k = 0; k < n; ++k) { 63 | a[k] = std::atan2(vy[k], vx[k]); 64 | } 65 | std::sort(idx.begin(), idx.end(), 66 | [&a](int& lhs, int& rhs) { return a[lhs] < a[rhs]; }); 67 | // form the polygon by connencting vectors 68 | double x_max = 0, y_max = 0, x_min = 0, y_min = 0; 69 | x[0] = y[0] = 0; 70 | for (int k = 1; k < n; ++k) { 71 | x[k] = x[k - 1] + vx[idx[k - 1]]; 72 | y[k] = y[k - 1] + vy[idx[k - 1]]; 73 | if (x[k] > x_max) { 74 | x_max = x[k]; 75 | } else if (x[k] < x_min) { 76 | x_min = x[k]; 77 | } 78 | if (y[k] > y_max) { 79 | y_max = y[k]; 80 | } else if (y[k] < y_min) { 81 | y_min = y[k]; 82 | } 83 | } 84 | // center and resize the polygon 85 | poly.resize(n, 2); 86 | double x_offset = -(x_max + x_min) / 2.0; 87 | double y_offset = -(y_max + y_min) / 2.0; 88 | double scale = 2.0 * r0 / std::max(x_max - x_min, y_max - y_min); 89 | for (int k = 0; k < n; ++k) { 90 | poly(k, 0) = scale * (x[k] + x_offset); 91 | poly(k, 1) = scale * (y[k] + y_offset); 92 | } 93 | return 0; 94 | } 95 | }; 96 | 97 | #endif // PAVEL_VALTR_01 -------------------------------------------------------------------------------- /src/sl_hw5/include/scene/scene_astar.hpp: -------------------------------------------------------------------------------- 1 | #ifndef SCENE_ASTAR_01 2 | #define SCENE_ASTAR_01 3 | 4 | #include 5 | #include 6 | 7 | #include "scene_base.hpp" 8 | #include "scene_obstacles_new.hpp" 9 | 10 | double Inf = 1.0e40; 11 | 12 | // Grid node object used by the astar algorithm 13 | class gridNode { 14 | public: 15 | Eigen::Vector2d pos; 16 | Eigen::Vector2i idx; 17 | int state; 18 | double g, f; 19 | gridNode* father; 20 | gridNode(Eigen::Vector2i _idx, Eigen::Vector2d _pos) 21 | : pos(_pos), idx(_idx), state(0), g(Inf), f(Inf), father(nullptr) {} 22 | ~gridNode() {} 23 | }; 24 | 25 | // The main object ro construct scene and search an initial path by Astar algorithm 26 | class sceneAstar : public sceneBase { 27 | public: 28 | sceneAstar(double _x0, double _y0, double _yaw0, double _lx, 29 | double _ly, double _ld, double _obs_d_min, double _obs_d_max, int _p_min, int _p_max) { 30 | reset_prj(_x0, _y0, _yaw0, _lx, _ly, _ld); 31 | obs_factory_ptr = new randomObstacleFactory1(_p_min, _p_max, 0., lx, 0., ly, _obs_d_min, _obs_d_max, x0, y0, yaw0); 32 | } 33 | ~sceneAstar() { 34 | obs_clear(); 35 | } 36 | 37 | // clear the list of obstacles 38 | int obs_clear() { 39 | for (auto& ob_ptr : obs) { 40 | if (ob_ptr != nullptr) { 41 | delete ob_ptr; 42 | } 43 | } 44 | obs.clear(); 45 | return 0; 46 | } 47 | 48 | // reset the path planning environment and prepare the grid information for 49 | // astar algorithm 50 | int reset_prj(double _x0, double _y0, double _yaw0, double _lx, double _ly, 51 | double _ld) { 52 | x0 = _x0; 53 | y0 = _y0; 54 | yaw0 = _yaw0; 55 | lx = _lx; 56 | ly = _ly; 57 | ld = _ld; 58 | ild = 1.0 / _ld; 59 | num_x = static_cast(std::floor(lx * ild)) + 1; 60 | num_y = static_cast(std::floor(ly * ild)) + 1; 61 | map0 = std::vector>(num_x); 62 | for (size_t i = 0; i < num_x; ++i) { 63 | for (size_t j = 0; j < num_y; ++j) { 64 | Eigen::Vector2i idx0 = {i, j}; 65 | map0[i].emplace_back(idx0, idx2pos(idx0)); 66 | } 67 | } 68 | obs_clear(); 69 | pos_start = {x0, y0}; 70 | pos_goal = map0[num_x - 1][num_y - 1].pos; 71 | 72 | return 0; 73 | } 74 | 75 | // reset the grid map in astar algorithm 76 | int reset_map() { 77 | for (size_t i = 0; i < num_x; ++i) { 78 | for (size_t j = 0; j < num_y; ++j) { 79 | map0[i][j].state = 0; 80 | map0[i][j].father = nullptr; 81 | map0[i][j].g = Inf; 82 | map0[i][j].f = Inf; 83 | } 84 | } 85 | return 0; 86 | } 87 | 88 | // convert grid map index to position in real world 89 | Eigen::Vector2d idx2pos(Eigen::Vector2i idx) { 90 | double lx1 = idx(0) * ld; 91 | double ly1 = idx(1) * ld; 92 | double x1 = x0 + lx1 * std::cos(yaw0) - ly1 * std::sin(yaw0); 93 | double y1 = y0 + lx1 * std::sin(yaw0) + ly1 * std::cos(yaw0); 94 | return {x1, y1}; 95 | } 96 | 97 | // convert position in real world to nearest grid map index 98 | Eigen::Vector2i pos2idx(Eigen::Vector2d pos) { 99 | double lx1 = 100 | (pos(0) - x0) * std::cos(yaw0) + (pos(1) - y0) * std::sin(yaw0); 101 | double ly1 = 102 | (x0 - pos(0)) * std::sin(yaw0) + (pos(1) - y0) * std::cos(yaw0); 103 | int idxx1 = static_cast( 104 | std::max(std::min(std::round(lx1 * ild), num_x - 1.0), 0.0)); 105 | int idxy1 = static_cast( 106 | std::max(std::min(std::round(ly1 * ild), num_y - 1.0), 0.0)); 107 | return {idxx1, idxy1}; 108 | } 109 | 110 | // check if a grid map index is collision free (overloading1) 111 | bool is_free(int ix, int iy) { 112 | if (ix < 0 || ix >= (int)num_x || iy < 0 || iy >= (int)num_y) { 113 | return false; 114 | } 115 | double x = map0[ix][iy].pos(0); 116 | double y = map0[ix][iy].pos(1); 117 | for (size_t k = 0; k < obs.size(); ++k) { 118 | if (!obs[k]->isfree(x, y)) { 119 | return false; 120 | } 121 | } 122 | return true; 123 | } 124 | 125 | // check if a grid map index is collision free (overloading2) 126 | bool is_free(Eigen::Vector2i idx) { 127 | int ix = idx(0); 128 | int iy = idx(1); 129 | return is_free(ix, iy); 130 | } 131 | 132 | // check if a position in real world is collision free (overloading1) 133 | bool is_free(double x, double y) { 134 | double cos_yaw = std::cos(yaw0); 135 | double sin_yaw = std::sin(yaw0); 136 | bool out1 = (x - x0) * sin_yaw - (y - y0) * cos_yaw >= 0; 137 | bool out2 = 138 | (x - x0 - lx * cos_yaw) * cos_yaw + (y - y0 - lx * sin_yaw) * sin_yaw >= 139 | 0; 140 | bool out3 = -(x - x0 - lx * cos_yaw + ly * sin_yaw) * sin_yaw + 141 | (y - y0 - lx * sin_yaw - ly * cos_yaw) * cos_yaw >= 142 | 0; 143 | bool out4 = -(x - x0 + ly * sin_yaw) * cos_yaw - 144 | (y - y0 - ly * cos_yaw) * sin_yaw >= 145 | 0; 146 | if (out1 || out2 || out3 || out4) { 147 | return false; 148 | } 149 | for (size_t k = 0; k < obs.size(); ++k) { 150 | if (!obs[k]->isfree(x, y)) { 151 | return false; 152 | } 153 | } 154 | return true; 155 | } 156 | 157 | // check if a position in real world is collision free (overloading2) 158 | bool is_free(Eigen::Vector2d pos) { 159 | double x = pos(0); 160 | double y = pos(1); 161 | return is_free(x, y); 162 | } 163 | 164 | // random obstacles to the planning environment 165 | int random_obstacles(int _obs_num) { 166 | obs_clear(); 167 | obs_num = _obs_num; 168 | for (int k = 0; k < _obs_num; ++k) { 169 | baseObstacle* ob_ptr; 170 | double dist_min; 171 | do { 172 | ob_ptr = obs_factory_ptr->create(); 173 | dist_min = Inf; 174 | for (int i = 0; i < static_cast(obs.size()); ++i) { 175 | double dist = obs[i]->distance(ob_ptr->x, ob_ptr->y); 176 | if (dist < dist_min) dist_min = dist; 177 | // std::cout << dist << std::endl; 178 | } 179 | 180 | } while (dist_min < 1.15 * ob_ptr->r); 181 | // std::cout << "[dist_min:" << dist_min << " dist_est:" << dist_min_est << ", r:" << ob_ptr->r << ", x:" << ob_ptr->x << ", y:" << ob_ptr->y << " i:" << (dist_min < 1.15 * ob_ptr->r) << "]" << std::endl; 182 | obs.push_back(ob_ptr); 183 | } 184 | return 0; 185 | } 186 | 187 | // set the goal position of path planning tasks 188 | int set_start(double _x, double _y) { 189 | pos_start(0) = _x; 190 | pos_start(1) = _y; 191 | return 0; 192 | } 193 | int set_goal(double _x, double _y) { 194 | pos_goal(0) = _x; 195 | pos_goal(1) = _y; 196 | return 0; 197 | } 198 | 199 | // get the heuristic value of a grid map index in Astar algorithm 200 | double heu(gridNode* n1, gridNode* n2) { 201 | int d_ix = std::abs(n1->idx(0) - n2->idx(0)); 202 | int d_iy = std::abs(n1->idx(1) - n2->idx(1)); 203 | int diag = std::min(d_ix, d_iy); 204 | int parallel = std::max(d_ix, d_iy) - diag; 205 | return (diag * std::sqrt(2) + parallel); 206 | } 207 | 208 | // Astar algorithm to search the initial path 209 | int astar_search() { 210 | reset_map(); 211 | std::multimap opens; 212 | Eigen::Vector2i idx_start = pos2idx(pos_start); 213 | Eigen::Vector2i idx_goal = pos2idx(pos_goal); 214 | gridNode* node_start = &map0[idx_start(0)][idx_start(1)]; 215 | gridNode* node_goal = &map0[idx_goal(0)][idx_goal(1)]; 216 | gridNode* node_current = nullptr; 217 | bool astar_path_found = 0; 218 | node_start->g = 0; 219 | node_start->f = heu(node_start, node_goal); 220 | node_start->state = 1; 221 | opens.insert({node_start->f, node_start}); 222 | while (!opens.empty()) { 223 | node_current = opens.begin()->second; 224 | opens.erase(opens.begin()); 225 | if (node_current->state == -1) { 226 | continue; 227 | } 228 | node_current->state = -1; 229 | if (node_current->idx == idx_goal) { 230 | astar_path_found = 1; 231 | break; 232 | } 233 | for (int d_ix = -1; d_ix <= 1; ++d_ix) { 234 | for (int d_iy = -1; d_iy <= 1; ++d_iy) { 235 | if (d_ix == 0 && d_iy == 0) continue; 236 | int ix1 = d_ix + node_current->idx(0); 237 | int iy1 = d_iy + node_current->idx(1); 238 | if (!is_free(ix1, iy1) || map0[ix1][iy1].state == -1) continue; 239 | gridNode* node_neighbor = &map0[ix1][iy1]; 240 | double edge = std::sqrt(d_ix * d_ix + d_iy * d_iy); 241 | double g = node_current->g + edge; 242 | double f = 243 | g + heu(node_neighbor, node_goal) * (1.0 + 1e-4); // tie breaker 244 | if (node_neighbor->state == 0 || 245 | (node_neighbor->state == 1 && // node unexplored 246 | node_neighbor->g > g)) { // or node in open set with greater g 247 | node_neighbor->g = g; 248 | node_neighbor->f = f; 249 | node_neighbor->father = node_current; 250 | node_neighbor->state = 1; 251 | opens.insert({f, node_neighbor}); 252 | } 253 | } 254 | } 255 | } 256 | if (astar_path_found) { 257 | double DIST_TOLERANCE = 0.6 * ld; 258 | grid_along_path.clear(); 259 | for (gridNode* n = node_current; n != nullptr; n = n->father) { 260 | grid_along_path.push_back(n); 261 | } 262 | size_t num = grid_along_path.size(); 263 | if ((pos_start - grid_along_path[num - 1]->pos).norm() > DIST_TOLERANCE) { 264 | include_start = 1; 265 | } else { 266 | include_start = 0; 267 | } 268 | if ((pos_goal - grid_along_path[0]->pos).norm() > DIST_TOLERANCE) { 269 | include_goal = 1; 270 | } else { 271 | include_goal = 0; 272 | } 273 | astar_path.resize(num + include_start + include_goal, 2); 274 | astar_path.row(0) = pos_start; 275 | if (include_start) { 276 | astar_path.row(1) = grid_along_path[num - 1]->pos; 277 | } 278 | for (size_t i = num - 2; i >= 1; --i) { 279 | astar_path.row(num - i - 1 + include_start) = grid_along_path[i]->pos; 280 | } 281 | if (include_goal) { 282 | astar_path.row(num - 1 + include_start) = grid_along_path[0]->pos; 283 | } 284 | astar_path.row(num - 1 + include_start + include_goal) = pos_goal; 285 | } 286 | 287 | return !astar_path_found; 288 | } 289 | 290 | // random gennerate an Astar path 291 | int random_astar_path() { 292 | double dist_min = 0.7 * (map0[0][0].pos - map0[num_x - 1][num_y - 1].pos).norm(); 293 | std::random_device dev; 294 | std::mt19937 re(dev()); 295 | std::uniform_int_distribution rand_x(0, num_x - 1); 296 | std::uniform_int_distribution rand_y(0, num_y - 1); 297 | double dist = 0, x0, y0, x1, y1; 298 | bool astar_state; 299 | do { 300 | do { 301 | x0 = rand_x(re); 302 | y0 = rand_y(re); 303 | x1 = rand_x(re); 304 | y1 = rand_y(re); 305 | dist = (map0[x0][y0].pos - map0[x1][y1].pos).norm(); 306 | // std::cout << "dist: " << dist << " occu_start:" << !is_free(map0[x0][y0].pos) << " occu_end:" << !is_free(map0[x1][y1].pos) << std::endl; 307 | } while (dist < dist_min || !is_free(map0[x0][y0].pos) || !is_free(map0[x1][y1].pos)); 308 | // std::cout << dist_min/0.7 <<"[goal x0: " << x0 << ", y0: " << y0 << ", x1: " << x1 << ", y1: " << y1 << std::endl; 309 | set_start(map0[x0][y0].pos(0), map0[x0][y0].pos(1)); 310 | set_goal(map0[x1][y1].pos(0), map0[x1][y1].pos(1)); 311 | astar_state = astar_search(); 312 | } while (astar_state); 313 | return astar_state; 314 | } 315 | 316 | // get value and grad info in distance field 317 | double dist_field(const double x1, const double y1, Vec* grad = nullptr) { 318 | double d0 = Inf, d1 = Inf; 319 | Vec g1(2); 320 | *grad << 0., 0.; 321 | #if 1 322 | for (int k = 0; k < static_cast(obs.size()); ++k) { 323 | d1 = obs[k]->distance(x1, y1, &g1); 324 | if (d1 < d0) { 325 | d0 = d1; 326 | if (grad != nullptr) { 327 | grad->operator()(0) = g1(0); 328 | grad->operator()(1) = g1(1); 329 | } 330 | } 331 | } 332 | #else 333 | #endif 334 | return d0; 335 | } 336 | 337 | double x0; 338 | double y0; 339 | double yaw0; 340 | double lx; 341 | double ly; 342 | double ld; 343 | double ild; 344 | int obs_num; 345 | size_t num_x = 0; 346 | size_t num_y = 0; 347 | std::vector> map0; 348 | std::vector obs; 349 | baseObstacleFactory* obs_factory_ptr; 350 | 351 | Eigen::Vector2d pos_start; 352 | Eigen::Vector2d pos_goal; 353 | std::vector grid_along_path; 354 | bool include_start, include_goal; 355 | Mat astar_path; 356 | }; 357 | 358 | #endif // SCENE_ASTAR_01 -------------------------------------------------------------------------------- /src/sl_hw5/include/scene/scene_base.hpp: -------------------------------------------------------------------------------- 1 | #ifndef SCENE_BASE_01 2 | #define SCENE_BASE_01 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | typedef Eigen::MatrixXd Mat; 10 | typedef Eigen::VectorXd Vec; 11 | 12 | // Base class of a scene that can provide artifital distance field information 13 | class sceneBase { 14 | public: 15 | virtual double dist_field(const double x1, const double y1, Vec* grad = nullptr) = 0; 16 | virtual ~sceneBase() {} 17 | }; 18 | 19 | #endif // SCENE_BASE_01 -------------------------------------------------------------------------------- /src/sl_hw5/include/scene/scene_obstacles.hpp: -------------------------------------------------------------------------------- 1 | #ifndef SCENE_OBSTACLES_01 2 | #define SCENE_OBSTACLES_01 3 | 4 | #include 5 | 6 | #include "pavel_valtr.hpp" 7 | #include "sdqp/sdqp.hpp" 8 | 9 | typedef Eigen::MatrixXd Mat; 10 | typedef Eigen::VectorXd Vec; 11 | 12 | // base classes of obstacles 13 | struct baseObstacle { 14 | double x; 15 | double y; 16 | double r; 17 | int type; 18 | virtual bool isfree(const double x1, const double y1) = 0; 19 | virtual double distance(const double x1, const double y1, Vec* grad = nullptr) = 0; 20 | baseObstacle(int _t, double _x, double _y, double _r) : x(_x), y(_y), r(_r), type(_t) {} 21 | virtual ~baseObstacle() {} 22 | }; 23 | 24 | // classes for cylinder obstacle 25 | struct cylinderObstacle : public baseObstacle { 26 | cylinderObstacle(const double _x = 0, const double _y = 0, const double _r = 1) : baseObstacle(0, _x, _y, _r) {} 27 | ~cylinderObstacle(){}; 28 | bool isfree(const double x1, const double y1) { 29 | return ((x1 - x) * (x1 - x) + (y1 - y) * (y1 - y) > r * r); 30 | } 31 | double distance(const double x1, const double y1, Vec* grad = nullptr) { 32 | double d12 = (x1 - x) * (x1 - x) + (y1 - y) * (y1 - y); 33 | double d22 = r * r; 34 | double d; 35 | if (d12 > d22) { 36 | d = std::sqrt(d12) - std::sqrt(d22); 37 | if (grad != nullptr) { 38 | grad->operator()(0) = (x1 - x) / d; 39 | grad->operator()(1) = (y1 - y) / d; 40 | } 41 | } else { 42 | d = 0; 43 | if (grad != nullptr) { 44 | grad->operator()(0) = grad->operator()(1) = 0; 45 | } 46 | } 47 | return d; 48 | } 49 | }; 50 | 51 | // classes for poly prism obstacle 52 | struct polyObstacle : public baseObstacle { 53 | int m; 54 | Mat poly; 55 | Eigen::Matrix Q; 56 | Eigen::Matrix c; 57 | Eigen::Matrix x; 58 | Mat A; 59 | Vec b; 60 | polyObstacle(const Mat& _poly, const double _x = 0, const double _y = 0, const double _r = 1) 61 | : baseObstacle(1, _x, _y, _r), m(_poly.rows()), poly(_poly), Q(Mat::Identity(2, 2) * 2), A(m, 2), b(m) { 62 | int j = m - 1; 63 | for (int k = 0; k < m; ++k) { 64 | A(k, 0) = poly(k, 1) - poly(j, 1); 65 | A(k, 1) = poly(j, 0) - poly(k, 0); 66 | b(k) = A(k, 0) * poly(j, 0) + A(k, 1) * poly(j, 1); 67 | j = k; 68 | } 69 | // std::cout << "[random poly object OK!]" << std::endl; 70 | } 71 | ~polyObstacle(){}; 72 | bool isfree(const double x1, const double y1) { 73 | x(0) = x1; 74 | x(1) = y1; 75 | return (A * x - b).maxCoeff() > 0; 76 | } 77 | double distance(const double x1, const double y1, Vec* grad = nullptr) { 78 | c(0) = -2 * x1; 79 | c(1) = -2 * y1; 80 | sdqp::sdqp<2>(Q, c, A, b, x); 81 | double d, d2 = (x1 - x(0)) * (x1 - x(0)) + (y1 - x(1)) * (y1 - x(1)); 82 | if (d2 > 1e-6) { 83 | d = std::sqrt(d2); 84 | if (grad != nullptr) { 85 | grad->operator()(0) = (x1 - x(0)) / d; 86 | grad->operator()(1) = (y1 - x(1)) / d; 87 | } 88 | } else { 89 | d = 0; 90 | if (grad != nullptr) { 91 | grad->operator()(0) = grad->operator()(1) = 0; 92 | } 93 | } 94 | // std::cout << "[x1:" << x1 << " y1:" << y1 << " x:" << x(0) << " y:" << x(1) << " d:" << d << "]" << std::endl; 95 | return d; 96 | } 97 | }; 98 | 99 | // class for simple factory type1 (random generation in specific range) 100 | 101 | struct baseObstacleFactory { 102 | virtual baseObstacle* create() = 0; 103 | virtual ~baseObstacleFactory() {} 104 | }; 105 | 106 | struct randomObstacleFactory1 : public baseObstacleFactory { 107 | double x0; 108 | double y0; 109 | double yaw0; 110 | randPoly rand_poly; 111 | std::random_device dev; 112 | std::mt19937 re; 113 | std::uniform_real_distribution rand_x; 114 | std::uniform_real_distribution rand_y; 115 | std::uniform_real_distribution rand_d; 116 | std::uniform_int_distribution rand_p; 117 | randomObstacleFactory1(int _p_min, int _p_max, double _x_min, double _x_max, 118 | double _y_min, double _y_max, double _d_min, double _d_max, 119 | double _x0, double _y0, double _yaw0) 120 | : x0(_x0), y0(_y0), yaw0(_yaw0), re(dev()), rand_x(_x_min, _x_max), rand_y(_y_min, _y_max), rand_d(_d_min, _d_max), rand_p(_p_min, _p_max) {} // 121 | baseObstacle* create() { 122 | int p1 = rand_p(re); 123 | 124 | double cos0 = std::cos(yaw0); 125 | double sin0 = std::sin(yaw0); 126 | double d1 = rand_d(re); 127 | 128 | double xr = rand_x(re); 129 | double yr = rand_y(re); 130 | double x1 = x0 + xr * cos0 - yr * sin0; 131 | double y1 = y0 + xr * sin0 + yr * cos0; 132 | 133 | baseObstacle* bo_ptr; 134 | if (p1 <= 3) { 135 | bo_ptr = new cylinderObstacle(x1, y1, d1); 136 | } else { 137 | Mat poly1; 138 | rand_poly.generate(p1, d1, poly1); 139 | // std::cout << "[random poly OK!]" << std::endl; 140 | for (int k = 0; k < poly1.rows(); ++k) { 141 | double dx = poly1(k, 0); 142 | double dy = poly1(k, 1); 143 | poly1(k, 0) = x1 + dx * cos0 - dy * sin0; 144 | poly1(k, 1) = y1 + dx * sin0 + dy * cos0; 145 | } 146 | // std::cout << "[random poly formed OK!]" << std::endl; 147 | bo_ptr = new polyObstacle(poly1, x1, y1, d1); 148 | } 149 | return bo_ptr; 150 | } 151 | ~randomObstacleFactory1() {} 152 | }; 153 | 154 | #endif // SCENE_OBSTACLES_01 -------------------------------------------------------------------------------- /src/sl_hw5/include/scene/scene_obstacles_new.hpp: -------------------------------------------------------------------------------- 1 | #ifndef SCENE_OBSTACLES_01 2 | #define SCENE_OBSTACLES_01 3 | 4 | #include 5 | 6 | #include "pavel_valtr.hpp" 7 | #include "sdqp/sdqp.hpp" 8 | 9 | typedef Eigen::MatrixXd Mat; 10 | typedef Eigen::VectorXd Vec; 11 | 12 | // base classes of obstacles 13 | struct baseObstacle { 14 | double x; 15 | double y; 16 | double r; 17 | int type; 18 | virtual bool isfree(const double x1, const double y1) = 0; 19 | virtual double distance(const double x1, const double y1, Vec* grad = nullptr) = 0; 20 | baseObstacle(int _t, double _x, double _y, double _r) : x(_x), y(_y), r(_r), type(_t) {} 21 | virtual ~baseObstacle() {} 22 | }; 23 | 24 | // classes for cylinder obstacle 25 | struct cylinderObstacle : public baseObstacle { 26 | cylinderObstacle(const double _x = 0, const double _y = 0, const double _r = 1) : baseObstacle(0, _x, _y, _r) {} 27 | ~cylinderObstacle(){}; 28 | bool isfree(const double x1, const double y1) { 29 | return ((x1 - x) * (x1 - x) + (y1 - y) * (y1 - y) > r * r); 30 | } 31 | double distance(const double x1, const double y1, Vec* grad = nullptr) { 32 | double d1 = std::max(std::sqrt((x1 - x) * (x1 - x) + (y1 - y) * (y1 - y)), 0.01); 33 | double d = d1 - r; 34 | if (grad != nullptr) { 35 | grad->operator()(0) = (x1 - x) / d1; 36 | grad->operator()(1) = (y1 - y) / d1; 37 | } 38 | return d; 39 | } 40 | }; 41 | 42 | // classes for poly prism obstacle 43 | struct polyObstacle : public baseObstacle { 44 | int m; 45 | Mat poly; 46 | Eigen::Matrix Q; 47 | Eigen::Matrix c; 48 | Eigen::Matrix x; 49 | Mat A; 50 | Vec b; 51 | polyObstacle(const Mat& _poly, const double _x = 0, const double _y = 0, const double _r = 1) 52 | : baseObstacle(1, _x, _y, _r), m(_poly.rows()), poly(_poly), Q(Mat::Identity(2, 2) * 2), A(m, 2), b(m) { 53 | int j = m - 1; 54 | for (int k = 0; k < m; ++k) { 55 | A(k, 0) = poly(k, 1) - poly(j, 1); 56 | A(k, 1) = poly(j, 0) - poly(k, 0); 57 | b(k) = A(k, 0) * poly(j, 0) + A(k, 1) * poly(j, 1); 58 | j = k; 59 | } 60 | // std::cout << "[random poly object OK!]" << std::endl; 61 | } 62 | ~polyObstacle(){}; 63 | bool isfree(const double x1, const double y1) { 64 | x(0) = x1; 65 | x(1) = y1; 66 | return (A * x - b).maxCoeff() > 0; 67 | } 68 | double distance(const double x1, const double y1, Vec* grad = nullptr) { 69 | if (isfree(x1, y1)) { 70 | c(0) = -2 * x1; 71 | c(1) = -2 * y1; 72 | sdqp::sdqp<2>(Q, c, A, b, x); 73 | double d, d2 = (x1 - x(0)) * (x1 - x(0)) + (y1 - x(1)) * (y1 - x(1)); 74 | if (d2 > 1e-6) { 75 | d = std::sqrt(d2); 76 | if (grad != nullptr) { 77 | grad->operator()(0) = (x1 - x(0)) / d; 78 | grad->operator()(1) = (y1 - x(1)) / d; 79 | } 80 | } else { 81 | d = 0; 82 | if (grad != nullptr) { 83 | grad->operator()(0) = grad->operator()(1) = 0; 84 | } 85 | } 86 | // std::cout << "[x1:" << x1 << " y1:" << y1 << " x:" << x(0) << " y:" << x(1) << " d:" << d << "]" << std::endl; 87 | return d; 88 | } else { 89 | // double d = 0; 90 | // if (grad != nullptr) { 91 | // grad->operator()(0) = grad->operator()(1) = 0; 92 | // } 93 | 94 | double d = 1e20; 95 | for (int k = 0; k < m; ++k) { 96 | double la = A(k, 0); 97 | double lb = A(k, 1); 98 | double lc = -b(k); 99 | double l2 = la * la + lb * lb; 100 | double d1 = std::abs(la * x1 + lb * y1 + lc) / std::sqrt(l2); 101 | if (d1 < d) { 102 | d = d1; 103 | if (grad != nullptr) { 104 | double dx = (lb * (lb * x1 - la * y1) - la * lc) / l2 - x1; 105 | double dy = (la * (la * y1 - lb * x1) - lb * lc) / l2 - y1; 106 | double norm_dxdy = std::min(std::sqrt(dx * dx + dy * dy), 0.01); 107 | grad->operator()(0) = dx / norm_dxdy; 108 | grad->operator()(1) = dy / norm_dxdy; 109 | } 110 | } 111 | } 112 | 113 | return -d; 114 | } 115 | } 116 | }; 117 | 118 | // class for simple factory type1 (random generation in specific range) 119 | 120 | struct baseObstacleFactory { 121 | virtual baseObstacle* create() = 0; 122 | virtual ~baseObstacleFactory() {} 123 | }; 124 | 125 | struct randomObstacleFactory1 : public baseObstacleFactory { 126 | double x0; 127 | double y0; 128 | double yaw0; 129 | randPoly rand_poly; 130 | std::random_device dev; 131 | std::mt19937 re; 132 | std::uniform_real_distribution rand_x; 133 | std::uniform_real_distribution rand_y; 134 | std::uniform_real_distribution rand_d; 135 | std::uniform_int_distribution rand_p; 136 | randomObstacleFactory1(int _p_min, int _p_max, double _x_min, double _x_max, 137 | double _y_min, double _y_max, double _d_min, double _d_max, 138 | double _x0, double _y0, double _yaw0) 139 | : x0(_x0), y0(_y0), yaw0(_yaw0), re(dev()), rand_x(_x_min, _x_max), rand_y(_y_min, _y_max), rand_d(_d_min, _d_max), rand_p(_p_min, _p_max) {} // 140 | baseObstacle* create() { 141 | int p1 = rand_p(re); 142 | 143 | double cos0 = std::cos(yaw0); 144 | double sin0 = std::sin(yaw0); 145 | double d1 = rand_d(re); 146 | 147 | double xr = rand_x(re); 148 | double yr = rand_y(re); 149 | double x1 = x0 + xr * cos0 - yr * sin0; 150 | double y1 = y0 + xr * sin0 + yr * cos0; 151 | 152 | baseObstacle* bo_ptr; 153 | if (p1 <= 3) { 154 | bo_ptr = new cylinderObstacle(x1, y1, d1); 155 | } else { 156 | Mat poly1; 157 | rand_poly.generate(p1, d1, poly1); 158 | // std::cout << "[random poly OK!]" << std::endl; 159 | for (int k = 0; k < poly1.rows(); ++k) { 160 | double dx = poly1(k, 0); 161 | double dy = poly1(k, 1); 162 | poly1(k, 0) = x1 + dx * cos0 - dy * sin0; 163 | poly1(k, 1) = y1 + dx * sin0 + dy * cos0; 164 | } 165 | // std::cout << "[random poly formed OK!]" << std::endl; 166 | bo_ptr = new polyObstacle(poly1, x1, y1, d1); 167 | } 168 | return bo_ptr; 169 | } 170 | ~randomObstacleFactory1() {} 171 | }; 172 | 173 | #endif // SCENE_OBSTACLES_01 -------------------------------------------------------------------------------- /src/sl_hw5/include/sdqp/sdqp.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | MIT License 3 | 4 | Copyright (c) 2022 Zhepei Wang (wangzhepei@live.com) 5 | 6 | Permission is hereby granted, free of charge, to any person obtaining a copy 7 | of this software and associated documentation files (the "Software"), to deal 8 | in the Software without restriction, including without limitation the rights 9 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | copies of the Software, and to permit persons to whom the Software is 11 | furnished to do so, subject to the following conditions: 12 | 13 | The above copyright notice and this permission notice shall be included in all 14 | copies or substantial portions of the Software. 15 | 16 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | SOFTWARE. 23 | */ 24 | 25 | #ifndef SDQP_HPP 26 | #define SDQP_HPP 27 | 28 | #include 29 | #include 30 | #include 31 | 32 | namespace sdqp 33 | { 34 | constexpr double eps = 1.0e-12; 35 | 36 | enum 37 | { 38 | MINIMUM = 0, 39 | INFEASIBLE, 40 | }; 41 | 42 | template 43 | inline void set_zero(double *x) 44 | { 45 | for (int i = 0; i < d; ++i) 46 | { 47 | x[i] = 0.0; 48 | } 49 | return; 50 | } 51 | 52 | template 53 | inline double dot(const double *x, 54 | const double *y) 55 | { 56 | double s = 0.0; 57 | for (int i = 0; i < d; ++i) 58 | { 59 | s += x[i] * y[i]; 60 | } 61 | return s; 62 | } 63 | 64 | template 65 | inline double sqr_norm(const double *x) 66 | { 67 | double s = 0.0; 68 | for (int i = 0; i < d; ++i) 69 | { 70 | s += x[i] * x[i]; 71 | } 72 | return s; 73 | } 74 | 75 | template 76 | inline void mul(const double *x, 77 | const double s, 78 | double *y) 79 | { 80 | for (int i = 0; i < d; ++i) 81 | { 82 | y[i] = x[i] * s; 83 | } 84 | return; 85 | } 86 | 87 | template 88 | inline int max_abs(const double *x) 89 | { 90 | int id = 0; 91 | double mag = std::fabs(x[0]); 92 | for (int i = 1; i < d; ++i) 93 | { 94 | const double s = std::fabs(x[i]); 95 | if (s > mag) 96 | { 97 | id = i; 98 | mag = s; 99 | } 100 | } 101 | return id; 102 | } 103 | 104 | template 105 | inline void cpy(const double *x, 106 | double *y) 107 | { 108 | for (int i = 0; i < d; ++i) 109 | { 110 | y[i] = x[i]; 111 | } 112 | return; 113 | } 114 | 115 | inline int move_to_front(const int i, 116 | int *next, 117 | int *prev) 118 | { 119 | if (i == 0 || i == next[0]) 120 | { 121 | return i; 122 | } 123 | const int previ = prev[i]; 124 | next[prev[i]] = next[i]; 125 | prev[next[i]] = prev[i]; 126 | next[i] = next[0]; 127 | prev[i] = 0; 128 | prev[next[i]] = i; 129 | next[0] = i; 130 | return previ; 131 | } 132 | 133 | template 134 | inline int min_norm(const double *halves, 135 | const int n, 136 | const int m, 137 | double *opt, 138 | double *work, 139 | int *next, 140 | int *prev) 141 | { 142 | int status = MINIMUM; 143 | set_zero(opt); 144 | if (m <= 0) 145 | { 146 | return status; 147 | } 148 | 149 | double *reflx = work; 150 | double *new_opt = reflx + d; 151 | double *new_halves = new_opt + (d - 1); 152 | double *new_work = new_halves + n * d; 153 | 154 | for (int i = 0; i != m; i = next[i]) 155 | { 156 | const double *plane_i = halves + (d + 1) * i; 157 | 158 | if (dot(opt, plane_i) + plane_i[d] > (d + 1) * eps) 159 | { 160 | const double s = sqr_norm(plane_i); 161 | 162 | if (s < (d + 1) * eps * eps) 163 | { 164 | return INFEASIBLE; 165 | } 166 | 167 | mul(plane_i, -plane_i[d] / s, opt); 168 | 169 | if (i == 0) 170 | { 171 | continue; 172 | } 173 | 174 | // stable Householder reflection with pivoting 175 | const int id = max_abs(opt); 176 | const double xnorm = std::sqrt(sqr_norm(opt)); 177 | cpy(opt, reflx); 178 | reflx[id] += opt[id] < 0.0 ? -xnorm : xnorm; 179 | const double h = -2.0 / sqr_norm(reflx); 180 | 181 | for (int j = 0; j != i; j = next[j]) 182 | { 183 | double *new_plane = new_halves + d * j; 184 | const double *old_plane = halves + (d + 1) * j; 185 | const double coeff = h * dot(old_plane, reflx); 186 | for (int k = 0; k < d; ++k) 187 | { 188 | const int l = k < id ? k : k - 1; 189 | new_plane[l] = k != id ? old_plane[k] + reflx[k] * coeff : new_plane[l]; 190 | } 191 | new_plane[d - 1] = dot(opt, old_plane) + old_plane[d]; 192 | } 193 | 194 | status = min_norm(new_halves, n, i, new_opt, new_work, next, prev); 195 | 196 | if (status == INFEASIBLE) 197 | { 198 | return INFEASIBLE; 199 | } 200 | 201 | double coeff = 0.0; 202 | for (int j = 0; j < d; ++j) 203 | { 204 | const int k = j < id ? j : j - 1; 205 | coeff += j != id ? reflx[j] * new_opt[k] : 0.0; 206 | } 207 | coeff *= h; 208 | for (int j = 0; j < d; ++j) 209 | { 210 | const int k = j < id ? j : j - 1; 211 | opt[j] += j != id ? new_opt[k] + reflx[j] * coeff : reflx[j] * coeff; 212 | } 213 | 214 | i = move_to_front(i, next, prev); 215 | } 216 | } 217 | 218 | return status; 219 | } 220 | 221 | template <> 222 | inline int min_norm<1>(const double *halves, 223 | const int n, 224 | const int m, 225 | double *opt, 226 | double *work, 227 | int *next, 228 | int *prev) 229 | { 230 | opt[0] = 0.0; 231 | bool l = false; 232 | bool r = false; 233 | 234 | for (int i = 0; i != m; i = next[i]) 235 | { 236 | const double a = halves[2 * i]; 237 | const double b = halves[2 * i + 1]; 238 | if (a * opt[0] + b > 2.0 * eps) 239 | { 240 | if (std::fabs(a) < 2.0 * eps) 241 | { 242 | return INFEASIBLE; 243 | } 244 | 245 | l = l || a < 0.0; 246 | r = r || a > 0.0; 247 | 248 | if (l && r) 249 | { 250 | return INFEASIBLE; 251 | } 252 | 253 | opt[0] = -b / a; 254 | } 255 | } 256 | 257 | return MINIMUM; 258 | } 259 | 260 | inline void rand_permutation(const int n, 261 | int *p) 262 | { 263 | typedef std::uniform_int_distribution rand_int; 264 | typedef rand_int::param_type rand_range; 265 | static std::mt19937_64 gen; 266 | static rand_int rdi(0, 1); 267 | int j, k; 268 | for (int i = 0; i < n; ++i) 269 | { 270 | p[i] = i; 271 | } 272 | for (int i = 0; i < n; ++i) 273 | { 274 | rdi.param(rand_range(0, n - i - 1)); 275 | j = rdi(gen) + i; 276 | k = p[j]; 277 | p[j] = p[i]; 278 | p[i] = k; 279 | } 280 | } 281 | 282 | template 283 | inline double sdmn(const Eigen::Matrix &A, 284 | const Eigen::Matrix &b, 285 | Eigen::Matrix &x) 286 | { 287 | x.setZero(); 288 | const int n = b.size(); 289 | if (n < 1) 290 | { 291 | return 0.0; 292 | } 293 | 294 | Eigen::VectorXi perm(n - 1); 295 | Eigen::VectorXi next(n); 296 | Eigen::VectorXi prev(n + 1); 297 | if (n > 1) 298 | { 299 | rand_permutation(n - 1, perm.data()); 300 | prev(0) = 0; 301 | next(0) = perm(0) + 1; 302 | prev(perm(0) + 1) = 0; 303 | for (int i = 0; i < n - 2; ++i) 304 | { 305 | next(perm(i) + 1) = perm(i + 1) + 1; 306 | prev(perm(i + 1) + 1) = perm(i) + 1; 307 | } 308 | next(perm(n - 2) + 1) = n; 309 | } 310 | else 311 | { 312 | prev(0) = 0; 313 | next(0) = 1; 314 | next(1) = 1; 315 | } 316 | 317 | Eigen::Matrix halves(d + 1, n); 318 | Eigen::VectorXd work((n + 2) * (d + 2) * (d - 1) / 2 + 1 - d); 319 | 320 | const Eigen::VectorXd scale = A.rowwise().norm(); 321 | halves.template topRows() = (A.array().colwise() / scale.array()).transpose(); 322 | halves.template bottomRows<1>() = (-b.array() / scale.array()).transpose(); 323 | 324 | const int status = min_norm(halves.data(), n, n, 325 | x.data(), work.data(), 326 | next.data(), prev.data()); 327 | 328 | double minimum = INFINITY; 329 | if (status != INFEASIBLE) 330 | { 331 | minimum = x.norm(); 332 | } 333 | 334 | return minimum; 335 | } 336 | 337 | /** 338 | * minimize 0.5 x' Q x + c' x 339 | * subject to A x <= b 340 | * Q must be positive definite 341 | **/ 342 | 343 | template 344 | inline double sdqp(const Eigen::Matrix &Q, 345 | const Eigen::Matrix &c, 346 | const Eigen::Matrix &A, 347 | const Eigen::Matrix &b, 348 | Eigen::Matrix &x) 349 | { 350 | Eigen::LLT> llt(Q); 351 | if (llt.info() != Eigen::Success) 352 | { 353 | return INFINITY; 354 | } 355 | 356 | const Eigen::Matrix As = llt.matrixU().template solve(A); 357 | const Eigen::Matrix v = llt.solve(c); 358 | const Eigen::Matrix bs = A * v + b; 359 | 360 | double minimum = sdmn(As, bs, x); 361 | if (!std::isinf(minimum)) 362 | { 363 | llt.matrixU().template solveInPlace(x); 364 | x -= v; 365 | minimum = 0.5 * (Q * x).dot(x) + c.dot(x); 366 | } 367 | 368 | return minimum; 369 | } 370 | 371 | } // namespace sdqp 372 | 373 | #endif 374 | -------------------------------------------------------------------------------- /src/sl_hw5/include/sdqp/sdqp_bak.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | MIT License 3 | 4 | Copyright (c) 2022 Zhepei Wang (wangzhepei@live.com) 5 | 6 | Permission is hereby granted, free of charge, to any person obtaining a copy 7 | of this software and associated documentation files (the "Software"), to 8 | deal in the Software without restriction, including without limitation the 9 | rights to use, copy, modify, merge, publish, distribute, sublicense, and/or 10 | sell 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 14 | all 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 21 | FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS 22 | IN THE SOFTWARE. 23 | */ 24 | 25 | #include 26 | 27 | #ifndef SDQP_HPP 28 | #define SDQP_HPP 29 | 30 | #include 31 | #include 32 | #include 33 | 34 | namespace sdqp { 35 | constexpr double eps = 1.0e-12; 36 | 37 | enum { 38 | MINIMUM = 0, 39 | INFEASIBLE, 40 | }; 41 | 42 | template 43 | inline void set_zero(double *x) { 44 | for (int i = 0; i < d; ++i) { 45 | x[i] = 0.0; 46 | } 47 | return; 48 | } 49 | 50 | template 51 | inline double dot(const double *x, const double *y) { 52 | double s = 0.0; 53 | for (int i = 0; i < d; ++i) { 54 | s += x[i] * y[i]; 55 | } 56 | return s; 57 | } 58 | 59 | template 60 | inline double sqr_norm(const double *x) { 61 | double s = 0.0; 62 | for (int i = 0; i < d; ++i) { 63 | s += x[i] * x[i]; 64 | } 65 | return s; 66 | } 67 | 68 | template 69 | inline void mul(const double *x, const double s, double *y) { 70 | for (int i = 0; i < d; ++i) { 71 | y[i] = x[i] * s; 72 | } 73 | return; 74 | } 75 | 76 | template 77 | inline int max_abs(const double *x) { 78 | int id = 0; 79 | double mag = std::fabs(x[0]); 80 | for (int i = 1; i < d; ++i) { 81 | const double s = std::fabs(x[i]); 82 | if (s > mag) { 83 | id = i; 84 | mag = s; 85 | } 86 | } 87 | return id; 88 | } 89 | 90 | template 91 | inline void cpy(const double *x, double *y) { 92 | for (int i = 0; i < d; ++i) { 93 | y[i] = x[i]; 94 | } 95 | return; 96 | } 97 | 98 | inline int move_to_front(const int i, int *next, int *prev) { 99 | if (i == 0 || i == next[0]) { 100 | return i; 101 | } 102 | const int previ = prev[i]; 103 | next[prev[i]] = next[i]; 104 | prev[next[i]] = prev[i]; 105 | next[i] = next[0]; 106 | prev[i] = 0; 107 | prev[next[i]] = i; 108 | next[0] = i; 109 | return previ; 110 | } 111 | 112 | template 113 | inline int min_norm(const double *halves, const int n, const int m, double *opt, 114 | double *work, int *next, int *prev) { 115 | int status = MINIMUM; 116 | set_zero(opt); 117 | if (m <= 0) { 118 | return status; 119 | } 120 | 121 | double *reflx = work; 122 | double *new_opt = reflx + d; 123 | double *new_halves = new_opt + (d - 1); 124 | double *new_work = new_halves + n * d; 125 | double new_origin[d] = {0.0}; 126 | 127 | for (int i = 0; i != m; i = next[i]) { 128 | const double *plane_i = halves + (d + 1) * i; 129 | 130 | if (dot(opt, plane_i) + plane_i[d] > (d + 1) * eps) { 131 | const double s = sqr_norm(plane_i); 132 | 133 | if (s < (d + 1) * eps * eps) { 134 | return INFEASIBLE; 135 | } 136 | 137 | mul(plane_i, -plane_i[d] / s, new_origin); //new_origin 138 | 139 | 140 | if (i == 0) { 141 | cpy(new_origin, opt); // 为什么它至关重要？？？？ 142 | continue; 143 | } 144 | const int id = max_abs(new_origin); //new_origin 145 | 146 | //////////////////////////////// HOMEWORK START 147 | /////////////////////////////////// 148 | // 149 | // MISSION TO BE ACCOMPLISHED: 150 | // 151 | // now we know the best solution "opt" violates the i-th halfspace. 152 | // Therefore, please project all previous i halfspaces (from the 0-th to 153 | // the (i-1)-th one) onto the boundary of the i-th halfspace, then store 154 | // all projected halfspaces in the double-type-c-array "new_halves". If 155 | // you successfully complete the mission, the sdqp_example should prints 156 | // optimal sol: 4.11111 9.15556 4.50022 157 | // optimal obj: 201.14 158 | // cons precision: *.********e-16 159 | // This means you obtained the correct exact solution (precision near 160 | // DBL_EPSILON) 161 | // 162 | // VARIABLES YOU NEED TO ACCESS: 163 | // 164 | // opt is a d-dimensional double-type-c-array 165 | // opt contains an optimal solution that meets all linear constraints from 166 | // the 0-th to the (i-1)-th one, but it is known to violate the i-th 167 | // halfspace here 168 | // 169 | // new_origin is also a d-dimensional double-type-c-array 170 | // new_origin contains the minimum norm point on the boundary of the i-th 171 | // plane 172 | // 173 | // you should calculate the vector 'u' of Householder (you can review this 174 | // concept in the course) with the i_d th natural normal Orthogonal basis 175 | // and store it in the Array reflx 176 | 177 | // you can read all previous halfspaces via the iteration below 178 | // 179 | // for (int j = 0; j != i; j = next[j]) 180 | // { 181 | // const double *halfspace = halves + (d + 1) * j; 182 | // // thus the j-th halfspace is the inequality below 183 | // // halfspace[0] * x1 + halfspace[1] * x2 + ... + halfspace[d-1] 184 | // * xd + halfspace[d] <= 0 185 | // } 186 | // 187 | // you can write or store all your projected halfspaces via the iteration 188 | // below 189 | // 190 | // for (int j = 0; j != i; j = next[j]) 191 | // { 192 | // double *proj_halfspace = new_halves + d * j; 193 | // // thus the j-th projected halfspace is the inequality below 194 | // // proj_halfspace[0] * y1 + proj_halfspace[1] * y2 + ... + 195 | // proj_halfspace[d-2] * y(d-1) + proj_halfspace[d-1] <= 0 196 | // // y1 to y(d-1) is the new coordinate constructed on the 197 | // boundary of the i-th halfspace 198 | // } 199 | // 200 | 201 | // FINISHED 202 | // for (int j = 0; j < d; ++j) { 203 | // reflx[j] = new_origin[j] + 204 | // (j != id ? 0 : (new_origin[j] > 0 ? 1 : -1) * std::sqrt(sqr_norm(new_origin))); 205 | // } 206 | const double xnorm = std::sqrt(sqr_norm(new_origin)); 207 | cpy(new_origin, reflx); 208 | reflx[id] += new_origin[id] < 0? -xnorm : xnorm; 209 | const double u1 = sqr_norm(reflx); 210 | for (int k = 0; k != i; k = next[k]) { 211 | const double *halfspace = halves + (d + 1) * k; 212 | double *proj_halfspace = new_halves + d * k; 213 | const double u2 = -2.0 * dot(halfspace, reflx) / u1; 214 | for (int j = 0; j < d; ++j) { 215 | if (j == id) continue; 216 | const int j2 = j < id ? j : j - 1; 217 | proj_halfspace[j2] = halfspace[j] + u2 * reflx[j]; 218 | } 219 | proj_halfspace[d - 1] = halfspace[d] + dot(halfspace, new_origin); 220 | } 221 | 222 | 223 | // // const double xnorm = std::sqrt(sqr_norm(opt)); 224 | // // cpy(opt, reflx); 225 | // // reflx[id] += opt[id] < 0.0 ? -xnorm : xnorm; 226 | // // const double h = -2.0 / sqr_norm(reflx); 227 | 228 | // // for (int j = 0; j != i; j = next[j]) 229 | // // { 230 | // // double *new_plane = new_halves + d * j; 231 | // // const double *old_plane = halves + (d + 1) * j; 232 | // // const double coeff = h * dot(old_plane, reflx); 233 | // // for (int k = 0; k < d; ++k) 234 | // // { 235 | // // const int l = k < id ? k : k - 1; 236 | // // new_plane[l] = k != id ? old_plane[k] + reflx[k] * coeff : new_plane[l]; 237 | // // } 238 | // // new_plane[d - 1] = dot(opt, old_plane) + old_plane[d]; 239 | // // } 240 | 241 | // const double xnorm = std::sqrt(sqr_norm(new_origin)); //new_origin 242 | // cpy(new_origin, reflx); //new_origin 243 | // reflx[id] += new_origin[id] < 0.0 ? -xnorm : xnorm; //new_origin 244 | // const double h = -2.0 / sqr_norm(reflx); 245 | 246 | // for (int j = 0; j != i; j = next[j]) 247 | // { 248 | // double *new_plane = new_halves + d * j; 249 | // const double *old_plane = halves + (d + 1) * j; 250 | // const double coeff = h * dot(old_plane, reflx); 251 | // for (int k = 0; k < d; ++k) 252 | // { 253 | // const int l = k < id ? k : k - 1; 254 | // new_plane[l] = k != id ? old_plane[k] + reflx[k] * coeff : new_plane[l]; 255 | // } 256 | // new_plane[d - 1] = dot(new_origin, old_plane) + old_plane[d]; //new_origin 257 | // } 258 | 259 | //////////////////////////////// HOMEWORK END 260 | /////////////////////////////////// 261 | 262 | status = min_norm(new_halves, n, i, new_opt, new_work, next, prev); 263 | 264 | if (status == INFEASIBLE) { 265 | return INFEASIBLE; 266 | } 267 | 268 | double coeff = 0.0; 269 | for (int j = 0; j < d; ++j) { 270 | const int k = j < id ? j : j - 1; 271 | coeff += j != id ? reflx[j] * new_opt[k] : 0.0; 272 | } 273 | coeff *= -2.0 / sqr_norm(reflx); 274 | for (int j = 0; j < d; ++j) { 275 | 276 | const int k = j < id ? j : j - 1; 277 | opt[j] = new_origin[j] += (j != id ? new_opt[k] + reflx[j] * coeff : reflx[j] * coeff); //new_origin[j] + 278 | 279 | } 280 | 281 | i = move_to_front(i, next, prev); 282 | } 283 | } 284 | 285 | return status; 286 | } 287 | 288 | template <> 289 | inline int min_norm<1>(const double *halves, const int n, const int m, 290 | double *opt, double *work, int *next, int *prev) { 291 | opt[0] = 0.0; 292 | bool l = false; 293 | bool r = false; 294 | 295 | for (int i = 0; i != m; i = next[i]) { 296 | const double a = halves[2 * i]; 297 | const double b = halves[2 * i + 1]; 298 | if (a * opt[0] + b > 2.0 * eps) { 299 | if (std::fabs(a) < 2.0 * eps) { 300 | return INFEASIBLE; 301 | } 302 | 303 | l = l || a < 0.0; 304 | r = r || a > 0.0; 305 | 306 | if (l && r) { 307 | return INFEASIBLE; 308 | } 309 | 310 | opt[0] = -b / a; 311 | } 312 | } 313 | 314 | return MINIMUM; 315 | } 316 | 317 | inline void rand_permutation(const int n, int *p) { 318 | typedef std::uniform_int_distribution rand_int; 319 | typedef rand_int::param_type rand_range; 320 | static std::mt19937_64 gen; 321 | static rand_int rdi(0, 1); 322 | int j, k; 323 | for (int i = 0; i < n; ++i) { 324 | p[i] = i; 325 | } 326 | for (int i = 0; i < n; ++i) { 327 | rdi.param(rand_range(0, n - i - 1)); 328 | j = rdi(gen) + i; 329 | k = p[j]; 330 | p[j] = p[i]; 331 | p[i] = k; 332 | } 333 | } 334 | 335 | template 336 | inline double sdmn(const Eigen::Matrix &A, 337 | const Eigen::Matrix &b, 338 | Eigen::Matrix &x) { 339 | x.setZero(); 340 | const int n = b.size(); 341 | if (n < 1) { 342 | return 0.0; 343 | } 344 | 345 | Eigen::VectorXi perm(n - 1); 346 | Eigen::VectorXi next(n); 347 | Eigen::VectorXi prev(n + 1); 348 | if (n > 1) { 349 | rand_permutation(n - 1, perm.data()); 350 | prev(0) = 0; 351 | next(0) = perm(0) + 1; 352 | prev(perm(0) + 1) = 0; 353 | for (int i = 0; i < n - 2; ++i) { 354 | next(perm(i) + 1) = perm(i + 1) + 1; 355 | prev(perm(i + 1) + 1) = perm(i) + 1; 356 | } 357 | next(perm(n - 2) + 1) = n; 358 | } else { 359 | prev(0) = 0; 360 | next(0) = 1; 361 | next(1) = 1; 362 | } 363 | 364 | Eigen::Matrix halves(d + 1, n); 365 | Eigen::VectorXd work((n + 2) * (d + 2) * (d - 1) / 2 + 1 - d); 366 | 367 | const Eigen::VectorXd scale = A.rowwise().norm(); 368 | halves.template topRows() = 369 | (A.array().colwise() / scale.array()).transpose(); 370 | halves.template bottomRows<1>() = (-b.array() / scale.array()).transpose(); 371 | 372 | const int status = min_norm(halves.data(), n, n, x.data(), work.data(), 373 | next.data(), prev.data()); 374 | 375 | double minimum = INFINITY; 376 | if (status != INFEASIBLE) { 377 | minimum = x.norm(); 378 | } 379 | 380 | 381 | // std::cout << "cons precision - 0: " << (A * x - b).maxCoeff() << std::endl; 382 | 383 | return minimum; 384 | } 385 | 386 | /** 387 | * minimize 0.5 x' Q x + c' x 388 | * subject to A x <= b 389 | * Q must be positive definite 390 | **/ 391 | 392 | template 393 | inline double sdqp(const Eigen::Matrix &Q, 394 | const Eigen::Matrix &c, 395 | const Eigen::Matrix &A, 396 | const Eigen::Matrix &b, 397 | Eigen::Matrix &x) { 398 | Eigen::LLT> llt(Q); 399 | if (llt.info() != Eigen::Success) { 400 | return INFINITY; 401 | } 402 | 403 | const Eigen::Matrix As = 404 | llt.matrixU().template solve(A); 405 | const Eigen::Matrix v = llt.solve(c); 406 | const Eigen::Matrix bs = A * v + b; 407 | 408 | double minimum = sdmn(As, bs, x); 409 | if (!std::isinf(minimum)) { 410 | llt.matrixU().template solveInPlace(x); 411 | x -= v; 412 | minimum = 0.5 * (Q * x).dot(x) + c.dot(x); 413 | } 414 | 415 | return minimum; 416 | } 417 | 418 | } // namespace sdqp 419 | 420 | #endif 421 | -------------------------------------------------------------------------------- /src/sl_hw5/include/traj_optim/conic_alm_topp.hpp: -------------------------------------------------------------------------------- 1 | // Reconstruction of the origin conic ALM TOPP algorithm 2 | // with the simplication of unnecessary matrix operations 3 | // and minor fixes in ALM algorithm steps 4 | // V0.1.5 20220831, tkalpha 5 | 6 | #ifndef CONIC_ALM_TOPP_01 7 | #define CONIC_ALM_TOPP_01 8 | 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | 15 | #include "lbfgs/lbfgs.hpp" 16 | 17 | typedef Eigen::Matrix3d Mat3; 18 | typedef Eigen::Vector3d Vec3; 19 | typedef Eigen::MatrixXd Mat; 20 | typedef Eigen::VectorXd Vec; 21 | typedef Eigen::SparseMatrix SpMat; 22 | 23 | struct conicALMTOPP2 { 24 | // path information 25 | Vec& s; 26 | Mat& q; 27 | Mat& qv; 28 | Mat& qa; 29 | int mode; 30 | double a_max; 31 | double v_max; 32 | double v_start; 33 | double v_end; 34 | 35 | // variables for conic alm algorithm 36 | Vec c; 37 | std::vector As; 38 | std::vector bs; 39 | SpMat Aieq; 40 | SpMat Aieq_transpose; 41 | Vec bieq; 42 | SpMat G; 43 | SpMat G_transpose; 44 | Vec h; 45 | double value; 46 | Vec x0; 47 | int pts, dim; 48 | int idx_a, idx_b, idx_c, idx_d; 49 | int dim_a, dim_b, dim_c, dim_d; 50 | 51 | // ALM algorithm variables 52 | std::vector mius; 53 | Vec eta; 54 | Vec lambda; 55 | Vec zeros_ieq; 56 | 57 | // L-BFGS parameters 58 | lbfgs::lbfgs_parameter_t pl2; 59 | 60 | // ALM algorithm parameters 61 | struct alm_param { 62 | double alm_rho = 1.0; 63 | double alm_beta = 1e3; 64 | double alm_gamma = 1.0; 65 | double alm_xi = 0.9; 66 | double alm_xi_min = 1e-8; 67 | double alm_epsilon_cons = 1e-3; 68 | double alm_epsilon_prec = 1e-3; 69 | int alm_iter_max = 80; 70 | int alm_iter_display = 1; 71 | } pa2; 72 | 73 | conicALMTOPP2(Vec& _s, Mat& _q, Mat& _qv, Mat& _qa, double _a_max, 74 | double _v_max, double _v_start, double _v_end) 75 | : s(_s), 76 | q(_q), 77 | qv(_qv), 78 | qa(_qa), 79 | mode(0), 80 | a_max(_a_max), 81 | v_max(_v_max), 82 | v_start(_v_start), 83 | v_end(_v_end), 84 | value(0), 85 | pl2(lbfgs::lbfgs_parameter_t()) { 86 | generate_topp_matrices(); 87 | generate_alm_variables(); 88 | } 89 | 90 | // basic matrices for topp socp problem 91 | int generate_topp_matrices() { 92 | pts = s.size(); 93 | dim = 4 * pts - 2; 94 | idx_a = 3 * pts - 1; 95 | idx_b = 0; 96 | idx_c = pts; 97 | idx_d = 2 * pts; 98 | dim_a = pts - 1; 99 | dim_b = pts; 100 | dim_c = pts; 101 | dim_d = pts - 1; 102 | // goal 103 | x0 = Vec::Zero(dim); 104 | c = Vec::Zero(dim); 105 | for (int k = 0; k < dim_d; ++k) { 106 | c(idx_d + k) = 2 * (s(k + 1) - s(k)); 107 | } 108 | // cones 109 | As.clear(); 110 | bs.clear(); 111 | for (int k = 0; k < dim_d; ++k) { 112 | SpMat A(3, dim); 113 | A.setZero(); 114 | Vec3 b(Vec3::Zero()); 115 | b(1) = 2; 116 | A.insert(0, idx_c + k) = A.insert(0, idx_c + k + 1) = 1; 117 | A.insert(0, idx_d + k) = 1; 118 | A.insert(2, idx_c + k) = A.insert(2, idx_c + k + 1) = 1; 119 | A.insert(2, idx_d + k) = -1; 120 | As.push_back(A); 121 | bs.push_back(b); 122 | } 123 | for (int k = 0; k < dim_b; ++k) { 124 | SpMat A(3, dim); 125 | A.setZero(); 126 | Vec3 b(Vec3::Zero()); 127 | b(0) = 1; 128 | b(2) = -1; 129 | A.insert(1, idx_c + k) = 2; 130 | A.insert(0, idx_b + k) = A.insert(2, idx_b + k) = 1; 131 | As.push_back(A); 132 | bs.push_back(b); 133 | } 134 | // inequality 135 | int num_ieq = 3 * dim_b + 4 * dim_a; 136 | Aieq.resize(num_ieq, dim); 137 | Aieq.setZero(); 138 | bieq = Vec::Zero(num_ieq); 139 | int pos = 0; 140 | for (int k = 0; k < dim_b; ++k) { 141 | Aieq.insert(pos + k, idx_b + k) = -1; 142 | } 143 | pos += dim_b; 144 | for (int k = 0; k < dim_b; ++k) { 145 | Aieq.insert(pos + k, idx_b + k) = std::pow(qv(k, 0), 2); 146 | bieq(pos + k) = std::pow(v_max, 2); 147 | } 148 | pos += dim_b; 149 | for (int k = 0; k < dim_b; ++k) { 150 | Aieq.insert(pos + k, idx_b + k) = std::pow(qv(k, 1), 2); 151 | bieq(pos + k) = std::pow(v_max, 2); 152 | } 153 | pos += dim_b; 154 | for (int k = 0; k < dim_a; ++k) { 155 | Aieq.insert(pos + k, idx_b + k) = qa(k, 0); 156 | Aieq.insert(pos + k, idx_a + k) = qv(k, 0); 157 | bieq(pos + k) = a_max; 158 | } 159 | pos += dim_a; 160 | for (int k = 0; k < dim_a; ++k) { 161 | Aieq.insert(pos + k, idx_b + k) = -qa(k, 0); 162 | Aieq.insert(pos + k, idx_a + k) = -qv(k, 0); 163 | bieq(pos + k) = a_max; 164 | } 165 | pos += dim_a; 166 | for (int k = 0; k < dim_a; ++k) { 167 | Aieq.insert(pos + k, idx_b + k) = qa(k, 1); 168 | Aieq.insert(pos + k, idx_a + k) = qv(k, 1); 169 | bieq(pos + k) = a_max; 170 | } 171 | pos += dim_a; 172 | for (int k = 0; k < dim_a; ++k) { 173 | Aieq.insert(pos + k, idx_b + k) = -qa(k, 1); 174 | Aieq.insert(pos + k, idx_a + k) = -qv(k, 1); 175 | bieq(pos + k) = a_max; 176 | } 177 | // equality 178 | int num_eq = dim_a + 2; 179 | G.resize(num_eq, dim); 180 | G.setZero(); 181 | h = Vec::Zero(num_eq); 182 | pos = 0; 183 | for (int k = 0; k < dim_a; ++k) { 184 | G.insert(pos + k, idx_b + k) = 1; 185 | G.insert(pos + k, idx_b + k + 1) = -1; 186 | G.insert(pos + k, idx_a + k) = 2 * (s(k + 1) - s(k)); 187 | } 188 | pos += dim_a; 189 | G.insert(pos, idx_b) = qv.row(0).squaredNorm(); 190 | h(pos) = std::pow(v_start, 2); 191 | G.insert(pos + 1, idx_b + dim_b - 1) = qv.row(dim_b - 1).squaredNorm(); 192 | h(pos + 1) = std::pow(v_end, 2); 193 | // simplify 194 | Aieq_transpose = Aieq.transpose(); 195 | G_transpose = G.transpose(); 196 | return 0; 197 | } 198 | 199 | // initialize ALM variables 200 | int generate_alm_variables() { 201 | mius.clear(); 202 | int n_cones = As.size(); 203 | for (int k = 0; k < n_cones; ++k) { 204 | mius.push_back(Vec3::Zero()); 205 | } 206 | eta = Vec::Zero(Aieq.rows()); 207 | lambda = Vec::Zero(G.rows()); 208 | zeros_ieq = Vec::Zero(Aieq.rows()); 209 | return 0; 210 | } 211 | 212 | // function to generate projection on 3d second order cone 213 | int get_conic3_prj(const Vec3& v, Vec3& prj) { 214 | double v0 = v(0); 215 | double v1_norm = std::sqrt(v(1) * v(1) + v(2) * v(2)); 216 | if (v0 <= -v1_norm) { 217 | prj = Vec3::Zero(); 218 | } else if (v0 >= v1_norm) { 219 | prj = v; 220 | } else { 221 | double c = (v0 + v1_norm) / v1_norm / 2; 222 | prj(0) = c * v1_norm; 223 | prj(1) = c * v(1); 224 | prj(2) = c * v(2); 225 | } 226 | return 0; 227 | } 228 | 229 | // objective function of conic ALM 230 | static double obj_fcn_alm(void* ptr_obj, const Vec& x, Vec& grad) { 231 | conicALMTOPP2& p = *(reinterpret_cast(ptr_obj)); 232 | 233 | double rho = p.pa2.alm_rho; 234 | Vec& c = p.c; 235 | std::vector& As = p.As; 236 | std::vector& bs = p.bs; 237 | SpMat& Aieq = p.Aieq; 238 | SpMat& Aieq_transpose = p.Aieq_transpose; 239 | Vec& bieq = p.bieq; 240 | SpMat& G = p.G; 241 | SpMat& G_transpose = p.G_transpose; 242 | Vec& h = p.h; 243 | 244 | std::vector& mius = p.mius; 245 | Vec& eta = p.eta; 246 | Vec& lambda = p.lambda; 247 | Vec& zeros_ieq = p.zeros_ieq; 248 | 249 | double value = c.dot(x); 250 | grad = c; 251 | 252 | Vec3 prj; 253 | double value_cone = 0; 254 | int n_cones = As.size(); 255 | for (int k = 0; k < n_cones; ++k) { 256 | p.get_conic3_prj((1.0 / rho) * mius[k] - As[k] * x - bs[k], prj); 257 | value_cone += 0.5 * rho * prj.squaredNorm(); 258 | grad -= rho * As[k].transpose() * prj; 259 | } 260 | 261 | Vec res_ieq = ((1.0 / rho) * eta + Aieq * x - bieq).cwiseMax(zeros_ieq); 262 | double value_ieq = 0.5 * rho * res_ieq.squaredNorm(); 263 | grad += rho * Aieq_transpose * res_ieq; 264 | 265 | Vec res_eq = (1.0 / rho) * lambda + G * x - h; 266 | double value_eq = 0.5 * rho * res_eq.squaredNorm(); 267 | grad += rho * G_transpose * res_eq; 268 | 269 | value = value + value_cone + value_ieq + value_eq; 270 | return value; 271 | } 272 | 273 | // get ALM error 274 | double get_alm_error(const Vec& x, double& err_cons, double& err_prec) { 275 | Vec grad; 276 | value = obj_fcn_alm(this, x, grad); 277 | //std::cout << " [d]:" << x.segment(idx_d, dim_d).transpose() << std::endl; 278 | double x_inf_norm = std::max(1.0, x.cwiseAbs().maxCoeff()); 279 | double grad_inf_norm = grad.cwiseAbs().maxCoeff(); 280 | double err_cone = 0; 281 | Vec3 prj; 282 | int n_cones = As.size(); 283 | for (int k = 0; k < n_cones; ++k) { 284 | get_conic3_prj((1.0 / pa2.alm_rho) * mius[k] - As[k] * x - bs[k], prj); 285 | double err_cone1 = 286 | ((1.0 / pa2.alm_rho) * mius[k] - prj).cwiseAbs().maxCoeff(); 287 | if (err_cone1 > err_cone) err_cone = err_cone1; 288 | } 289 | double err_ieq = ((-1.0 / pa2.alm_rho) * eta) 290 | .cwiseMax(Aieq * x - bieq) 291 | .cwiseAbs() 292 | .maxCoeff(); 293 | double err_eq = (G * x - h).cwiseAbs().maxCoeff(); 294 | 295 | err_cons = std::max(std::max(err_cons, err_ieq), err_eq) / x_inf_norm; 296 | err_prec = grad_inf_norm / x_inf_norm; 297 | return value; 298 | } 299 | 300 | // ALM solving process 301 | int alm_solve(Vec& x1, double& value1, alm_param& pa, 302 | lbfgs::lbfgs_parameter_t pl) { 303 | pa2 = pa; 304 | pl2 = pl; 305 | int n_cones = As.size(); 306 | for (int k = 0; k < n_cones; ++k) { 307 | mius[k].setZero(); 308 | } 309 | eta.setZero(); 310 | lambda.setZero(); 311 | Vec x = x1; 312 | int lbfgs_ret, valid = 1; 313 | int alm_count = 0; 314 | double value; 315 | double err_cons, err_prec; 316 | value = get_alm_error(x, err_cons, err_prec); 317 | while (err_cons > pa2.alm_epsilon_cons || err_prec > pa2.alm_epsilon_prec) { 318 | // lbfgs inner layer optimize 319 | pl2.g_epsilon = 320 | std::max(std::pow(pa2.alm_xi, alm_count), pa2.alm_xi_min) * 321 | std::min(1.0, err_cons); 322 | pl2.g_epsilon = std::max(4e-5, pl2.g_epsilon); 323 | lbfgs_ret = 324 | lbfgs::lbfgs_optimize(x, value, obj_fcn_alm, nullptr, this, pl2); 325 | // alm value update 326 | int n_cones = As.size(); 327 | for (int k = 0; k < n_cones; ++k) { 328 | get_conic3_prj(mius[k] - pa2.alm_rho * (As[k] * x + bs[k]), mius[k]); 329 | } 330 | eta = (eta + pa2.alm_rho * (Aieq * x - bieq)).cwiseMax(zeros_ieq); 331 | lambda += pa2.alm_rho * (G * x - h); 332 | // conditions udate 333 | value = get_alm_error(x, err_cons, err_prec); 334 | if (pa2.alm_iter_display > 0) { 335 | std::cout << "ALM iter: " << alm_count << " rho:" << pa2.alm_rho 336 | << " lbfgs:" << lbfgs_ret << " ALM obj:" << value 337 | << " err_cons:" << err_cons << " err_prec:" << err_prec 338 | << std::endl; 339 | } 340 | if ((lbfgs_ret != 0 && lbfgs_ret != 1) || alm_count > pa2.alm_iter_max) { 341 | valid = 0; 342 | break; 343 | } 344 | // alm rho update 345 | ++alm_count; 346 | pa2.alm_rho = std::min(pa2.alm_beta, (1.0 + pa2.alm_gamma) * pa2.alm_rho); 347 | } 348 | x1 = x; 349 | value1 = value; 350 | return !valid; 351 | } 352 | 353 | // solve interface 354 | int solve(double& value, Vec& xa, Vec& xb, Vec& xc, Vec& xd) { 355 | // x0.segment(idx_a, dim_a) = Vec::Zero(dim_a); 356 | // x0.segment(idx_b, dim_b) = Vec::Ones(dim_b); 357 | // x0.segment(idx_c, dim_c) = Vec::Ones(dim_c); 358 | // x0.segment(idx_d, dim_d) = Vec::Ones(dim_d) * 0.5; 359 | x0.setZero(); 360 | conicALMTOPP2::alm_param pa; 361 | lbfgs::lbfgs_parameter_t pl; 362 | int topp_ret = alm_solve(x0, value, pa, pl); 363 | get_result(xa, xb, xc, xd); 364 | value = c.dot(x0); 365 | return topp_ret; 366 | } 367 | 368 | // get result 369 | int get_result(Vec& xa, Vec& xb, Vec& xc, Vec& xd){ 370 | xa = x0.segment(idx_a, dim_a); 371 | xb = x0.segment(idx_b, dim_b); 372 | xc = x0.segment(idx_c, dim_c); 373 | xd = x0.segment(idx_d, dim_d); 374 | return 0; 375 | } 376 | }; 377 | 378 | #endif // CONIC_ALM_TOPP_01 -------------------------------------------------------------------------------- /src/sl_hw5/include/traj_optim/spline_opt.hpp: -------------------------------------------------------------------------------- 1 | #ifndef SPLINE_OPT_01 2 | #define SPLINE_OPT_01 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | #include "lbfgs/lbfgs.hpp" 9 | #include "scene/scene_base.hpp" 10 | 11 | typedef Eigen::SparseMatrix SpMat; 12 | 13 | struct cubicSplineOpt { 14 | // basic information 15 | sceneBase& scene0; 16 | Mat& path0; 17 | double step; 18 | Mat points0; 19 | Mat points1; 20 | Mat param1; 21 | int segs0; 22 | 23 | // variables for spline generation 24 | Eigen::ConjugateGradient, Eigen::Upper> solver; 25 | SpMat A, B, C, M, E, F; 26 | Mat d_D_d_p, d_c_d_p, d_d_d_p; 27 | 28 | // parameters for risk field 29 | double c0, c1; 30 | 31 | // variables for TOPP 32 | Vec s1; 33 | Mat q1, qv1, qa1; 34 | 35 | cubicSplineOpt(sceneBase& _scene, Mat& _path, double _step, double _c0 = 200, double _c1 = -10) 36 | : scene0(_scene), path0(_path), step(_step), c0(_c0), c1(_c1) { 37 | generate_inital_cubic_spline_points(step); 38 | generate_static_matrices(); 39 | } 40 | 41 | // generate initial cubic spline points with fixed step 42 | int generate_inital_cubic_spline_points(double s) { 43 | double len0 = 0; 44 | for (int k = 1; k < path0.rows(); ++k) { 45 | len0 += (path0.row(k) - path0.row(k - 1)).norm(); 46 | } 47 | int num_est = static_cast((std::floor(len0 / s) + 1) * 1.5); 48 | 49 | int num = 0; 50 | Mat points02(num_est, 2); 51 | points02.row(num++) = path0.row(0); 52 | double residual = 0; 53 | for (int k = 1; k < path0.rows(); ++k) { 54 | double offset = -residual; 55 | double edge = (path0.row(k) - path0.row(k - 1)).norm(); 56 | residual += edge; 57 | while (residual >= s) { 58 | double location = (offset + step) / edge; 59 | points02.row(num++) = (1 - location) * path0.row(k - 1) + location * path0.row(k); 60 | residual -= step; 61 | offset += step; 62 | } 63 | } 64 | if (residual < 0.67 * s) { 65 | points02.row(num - 1) = path0.row(path0.rows() - 1); 66 | } else { 67 | points02.row(num++) = path0.row(path0.rows() - 1); 68 | } 69 | points0 = points02.block(0, 0, num, 2); 70 | segs0 = num - 1; 71 | return 0; 72 | } 73 | 74 | // generate static matrices for initial cubic spline points 75 | int generate_static_matrices() { 76 | A = SpMat(segs0 - 1, segs0 - 1); 77 | B = SpMat(segs0 - 1, segs0 - 1); 78 | for (int i = 0; i < segs0 - 2; ++i) { 79 | A.insert(i, i) = 4; 80 | A.insert(i, i + 1) = 1; 81 | A.insert(i + 1, i) = 1; 82 | } 83 | A.insert(segs0 - 2, segs0 - 2) = 4; 84 | for (int i = 0; i < segs0 - 2; ++i) { 85 | B.insert(i + 1, i) = -3; 86 | B.insert(i, i + 1) = 3; 87 | } 88 | C = SpMat(segs0, segs0 - 1); 89 | M = SpMat(segs0, segs0 - 1); 90 | E = SpMat(segs0, segs0 - 1); 91 | F = SpMat(segs0, segs0 - 1); 92 | for (int i = 0; i < segs0 - 1; ++i) { 93 | C.insert(i, i) = 3; 94 | C.insert(i + 1, i) = -3; 95 | } 96 | for (int i = 0; i < segs0 - 1; ++i) { 97 | M.insert(i, i) = -1; 98 | M.insert(i + 1, i) = -2; 99 | } 100 | for (int i = 0; i < segs0 - 1; ++i) { 101 | E.insert(i, i) = -2; 102 | E.insert(i + 1, i) = 2; 103 | } 104 | for (int i = 0; i < segs0 - 1; ++i) { 105 | F.insert(i, i) = 1; 106 | F.insert(i + 1, i) = 1; 107 | } 108 | solver.analyzePattern(A); 109 | solver.factorize(A); 110 | d_D_d_p = solver.solve(B); 111 | d_c_d_p = C + M * d_D_d_p; 112 | d_d_d_p = E + F * d_D_d_p; 113 | return 0; 114 | } 115 | 116 | // convert path points to cubic spline coefficients 117 | int path_to_coeff(const Mat& path, Vec& coeff) { 118 | int num = path.rows(); 119 | coeff.resize(num * 2 - 4); 120 | for (int i = 1; i < num - 1; ++i) { 121 | coeff(i - 1) = path(i, 0); 122 | coeff(i + num - 3) = path(i, 1); 123 | } 124 | return 0; 125 | } 126 | 127 | // convert cubic spline coefficients to path points 128 | int coeff_to_path(const Vec& coeff, const Mat& path0, Mat& path) { 129 | int num = path0.rows(); 130 | path.resize(num, 2); 131 | path.row(0) = path0.row(0); 132 | path.row(num - 1) = path0.row(num - 1); 133 | for (int i = 1; i < num - 1; ++i) { 134 | path(i, 0) = coeff(i - 1); 135 | path(i, 1) = coeff(i + num - 3); 136 | } 137 | return 0; 138 | } 139 | 140 | // convert path points to cubic spline parameters 141 | int path_to_param(const Mat& path, Mat& param) { 142 | int segs = path.rows() - 1; 143 | Vec Dx(Vec::Zero(segs + 1)); 144 | Vec Dy(Vec::Zero(segs + 1)); 145 | param.resize(segs, 8); 146 | Dx.segment(1, segs - 1) = 3 * solver.solve(path.block(2, 0, segs - 1, 1) - 147 | path.block(0, 0, segs - 1, 1)); 148 | Dy.segment(1, segs - 1) = 3 * solver.solve(path.block(2, 1, segs - 1, 1) - 149 | path.block(0, 1, segs - 1, 1)); 150 | for (int i = 0; i < segs; ++i) { 151 | param(i, 0) = path(i, 0); 152 | param(i, 1) = Dx(i); 153 | param(i, 2) = 3 * (path(i + 1, 0) - path(i, 0)) - 2 * Dx(i) - Dx(i + 1); 154 | param(i, 3) = 2 * (path(i, 0) - path(i + 1, 0)) + Dx(i) + Dx(i + 1); 155 | param(i, 4) = path(i, 1); 156 | param(i, 5) = Dy(i); 157 | param(i, 6) = 3 * (path(i + 1, 1) - path(i, 1)) - 2 * Dy(i) - Dy(i + 1); 158 | param(i, 7) = 2 * (path(i, 1) - path(i + 1, 1)) + Dy(i) + Dy(i + 1); 159 | } 160 | return 0; 161 | } 162 | 163 | // convert cubic spline parameters to interpolated points 164 | int param_to_interpolated(const Mat& param, Mat interp, int segs) { 165 | int end = segs * param.rows(); 166 | interp.resize(end + 1, 2); 167 | for (int i = 0; i < param.rows(); ++i) { 168 | for (int j = 0; j < segs; ++j) { 169 | double t = j * 1.0 / segs; 170 | interp(j + i * segs, 0) = param(i, 0) + param(i, 1) * t + 171 | param(i, 2) * t * t + param(i, 3) * t * t * t; 172 | interp(j + i * segs, 1) = param(i, 4) + param(i, 5) * t + 173 | param(i, 6) * t * t + param(i, 7) * t * t * t; 174 | } 175 | interp(end, 0) = param(end, 0) + param(end, 1) + param(end, 2) + param(end, 3); 176 | interp(end, 1) = param(end, 4) + param(end, 5) + param(end, 6) + param(end, 7); 177 | } 178 | return 0; 179 | } 180 | 181 | // parameter to q, dq_ds, d2q_ds2 for TOPP problem 182 | // int param_to_q(const Mat& param, Vec& s, Mat& q, Mat& qv, Mat& qa, int segs) { 183 | // int num = param.rows(); 184 | // int pts = num * segs + 1; 185 | // auto cubic_x = [¶m](int i, double t) { 186 | // return param(i, 0) + param(i, 1) * t + param(i, 2) * t * t + 187 | // param(i, 3) * t * t * t; 188 | // }; 189 | // auto cubic_y = [&](int i, double t) { 190 | // return param(i, 4) + param(i, 5) * t + param(i, 6) * t * t + 191 | // param(i, 7) * t * t * t; 192 | // }; 193 | // auto dx_dt = [&](int i, double t) { 194 | // return param(i, 1) + 2 * param(i, 2) * t + 3 * param(i, 3) * t * t; 195 | // }; 196 | // auto dy_dt = [&](int i, double t) { 197 | // return param(i, 5) + 2 * param(i, 6) * t + 3 * param(i, 7) * t * t; 198 | // }; 199 | // auto ds_dt = [&](int i, double t) { 200 | // return std::sqrt(std::pow(dx_dt(i, t), 2) + std::pow(dy_dt(i, t), 2)); 201 | // }; 202 | // auto d2x_dt2 = [&](int i, double t) { 203 | // return 2 * param(i, 2) + 6 * param(i, 3) * t; 204 | // }; 205 | // auto d2y_dt2 = [&](int i, double t) { 206 | // return 2 * param(i, 6) + 6 * param(i, 7) * t; 207 | // }; 208 | // s.resize(pts); 209 | // q.resize(pts, 2); 210 | // qv.resize(pts, 2); 211 | // qa.resize(pts, 2); 212 | // double dt = 1.0 / segs; 213 | // double current_length = 0; 214 | // for (int i = 0; i < num; ++i) { 215 | // for (int j = 0; j < segs; ++j) { 216 | // double t = j * dt; 217 | // s(j + i * segs) = current_length; 218 | // q(j + i * segs, 0) = cubic_x(i, t); 219 | // q(j + i * segs, 1) = cubic_y(i, t); 220 | // // average by numeric integration in Simpson's rule 221 | // double dx_dt0 = 222 | // (dx_dt(i, t) + 4 * dx_dt(i, t + 0.5 * dt) + dx_dt(i, t + dt)) / 6; 223 | // double dy_dt0 = 224 | // (dy_dt(i, t) + 4 * dy_dt(i, t + 0.5 * dt) + dy_dt(i, t + dt)) / 6; 225 | // double ds_dt0 = 226 | // (ds_dt(i, t) + 4 * ds_dt(i, t + 0.5 * dt) + ds_dt(i, t + dt)) / 6; 227 | // current_length += dt * ds_dt0; 228 | // double d2x_dt20 = 229 | // (d2x_dt2(i, t) + 4 * d2x_dt2(i, t + 0.5 * dt) + d2x_dt2(i, t + dt)) / 230 | // 6; 231 | // double d2y_dt20 = 232 | // (d2y_dt2(i, t) + 4 * d2y_dt2(i, t + 0.5 * dt) + d2y_dt2(i, t + dt)) / 233 | // 6; 234 | // qv(j + i * segs, 0) = dx_dt0 / ds_dt0; 235 | // qv(j + i * segs, 1) = dy_dt0 / ds_dt0; 236 | // qa(j + i * segs, 0) = (d2x_dt20 * dy_dt0 - d2y_dt20 * dx_dt0) * dy_dt0 / 237 | // std::pow(ds_dt0, 4); 238 | // qa(j + i * segs, 1) = (d2y_dt20 * dx_dt0 - d2x_dt20 * dy_dt0) * dx_dt0 / 239 | // std::pow(ds_dt0, 4); 240 | // } 241 | // } 242 | // s(pts - 1) = current_length; 243 | // qv(pts - 1, 0) = qv(pts - 2, 0); 244 | // qv(pts - 1, 1) = qv(pts - 2, 1); 245 | // qa(pts - 1, 0) = qa(pts - 2, 0); 246 | // qa(pts - 1, 1) = qa(pts - 2, 1); 247 | // return 0; 248 | // } 249 | int param_to_q(const Mat& param, Vec& s, Mat& q, Mat& qv, Mat& qa, int segs) { 250 | int num = param.rows(); 251 | int pts = num * segs + 1; 252 | auto cubic_x = [¶m](int i, double t) { 253 | return param(i, 0) + param(i, 1) * t + param(i, 2) * t * t + 254 | param(i, 3) * t * t * t; 255 | }; 256 | auto cubic_y = [&](int i, double t) { 257 | return param(i, 4) + param(i, 5) * t + param(i, 6) * t * t + 258 | param(i, 7) * t * t * t; 259 | }; 260 | auto dx_dt = [&](int i, double t) { 261 | return param(i, 1) + 2 * param(i, 2) * t + 3 * param(i, 3) * t * t; 262 | }; 263 | auto dy_dt = [&](int i, double t) { 264 | return param(i, 5) + 2 * param(i, 6) * t + 3 * param(i, 7) * t * t; 265 | }; 266 | auto ds_dt = [&](int i, double t) { 267 | return std::sqrt(std::pow(dx_dt(i, t), 2) + std::pow(dy_dt(i, t), 2)); 268 | }; 269 | auto d2x_dt2 = [&](int i, double t) { 270 | return 2 * param(i, 2) + 6 * param(i, 3) * t; 271 | }; 272 | auto d2y_dt2 = [&](int i, double t) { 273 | return 2 * param(i, 6) + 6 * param(i, 7) * t; 274 | }; 275 | s.resize(pts); 276 | q.resize(pts, 2); 277 | qv.resize(pts, 2); 278 | qa.resize(pts, 2); 279 | double dt = 1.0 / segs; 280 | double current_length = 0; 281 | for (int i = 0; i < num; ++i) { 282 | for (int j = 0; j < segs; ++j) { 283 | double t = j * dt; 284 | s(j + i * segs) = current_length; 285 | q(j + i * segs, 0) = cubic_x(i, t); 286 | q(j + i * segs, 1) = cubic_y(i, t); 287 | // average by numeric integration in Simpson's rule 288 | double dx_dt0 = 289 | (dx_dt(i, t) + 4 * dx_dt(i, t + 0.5 * dt) + dx_dt(i, t + dt)) / 6; 290 | double dy_dt0 = 291 | (dy_dt(i, t) + 4 * dy_dt(i, t + 0.5 * dt) + dy_dt(i, t + dt)) / 6; 292 | double ds_dt0 = 293 | (ds_dt(i, t) + 4 * ds_dt(i, t + 0.5 * dt) + ds_dt(i, t + dt)) / 6; 294 | current_length += dt * ds_dt0; 295 | double d2x_dt20 = 296 | (d2x_dt2(i, t) + 4 * d2x_dt2(i, t + 0.5 * dt) + d2x_dt2(i, t + dt)) / 297 | 6; 298 | double d2y_dt20 = 299 | (d2y_dt2(i, t) + 4 * d2y_dt2(i, t + 0.5 * dt) + d2y_dt2(i, t + dt)) / 300 | 6; 301 | qv(j + i * segs, 0) = dx_dt0 / ds_dt0; 302 | qv(j + i * segs, 1) = dy_dt0 / ds_dt0; 303 | qa(j + i * segs, 0) = (d2x_dt20 * dy_dt0 - d2y_dt20 * dx_dt0) * dy_dt0 / 304 | std::pow(ds_dt0, 4); 305 | qa(j + i * segs, 1) = (d2y_dt20 * dx_dt0 - d2x_dt20 * dy_dt0) * dx_dt0 / 306 | std::pow(ds_dt0, 4); 307 | } 308 | } 309 | s(pts - 1) = current_length; 310 | qv(pts - 1, 0) = qv(pts - 2, 0); 311 | qv(pts - 1, 1) = qv(pts - 2, 1); 312 | qa(pts - 1, 0) = qa(pts - 2, 0); 313 | qa(pts - 1, 1) = qa(pts - 2, 1); 314 | return 0; 315 | } 316 | 317 | // cost function for optimization methods 318 | static double cost_function(void* ptr, const Vec& x, Vec& g) { 319 | cubicSplineOpt* opt = reinterpret_cast(ptr); 320 | int segs = opt->segs0; 321 | double cost_energy; 322 | double cost_potential; 323 | Vec g_energy(x.size()); 324 | Vec g_potential(x.size()); 325 | Mat path, param; 326 | opt->coeff_to_path(x, opt->points0, path); 327 | 328 | opt->path_to_param(path, param); 329 | Vec cx = param.col(2); 330 | Vec dx = param.col(3); 331 | Vec cy = param.col(6); 332 | Vec dy = param.col(7); 333 | 334 | // sretch energy goal 335 | cost_energy = 12 * dx.dot(dx) + 12 * cx.dot(dx) + 4 * cx.dot(cx) + 336 | 12 * dy.dot(dy) + 12 * cy.dot(dy) + 4 * cy.dot(cy); 337 | 338 | Vec d_energy_d_x = (12 * dx + 8 * cx).transpose() * opt->d_c_d_p + 339 | (24 * dx + 12 * cx).transpose() * opt->d_d_d_p; 340 | Vec d_energy_d_y = (12 * dy + 8 * cy).transpose() * opt->d_c_d_p + 341 | (24 * dy + 12 * cy).transpose() * opt->d_d_d_p; 342 | g_energy.head(segs - 1) = d_energy_d_x; 343 | g_energy.tail(segs - 1) = d_energy_d_y; 344 | 345 | // distance risk goal 346 | cost_potential = 0; 347 | Vec dist_grad(2); 348 | for (int i = 1; i < segs; ++i) { 349 | double x1 = path(i, 0); 350 | double y1 = path(i, 1); 351 | double dist = opt->scene0.dist_field(x1, y1, &dist_grad); 352 | // f(x) = c0*exp(c1*x); 353 | if (dist < 1e-4) { 354 | cost_potential += 0; 355 | g_potential(i - 1) = g_potential(i + segs - 2) = 0; 356 | } else { 357 | cost_potential += opt->c0 * std::exp(opt->c1 * dist); 358 | double gain = opt->c1 * opt->c0 * std::exp(opt->c1 * dist); 359 | g_potential(i - 1) = gain * dist_grad(0); 360 | g_potential(i + segs - 2) = gain * dist_grad(1); 361 | } 362 | } 363 | 364 | g = g_energy + g_potential; 365 | // std::cout << "{{{{ step2: " 366 | // << " line0:" << path.row(0) << " \nline1:" << path.row(1) << std::endl; 367 | return cost_energy + cost_potential; 368 | } 369 | 370 | // information monitoring in L-BFGS algorithm 371 | static int monitor_progress(void* /*instance*/, const Eigen::VectorXd& /*x*/, 372 | const Eigen::VectorXd& g, const double fx, 373 | const double /*step*/, const int k, 374 | const int /*ls*/) { 375 | std::cout << "================================\n" 376 | << "Iteration: " << k << "\n" 377 | << "Cost: " << fx << "\n" 378 | << "Gradient Inf Norm: " << g.cwiseAbs().maxCoeff() << std::endl; 379 | return 0; 380 | } 381 | 382 | // optimizing the spline by L-BFGS algorithm 383 | int path_opt_lbfgs() { 384 | double final_cost; 385 | Vec coeff; 386 | path_to_coeff(points0, coeff); 387 | int ret = 0; 388 | lbfgs::lbfgs_parameter_t lbfgs_param; 389 | lbfgs_param.s_curv_coeff = 0.7; 390 | lbfgs_param.f_dec_coeff = 1e-4; 391 | lbfgs_param.g_epsilon = 1e-5; 392 | lbfgs_param.past = 3; 393 | lbfgs_param.delta = 1e-4; 394 | 395 | std::cout << "{{{{ step1: " << points0.rows() << " " << points0.cols() << " " << coeff.size() << std::endl; 396 | 397 | ret = lbfgs::lbfgs_optimize(coeff, final_cost, cost_function, 398 | monitor_progress, this, 399 | lbfgs_param); 400 | 401 | coeff_to_path(coeff, points0, points1); 402 | path_to_param(points1, param1); 403 | 404 | std::cout << std::setprecision(6) << "================================" 405 | << "\n" 406 | << "L-BFGS Optimization Returned: " << ret << "\n" 407 | << "Minimized Cost: " << final_cost << std::endl; 408 | 409 | std::cout << param1 << std::endl; 410 | 411 | return ret; 412 | } 413 | 414 | // topp preparations 415 | int topp_prepare(int segs) { 416 | return param_to_q(param1, s1, q1, qv1, qa1, segs); 417 | } 418 | }; 419 | 420 | #endif // SPLINE_OPT_01 -------------------------------------------------------------------------------- /src/sl_hw5/launch/main.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | -------------------------------------------------------------------------------- /src/sl_hw5/launch/rviz_config/rviz_test0.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | - /MarkerArray1 10 | - /MarkerArray1/Namespaces1 11 | - /Axes1 12 | Splitter Ratio: 0.7202215790748596 13 | Tree Height: 359 14 | - Class: rviz/Selection 15 | Name: Selection 16 | - Class: rviz/Tool Properties 17 | Expanded: 18 | - /2D Pose Estimate1 19 | - /2D Nav Goal1 20 | - /Publish Point1 21 | Name: Tool Properties 22 | Splitter Ratio: 0.5886790156364441 23 | - Class: rviz/Views 24 | Expanded: 25 | - /Current View1 26 | Name: Views 27 | Splitter Ratio: 0.5 28 | - Class: rviz/Time 29 | Experimental: false 30 | Name: Time 31 | SyncMode: 0 32 | SyncSource: "" 33 | Preferences: 34 | PromptSaveOnExit: true 35 | Toolbars: 36 | toolButtonStyle: 2 37 | Visualization Manager: 38 | Class: "" 39 | Displays: 40 | - Alpha: 0.5 41 | Cell Size: 1 42 | Class: rviz/Grid 43 | Color: 160; 160; 164 44 | Enabled: false 45 | Line Style: 46 | Line Width: 0.029999999329447746 47 | Value: Lines 48 | Name: Grid 49 | Normal Cell Count: 0 50 | Offset: 51 | X: 0 52 | Y: 0 53 | Z: 0 54 | Plane: XY 55 | Plane Cell Count: 10 56 | Reference Frame: 57 | Value: false 58 | - Class: rviz/MarkerArray 59 | Enabled: true 60 | Marker Topic: /sl_hw5_array 61 | Name: MarkerArray 62 | Namespaces: 63 | sl_hw5_test: true 64 | Queue Size: 102 65 | Value: true 66 | - Alpha: 0.4000000059604645 67 | Class: rviz/Axes 68 | Enabled: true 69 | Length: 10 70 | Name: Axes 71 | Radius: 0.10000000149011612 72 | Reference Frame: 73 | Show Trail: false 74 | Value: true 75 | Enabled: true 76 | Global Options: 77 | Background Color: 48; 48; 48 78 | Default Light: true 79 | Fixed Frame: map 80 | Frame Rate: 120 81 | Name: root 82 | Tools: 83 | - Class: rviz/Interact 84 | Hide Inactive Objects: true 85 | - Class: rviz/MoveCamera 86 | - Class: rviz/Select 87 | - Class: rviz/FocusCamera 88 | - Class: rviz/Measure 89 | - Class: rviz/SetInitialPose 90 | Theta std deviation: 0.2617993950843811 91 | Topic: /initialpose 92 | X std deviation: 0.5 93 | Y std deviation: 0.5 94 | - Class: rviz/SetGoal 95 | Topic: /move_base_simple/goal 96 | - Class: rviz/PublishPoint 97 | Single click: true 98 | Topic: /clicked_point 99 | Value: true 100 | Views: 101 | Current: 102 | Class: rviz/Orbit 103 | Distance: 59.623226165771484 104 | Enable Stereo Rendering: 105 | Stereo Eye Separation: 0.05999999865889549 106 | Stereo Focal Distance: 1 107 | Swap Stereo Eyes: false 108 | Value: false 109 | Field of View: 0.7853981852531433 110 | Focal Point: 111 | X: 0.22603222727775574 112 | Y: 0.8453767895698547 113 | Z: 4.581772327423096 114 | Focal Shape Fixed Size: true 115 | Focal Shape Size: 0.05000000074505806 116 | Invert Z Axis: false 117 | Name: Current View 118 | Near Clip Distance: 0.009999999776482582 119 | Pitch: 1.5697963237762451 120 | Target Frame: 121 | Yaw: 4.715488910675049 122 | Saved: ~ 123 | Window Geometry: 124 | Displays: 125 | collapsed: true 126 | Height: 946 127 | Hide Left Dock: true 128 | Hide Right Dock: true 129 | QMainWindow State: 000000ff00000000fd00000004000000000000016b0000035dfc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c00610079007300000000410000035d000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000035dfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a0056006900650077007300000000410000035d000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007600000003efc0100000002fb0000000800540069006d00650100000000000007600000030000fffffffb0000000800540069006d00650100000000000004500000000000000000000007600000030f00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 130 | Selection: 131 | collapsed: false 132 | Time: 133 | collapsed: false 134 | Tool Properties: 135 | collapsed: false 136 | Views: 137 | collapsed: true 138 | Width: 1888 139 | X: 30 140 | Y: 28 141 | -------------------------------------------------------------------------------- /src/sl_hw5/launch/test0.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | -------------------------------------------------------------------------------- /src/sl_hw5/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | sl_hw5 4 | 0.0.3 5 | sl_hw5 by tkalpha 6 | 7 | 8 | 9 | 10 | tkalpha 11 | 12 | 13 | 14 | 15 | 16 | MIT 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | catkin 52 | geometry_msgs 53 | roscpp 54 | rospy 55 | std_msgs 56 | geometry_msgs 57 | roscpp 58 | rospy 59 | std_msgs 60 | geometry_msgs 61 | roscpp 62 | rospy 63 | std_msgs 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | -------------------------------------------------------------------------------- /src/sl_hw5/src/main.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include "ros1_disp/scene_dis.hpp" 4 | #include "ros1_disp/spline_opt_dis.hpp" 5 | #include "ros1_disp/topp_dis.hpp" 6 | #include "scene/scene_astar.hpp" 7 | #include "traj_optim/spline_opt.hpp" 8 | #include "traj_optim/conic_alm_topp.hpp" 9 | 10 | int main(int argc, char** argv) { 11 | ros::init(argc, argv, "sl_hw5"); 12 | ros::NodeHandle nh; 13 | // initialize a scene 14 | sceneAstar sa(-50., -25., 0., 100., 50., 0.5, 3.6, 12.9, 3, 12); 15 | std::cout << "[[[ scene generation OK!]]]" << std::endl; 16 | sa.random_obstacles(26); 17 | std::cout << "[[[ obstacles generation OK!]]]" << std::endl; 18 | // std:: cout <<"{{{"< 2 | 3 | #include "ros1_disp/rviz_dis.hpp" 4 | #include "scene/pavel_valtr.hpp" 5 | #include "sdqp/sdqp.hpp" 6 | 7 | int main(int argc, char** argv) { 8 | ros::init(argc, argv, "sl_hw5"); 9 | ros::NodeHandle nh; 10 | // initialize a polygon 11 | rviz1DisSimp rd0(nh, "sl_hw5_array", "map", "sl_hw5_test"); 12 | Mat poly1(4, 2); 13 | poly1 << 0, 0, 1, 0, 2, 1, 0, 1; 14 | randPoly rp; 15 | rp.generate(8, 5.6, poly1); 16 | 17 | // formulate the constraints 18 | double x0 = -5, y0 = -4; 19 | int num = poly1.rows(); 20 | Eigen::Matrix Q(Mat::Identity(2, 2)); 21 | Eigen::Matrix c; 22 | Eigen::Matrix x; 23 | c << -2 * x0, -2 * y0; 24 | Mat A(num, 2); 25 | Vec b(num); 26 | int j = num - 1; 27 | for (int k = 0; k < num; ++k) { 28 | A(k, 0) = -(poly1(j, 1) - poly1(k, 1)); 29 | A(k, 1) = -(poly1(k, 0) - poly1(j, 0)); 30 | b(k) = (A(k, 0) * poly1(j, 0) + A(k, 1) * poly1(j, 1)); 31 | j = k; 32 | } 33 | double minobj = sdqp::sdqp<2>(Q, c, A, b, x); 34 | std::cout << Q << "\n\n" 35 | << c << "\n\n" 36 | << A << "\n\n" 37 | << b << std::endl; 38 | std::cout << x.transpose() << " " << minobj << std::endl; 39 | 40 | // draw out the scene 41 | rd0.add_convex_poly_flat(0, poly1, 0.1); 42 | rd0.add_arrow(1, x0, y0, 0, x(0), x(1), 0, 0.2); 43 | rd0.send(); 44 | } 45 | 46 | // #include 47 | // #include 48 | 49 | // #include 50 | 51 | // int main(int argc, char** argv) { 52 | // ros::init(argc, argv, "points_and_lines"); 53 | // ros::NodeHandle n; 54 | // ros::Publisher marker_pub = 55 | // n.advertise("sl_hw5_array", 10); 56 | 57 | // ros::Rate r(30); 58 | 59 | // float f = 0.0; 60 | // while (ros::ok()) { 61 | // // %Tag(MARKER_INIT)% 62 | // visualization_msgs::Marker points, line_strip, line_list; 63 | // points.header.frame_id = line_strip.header.frame_id = 64 | // line_list.header.frame_id = "map"; 65 | // points.header.stamp = line_strip.header.stamp = line_list.header.stamp = 66 | // ros::Time::now(); 67 | // points.ns = line_strip.ns = line_list.ns = "points_and_lines"; 68 | // points.action = line_strip.action = line_list.action = 69 | // visualization_msgs::Marker::ADD; 70 | // points.pose.orientation.w = line_strip.pose.orientation.w = 71 | // line_list.pose.orientation.w = 1.0; 72 | // // %EndTag(MARKER_INIT)% 73 | 74 | // // %Tag(ID)% 75 | // points.id = 0; 76 | // line_strip.id = 1; 77 | // line_list.id = 2; 78 | // // %EndTag(ID)% 79 | 80 | // // %Tag(TYPE)% 81 | // points.type = visualization_msgs::Marker::POINTS; 82 | // line_strip.type = visualization_msgs::Marker::LINE_STRIP; 83 | // line_list.type = visualization_msgs::Marker::LINE_LIST; 84 | // // %EndTag(TYPE)% 85 | 86 | // // %Tag(SCALE)% 87 | // // POINTS markers use x and y scale for width/height respectively 88 | // points.scale.x = 0.2; 89 | // points.scale.y = 0.2; 90 | 91 | // // LINE_STRIP/LINE_LIST markers use only the x component of scale, for the 92 | // // line width 93 | // line_strip.scale.x = 0.1; 94 | // line_list.scale.x = 0.1; 95 | // // %EndTag(SCALE)% 96 | 97 | // // %Tag(COLOR)% 98 | // // Points are green 99 | // points.color.g = 1.0f; 100 | // points.color.a = 1.0; 101 | 102 | // // Line strip is blue 103 | // line_strip.color.b = 1.0; 104 | // line_strip.color.a = 1.0; 105 | 106 | // // Line list is red 107 | // line_list.color.r = 1.0; 108 | // line_list.color.a = 1.0; 109 | // // %EndTag(COLOR)% 110 | 111 | // // %Tag(HELIX)% 112 | // // Create the vertices for the points and lines 113 | // for (uint32_t i = 0; i < 100; ++i) { 114 | // float y = 5 * sin(f + i / 100.0f * 2 * M_PI); 115 | // float z = 5 * cos(f + i / 100.0f * 2 * M_PI); 116 | 117 | // geometry_msgs::Point p; 118 | // p.x = (int32_t)i - 50; 119 | // p.y = y; 120 | // p.z = z; 121 | 122 | // points.points.push_back(p); 123 | // line_strip.points.push_back(p); 124 | 125 | // // The line list needs two points for each line 126 | // line_list.points.push_back(p); 127 | // p.z += 1.0; 128 | // line_list.points.push_back(p); 129 | // } 130 | // // %EndTag(HELIX)% 131 | 132 | // marker_pub.publish(points); 133 | // marker_pub.publish(line_strip); 134 | // marker_pub.publish(line_list); 135 | 136 | // r.sleep(); 137 | 138 | // f += 0.04; 139 | // } 140 | // } 141 | // // %EndTag(FULLTEXT)% 142 | -------------------------------------------------------------------------------- /tkalpha-第5次作业.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tk166/Shenlan_Numerical-Optimization_in_Robotics/89129bd22531a55a481c60c3aa8da2a7fc51a1c0/tkalpha-第5次作业.pdf --------------------------------------------------------------------------------