├── Course_1 ├── readme.md ├── L1_HW_Solution │ ├── readme.md │ └── Example_1 │ │ ├── readme.md │ │ ├── readme.pdf │ │ ├── result.png │ │ ├── Declaration.txt │ │ └── GradientDescent_2D.py ├── L1_HW_Hints.pdf └── L1_HW_Instruction.pdf ├── Course_3 ├── readme.md ├── Example_1 │ ├── readme.md │ └── L3_HW_Solution │ │ ├── L3_HW_Solution │ │ ├── task_3 │ │ │ ├── car_msgs │ │ │ │ ├── msg │ │ │ │ │ └── CarCmd.msg │ │ │ │ ├── CMakeLists.txt │ │ │ │ └── package.xml │ │ │ ├── car_simulator │ │ │ │ ├── config │ │ │ │ │ ├── car_simulator.yaml │ │ │ │ │ └── rviz_sim.rviz │ │ │ │ ├── nodelet_plugin.xml │ │ │ │ ├── launch │ │ │ │ │ └── car_simulator.launch │ │ │ │ ├── package.xml │ │ │ │ ├── CMakeLists.txt │ │ │ │ └── src │ │ │ │ │ └── car_simulator_nodelet.cpp │ │ │ └── mpc_car │ │ │ │ ├── nodelet_plugin.xml │ │ │ │ ├── launch │ │ │ │ ├── play_bag.launch │ │ │ │ └── simulation.launch │ │ │ │ ├── package.xml │ │ │ │ ├── CMakeLists.txt │ │ │ │ ├── include │ │ │ │ └── iosqp │ │ │ │ │ └── iosqp.hpp │ │ │ │ ├── src │ │ │ │ └── mpc_car_nodelet.cpp │ │ │ │ └── config │ │ │ │ └── mpc_car.yaml │ │ ├── task_2.gif │ │ ├── alm_smooth.gif │ │ ├── CHENXXX - 数值优化第三章作业.pdf │ │ ├── task_2_2 │ │ │ └── collision_distance_computation │ │ │ │ ├── launch │ │ │ │ ├── run.launch │ │ │ │ └── show.rviz │ │ │ │ ├── package.xml │ │ │ │ ├── src │ │ │ │ └── main.cpp │ │ │ │ └── CMakeLists.txt │ │ └── task_2_1 │ │ │ ├── CMakeLists.txt │ │ │ └── example │ │ │ └── sdqp_example.cpp │ │ └── Declaration.txt ├── L3_Hw_Hints.pdf └── L3_HW_Instruction.pdf ├── Course_2 ├── Example_1 │ ├── readme.md │ ├── src │ │ ├── CMakeLists.txt │ │ ├── gcopter │ │ │ ├── launch │ │ │ │ ├── curve_gen.launch │ │ │ │ └── global_planning.launch │ │ │ ├── config │ │ │ │ ├── curve_gen.yaml │ │ │ │ └── global_planning.yaml │ │ │ ├── CMakeLists.txt │ │ │ ├── package.xml │ │ │ ├── src │ │ │ │ └── curve_gen.cpp │ │ │ └── include │ │ │ │ └── gcopter │ │ │ │ ├── cubic_curve.hpp │ │ │ │ ├── path_smoother.hpp │ │ │ │ ├── geo_utils.hpp │ │ │ │ ├── voxel_dilater.hpp │ │ │ │ ├── voxel_map.hpp │ │ │ │ └── sfc_gen.hpp │ │ ├── map_gen │ │ │ └── mockamap │ │ │ │ ├── README.md │ │ │ │ ├── launch │ │ │ │ ├── post2d.launch │ │ │ │ ├── maze3d.launch │ │ │ │ ├── maze2d.launch │ │ │ │ ├── perlin3d.launch │ │ │ │ └── mockamap.launch │ │ │ │ ├── include │ │ │ │ ├── perlinnoise.hpp │ │ │ │ └── maps.hpp │ │ │ │ ├── package.xml │ │ │ │ ├── src │ │ │ │ ├── mockamap.cpp │ │ │ │ ├── perlinnoise.cpp │ │ │ │ └── ces_randommap.cpp │ │ │ │ ├── config │ │ │ │ └── rviz.rviz │ │ │ │ └── CMakeLists.txt │ │ ├── LICENSE │ │ └── README.md │ └── report.pdf ├── L2_HW_Hints.pdf └── L2_HW_Instruction.pdf ├── Course_4 ├── Example_1 │ ├── readme,md │ └── L4_HW_Solution │ │ ├── L4_HW_Solution │ │ ├── SDQP │ │ │ ├── CMakeFiles │ │ │ │ ├── progress.marks │ │ │ │ ├── sdqp_example.dir │ │ │ │ │ ├── progress.make │ │ │ │ │ ├── link.txt │ │ │ │ │ ├── example │ │ │ │ │ │ └── sdqp_example.cpp.o │ │ │ │ │ ├── flags.make │ │ │ │ │ ├── cmake_clean.cmake │ │ │ │ │ ├── DependInfo.cmake │ │ │ │ │ └── build.make │ │ │ │ ├── cmake.check_cache │ │ │ │ ├── feature_tests.bin │ │ │ │ ├── 3.10.2 │ │ │ │ │ ├── CompilerIdC │ │ │ │ │ │ └── a.out │ │ │ │ │ ├── CompilerIdCXX │ │ │ │ │ │ └── a.out │ │ │ │ │ ├── CMakeDetermineCompilerABI_C.bin │ │ │ │ │ ├── CMakeDetermineCompilerABI_CXX.bin │ │ │ │ │ ├── CMakeSystem.cmake │ │ │ │ │ ├── CMakeCCompiler.cmake │ │ │ │ │ └── CMakeCXXCompiler.cmake │ │ │ │ ├── TargetDirectories.txt │ │ │ │ ├── CMakeDirectoryInformation.cmake │ │ │ │ ├── feature_tests.c │ │ │ │ ├── Makefile.cmake │ │ │ │ └── Makefile2 │ │ │ ├── readme │ │ │ ├── sdqp_example │ │ │ ├── CMakeLists.txt │ │ │ ├── example │ │ │ │ └── sdqp_example.cpp │ │ │ ├── cmake_install.cmake │ │ │ ├── README.md │ │ │ └── Makefile │ │ └── socp │ │ │ ├── readme │ │ │ ├── CMakeLists.txt │ │ │ └── socp.cpp │ │ └── Declaration.txt ├── L4_HW_Hints.pdf └── L4_HW_Instruction.pdf └── README.md /Course_1/readme.md: -------------------------------------------------------------------------------- 1 | 2 | -------------------------------------------------------------------------------- /Course_3/readme.md: -------------------------------------------------------------------------------- 1 | 2 | -------------------------------------------------------------------------------- /Course_2/Example_1/readme.md: -------------------------------------------------------------------------------- 1 | 2 | -------------------------------------------------------------------------------- /Course_3/Example_1/readme.md: -------------------------------------------------------------------------------- 1 | 2 | -------------------------------------------------------------------------------- /Course_4/Example_1/readme,md: -------------------------------------------------------------------------------- 1 | 2 | -------------------------------------------------------------------------------- /Course_1/L1_HW_Solution/readme.md: -------------------------------------------------------------------------------- 1 | 2 | -------------------------------------------------------------------------------- /Course_1/L1_HW_Solution/Example_1/readme.md: -------------------------------------------------------------------------------- 1 | 2 | -------------------------------------------------------------------------------- /Course_2/Example_1/src/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | /opt/ros/noetic/share/catkin/cmake/toplevel.cmake -------------------------------------------------------------------------------- /Course_4/Example_1/L4_HW_Solution/L4_HW_Solution/SDQP/CMakeFiles/progress.marks: -------------------------------------------------------------------------------- 1 | 2 2 | -------------------------------------------------------------------------------- /Course_3/Example_1/L3_HW_Solution/L3_HW_Solution/task_3/car_msgs/msg/CarCmd.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | float64 a 3 | float64 delta -------------------------------------------------------------------------------- /Course_1/L1_HW_Hints.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Jinqi-J/Numerical-Optimization-in-Robotics-Homework/HEAD/Course_1/L1_HW_Hints.pdf -------------------------------------------------------------------------------- /Course_2/L2_HW_Hints.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Jinqi-J/Numerical-Optimization-in-Robotics-Homework/HEAD/Course_2/L2_HW_Hints.pdf -------------------------------------------------------------------------------- /Course_3/L3_Hw_Hints.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Jinqi-J/Numerical-Optimization-in-Robotics-Homework/HEAD/Course_3/L3_Hw_Hints.pdf -------------------------------------------------------------------------------- /Course_4/L4_HW_Hints.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Jinqi-J/Numerical-Optimization-in-Robotics-Homework/HEAD/Course_4/L4_HW_Hints.pdf -------------------------------------------------------------------------------- /Course_1/L1_HW_Instruction.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Jinqi-J/Numerical-Optimization-in-Robotics-Homework/HEAD/Course_1/L1_HW_Instruction.pdf -------------------------------------------------------------------------------- /Course_2/Example_1/report.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Jinqi-J/Numerical-Optimization-in-Robotics-Homework/HEAD/Course_2/Example_1/report.pdf -------------------------------------------------------------------------------- /Course_2/L2_HW_Instruction.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Jinqi-J/Numerical-Optimization-in-Robotics-Homework/HEAD/Course_2/L2_HW_Instruction.pdf -------------------------------------------------------------------------------- /Course_3/Example_1/L3_HW_Solution/Declaration.txt: -------------------------------------------------------------------------------- 1 | 特别声明： 2 | 3 | 本项作业仅供参考学习与交流讨论，如有深蓝学员用于抄袭深蓝学院课程作业，一经发现取消该次作业成绩，并取消该学员在本门课程暨机器人学中的数值优化所有作业的优秀资格。 -------------------------------------------------------------------------------- /Course_3/L3_HW_Instruction.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Jinqi-J/Numerical-Optimization-in-Robotics-Homework/HEAD/Course_3/L3_HW_Instruction.pdf -------------------------------------------------------------------------------- /Course_4/Example_1/L4_HW_Solution/Declaration.txt: -------------------------------------------------------------------------------- 1 | 特别声明： 2 | 3 | 本项作业仅供参考学习与交流讨论，如有深蓝学员用于抄袭深蓝学院课程作业，一经发现取消该次作业成绩，并取消该学员在本门课程暨机器人学中的数值优化所有作业的优秀资格。 -------------------------------------------------------------------------------- /Course_4/L4_HW_Instruction.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Jinqi-J/Numerical-Optimization-in-Robotics-Homework/HEAD/Course_4/L4_HW_Instruction.pdf -------------------------------------------------------------------------------- /Course_4/Example_1/L4_HW_Solution/L4_HW_Solution/SDQP/CMakeFiles/sdqp_example.dir/progress.make: -------------------------------------------------------------------------------- 1 | CMAKE_PROGRESS_1 = 1 2 | CMAKE_PROGRESS_2 = 2 3 | 4 | -------------------------------------------------------------------------------- /Course_3/Example_1/L3_HW_Solution/L3_HW_Solution/task_3/car_simulator/config/car_simulator.yaml: -------------------------------------------------------------------------------- 1 | l: 1.282 2 | x: 0.0 3 | y: 0.0 4 | phi: 0.0 5 | v: 0.0 6 | 7 | delay: 0.125 -------------------------------------------------------------------------------- /Course_4/Example_1/L4_HW_Solution/L4_HW_Solution/SDQP/CMakeFiles/cmake.check_cache: -------------------------------------------------------------------------------- 1 | # This file is generated by cmake for dependency checking of the CMakeCache.txt file 2 | -------------------------------------------------------------------------------- /Course_1/L1_HW_Solution/Example_1/readme.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Jinqi-J/Numerical-Optimization-in-Robotics-Homework/HEAD/Course_1/L1_HW_Solution/Example_1/readme.pdf -------------------------------------------------------------------------------- /Course_1/L1_HW_Solution/Example_1/result.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Jinqi-J/Numerical-Optimization-in-Robotics-Homework/HEAD/Course_1/L1_HW_Solution/Example_1/result.png -------------------------------------------------------------------------------- /Course_4/Example_1/L4_HW_Solution/L4_HW_Solution/SDQP/readme: -------------------------------------------------------------------------------- 1 | ./sdqp_example 1e-4 2 | The program will ouput x and the optimal value. 3 | For more information, please see the committed homework. 4 | -------------------------------------------------------------------------------- /Course_3/Example_1/L3_HW_Solution/L3_HW_Solution/task_2.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Jinqi-J/Numerical-Optimization-in-Robotics-Homework/HEAD/Course_3/Example_1/L3_HW_Solution/L3_HW_Solution/task_2.gif -------------------------------------------------------------------------------- /Course_3/Example_1/L3_HW_Solution/L3_HW_Solution/alm_smooth.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Jinqi-J/Numerical-Optimization-in-Robotics-Homework/HEAD/Course_3/Example_1/L3_HW_Solution/L3_HW_Solution/alm_smooth.gif -------------------------------------------------------------------------------- /Course_1/L1_HW_Solution/Example_1/Declaration.txt: -------------------------------------------------------------------------------- 1 | 特别声明： 2 | 3 | 本作业示例由邱润其同学提供，关于示例内容，有交流意愿的朋友可以联系其本人，此处给出邮箱地址：qiurunqi@foxmail.com 4 | 5 | 本项作业仅供参考学习与交流讨论，如有深蓝学员用于抄袭深蓝学院课程作业，一经发现取消该次作业成绩，并取消该学员在本门课程暨机器人学中的数值优化所有作业的优秀资格 -------------------------------------------------------------------------------- /Course_4/Example_1/L4_HW_Solution/L4_HW_Solution/SDQP/CMakeFiles/sdqp_example.dir/link.txt: -------------------------------------------------------------------------------- 1 | /usr/bin/c++ -std=c++11 -O3 -Wall -fPIC -rdynamic CMakeFiles/sdqp_example.dir/example/sdqp_example.cpp.o -o sdqp_example 2 | -------------------------------------------------------------------------------- /Course_4/Example_1/L4_HW_Solution/L4_HW_Solution/SDQP/sdqp_example: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Jinqi-J/Numerical-Optimization-in-Robotics-Homework/HEAD/Course_4/Example_1/L4_HW_Solution/L4_HW_Solution/SDQP/sdqp_example -------------------------------------------------------------------------------- /Course_3/Example_1/L3_HW_Solution/L3_HW_Solution/CHENXXX - 数值优化第三章作业.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Jinqi-J/Numerical-Optimization-in-Robotics-Homework/HEAD/Course_3/Example_1/L3_HW_Solution/L3_HW_Solution/CHENXXX - 数值优化第三章作业.pdf -------------------------------------------------------------------------------- /Course_4/Example_1/L4_HW_Solution/L4_HW_Solution/SDQP/CMakeFiles/feature_tests.bin: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Jinqi-J/Numerical-Optimization-in-Robotics-Homework/HEAD/Course_4/Example_1/L4_HW_Solution/L4_HW_Solution/SDQP/CMakeFiles/feature_tests.bin -------------------------------------------------------------------------------- /Course_4/Example_1/L4_HW_Solution/L4_HW_Solution/SDQP/CMakeFiles/3.10.2/CompilerIdC/a.out: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Jinqi-J/Numerical-Optimization-in-Robotics-Homework/HEAD/Course_4/Example_1/L4_HW_Solution/L4_HW_Solution/SDQP/CMakeFiles/3.10.2/CompilerIdC/a.out -------------------------------------------------------------------------------- /Course_4/Example_1/L4_HW_Solution/L4_HW_Solution/SDQP/CMakeFiles/TargetDirectories.txt: -------------------------------------------------------------------------------- 1 | /home/pizhou/chapter_4/SDQP/CMakeFiles/rebuild_cache.dir 2 | /home/pizhou/chapter_4/SDQP/CMakeFiles/sdqp_example.dir 3 | /home/pizhou/chapter_4/SDQP/CMakeFiles/edit_cache.dir 4 | -------------------------------------------------------------------------------- /Course_4/Example_1/L4_HW_Solution/L4_HW_Solution/socp/readme: -------------------------------------------------------------------------------- 1 | minimize c.transpose() * x 2 | subject to : 3 | n (n can be arbitrary) socp constraints: 4 | A_1 * x + b 5 | ... 6 | A_n * x + b 7 | and equality constraint: 8 | G * x = h 9 | 10 | -------------------------------------------------------------------------------- /Course_4/Example_1/L4_HW_Solution/L4_HW_Solution/SDQP/CMakeFiles/3.10.2/CompilerIdCXX/a.out: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Jinqi-J/Numerical-Optimization-in-Robotics-Homework/HEAD/Course_4/Example_1/L4_HW_Solution/L4_HW_Solution/SDQP/CMakeFiles/3.10.2/CompilerIdCXX/a.out -------------------------------------------------------------------------------- /Course_3/Example_1/L3_HW_Solution/L3_HW_Solution/task_3/mpc_car/nodelet_plugin.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | -------------------------------------------------------------------------------- /Course_4/Example_1/L4_HW_Solution/L4_HW_Solution/SDQP/CMakeFiles/3.10.2/CMakeDetermineCompilerABI_C.bin: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Jinqi-J/Numerical-Optimization-in-Robotics-Homework/HEAD/Course_4/Example_1/L4_HW_Solution/L4_HW_Solution/SDQP/CMakeFiles/3.10.2/CMakeDetermineCompilerABI_C.bin -------------------------------------------------------------------------------- /Course_4/Example_1/L4_HW_Solution/L4_HW_Solution/SDQP/CMakeFiles/3.10.2/CMakeDetermineCompilerABI_CXX.bin: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Jinqi-J/Numerical-Optimization-in-Robotics-Homework/HEAD/Course_4/Example_1/L4_HW_Solution/L4_HW_Solution/SDQP/CMakeFiles/3.10.2/CMakeDetermineCompilerABI_CXX.bin -------------------------------------------------------------------------------- /Course_4/Example_1/L4_HW_Solution/L4_HW_Solution/SDQP/CMakeFiles/sdqp_example.dir/example/sdqp_example.cpp.o: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Jinqi-J/Numerical-Optimization-in-Robotics-Homework/HEAD/Course_4/Example_1/L4_HW_Solution/L4_HW_Solution/SDQP/CMakeFiles/sdqp_example.dir/example/sdqp_example.cpp.o -------------------------------------------------------------------------------- /Course_3/Example_1/L3_HW_Solution/L3_HW_Solution/task_3/car_simulator/nodelet_plugin.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | -------------------------------------------------------------------------------- /Course_3/Example_1/L3_HW_Solution/L3_HW_Solution/task_3/mpc_car/launch/play_bag.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /Course_4/Example_1/L4_HW_Solution/L4_HW_Solution/SDQP/CMakeFiles/sdqp_example.dir/flags.make: -------------------------------------------------------------------------------- 1 | # CMAKE generated file: DO NOT EDIT! 2 | # Generated by "Unix Makefiles" Generator, CMake Version 3.10 3 | 4 | # compile CXX with /usr/bin/c++ 5 | CXX_FLAGS = -std=c++11 -O3 -Wall -fPIC 6 | 7 | CXX_DEFINES = 8 | 9 | CXX_INCLUDES = -I/home/pizhou/chapter_4/SDQP/include -I/usr/include/eigen3 10 | 11 | -------------------------------------------------------------------------------- /Course_3/Example_1/L3_HW_Solution/L3_HW_Solution/task_2_2/collision_distance_computation/launch/run.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | -------------------------------------------------------------------------------- /Course_3/Example_1/L3_HW_Solution/L3_HW_Solution/task_3/car_simulator/launch/car_simulator.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | -------------------------------------------------------------------------------- /Course_4/Example_1/L4_HW_Solution/L4_HW_Solution/SDQP/CMakeFiles/sdqp_example.dir/cmake_clean.cmake: -------------------------------------------------------------------------------- 1 | file(REMOVE_RECURSE 2 | "CMakeFiles/sdqp_example.dir/example/sdqp_example.cpp.o" 3 | "sdqp_example.pdb" 4 | "sdqp_example" 5 | ) 6 | 7 | # Per-language clean rules from dependency scanning. 8 | foreach(lang CXX) 9 | include(CMakeFiles/sdqp_example.dir/cmake_clean_${lang}.cmake OPTIONAL) 10 | endforeach() 11 | -------------------------------------------------------------------------------- /Course_4/Example_1/L4_HW_Solution/L4_HW_Solution/socp/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | 3 | project(socp) 4 | 5 | set(CMAKE_CXX_FLAGS "-std=c++11") 6 | set(CMAKE_BUILD_TYPE "Release") 7 | set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall -fPIC") 8 | 9 | find_package(Eigen3 REQUIRED) 10 | 11 | include_directories( 12 | include 13 | ${EIGEN3_INCLUDE_DIRS} 14 | ) 15 | 16 | add_executable(${PROJECT_NAME} socp.cpp) 17 | -------------------------------------------------------------------------------- /Course_2/Example_1/src/gcopter/launch/curve_gen.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | -------------------------------------------------------------------------------- /Course_4/Example_1/L4_HW_Solution/L4_HW_Solution/SDQP/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | 3 | project(sdqp) 4 | 5 | set(CMAKE_CXX_FLAGS "-std=c++11") 6 | set(CMAKE_BUILD_TYPE "Release") 7 | set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall -fPIC") 8 | 9 | find_package(Eigen3 REQUIRED) 10 | 11 | include_directories( 12 | include 13 | ${EIGEN3_INCLUDE_DIRS} 14 | ) 15 | 16 | add_executable(${PROJECT_NAME}_example example/sdqp_example.cpp) 17 | -------------------------------------------------------------------------------- /Course_3/Example_1/L3_HW_Solution/L3_HW_Solution/task_2_1/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | 3 | project(sdqp) 4 | 5 | set(CMAKE_CXX_FLAGS "-std=c++11") 6 | set(CMAKE_BUILD_TYPE "Release") 7 | set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall -fPIC") 8 | 9 | find_package(Eigen3 REQUIRED) 10 | 11 | include_directories( 12 | include 13 | ${EIGEN3_INCLUDE_DIRS} 14 | ) 15 | 16 | add_executable(${PROJECT_NAME}_example example/sdqp_example.cpp) 17 | -------------------------------------------------------------------------------- /Course_2/Example_1/src/map_gen/mockamap/README.md: -------------------------------------------------------------------------------- 1 | # mockamap 2 | a simple map generator based on ROS 3 | 4 | demo cases: 5 | 6 | * perlin3d map 7 | 8 | ![Alt text](https://github.com/HKUST-Aerial-Robotics/mockamap/blob/master/images/perlin3d.png) 9 | 10 | * post2d map 11 | 12 | ![Alt text](https://github.com/HKUST-Aerial-Robotics/mockamap/blob/master/images/post2d.png) 13 | 14 | @misc{mockamap, 15 | author = "William.Wu", 16 | title = "mockamap", 17 | howpublished = "\url{https://github.com/HKUST-Aerial-Robotics/mockamap }", 18 | } 19 | -------------------------------------------------------------------------------- /Course_4/Example_1/L4_HW_Solution/L4_HW_Solution/SDQP/CMakeFiles/3.10.2/CMakeSystem.cmake: -------------------------------------------------------------------------------- 1 | set(CMAKE_HOST_SYSTEM "Linux-5.4.0-122-generic") 2 | set(CMAKE_HOST_SYSTEM_NAME "Linux") 3 | set(CMAKE_HOST_SYSTEM_VERSION "5.4.0-122-generic") 4 | set(CMAKE_HOST_SYSTEM_PROCESSOR "x86_64") 5 | 6 | 7 | 8 | set(CMAKE_SYSTEM "Linux-5.4.0-122-generic") 9 | set(CMAKE_SYSTEM_NAME "Linux") 10 | set(CMAKE_SYSTEM_VERSION "5.4.0-122-generic") 11 | set(CMAKE_SYSTEM_PROCESSOR "x86_64") 12 | 13 | set(CMAKE_CROSSCOMPILING "FALSE") 14 | 15 | set(CMAKE_SYSTEM_LOADED 1) 16 | -------------------------------------------------------------------------------- /Course_3/Example_1/L3_HW_Solution/L3_HW_Solution/task_3/car_msgs/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(car_msgs) 3 | 4 | add_compile_options(-std=c++17) 5 | 6 | find_package(catkin REQUIRED COMPONENTS 7 | message_generation 8 | std_msgs 9 | ) 10 | 11 | add_message_files( 12 | DIRECTORY msg 13 | FILES 14 | CarCmd.msg 15 | ) 16 | 17 | generate_messages( 18 | DEPENDENCIES 19 | std_msgs 20 | ) 21 | 22 | catkin_package( 23 | CATKIN_DEPENDS message_runtime std_msgs 24 | ) 25 | 26 | include_directories( 27 | ${catkin_INCLUDE_DIRS} 28 | ) -------------------------------------------------------------------------------- /Course_3/Example_1/L3_HW_Solution/L3_HW_Solution/task_3/mpc_car/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | mpc_car 4 | 0.0.0 5 | The mpc_car package 6 | 7 | ji 8 | 9 | GPLv3 10 | 11 | catkin 12 | roscpp 13 | nodelet 14 | car_msgs 15 | 16 | 17 | 18 | 19 | 20 | -------------------------------------------------------------------------------- /Course_3/Example_1/L3_HW_Solution/L3_HW_Solution/task_3/car_simulator/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | car_simulator 4 | 0.0.0 5 | The car_simulator package 6 | 7 | ji 8 | 9 | GPLv3 10 | 11 | catkin 12 | roscpp 13 | nodelet 14 | car_msgs 15 | 16 | 17 | 18 | 19 | 20 | -------------------------------------------------------------------------------- /Course_3/Example_1/L3_HW_Solution/L3_HW_Solution/task_3/car_msgs/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | car_msgs 4 | 0.0.0 5 | The car_msgs package 6 | 7 | ji 8 | 9 | GPLv3 10 | 11 | catkin 12 | message_generation 13 | message_runtime 14 | message_runtime 15 | 16 | std_msgs 17 | 18 | 19 | -------------------------------------------------------------------------------- /Course_3/Example_1/L3_HW_Solution/L3_HW_Solution/task_3/mpc_car/launch/simulation.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | -------------------------------------------------------------------------------- /Course_2/Example_1/src/gcopter/config/curve_gen.yaml: -------------------------------------------------------------------------------- 1 | TargetTopic: '/move_base_simple/goal' 2 | 3 | PenaltyWeight: 100.0 4 | 5 | CircleObs: [ 0.0, 0.0, 4.0, 6 | 1.0, 8.0, 3.0, 7 | -8.0, 1.0, 2.0, 8 | -5.0, 16.0, 5.0, 9 | 13.0, 2.0, 6.0, 10 | -15.0, -12.0, 5.0, 11 | -18.0, 6.0, 8.0, 12 | 12.0, -15.0, 4.0, 13 | 20.0, 19.0, 4.5, 14 | -3.0, -18.0, 6.5] 15 | 16 | PieceLength: 0.5 17 | 18 | RelCostTol: 1.0e-6 19 | -------------------------------------------------------------------------------- /Course_4/Example_1/L4_HW_Solution/L4_HW_Solution/SDQP/CMakeFiles/CMakeDirectoryInformation.cmake: -------------------------------------------------------------------------------- 1 | # CMAKE generated file: DO NOT EDIT! 2 | # Generated by "Unix Makefiles" Generator, CMake Version 3.10 3 | 4 | # Relative path conversion top directories. 5 | set(CMAKE_RELATIVE_PATH_TOP_SOURCE "/home/pizhou/chapter_4/SDQP") 6 | set(CMAKE_RELATIVE_PATH_TOP_BINARY "/home/pizhou/chapter_4/SDQP") 7 | 8 | # Force unix paths in dependencies. 9 | set(CMAKE_FORCE_UNIX_PATHS 1) 10 | 11 | 12 | # The C and CXX include file regular expressions for this directory. 13 | set(CMAKE_C_INCLUDE_REGEX_SCAN "^.*$") 14 | set(CMAKE_C_INCLUDE_REGEX_COMPLAIN "^$") 15 | set(CMAKE_CXX_INCLUDE_REGEX_SCAN ${CMAKE_C_INCLUDE_REGEX_SCAN}) 16 | set(CMAKE_CXX_INCLUDE_REGEX_COMPLAIN ${CMAKE_C_INCLUDE_REGEX_COMPLAIN}) 17 | -------------------------------------------------------------------------------- /Course_2/Example_1/src/gcopter/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | 3 | project(gcopter) 4 | 5 | set(CMAKE_CXX_FLAGS "-std=c++14") 6 | set(CMAKE_BUILD_TYPE "Release") 7 | set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall -fPIC") 8 | 9 | find_package(Eigen3 REQUIRED) 10 | find_package(ompl REQUIRED) 11 | 12 | find_package(catkin REQUIRED COMPONENTS 13 | roscpp 14 | std_msgs 15 | geometry_msgs 16 | sensor_msgs 17 | visualization_msgs 18 | ) 19 | 20 | include_directories( 21 | ${catkin_INCLUDE_DIRS} 22 | ${OMPL_INCLUDE_DIRS} 23 | ${EIGEN3_INCLUDE_DIRS} 24 | include 25 | ) 26 | 27 | catkin_package() 28 | 29 | add_executable(curve_gen src/curve_gen.cpp) 30 | 31 | target_link_libraries(curve_gen 32 | ${OMPL_LIBRARIES} 33 | ${catkin_LIBRARIES} 34 | ) 35 | -------------------------------------------------------------------------------- /Course_4/Example_1/L4_HW_Solution/L4_HW_Solution/SDQP/CMakeFiles/sdqp_example.dir/DependInfo.cmake: -------------------------------------------------------------------------------- 1 | # The set of languages for which implicit dependencies are needed: 2 | set(CMAKE_DEPENDS_LANGUAGES 3 | "CXX" 4 | ) 5 | # The set of files for implicit dependencies of each language: 6 | set(CMAKE_DEPENDS_CHECK_CXX 7 | "/home/pizhou/chapter_4/SDQP/example/sdqp_example.cpp" "/home/pizhou/chapter_4/SDQP/CMakeFiles/sdqp_example.dir/example/sdqp_example.cpp.o" 8 | ) 9 | set(CMAKE_CXX_COMPILER_ID "GNU") 10 | 11 | # The include file search paths: 12 | set(CMAKE_CXX_TARGET_INCLUDE_PATH 13 | "include" 14 | "/usr/include/eigen3" 15 | ) 16 | 17 | # Targets to which this target links. 18 | set(CMAKE_TARGET_LINKED_INFO_FILES 19 | ) 20 | 21 | # Fortran module output directory. 22 | set(CMAKE_Fortran_TARGET_MODULE_DIR "") 23 | -------------------------------------------------------------------------------- /Course_2/Example_1/src/gcopter/package.xml: -------------------------------------------------------------------------------- 1 | 2 | gcopter 3 | Zhepei Wang 4 | Zhepei Wang 5 | 0.1.0 6 | 7 | the gcopter package 8 | 9 | BSD 10 | 11 | catkin 12 | 13 | roscpp 14 | std_msgs 15 | geometry_msgs 16 | sensor_msgs 17 | visualization_msgs 18 | roscpp 19 | std_msgs 20 | geometry_msgs 21 | sensor_msgs 22 | visualization_msgs 23 | 24 | 25 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Numerical-Optimization-in-Robotics-Homework 2 | 3 | - This is the homework solution of 《Numerical Optimization in Robotics》 of Deepbule, ***only for studying***! 4 | 5 | ## Acknowledgement 6 | 7 | - ***No Cheating!!!***: It is of importance for you to konw that if you apply this repository for ***cheating*** in the homework, the TA(Teach Assistant) will perceive it and ***cancel your grade!*** 8 | 9 | - ***Few Instruction***: The examples in this repository were selected from the ***excellent homework of students*** in the course and the TA have ***already get the permits*** to use these homework. As a result, specific questions about the example content ***may not necessarily be answered*** by the teach assistants. 10 | 11 |
12 | 13 |
14 | -------------------------------------------------------------------------------- /Course_3/Example_1/L3_HW_Solution/L3_HW_Solution/task_3/car_simulator/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(car_simulator) 3 | 4 | add_compile_options(-std=c++17) 5 | set(CMAKE_BUILD_TYPE "Release") 6 | set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall") 7 | 8 | find_package(Eigen3 REQUIRED) 9 | find_package(catkin REQUIRED COMPONENTS 10 | roscpp 11 | nodelet 12 | car_msgs 13 | ) 14 | 15 | catkin_package( 16 | INCLUDE_DIRS ${EIGEN3_INCLUDE_DIR} 17 | CATKIN_DEPENDS roscpp nodelet car_msgs 18 | DEPENDS EIGEN3 19 | ) 20 | 21 | include_directories( 22 | ${EIGEN3_INCLUDE_DIR} 23 | ${catkin_INCLUDE_DIRS} 24 | ) 25 | 26 | add_library(${PROJECT_NAME}_nodelet 27 | src/${PROJECT_NAME}_nodelet.cpp 28 | ) 29 | 30 | target_link_libraries(${PROJECT_NAME}_nodelet 31 | ${catkin_LIBRARIES} 32 | ) 33 | 34 | add_dependencies(${PROJECT_NAME}_nodelet 35 | ${catkin_EXPORTED_TARGETS} 36 | ) 37 | -------------------------------------------------------------------------------- /Course_4/Example_1/L4_HW_Solution/L4_HW_Solution/SDQP/CMakeFiles/feature_tests.c: -------------------------------------------------------------------------------- 1 | 2 | const char features[] = {"\n" 3 | "C_FEATURE:" 4 | #if (__GNUC__ * 100 + __GNUC_MINOR__) >= 304 5 | "1" 6 | #else 7 | "0" 8 | #endif 9 | "c_function_prototypes\n" 10 | "C_FEATURE:" 11 | #if (__GNUC__ * 100 + __GNUC_MINOR__) >= 304 && defined(__STDC_VERSION__) && __STDC_VERSION__ >= 199901L 12 | "1" 13 | #else 14 | "0" 15 | #endif 16 | "c_restrict\n" 17 | "C_FEATURE:" 18 | #if (__GNUC__ * 100 + __GNUC_MINOR__) >= 406 && defined(__STDC_VERSION__) && __STDC_VERSION__ >= 201000L 19 | "1" 20 | #else 21 | "0" 22 | #endif 23 | "c_static_assert\n" 24 | "C_FEATURE:" 25 | #if (__GNUC__ * 100 + __GNUC_MINOR__) >= 304 && defined(__STDC_VERSION__) && __STDC_VERSION__ >= 199901L 26 | "1" 27 | #else 28 | "0" 29 | #endif 30 | "c_variadic_macros\n" 31 | 32 | }; 33 | 34 | int main(int argc, char** argv) { (void)argv; return features[argc]; } 35 | -------------------------------------------------------------------------------- /Course_3/Example_1/L3_HW_Solution/L3_HW_Solution/task_3/mpc_car/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(mpc_car) 3 | 4 | add_compile_options(-std=c++17) 5 | set(CMAKE_BUILD_TYPE "Release") 6 | set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall") 7 | 8 | find_package(Eigen3 REQUIRED) 9 | find_package(osqp REQUIRED) 10 | find_package(catkin REQUIRED COMPONENTS 11 | roscpp 12 | nodelet 13 | car_msgs 14 | ) 15 | 16 | catkin_package( 17 | INCLUDE_DIRS include ${EIGEN3_INCLUDE_DIR} 18 | CATKIN_DEPENDS roscpp nodelet car_msgs 19 | DEPENDS EIGEN3 20 | ) 21 | 22 | include_directories( 23 | include 24 | ${EIGEN3_INCLUDE_DIR} 25 | ${catkin_INCLUDE_DIRS} 26 | ) 27 | 28 | add_library(${PROJECT_NAME}_nodelet 29 | src/${PROJECT_NAME}_nodelet.cpp 30 | ) 31 | 32 | target_link_libraries(${PROJECT_NAME}_nodelet 33 | ${catkin_LIBRARIES} 34 | osqp::osqp 35 | ) 36 | 37 | add_dependencies(${PROJECT_NAME}_nodelet 38 | ${catkin_EXPORTED_TARGETS} 39 | ) 40 | -------------------------------------------------------------------------------- /Course_2/Example_1/src/map_gen/mockamap/launch/post2d.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 24 | 25 | -------------------------------------------------------------------------------- /Course_2/Example_1/src/gcopter/config/global_planning.yaml: -------------------------------------------------------------------------------- 1 | MapTopic: '/voxel_map' 2 | 3 | TargetTopic: '/move_base_simple/goal' 4 | 5 | DilateRadius: 0.5 6 | 7 | VoxelWidth: 0.25 8 | 9 | MapBound: [-25.0, 25.0, -25.0, 25.0, 0.0, 5.0] 10 | 11 | TimeoutRRT: 0.02 12 | 13 | MaxVelMag: 4.0 14 | 15 | MaxBdrMag: 2.1 16 | 17 | MaxTiltAngle: 1.05 18 | 19 | MinThrust: 2.0 20 | 21 | MaxThrust: 12.0 22 | 23 | VehicleMass: 0.61 24 | 25 | GravAcc: 9.8 26 | 27 | HorizDrag: 0.70 28 | 29 | VertDrag: 0.80 30 | 31 | ParasDrag: 0.01 32 | 33 | SpeedEps: 0.0001 34 | 35 | WeightT: 20.0 36 | 37 | ChiVec: [1.0e+4, 1.0e+4, 1.0e+4, 1.0e+4, 1.0e+5] 38 | 39 | SmoothingEps: 1.0e-2 40 | 41 | IntegralIntervs: 16 42 | 43 | RelCostTol: 1.0e-5 44 | -------------------------------------------------------------------------------- /Course_2/Example_1/src/map_gen/mockamap/launch/maze3d.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 26 | 27 | -------------------------------------------------------------------------------- /Course_2/Example_1/src/map_gen/mockamap/include/perlinnoise.hpp: -------------------------------------------------------------------------------- 1 | #ifndef PERLINNOISE_HPP 2 | #define PERLINNOISE_HPP 3 | 4 | #include 5 | 6 | // THIS CLASS IS A TRANSLATION TO C++14 FROM THE REFERENCE 7 | // JAVA IMPLEMENTATION OF THE IMPROVED PERLIN FUNCTION (see 8 | // http://mrl.nyu.edu/~perlin/noise/) 9 | // THE ORIGINAL JAVA IMPLEMENTATION IS COPYRIGHT 2002 KEN PERLIN 10 | 11 | // I ADDED AN EXTRA METHOD THAT GENERATES A NEW PERMUTATION VECTOR (THIS IS NOT 12 | // PRESENT IN THE ORIGINAL IMPLEMENTATION) 13 | 14 | class PerlinNoise 15 | { 16 | // The permutation vector 17 | std::vector p; 18 | 19 | public: 20 | // Initialize with the reference values for the permutation vector 21 | PerlinNoise(); 22 | // Generate a new permutation vector based on the value of seed 23 | PerlinNoise(unsigned int seed); 24 | // Get a noise value, for 2D images z can have any value 25 | double noise(double x, double y, double z); 26 | 27 | private: 28 | double fade(double t); 29 | double lerp(double t, double a, double b); 30 | double grad(int hash, double x, double y, double z); 31 | }; 32 | 33 | #endif // PERLINNOISE_HPP 34 | -------------------------------------------------------------------------------- /Course_2/Example_1/src/map_gen/mockamap/launch/maze2d.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 26 | 27 | -------------------------------------------------------------------------------- /Course_3/Example_1/L3_HW_Solution/L3_HW_Solution/task_2_1/example/sdqp_example.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include "sdqp/sdqp.hpp" 4 | 5 | using namespace std; 6 | using namespace Eigen; 7 | 8 | int main(int argc, char **argv) 9 | { 10 | int m = 7; 11 | Eigen::Matrix Q; 12 | Eigen::Matrix c; 13 | Eigen::Matrix x; // decision variables 14 | Eigen::Matrix A(m, 3); // constraint matrix 15 | Eigen::VectorXd b(m); // constraint bound 16 | 17 | Q << 2.0, 1.0, 1.0, 1.0, 2.0, 1.0, 1.0, 1.0, 2.0; 18 | c << 1.2, 2.5, -10.0; 19 | 20 | A << 1.0, 0.0, 0.0, 21 | 0.0, 1.0, 0.0, 22 | 0.0, 0.0, 1.0, 23 | -0.7, 0.5, 0.0, 24 | 0.5, -1.0, 0.0, 25 | 0.0, 0.13, -1.0, 26 | 0.1, -3.0, -1.3; 27 | b << 10.0, 10.0, 10.0, 1.7, -7.1, -3.31, 2.59; 28 | 29 | double minobj = sdqp::sdqp<3>(Q, c, A, b, x); 30 | 31 | std::cout << "optimal sol: " << x.transpose() << std::endl; 32 | std::cout << "optimal obj: " << minobj << std::endl; 33 | std::cout << "cons precision: " << (A * x - b).maxCoeff() << std::endl; 34 | 35 | return 0; 36 | } 37 | -------------------------------------------------------------------------------- /Course_2/Example_1/src/LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright(c) 2021 Zhepei Wang (wangzhepei@live.com) and Fei Gao (fgaoaa@zju.edu.cn) 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /Course_2/Example_1/src/map_gen/mockamap/launch/perlin3d.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 19 | 21 | 22 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | -------------------------------------------------------------------------------- /Course_2/Example_1/src/gcopter/launch/global_planning.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 6 | 7 | 8 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | -------------------------------------------------------------------------------- /Course_4/Example_1/L4_HW_Solution/L4_HW_Solution/SDQP/example/sdqp_example.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include "sdqp/sdqp.hpp" 4 | 5 | using namespace std; 6 | using namespace Eigen; 7 | 8 | int main(int argc, char **argv) 9 | { 10 | int m = 5; 11 | Eigen::Matrix Q; 12 | Eigen::Matrix c; 13 | Eigen::Matrix x; // decision variables 14 | Eigen::Matrix A(m, 3); // constraint matrix 15 | Eigen::VectorXd b(m); // constraint bound 16 | 17 | Q << 8.0, -6, 2.0, -6, 6, -3, 2, -3, 2; 18 | c << 1.0, 3.0, -2.0; 19 | 20 | A << 0.0, -1.0, -2.0, 21 | -1.0, 1.0, -3.0, 22 | 1.0, -2.0, 0.0, 23 | -1.0, -2.0, -1.0, 24 | 3.0, 5.0, 1.0; 25 | b << -1, 2, 7, 2, -1; 26 | 27 | Eigen::Matrix y; // decision variables 28 | y << -103.0/97, -93.0/97, 95.0/97; 29 | 30 | double coeff = 1; 31 | Vector3d xi = Vector3d::Zero(); 32 | while(true) 33 | { 34 | Eigen::Matrix P= Q + 2 * Eigen::Matrix3d::Identity(); 35 | sdqp::sdqp<3>(P, c - 2 * xi, A, b, x); 36 | cout << "x = " << x << endl; 37 | if( (x - xi).norm() <= atof(argv[1])) 38 | { 39 | break; 40 | } 41 | xi = x; 42 | } 43 | 44 | std::cout << "coeff :" << coeff << std::endl; 45 | std::cout << "optimal sol: " << x.transpose() << std::endl; 46 | std::cout << "optimal obj: " << 0.5 * x.transpose() * Q * x + c.transpose() * x<< std::endl; 47 | 48 | return 0; 49 | } 50 | -------------------------------------------------------------------------------- /Course_2/Example_1/src/map_gen/mockamap/include/maps.hpp: -------------------------------------------------------------------------------- 1 | #ifndef MAPS_HPP 2 | #define MAPS_HPP 3 | 4 | #include 5 | 6 | #include 7 | #include 8 | #include 9 | 10 | namespace mocka { 11 | 12 | class Maps { 13 | public: 14 | typedef struct BasicInfo { 15 | ros::NodeHandle *nh_private; 16 | int sizeX; 17 | int sizeY; 18 | int sizeZ; 19 | int seed; 20 | double scale; 21 | sensor_msgs::PointCloud2 *output; 22 | pcl::PointCloud *cloud; 23 | } BasicInfo; 24 | 25 | BasicInfo getInfo() const; 26 | void setInfo(const BasicInfo &value); 27 | 28 | public: 29 | Maps(); 30 | 31 | public: 32 | void generate(int type); 33 | 34 | private: 35 | BasicInfo info; 36 | 37 | private: 38 | void pcl2ros(); 39 | 40 | void perlin3D(); 41 | void maze2D(); 42 | void randomMapGenerate(); 43 | void Maze3DGen(); 44 | void recursiveDivision(int xl, int xh, int yl, int yh, Eigen::MatrixXi &maze); 45 | void recursizeDivisionMaze(Eigen::MatrixXi &maze); 46 | void optimizeMap(); 47 | }; 48 | 49 | class MazePoint { 50 | private: 51 | pcl::PointXYZ point; 52 | double dist1; 53 | double dist2; 54 | int point1; 55 | int point2; 56 | bool isdoor; 57 | 58 | public: 59 | pcl::PointXYZ getPoint(); 60 | int getPoint1(); 61 | int getPoint2(); 62 | double getDist1(); 63 | double getDist2(); 64 | void setPoint(pcl::PointXYZ p); 65 | void setPoint1(int p); 66 | void setPoint2(int p); 67 | void setDist1(double set); 68 | void setDist2(double set); 69 | }; 70 | 71 | } // namespace mocka 72 | 73 | #endif // MAPS_HPP 74 | -------------------------------------------------------------------------------- /Course_4/Example_1/L4_HW_Solution/L4_HW_Solution/SDQP/cmake_install.cmake: -------------------------------------------------------------------------------- 1 | # Install script for directory: /home/pizhou/chapter_4/SDQP 2 | 3 | # Set the install prefix 4 | if(NOT DEFINED CMAKE_INSTALL_PREFIX) 5 | set(CMAKE_INSTALL_PREFIX "/usr/local") 6 | endif() 7 | string(REGEX REPLACE "/$" "" CMAKE_INSTALL_PREFIX "${CMAKE_INSTALL_PREFIX}") 8 | 9 | # Set the install configuration name. 10 | if(NOT DEFINED CMAKE_INSTALL_CONFIG_NAME) 11 | if(BUILD_TYPE) 12 | string(REGEX REPLACE "^[^A-Za-z0-9_]+" "" 13 | CMAKE_INSTALL_CONFIG_NAME "${BUILD_TYPE}") 14 | else() 15 | set(CMAKE_INSTALL_CONFIG_NAME "Release") 16 | endif() 17 | message(STATUS "Install configuration: \"${CMAKE_INSTALL_CONFIG_NAME}\"") 18 | endif() 19 | 20 | # Set the component getting installed. 21 | if(NOT CMAKE_INSTALL_COMPONENT) 22 | if(COMPONENT) 23 | message(STATUS "Install component: \"${COMPONENT}\"") 24 | set(CMAKE_INSTALL_COMPONENT "${COMPONENT}") 25 | else() 26 | set(CMAKE_INSTALL_COMPONENT) 27 | endif() 28 | endif() 29 | 30 | # Install shared libraries without execute permission? 31 | if(NOT DEFINED CMAKE_INSTALL_SO_NO_EXE) 32 | set(CMAKE_INSTALL_SO_NO_EXE "1") 33 | endif() 34 | 35 | # Is this installation the result of a crosscompile? 36 | if(NOT DEFINED CMAKE_CROSSCOMPILING) 37 | set(CMAKE_CROSSCOMPILING "FALSE") 38 | endif() 39 | 40 | if(CMAKE_INSTALL_COMPONENT) 41 | set(CMAKE_INSTALL_MANIFEST "install_manifest_${CMAKE_INSTALL_COMPONENT}.txt") 42 | else() 43 | set(CMAKE_INSTALL_MANIFEST "install_manifest.txt") 44 | endif() 45 | 46 | string(REPLACE ";" "\n" CMAKE_INSTALL_MANIFEST_CONTENT 47 | "${CMAKE_INSTALL_MANIFEST_FILES}") 48 | file(WRITE "/home/pizhou/chapter_4/SDQP/${CMAKE_INSTALL_MANIFEST}" 49 | "${CMAKE_INSTALL_MANIFEST_CONTENT}") 50 | -------------------------------------------------------------------------------- /Course_4/Example_1/L4_HW_Solution/L4_HW_Solution/SDQP/CMakeFiles/Makefile.cmake: -------------------------------------------------------------------------------- 1 | # CMAKE generated file: DO NOT EDIT! 2 | # Generated by "Unix Makefiles" Generator, CMake Version 3.10 3 | 4 | # The generator used is: 5 | set(CMAKE_DEPENDS_GENERATOR "Unix Makefiles") 6 | 7 | # The top level Makefile was generated from the following files: 8 | set(CMAKE_MAKEFILE_DEPENDS 9 | "CMakeCache.txt" 10 | "CMakeFiles/3.10.2/CMakeCCompiler.cmake" 11 | "CMakeFiles/3.10.2/CMakeCXXCompiler.cmake" 12 | "CMakeFiles/3.10.2/CMakeSystem.cmake" 13 | "CMakeLists.txt" 14 | "/usr/lib/cmake/eigen3/Eigen3Config.cmake" 15 | "/usr/lib/cmake/eigen3/Eigen3ConfigVersion.cmake" 16 | "/usr/lib/cmake/eigen3/Eigen3Targets.cmake" 17 | "/usr/share/cmake-3.10/Modules/CMakeCInformation.cmake" 18 | "/usr/share/cmake-3.10/Modules/CMakeCXXInformation.cmake" 19 | "/usr/share/cmake-3.10/Modules/CMakeCommonLanguageInclude.cmake" 20 | "/usr/share/cmake-3.10/Modules/CMakeGenericSystem.cmake" 21 | "/usr/share/cmake-3.10/Modules/CMakeLanguageInformation.cmake" 22 | "/usr/share/cmake-3.10/Modules/CMakeSystemSpecificInformation.cmake" 23 | "/usr/share/cmake-3.10/Modules/CMakeSystemSpecificInitialize.cmake" 24 | "/usr/share/cmake-3.10/Modules/Compiler/CMakeCommonCompilerMacros.cmake" 25 | "/usr/share/cmake-3.10/Modules/Compiler/GNU-C.cmake" 26 | "/usr/share/cmake-3.10/Modules/Compiler/GNU-CXX.cmake" 27 | "/usr/share/cmake-3.10/Modules/Compiler/GNU.cmake" 28 | "/usr/share/cmake-3.10/Modules/Platform/Linux-GNU-C.cmake" 29 | "/usr/share/cmake-3.10/Modules/Platform/Linux-GNU-CXX.cmake" 30 | "/usr/share/cmake-3.10/Modules/Platform/Linux-GNU.cmake" 31 | "/usr/share/cmake-3.10/Modules/Platform/Linux.cmake" 32 | "/usr/share/cmake-3.10/Modules/Platform/UnixPaths.cmake" 33 | ) 34 | 35 | # The corresponding makefile is: 36 | set(CMAKE_MAKEFILE_OUTPUTS 37 | "Makefile" 38 | "CMakeFiles/cmake.check_cache" 39 | ) 40 | 41 | # Byproducts of CMake generate step: 42 | set(CMAKE_MAKEFILE_PRODUCTS 43 | "CMakeFiles/CMakeDirectoryInformation.cmake" 44 | ) 45 | 46 | # Dependency information for all targets: 47 | set(CMAKE_DEPEND_INFO_FILES 48 | "CMakeFiles/sdqp_example.dir/DependInfo.cmake" 49 | ) 50 | -------------------------------------------------------------------------------- /Course_1/L1_HW_Solution/Example_1/GradientDescent_2D.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import matplotlib.pyplot as plt 3 | 4 | def Rosenbrock(x): 5 | return 100*(x[0]**2.0 - x[1])**2.0 + (x[0] - 1)**2 6 | 7 | def RosenbrockGradient(x): 8 | gradX1 = 400 * x[0] * (x[0]**2 - x[1]) + 2*(x[0] - 1) 9 | gradX2 = -200 * (x[0]**2 - x[1]) 10 | grad = np.array([gradX1, gradX2]) 11 | return grad 12 | 13 | def Armijo(x, grad): 14 | c = 0.1 15 | tau = 1 16 | x1 = x[0] - tau * grad[0] 17 | x2 = x[1] - tau * grad[1] 18 | nextX = np.array([x1, x2]) 19 | 20 | while Rosenbrock(nextX) > Rosenbrock(x) + (c * tau) * np.dot(grad, grad): 21 | tau *= 0.5 22 | x1 = x[0] - tau * grad[0] 23 | x2 = x[1] - tau * grad[1] 24 | nextX = np.array([x1, x2]) 25 | 26 | alpha = tau 27 | return alpha 28 | 29 | def LineSearch(x0): 30 | pointList = x0 31 | 32 | iter = 1 33 | maxIter = 5000 34 | 35 | x = x0 36 | error = 10 37 | tolerance = 0.01 38 | 39 | while (iter < maxIter) and (error > tolerance): 40 | grad = RosenbrockGradient(x) 41 | error = np.linalg.norm(grad) 42 | 43 | alpha = Armijo(x, grad) 44 | 45 | X0 = x[0] - alpha * grad[0] 46 | X1 = x[1] - alpha * grad[1] 47 | x = np.array([X0, X1]) 48 | 49 | iter += 1 50 | pointList = np.row_stack((pointList, x)) 51 | 52 | print("Iteration: ", iter, ", Error", error, ", Local Minimum: ", x) 53 | 54 | return x, pointList 55 | 56 | if __name__ == '__main__': 57 | x0 = np.array([0, 0]) 58 | globalMinimum, pointList = LineSearch(x0) 59 | 60 | x = np.arange(-0.5, 1.5, 0.1) 61 | y = np.arange(-0.5, 1.5, 0.1) 62 | X, Y = np.meshgrid(x, y) 63 | Z = 100*(X**2.0 - Y)**2.0 + (X - 1)**2 64 | 65 | plt.figure(figsize=(6, 6)) 66 | plt.contourf(X, Y, Z) 67 | plt.contour(X, Y, Z) 68 | 69 | lastI = 0 70 | for i in range(pointList.shape[0] - 1): 71 | 72 | if i % 300 == 0: 73 | plt.scatter(pointList[i, 0], pointList[i, 1], s=10) 74 | 75 | xAxis = np.array([pointList[lastI, 0], pointList[i, 0]]) 76 | yAxis = np.array([pointList[lastI, 1], pointList[i, 1]]) 77 | lastI = i 78 | 79 | plt.plot(xAxis, yAxis) 80 | plt.pause(0.1) 81 | 82 | plt.show() -------------------------------------------------------------------------------- /Course_2/Example_1/src/map_gen/mockamap/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | mockamap 4 | 0.1.0 5 | The mockamap package 6 | 7 | 8 | 9 | 10 | William.Wu 11 | 12 | 13 | 14 | 15 | 16 | GPLv3 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | William.Wu 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | catkin 44 | roscpp 45 | pcl_ros 46 | pcl_conversions 47 | roscpp 48 | pcl_ros 49 | pcl_conversions 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | -------------------------------------------------------------------------------- /Course_2/Example_1/src/map_gen/mockamap/launch/mockamap.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 22 | 24 | 25 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 56 | 57 | -------------------------------------------------------------------------------- /Course_4/Example_1/L4_HW_Solution/L4_HW_Solution/SDQP/CMakeFiles/3.10.2/CMakeCCompiler.cmake: -------------------------------------------------------------------------------- 1 | set(CMAKE_C_COMPILER "/usr/bin/cc") 2 | set(CMAKE_C_COMPILER_ARG1 "") 3 | set(CMAKE_C_COMPILER_ID "GNU") 4 | set(CMAKE_C_COMPILER_VERSION "7.5.0") 5 | set(CMAKE_C_COMPILER_VERSION_INTERNAL "") 6 | set(CMAKE_C_COMPILER_WRAPPER "") 7 | set(CMAKE_C_STANDARD_COMPUTED_DEFAULT "11") 8 | set(CMAKE_C_COMPILE_FEATURES "c_std_90;c_function_prototypes;c_std_99;c_restrict;c_variadic_macros;c_std_11;c_static_assert") 9 | set(CMAKE_C90_COMPILE_FEATURES "c_std_90;c_function_prototypes") 10 | set(CMAKE_C99_COMPILE_FEATURES "c_std_99;c_restrict;c_variadic_macros") 11 | set(CMAKE_C11_COMPILE_FEATURES "c_std_11;c_static_assert") 12 | 13 | set(CMAKE_C_PLATFORM_ID "Linux") 14 | set(CMAKE_C_SIMULATE_ID "") 15 | set(CMAKE_C_SIMULATE_VERSION "") 16 | 17 | 18 | 19 | set(CMAKE_AR "/usr/bin/ar") 20 | set(CMAKE_C_COMPILER_AR "/usr/bin/gcc-ar-7") 21 | set(CMAKE_RANLIB "/usr/bin/ranlib") 22 | set(CMAKE_C_COMPILER_RANLIB "/usr/bin/gcc-ranlib-7") 23 | set(CMAKE_LINKER "/usr/bin/ld") 24 | set(CMAKE_COMPILER_IS_GNUCC 1) 25 | set(CMAKE_C_COMPILER_LOADED 1) 26 | set(CMAKE_C_COMPILER_WORKS TRUE) 27 | set(CMAKE_C_ABI_COMPILED TRUE) 28 | set(CMAKE_COMPILER_IS_MINGW ) 29 | set(CMAKE_COMPILER_IS_CYGWIN ) 30 | if(CMAKE_COMPILER_IS_CYGWIN) 31 | set(CYGWIN 1) 32 | set(UNIX 1) 33 | endif() 34 | 35 | set(CMAKE_C_COMPILER_ENV_VAR "CC") 36 | 37 | if(CMAKE_COMPILER_IS_MINGW) 38 | set(MINGW 1) 39 | endif() 40 | set(CMAKE_C_COMPILER_ID_RUN 1) 41 | set(CMAKE_C_SOURCE_FILE_EXTENSIONS c;m) 42 | set(CMAKE_C_IGNORE_EXTENSIONS h;H;o;O;obj;OBJ;def;DEF;rc;RC) 43 | set(CMAKE_C_LINKER_PREFERENCE 10) 44 | 45 | # Save compiler ABI information. 46 | set(CMAKE_C_SIZEOF_DATA_PTR "8") 47 | set(CMAKE_C_COMPILER_ABI "ELF") 48 | set(CMAKE_C_LIBRARY_ARCHITECTURE "x86_64-linux-gnu") 49 | 50 | if(CMAKE_C_SIZEOF_DATA_PTR) 51 | set(CMAKE_SIZEOF_VOID_P "${CMAKE_C_SIZEOF_DATA_PTR}") 52 | endif() 53 | 54 | if(CMAKE_C_COMPILER_ABI) 55 | set(CMAKE_INTERNAL_PLATFORM_ABI "${CMAKE_C_COMPILER_ABI}") 56 | endif() 57 | 58 | if(CMAKE_C_LIBRARY_ARCHITECTURE) 59 | set(CMAKE_LIBRARY_ARCHITECTURE "x86_64-linux-gnu") 60 | endif() 61 | 62 | set(CMAKE_C_CL_SHOWINCLUDES_PREFIX "") 63 | if(CMAKE_C_CL_SHOWINCLUDES_PREFIX) 64 | set(CMAKE_CL_SHOWINCLUDES_PREFIX "${CMAKE_C_CL_SHOWINCLUDES_PREFIX}") 65 | endif() 66 | 67 | 68 | 69 | 70 | 71 | set(CMAKE_C_IMPLICIT_LINK_LIBRARIES "gcc;gcc_s;c;gcc;gcc_s") 72 | set(CMAKE_C_IMPLICIT_LINK_DIRECTORIES "/usr/lib/gcc/x86_64-linux-gnu/7;/usr/lib/x86_64-linux-gnu;/usr/lib;/lib/x86_64-linux-gnu;/lib") 73 | set(CMAKE_C_IMPLICIT_LINK_FRAMEWORK_DIRECTORIES "") 74 | -------------------------------------------------------------------------------- /Course_4/Example_1/L4_HW_Solution/L4_HW_Solution/SDQP/README.md: -------------------------------------------------------------------------------- 1 | # SDQP 2 | 3 | SDQP: Small-Dimensional Strictly Convex Quadratic Programming in Linear Time 4 | 5 | # About 6 | 7 | 1. This solver is super efficient for small-dimensional strictly convex QP with any constraint number, mostly encountered in computational geometry. It enjoys __linear complexity about the constraint number__. 8 | 9 | 2. The speed is faster than most numerical solvers in small-dimensional LP (<10) with a large constraints number (>100). 10 | 11 | 3. This solver computes __exact solutions or report infeasibility__. 12 | 13 | 4. This solver generalizes [Seidel's algorithm](https://link.springer.com/article/10.1007/BF02574699) __from LP to strictly convex QP__. 14 | 15 | 4. This solver is very elegant thus only [a header file](https://github.com/ZJU-FAST-Lab/SDQP/blob/main/include/sdqp/sdqp.hpp) with less than 400 lines is all you need. 16 | 17 | If our lib helps your research, please cite us 18 | ``` 19 | @misc{WANG2022SDQP, 20 | title={{SDQP: Small-Dimensional Strictly Convex Quadratic Programming in Linear Time}}, 21 | author={Wang, Zhepei and Gao, Fei}, 22 | year={2022}, 23 | url={https://github.com/ZJU-FAST-Lab/SDQP} 24 | } 25 | ``` 26 | 27 | # Interface 28 | 29 | To solve a linear programming: 30 | 31 | min 0.5 x' Q x + c' x, 32 | s.t. A x <= b, 33 | 34 | where x and c are d-dimensional vectors, Q an dxd positive definite matrix, b an m-dimensional vector, A an mxd matrix. It is assumed that d is small (<10) while m can be arbitrary value (1<= m <= 1e+8). 35 | 36 | Only one function is all you need: 37 | 38 | double sdqp(const Eigen::Matrix &Q, 39 | const Eigen::Matrix &c, 40 | const Eigen::Matrix &A, 41 | const Eigen::Matrix &b, 42 | Eigen::Matrix &x) 43 | 44 | Input: 45 | 46 | Q: positive definite matrix 47 | c: linear coefficient vector 48 | A: constraint matrix 49 | b: constraint bound vector 50 | 51 | Output: 52 | 53 | x: optimal solution if solved 54 | return: finite value if solved 55 | infinity if infeasible 56 | 57 | # Reference 58 | 59 | 1. Seidel, R., 1991. Small-dimensional linear programming and convex hulls made easy. Discrete & Computational Geometry, 6(3), pp.423-434. 60 | 61 | # Maintaince and Ack 62 | 63 | Thank Zijie Chen for fixing the conversion from QP to minimum norm. 64 | 65 | If any bug, please contact [Zhepei Wang](https://zhepeiwang.github.io/) (). 66 | -------------------------------------------------------------------------------- /Course_3/Example_1/L3_HW_Solution/L3_HW_Solution/task_2_2/collision_distance_computation/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | collision_distance_computation 4 | 0.0.0 5 | The collision_distance_computation package 6 | 7 | 8 | 9 | 10 | kris 11 | 12 | 13 | 14 | 15 | 16 | TODO 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 | roscpp 53 | rospy 54 | roscpp 55 | rospy 56 | roscpp 57 | rospy 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | -------------------------------------------------------------------------------- /Course_2/Example_1/src/map_gen/mockamap/src/mockamap.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | #include 9 | #include 10 | #include 11 | 12 | #include "maps.hpp" 13 | 14 | void 15 | optimizeMap(mocka::Maps::BasicInfo& in) 16 | { 17 | std::vector* temp = new std::vector; 18 | 19 | pcl::KdTreeFLANN kdtree; 20 | pcl::PointCloud::Ptr cloud(new pcl::PointCloud); 21 | 22 | cloud->width = in.cloud->width; 23 | cloud->height = in.cloud->height; 24 | cloud->points.resize(cloud->width * cloud->height); 25 | 26 | for (int i = 0; i < cloud->width; i++) 27 | { 28 | cloud->points[i].x = in.cloud->points[i].x; 29 | cloud->points[i].y = in.cloud->points[i].y; 30 | cloud->points[i].z = in.cloud->points[i].z; 31 | } 32 | 33 | kdtree.setInputCloud(cloud); 34 | double radius = 1.75 / in.scale; // 1.75 is the rounded up value of sqrt(3) 35 | 36 | for (int i = 0; i < cloud->width; i++) 37 | { 38 | std::vector pointIdxRadiusSearch; 39 | std::vector pointRadiusSquaredDistance; 40 | 41 | if (kdtree.radiusSearch(cloud->points[i], radius, pointIdxRadiusSearch, 42 | pointRadiusSquaredDistance) >= 27) 43 | { 44 | temp->push_back(i); 45 | } 46 | } 47 | for (int i = temp->size() - 1; i >= 0; i--) 48 | { 49 | in.cloud->points.erase(in.cloud->points.begin() + 50 | temp->at(i)); // erasing the enclosed points 51 | } 52 | in.cloud->width -= temp->size(); 53 | 54 | pcl::toROSMsg(*in.cloud, *in.output); 55 | in.output->header.frame_id = "odom"; 56 | ROS_INFO("finish: number of points after optimization %d", in.cloud->width); 57 | delete temp; 58 | return; 59 | } 60 | 61 | int 62 | main(int argc, char** argv) 63 | { 64 | ros::init(argc, argv, "mockamap"); 65 | ros::NodeHandle nh; 66 | ros::NodeHandle nh_private("~"); 67 | 68 | ros::Publisher pcl_pub = 69 | nh.advertise("mock_map", 1); 70 | pcl::PointCloud cloud; 71 | sensor_msgs::PointCloud2 output; 72 | // Fill in the cloud data 73 | 74 | int seed; 75 | 76 | int sizeX; 77 | int sizeY; 78 | int sizeZ; 79 | 80 | double scale; 81 | double update_freq; 82 | 83 | int type; 84 | 85 | nh_private.param("seed", seed, 4546); 86 | nh_private.param("update_freq", update_freq, 1.0); 87 | nh_private.param("resolution", scale, 0.38); 88 | nh_private.param("x_length", sizeX, 100); 89 | nh_private.param("y_length", sizeY, 100); 90 | nh_private.param("z_length", sizeZ, 10); 91 | 92 | nh_private.param("type", type, 1); 93 | 94 | scale = 1 / scale; 95 | sizeX = sizeX * scale; 96 | sizeY = sizeY * scale; 97 | sizeZ = sizeZ * scale; 98 | 99 | mocka::Maps::BasicInfo info; 100 | info.nh_private = &nh_private; 101 | info.sizeX = sizeX; 102 | info.sizeY = sizeY; 103 | info.sizeZ = sizeZ; 104 | info.seed = seed; 105 | info.scale = scale; 106 | info.output = &output; 107 | info.cloud = &cloud; 108 | 109 | mocka::Maps map; 110 | map.setInfo(info); 111 | map.generate(type); 112 | 113 | // optimizeMap(info); 114 | 115 | //! @note publish loop 116 | ros::Rate loop_rate(update_freq); 117 | while (ros::ok()) 118 | { 119 | pcl_pub.publish(output); 120 | ros::spinOnce(); 121 | loop_rate.sleep(); 122 | } 123 | return 0; 124 | } 125 | -------------------------------------------------------------------------------- /Course_2/Example_1/src/gcopter/src/curve_gen.cpp: -------------------------------------------------------------------------------- 1 | #include "misc/visualizer.hpp" 2 | #include "gcopter/cubic_curve.hpp" 3 | #include "gcopter/cubic_spline.hpp" 4 | #include "gcopter/path_smoother.hpp" 5 | 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | 19 | struct Config 20 | { 21 | std::string targetTopic; 22 | double penaltyWeight; 23 | Eigen::Matrix3Xd circleObs; 24 | double pieceLength; 25 | double relCostTol; 26 | 27 | Config(const ros::NodeHandle &nh_priv) 28 | { 29 | std::vector circleObsVec; 30 | 31 | nh_priv.getParam("TargetTopic", targetTopic); 32 | nh_priv.getParam("PenaltyWeight", penaltyWeight); 33 | nh_priv.getParam("CircleObs", circleObsVec); 34 | nh_priv.getParam("PieceLength", pieceLength); 35 | nh_priv.getParam("RelCostTol", relCostTol); 36 | 37 | circleObs = Eigen::Map>( 38 | circleObsVec.data(), 3, circleObsVec.size() / 3); 39 | } 40 | }; 41 | 42 | class CurveGen 43 | { 44 | private: 45 | Config config; 46 | ros::NodeHandle nh; 47 | ros::Subscriber targetSub; 48 | 49 | Visualizer visualizer; 50 | std::vector startGoal; 51 | 52 | CubicCurve curve; 53 | 54 | public: 55 | CurveGen(ros::NodeHandle &nh_) 56 | : config(ros::NodeHandle("~")), 57 | nh(nh_), 58 | visualizer(nh) 59 | { 60 | targetSub = nh.subscribe(config.targetTopic, 1, &CurveGen::targetCallBack, this, 61 | ros::TransportHints().tcpNoDelay()); 62 | } 63 | 64 | inline void vizObs() 65 | { 66 | visualizer.visualizeDisks(config.circleObs); 67 | } 68 | 69 | inline void plan() 70 | { 71 | if (startGoal.size() == 2) 72 | { 73 | const int N = (startGoal.back() - startGoal.front()).norm() / config.pieceLength; 74 | Eigen::Matrix2Xd innerPoints(2, N - 1); 75 | for (int i = 0; i < N - 1; ++i) 76 | { 77 | innerPoints.col(i) = (startGoal.back() - startGoal.front()) * (i + 1.0) / N + startGoal.front(); 78 | } 79 | 80 | path_smoother::PathSmoother pathSmoother; 81 | pathSmoother.setup(startGoal.front(), startGoal.back(), N, config.circleObs, config.penaltyWeight); 82 | CubicCurve curve; 83 | 84 | if (std::isinf(pathSmoother.optimize(curve, innerPoints, config.relCostTol))) 85 | { 86 | return; 87 | } 88 | 89 | if (curve.getPieceNum() > 0) 90 | { 91 | visualizer.visualize(curve); 92 | } 93 | } 94 | } 95 | 96 | inline void targetCallBack(const geometry_msgs::PoseStamped::ConstPtr &msg) 97 | { 98 | 99 | if (startGoal.size() >= 2) 100 | { 101 | startGoal.clear(); 102 | } 103 | 104 | startGoal.emplace_back(msg->pose.position.x, msg->pose.position.y); 105 | 106 | plan(); 107 | 108 | return; 109 | } 110 | }; 111 | 112 | int main(int argc, char **argv) 113 | { 114 | ros::init(argc, argv, "curve_gen_node"); 115 | ros::NodeHandle nh_; 116 | 117 | CurveGen curveGen(nh_); 118 | 119 | ros::Duration(2.0).sleep(); 120 | 121 | curveGen.vizObs(); 122 | 123 | ros::spin(); 124 | 125 | return 0; 126 | } 127 | -------------------------------------------------------------------------------- /Course_4/Example_1/L4_HW_Solution/L4_HW_Solution/SDQP/CMakeFiles/Makefile2: -------------------------------------------------------------------------------- 1 | # CMAKE generated file: DO NOT EDIT! 2 | # Generated by "Unix Makefiles" Generator, CMake Version 3.10 3 | 4 | # Default target executed when no arguments are given to make. 5 | default_target: all 6 | 7 | .PHONY : default_target 8 | 9 | # The main recursive all target 10 | all: 11 | 12 | .PHONY : all 13 | 14 | # The main recursive preinstall target 15 | preinstall: 16 | 17 | .PHONY : preinstall 18 | 19 | #============================================================================= 20 | # Special targets provided by cmake. 21 | 22 | # Disable implicit rules so canonical targets will work. 23 | .SUFFIXES: 24 | 25 | 26 | # Remove some rules from gmake that .SUFFIXES does not remove. 27 | SUFFIXES = 28 | 29 | .SUFFIXES: .hpux_make_needs_suffix_list 30 | 31 | 32 | # Suppress display of executed commands. 33 | $(VERBOSE).SILENT: 34 | 35 | 36 | # A target that is always out of date. 37 | cmake_force: 38 | 39 | .PHONY : cmake_force 40 | 41 | #============================================================================= 42 | # Set environment variables for the build. 43 | 44 | # The shell in which to execute make rules. 45 | SHELL = /bin/sh 46 | 47 | # The CMake executable. 48 | CMAKE_COMMAND = /usr/bin/cmake 49 | 50 | # The command to remove a file. 51 | RM = /usr/bin/cmake -E remove -f 52 | 53 | # Escaping for special characters. 54 | EQUALS = = 55 | 56 | # The top-level source directory on which CMake was run. 57 | CMAKE_SOURCE_DIR = /home/pizhou/chapter_4/SDQP 58 | 59 | # The top-level build directory on which CMake was run. 60 | CMAKE_BINARY_DIR = /home/pizhou/chapter_4/SDQP 61 | 62 | #============================================================================= 63 | # Target rules for target CMakeFiles/sdqp_example.dir 64 | 65 | # All Build rule for target. 66 | CMakeFiles/sdqp_example.dir/all: 67 | $(MAKE) -f CMakeFiles/sdqp_example.dir/build.make CMakeFiles/sdqp_example.dir/depend 68 | $(MAKE) -f CMakeFiles/sdqp_example.dir/build.make CMakeFiles/sdqp_example.dir/build 69 | @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --progress-dir=/home/pizhou/chapter_4/SDQP/CMakeFiles --progress-num=1,2 "Built target sdqp_example" 70 | .PHONY : CMakeFiles/sdqp_example.dir/all 71 | 72 | # Include target in all. 73 | all: CMakeFiles/sdqp_example.dir/all 74 | 75 | .PHONY : all 76 | 77 | # Build rule for subdir invocation for target. 78 | CMakeFiles/sdqp_example.dir/rule: cmake_check_build_system 79 | $(CMAKE_COMMAND) -E cmake_progress_start /home/pizhou/chapter_4/SDQP/CMakeFiles 2 80 | $(MAKE) -f CMakeFiles/Makefile2 CMakeFiles/sdqp_example.dir/all 81 | $(CMAKE_COMMAND) -E cmake_progress_start /home/pizhou/chapter_4/SDQP/CMakeFiles 0 82 | .PHONY : CMakeFiles/sdqp_example.dir/rule 83 | 84 | # Convenience name for target. 85 | sdqp_example: CMakeFiles/sdqp_example.dir/rule 86 | 87 | .PHONY : sdqp_example 88 | 89 | # clean rule for target. 90 | CMakeFiles/sdqp_example.dir/clean: 91 | $(MAKE) -f CMakeFiles/sdqp_example.dir/build.make CMakeFiles/sdqp_example.dir/clean 92 | .PHONY : CMakeFiles/sdqp_example.dir/clean 93 | 94 | # clean rule for target. 95 | clean: CMakeFiles/sdqp_example.dir/clean 96 | 97 | .PHONY : clean 98 | 99 | #============================================================================= 100 | # Special targets to cleanup operation of make. 101 | 102 | # Special rule to run CMake to check the build system integrity. 103 | # No rule that depends on this can have commands that come from listfiles 104 | # because they might be regenerated. 105 | cmake_check_build_system: 106 | $(CMAKE_COMMAND) -H$(CMAKE_SOURCE_DIR) -B$(CMAKE_BINARY_DIR) --check-build-system CMakeFiles/Makefile.cmake 0 107 | .PHONY : cmake_check_build_system 108 | 109 | -------------------------------------------------------------------------------- /Course_3/Example_1/L3_HW_Solution/L3_HW_Solution/task_3/mpc_car/include/iosqp/iosqp.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | #include 6 | #include 7 | 8 | /** 9 | * osqp interface: 10 | * minimize 0.5 x' P x + q' x 11 | * subject to l <= A x <= u 12 | **/ 13 | 14 | namespace osqp { 15 | 16 | class IOSQP { 17 | public: 18 | IOSQP() : UNBOUNDED_VAL(OSQP_INFTY), 19 | pWork(nullptr), 20 | pSettings(nullptr), 21 | pData(nullptr) { 22 | pSettings = (OSQPSettings *)c_malloc(sizeof(OSQPSettings)); 23 | pData = (OSQPData *)c_malloc(sizeof(OSQPData)); 24 | if (pSettings) 25 | osqp_set_default_settings(pSettings); 26 | } 27 | 28 | ~IOSQP() { 29 | if (pWork) 30 | osqp_cleanup(pWork); 31 | if (pSettings) 32 | c_free(pSettings); 33 | if (pData) 34 | c_free(pData); 35 | } 36 | 37 | const double UNBOUNDED_VAL; 38 | 39 | inline c_int setMats(Eigen::SparseMatrix &P, 40 | Eigen::VectorXd &q, 41 | Eigen::SparseMatrix &A, 42 | Eigen::VectorXd &l, 43 | Eigen::VectorXd &u, 44 | const double &eps_abs = 1e-3, 45 | const double &eps_rel = 1e-3) { 46 | if (pWork) 47 | osqp_cleanup(pWork); 48 | 49 | P = P.triangularView(); 50 | P.makeCompressed(); 51 | A.makeCompressed(); 52 | 53 | pData->n = P.rows(); 54 | pData->m = A.rows(); 55 | 56 | Eigen::Map iIdxP(P.innerIndexPtr(), P.nonZeros()); 57 | Eigen::Map oIdxP(P.outerIndexPtr(), P.cols() + 1); 58 | Eigen::Matrix innerIdxP = iIdxP.cast(); 59 | Eigen::Matrix outerIdxP = oIdxP.cast(); 60 | pData->P = csc_matrix(pData->n, 61 | pData->n, 62 | P.nonZeros(), 63 | P.valuePtr(), 64 | innerIdxP.data(), 65 | outerIdxP.data()); 66 | 67 | Eigen::Map iIdxA(A.innerIndexPtr(), A.nonZeros()); 68 | Eigen::Map oIdxA(A.outerIndexPtr(), A.cols() + 1); 69 | Eigen::Matrix innerIdxA = iIdxA.cast(); 70 | Eigen::Matrix outerIdxA = oIdxA.cast(); 71 | pData->A = csc_matrix(pData->m, 72 | pData->n, 73 | A.nonZeros(), 74 | A.valuePtr(), 75 | innerIdxA.data(), 76 | outerIdxA.data()); 77 | 78 | Eigen::Matrix pDqVec = q.cast(); 79 | pData->q = pDqVec.data(); 80 | Eigen::Matrix pDlVec = l.cast(); 81 | pData->l = pDlVec.data(); 82 | Eigen::Matrix pDuVec = u.cast(); 83 | pData->u = pDuVec.data(); 84 | 85 | pSettings->eps_abs = eps_abs; 86 | pSettings->eps_rel = eps_rel; 87 | 88 | c_int exitflag = osqp_setup(&pWork, pData, pSettings); 89 | 90 | if (pData->A) 91 | c_free(pData->A); 92 | if (pData->P) 93 | c_free(pData->P); 94 | 95 | return exitflag; 96 | } 97 | 98 | inline c_int solve() const { 99 | return osqp_solve(pWork); 100 | } 101 | 102 | inline c_int getStatus() const { 103 | return pWork->info->status_val; 104 | } 105 | 106 | inline Eigen::VectorXd getPrimalSol() const { 107 | return Eigen::Map(pWork->solution->x, 108 | pWork->data->n); 109 | } 110 | 111 | private: 112 | OSQPWorkspace *pWork; 113 | OSQPSettings *pSettings; 114 | OSQPData *pData; 115 | }; 116 | 117 | } // namespace osqp -------------------------------------------------------------------------------- /Course_3/Example_1/L3_HW_Solution/L3_HW_Solution/task_3/mpc_car/src/mpc_car_nodelet.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | 6 | #include 7 | #include 8 | 9 | #include 10 | 11 | namespace mpc_car { 12 | class Nodelet : public nodelet::Nodelet { 13 | private: 14 | std::shared_ptr mpcPtr_; 15 | ros::Timer plan_timer_; 16 | ros::Subscriber odom_sub_; 17 | ros::Publisher cmd_pub_; 18 | VectorX state_; 19 | bool init = false; 20 | double delay_ = 0.0; 21 | bool nmpc_ = false; 22 | 23 | void plan_timer_callback(const ros::TimerEvent& event) { 24 | if (init) { 25 | int ret = 0; 26 | if (nmpc_) { 27 | ros::Time t1 = ros::Time::now(); 28 | // ret = mpcPtr_->solveNMPC(state_); 29 | ret = mpcPtr_->SolvePHR(state_); 30 | ros::Time t2 = ros::Time::now(); 31 | double solve_time = (t2 - t1).toSec(); 32 | std::cout << "solve nmpc costs: " << 1e3 * solve_time << "ms" << std::endl; 33 | 34 | std::ofstream foutC("/media/kris/Workspace/numerical_optimization/section_3/task_3/time.txt", std::ios::app); 35 | foutC.setf(std::ios::scientific, std::ios::floatfield); 36 | foutC.precision(15); 37 | foutC << 1e3 * solve_time << std::endl; 38 | foutC.close(); 39 | } else { 40 | ros::Time t1 = ros::Time::now(); 41 | ret = mpcPtr_->solveQP(state_); 42 | ros::Time t2 = ros::Time::now(); 43 | double solve_time = (t2 - t1).toSec(); 44 | std::cout << "solve qp costs: " << 1e3 * solve_time << "ms" << std::endl; 45 | 46 | std::ofstream foutC("/media/kris/Workspace/numerical_optimization/section_3/task_3/time.txt", std::ios::app); 47 | foutC.setf(std::ios::scientific, std::ios::floatfield); 48 | foutC.precision(15); 49 | foutC << 1e3 * solve_time << std::endl; 50 | foutC.close(); 51 | } 52 | // assert(ret == 1); 53 | // TODO 54 | car_msgs::CarCmd msg; 55 | msg.header.frame_id = "world"; 56 | msg.header.stamp = ros::Time::now(); 57 | 58 | VectorX x; 59 | VectorU u; 60 | mpcPtr_->getPredictXU(0, x, u); 61 | std::cout << "u: " << u.transpose() << std::endl; 62 | std::cout << "x: " << x.transpose() << std::endl; 63 | std::cout << std::endl; 64 | // 发送控制 65 | msg.a = u(0); 66 | msg.delta = u(1); 67 | cmd_pub_.publish(msg); 68 | 69 | mpcPtr_->visualization(); 70 | } 71 | return; 72 | } 73 | 74 | // 机器人状态估计的信息 x y yaw v 75 | void odom_call_back(const nav_msgs::Odometry::ConstPtr& msg) { 76 | double x = msg->pose.pose.position.x; 77 | double y = msg->pose.pose.position.y; 78 | Eigen::Quaterniond q(msg->pose.pose.orientation.w, 79 | msg->pose.pose.orientation.x, 80 | msg->pose.pose.orientation.y, 81 | msg->pose.pose.orientation.z); 82 | Eigen::Vector3d euler = q.toRotationMatrix().eulerAngles(0, 1, 2); 83 | Eigen::Vector2d v(msg->twist.twist.linear.x, msg->twist.twist.linear.y); 84 | state_ << x, y, euler.z(), v.norm(); 85 | init = true; 86 | } 87 | 88 | public: 89 | // 类似普通节点中的main() 90 | void onInit(void) { 91 | ros::NodeHandle nh(getMTPrivateNodeHandle()); 92 | mpcPtr_ = std::make_shared(nh); 93 | double dt = 0; 94 | nh.getParam("dt", dt); 95 | nh.getParam("delay", delay_); 96 | nh.getParam("nmpc", nmpc_); 97 | 98 | plan_timer_ = nh.createTimer(ros::Duration(dt), &Nodelet::plan_timer_callback, this); 99 | odom_sub_ = nh.subscribe("odom", 1, &Nodelet::odom_call_back, this); 100 | cmd_pub_ = nh.advertise("car_cmd", 1); 101 | } 102 | }; 103 | } // namespace mpc_car 104 | 105 | #include 106 | PLUGINLIB_EXPORT_CLASS(mpc_car::Nodelet, nodelet::Nodelet); -------------------------------------------------------------------------------- /Course_3/Example_1/L3_HW_Solution/L3_HW_Solution/task_3/car_simulator/src/car_simulator_nodelet.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | 6 | #include 7 | #include 8 | 9 | struct Car { 10 | double l; 11 | Eigen::Vector4d state; 12 | // state: x, y, phi, v 13 | // input: a, delta 14 | inline void setInitialState(const Eigen::Vector4d& s) { 15 | state = s; 16 | } 17 | inline Eigen::Vector4d diff(const Eigen::Vector4d& s, 18 | const Eigen::Vector2d& input) const { 19 | Eigen::Vector4d ds; 20 | double phi = s(2); 21 | double v = s(3); 22 | double a = input(0); 23 | double delta = input(1); 24 | ds(0) = v * cos(phi); 25 | ds(1) = v * sin(phi); 26 | ds(2) = v / l * tan(delta); 27 | ds(3) = a; 28 | return ds; 29 | } 30 | 31 | void step(const Eigen::Vector2d& input, const double dt) { 32 | // Runge–Kutta 33 | Eigen::Vector4d k1 = diff(state, input); 34 | Eigen::Vector4d k2 = diff(state + k1 * dt / 2, input); 35 | Eigen::Vector4d k3 = diff(state + k1 * dt / 2, input); 36 | Eigen::Vector4d k4 = diff(state + k3 * dt, input); 37 | state = state + (k1 + k2 * 2 + k3 * 2 + k4) * dt / 6; 38 | } 39 | }; 40 | 41 | namespace car_simulator { 42 | class Nodelet : public nodelet::Nodelet { 43 | private: 44 | Car car; 45 | double delay_ = 0.0; 46 | Eigen::Vector2d input_; 47 | ros::Publisher odom_pub_; 48 | ros::Subscriber cmd_sub_; 49 | ros::Timer sim_timer_; 50 | 51 | struct DelayedMsg { 52 | ros::Time t; 53 | double a, delta; 54 | DelayedMsg() {} 55 | DelayedMsg(const ros::Time& _t, double _a, double _delta) : t(_t), a(_a), delta(_delta) {} 56 | }; 57 | std::deque delayedMsgs_; 58 | 59 | void cmd_callback(const car_msgs::CarCmd::ConstPtr& msg) { 60 | delayedMsgs_.emplace_back(ros::Time::now(), msg->a, msg->delta); 61 | // input_(0) = msg->a; 62 | // input_(1) = msg->delta; 63 | } 64 | void timer_callback(const ros::TimerEvent& event) { 65 | if (!delayedMsgs_.empty()) { 66 | auto& msg = delayedMsgs_.front(); 67 | if ((ros::Time::now() - msg.t).toSec() > delay_) { 68 | input_(0) = msg.a; 69 | input_(1) = msg.delta; 70 | delayedMsgs_.pop_front(); 71 | } 72 | } 73 | 74 | car.step(input_, 1.0 / 400); 75 | nav_msgs::Odometry odom_msg; 76 | odom_msg.header.stamp = ros::Time::now(); 77 | odom_msg.header.frame_id = "world"; 78 | odom_msg.pose.pose.position.x = car.state(0); 79 | odom_msg.pose.pose.position.y = car.state(1); 80 | odom_msg.pose.pose.position.z = 0.0; 81 | double phi = car.state(2); 82 | double v = car.state(3); 83 | odom_msg.pose.pose.orientation.x = 0.0; 84 | odom_msg.pose.pose.orientation.y = 0.0; 85 | odom_msg.pose.pose.orientation.z = sin(phi / 2); 86 | odom_msg.pose.pose.orientation.w = cos(phi / 2); 87 | 88 | odom_msg.twist.twist.linear.x = v * cos(phi); 89 | odom_msg.twist.twist.linear.y = v * sin(phi); 90 | odom_msg.twist.twist.linear.z = 0.0; 91 | 92 | odom_pub_.publish(odom_msg); 93 | } 94 | 95 | public: 96 | void onInit(void) { 97 | ros::NodeHandle nh(getMTPrivateNodeHandle()); 98 | nh.getParam("l", car.l); 99 | Eigen::Vector4d initS; 100 | nh.getParam("x", initS(0)); 101 | nh.getParam("y", initS(1)); 102 | nh.getParam("phi", initS(2)); 103 | nh.getParam("v", initS(3)); 104 | nh.getParam("delay", delay_); 105 | input_.setZero(); 106 | car.setInitialState(initS); 107 | 108 | odom_pub_ = nh.advertise("odom", 1); 109 | cmd_sub_ = nh.subscribe("car_cmd", 1, &Nodelet::cmd_callback, this, ros::TransportHints().tcpNoDelay()); 110 | sim_timer_ = nh.createTimer(ros::Duration(1.0 / 400), &Nodelet::timer_callback, this); 111 | } 112 | }; 113 | } // namespace car_simulator 114 | 115 | #include 116 | PLUGINLIB_EXPORT_CLASS(car_simulator::Nodelet, nodelet::Nodelet); -------------------------------------------------------------------------------- /Course_2/Example_1/src/map_gen/mockamap/src/perlinnoise.cpp: -------------------------------------------------------------------------------- 1 | #include "perlinnoise.hpp" 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | // THIS IS A DIRECT TRANSLATION TO C++14 FROM THE REFERENCE 9 | // JAVA IMPLEMENTATION OF THE IMPROVED PERLIN FUNCTION (see 10 | // http://mrl.nyu.edu/~perlin/noise/) 11 | // THE ORIGINAL JAVA IMPLEMENTATION IS COPYRIGHT 2002 KEN PERLIN 12 | 13 | // I ADDED AN EXTRA METHOD THAT GENERATES A NEW PERMUTATION VECTOR (THIS IS NOT 14 | // PRESENT IN THE ORIGINAL IMPLEMENTATION) 15 | 16 | // Initialize with the reference values for the permutation vector 17 | PerlinNoise::PerlinNoise() 18 | { 19 | 20 | // Initialize the permutation vector with the reference values 21 | p = { 151, 160, 137, 91, 90, 15, 131, 13, 201, 95, 96, 53, 194, 233, 22 | 7, 225, 140, 36, 103, 30, 69, 142, 8, 99, 37, 240, 21, 10, 23 | 23, 190, 6, 148, 247, 120, 234, 75, 0, 26, 197, 62, 94, 252, 24 | 219, 203, 117, 35, 11, 32, 57, 177, 33, 88, 237, 149, 56, 87, 25 | 174, 20, 125, 136, 171, 168, 68, 175, 74, 165, 71, 134, 139, 48, 26 | 27, 166, 77, 146, 158, 231, 83, 111, 229, 122, 60, 211, 133, 230, 27 | 220, 105, 92, 41, 55, 46, 245, 40, 244, 102, 143, 54, 65, 25, 28 | 63, 161, 1, 216, 80, 73, 209, 76, 132, 187, 208, 89, 18, 169, 29 | 200, 196, 135, 130, 116, 188, 159, 86, 164, 100, 109, 198, 173, 186, 30 | 3, 64, 52, 217, 226, 250, 124, 123, 5, 202, 38, 147, 118, 126, 31 | 255, 82, 85, 212, 207, 206, 59, 227, 47, 16, 58, 17, 182, 189, 32 | 28, 42, 223, 183, 170, 213, 119, 248, 152, 2, 44, 154, 163, 70, 33 | 221, 153, 101, 155, 167, 43, 172, 9, 129, 22, 39, 253, 19, 98, 34 | 108, 110, 79, 113, 224, 232, 178, 185, 112, 104, 218, 246, 97, 228, 35 | 251, 34, 242, 193, 238, 210, 144, 12, 191, 179, 162, 241, 81, 51, 36 | 145, 235, 249, 14, 239, 107, 49, 192, 214, 31, 181, 199, 106, 157, 37 | 184, 84, 204, 176, 115, 121, 50, 45, 127, 4, 150, 254, 138, 236, 38 | 205, 93, 222, 114, 67, 29, 24, 72, 243, 141, 128, 195, 78, 66, 39 | 215, 61, 156, 180 }; 40 | // Duplicate the permutation vector 41 | p.insert(p.end(), p.begin(), p.end()); 42 | } 43 | 44 | // Generate a new permutation vector based on the value of seed 45 | PerlinNoise::PerlinNoise(unsigned int seed) 46 | { 47 | p.resize(256); 48 | 49 | // Fill p with values from 0 to 255 50 | std::iota(p.begin(), p.end(), 0); 51 | 52 | // Initialize a random engine with seed 53 | std::default_random_engine engine(seed); 54 | 55 | // Suffle using the above random engine 56 | std::shuffle(p.begin(), p.end(), engine); 57 | 58 | // Duplicate the permutation vector 59 | p.insert(p.end(), p.begin(), p.end()); 60 | } 61 | 62 | double 63 | PerlinNoise::noise(double x, double y, double z) 64 | { 65 | // Find the unit cube that contains the point 66 | int X = (int)floor(x) & 255; 67 | int Y = (int)floor(y) & 255; 68 | int Z = (int)floor(z) & 255; 69 | 70 | // Find relative x, y,z of point in cube 71 | x -= floor(x); 72 | y -= floor(y); 73 | z -= floor(z); 74 | 75 | // Compute fade curves for each of x, y, z 76 | double u = fade(x); 77 | double v = fade(y); 78 | double w = fade(z); 79 | 80 | // Hash coordinates of the 8 cube corners 81 | int A = p[X] + Y; 82 | int AA = p[A] + Z; 83 | int AB = p[A + 1] + Z; 84 | int B = p[X + 1] + Y; 85 | int BA = p[B] + Z; 86 | int BB = p[B + 1] + Z; 87 | 88 | // Add blended results from 8 corners of cube 89 | double res = 90 | lerp(w, // 91 | lerp(v, // 92 | lerp(u, grad(p[AA], x, y, z), grad(p[BA], x - 1, y, z)), 93 | lerp(u, grad(p[AB], x, y - 1, z), grad(p[BB], x - 1, y - 1, z))), 94 | lerp(v, // 95 | lerp(u, // 96 | grad(p[AA + 1], x, y, z - 1), // 97 | grad(p[BA + 1], x - 1, y, z - 1)), 98 | lerp(u, // 99 | grad(p[AB + 1], x, y - 1, z - 1), 100 | grad(p[BB + 1], x - 1, y - 1, z - 1)))); 101 | return (res + 1.0) / 2.0; 102 | } 103 | 104 | double 105 | PerlinNoise::fade(double t) 106 | { 107 | return t * t * t * (t * (t * 6 - 15) + 10); 108 | } 109 | 110 | double 111 | PerlinNoise::lerp(double t, double a, double b) 112 | { 113 | return a + t * (b - a); 114 | } 115 | 116 | double 117 | PerlinNoise::grad(int hash, double x, double y, double z) 118 | { 119 | int h = hash & 15; 120 | // Convert lower 4 bits of hash into 12 gradient directions 121 | double u = h < 8 ? x : y, v = h < 4 ? y : h == 12 || h == 14 ? x : z; 122 | return ((h & 1) == 0 ? u : -u) + ((h & 2) == 0 ? v : -v); 123 | } 124 | -------------------------------------------------------------------------------- /Course_4/Example_1/L4_HW_Solution/L4_HW_Solution/SDQP/CMakeFiles/3.10.2/CMakeCXXCompiler.cmake: -------------------------------------------------------------------------------- 1 | set(CMAKE_CXX_COMPILER "/usr/bin/c++") 2 | set(CMAKE_CXX_COMPILER_ARG1 "") 3 | set(CMAKE_CXX_COMPILER_ID "GNU") 4 | set(CMAKE_CXX_COMPILER_VERSION "7.5.0") 5 | set(CMAKE_CXX_COMPILER_VERSION_INTERNAL "") 6 | set(CMAKE_CXX_COMPILER_WRAPPER "") 7 | set(CMAKE_CXX_STANDARD_COMPUTED_DEFAULT "14") 8 | set(CMAKE_CXX_COMPILE_FEATURES "cxx_std_98;cxx_template_template_parameters;cxx_std_11;cxx_alias_templates;cxx_alignas;cxx_alignof;cxx_attributes;cxx_auto_type;cxx_constexpr;cxx_decltype;cxx_decltype_incomplete_return_types;cxx_default_function_template_args;cxx_defaulted_functions;cxx_defaulted_move_initializers;cxx_delegating_constructors;cxx_deleted_functions;cxx_enum_forward_declarations;cxx_explicit_conversions;cxx_extended_friend_declarations;cxx_extern_templates;cxx_final;cxx_func_identifier;cxx_generalized_initializers;cxx_inheriting_constructors;cxx_inline_namespaces;cxx_lambdas;cxx_local_type_template_args;cxx_long_long_type;cxx_noexcept;cxx_nonstatic_member_init;cxx_nullptr;cxx_override;cxx_range_for;cxx_raw_string_literals;cxx_reference_qualified_functions;cxx_right_angle_brackets;cxx_rvalue_references;cxx_sizeof_member;cxx_static_assert;cxx_strong_enums;cxx_thread_local;cxx_trailing_return_types;cxx_unicode_literals;cxx_uniform_initialization;cxx_unrestricted_unions;cxx_user_literals;cxx_variadic_macros;cxx_variadic_templates;cxx_std_14;cxx_aggregate_default_initializers;cxx_attribute_deprecated;cxx_binary_literals;cxx_contextual_conversions;cxx_decltype_auto;cxx_digit_separators;cxx_generic_lambdas;cxx_lambda_init_captures;cxx_relaxed_constexpr;cxx_return_type_deduction;cxx_variable_templates;cxx_std_17") 9 | set(CMAKE_CXX98_COMPILE_FEATURES "cxx_std_98;cxx_template_template_parameters") 10 | set(CMAKE_CXX11_COMPILE_FEATURES "cxx_std_11;cxx_alias_templates;cxx_alignas;cxx_alignof;cxx_attributes;cxx_auto_type;cxx_constexpr;cxx_decltype;cxx_decltype_incomplete_return_types;cxx_default_function_template_args;cxx_defaulted_functions;cxx_defaulted_move_initializers;cxx_delegating_constructors;cxx_deleted_functions;cxx_enum_forward_declarations;cxx_explicit_conversions;cxx_extended_friend_declarations;cxx_extern_templates;cxx_final;cxx_func_identifier;cxx_generalized_initializers;cxx_inheriting_constructors;cxx_inline_namespaces;cxx_lambdas;cxx_local_type_template_args;cxx_long_long_type;cxx_noexcept;cxx_nonstatic_member_init;cxx_nullptr;cxx_override;cxx_range_for;cxx_raw_string_literals;cxx_reference_qualified_functions;cxx_right_angle_brackets;cxx_rvalue_references;cxx_sizeof_member;cxx_static_assert;cxx_strong_enums;cxx_thread_local;cxx_trailing_return_types;cxx_unicode_literals;cxx_uniform_initialization;cxx_unrestricted_unions;cxx_user_literals;cxx_variadic_macros;cxx_variadic_templates") 11 | set(CMAKE_CXX14_COMPILE_FEATURES "cxx_std_14;cxx_aggregate_default_initializers;cxx_attribute_deprecated;cxx_binary_literals;cxx_contextual_conversions;cxx_decltype_auto;cxx_digit_separators;cxx_generic_lambdas;cxx_lambda_init_captures;cxx_relaxed_constexpr;cxx_return_type_deduction;cxx_variable_templates") 12 | set(CMAKE_CXX17_COMPILE_FEATURES "cxx_std_17") 13 | 14 | set(CMAKE_CXX_PLATFORM_ID "Linux") 15 | set(CMAKE_CXX_SIMULATE_ID "") 16 | set(CMAKE_CXX_SIMULATE_VERSION "") 17 | 18 | 19 | 20 | set(CMAKE_AR "/usr/bin/ar") 21 | set(CMAKE_CXX_COMPILER_AR "/usr/bin/gcc-ar-7") 22 | set(CMAKE_RANLIB "/usr/bin/ranlib") 23 | set(CMAKE_CXX_COMPILER_RANLIB "/usr/bin/gcc-ranlib-7") 24 | set(CMAKE_LINKER "/usr/bin/ld") 25 | set(CMAKE_COMPILER_IS_GNUCXX 1) 26 | set(CMAKE_CXX_COMPILER_LOADED 1) 27 | set(CMAKE_CXX_COMPILER_WORKS TRUE) 28 | set(CMAKE_CXX_ABI_COMPILED TRUE) 29 | set(CMAKE_COMPILER_IS_MINGW ) 30 | set(CMAKE_COMPILER_IS_CYGWIN ) 31 | if(CMAKE_COMPILER_IS_CYGWIN) 32 | set(CYGWIN 1) 33 | set(UNIX 1) 34 | endif() 35 | 36 | set(CMAKE_CXX_COMPILER_ENV_VAR "CXX") 37 | 38 | if(CMAKE_COMPILER_IS_MINGW) 39 | set(MINGW 1) 40 | endif() 41 | set(CMAKE_CXX_COMPILER_ID_RUN 1) 42 | set(CMAKE_CXX_IGNORE_EXTENSIONS inl;h;hpp;HPP;H;o;O;obj;OBJ;def;DEF;rc;RC) 43 | set(CMAKE_CXX_SOURCE_FILE_EXTENSIONS C;M;c++;cc;cpp;cxx;mm;CPP) 44 | set(CMAKE_CXX_LINKER_PREFERENCE 30) 45 | set(CMAKE_CXX_LINKER_PREFERENCE_PROPAGATES 1) 46 | 47 | # Save compiler ABI information. 48 | set(CMAKE_CXX_SIZEOF_DATA_PTR "8") 49 | set(CMAKE_CXX_COMPILER_ABI "ELF") 50 | set(CMAKE_CXX_LIBRARY_ARCHITECTURE "x86_64-linux-gnu") 51 | 52 | if(CMAKE_CXX_SIZEOF_DATA_PTR) 53 | set(CMAKE_SIZEOF_VOID_P "${CMAKE_CXX_SIZEOF_DATA_PTR}") 54 | endif() 55 | 56 | if(CMAKE_CXX_COMPILER_ABI) 57 | set(CMAKE_INTERNAL_PLATFORM_ABI "${CMAKE_CXX_COMPILER_ABI}") 58 | endif() 59 | 60 | if(CMAKE_CXX_LIBRARY_ARCHITECTURE) 61 | set(CMAKE_LIBRARY_ARCHITECTURE "x86_64-linux-gnu") 62 | endif() 63 | 64 | set(CMAKE_CXX_CL_SHOWINCLUDES_PREFIX "") 65 | if(CMAKE_CXX_CL_SHOWINCLUDES_PREFIX) 66 | set(CMAKE_CL_SHOWINCLUDES_PREFIX "${CMAKE_CXX_CL_SHOWINCLUDES_PREFIX}") 67 | endif() 68 | 69 | 70 | 71 | 72 | 73 | set(CMAKE_CXX_IMPLICIT_LINK_LIBRARIES "stdc++;m;gcc_s;gcc;c;gcc_s;gcc") 74 | set(CMAKE_CXX_IMPLICIT_LINK_DIRECTORIES "/usr/lib/gcc/x86_64-linux-gnu/7;/usr/lib/x86_64-linux-gnu;/usr/lib;/lib/x86_64-linux-gnu;/lib") 75 | set(CMAKE_CXX_IMPLICIT_LINK_FRAMEWORK_DIRECTORIES "") 76 | -------------------------------------------------------------------------------- /Course_2/Example_1/src/gcopter/include/gcopter/cubic_curve.hpp: -------------------------------------------------------------------------------- 1 | #ifndef CUBIC_CURVE_HPP 2 | #define CUBIC_CURVE_HPP 3 | 4 | #include 5 | 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | class CubicPolynomial 12 | { 13 | private: 14 | double duration; 15 | Eigen::Matrix coeffMat; 16 | 17 | public: 18 | CubicPolynomial() = default; 19 | 20 | CubicPolynomial(double dur, const Eigen::Matrix &cMat) 21 | : duration(dur), coeffMat(cMat) {} 22 | 23 | inline int getDim() const 24 | { 25 | return 2; 26 | } 27 | 28 | inline int getDegree() const 29 | { 30 | return 3; 31 | } 32 | 33 | inline double getDuration() const 34 | { 35 | return duration; 36 | } 37 | 38 | inline const Eigen::Matrix &getCoeffMat() const 39 | { 40 | return coeffMat; 41 | } 42 | 43 | inline Eigen::Vector2d getPos(const double &t) const 44 | { 45 | return coeffMat.col(3) + t * (coeffMat.col(2) + t * (coeffMat.col(1) + t * coeffMat.col(0))); 46 | } 47 | }; 48 | 49 | class CubicCurve 50 | { 51 | private: 52 | typedef std::vector Pieces; 53 | Pieces pieces; 54 | 55 | public: 56 | CubicCurve() = default; 57 | 58 | CubicCurve(const std::vector &durs, 59 | const std::vector> &cMats) 60 | { 61 | const int N = std::min(durs.size(), cMats.size()); 62 | pieces.reserve(N); 63 | for (int i = 0; i < N; ++i) 64 | { 65 | pieces.emplace_back(durs[i], cMats[i]); 66 | } 67 | } 68 | 69 | inline int getPieceNum() const 70 | { 71 | return pieces.size(); 72 | } 73 | 74 | inline Eigen::VectorXd getDurations() const 75 | { 76 | const int N = getPieceNum(); 77 | Eigen::VectorXd durations(N); 78 | for (int i = 0; i < N; ++i) 79 | { 80 | durations(i) = pieces[i].getDuration(); 81 | } 82 | return durations; 83 | } 84 | 85 | inline double getTotalDuration() const 86 | { 87 | const int N = getPieceNum(); 88 | double totalDuration = 0.0; 89 | for (int i = 0; i < N; ++i) 90 | { 91 | totalDuration += pieces[i].getDuration(); 92 | } 93 | return totalDuration; 94 | } 95 | 96 | inline Eigen::Matrix2Xd getPositions() const 97 | { 98 | const int N = getPieceNum(); 99 | Eigen::Matrix2Xd positions(2, N + 1); 100 | for (int i = 0; i < N; ++i) 101 | { 102 | positions.col(i) = pieces[i].getCoeffMat().col(3); 103 | } 104 | positions.col(N) = pieces[N - 1].getPos(pieces[N - 1].getDuration()); 105 | return positions; 106 | } 107 | 108 | inline const CubicPolynomial &operator[](int i) const 109 | { 110 | return pieces[i]; 111 | } 112 | 113 | inline CubicPolynomial &operator[](int i) 114 | { 115 | return pieces[i]; 116 | } 117 | 118 | inline void clear(void) 119 | { 120 | pieces.clear(); 121 | return; 122 | } 123 | 124 | inline Pieces::const_iterator begin() const 125 | { 126 | return pieces.begin(); 127 | } 128 | 129 | inline Pieces::const_iterator end() const 130 | { 131 | return pieces.end(); 132 | } 133 | 134 | inline Pieces::iterator begin() 135 | { 136 | return pieces.begin(); 137 | } 138 | 139 | inline Pieces::iterator end() 140 | { 141 | return pieces.end(); 142 | } 143 | 144 | inline void reserve(const int &n) 145 | { 146 | pieces.reserve(n); 147 | return; 148 | } 149 | 150 | inline void emplace_back(const CubicPolynomial &piece) 151 | { 152 | pieces.emplace_back(piece); 153 | return; 154 | } 155 | 156 | inline void emplace_back(const double &dur, 157 | const Eigen::Matrix &cMat) 158 | { 159 | pieces.emplace_back(dur, cMat); 160 | return; 161 | } 162 | 163 | inline void append(const CubicCurve &traj) 164 | { 165 | pieces.insert(pieces.end(), traj.begin(), traj.end()); 166 | return; 167 | } 168 | 169 | inline int locatePieceIdx(double &t) const 170 | { 171 | const int N = getPieceNum(); 172 | int idx; 173 | double dur; 174 | for (idx = 0; 175 | idx < N && 176 | t > (dur = pieces[idx].getDuration()); 177 | idx++) 178 | { 179 | t -= dur; 180 | } 181 | if (idx == N) 182 | { 183 | idx--; 184 | t += pieces[idx].getDuration(); 185 | } 186 | return idx; 187 | } 188 | 189 | inline Eigen::Vector2d getPos(double t) const 190 | { 191 | const int pieceIdx = locatePieceIdx(t); 192 | return pieces[pieceIdx].getPos(t); 193 | } 194 | 195 | inline Eigen::Vector2d getJuncPos(const int juncIdx) const 196 | { 197 | if (juncIdx != getPieceNum()) 198 | { 199 | return pieces[juncIdx].getCoeffMat().col(3); 200 | } 201 | else 202 | { 203 | return pieces[juncIdx - 1].getPos(pieces[juncIdx - 1].getDuration()); 204 | } 205 | } 206 | }; 207 | 208 | #endif 209 | -------------------------------------------------------------------------------- /Course_4/Example_1/L4_HW_Solution/L4_HW_Solution/SDQP/CMakeFiles/sdqp_example.dir/build.make: -------------------------------------------------------------------------------- 1 | # CMAKE generated file: DO NOT EDIT! 2 | # Generated by "Unix Makefiles" Generator, CMake Version 3.10 3 | 4 | # Delete rule output on recipe failure. 5 | .DELETE_ON_ERROR: 6 | 7 | 8 | #============================================================================= 9 | # Special targets provided by cmake. 10 | 11 | # Disable implicit rules so canonical targets will work. 12 | .SUFFIXES: 13 | 14 | 15 | # Remove some rules from gmake that .SUFFIXES does not remove. 16 | SUFFIXES = 17 | 18 | .SUFFIXES: .hpux_make_needs_suffix_list 19 | 20 | 21 | # Suppress display of executed commands. 22 | $(VERBOSE).SILENT: 23 | 24 | 25 | # A target that is always out of date. 26 | cmake_force: 27 | 28 | .PHONY : cmake_force 29 | 30 | #============================================================================= 31 | # Set environment variables for the build. 32 | 33 | # The shell in which to execute make rules. 34 | SHELL = /bin/sh 35 | 36 | # The CMake executable. 37 | CMAKE_COMMAND = /usr/bin/cmake 38 | 39 | # The command to remove a file. 40 | RM = /usr/bin/cmake -E remove -f 41 | 42 | # Escaping for special characters. 43 | EQUALS = = 44 | 45 | # The top-level source directory on which CMake was run. 46 | CMAKE_SOURCE_DIR = /home/pizhou/chapter_4/SDQP 47 | 48 | # The top-level build directory on which CMake was run. 49 | CMAKE_BINARY_DIR = /home/pizhou/chapter_4/SDQP 50 | 51 | # Include any dependencies generated for this target. 52 | include CMakeFiles/sdqp_example.dir/depend.make 53 | 54 | # Include the progress variables for this target. 55 | include CMakeFiles/sdqp_example.dir/progress.make 56 | 57 | # Include the compile flags for this target's objects. 58 | include CMakeFiles/sdqp_example.dir/flags.make 59 | 60 | CMakeFiles/sdqp_example.dir/example/sdqp_example.cpp.o: CMakeFiles/sdqp_example.dir/flags.make 61 | CMakeFiles/sdqp_example.dir/example/sdqp_example.cpp.o: example/sdqp_example.cpp 62 | @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --green --progress-dir=/home/pizhou/chapter_4/SDQP/CMakeFiles --progress-num=$(CMAKE_PROGRESS_1) "Building CXX object CMakeFiles/sdqp_example.dir/example/sdqp_example.cpp.o" 63 | /usr/bin/c++ $(CXX_DEFINES) $(CXX_INCLUDES) $(CXX_FLAGS) -o CMakeFiles/sdqp_example.dir/example/sdqp_example.cpp.o -c /home/pizhou/chapter_4/SDQP/example/sdqp_example.cpp 64 | 65 | CMakeFiles/sdqp_example.dir/example/sdqp_example.cpp.i: cmake_force 66 | @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --green "Preprocessing CXX source to CMakeFiles/sdqp_example.dir/example/sdqp_example.cpp.i" 67 | /usr/bin/c++ $(CXX_DEFINES) $(CXX_INCLUDES) $(CXX_FLAGS) -E /home/pizhou/chapter_4/SDQP/example/sdqp_example.cpp > CMakeFiles/sdqp_example.dir/example/sdqp_example.cpp.i 68 | 69 | CMakeFiles/sdqp_example.dir/example/sdqp_example.cpp.s: cmake_force 70 | @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --green "Compiling CXX source to assembly CMakeFiles/sdqp_example.dir/example/sdqp_example.cpp.s" 71 | /usr/bin/c++ $(CXX_DEFINES) $(CXX_INCLUDES) $(CXX_FLAGS) -S /home/pizhou/chapter_4/SDQP/example/sdqp_example.cpp -o CMakeFiles/sdqp_example.dir/example/sdqp_example.cpp.s 72 | 73 | CMakeFiles/sdqp_example.dir/example/sdqp_example.cpp.o.requires: 74 | 75 | .PHONY : CMakeFiles/sdqp_example.dir/example/sdqp_example.cpp.o.requires 76 | 77 | CMakeFiles/sdqp_example.dir/example/sdqp_example.cpp.o.provides: CMakeFiles/sdqp_example.dir/example/sdqp_example.cpp.o.requires 78 | $(MAKE) -f CMakeFiles/sdqp_example.dir/build.make CMakeFiles/sdqp_example.dir/example/sdqp_example.cpp.o.provides.build 79 | .PHONY : CMakeFiles/sdqp_example.dir/example/sdqp_example.cpp.o.provides 80 | 81 | CMakeFiles/sdqp_example.dir/example/sdqp_example.cpp.o.provides.build: CMakeFiles/sdqp_example.dir/example/sdqp_example.cpp.o 82 | 83 | 84 | # Object files for target sdqp_example 85 | sdqp_example_OBJECTS = \ 86 | "CMakeFiles/sdqp_example.dir/example/sdqp_example.cpp.o" 87 | 88 | # External object files for target sdqp_example 89 | sdqp_example_EXTERNAL_OBJECTS = 90 | 91 | sdqp_example: CMakeFiles/sdqp_example.dir/example/sdqp_example.cpp.o 92 | sdqp_example: CMakeFiles/sdqp_example.dir/build.make 93 | sdqp_example: CMakeFiles/sdqp_example.dir/link.txt 94 | @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --green --bold --progress-dir=/home/pizhou/chapter_4/SDQP/CMakeFiles --progress-num=$(CMAKE_PROGRESS_2) "Linking CXX executable sdqp_example" 95 | $(CMAKE_COMMAND) -E cmake_link_script CMakeFiles/sdqp_example.dir/link.txt --verbose=$(VERBOSE) 96 | 97 | # Rule to build all files generated by this target. 98 | CMakeFiles/sdqp_example.dir/build: sdqp_example 99 | 100 | .PHONY : CMakeFiles/sdqp_example.dir/build 101 | 102 | CMakeFiles/sdqp_example.dir/requires: CMakeFiles/sdqp_example.dir/example/sdqp_example.cpp.o.requires 103 | 104 | .PHONY : CMakeFiles/sdqp_example.dir/requires 105 | 106 | CMakeFiles/sdqp_example.dir/clean: 107 | $(CMAKE_COMMAND) -P CMakeFiles/sdqp_example.dir/cmake_clean.cmake 108 | .PHONY : CMakeFiles/sdqp_example.dir/clean 109 | 110 | CMakeFiles/sdqp_example.dir/depend: 111 | cd /home/pizhou/chapter_4/SDQP && $(CMAKE_COMMAND) -E cmake_depends "Unix Makefiles" /home/pizhou/chapter_4/SDQP /home/pizhou/chapter_4/SDQP /home/pizhou/chapter_4/SDQP /home/pizhou/chapter_4/SDQP /home/pizhou/chapter_4/SDQP/CMakeFiles/sdqp_example.dir/DependInfo.cmake --color=$(COLOR) 112 | .PHONY : CMakeFiles/sdqp_example.dir/depend 113 | 114 | -------------------------------------------------------------------------------- /Course_4/Example_1/L4_HW_Solution/L4_HW_Solution/SDQP/Makefile: -------------------------------------------------------------------------------- 1 | # CMAKE generated file: DO NOT EDIT! 2 | # Generated by "Unix Makefiles" Generator, CMake Version 3.10 3 | 4 | # Default target executed when no arguments are given to make. 5 | default_target: all 6 | 7 | .PHONY : default_target 8 | 9 | # Allow only one "make -f Makefile2" at a time, but pass parallelism. 10 | .NOTPARALLEL: 11 | 12 | 13 | #============================================================================= 14 | # Special targets provided by cmake. 15 | 16 | # Disable implicit rules so canonical targets will work. 17 | .SUFFIXES: 18 | 19 | 20 | # Remove some rules from gmake that .SUFFIXES does not remove. 21 | SUFFIXES = 22 | 23 | .SUFFIXES: .hpux_make_needs_suffix_list 24 | 25 | 26 | # Suppress display of executed commands. 27 | $(VERBOSE).SILENT: 28 | 29 | 30 | # A target that is always out of date. 31 | cmake_force: 32 | 33 | .PHONY : cmake_force 34 | 35 | #============================================================================= 36 | # Set environment variables for the build. 37 | 38 | # The shell in which to execute make rules. 39 | SHELL = /bin/sh 40 | 41 | # The CMake executable. 42 | CMAKE_COMMAND = /usr/bin/cmake 43 | 44 | # The command to remove a file. 45 | RM = /usr/bin/cmake -E remove -f 46 | 47 | # Escaping for special characters. 48 | EQUALS = = 49 | 50 | # The top-level source directory on which CMake was run. 51 | CMAKE_SOURCE_DIR = /home/pizhou/chapter_4/SDQP 52 | 53 | # The top-level build directory on which CMake was run. 54 | CMAKE_BINARY_DIR = /home/pizhou/chapter_4/SDQP 55 | 56 | #============================================================================= 57 | # Targets provided globally by CMake. 58 | 59 | # Special rule for the target rebuild_cache 60 | rebuild_cache: 61 | @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --cyan "Running CMake to regenerate build system..." 62 | /usr/bin/cmake -H$(CMAKE_SOURCE_DIR) -B$(CMAKE_BINARY_DIR) 63 | .PHONY : rebuild_cache 64 | 65 | # Special rule for the target rebuild_cache 66 | rebuild_cache/fast: rebuild_cache 67 | 68 | .PHONY : rebuild_cache/fast 69 | 70 | # Special rule for the target edit_cache 71 | edit_cache: 72 | @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --cyan "No interactive CMake dialog available..." 73 | /usr/bin/cmake -E echo No\ interactive\ CMake\ dialog\ available. 74 | .PHONY : edit_cache 75 | 76 | # Special rule for the target edit_cache 77 | edit_cache/fast: edit_cache 78 | 79 | .PHONY : edit_cache/fast 80 | 81 | # The main all target 82 | all: cmake_check_build_system 83 | $(CMAKE_COMMAND) -E cmake_progress_start /home/pizhou/chapter_4/SDQP/CMakeFiles /home/pizhou/chapter_4/SDQP/CMakeFiles/progress.marks 84 | $(MAKE) -f CMakeFiles/Makefile2 all 85 | $(CMAKE_COMMAND) -E cmake_progress_start /home/pizhou/chapter_4/SDQP/CMakeFiles 0 86 | .PHONY : all 87 | 88 | # The main clean target 89 | clean: 90 | $(MAKE) -f CMakeFiles/Makefile2 clean 91 | .PHONY : clean 92 | 93 | # The main clean target 94 | clean/fast: clean 95 | 96 | .PHONY : clean/fast 97 | 98 | # Prepare targets for installation. 99 | preinstall: all 100 | $(MAKE) -f CMakeFiles/Makefile2 preinstall 101 | .PHONY : preinstall 102 | 103 | # Prepare targets for installation. 104 | preinstall/fast: 105 | $(MAKE) -f CMakeFiles/Makefile2 preinstall 106 | .PHONY : preinstall/fast 107 | 108 | # clear depends 109 | depend: 110 | $(CMAKE_COMMAND) -H$(CMAKE_SOURCE_DIR) -B$(CMAKE_BINARY_DIR) --check-build-system CMakeFiles/Makefile.cmake 1 111 | .PHONY : depend 112 | 113 | #============================================================================= 114 | # Target rules for targets named sdqp_example 115 | 116 | # Build rule for target. 117 | sdqp_example: cmake_check_build_system 118 | $(MAKE) -f CMakeFiles/Makefile2 sdqp_example 119 | .PHONY : sdqp_example 120 | 121 | # fast build rule for target. 122 | sdqp_example/fast: 123 | $(MAKE) -f CMakeFiles/sdqp_example.dir/build.make CMakeFiles/sdqp_example.dir/build 124 | .PHONY : sdqp_example/fast 125 | 126 | example/sdqp_example.o: example/sdqp_example.cpp.o 127 | 128 | .PHONY : example/sdqp_example.o 129 | 130 | # target to build an object file 131 | example/sdqp_example.cpp.o: 132 | $(MAKE) -f CMakeFiles/sdqp_example.dir/build.make CMakeFiles/sdqp_example.dir/example/sdqp_example.cpp.o 133 | .PHONY : example/sdqp_example.cpp.o 134 | 135 | example/sdqp_example.i: example/sdqp_example.cpp.i 136 | 137 | .PHONY : example/sdqp_example.i 138 | 139 | # target to preprocess a source file 140 | example/sdqp_example.cpp.i: 141 | $(MAKE) -f CMakeFiles/sdqp_example.dir/build.make CMakeFiles/sdqp_example.dir/example/sdqp_example.cpp.i 142 | .PHONY : example/sdqp_example.cpp.i 143 | 144 | example/sdqp_example.s: example/sdqp_example.cpp.s 145 | 146 | .PHONY : example/sdqp_example.s 147 | 148 | # target to generate assembly for a file 149 | example/sdqp_example.cpp.s: 150 | $(MAKE) -f CMakeFiles/sdqp_example.dir/build.make CMakeFiles/sdqp_example.dir/example/sdqp_example.cpp.s 151 | .PHONY : example/sdqp_example.cpp.s 152 | 153 | # Help Target 154 | help: 155 | @echo "The following are some of the valid targets for this Makefile:" 156 | @echo "... all (the default if no target is provided)" 157 | @echo "... clean" 158 | @echo "... depend" 159 | @echo "... rebuild_cache" 160 | @echo "... sdqp_example" 161 | @echo "... edit_cache" 162 | @echo "... example/sdqp_example.o" 163 | @echo "... example/sdqp_example.i" 164 | @echo "... example/sdqp_example.s" 165 | .PHONY : help 166 | 167 | 168 | 169 | #============================================================================= 170 | # Special targets to cleanup operation of make. 171 | 172 | # Special rule to run CMake to check the build system integrity. 173 | # No rule that depends on this can have commands that come from listfiles 174 | # because they might be regenerated. 175 | cmake_check_build_system: 176 | $(CMAKE_COMMAND) -H$(CMAKE_SOURCE_DIR) -B$(CMAKE_BINARY_DIR) --check-build-system CMakeFiles/Makefile.cmake 0 177 | .PHONY : cmake_check_build_system 178 | 179 | -------------------------------------------------------------------------------- /Course_3/Example_1/L3_HW_Solution/L3_HW_Solution/task_2_2/collision_distance_computation/launch/show.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 0 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | Splitter Ratio: 0.642276406288147 9 | Tree Height: 554 10 | - Class: rviz/Selection 11 | Name: Selection 12 | - Class: rviz/Tool Properties 13 | Expanded: 14 | - /2D Pose Estimate1 15 | - /2D Nav Goal1 16 | - /Publish Point1 17 | Name: Tool Properties 18 | Splitter Ratio: 0.5886790156364441 19 | - Class: rviz/Views 20 | Expanded: 21 | - /Current View1 22 | Name: Views 23 | Splitter Ratio: 0.5 24 | - Class: rviz/Time 25 | Experimental: false 26 | Name: Time 27 | SyncMode: 0 28 | SyncSource: "" 29 | - Class: autoware_rviz_debug/DecisionMakerPanel 30 | Name: DecisionMakerPanel 31 | Preferences: 32 | PromptSaveOnExit: true 33 | Toolbars: 34 | toolButtonStyle: 2 35 | Visualization Manager: 36 | Class: "" 37 | Displays: 38 | - Alpha: 0.5 39 | Cell Size: 1 40 | Class: rviz/Grid 41 | Color: 160; 160; 164 42 | Enabled: true 43 | Line Style: 44 | Line Width: 0.029999999329447746 45 | Value: Lines 46 | Name: Grid 47 | Normal Cell Count: 0 48 | Offset: 49 | X: 0 50 | Y: 0 51 | Z: 0 52 | Plane: XY 53 | Plane Cell Count: 10 54 | Reference Frame: 55 | Value: true 56 | - Class: rviz/MarkerArray 57 | Enabled: true 58 | Marker Topic: /obstacle_array 59 | Name: obstacle 60 | Namespaces: 61 | obstacle: true 62 | Queue Size: 100 63 | Value: true 64 | - Class: rviz/Marker 65 | Enabled: true 66 | Marker Topic: /robot_center 67 | Name: robot_center 68 | Namespaces: 69 | robot_center: true 70 | Queue Size: 100 71 | Value: true 72 | - Class: rviz/Marker 73 | Enabled: true 74 | Marker Topic: /collision_arrow 75 | Name: collision_arrow 76 | Namespaces: 77 | collisin_arrow: true 78 | Queue Size: 100 79 | Value: true 80 | Enabled: true 81 | Global Options: 82 | Background Color: 238; 238; 236 83 | Default Light: true 84 | Fixed Frame: map 85 | Frame Rate: 30 86 | Name: root 87 | Tools: 88 | - Class: rviz/Interact 89 | Hide Inactive Objects: true 90 | - Class: rviz/MoveCamera 91 | - Class: rviz/Select 92 | - Class: rviz/FocusCamera 93 | - Class: rviz/Measure 94 | - Class: rviz/SetInitialPose 95 | Theta std deviation: 0.2617993950843811 96 | Topic: /initialpose 97 | X std deviation: 0.5 98 | Y std deviation: 0.5 99 | - Class: rviz/SetGoal 100 | Topic: /move_base_simple/goal 101 | - Class: rviz/PublishPoint 102 | Single click: true 103 | Topic: /clicked_point 104 | Value: true 105 | Views: 106 | Current: 107 | Angle: 0 108 | Class: rviz/TopDownOrtho 109 | Enable Stereo Rendering: 110 | Stereo Eye Separation: 0.05999999865889549 111 | Stereo Focal Distance: 1 112 | Swap Stereo Eyes: false 113 | Value: false 114 | Invert Z Axis: false 115 | Name: Current View 116 | Near Clip Distance: 0.009999999776482582 117 | Scale: 53.70756912231445 118 | Target Frame: base_link 119 | Value: TopDownOrtho (rviz) 120 | X: 0.3137969374656677 121 | Y: -0.056735169142484665 122 | Saved: ~ 123 | Window Geometry: 124 | DecisionMakerPanel: 125 | collapsed: false 126 | Displays: 127 | collapsed: true 128 | Height: 705 129 | Hide Left Dock: true 130 | Hide Right Dock: true 131 | QMainWindow State: 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 132 | Selection: 133 | collapsed: false 134 | Time: 135 | collapsed: false 136 | Tool Properties: 137 | collapsed: false 138 | Views: 139 | collapsed: true 140 | Width: 817 141 | X: 478 142 | Y: 120 143 | -------------------------------------------------------------------------------- /Course_2/Example_1/src/gcopter/include/gcopter/path_smoother.hpp: -------------------------------------------------------------------------------- 1 | #ifndef PATH_SMOOTHER_HPP 2 | #define PATH_SMOOTHER_HPP 3 | 4 | #include "cubic_spline.hpp" 5 | #include "lbfgs.hpp" 6 | 7 | #include 8 | 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | 15 | namespace path_smoother 16 | { 17 | 18 | class PathSmoother 19 | { 20 | private: 21 | cubic_spline::CubicSpline cubSpline; 22 | 23 | int pieceN; 24 | Eigen::Matrix3Xd diskObstacles; 25 | double penaltyWeight; 26 | Eigen::Vector2d headP; 27 | Eigen::Vector2d tailP; 28 | Eigen::Matrix2Xd points; 29 | Eigen::Matrix2Xd gradByPoints; 30 | 31 | lbfgs::lbfgs_parameter_t lbfgs_params; 32 | 33 | private: 34 | static inline double costFunction(void *ptr, 35 | const Eigen::VectorXd &x, 36 | Eigen::VectorXd &g) 37 | { 38 | // Hints:This is a function which define the costfunction, you can use 39 | // the content which *ptr point to initialize a quote of Class: 40 | // PathSmoother, the you can use the quote to visit the cubSpline 41 | // TODO 42 | auto instance = reinterpret_cast(ptr); 43 | const int points_nums = instance->pieceN - 1; 44 | Eigen::Matrix2Xd grad; 45 | 46 | grad.resize(2, points_nums); 47 | grad.setZero(); 48 | 49 | double cost = 0.0; 50 | 51 | // Step 1: 送入目标点，计算曲线系数 52 | Eigen::Matrix2Xd inPs; 53 | inPs.resize(2, points_nums); 54 | inPs.row(0) = x.head(points_nums); 55 | inPs.row(1) = x.tail(points_nums); 56 | instance->cubSpline.setInnerPoints(inPs); 57 | 58 | // Step 2: 计算能量函数值和梯度 59 | double energy = 0.0; 60 | Eigen::Matrix2Xd energy_grad; 61 | energy_grad.resize(2, points_nums); 62 | energy_grad.setZero(); 63 | instance->cubSpline.getStretchEnergy(energy); 64 | instance->cubSpline.getGrad(energy_grad); 65 | 66 | cost += energy; 67 | grad += energy_grad; 68 | 69 | // Step3: 计算碰撞函数和代价 70 | double obstacles = 0.0; 71 | Eigen::Matrix2Xd potential_grad; 72 | potential_grad.resize(2, points_nums); 73 | potential_grad.setZero(); 74 | 75 | for (int i = 0; i < points_nums; ++i) 76 | { 77 | for (int j = 0; j < instance->diskObstacles.cols(); ++j) 78 | { 79 | Eigen::Vector2d diff = inPs.col(i) - instance->diskObstacles.col(j).head(2); 80 | double distance = diff.norm(); 81 | double delta = instance->diskObstacles(2, j) - distance; 82 | 83 | if (delta > 0.0) 84 | { 85 | obstacles += instance->penaltyWeight * delta; 86 | potential_grad.col(i) += instance->penaltyWeight * ( -diff / distance); 87 | } 88 | } 89 | } 90 | cost += obstacles; 91 | grad += potential_grad; 92 | 93 | // Step4: 得到结果 94 | g.setZero(); 95 | g.head(points_nums) = grad.row(0).transpose(); 96 | g.tail(points_nums) = grad.row(1).transpose(); 97 | 98 | 99 | // std::cout << std::setprecision(10) 100 | std::cout << "------------------------" << "\n"; 101 | std::cout << "Function Value: " << cost << "\n"; 102 | std::cout << "Gradient Inf Norm: " << g.cwiseAbs().maxCoeff() << "\n"; 103 | std::cout << "------------------------" << "\n"; 104 | 105 | return cost; 106 | } 107 | 108 | public: 109 | inline bool setup(const Eigen::Vector2d &initialP, 110 | const Eigen::Vector2d &terminalP, 111 | const int &pieceNum, 112 | const Eigen::Matrix3Xd &diskObs, 113 | const double penaWeight) 114 | { 115 | pieceN = pieceNum; 116 | diskObstacles = diskObs; 117 | penaltyWeight = penaWeight; 118 | headP = initialP; 119 | tailP = terminalP; 120 | 121 | cubSpline.setConditions(headP, tailP, pieceN); 122 | 123 | points.resize(2, pieceN - 1); 124 | gradByPoints.resize(2, pieceN - 1); 125 | 126 | return true; 127 | } 128 | 129 | 130 | inline double optimize(CubicCurve &curve, 131 | const Eigen::Matrix2Xd &iniInPs, 132 | const double &relCostTol) 133 | { 134 | // NOT TODO 135 | Eigen::VectorXd x(pieceN * 2 - 2); 136 | Eigen::Map innerP(x.data(), 2, pieceN - 1); 137 | innerP = iniInPs; 138 | 139 | double minCost = 0.0; 140 | lbfgs_params.mem_size = 64; 141 | lbfgs_params.past = 3; 142 | lbfgs_params.min_step = 1.0e-32; 143 | lbfgs_params.g_epsilon = 1.0e-6; 144 | lbfgs_params.delta = relCostTol; 145 | 146 | int ret = lbfgs::lbfgs_optimize(x, 147 | minCost, 148 | &PathSmoother::costFunction, 149 | nullptr, 150 | this, 151 | lbfgs_params); 152 | 153 | if (ret >= 0) 154 | { 155 | // cubSpline.setInnerPoints(innerP); 156 | cubSpline.getCurve(curve); 157 | } 158 | else 159 | { 160 | curve.clear(); 161 | minCost = INFINITY; 162 | std::cout << "Optimization Failed: " 163 | << lbfgs::lbfgs_strerror(ret) 164 | << std::endl; 165 | } 166 | 167 | return minCost; 168 | } 169 | }; 170 | 171 | } 172 | 173 | #endif -------------------------------------------------------------------------------- /Course_3/Example_1/L3_HW_Solution/L3_HW_Solution/task_2_2/collision_distance_computation/src/main.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | 7 | #include 8 | 9 | #include 10 | #include 11 | #include 12 | #include 13 | 14 | #include "../include/sdqp.hpp" 15 | 16 | std::vector obs_lists; 17 | Eigen::Vector2d obs_center; 18 | Eigen::Vector2d robot_center; 19 | 20 | ros::Publisher obstacle_pub; 21 | ros::Publisher robot_pub; 22 | ros::Publisher arrow_pub; 23 | 24 | ros::Subscriber robot_sub; 25 | 26 | void ObstacleGeneration(){ 27 | std::random_device rd; 28 | std::mt19937 gen(rd()); 29 | obs_center = Eigen::Vector2d(1.0,1.0); 30 | std::normal_distribution random_x(obs_center.x(), 1.0); 31 | std::normal_distribution random_y(obs_center.y(), 1.0); 32 | 33 | obs_lists.push_back(obs_center); 34 | for(size_t i=0; i<10; i++){ 35 | double x = random_x(gen); 36 | double y = random_y(gen); 37 | obs_lists.push_back(Eigen::Vector2d(x,y)); 38 | } 39 | for(auto& i : obs_lists){ 40 | std::cout<pose.position.x, 77 | msg->pose.position.y); 78 | 79 | // 2-Dimensional QP Problem 80 | int m = 2; 81 | Eigen::Vector2d z = Eigen::Vector2d::Zero(); 82 | Eigen::Matrix2d Q = Eigen::Matrix2d::Identity(); 83 | Eigen::Vector2d c = Eigen::Vector2d::Zero(); 84 | Eigen::MatrixXd A; 85 | A.resize(obs_lists.size(), 2); 86 | for(size_t i=0; i(Q, c, A, b, z); 96 | 97 | Eigen::Vector2d x; 98 | std::cout<<"======= result ======="<header.stamp; 114 | robot.id = 0; 115 | robot.type = visualization_msgs::Marker::SPHERE; 116 | robot.action = visualization_msgs::Marker::ADD; 117 | robot.ns = "robot_center"; 118 | robot.pose.position.x = robot_center.x(); 119 | robot.pose.position.y = robot_center.y(); 120 | robot.pose.position.z = 0.0; 121 | robot.pose.orientation.x = 0.0; 122 | robot.pose.orientation.y = 0.0; 123 | robot.pose.orientation.z = 0.0; 124 | robot.pose.orientation.w = 1.0; 125 | robot.scale.x = 0.2; 126 | robot.scale.y = 0.2; 127 | robot.scale.z = 0.2; 128 | robot.color.a = 1.0; 129 | robot.color.b = 0.0; 130 | robot.color.g = 1.0; 131 | robot.color.r = 0.0; 132 | robot_pub.publish(robot); 133 | 134 | // collision arrow 135 | if(cost != INFINITY){ 136 | visualization_msgs::Marker arrow; 137 | arrow.header.frame_id = "map"; 138 | arrow.header.stamp = msg->header.stamp; 139 | arrow.id = 0; 140 | arrow.action = visualization_msgs::Marker::ADD; 141 | arrow.ns = "collisin_arrow"; 142 | arrow.scale.x = 0.1; 143 | arrow.scale.y = 0.1; 144 | arrow.scale.z = 0.5; 145 | arrow.color.a = 1.0; 146 | arrow.color.b = 1.0; 147 | arrow.color.g = 0.0; 148 | arrow.color.r = 0.0; 149 | geometry_msgs::Point p1, p2; 150 | p1.x = robot_center.x(); 151 | p1.y = robot_center.y(); 152 | p1.z = 0.0; 153 | p2.x = x.x(); 154 | p2.y = x.y(); 155 | p2.z = 0.0; 156 | arrow.points.push_back(p1); 157 | arrow.points.push_back(p2); 158 | arrow_pub.publish(arrow); 159 | } else { 160 | visualization_msgs::Marker arrow; 161 | arrow.header.frame_id = "map"; 162 | arrow.header.stamp = msg->header.stamp; 163 | arrow.id = 0; 164 | arrow.ns = "collisin_arrow"; 165 | arrow.action = visualization_msgs::Marker::DELETE; 166 | arrow.color.a = 0.0; 167 | arrow_pub.publish(arrow); 168 | } 169 | 170 | } 171 | 172 | int main(int argc, char** argv){ 173 | ros::init(argc, argv,"collision_distance_computation_node"); 174 | ros::NodeHandle nh; 175 | 176 | obstacle_pub = nh.advertise( 177 | "obstacle_array", 1, true); 178 | robot_pub = nh.advertise( 179 | "robot_center", 1, true); 180 | arrow_pub = nh.advertise( 181 | "collision_arrow", 1, true); 182 | 183 | robot_sub = nh.subscribe("/move_base_simple/goal", 1, CallBack); 184 | 185 | ObstacleGeneration(); 186 | 187 | ros::spin(); 188 | } -------------------------------------------------------------------------------- /Course_3/Example_1/L3_HW_Solution/L3_HW_Solution/task_3/mpc_car/config/mpc_car.yaml: -------------------------------------------------------------------------------- 1 | # reference path 2 | # track_points_x: [0,0.5, 1,1.7, 2, 1.7, 1,0, -1,-1.7,-2,-2.3,-3.0, -3.7, -4.0, -4.0, -4.0] 3 | # track_points_y: [1, 1, 1,0.7, 0,-0.7,-1,-1, -1,-0.7, 0, 0.7, 1.0, 0.7, 0.0, -0.5, -1.0] 4 | # track_points_x: [0.0 ,0.25, 0.5 , 0.85, 1, 0.85, 0.5, 0, -0.5, -0.85,-1,-1.15,-1.5, -1.85, -2.0, -2.0, -2.0] 5 | # track_points_y: [0.5 , 0.5, 0.5 , 0.35, 0,-0.35, -0.5,-0.5, -0.5, -0.35, 0, 0.35, 0.5, 0.35, 0.0, -0.25, -0.5] 6 | 7 | track_points_x: [0,0.0174524,0.0348995,0.052336,0.0697565,0.0871557,0.104528,0.121869,0.139173,0.156434,0.173648,0.190809,0.207912,0.224951,0.241922,0.258819,0.275637,0.292372,0.309017,0.325568,0.34202,0.358368,0.374607,0.390731,0.406737,0.422618,0.438371,0.45399,0.469472,0.48481,0.5,0.515038,0.529919,0.544639,0.559193,0.573576,0.587785,0.601815,0.615661,0.62932,0.642788,0.656059,0.669131,0.681998,0.694658,0.707107,0.71934,0.731354,0.743145,0.75471,0.766044,0.777146,0.788011,0.798636,0.809017,0.819152,0.829038,0.838671,0.848048,0.857167,0.866025,0.87462,0.882948,0.891007,0.898794,0.906308,0.913545,0.920505,0.927184,0.93358,0.939693,0.945519,0.951057,0.956305,0.961262,0.965926,0.970296,0.97437,0.978148,0.981627,0.984808,0.987688,0.990268,0.992546,0.994522,0.996195,0.997564,0.99863,0.999391,0.999848,1,0.999848,0.999391,0.99863,0.997564,0.996195,0.994522,0.992546,0.990268,0.987688,0.984808,0.981627,0.978148,0.97437,0.970296,0.965926,0.961262,0.956305,0.951057,0.945519,0.939693,0.93358,0.927184,0.920505,0.913545,0.906308,0.898794,0.891007,0.882948,0.87462,0.866025,0.857167,0.848048,0.838671,0.829038,0.819152,0.809017,0.798636,0.788011,0.777146,0.766044,0.75471,0.743145,0.731354,0.71934,0.707107,0.694658,0.681998,0.669131,0.656059,0.642788,0.62932,0.615661,0.601815,0.587785,0.573576,0.559193,0.544639,0.529919,0.515038,0.5,0.48481,0.469472,0.45399,0.438371,0.422618,0.406737,0.390731,0.374607,0.358368,0.34202,0.325568,0.309017,0.292372,0.275637,0.258819,0.241922,0.224951,0.207912,0.190809,0.173648,0.156434,0.139173,0.121869,0.104528,0.0871557,0.0697565,0.052336,0.0348995,0.0174524,-4.979e-17,-0.0349048,-0.069799,-0.104672,-0.139513,-0.174311,-0.209057,-0.243739,-0.278346,-0.312869,-0.347296,-0.381618,-0.415823,-0.449902,-0.483844,-0.517638,-0.551275,-0.584743,-0.618034,-0.651136,-0.68404,-0.716736,-0.749213,-0.781462,-0.813473,-0.845237,-0.876742,-0.907981,-0.938943,-0.969619,-1,-1.03008,-1.05984,-1.08928,-1.11839,-1.14715,-1.17557,-1.20363,-1.23132,-1.25864,-1.28558,-1.31212,-1.33826,-1.364,-1.38932,-1.41421,-1.43868,-1.46271,-1.48629,-1.50942,-1.53209,-1.55429,-1.57602,-1.59727,-1.61803,-1.6383,-1.65808,-1.67734,-1.6961,-1.71433,-1.73205,-1.74924,-1.7659,-1.78201,-1.79759,-1.81262,-1.82709,-1.84101,-1.85437,-1.86716,-1.87939,-1.89104,-1.90211,-1.91261,-1.92252,-1.93185,-1.94059,-1.94874,-1.9563,-1.96325,-1.96962,-1.97538,-1.98054,-1.98509,-1.98904,-1.99239,-1.99513,-1.99726,-1.99878,-1.9997,-2,-1.99954,-1.99817,-1.99589,-1.99269,-1.98858,-1.98357,-1.97764,-1.9708,-1.96307,-1.95442,-1.94488,-1.93444,-1.92311,-1.91089,-1.89778,-1.88379,-1.86891,-1.85317,-1.83656,-1.81908,-1.80074,-1.78155,-1.76151,-1.74064,-1.71892,-1.69638,-1.67302,-1.64884,-1.62386,-1.59808,-1.5715,-1.54414,-1.51601,-1.48711,-1.45746,-1.42705,-1.39591,-1.36403,-1.33144,-1.29813,-1.26413,-1.22943,-1.19406,-1.15802,-1.12132,-1.08398,-1.046,-1.00739,-0.968177,-0.928363,-0.887961,-0.846984,-0.805445,-0.763356,-0.720729,-0.677579,-0.633917,-0.589758,-0.545114,-0.5,-0.454429,-0.408415,-0.361971,-0.315113,-0.267855,-0.22021,-0.172193,-0.12382,-0.0751038,-0.0260604,0.0232955,0.072949,0.122885,0.173088,0.223543,0.274234,0.325147,0.376265,0.427573,0.479055,0.530697,0.582481,0.634392,0.686415,0.738533,0.790731,0.842992,0.895302,0.947643,1] 8 | track_points_y: [0,0.000152305,0.000609173,0.00137047,0.00243595,0.0038053,0.0054781,0.00745385,0.00973193,0.0123117,0.0151922,0.0183728,0.0218524,0.0256299,0.0297043,0.0340742,0.0387383,0.0436952,0.0489435,0.0544814,0.0603074,0.0664196,0.0728161,0.0794951,0.0864545,0.0936922,0.101206,0.108993,0.117052,0.12538,0.133975,0.142833,0.151952,0.161329,0.170962,0.180848,0.190983,0.201364,0.211989,0.222854,0.233956,0.24529,0.256855,0.268646,0.28066,0.292893,0.305342,0.318002,0.330869,0.343941,0.357212,0.37068,0.384339,0.398185,0.412215,0.426424,0.440807,0.455361,0.470081,0.484962,0.5,0.51519,0.530528,0.54601,0.561629,0.577382,0.593263,0.609269,0.625393,0.641632,0.65798,0.674432,0.690983,0.707628,0.724363,0.741181,0.758078,0.775049,0.792088,0.809191,0.826352,0.843566,0.860827,0.878131,0.895472,0.912844,0.930244,0.947664,0.965101,0.982548,1,1.01745,1.0349,1.05234,1.06976,1.08716,1.10453,1.12187,1.13917,1.15643,1.17365,1.19081,1.20791,1.22495,1.24192,1.25882,1.27564,1.29237,1.30902,1.32557,1.34202,1.35837,1.37461,1.39073,1.40674,1.42262,1.43837,1.45399,1.46947,1.48481,1.5,1.51504,1.52992,1.54464,1.55919,1.57358,1.58779,1.60182,1.61566,1.62932,1.64279,1.65606,1.66913,1.682,1.69466,1.70711,1.71934,1.73135,1.74314,1.75471,1.76604,1.77715,1.78801,1.79864,1.80902,1.81915,1.82904,1.83867,1.84805,1.85717,1.86603,1.87462,1.88295,1.89101,1.89879,1.90631,1.91355,1.9205,1.92718,1.93358,1.93969,1.94552,1.95106,1.9563,1.96126,1.96593,1.9703,1.97437,1.97815,1.98163,1.98481,1.98769,1.99027,1.99255,1.99452,1.99619,1.99756,1.99863,1.99939,1.99985,2,1.9997,1.99878,1.99726,1.99513,1.99239,1.98904,1.98509,1.98054,1.97538,1.96962,1.96325,1.9563,1.94874,1.94059,1.93185,1.92252,1.91261,1.90211,1.89104,1.87939,1.86716,1.85437,1.84101,1.82709,1.81262,1.79759,1.78201,1.7659,1.74924,1.73205,1.71433,1.6961,1.67734,1.65808,1.6383,1.61803,1.59727,1.57602,1.55429,1.53209,1.50942,1.48629,1.46271,1.43868,1.41421,1.38932,1.364,1.33826,1.31212,1.28558,1.25864,1.23132,1.20363,1.17557,1.14715,1.11839,1.08928,1.05984,1.03008,1,0.969619,0.938943,0.907981,0.876742,0.845237,0.813473,0.781462,0.749213,0.716736,0.68404,0.651136,0.618034,0.584743,0.551275,0.517638,0.483844,0.449902,0.415823,0.381618,0.347296,0.312869,0.278346,0.243739,0.209057,0.174311,0.139513,0.104672,0.069799,0.0349048,2.98023e-08,-0.0523572,-0.104698,-0.157008,-0.209269,-0.261467,-0.313585,-0.365608,-0.417519,-0.469303,-0.520945,-0.572427,-0.623735,-0.674853,-0.725766,-0.776457,-0.826912,-0.877115,-0.927051,-0.976704,-1.02606,-1.0751,-1.12382,-1.17219,-1.22021,-1.26785,-1.31511,-1.36197,-1.40841,-1.45443,-1.5,-1.54511,-1.58976,-1.63392,-1.67758,-1.72073,-1.76336,-1.80545,-1.84698,-1.88796,-1.92836,-1.96818,-2.00739,-2.046,-2.08398,-2.12132,-2.15802,-2.19406,-2.22943,-2.26413,-2.29813,-2.33144,-2.36403,-2.39591,-2.42705,-2.45746,-2.48711,-2.51601,-2.54414,-2.5715,-2.59808,-2.62386,-2.64884,-2.67302,-2.69638,-2.71892,-2.74064,-2.76151,-2.78155,-2.80074,-2.81908,-2.83656,-2.85317,-2.86891,-2.88379,-2.89778,-2.91089,-2.92311,-2.93444,-2.94488,-2.95442,-2.96306,-2.9708,-2.97764,-2.98357,-2.98858,-2.99269,-2.99589,-2.99817,-2.99954,-3] 9 | 10 | 11 | 12 | desired_v: 0.8 # desired velocity 13 | 14 | ll: 1.282 # ll of the car 15 | dt: 0.05 16 | rho: 1.0 # J = (x-x_r)^2+ (y-y_r)^2 + rho * (phi-phi_r)^2 17 | N: 40 # horizon 40 18 | rhoN: 1.0 19 | 20 | # lower and upper bound 21 | v_max: 2.0 22 | a_max: 2.0 23 | delta_max: 1.3 24 | ddelta_max: 1.3 25 | 26 | # ctrl delay 27 | delay: 0.125 28 | 29 | # true for nmpc, false for qp 30 | nmpc: true 31 | -------------------------------------------------------------------------------- /Course_2/Example_1/src/gcopter/include/gcopter/geo_utils.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | MIT License 3 | 4 | Copyright (c) 2021 Zhepei Wang (wangzhepei@live.com) 5 | 6 | Permission is hereby granted, free of charge, to any person obtaining a copy 7 | of this software and associated documentation files (the "Software"), to deal 8 | in the Software without restriction, including without limitation the rights 9 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | copies of the Software, and to permit persons to whom the Software is 11 | furnished to do so, subject to the following conditions: 12 | 13 | The above copyright notice and this permission notice shall be included in all 14 | copies or substantial portions of the Software. 15 | 16 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | SOFTWARE. 23 | */ 24 | 25 | #ifndef GEO_UTILS_HPP 26 | #define GEO_UTILS_HPP 27 | 28 | #include "quickhull.hpp" 29 | #include "sdlp.hpp" 30 | 31 | #include 32 | 33 | #include 34 | #include 35 | #include 36 | #include 37 | 38 | namespace geo_utils 39 | { 40 | 41 | // Each row of hPoly is defined by h0, h1, h2, h3 as 42 | // h0*x + h1*y + h2*z + h3 <= 0 43 | inline bool findInterior(const Eigen::MatrixX4d &hPoly, 44 | Eigen::Vector3d &interior) 45 | { 46 | const int m = hPoly.rows(); 47 | 48 | Eigen::MatrixX4d A(m, 4); 49 | Eigen::VectorXd b(m); 50 | Eigen::Vector4d c, x; 51 | const Eigen::ArrayXd hNorm = hPoly.leftCols<3>().rowwise().norm(); 52 | A.leftCols<3>() = hPoly.leftCols<3>().array().colwise() / hNorm; 53 | A.rightCols<1>().setConstant(1.0); 54 | b = -hPoly.rightCols<1>().array() / hNorm; 55 | c.setZero(); 56 | c(3) = -1.0; 57 | 58 | const double minmaxsd = sdlp::linprog<4>(c, A, b, x); 59 | interior = x.head<3>(); 60 | 61 | return minmaxsd < 0.0 && !std::isinf(minmaxsd); 62 | } 63 | 64 | inline bool overlap(const Eigen::MatrixX4d &hPoly0, 65 | const Eigen::MatrixX4d &hPoly1, 66 | const double eps = 1.0e-6) 67 | 68 | { 69 | const int m = hPoly0.rows(); 70 | const int n = hPoly1.rows(); 71 | Eigen::MatrixX4d A(m + n, 4); 72 | Eigen::Vector4d c, x; 73 | Eigen::VectorXd b(m + n); 74 | A.leftCols<3>().topRows(m) = hPoly0.leftCols<3>(); 75 | A.leftCols<3>().bottomRows(n) = hPoly1.leftCols<3>(); 76 | A.rightCols<1>().setConstant(1.0); 77 | b.topRows(m) = -hPoly0.rightCols<1>(); 78 | b.bottomRows(n) = -hPoly1.rightCols<1>(); 79 | c.setZero(); 80 | c(3) = -1.0; 81 | 82 | const double minmaxsd = sdlp::linprog<4>(c, A, b, x); 83 | 84 | return minmaxsd < -eps && !std::isinf(minmaxsd); 85 | } 86 | 87 | struct filterLess 88 | { 89 | inline bool operator()(const Eigen::Vector3d &l, 90 | const Eigen::Vector3d &r) 91 | { 92 | return l(0) < r(0) || 93 | (l(0) == r(0) && 94 | (l(1) < r(1) || 95 | (l(1) == r(1) && 96 | l(2) < r(2)))); 97 | } 98 | }; 99 | 100 | inline void filterVs(const Eigen::Matrix3Xd &rV, 101 | const double &epsilon, 102 | Eigen::Matrix3Xd &fV) 103 | { 104 | const double mag = std::max(fabs(rV.maxCoeff()), fabs(rV.minCoeff())); 105 | const double res = mag * std::max(fabs(epsilon) / mag, DBL_EPSILON); 106 | std::set filter; 107 | fV = rV; 108 | int offset = 0; 109 | Eigen::Vector3d quanti; 110 | for (int i = 0; i < rV.cols(); i++) 111 | { 112 | quanti = (rV.col(i) / res).array().round(); 113 | if (filter.find(quanti) == filter.end()) 114 | { 115 | filter.insert(quanti); 116 | fV.col(offset) = rV.col(i); 117 | offset++; 118 | } 119 | } 120 | fV = fV.leftCols(offset).eval(); 121 | return; 122 | } 123 | 124 | // Each row of hPoly is defined by h0, h1, h2, h3 as 125 | // h0*x + h1*y + h2*z + h3 <= 0 126 | // proposed epsilon is 1.0e-6 127 | inline void enumerateVs(const Eigen::MatrixX4d &hPoly, 128 | const Eigen::Vector3d &inner, 129 | Eigen::Matrix3Xd &vPoly, 130 | const double epsilon = 1.0e-6) 131 | { 132 | const Eigen::VectorXd b = -hPoly.rightCols<1>() - hPoly.leftCols<3>() * inner; 133 | const Eigen::Matrix A = 134 | (hPoly.leftCols<3>().array().colwise() / b.array()).transpose(); 135 | 136 | quickhull::QuickHull qh; 137 | const double qhullEps = std::min(epsilon, quickhull::defaultEps()); 138 | // CCW is false because the normal in quickhull towards interior 139 | const auto cvxHull = qh.getConvexHull(A.data(), A.cols(), false, true, qhullEps); 140 | const auto &idBuffer = cvxHull.getIndexBuffer(); 141 | const int hNum = idBuffer.size() / 3; 142 | Eigen::Matrix3Xd rV(3, hNum); 143 | Eigen::Vector3d normal, point, edge0, edge1; 144 | for (int i = 0; i < hNum; i++) 145 | { 146 | point = A.col(idBuffer[3 * i + 1]); 147 | edge0 = point - A.col(idBuffer[3 * i]); 148 | edge1 = A.col(idBuffer[3 * i + 2]) - point; 149 | normal = edge0.cross(edge1); //cross in CW gives an outter normal 150 | rV.col(i) = normal / normal.dot(point); 151 | } 152 | filterVs(rV, epsilon, vPoly); 153 | vPoly = (vPoly.array().colwise() + inner.array()).eval(); 154 | return; 155 | } 156 | 157 | // Each row of hPoly is defined by h0, h1, h2, h3 as 158 | // h0*x + h1*y + h2*z + h3 <= 0 159 | // proposed epsilon is 1.0e-6 160 | inline bool enumerateVs(const Eigen::MatrixX4d &hPoly, 161 | Eigen::Matrix3Xd &vPoly, 162 | const double epsilon = 1.0e-6) 163 | { 164 | Eigen::Vector3d inner; 165 | if (findInterior(hPoly, inner)) 166 | { 167 | enumerateVs(hPoly, inner, vPoly, epsilon); 168 | return true; 169 | } 170 | else 171 | { 172 | return false; 173 | } 174 | } 175 | 176 | } // namespace geo_utils 177 | 178 | #endif 179 | -------------------------------------------------------------------------------- /Course_2/Example_1/src/README.md: -------------------------------------------------------------------------------- 1 | # GCOPTER 2 | 3 | __GCOPTER__ is an efficient and versatile multicopter trajectory optimizer built upon a novel sparse trajectory representation named [__MINCO__](https://arxiv.org/pdf/2103.00190.pdf). __User-defined state-input constraints__ for dynamics involving [__nonlinear drag effects__](https://github.com/ZJU-FAST-Lab/GCOPTER/blob/main/misc/flatness.pdf) are supported. 4 | 5 | ## Updates 6 | 7 | * **Mar 11, 2022** - A minimal but non-trivial example for global kinodynamic planning is released. Modules for trajectory optimization, quadcopter dynamics with nonlinear drags, [fast iterative region inflation for corridor generation](https://github.com/ZJU-FAST-Lab/GCOPTER/blob/main/gcopter/include/gcopter/firi.hpp), non-uniform MINCO (s=3), etc., are released. 8 | 9 | * **Mar 15, 2022** - Released non-uniform MINCO for s=2 and s=4. 10 | 11 | * **May 19, 2022** - Released a doc to detail [differential flatness for multicopters under nonlinear drag effects](https://github.com/ZJU-FAST-Lab/GCOPTER/blob/main/misc/flatness.pdf). Add code links for all projects powered by MINCO. 12 | 13 | * **Plan** - __More examples are on the way__, including uniform MINCO (s=2,3,4), trajectory generation for tube-shaped and sphere-shaped corridors, local replanner, whole-body SE(3) planner, interfaces for external constraints, augmented Lagrangian, and so on. 14 | 15 | ## About 16 | 17 | If our repo helps your academic projects, please cite our paper. Thank you! 18 | 19 | __Author__: [Zhepei Wang](https://zhepeiwang.github.io) and [Fei Gao](https://scholar.google.com/citations?hl=en&user=4RObDv0AAAAJ) from [ZJU FAST Lab](http://zju-fast.com). 20 | 21 | __Paper__: [Geometrically Constrained Trajectory Optimization for Multicopters](https://arxiv.org/abs/2103.00190), Zhepei Wang, Xin Zhou, Chao Xu, and Fei Gao, [IEEE Transactions on Robotics](https://doi.org/10.1109/TRO.2022.3160022) (__T-RO__), Regular Paper. 22 | ``` 23 | @article{WANG2022GCOPTER, 24 | title={Geometrically Constrained Trajectory Optimization for Multicopters}, 25 | author={Wang, Zhepei and Zhou, Xin and Xu, Chao and Gao, Fei}, 26 | journal={IEEE Transactions on Robotics}, 27 | year={2022}, 28 | volume={}, 29 | number={}, 30 | pages={1-20}, 31 | doi={10.1109/TRO.2022.3160022} 32 | } 33 | ``` 34 | 35 | ## Applications 36 | 37 | ### Example 1: Global Trajectory Planning 38 | 39 | This is a minimal yet non-trivial example of our trajectory optimizer for real-time high-quality corridor and global trajectory generation subject to dynamic constraints. For installation, the following terminal commands are helpful. 40 | 41 | sudo apt update 42 | sudo apt install cpufrequtils 43 | sudo apt install libompl-dev 44 | sudo cpufreq-set -g performance 45 | mkdir ROS; cd ROS; mkdir src; cd src 46 | git clone https://github.com/ZJU-FAST-Lab/GCOPTER.git 47 | cd .. 48 | catkin_make 49 | source devel/setup.bash 50 | roslaunch gcopter global_planning.launch 51 | 52 | After conduct the command, you will see the windows for rviz and rqt_plot. Please follow the gif below for global trajectory planning in a random map. 53 |

54 | 55 |

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

63 | 64 |

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

70 | 71 |

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

77 | 78 |

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

84 | 85 |

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

91 | 92 |

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

98 | 99 |

100 | 101 | 102 | - A variety of applications powered by __GCOPTER__ or __MINCO__ are not listed here, such as [visibility-aware aerial tracking](https://arxiv.org/abs/2109.07111), and [planning with nonlinear drag effects](https://arxiv.org/abs/2109.08403), etc. 103 | 104 | ## Powerful Submodules 105 | - [SDLP: Seidel's Algorithm](https://github.com/ZJU-FAST-Lab/SDLP) on Linear-Complexity Linear Programming for Computational Geometry. 106 | - [VertexEnumeration3D](https://github.com/ZJU-FAST-Lab/VertexEnumeration3D): Highly Efficient Vertex Enumeration for 3D Convex Polytopes (Outperforms [cddlib](https://github.com/cddlib/cddlib) in 3D). 107 | - [LBFGS-Lite](https://github.com/ZJU-FAST-Lab/LBFGS-Lite): An Easy-to-Use Header-Only L-BFGS Solver. 108 | -------------------------------------------------------------------------------- /Course_2/Example_1/src/map_gen/mockamap/config/rviz.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | - /Path1 10 | - /PointCloud22 11 | Splitter Ratio: 0.5 12 | Tree Height: 489 13 | - Class: rviz/Selection 14 | Name: Selection 15 | - Class: rviz/Tool Properties 16 | Expanded: 17 | - /2D Pose Estimate1 18 | - /2D Nav Goal1 19 | - /Publish Point1 20 | - /Game Like Input1 21 | Name: Tool Properties 22 | Splitter Ratio: 0.588679 23 | - Class: rviz/Views 24 | Expanded: 25 | - /Current View1 26 | Name: Views 27 | Splitter Ratio: 0.5 28 | - Class: rviz/Time 29 | Experimental: false 30 | Name: Time 31 | SyncMode: 0 32 | SyncSource: PointCloud2 33 | - Class: mockamap/PCDView 34 | Input: /mock_map 35 | Name: PCDView 36 | Output: /mock_map/cut 37 | Visualization Manager: 38 | Class: "" 39 | Displays: 40 | - Alpha: 0.5 41 | Cell Size: 1 42 | Class: rviz/Grid 43 | Color: 160; 160; 164 44 | Enabled: true 45 | Line Style: 46 | Line Width: 0.03 47 | Value: Lines 48 | Name: Grid 49 | Normal Cell Count: 0 50 | Offset: 51 | X: 0 52 | Y: 0 53 | Z: 0 54 | Plane: XY 55 | Plane Cell Count: 10 56 | Reference Frame: 57 | Value: true 58 | - Alpha: 1 59 | Autocompute Intensity Bounds: true 60 | Autocompute Value Bounds: 61 | Max Value: 1.9 62 | Min Value: 0 63 | Value: true 64 | Axis: Z 65 | Channel Name: intensity 66 | Class: rviz/PointCloud2 67 | Color: 255; 255; 255 68 | Color Transformer: AxisColor 69 | Decay Time: 0 70 | Enabled: false 71 | Invert Rainbow: false 72 | Max Color: 255; 255; 255 73 | Max Intensity: 4096 74 | Min Color: 0; 0; 0 75 | Min Intensity: 0 76 | Name: PointCloud2 77 | Position Transformer: XYZ 78 | Queue Size: 10 79 | Selectable: true 80 | Size (Pixels): 3 81 | Size (m): 0.1 82 | Style: Boxes 83 | Topic: /mock_map 84 | Unreliable: false 85 | Use Fixed Frame: true 86 | Use rainbow: true 87 | Value: false 88 | - Alpha: 1 89 | Buffer Length: 1 90 | Class: rviz/Path 91 | Color: 255; 85; 255 92 | Enabled: true 93 | Head Diameter: 0.3 94 | Head Length: 0.2 95 | Length: 0.3 96 | Line Style: Lines 97 | Line Width: 0.03 98 | Name: Path 99 | Offset: 100 | X: 0 101 | Y: 0 102 | Z: 0 103 | Pose Style: None 104 | Radius: 0.03 105 | Shaft Diameter: 0.1 106 | Shaft Length: 0.1 107 | Topic: /theta_star_node/output 108 | Unreliable: false 109 | Value: true 110 | - Alpha: 1 111 | Autocompute Intensity Bounds: true 112 | Autocompute Value Bounds: 113 | Max Value: 1.9 114 | Min Value: 0 115 | Value: true 116 | Axis: Z 117 | Channel Name: intensity 118 | Class: rviz/PointCloud2 119 | Color: 255; 255; 255 120 | Color Transformer: AxisColor 121 | Decay Time: 0 122 | Enabled: true 123 | Invert Rainbow: false 124 | Max Color: 255; 255; 255 125 | Max Intensity: 4096 126 | Min Color: 0; 0; 0 127 | Min Intensity: 0 128 | Name: PointCloud2 129 | Position Transformer: XYZ 130 | Queue Size: 10 131 | Selectable: true 132 | Size (Pixels): 3 133 | Size (m): 0.1 134 | Style: Boxes 135 | Topic: /mock_map/cut 136 | Unreliable: false 137 | Use Fixed Frame: true 138 | Use rainbow: true 139 | Value: true 140 | Enabled: true 141 | Global Options: 142 | Background Color: 48; 48; 48 143 | Fixed Frame: map 144 | Frame Rate: 30 145 | Name: root 146 | Tools: 147 | - Class: rviz/Interact 148 | Hide Inactive Objects: true 149 | - Class: rviz/MoveCamera 150 | - Class: rviz/Select 151 | - Class: rviz/FocusCamera 152 | - Class: rviz/Measure 153 | - Class: rviz/SetInitialPose 154 | Topic: /initialpose 155 | - Class: rviz/SetGoal 156 | Topic: /move_base_simple/goal 157 | - Class: rviz/PublishPoint 158 | Single click: true 159 | Topic: /clicked_point 160 | - Boost Property: 0.5 161 | Class: rviz/FPSMotionTool 162 | Fallback Tool: rviz/Interact 163 | Fallback ViewController: rviz/Orbit 164 | Fly Mode: false 165 | Left Hand Mode: false 166 | Step Length: 0.1 167 | - Class: rviz_plugins/GameLikeInput 168 | RangeX_max: 1000 169 | RangeX_min: -1000 170 | RangeY_max: 1000 171 | RangeY_min: -1000 172 | RangeZ_max: 1000 173 | RangeZ_min: -1000 174 | TopicPoint: point_list 175 | TopicSelect: select_list 176 | TopicSwarm: swarm 177 | Value: true 178 | Views: 179 | Current: 180 | Class: rviz/Orbit 181 | Distance: 9.57022 182 | Enable Stereo Rendering: 183 | Stereo Eye Separation: 0.06 184 | Stereo Focal Distance: 1 185 | Swap Stereo Eyes: false 186 | Value: false 187 | Focal Point: 188 | X: -2.02708 189 | Y: -4.23441 190 | Z: 0.814007 191 | Name: Current View 192 | Near Clip Distance: 0.01 193 | Pitch: 0.525203 194 | Target Frame: 195 | Value: Orbit (rviz) 196 | Yaw: 1.88225 197 | Saved: ~ 198 | Window Geometry: 199 | Displays: 200 | collapsed: false 201 | Height: 1056 202 | Hide Left Dock: false 203 | Hide Right Dock: false 204 | PCDView: 205 | collapsed: false 206 | QMainWindow State: 000000ff00000000fd00000004000000000000027000000396fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afc0000002800000296000000fb0100001dfa000000010100000002fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000000ffffffff0000009800fffffffb000000100044006900730070006c00610079007301000000000000016a0000016a00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000c00540065006c0065006f0070000000024d0000009e0000000000000000fb0000000e005000430044005600690065007701000002c4000000fa000000cd00ffffff000000010000010f00000396fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000002800000396000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073f0000003efc0100000002fb0000000800540069006d006501000000000000073f000002f600fffffffb0000000800540069006d00650100000000000004500000000000000000000003b40000039600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 207 | Selection: 208 | collapsed: false 209 | Time: 210 | collapsed: false 211 | Tool Properties: 212 | collapsed: false 213 | Views: 214 | collapsed: false 215 | Width: 1855 216 | X: 65 217 | Y: 24 218 | -------------------------------------------------------------------------------- /Course_2/Example_1/src/gcopter/include/gcopter/voxel_dilater.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | MIT License 3 | 4 | Copyright (c) 2021 Zhepei Wang (wangzhepei@live.com) 5 | 6 | Permission is hereby granted, free of charge, to any person obtaining a copy 7 | of this software and associated documentation files (the "Software"), to deal 8 | in the Software without restriction, including without limitation the rights 9 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | copies of the Software, and to permit persons to whom the Software is 11 | furnished to do so, subject to the following conditions: 12 | 13 | The above copyright notice and this permission notice shall be included in all 14 | copies or substantial portions of the Software. 15 | 16 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | SOFTWARE. 23 | */ 24 | 25 | #ifndef VOXEL_DILATER 26 | #define VOXEL_DILATER(i, j, k, x, y, z, sy, sz, bx, by, bz, ck, ogm, ofst, val, fdl) \ 27 | (ck) = (x) == 0 || (x) == (bx) || (y) == 0 || (y) == (by) || (z) == 0 || (z) == (bz); \ 28 | (i) = (x) - 1; (j) = (y) - (sy); (k) = (z) - (sz); (ofst) = (i) + (j) + (k); if ((!(ck) || ((ck) && (i) >= 0 && (j) >= 0 && (k) >= 0 )) && (ogm)[(ofst)] == 0) { (ogm)[(ofst)] = (val); (fdl).emplace_back((i), (j), (k)); } \ 29 | (i) = (x) - 1; (j) = (y) - (sy); (k) = (z); (ofst) = (i) + (j) + (k); if ((!(ck) || ((ck) && (i) >= 0 && (j) >= 0 )) && (ogm)[(ofst)] == 0) { (ogm)[(ofst)] = (val); (fdl).emplace_back((i), (j), (k)); } \ 30 | (i) = (x) - 1; (j) = (y) - (sy); (k) = (z) + (sz); (ofst) = (i) + (j) + (k); if ((!(ck) || ((ck) && (i) >= 0 && (j) >= 0 && (k) <= (bz))) && (ogm)[(ofst)] == 0) { (ogm)[(ofst)] = (val); (fdl).emplace_back((i), (j), (k)); } \ 31 | (i) = (x) - 1; (j) = (y); (k) = (z) - (sz); (ofst) = (i) + (j) + (k); if ((!(ck) || ((ck) && (i) >= 0 && (k) >= 0 )) && (ogm)[(ofst)] == 0) { (ogm)[(ofst)] = (val); (fdl).emplace_back((i), (j), (k)); } \ 32 | (i) = (x) - 1; (j) = (y); (k) = (z); (ofst) = (i) + (j) + (k); if ((!(ck) || ((ck) && (i) >= 0 )) && (ogm)[(ofst)] == 0) { (ogm)[(ofst)] = (val); (fdl).emplace_back((i), (j), (k)); } \ 33 | (i) = (x) - 1; (j) = (y); (k) = (z) + (sz); (ofst) = (i) + (j) + (k); if ((!(ck) || ((ck) && (i) >= 0 && (k) <= (bz))) && (ogm)[(ofst)] == 0) { (ogm)[(ofst)] = (val); (fdl).emplace_back((i), (j), (k)); } \ 34 | (i) = (x) - 1; (j) = (y) + (sy); (k) = (z) - (sz); (ofst) = (i) + (j) + (k); if ((!(ck) || ((ck) && (i) >= 0 && (j) <= (by) && (k) >= 0 )) && (ogm)[(ofst)] == 0) { (ogm)[(ofst)] = (val); (fdl).emplace_back((i), (j), (k)); } \ 35 | (i) = (x) - 1; (j) = (y) + (sy); (k) = (z); (ofst) = (i) + (j) + (k); if ((!(ck) || ((ck) && (i) >= 0 && (j) <= (by) )) && (ogm)[(ofst)] == 0) { (ogm)[(ofst)] = (val); (fdl).emplace_back((i), (j), (k)); } \ 36 | (i) = (x) - 1; (j) = (y) + (sy); (k) = (z) + (sz); (ofst) = (i) + (j) + (k); if ((!(ck) || ((ck) && (i) >= 0 && (j) <= (by) && (k) <= (bz))) && (ogm)[(ofst)] == 0) { (ogm)[(ofst)] = (val); (fdl).emplace_back((i), (j), (k)); } \ 37 | (i) = (x); (j) = (y) - (sy); (k) = (z) - (sz); (ofst) = (i) + (j) + (k); if ((!(ck) || ((ck) && (j) >= 0 && (k) >= 0 )) && (ogm)[(ofst)] == 0) { (ogm)[(ofst)] = (val); (fdl).emplace_back((i), (j), (k)); } \ 38 | (i) = (x); (j) = (y) - (sy); (k) = (z); (ofst) = (i) + (j) + (k); if ((!(ck) || ((ck) && (j) >= 0 )) && (ogm)[(ofst)] == 0) { (ogm)[(ofst)] = (val); (fdl).emplace_back((i), (j), (k)); } \ 39 | (i) = (x); (j) = (y) - (sy); (k) = (z) + (sz); (ofst) = (i) + (j) + (k); if ((!(ck) || ((ck) && (j) >= 0 && (k) <= (bz))) && (ogm)[(ofst)] == 0) { (ogm)[(ofst)] = (val); (fdl).emplace_back((i), (j), (k)); } \ 40 | (i) = (x); (j) = (y); (k) = (z) - (sz); (ofst) = (i) + (j) + (k); if ((!(ck) || ((ck) && (k) >= 0 )) && (ogm)[(ofst)] == 0) { (ogm)[(ofst)] = (val); (fdl).emplace_back((i), (j), (k)); } \ 41 | (i) = (x); (j) = (y); (k) = (z) + (sz); (ofst) = (i) + (j) + (k); if ((!(ck) || ((ck) && (k) <= (bz))) && (ogm)[(ofst)] == 0) { (ogm)[(ofst)] = (val); (fdl).emplace_back((i), (j), (k)); } \ 42 | (i) = (x); (j) = (y) + (sy); (k) = (z) - (sz); (ofst) = (i) + (j) + (k); if ((!(ck) || ((ck) && (j) <= (by) && (k) >= 0 )) && (ogm)[(ofst)] == 0) { (ogm)[(ofst)] = (val); (fdl).emplace_back((i), (j), (k)); } \ 43 | (i) = (x); (j) = (y) + (sy); (k) = (z); (ofst) = (i) + (j) + (k); if ((!(ck) || ((ck) && (j) <= (by) )) && (ogm)[(ofst)] == 0) { (ogm)[(ofst)] = (val); (fdl).emplace_back((i), (j), (k)); } \ 44 | (i) = (x); (j) = (y) + (sy); (k) = (z) + (sz); (ofst) = (i) + (j) + (k); if ((!(ck) || ((ck) && (j) <= (by) && (k) <= (bz))) && (ogm)[(ofst)] == 0) { (ogm)[(ofst)] = (val); (fdl).emplace_back((i), (j), (k)); } \ 45 | (i) = (x) + 1; (j) = (y) - (sy); (k) = (z) - (sz); (ofst) = (i) + (j) + (k); if ((!(ck) || ((ck) && (i) <= (bx) && (j) >= 0 && (k) >= 0 )) && (ogm)[(ofst)] == 0) { (ogm)[(ofst)] = (val); (fdl).emplace_back((i), (j), (k)); } \ 46 | (i) = (x) + 1; (j) = (y) - (sy); (k) = (z); (ofst) = (i) + (j) + (k); if ((!(ck) || ((ck) && (i) <= (bx) && (j) >= 0 )) && (ogm)[(ofst)] == 0) { (ogm)[(ofst)] = (val); (fdl).emplace_back((i), (j), (k)); } \ 47 | (i) = (x) + 1; (j) = (y) - (sy); (k) = (z) + (sz); (ofst) = (i) + (j) + (k); if ((!(ck) || ((ck) && (i) <= (bx) && (j) >= 0 && (k) <= (bz))) && (ogm)[(ofst)] == 0) { (ogm)[(ofst)] = (val); (fdl).emplace_back((i), (j), (k)); } \ 48 | (i) = (x) + 1; (j) = (y); (k) = (z) - (sz); (ofst) = (i) + (j) + (k); if ((!(ck) || ((ck) && (i) <= (bx) && (k) >= 0 )) && (ogm)[(ofst)] == 0) { (ogm)[(ofst)] = (val); (fdl).emplace_back((i), (j), (k)); } \ 49 | (i) = (x) + 1; (j) = (y); (k) = (z); (ofst) = (i) + (j) + (k); if ((!(ck) || ((ck) && (i) <= (bx) )) && (ogm)[(ofst)] == 0) { (ogm)[(ofst)] = (val); (fdl).emplace_back((i), (j), (k)); } \ 50 | (i) = (x) + 1; (j) = (y); (k) = (z) + (sz); (ofst) = (i) + (j) + (k); if ((!(ck) || ((ck) && (i) <= (bx) && (k) <= (bz))) && (ogm)[(ofst)] == 0) { (ogm)[(ofst)] = (val); (fdl).emplace_back((i), (j), (k)); } \ 51 | (i) = (x) + 1; (j) = (y) + (sy); (k) = (z) - (sz); (ofst) = (i) + (j) + (k); if ((!(ck) || ((ck) && (i) <= (bx) && (j) <= (by) && (k) >= 0 )) && (ogm)[(ofst)] == 0) { (ogm)[(ofst)] = (val); (fdl).emplace_back((i), (j), (k)); } \ 52 | (i) = (x) + 1; (j) = (y) + (sy); (k) = (z); (ofst) = (i) + (j) + (k); if ((!(ck) || ((ck) && (i) <= (bx) && (j) <= (by) )) && (ogm)[(ofst)] == 0) { (ogm)[(ofst)] = (val); (fdl).emplace_back((i), (j), (k)); } \ 53 | (i) = (x) + 1; (j) = (y) + (sy); (k) = (z) + (sz); (ofst) = (i) + (j) + (k); if ((!(ck) || ((ck) && (i) <= (bx) && (j) <= (by) && (k) <= (bz))) && (ogm)[(ofst)] == 0) { (ogm)[(ofst)] = (val); (fdl).emplace_back((i), (j), (k)); } 54 | #endif 55 | -------------------------------------------------------------------------------- /Course_4/Example_1/L4_HW_Solution/L4_HW_Solution/socp/socp.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | using namespace Eigen; 6 | using namespace std; 7 | 8 | 9 | class SocpSolver 10 | { 11 | public: 12 | SocpSolver(const VectorXd& c, const vector& As, const vector& bs, const MatrixXd& G, const VectorXd& h) 13 | { 14 | c_ = c; 15 | As_ = As; 16 | bs_ = bs; 17 | G_ = G; 18 | h_ = h; 19 | } 20 | 21 | bool is_all_projection_valid(const VectorXd& x, double p, const vector& us) 22 | { 23 | for(unsigned int i = 0; i< As_.size(); i++) 24 | { 25 | if((us[i]/p - get_projection(us[i]/p - (As_[i]*x + bs_[i]))).lpNorm() >= 1e-4) 26 | { 27 | return false; 28 | } 29 | } 30 | return true; 31 | } 32 | 33 | VectorXd solve() 34 | { 35 | double p = 1; 36 | VectorXd lamda = VectorXd::Zero(h_.size()); 37 | VectorXd x(c_.size()); 38 | x.setZero(); 39 | vector us; 40 | for(unsigned int i = 0; i < As_.size(); i++) 41 | { 42 | VectorXd u(bs_[i].size()); 43 | u.setZero(); 44 | us.push_back(u); 45 | } 46 | while(true) 47 | { 48 | get_min_x(x, p, lamda, us); 49 | if((G_ * x - h_).lpNorm() < 1e-4 and is_all_projection_valid(x, p, us) and get_gradient(x, p, lamda, us).norm()/x.norm() < 1e-4) 50 | { 51 | break; 52 | } 53 | lamda = lamda + p * ((G_*x) - h_); 54 | for(unsigned j = 0; j < As_.size(); j++) 55 | { 56 | us[j] = get_projection(us[j] - p * (As_[j]*x + bs_[j])); 57 | } 58 | p = 2 * p; 59 | if(p > 1000) 60 | { 61 | p = 1000; 62 | } 63 | } 64 | return x; 65 | 66 | } 67 | 68 | MatrixXd do_get_hession(const VectorXd& x, double p, const VectorXd& lamda, const MatrixXd& A, const VectorXd& b, const VectorXd& u) 69 | { 70 | VectorXd inner = u - p * (A * x + b); 71 | double x0 = inner[0]; 72 | VectorXd x1(u.size() - 1); 73 | x1[0] = inner[1]; 74 | x1[1] = inner[2]; 75 | x1[2] = inner[3]; 76 | x1[3] = inner[4]; 77 | x1[4] = inner[5]; 78 | x1[5] = inner[6]; 79 | x1[6] = inner[7]; 80 | MatrixXd hessian(u.size(), u.size()); 81 | if(x1.norm() <= x0) 82 | { 83 | hessian.setIdentity() ; 84 | } 85 | else if(x1.norm() <= -1 * x0) 86 | { 87 | hessian.setZero(); 88 | } 89 | else 90 | { 91 | MatrixXd sub_hessian = (x0 + x1.norm())/(2 * x1.norm()) * MatrixXd::Identity(x1.size(), x1.size()) - x0 * x1 * x1.transpose()/(2*x1.norm() * x1.norm() *x1.norm()); 92 | hessian(0, 0) = 1.0/2; 93 | for(int i = 1; i < 8; i++) 94 | { 95 | hessian(0, i) = x1[i - 1]/(2*x1.norm()); 96 | hessian(i, 0) = x1[i - 1]/(2*x1.norm()); 97 | } 98 | for(int i = 1; i < 8; i++) 99 | { 100 | for(int j = 1; j < 8; j++) 101 | { 102 | hessian(i, j) = sub_hessian(i -1, j -1); 103 | } 104 | } 105 | 106 | } 107 | return p * A.transpose() * hessian * A; 108 | 109 | 110 | 111 | } 112 | 113 | MatrixXd get_hession(const VectorXd& x, double p, const VectorXd& lamda, const vector& us) 114 | { 115 | MatrixXd hession = p * G_.transpose() * G_; 116 | for(unsigned int i = 0; i < As_.size(); i++) 117 | { 118 | hession += do_get_hession(x, p, lamda, As_[i], bs_[i], us[i]); 119 | } 120 | return hession; 121 | 122 | } 123 | 124 | VectorXd get_projection(const VectorXd& inner) 125 | { 126 | double x0 = inner[0]; 127 | VectorXd x1(inner.size() - 1); 128 | x1[0] = inner[1]; 129 | x1[1] = inner[2]; 130 | x1[2] = inner[3]; 131 | x1[3] = inner[4]; 132 | x1[4] = inner[5]; 133 | x1[5] = inner[6]; 134 | x1[6] = inner[7]; 135 | VectorXd g(inner.size()); 136 | g.setZero(); 137 | if(x0 <= -1 * x1.norm()) 138 | { 139 | g.setZero() ; 140 | } 141 | else if(abs(x0) < x1.norm()) 142 | { 143 | g[0] = x1.norm(); 144 | for(int i = 0; i < 7; i++) 145 | { 146 | g[i+1] = x1[i]; 147 | } 148 | 149 | g = (x0 + x1.norm())/(2 * x1.norm()) * g; 150 | } 151 | else 152 | { 153 | g = inner; 154 | } 155 | return g; 156 | 157 | } 158 | VectorXd get_gradient(const VectorXd& x, double p, const VectorXd& lamda, const vector& us) 159 | { 160 | VectorXd gradient = c_ + G_.transpose() * (lamda + p * ((G_ * x) - h_)); 161 | for(unsigned int i = 0; i < As_.size(); i++) 162 | { 163 | gradient -= As_[i].transpose() * get_projection(us[i] - p * (As_[i]*x + bs_[i])); 164 | } 165 | return gradient; 166 | } 167 | 168 | double get_value(const VectorXd& x, double p, const VectorXd& lamda, const vector& us) 169 | { 170 | double value = c_.transpose() * x + 0.5 * p * (pow(((G_*x) -h_ + lamda/p).norm(), 2)); 171 | for(unsigned int i = 0; i < As_.size(); i++) 172 | { 173 | value += 0.5 * p * pow(get_projection(us[i]/p - (As_[i]*x + bs_[i])).norm(), 2); 174 | } 175 | return value; 176 | } 177 | 178 | double get_min_x(VectorXd &x, double p, const VectorXd& lamda, const vector& us) 179 | { 180 | while(true) 181 | { 182 | double value = get_value(x, p, lamda, us); 183 | auto gradient = get_gradient(x, p, lamda, us); 184 | if(gradient.norm() / x.norm() < 1e-4) 185 | { 186 | return value; 187 | } 188 | auto hession = get_hession(x, p, lamda, us); 189 | auto d = -1 * hession.inverse() * gradient; 190 | double t = 1; 191 | while(true) 192 | { 193 | if(value - get_value(x + t * d, p, lamda, us) >= -1e-4 * t * d.transpose() * gradient) 194 | { 195 | break; 196 | } 197 | t = t/2; 198 | } 199 | x = x + t * d; 200 | } 201 | return 0; 202 | } 203 | private: 204 | VectorXd c_; 205 | 206 | MatrixXd G_; 207 | 208 | 209 | VectorXd h_; 210 | 211 | vector As_ ; 212 | 213 | vector bs_ ; 214 | }; 215 | 216 | int main() 217 | { 218 | VectorXd c(7); 219 | c[0] = 1.0; 220 | c[1] = 2.0; 221 | c[2] = 3.0; 222 | c[3] = 4.0; 223 | c[4] = 5.0; 224 | c[5] = 6.0; 225 | c[6] = 7.0; 226 | 227 | MatrixXd G(1, 7); 228 | G(0, 0) = 0; 229 | G(0, 1) = 0; 230 | G(0, 2) = 0; 231 | G(0, 3) = 0; 232 | G(0, 4) = 0; 233 | G(0, 5) = 0; 234 | G(0, 6) = 0; 235 | 236 | 237 | VectorXd h = VectorXd(1); 238 | h[0] = 0; 239 | 240 | MatrixXd A = Matrix ::Zero(); 241 | A(0, 0) = 1; 242 | A(1, 0) = 7; 243 | A(2, 1) = 6; 244 | A(3, 2) = 5; 245 | A(4, 3) = 4; 246 | A(5, 4) = 3; 247 | A(6, 5) = 2; 248 | A(7, 6) = 1; 249 | 250 | VectorXd b = Matrix::Zero(); 251 | b[0] = 1; 252 | b[1] = 1; 253 | b[2] = 3; 254 | b[3] = 5; 255 | b[4] = 7; 256 | b[5] = 9; 257 | b[6] = 11; 258 | b[7] = 13; 259 | 260 | 261 | SocpSolver socpSolver(c, {A}, {b}, G, h); 262 | auto x = socpSolver.solve(); 263 | double value = c.transpose() * x; 264 | cout << "x = " << x << endl; 265 | cout << "value = " << value << endl; 266 | // cout << "x = " << x < 54 | Value: false 55 | - Angle Tolerance: 0.0010000000474974513 56 | Class: rviz/Odometry 57 | Covariance: 58 | Orientation: 59 | Alpha: 0.5 60 | Color: 255; 255; 127 61 | Color Style: Unique 62 | Frame: Local 63 | Offset: 1 64 | Scale: 1 65 | Value: true 66 | Position: 67 | Alpha: 0.30000001192092896 68 | Color: 204; 51; 204 69 | Scale: 1 70 | Value: true 71 | Value: true 72 | Enabled: true 73 | Keep: 1 74 | Name: Odometry 75 | Position Tolerance: 0.0010000000474974513 76 | Queue Size: 1 77 | Shape: 78 | Alpha: 0.5 79 | Axes Length: 1 80 | Axes Radius: 0.10000000149011612 81 | Color: 0; 0; 255 82 | Head Length: 0.28200000524520874 83 | Head Radius: 0.5 84 | Shaft Length: 1 85 | Shaft Radius: 0.5 86 | Value: Arrow 87 | Topic: /odom 88 | Unreliable: false 89 | Value: true 90 | - Alpha: 1 91 | Buffer Length: 1 92 | Class: rviz/Path 93 | Color: 25; 255; 0 94 | Enabled: true 95 | Head Diameter: 0.30000001192092896 96 | Head Length: 0.20000000298023224 97 | Length: 0.30000001192092896 98 | Line Style: Lines 99 | Line Width: 0.029999999329447746 100 | Name: reference 101 | Offset: 102 | X: 0 103 | Y: 0 104 | Z: 0 105 | Pose Color: 255; 85; 255 106 | Pose Style: None 107 | Queue Size: 10 108 | Radius: 0.029999999329447746 109 | Shaft Diameter: 0.10000000149011612 110 | Shaft Length: 0.10000000149011612 111 | Topic: /mpc_car/reference_path 112 | Unreliable: false 113 | Value: true 114 | - Alpha: 1 115 | Buffer Length: 1 116 | Class: rviz/Path 117 | Color: 255; 0; 0 118 | Enabled: true 119 | Head Diameter: 0.30000001192092896 120 | Head Length: 0.20000000298023224 121 | Length: 0.30000001192092896 122 | Line Style: Billboards 123 | Line Width: 0.029999999329447746 124 | Name: traj 125 | Offset: 126 | X: 0 127 | Y: 0 128 | Z: 0 129 | Pose Color: 255; 85; 255 130 | Pose Style: None 131 | Queue Size: 10 132 | Radius: 0.029999999329447746 133 | Shaft Diameter: 0.10000000149011612 134 | Shaft Length: 0.10000000149011612 135 | Topic: /mpc_car/traj 136 | Unreliable: false 137 | Value: true 138 | - Alpha: 1 139 | Buffer Length: 1 140 | Class: rviz/Path 141 | Color: 255; 0; 255 142 | Enabled: true 143 | Head Diameter: 0.30000001192092896 144 | Head Length: 0.20000000298023224 145 | Length: 0.30000001192092896 146 | Line Style: Billboards 147 | Line Width: 0.029999999329447746 148 | Name: traj_delay 149 | Offset: 150 | X: 0 151 | Y: 0 152 | Z: 0 153 | Pose Color: 255; 85; 255 154 | Pose Style: None 155 | Queue Size: 10 156 | Radius: 0.029999999329447746 157 | Shaft Diameter: 0.10000000149011612 158 | Shaft Length: 0.10000000149011612 159 | Topic: /mpc_car/traj_delay 160 | Unreliable: false 161 | Value: true 162 | Enabled: true 163 | Global Options: 164 | Background Color: 48; 48; 48 165 | Default Light: true 166 | Fixed Frame: world 167 | Frame Rate: 30 168 | Name: root 169 | Tools: 170 | - Class: rviz/Interact 171 | Hide Inactive Objects: true 172 | - Class: rviz/MoveCamera 173 | - Class: rviz/Select 174 | - Class: rviz/FocusCamera 175 | - Class: rviz/Measure 176 | - Class: rviz/SetInitialPose 177 | Theta std deviation: 0.2617993950843811 178 | Topic: /initialpose 179 | X std deviation: 0.5 180 | Y std deviation: 0.5 181 | - Class: rviz/SetGoal 182 | Topic: /move_base_simple/goal 183 | - Class: rviz/PublishPoint 184 | Single click: true 185 | Topic: /clicked_point 186 | Value: true 187 | Views: 188 | Current: 189 | Angle: 0 190 | Class: rviz/TopDownOrtho 191 | Enable Stereo Rendering: 192 | Stereo Eye Separation: 0.05999999865889549 193 | Stereo Focal Distance: 1 194 | Swap Stereo Eyes: false 195 | Value: false 196 | Invert Z Axis: false 197 | Name: Current View 198 | Near Clip Distance: 0.009999999776482582 199 | Scale: 103.15830993652344 200 | Target Frame: world 201 | X: -0.5730874538421631 202 | Y: -0.29655691981315613 203 | Saved: ~ 204 | Window Geometry: 205 | Displays: 206 | collapsed: false 207 | Height: 1027 208 | Hide Left Dock: false 209 | Hide Right Dock: false 210 | QMainWindow State: 000000ff00000000fd0000000400000000000001ce00000365fc020000000afb0000001200530065006c0065006300740069006f006e000000003d0000009b0000005c00fffffffb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afc0000003d00000365000000c900fffffffa000000010100000002fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730000000000ffffffff0000000000000000fb000000100044006900730070006c00610079007301000000000000016a0000015600fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000c00540065006c0065006f0070000000024d0000009e0000000000000000fb0000000e005000430044005600690065007700000002b5000000ed0000000000000000fb0000000a0064006500700074006800000002170000018b0000000000000000000000010000018b00000365fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d00000365000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007440000003efc0100000002fb0000000800540069006d0065010000000000000744000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000003df0000036500000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 211 | Selection: 212 | collapsed: false 213 | Time: 214 | collapsed: false 215 | Views: 216 | collapsed: false 217 | Width: 1860 218 | X: 60 219 | Y: 154 220 | -------------------------------------------------------------------------------- /Course_3/Example_1/L3_HW_Solution/L3_HW_Solution/task_2_2/collision_distance_computation/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(collision_distance_computation) 3 | 4 | ## Compile as C++11, supported in ROS Kinetic and newer 5 | add_compile_options(-std=c++17) 6 | 7 | ## Find catkin macros and libraries 8 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 9 | ## is used, also find other catkin packages 10 | find_package(Eigen3 REQUIRED) 11 | 12 | find_package(catkin REQUIRED COMPONENTS 13 | roscpp 14 | rospy 15 | std_msgs 16 | ) 17 | 18 | ## System dependencies are found with CMake's conventions 19 | # find_package(Boost REQUIRED COMPONENTS system) 20 | 21 | 22 | ## Uncomment this if the package has a setup.py. This macro ensures 23 | ## modules and global scripts declared therein get installed 24 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 25 | # catkin_python_setup() 26 | 27 | ################################################ 28 | ## Declare ROS messages, services and actions ## 29 | ################################################ 30 | 31 | ## To declare and build messages, services or actions from within this 32 | ## package, follow these steps: 33 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 34 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 35 | ## * In the file package.xml: 36 | ## * add a build_depend tag for "message_generation" 37 | ## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET 38 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 39 | ## but can be declared for certainty nonetheless: 40 | ## * add a exec_depend tag for "message_runtime" 41 | ## * In this file (CMakeLists.txt): 42 | ## * add "message_generation" and every package in MSG_DEP_SET to 43 | ## find_package(catkin REQUIRED COMPONENTS ...) 44 | ## * add "message_runtime" and every package in MSG_DEP_SET to 45 | ## catkin_package(CATKIN_DEPENDS ...) 46 | ## * uncomment the add_*_files sections below as needed 47 | ## and list every .msg/.srv/.action file to be processed 48 | ## * uncomment the generate_messages entry below 49 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 50 | 51 | ## Generate messages in the 'msg' folder 52 | # add_message_files( 53 | # FILES 54 | # Message1.msg 55 | # Message2.msg 56 | # ) 57 | 58 | ## Generate services in the 'srv' folder 59 | # add_service_files( 60 | # FILES 61 | # Service1.srv 62 | # Service2.srv 63 | # ) 64 | 65 | ## Generate actions in the 'action' folder 66 | # add_action_files( 67 | # FILES 68 | # Action1.action 69 | # Action2.action 70 | # ) 71 | 72 | ## Generate added messages and services with any dependencies listed here 73 | # generate_messages( 74 | # DEPENDENCIES 75 | # std_msgs # Or other packages containing msgs 76 | # ) 77 | 78 | ################################################ 79 | ## Declare ROS dynamic reconfigure parameters ## 80 | ################################################ 81 | 82 | ## To declare and build dynamic reconfigure parameters within this 83 | ## package, follow these steps: 84 | ## * In the file package.xml: 85 | ## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" 86 | ## * In this file (CMakeLists.txt): 87 | ## * add "dynamic_reconfigure" to 88 | ## find_package(catkin REQUIRED COMPONENTS ...) 89 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 90 | ## and list every .cfg file to be processed 91 | 92 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 93 | # generate_dynamic_reconfigure_options( 94 | # cfg/DynReconf1.cfg 95 | # cfg/DynReconf2.cfg 96 | # ) 97 | 98 | ################################### 99 | ## catkin specific configuration ## 100 | ################################### 101 | ## The catkin_package macro generates cmake config files for your package 102 | ## Declare things to be passed to dependent projects 103 | ## INCLUDE_DIRS: uncomment this if your package contains header files 104 | ## LIBRARIES: libraries you create in this project that dependent projects also need 105 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 106 | ## DEPENDS: system dependencies of this project that dependent projects also need 107 | catkin_package( 108 | # INCLUDE_DIRS include 109 | # LIBRARIES collision_distance_computation 110 | # CATKIN_DEPENDS roscpp rospy 111 | # DEPENDS system_lib 112 | ) 113 | 114 | ########### 115 | ## Build ## 116 | ########### 117 | 118 | ## Specify additional locations of header files 119 | ## Your package locations should be listed before other locations 120 | include_directories( 121 | # include 122 | ${catkin_INCLUDE_DIRS} 123 | ${EIGEN3_INCLUDE_DIRS} 124 | ) 125 | 126 | ## Declare a C++ library 127 | # add_library(${PROJECT_NAME} 128 | # src/${PROJECT_NAME}/collision_distance_computation.cpp 129 | # ) 130 | 131 | ## Add cmake target dependencies of the library 132 | ## as an example, code may need to be generated before libraries 133 | ## either from message generation or dynamic reconfigure 134 | # add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 135 | 136 | ## Declare a C++ executable 137 | ## With catkin_make all packages are built within a single CMake context 138 | ## The recommended prefix ensures that target names across packages don't collide 139 | add_executable(collision_distance_computation_node src/main.cpp) 140 | 141 | ## Rename C++ executable without prefix 142 | ## The above recommended prefix causes long target names, the following renames the 143 | ## target back to the shorter version for ease of user use 144 | ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" 145 | # set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") 146 | 147 | ## Add cmake target dependencies of the executable 148 | ## same as for the library above 149 | # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 150 | 151 | ## Specify libraries to link a library or executable target against 152 | target_link_libraries(${PROJECT_NAME}_node 153 | ${catkin_LIBRARIES} 154 | ) 155 | 156 | ############# 157 | ## Install ## 158 | ############# 159 | 160 | # all install targets should use catkin DESTINATION variables 161 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 162 | 163 | ## Mark executable scripts (Python etc.) for installation 164 | ## in contrast to setup.py, you can choose the destination 165 | # catkin_install_python(PROGRAMS 166 | # scripts/my_python_script 167 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 168 | # ) 169 | 170 | ## Mark executables for installation 171 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html 172 | # install(TARGETS ${PROJECT_NAME}_node 173 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 174 | # ) 175 | 176 | ## Mark libraries for installation 177 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html 178 | # install(TARGETS ${PROJECT_NAME} 179 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 180 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 181 | # RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} 182 | # ) 183 | 184 | ## Mark cpp header files for installation 185 | # install(DIRECTORY include/${PROJECT_NAME}/ 186 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 187 | # FILES_MATCHING PATTERN "*.h" 188 | # PATTERN ".svn" EXCLUDE 189 | # ) 190 | 191 | ## Mark other files for installation (e.g. launch and bag files, etc.) 192 | # install(FILES 193 | # # myfile1 194 | # # myfile2 195 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 196 | # ) 197 | 198 | ############# 199 | ## Testing ## 200 | ############# 201 | 202 | ## Add gtest based cpp test target and link libraries 203 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_collision_distance_computation.cpp) 204 | # if(TARGET ${PROJECT_NAME}-test) 205 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 206 | # endif() 207 | 208 | ## Add folders to be run by python nosetests 209 | # catkin_add_nosetests(test) 210 | -------------------------------------------------------------------------------- /Course_2/Example_1/src/map_gen/mockamap/src/ces_randommap.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | 22 | #include 23 | #include 24 | #include 25 | 26 | #include 27 | #include 28 | #include 29 | 30 | #include "visualization_msgs/Marker.h" 31 | #include "visualization_msgs/MarkerArray.h" 32 | 33 | //! @todo historical above 34 | #include "maps.hpp" 35 | 36 | using namespace std; 37 | using namespace mocka; 38 | 39 | #if MAP_OR_WORLD 40 | const string kFrameIdNs_ = "/map"; 41 | #else 42 | const string kFrameIdNs_ = "/world"; 43 | #endif 44 | 45 | pcl::search::KdTree kdtreeLocalMap; 46 | vector pointIdxRadiusSearch; 47 | vector pointRadiusSquaredDistance; 48 | 49 | ros::Publisher _local_map_pub; 50 | ros::Publisher _local_map_inflate_pub; 51 | ros::Publisher _global_map_pub; 52 | 53 | ros::Subscriber _map_sub; 54 | ros::Subscriber _odom_sub; 55 | 56 | deque _odom_queue; 57 | vector _state; 58 | const size_t _odom_queue_size = 200; 59 | nav_msgs::Odometry _odom; 60 | 61 | double z_limit; 62 | double _SenseRate; 63 | double _sensing_range; 64 | 65 | // ros::Timer vis_map; 66 | bool map_ok = false; 67 | bool _has_odom = false; 68 | 69 | sensor_msgs::PointCloud2 globalMap_pcd; 70 | sensor_msgs::PointCloud2 localMap_pcd; 71 | pcl::PointCloud cloudMap; 72 | ros::Time begin_time = ros::TIME_MAX; 73 | 74 | typedef Eigen::Vector3d ObsPos; 75 | typedef Eigen::Vector3d ObsSize; // x, y, height --- z 76 | typedef pair Obstacle; 77 | std::vector obstacle_list; 78 | 79 | void 80 | fixedMapGenerate() 81 | { 82 | double _resolution; 83 | 84 | cloudMap.points.clear(); 85 | obstacle_list.push_back( 86 | make_pair(ObsPos(-7.0, 1.0, 0.0), ObsSize(1.0, 3.0, 5.0))); 87 | obstacle_list.push_back( 88 | make_pair(ObsPos(-1.0, 1.0, 0.0), ObsSize(1.0, 3.0, 5.0))); 89 | obstacle_list.push_back( 90 | make_pair(ObsPos(10.0, 1.0, 0.0), ObsSize(1.0, 3.0, 5.0))); 91 | obstacle_list.push_back( 92 | make_pair(ObsPos(16.0, 1.0, 0.0), ObsSize(1.0, 3.0, 5.0))); 93 | obstacle_list.push_back( 94 | make_pair(ObsPos(-4.0, -1.0, 0.0), ObsSize(1.0, 3.0, 5.0))); 95 | obstacle_list.push_back( 96 | make_pair(ObsPos(13.0, -1.0, 0.0), ObsSize(1.0, 3.0, 5.0))); 97 | 98 | obstacle_list.push_back( 99 | make_pair(ObsPos(5.0, 2.5, 0.0), ObsSize(30.0, 1.0, 5.0))); 100 | obstacle_list.push_back( 101 | make_pair(ObsPos(5.0, -2.5, 0.0), ObsSize(30.0, 1.0, 5.0))); 102 | 103 | int num_total_obs = obstacle_list.size(); 104 | pcl::PointXYZ pt_insert; 105 | 106 | for (int i = 0; i < num_total_obs; i++) 107 | { 108 | double x, y, z; 109 | double lx, ly, lz; 110 | x = (obstacle_list[i].first)[0]; 111 | y = (obstacle_list[i].first)[1]; 112 | z = (obstacle_list[i].first)[2]; 113 | lx = (obstacle_list[i].second)[0]; 114 | ly = (obstacle_list[i].second)[1]; 115 | lz = (obstacle_list[i].second)[2]; 116 | 117 | int num_mesh_x = ceil(lx / _resolution); 118 | int num_mesh_y = ceil(ly / _resolution); 119 | int num_mesh_z = ceil(lz / _resolution); 120 | 121 | int left_x, right_x, left_y, right_y, left_z, right_z; 122 | left_x = -num_mesh_x / 2; 123 | right_x = num_mesh_x / 2; 124 | left_y = -num_mesh_y / 2; 125 | right_y = num_mesh_y / 2; 126 | left_z = 0; 127 | right_z = num_mesh_z; 128 | 129 | for (int r = left_x; r < right_x; r++) 130 | for (int s = left_y; s < right_y; s++) 131 | { 132 | for (int t = left_z; t < right_z; t++) 133 | { 134 | if ((r - left_x) * (r - right_x + 1) * (s - left_y) * 135 | (s - right_y + 1) * (t - left_z) * (t - right_z + 1) == 136 | 0) 137 | { 138 | pt_insert.x = x + r * _resolution; 139 | pt_insert.y = y + s * _resolution; 140 | pt_insert.z = z + t * _resolution; 141 | cloudMap.points.push_back(pt_insert); 142 | } 143 | } 144 | } 145 | } 146 | 147 | cloudMap.width = cloudMap.points.size(); 148 | cloudMap.height = 1; 149 | cloudMap.is_dense = true; 150 | 151 | ROS_WARN("Finished generate random map "); 152 | cout << cloudMap.size() << endl; 153 | kdtreeLocalMap.setInputCloud(cloudMap.makeShared()); 154 | map_ok = true; 155 | } 156 | 157 | void 158 | rcvOdometryCallbck(const nav_msgs::Odometry odom) 159 | { 160 | if (odom.child_frame_id == "X" || odom.child_frame_id == "O") 161 | return; 162 | _odom = odom; 163 | _has_odom = true; 164 | 165 | _state = { _odom.pose.pose.position.x, 166 | _odom.pose.pose.position.y, 167 | _odom.pose.pose.position.z, 168 | _odom.twist.twist.linear.x, 169 | _odom.twist.twist.linear.y, 170 | _odom.twist.twist.linear.z, 171 | 0.0, 172 | 0.0, 173 | 0.0 }; 174 | 175 | _odom_queue.push_back(odom); 176 | while (_odom_queue.size() > _odom_queue_size) 177 | _odom_queue.pop_front(); 178 | } 179 | 180 | int frequence_division_global = 40; 181 | 182 | void 183 | publishAllPoints() 184 | { 185 | if (!map_ok) 186 | return; 187 | 188 | if ((ros::Time::now() - begin_time).toSec() > 7.0) 189 | return; 190 | 191 | frequence_division_global--; 192 | if (frequence_division_global == 0) 193 | { 194 | pcl::toROSMsg(cloudMap, globalMap_pcd); 195 | globalMap_pcd.header.frame_id = kFrameIdNs_; 196 | _global_map_pub.publish(globalMap_pcd); 197 | frequence_division_global = 40; 198 | ROS_ERROR("[SERVER]Publish one global map"); 199 | } 200 | } 201 | 202 | void 203 | pubSensedPoints() 204 | { 205 | if (!map_ok || !_has_odom) 206 | return; 207 | 208 | ros::Time time_bef_sensing = ros::Time::now(); 209 | 210 | pcl::PointCloud localMap; 211 | 212 | pcl::PointXYZ searchPoint(_state[0], _state[1], _state[2]); 213 | pointIdxRadiusSearch.clear(); 214 | pointRadiusSquaredDistance.clear(); 215 | 216 | pcl::PointXYZ ptInNoflation; 217 | 218 | if (kdtreeLocalMap.radiusSearch(searchPoint, _sensing_range, 219 | pointIdxRadiusSearch, 220 | pointRadiusSquaredDistance) > 0) 221 | { 222 | for (size_t i = 0; i < pointIdxRadiusSearch.size(); ++i) 223 | { 224 | ptInNoflation = cloudMap.points[pointIdxRadiusSearch[i]]; 225 | localMap.points.push_back(ptInNoflation); 226 | } 227 | } 228 | else 229 | { 230 | // ROS_ERROR("[Map server] No obstacles ."); 231 | // cout< 5.0) 253 | return; 254 | 255 | frequence_division_global--; 256 | if (frequence_division_global == 0) 257 | { 258 | pcl::toROSMsg(cloudMap, globalMap_pcd); 259 | globalMap_pcd.header.frame_id = kFrameIdNs_; 260 | _global_map_pub.publish(globalMap_pcd); 261 | frequence_division_global = 40; 262 | ROS_INFO("[SERVER]Publish one global map"); 263 | } 264 | } 265 | -------------------------------------------------------------------------------- /Course_2/Example_1/src/gcopter/include/gcopter/voxel_map.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | MIT License 3 | 4 | Copyright (c) 2021 Zhepei Wang (wangzhepei@live.com) 5 | 6 | Permission is hereby granted, free of charge, to any person obtaining a copy 7 | of this software and associated documentation files (the "Software"), to deal 8 | in the Software without restriction, including without limitation the rights 9 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | copies of the Software, and to permit persons to whom the Software is 11 | furnished to do so, subject to the following conditions: 12 | 13 | The above copyright notice and this permission notice shall be included in all 14 | copies or substantial portions of the Software. 15 | 16 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | SOFTWARE. 23 | */ 24 | 25 | #ifndef VOXEL_MAP_HPP 26 | #define VOXEL_MAP_HPP 27 | 28 | #include "voxel_dilater.hpp" 29 | #include 30 | #include 31 | #include 32 | 33 | namespace voxel_map 34 | { 35 | 36 | constexpr uint8_t Unoccupied = 0; 37 | constexpr uint8_t Occupied = 1; 38 | constexpr uint8_t Dilated = 2; 39 | 40 | class VoxelMap 41 | { 42 | 43 | public: 44 | VoxelMap() = default; 45 | VoxelMap(const Eigen::Vector3i &size, 46 | const Eigen::Vector3d &origin, 47 | const double &voxScale) 48 | : mapSize(size), 49 | o(origin), 50 | scale(voxScale), 51 | voxNum(mapSize.prod()), 52 | step(1, mapSize(0), mapSize(1) * mapSize(0)), 53 | oc(o + Eigen::Vector3d::Constant(0.5 * scale)), 54 | bounds((mapSize.array() - 1) * step.array()), 55 | stepScale(step.cast().cwiseInverse() * scale), 56 | voxels(voxNum, Unoccupied) {} 57 | 58 | private: 59 | Eigen::Vector3i mapSize; 60 | Eigen::Vector3d o; 61 | double scale; 62 | int voxNum; 63 | Eigen::Vector3i step; 64 | Eigen::Vector3d oc; 65 | Eigen::Vector3i bounds; 66 | Eigen::Vector3d stepScale; 67 | std::vector voxels; 68 | std::vector surf; 69 | 70 | public: 71 | inline Eigen::Vector3i getSize(void) const 72 | { 73 | return mapSize; 74 | } 75 | 76 | inline double getScale(void) const 77 | { 78 | return scale; 79 | } 80 | 81 | inline Eigen::Vector3d getOrigin(void) const 82 | { 83 | return o; 84 | } 85 | 86 | inline Eigen::Vector3d getCorner(void) const 87 | { 88 | return mapSize.cast() * scale + o; 89 | } 90 | 91 | inline const std::vector &getVoxels(void) const 92 | { 93 | return voxels; 94 | } 95 | 96 | inline void setOccupied(const Eigen::Vector3d &pos) 97 | { 98 | const Eigen::Vector3i id = ((pos - o) / scale).cast(); 99 | if (id(0) >= 0 && id(1) >= 0 && id(2) >= 0 && 100 | id(0) < mapSize(0) && id(1) < mapSize(1) && id(2) < mapSize(2)) 101 | { 102 | voxels[id.dot(step)] = Occupied; 103 | } 104 | } 105 | 106 | inline void setOccupied(const Eigen::Vector3i &id) 107 | { 108 | if (id(0) >= 0 && id(1) >= 0 && id(2) >= 0 && 109 | id(0) < mapSize(0) && id(1) < mapSize(1) && id(2) < mapSize(2)) 110 | { 111 | voxels[id.dot(step)] = Occupied; 112 | } 113 | } 114 | 115 | inline void dilate(const int &r) 116 | { 117 | if (r <= 0) 118 | { 119 | return; 120 | } 121 | else 122 | { 123 | std::vector lvec, cvec; 124 | lvec.reserve(voxNum); 125 | cvec.reserve(voxNum); 126 | int i, j, k, idx; 127 | bool check; 128 | for (int x = 0; x <= bounds(0); x++) 129 | { 130 | for (int y = 0; y <= bounds(1); y += step(1)) 131 | { 132 | for (int z = 0; z <= bounds(2); z += step(2)) 133 | { 134 | if (voxels[x + y + z] == Occupied) 135 | { 136 | VOXEL_DILATER(i, j, k, 137 | x, y, z, 138 | step(1), step(2), 139 | bounds(0), bounds(1), bounds(2), 140 | check, voxels, idx, Dilated, cvec) 141 | } 142 | } 143 | } 144 | } 145 | 146 | for (int loop = 1; loop < r; loop++) 147 | { 148 | std::swap(cvec, lvec); 149 | for (const Eigen::Vector3i &id : lvec) 150 | { 151 | VOXEL_DILATER(i, j, k, 152 | id(0), id(1), id(2), 153 | step(1), step(2), 154 | bounds(0), bounds(1), bounds(2), 155 | check, voxels, idx, Dilated, cvec) 156 | } 157 | lvec.clear(); 158 | } 159 | 160 | surf = cvec; 161 | } 162 | } 163 | 164 | inline void getSurfInBox(const Eigen::Vector3i ¢er, 165 | const int &halfWidth, 166 | std::vector &points) const 167 | { 168 | for (const Eigen::Vector3i &id : surf) 169 | { 170 | if (std::abs(id(0) - center(0)) <= halfWidth && 171 | std::abs(id(1) / step(1) - center(1)) <= halfWidth && 172 | std::abs(id(2) / step(2) - center(2)) <= halfWidth) 173 | { 174 | points.push_back(id.cast().cwiseProduct(stepScale) + oc); 175 | } 176 | } 177 | 178 | return; 179 | } 180 | 181 | inline void getSurf(std::vector &points) const 182 | { 183 | points.reserve(surf.size()); 184 | for (const Eigen::Vector3i &id : surf) 185 | { 186 | points.push_back(id.cast().cwiseProduct(stepScale) + oc); 187 | } 188 | return; 189 | } 190 | 191 | inline bool query(const Eigen::Vector3d &pos) const 192 | { 193 | const Eigen::Vector3i id = ((pos - o) / scale).cast(); 194 | if (id(0) >= 0 && id(1) >= 0 && id(2) >= 0 && 195 | id(0) < mapSize(0) && id(1) < mapSize(1) && id(2) < mapSize(2)) 196 | { 197 | return voxels[id.dot(step)]; 198 | } 199 | else 200 | { 201 | return true; 202 | } 203 | } 204 | 205 | inline bool query(const Eigen::Vector3i &id) const 206 | { 207 | if (id(0) >= 0 && id(1) >= 0 && id(2) >= 0 && 208 | id(0) < mapSize(0) && id(1) < mapSize(1) && id(2) < mapSize(2)) 209 | { 210 | return voxels[id.dot(step)]; 211 | } 212 | else 213 | { 214 | return true; 215 | } 216 | } 217 | 218 | inline Eigen::Vector3d posI2D(const Eigen::Vector3i &id) const 219 | { 220 | return id.cast() * scale + oc; 221 | } 222 | 223 | inline Eigen::Vector3i posD2I(const Eigen::Vector3d &pos) const 224 | { 225 | return ((pos - o) / scale).cast(); 226 | } 227 | }; 228 | } 229 | 230 | #endif 231 | -------------------------------------------------------------------------------- /Course_2/Example_1/src/gcopter/include/gcopter/sfc_gen.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | MIT License 3 | 4 | Copyright (c) 2021 Zhepei Wang (wangzhepei@live.com) 5 | 6 | Permission is hereby granted, free of charge, to any person obtaining a copy 7 | of this software and associated documentation files (the "Software"), to deal 8 | in the Software without restriction, including without limitation the rights 9 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | copies of the Software, and to permit persons to whom the Software is 11 | furnished to do so, subject to the following conditions: 12 | 13 | The above copyright notice and this permission notice shall be included in all 14 | copies or substantial portions of the Software. 15 | 16 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | SOFTWARE. 23 | */ 24 | 25 | #ifndef SFC_GEN_HPP 26 | #define SFC_GEN_HPP 27 | 28 | #include "geo_utils.hpp" 29 | #include "firi.hpp" 30 | 31 | #include 32 | #include 33 | #include 34 | #include 35 | #include 36 | #include 37 | 38 | #include 39 | #include 40 | #include 41 | 42 | namespace sfc_gen 43 | { 44 | 45 | template 46 | inline double planPath(const Eigen::Vector3d &s, 47 | const Eigen::Vector3d &g, 48 | const Eigen::Vector3d &lb, 49 | const Eigen::Vector3d &hb, 50 | const Map *mapPtr, 51 | const double &timeout, 52 | std::vector &p) 53 | { 54 | auto space(std::make_shared(3)); 55 | 56 | ompl::base::RealVectorBounds bounds(3); 57 | bounds.setLow(0, 0.0); 58 | bounds.setHigh(0, hb(0) - lb(0)); 59 | bounds.setLow(1, 0.0); 60 | bounds.setHigh(1, hb(1) - lb(1)); 61 | bounds.setLow(2, 0.0); 62 | bounds.setHigh(2, hb(2) - lb(2)); 63 | space->setBounds(bounds); 64 | 65 | auto si(std::make_shared(space)); 66 | 67 | si->setStateValidityChecker( 68 | [&](const ompl::base::State *state) 69 | { 70 | const auto *pos = state->as(); 71 | const Eigen::Vector3d position(lb(0) + (*pos)[0], 72 | lb(1) + (*pos)[1], 73 | lb(2) + (*pos)[2]); 74 | return mapPtr->query(position) == 0; 75 | }); 76 | si->setup(); 77 | 78 | ompl::msg::setLogLevel(ompl::msg::LOG_NONE); 79 | 80 | ompl::base::ScopedState<> start(space), goal(space); 81 | start[0] = s(0) - lb(0); 82 | start[1] = s(1) - lb(1); 83 | start[2] = s(2) - lb(2); 84 | goal[0] = g(0) - lb(0); 85 | goal[1] = g(1) - lb(1); 86 | goal[2] = g(2) - lb(2); 87 | 88 | auto pdef(std::make_shared(si)); 89 | pdef->setStartAndGoalStates(start, goal); 90 | pdef->setOptimizationObjective(std::make_shared(si)); 91 | auto planner(std::make_shared(si)); 92 | planner->setProblemDefinition(pdef); 93 | planner->setup(); 94 | 95 | ompl::base::PlannerStatus solved; 96 | solved = planner->ompl::base::Planner::solve(timeout); 97 | 98 | double cost = INFINITY; 99 | if (solved) 100 | { 101 | p.clear(); 102 | const ompl::geometric::PathGeometric path_ = 103 | ompl::geometric::PathGeometric( 104 | dynamic_cast(*pdef->getSolutionPath())); 105 | for (size_t i = 0; i < path_.getStateCount(); i++) 106 | { 107 | const auto state = path_.getState(i)->as()->values; 108 | p.emplace_back(lb(0) + state[0], lb(1) + state[1], lb(2) + state[2]); 109 | } 110 | cost = pdef->getSolutionPath()->cost(pdef->getOptimizationObjective()).value(); 111 | } 112 | 113 | return cost; 114 | } 115 | 116 | inline void convexCover(const std::vector &path, 117 | const std::vector &points, 118 | const Eigen::Vector3d &lowCorner, 119 | const Eigen::Vector3d &highCorner, 120 | const double &progress, 121 | const double &range, 122 | std::vector &hpolys, 123 | const double eps = 1.0e-6) 124 | { 125 | hpolys.clear(); 126 | const int n = path.size(); 127 | Eigen::Matrix bd = Eigen::Matrix::Zero(); 128 | bd(0, 0) = 1.0; 129 | bd(1, 0) = -1.0; 130 | bd(2, 1) = 1.0; 131 | bd(3, 1) = -1.0; 132 | bd(4, 2) = 1.0; 133 | bd(5, 2) = -1.0; 134 | 135 | Eigen::MatrixX4d hp, gap; 136 | Eigen::Vector3d a, b = path[0]; 137 | std::vector valid_pc; 138 | std::vector bs; 139 | valid_pc.reserve(points.size()); 140 | for (int i = 1; i < n;) 141 | { 142 | a = b; 143 | if ((a - path[i]).norm() > progress) 144 | { 145 | b = (path[i] - a).normalized() * progress + a; 146 | } 147 | else 148 | { 149 | b = path[i]; 150 | i++; 151 | } 152 | bs.emplace_back(b); 153 | 154 | bd(0, 3) = -std::min(std::max(a(0), b(0)) + range, highCorner(0)); 155 | bd(1, 3) = +std::max(std::min(a(0), b(0)) - range, lowCorner(0)); 156 | bd(2, 3) = -std::min(std::max(a(1), b(1)) + range, highCorner(1)); 157 | bd(3, 3) = +std::max(std::min(a(1), b(1)) - range, lowCorner(1)); 158 | bd(4, 3) = -std::min(std::max(a(2), b(2)) + range, highCorner(2)); 159 | bd(5, 3) = +std::max(std::min(a(2), b(2)) - range, lowCorner(2)); 160 | 161 | valid_pc.clear(); 162 | for (const Eigen::Vector3d &p : points) 163 | { 164 | if ((bd.leftCols<3>() * p + bd.rightCols<1>()).maxCoeff() < 0.0) 165 | { 166 | valid_pc.emplace_back(p); 167 | } 168 | } 169 | Eigen::Map> pc(valid_pc[0].data(), 3, valid_pc.size()); 170 | 171 | firi::firi(bd, pc, a, b, hp); 172 | 173 | if (hpolys.size() != 0) 174 | { 175 | const Eigen::Vector4d ah(a(0), a(1), a(2), 1.0); 176 | if (3 <= ((hp * ah).array() > -eps).cast().sum() + 177 | ((hpolys.back() * ah).array() > -eps).cast().sum()) 178 | { 179 | firi::firi(bd, pc, a, a, gap, 1); 180 | hpolys.emplace_back(gap); 181 | } 182 | } 183 | 184 | hpolys.emplace_back(hp); 185 | } 186 | } 187 | 188 | inline void shortCut(std::vector &hpolys) 189 | { 190 | std::vector htemp = hpolys; 191 | if (htemp.size() == 1) 192 | { 193 | Eigen::MatrixX4d headPoly = htemp.front(); 194 | htemp.insert(htemp.begin(), headPoly); 195 | } 196 | hpolys.clear(); 197 | 198 | int M = htemp.size(); 199 | Eigen::MatrixX4d hPoly; 200 | bool overlap; 201 | std::deque idices; 202 | idices.push_front(M - 1); 203 | for (int i = M - 1; i >= 0; i--) 204 | { 205 | for (int j = 0; j < i; j++) 206 | { 207 | if (j < i - 1) 208 | { 209 | overlap = geo_utils::overlap(htemp[i], htemp[j], 0.01); 210 | } 211 | else 212 | { 213 | overlap = true; 214 | } 215 | if (overlap) 216 | { 217 | idices.push_front(j); 218 | i = j + 1; 219 | break; 220 | } 221 | } 222 | } 223 | for (const auto &ele : idices) 224 | { 225 | hpolys.push_back(htemp[ele]); 226 | } 227 | } 228 | 229 | } 230 | 231 | #endif 232 | --------------------------------------------------------------------------------