├── .gitignore ├── .gitmodules ├── BARN_dataset └── npy_to_txt.py ├── CMakeLists.txt ├── README.md ├── corridor ├── corridor.h └── corridor_param.h ├── doc ├── 3Dplot │ ├── log-mppi.png │ ├── mppi-ipddp.png │ ├── mppi.png │ └── smooth-mppi.png ├── Barn │ ├── distance.png │ ├── extend.png │ ├── inflate.png │ └── original.png └── Boxplot │ ├── curvature.png │ └── seconds.png ├── graph ├── box.py ├── graph.py ├── log_mppi.txt ├── mppi.txt ├── mppi_ipddp.txt ├── other_mppi.xlsx └── smooth_mppi.txt ├── model ├── model_base.h ├── wmrobot.h └── wmrobot_map.h ├── mppi_ipddp ├── collision_checker.h ├── mppi_ipddp.h ├── parameter.h └── show.h └── src ├── example.cpp ├── log_mppi.cpp ├── main.cpp ├── mppi.cpp ├── mppi_ipddp.cpp ├── receding.cpp ├── smooth_mppi.cpp └── test_with_map.cpp /.gitignore: -------------------------------------------------------------------------------- 1 | .vscode/ 2 | build/ 3 | eigen/ 4 | matplotlibcpp/ 5 | autodiff/ 6 | EigenRand/ 7 | BARN_dataset/txt_files 8 | -------------------------------------------------------------------------------- /.gitmodules: -------------------------------------------------------------------------------- 1 | [submodule "ipddp"] 2 | path = ipddp 3 | branch = dev_mppi_ipddp 4 | url = git@github.com:MC00614/ipddp_cpp.git 5 | [submodule "mppi"] 6 | path = mppi 7 | branch = main 8 | url = git@github.com:MC00614/mppi_cpp.git 9 | -------------------------------------------------------------------------------- /BARN_dataset/npy_to_txt.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import matplotlib.pyplot as plt 3 | 4 | # for i in range(300): 5 | for i in range(299,-1,-1): 6 | npy_file = f'grid_files/grid_{i}.npy' 7 | txt_file = f'inflated_txt_files/output_{i}.txt' 8 | 9 | raw_data = np.load(npy_file) 10 | raw_data = np.array(raw_data, dtype=np.float64) 11 | bracket = [1] + [0 for _ in range(28)] + [1] 12 | bracket = np.array(bracket, dtype=np.float64) 13 | for _ in range(10): 14 | raw_data = np.insert(raw_data, 0, bracket, axis = 1) 15 | for _ in range(10): 16 | raw_data = np.insert(raw_data, raw_data.shape[1], bracket, axis = 1) 17 | 18 | # plt.imshow(raw_data, cmap = 'Greys') 19 | 20 | rows = len(raw_data) 21 | cols = len(raw_data[0]) 22 | 23 | BLOCK = 10 24 | raw_data[raw_data==1] = BLOCK 25 | 26 | data = raw_data 27 | for row in range(rows): 28 | for col in range(cols): 29 | if raw_data[row][col] == BLOCK: 30 | for nr in range(max(0,row-1), min(row+1,rows)): 31 | for nc in range(max(0,col-1), min(col+1,cols)): 32 | data[nr][nc] = BLOCK 33 | 34 | rmax = 5 35 | inflated_data = np.full_like(data, rmax) 36 | for row in range(rows): 37 | for col in range(cols): 38 | if data[row][col] == BLOCK: 39 | for nr in range(max(0,row-rmax), min(row+rmax,rows)): 40 | for nc in range(max(0,col-rmax), min(col+rmax,cols)): 41 | dr = abs(nr - row) 42 | dc = abs(nc - col) 43 | in_rectangle = 0 44 | if dr==0 and dc==0: 45 | pass 46 | elif dr!=0 and dc!=0: 47 | mul = 0.5/max(dr, dc) 48 | in_rectangle = ((dr)**2 + (dc)**2)**0.5*mul 49 | else: 50 | in_rectangle = 0.5 51 | distance = ((nr - row)**2 + (nc - col)**2)**0.5 - in_rectangle 52 | if distance > rmax: 53 | continue 54 | if distance == 0: 55 | continue 56 | # print(distance) 57 | if inflated_data[nr][nc] < distance: 58 | # print(inflated_data[nr][nc], distance) 59 | continue 60 | inflated_data[nr][nc] = distance 61 | 62 | inflated_data[data == BLOCK] = BLOCK 63 | new_data = np.full_like(data, rmax) 64 | 65 | for row in range(rows): 66 | for col in range(cols): 67 | if inflated_data[row][col] != BLOCK: 68 | min_distance = 10 69 | for nr in range(max(0,row-3), min(row+3,rows)): 70 | for nc in range(max(0,col-3), min(col+3,cols)): 71 | min_distance = min(min_distance, inflated_data[nr][nc]) 72 | new_data[row][col] = min_distance 73 | new_data[data == BLOCK] = BLOCK 74 | 75 | # print(inflated_data) 76 | # plt.figure() 77 | # plt.imshow(inflated_data, cmap = 'Greys') 78 | 79 | # plt.figure(figsize=(25,25)) 80 | # plt.imshow(new_data, cmap = 'Greys') 81 | # plt.show() 82 | 83 | np.savetxt(txt_file, inflated_data, fmt='%.5e') 84 | 85 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(mppi_ipddp) 3 | find_package (Eigen3 REQUIRED NO_MODULE) 4 | find_package(OpenMP REQUIRED) 5 | 6 | # autodiff requires a c++17 enabled compiler 7 | set(CMAKE_CXX_STANDARD 17) # ensure cmake instructs compiler to use C++17 8 | set(CMAKE_CXX_STANDARD_REQUIRED ON) # ensure the C++ standard given before is actually used 9 | set(CMAKE_CXX_EXTENSIONS OFF) # avoid compile flags of the type -std=gnu++1z added by cmake 10 | 11 | include_directories( 12 | EigenRand/ 13 | autodiff/ 14 | ${EIGEN3_INCLUDE_DIRS} 15 | 16 | mppi/mppi/ 17 | corridor/ 18 | ipddp/ipddp/ 19 | mppi_ipddp/ 20 | model/ 21 | ) 22 | 23 | set(COMMON_SOURCES 24 | src/test_with_map.cpp 25 | src/log_mppi.cpp 26 | src/mppi_ipddp.cpp 27 | src/smooth_mppi.cpp 28 | src/mppi.cpp 29 | src/receding.cpp 30 | src/example.cpp 31 | ) 32 | 33 | foreach(SRC ${COMMON_SOURCES}) 34 | get_filename_component(EXE_NAME ${SRC} NAME_WE) 35 | add_executable(${EXE_NAME} ${SRC}) 36 | target_compile_options(${EXE_NAME} PRIVATE -O3) 37 | target_link_libraries(${EXE_NAME} OpenMP::OpenMP_CXX) 38 | endforeach() -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # MPPI-IPDDP C++ 2 | 3 | Implementation of the [MPPI-IPDDP](https://arxiv.org/abs/2208.02439) in C++. 4 | 5 | # Key Features 6 | - Header only library solver 7 | - CPU multi-processing 8 | - Benchmark with MPPI variants and multiple environments 9 | - Collision Checker can be easily modified on demand 10 | - Easy custom model configuration 11 | 12 | # Usage 13 | ## Download Dependency 14 | ### Build Dependancy 15 | **EigenRand** and **autodiff** need to be named as in CMakeLists.txt. 16 | - [Eigen 3.3.9](https://gitlab.com/libeigen/eigen/-/releases/3.3.9) 17 | - [EigenRand](https://github.com/bab2min/EigenRand) 18 | - [autodiff](https://github.com/autodiff/autodiff) 19 | 20 | ### Runtime Only Dependancy 21 | For testing in diffent environment, we use `.npy` maps from `BARN Dataset`. 22 | - [BARN_dataset](https://www.cs.utexas.edu/~xiao/BARN/BARN.html) 23 | 24 | ## Build with CMake 25 | Note that **EigenRand** and **autodiff** need to be under **mppi_ipddp_cpp**. 26 | ```bash 27 | git clone https://github.com/MC00614/mppi_ipddp_cpp.git 28 | cd mppi_ipddp_cpp 29 | mkdir build && cd build 30 | cmake.. && make 31 | ``` 32 | For simple usage, refer [example.cpp](src/example.cpp) with Wheeled Mobile Robot in 2D environment. 33 | 34 | 35 | # Performance comparison with MPPI variants 36 | ## Box Plot 37 | | **Seconds** | **Curvature** | 38 | |:----------:|:----------:| 39 | | ![seconds](doc/Boxplot/seconds.png) | ![curvature](doc/Boxplot/curvature.png) | ! 40 | 41 | ## Results for MPPI 42 | | **MPPI** | **Log-MPPI** | 43 | |:----------:|:----------:| 44 | | ![MPPI](doc/3Dplot/mppi.png) | ![Log-MPPI](doc/3Dplot/log-mppi.png) | ! 45 | | **Smooth-MPPI** | **MPPI-IPDDP** | 46 | | ![Smooth-MPPI](doc/3Dplot/smooth-mppi.png) | ![MPPI-IPDDP](doc/3Dplot/mppi-ipddp.png) | ! 47 | 48 | 49 | To reproduce this, refer [main.cpp](src/main.cpp) with **Specific Target** and **Number of Simulations** 50 | ```cpp 51 | // Target: MPPI-IPDDP, Simulations: 10 52 | ./main MPPI-IPDDP 10 53 | ``` 54 | Graphical tool for plot can be found in [graph](graph/) (`box.py`, `graph.py`), which is written in Python. 55 | 56 | # Performance in Multiple Environments (BARN Dataset) 57 | ## Map Conversion 58 | | **(a) Original** | **(b) Extend** | 59 | |:----------:|:----------:| 60 | | ![original](doc/Barn/original.png) | ![extend](doc/Barn/extend.png) | ! 61 | | **(c) Inflate** | **(d) Distance** | 62 | | ![inflate](doc/Barn/inflate.png) | ![distance](doc/Barn/distance.png) | ! 63 | 64 | ## Results 65 | | Metric | MPPI | Log MPPI | Smooth MPPI | MPPI IPDDP | 66 | |:-----------:|:------:|:--------:|:-----------:|:----------:| 67 | | **$N_u$** | 3200 | 3200 | 12800 | 1600 | 68 | | **$𝜮𝒖$** | 0.2 | 0.1 | 0.3 | 0.4 | 69 | | **Success** | 97 | 97 | 91.3 | 95.7 | 70 | | **Q1 Time** | 0.121522| 0.169059 | 0.446100 | 0.274369 | 71 | | **Q2 Time** | 0.139242| 0.197344 | 0.511798 | 0.299426 | 72 | | **Q3 Time** | 0.179549| 0.232929 | 0.598198 | 0.346971 | 73 | | **MSC** | 0.002528| 0.003089 | 0.005856 | 0.000139 | 74 | 75 | To test with 300 maps, use [test_with_map.cpp](src/test_with_map.cpp) with **Specific Target** 76 | ```cpp 77 | // Target: MPPI-IPDDP 78 | ./test_with_map MPPI-IPDDP 79 | ``` 80 | 81 | # Future Works 82 | - GPU Acceleration 83 | - More general constraint handling in model 84 | - Custom implementation of diffential with dual number -------------------------------------------------------------------------------- /corridor/corridor.h: -------------------------------------------------------------------------------- 1 | #include "corridor_param.h" 2 | #include "model_base.h" 3 | #include "collision_checker.h" 4 | 5 | #include 6 | 7 | #include 8 | #include 9 | 10 | #include 11 | 12 | class Corridor { 13 | public: 14 | Corridor(ModelBase model); 15 | ~Corridor(); 16 | void init(CorridorParam corridor_param); 17 | void setCollisionChecker(CollisionChecker *collision_checker); 18 | void solve(const Eigen::MatrixXd &X, Eigen::MatrixXd &C, Eigen::VectorXd &R); 19 | void hz(Eigen::MatrixXd &Z); 20 | 21 | Eigen::MatrixXd getResC(); 22 | Eigen::VectorXd getResR(); 23 | 24 | private: 25 | int N; 26 | int max_iter; 27 | int Nz; 28 | double gamma_z; 29 | Eigen::MatrixXd sigma_z; 30 | double lambda_c; 31 | double lambda_r; 32 | double r_max; 33 | CollisionChecker *collision_checker; 34 | int center_point; 35 | 36 | std::mt19937_64 urng{static_cast(std::time(nullptr))}; 37 | // std::mt19937_64 urng{1}; 38 | Eigen::Rand::NormalGen norm_gen{0.0, 1.0}; 39 | 40 | Eigen::MatrixXd Z; 41 | }; 42 | 43 | Corridor::Corridor(ModelBase model) { 44 | this->N = model.N; 45 | this->center_point = model.center_point; 46 | } 47 | 48 | Corridor::~Corridor() { 49 | } 50 | 51 | void Corridor::init(CorridorParam corridor_param) { 52 | this->max_iter = corridor_param.max_iter; 53 | this->Nz = corridor_param.Nz; 54 | this->gamma_z = corridor_param.gamma_z; 55 | this->sigma_z = corridor_param.sigma_z; 56 | this->lambda_c = corridor_param.lambda_c; 57 | this->lambda_r = corridor_param.lambda_r; 58 | this->r_max = corridor_param.r_max; 59 | Z.resize(center_point + 1, N); 60 | } 61 | 62 | void Corridor::setCollisionChecker(CollisionChecker *collision_checker) { 63 | this->collision_checker = collision_checker; 64 | } 65 | 66 | void Corridor::solve(const Eigen::MatrixXd &X, Eigen::MatrixXd &C, Eigen::VectorXd &R) { 67 | Z.topRows(center_point) = X.topRows(center_point).leftCols(N); 68 | Z.bottomRows(1) = Eigen::MatrixXd::Zero(1, N); 69 | 70 | for (int iter = 0; iter < max_iter; ++iter) { 71 | #pragma omp parallel for 72 | for (int t = 0; t < N; ++t) { 73 | Eigen::MatrixXd Zi(center_point + 1, Nz); 74 | Zi = Z.col(t).replicate(1, Nz) + (this->sigma_z * norm_gen.template generate(center_point + 1, Nz, urng)); 75 | hz(Zi); 76 | Eigen::VectorXd costs(Nz); 77 | costs = lambda_c * (Zi.topRows(center_point).colwise() - X.col(t).topRows(center_point)).colwise().norm(); 78 | costs -= lambda_r * Zi.bottomRows(1).transpose(); 79 | for (int i = 0; i < Nz; ++i) { 80 | if (collision_checker->getCollisionCircle(Zi.col(i))) { 81 | costs(i) = 1e8; 82 | } 83 | } 84 | double min_cost = costs.minCoeff(); 85 | 86 | Eigen::VectorXd weights; 87 | weights = (-gamma_z * (costs.array() - min_cost)).exp(); 88 | double total_weight = weights.sum(); 89 | weights /= total_weight; 90 | Z.col(t) = Zi * weights; 91 | } 92 | hz(Z); 93 | } 94 | C = Z.topRows(center_point); 95 | R = Z.bottomRows(1).transpose(); 96 | } 97 | 98 | void Corridor::hz(Eigen::MatrixXd &Z) { 99 | Z.row(center_point) = Z.row(center_point).cwiseAbs().cwiseMin(r_max); 100 | }; 101 | 102 | Eigen::MatrixXd Corridor::getResC() { 103 | return this->Z.topRows(center_point); 104 | }; 105 | 106 | Eigen::VectorXd Corridor::getResR() { 107 | return this->Z.bottomRows(1); 108 | }; -------------------------------------------------------------------------------- /corridor/corridor_param.h: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | struct CorridorParam { 4 | int max_iter; 5 | int Nz; 6 | double gamma_z; 7 | Eigen::MatrixXd sigma_z; 8 | double lambda_c; 9 | double lambda_r; 10 | double r_max; 11 | }; -------------------------------------------------------------------------------- /doc/3Dplot/log-mppi.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MC00614/mppi_ipddp_cpp/009aecadaa7e46aec6a25f1fdfc73d56f480a0a6/doc/3Dplot/log-mppi.png -------------------------------------------------------------------------------- /doc/3Dplot/mppi-ipddp.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MC00614/mppi_ipddp_cpp/009aecadaa7e46aec6a25f1fdfc73d56f480a0a6/doc/3Dplot/mppi-ipddp.png -------------------------------------------------------------------------------- /doc/3Dplot/mppi.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MC00614/mppi_ipddp_cpp/009aecadaa7e46aec6a25f1fdfc73d56f480a0a6/doc/3Dplot/mppi.png -------------------------------------------------------------------------------- /doc/3Dplot/smooth-mppi.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MC00614/mppi_ipddp_cpp/009aecadaa7e46aec6a25f1fdfc73d56f480a0a6/doc/3Dplot/smooth-mppi.png -------------------------------------------------------------------------------- /doc/Barn/distance.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MC00614/mppi_ipddp_cpp/009aecadaa7e46aec6a25f1fdfc73d56f480a0a6/doc/Barn/distance.png -------------------------------------------------------------------------------- /doc/Barn/extend.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MC00614/mppi_ipddp_cpp/009aecadaa7e46aec6a25f1fdfc73d56f480a0a6/doc/Barn/extend.png -------------------------------------------------------------------------------- /doc/Barn/inflate.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MC00614/mppi_ipddp_cpp/009aecadaa7e46aec6a25f1fdfc73d56f480a0a6/doc/Barn/inflate.png -------------------------------------------------------------------------------- /doc/Barn/original.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MC00614/mppi_ipddp_cpp/009aecadaa7e46aec6a25f1fdfc73d56f480a0a6/doc/Barn/original.png -------------------------------------------------------------------------------- /doc/Boxplot/curvature.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MC00614/mppi_ipddp_cpp/009aecadaa7e46aec6a25f1fdfc73d56f480a0a6/doc/Boxplot/curvature.png -------------------------------------------------------------------------------- /doc/Boxplot/seconds.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MC00614/mppi_ipddp_cpp/009aecadaa7e46aec6a25f1fdfc73d56f480a0a6/doc/Boxplot/seconds.png -------------------------------------------------------------------------------- /graph/box.py: -------------------------------------------------------------------------------- 1 | import pandas as pd 2 | import numpy as np 3 | import matplotlib.pyplot as plt 4 | from matplotlib.ticker import ScalarFormatter 5 | 6 | handling_datas = ['MPPI', 'Log_MPPI', 'Smooth_MPPI', 'MPPI_IPDDP'] 7 | 8 | # fig, axs = plt.subplots(2, 2, figsize=(20, 18), sharey=True) 9 | 10 | Z1 = {} 11 | Z2 = {} 12 | Z3 = {} 13 | 14 | for i, handling_data in enumerate(handling_datas): 15 | file_path = 'other_mppi.xlsx' 16 | df = pd.read_excel(file_path, sheet_name=handling_data) 17 | avg_time = df['avg_time'].values 18 | a_msc_x = df['a_msc_x'].values 19 | a_msc_u = df['a_msc_u'].values 20 | 21 | avg_time = avg_time[avg_time != 10] 22 | a_msc_x = a_msc_x[a_msc_x != 0] 23 | a_msc_u = a_msc_u[a_msc_u != 0] 24 | 25 | Z1[handling_data] = avg_time 26 | Z2[handling_data] = a_msc_x 27 | Z3[handling_data] = a_msc_u 28 | 29 | offset = 0.22 30 | print('Average Time') 31 | fig, ax = plt.subplots(figsize=(15, 10)) 32 | ax.set_title('Average Time', fontsize=35, fontweight='bold', pad=25) 33 | ax.set_ylabel('seconds', fontsize=25, labelpad=15) 34 | ax.set_xticklabels(handling_datas, fontsize=25) 35 | ax.set_ylim(-0.1, 10.1) 36 | ax.tick_params(axis='y', labelsize=20) 37 | for i, handling_data in enumerate(handling_datas): 38 | box1 = ax.boxplot(Z1[handling_data], positions=[i], widths=0.4, patch_artist=True) 39 | for patch in box1['boxes']: 40 | patch.set_facecolor('lightblue') 41 | stats1 = box1['whiskers'] 42 | median = box1['medians'][0].get_ydata()[0] 43 | q1 = stats1[0].get_ydata()[1] 44 | q3 = stats1[1].get_ydata()[1] 45 | print(f'{handling_data}\t{q1:.6f}\t{median:.6f}\t{q3:.6f}') 46 | 47 | ax.text(i+offset, q1, f'Q1: {q1:.3f}', fontsize=15, ha='left') 48 | ax.text(i+offset, median, f'Median: {median:.3f}', fontsize=15, ha='left') 49 | ax.text(i+offset, q3, f'Q3: {q3:.3f}', fontsize=15, ha='left') 50 | 51 | offset = -0.29 52 | print('Mean Squared Curvature X') 53 | fig, ax = plt.subplots(figsize=(15, 10)) 54 | ax.set_title('Mean Squared Curvature X', fontsize=35, fontweight='bold', pad=25) 55 | ax.set_ylabel('curvature', fontsize=25, labelpad=15) 56 | ax.set_xticklabels(handling_datas, fontsize=25) 57 | ax.set_ylim(-0.001, 0.015) 58 | ax.yaxis.set_major_formatter(ScalarFormatter(useMathText=True)) 59 | ax.ticklabel_format(style='sci', axis='y', scilimits=(0,0)) 60 | ax.tick_params(axis='y', labelsize=20) 61 | ax.yaxis.get_offset_text().set_fontsize(20) 62 | for i, handling_data in enumerate(handling_datas[:3]): 63 | box1 = ax.boxplot(Z2[handling_data], positions=[i], widths=0.5, patch_artist=True) 64 | for patch in box1['boxes']: 65 | patch.set_facecolor('lightblue') 66 | stats1 = box1['whiskers'] 67 | median = box1['medians'][0].get_ydata()[0] 68 | q1 = stats1[0].get_ydata()[1] 69 | q3 = stats1[1].get_ydata()[1] 70 | print(f'{handling_data}\t{q1:.6f}\t{median:.6f}\t{q3:.6f}') 71 | 72 | ax.text(i+offset, q1, f'Q1: {q1:.6f}', fontsize=15, ha='right') 73 | ax.text(i+offset, median, f'Median: {median:.6f}', fontsize=15, ha='right') 74 | ax.text(i+offset, q3, f'Q3: {q3:.6f}', fontsize=15, ha='right') 75 | ax3 = ax.twinx() 76 | ax3.spines['right'].set_position(('outward', -450)) 77 | ax3.set_ylim(-0.00001, 0.00033) 78 | ax3.yaxis.set_major_formatter(ScalarFormatter(useMathText=True)) 79 | ax3.ticklabel_format(style='sci', axis='y', scilimits=(0,0)) 80 | ax3.tick_params(axis='y', labelsize=20) 81 | ax3.yaxis.get_offset_text().set_fontsize(20) 82 | ax3.yaxis.get_offset_text().set_position((0.76, 0)) 83 | box3 = ax3.boxplot(Z2['MPPI_IPDDP'], positions=[3.5], widths=0.5, patch_artist=True) 84 | for patch in box3['boxes']: 85 | patch.set_facecolor('lightblue') 86 | stats1 = box3['whiskers'] 87 | median = box3['medians'][0].get_ydata()[0] 88 | q1 = stats1[0].get_ydata()[1] 89 | q3 = stats1[1].get_ydata()[1] 90 | offset = 0.2 91 | ax3.text(3+offset, q1, f'Q1: {q1:.6f}', fontsize=15, ha='right') 92 | ax3.text(3+offset, median, f'Median: {median:.6f}', fontsize=15, ha='right') 93 | ax3.text(3+offset, q3, f'Q3: {q3:.6f}', fontsize=15, ha='right') 94 | print(f'{"MPPI_IPDDP"}\t{q1:.6f}\t{median:.6f}\t{q3:.6f}') 95 | 96 | 97 | offset = -0.29 98 | print('Mean Squared Curvature U') 99 | fig, ax = plt.subplots(figsize=(15, 10)) 100 | ax.set_title('Mean Squared Curvature U', fontsize=35, fontweight='bold', pad=25) 101 | ax.set_ylabel('curvature', fontsize=25, labelpad=15) 102 | ax.set_xticklabels(handling_datas, fontsize=25) 103 | ax.set_ylim(-0.0, 6.5) 104 | ax.yaxis.set_major_formatter(ScalarFormatter(useMathText=True)) 105 | ax.ticklabel_format(style='sci', axis='y', scilimits=(0,0)) 106 | ax.tick_params(axis='y', labelsize=20) 107 | ax.yaxis.get_offset_text().set_fontsize(20) 108 | for i, handling_data in enumerate(handling_datas[:3]): 109 | box1 = ax.boxplot(Z3[handling_data], positions=[i], widths=0.5, patch_artist=True) 110 | for patch in box1['boxes']: 111 | patch.set_facecolor('lightblue') 112 | stats1 = box1['whiskers'] 113 | median = box1['medians'][0].get_ydata()[0] 114 | q1 = stats1[0].get_ydata()[1] 115 | q3 = stats1[1].get_ydata()[1] 116 | print(f'{handling_data}\t{q1:.6f}\t{median:.6f}\t{q3:.6f}') 117 | 118 | ax.text(i+offset, q1, f'Q1: {q1:.6f}', fontsize=15, ha='right') 119 | ax.text(i+offset, median, f'Median: {median:.6f}', fontsize=15, ha='right') 120 | ax.text(i+offset, q3, f'Q3: {q3:.6f}', fontsize=15, ha='right') 121 | ax3 = ax.twinx() 122 | ax3.spines['right'].set_position(('outward', -450)) 123 | # ax3.set_ylabel(r'curvature [x $10^{-1}$]', fontsize=25, labelpad=15) 124 | ax3.set_ylim(-0.0, 0.15) 125 | ax3.yaxis.set_major_formatter(ScalarFormatter(useMathText=True)) 126 | ax3.ticklabel_format(style='sci', axis='y', scilimits=(0,0)) 127 | ax3.tick_params(axis='y', labelsize=20) 128 | ax3.yaxis.get_offset_text().set_fontsize(20) 129 | ax3.yaxis.get_offset_text().set_position((0.76, 0)) 130 | box3 = ax3.boxplot(Z3['MPPI_IPDDP'], positions=[3.5], widths=0.5, patch_artist=True) 131 | for patch in box3['boxes']: 132 | patch.set_facecolor('lightblue') 133 | stats1 = box3['whiskers'] 134 | median = box3['medians'][0].get_ydata()[0] 135 | q1 = stats1[0].get_ydata()[1] 136 | q3 = stats1[1].get_ydata()[1] 137 | offset = 0.2 138 | # ax3.text(3+offset, q1, f'Q1: {q1:.6f}', fontsize=15, ha='right') 139 | ax3.text(3+offset, q1 - 0.001, f'Q1: {q1:.6f}', fontsize=15, ha='right') 140 | ax3.text(3+offset, median, f'Median: {median:.6f}', fontsize=15, ha='right') 141 | ax3.text(3+offset, q3, f'Q3: {q3:.6f}', fontsize=15, ha='right') 142 | print(f'{"MPPI_IPDDP"}\t{q1:.6f}\t{median:.6f}\t{q3:.6f}') 143 | 144 | plt.subplots_adjust(wspace=0.3, hspace=0.3) 145 | plt.show() 146 | -------------------------------------------------------------------------------- /graph/graph.py: -------------------------------------------------------------------------------- 1 | import pandas as pd 2 | import numpy as np 3 | import matplotlib.pyplot as plt 4 | from mpl_toolkits.mplot3d import Axes3D 5 | 6 | handling_datas = ['MPPI', 'Log_MPPI', 'Smooth_MPPI', 'MPPI_IPDDP'] 7 | 8 | for handling_data in handling_datas: 9 | file_path = 'other_mppi.xlsx' 10 | df = pd.read_excel(file_path, sheet_name=handling_data) 11 | 12 | # N S_u P 13 | X = [i/10 for i in range(1,10)] 14 | Y = [100 * (2**i) for i in range(9)] 15 | X, Y = np.meshgrid(X, Y) 16 | 17 | Z1 = df['P'].values.reshape(X.shape) 18 | Z2 = df['avg_time'].values.reshape(X.shape) 19 | Z3 = df['a_msc_x'].values.reshape(X.shape) 20 | Z4 = df['a_msc_u'].values.reshape(X.shape) 21 | Z3[Z3 == 0] = 0.015 22 | Z4[Z4 == 0] = 6.5 23 | 24 | fig = plt.figure() 25 | fig.suptitle(handling_data, fontsize=50, fontweight='bold') 26 | 27 | ax1 = fig.add_subplot(221, projection='3d') 28 | surf = ax1.plot_surface(X, Y, Z1, cmap='coolwarm') 29 | ax1.set_xlabel('Sigma_u', fontsize=20, labelpad=15) 30 | ax1.set_ylabel('N', fontsize=20, labelpad=15) 31 | ax1.set_zlabel('Success', fontsize=20, labelpad=15) 32 | ax1.view_init(azim=-130, elev=50) 33 | ax1.set_zlim(0, 10) 34 | ax1.set_title('Success', fontsize=30, pad=50, fontweight='bold') 35 | 36 | ax1 = fig.add_subplot(222, projection='3d') 37 | surf = ax1.plot_surface(X, Y, Z2, cmap='coolwarm') 38 | ax1.set_xlabel('Sigma_u', fontsize=20, labelpad=15) 39 | ax1.set_ylabel('N', fontsize=20, labelpad=15) 40 | ax1.set_zlabel('Time', fontsize=20, labelpad=15) 41 | ax1.set_zlim(0, 10) 42 | ax1.view_init(azim=-130, elev=50) 43 | ax1.set_title('Average Time', fontsize=30, pad=50, fontweight='bold') 44 | 45 | ax1 = fig.add_subplot(223, projection='3d') 46 | surf = ax1.plot_surface(X, Y, Z3, cmap='coolwarm') 47 | ax1.set_xlabel('Sigma_u', fontsize=20, labelpad=15) 48 | ax1.set_ylabel('N', fontsize=20, labelpad=15) 49 | ax1.set_zlabel('Curvature', fontsize=20, labelpad=15) 50 | ax1.view_init(azim=-130, elev=50) 51 | ax1.set_zlim(0, 0.015) 52 | ax1.set_title('Mean Squared Curvature X', fontsize=30, pad=50, fontweight='bold') 53 | 54 | ax1 = fig.add_subplot(224, projection='3d') 55 | surf = ax1.plot_surface(X, Y, Z4, cmap='coolwarm') 56 | ax1.set_xlabel('Sigma_u', fontsize=20, labelpad=15) 57 | ax1.set_ylabel('N', fontsize=20, labelpad=15) 58 | ax1.set_zlabel('Curvature', fontsize=20, labelpad=15) 59 | ax1.view_init(azim=-130, elev=50) 60 | ax1.set_zlim(0, 6.5) 61 | ax1.set_title('Mean Squared Curvature U', fontsize=30, pad=50, fontweight='bold') 62 | 63 | plt.show() -------------------------------------------------------------------------------- /graph/log_mppi.txt: -------------------------------------------------------------------------------- 1 | Target = Log-MPPI 2 | Simulate 10 times for each 3 | N S_u P F a_msc_x a_msc_u a_tv_x a_tv_u avg_time min_time max_time 4 | 100 0.10 10 0 0.006000 2.704670 11.032316 66.906372 0.030440 0.020399 0.051635 5 | 100 0.20 10 0 0.008071 3.688700 12.045502 75.843299 0.250707 0.068128 0.554410 6 | 100 0.30 3 7 0.009055 4.171926 12.301535 80.005965 0.506180 0.142427 0.823049 7 | 100 0.40 0 10 0.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000 8 | 100 0.50 0 10 0.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000 9 | 100 0.60 0 10 0.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000 10 | 100 0.70 0 10 0.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000 11 | 100 0.80 0 10 0.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000 12 | 100 0.90 0 10 0.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000 13 | 200 0.10 10 0 0.005556 2.542024 10.964405 65.461876 0.029154 0.021149 0.036560 14 | 200 0.20 10 0 0.007795 3.436761 11.938850 74.392763 0.073711 0.011294 0.161187 15 | 200 0.30 5 5 0.009125 4.169753 12.218773 81.573938 0.275299 0.018638 0.532678 16 | 200 0.40 0 10 0.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000 17 | 200 0.50 0 10 0.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000 18 | 200 0.60 0 10 0.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000 19 | 200 0.70 0 10 0.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000 20 | 200 0.80 0 10 0.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000 21 | 200 0.90 0 10 0.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000 22 | 400 0.10 10 0 0.005522 2.529974 10.828602 63.740944 0.043885 0.033835 0.056738 23 | 400 0.20 10 0 0.007644 3.571951 11.706484 74.462516 0.073159 0.027127 0.168852 24 | 400 0.30 8 2 0.008831 4.055632 12.117893 78.762751 0.189513 0.046018 0.595009 25 | 400 0.40 1 9 0.008142 3.607119 12.121251 76.858412 0.819842 0.819842 0.819842 26 | 400 0.50 0 10 0.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000 27 | 400 0.60 0 10 0.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000 28 | 400 0.70 0 10 0.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000 29 | 400 0.80 0 10 0.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000 30 | 400 0.90 0 10 0.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000 31 | 800 0.10 10 0 0.004605 2.015324 10.571164 59.303724 0.072480 0.058909 0.093926 32 | 800 0.20 10 0 0.006633 2.871187 11.389261 68.390137 0.079469 0.037784 0.111523 33 | 800 0.30 10 0 0.007614 3.372039 11.922167 74.403636 0.256652 0.065313 0.949305 34 | 800 0.40 8 2 0.008863 3.944607 12.407161 78.700463 0.264028 0.068574 0.983465 35 | 800 0.50 3 7 0.011197 5.143063 12.732709 88.720991 0.418619 0.068885 0.663885 36 | 800 0.60 0 10 0.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000 37 | 800 0.70 0 10 0.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000 38 | 800 0.80 0 10 0.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000 39 | 800 0.90 0 10 0.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000 40 | 1600 0.10 10 0 0.005217 2.416279 10.653382 62.154036 0.122428 0.105258 0.154310 41 | 1600 0.20 10 0 0.006368 2.854137 11.193235 68.658899 0.110172 0.080106 0.180729 42 | 1600 0.30 10 0 0.007645 3.344164 11.783635 74.261268 0.213947 0.057488 0.522363 43 | 1600 0.40 10 0 0.008329 3.644244 12.185344 76.346148 0.343644 0.072716 0.776303 44 | 1600 0.50 3 7 0.008112 3.681551 12.444517 76.527344 0.574459 0.311659 0.903522 45 | 1600 0.60 0 10 0.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000 46 | 1600 0.70 1 9 0.010557 5.122912 12.289346 90.461534 0.893564 0.893564 0.893564 47 | 1600 0.80 0 10 0.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000 48 | 1600 0.90 0 10 0.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000 49 | 3200 0.10 10 0 0.004263 1.919243 10.295125 56.803893 0.221797 0.190629 0.257551 50 | 3200 0.20 10 0 0.006232 2.755755 11.066600 68.312091 0.174622 0.147187 0.217098 51 | 3200 0.30 10 0 0.007556 3.427808 11.744074 71.921503 0.225761 0.137200 0.488231 52 | 3200 0.40 10 0 0.007944 3.601576 12.104335 74.899762 0.432855 0.143960 0.996984 53 | 3200 0.50 5 5 0.009308 4.248082 12.376776 79.651227 0.580325 0.162821 0.991238 54 | 3200 0.60 1 9 0.008369 3.608936 12.946547 74.592923 0.999672 0.999672 0.999672 55 | 3200 0.70 0 10 0.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000 56 | 3200 0.80 0 10 0.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000 57 | 3200 0.90 0 10 0.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000 58 | 6400 0.10 10 0 0.004332 1.998206 10.175142 57.665125 0.389870 0.346617 0.458665 59 | 6400 0.20 10 0 0.005549 2.508951 10.893582 64.708690 0.331087 0.236163 0.443830 60 | 6400 0.30 10 0 0.006952 3.150615 11.303429 71.890767 0.369531 0.206640 0.630795 61 | 6400 0.40 10 0 0.008185 3.676630 11.935936 75.108659 0.457825 0.246020 0.795908 62 | 6400 0.50 6 4 0.009174 4.102336 12.444349 78.978201 0.573284 0.389565 0.832356 63 | 6400 0.60 1 9 0.008845 3.758876 12.834204 74.654649 0.914847 0.914847 0.914847 64 | 6400 0.70 1 9 0.011057 4.987999 12.297216 87.563178 0.535131 0.535131 0.535131 65 | 6400 0.80 0 10 0.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000 66 | 6400 0.90 0 10 0.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000 67 | 12800 0.10 10 0 0.004506 2.088342 10.266513 59.736247 0.698519 0.648432 0.787971 68 | 12800 0.20 10 0 0.005675 2.514636 10.999735 65.186540 0.507063 0.423783 0.552300 69 | 12800 0.30 10 0 0.007056 3.182473 11.448163 71.893336 0.553738 0.376639 0.967283 70 | 12800 0.40 9 1 0.007717 3.479499 11.800192 74.077051 0.507612 0.373122 0.659378 71 | 12800 0.50 4 6 0.009184 4.324051 12.259238 79.879651 0.651813 0.456473 0.915344 72 | 12800 0.60 2 8 0.009862 4.775135 12.014064 86.120489 0.953326 0.909351 0.997302 73 | 12800 0.70 1 9 0.012070 5.730445 12.594779 96.982602 0.405564 0.405564 0.405564 74 | 12800 0.80 0 10 0.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000 75 | 12800 0.90 0 10 0.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000 76 | 25600 0.10 0 10 0.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000 77 | 25600 0.20 4 6 0.006285 2.855945 10.967635 68.493759 0.877279 0.853857 0.939981 78 | 25600 0.30 6 4 0.006262 2.746781 11.426269 68.319429 0.836660 0.681124 0.942969 79 | 25600 0.40 3 7 0.009255 4.474263 12.020592 83.085550 0.699361 0.600804 0.755478 80 | 25600 0.50 3 7 0.009166 3.944028 12.431957 79.334662 0.759034 0.677585 0.918209 81 | 25600 0.60 4 6 0.010045 4.620518 12.520149 83.983228 0.768979 0.582152 0.988094 82 | 25600 0.70 0 10 0.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000 83 | 25600 0.80 0 10 0.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000 84 | 25600 0.90 0 10 0.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000 85 | -------------------------------------------------------------------------------- /graph/mppi.txt: -------------------------------------------------------------------------------- 1 | Target = MPPI 2 | Simulate 10 times for each 3 | N S_u P F a_msc_x a_msc_u a_tv_x a_tv_u avg_time min_time max_time 4 | 100 0.10 8 2 0.004664 2.040511 10.740099 58.767047 0.556024 0.206213 0.966229 5 | 100 0.20 10 0 0.005002 2.220486 10.785489 60.816347 0.020795 0.013744 0.029600 6 | 100 0.30 10 0 0.006301 2.774188 11.471136 67.575776 0.152409 0.026747 0.586280 7 | 100 0.40 1 9 0.006528 2.864798 11.395170 66.927563 0.574392 0.574392 0.574392 8 | 100 0.50 0 10 0.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000 9 | 100 0.60 0 10 0.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000 10 | 100 0.70 0 10 0.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000 11 | 100 0.80 0 10 0.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000 12 | 100 0.90 0 10 0.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000 13 | 200 0.10 0 10 0.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000 14 | 200 0.20 10 0 0.005219 2.333605 10.837939 61.473175 0.026396 0.017567 0.040371 15 | 200 0.30 10 0 0.006237 2.773095 11.330797 67.256999 0.071690 0.016010 0.165794 16 | 200 0.40 8 2 0.007257 3.265688 11.624479 73.069231 0.318605 0.046265 0.694450 17 | 200 0.50 0 10 0.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000 18 | 200 0.60 0 10 0.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000 19 | 200 0.70 0 10 0.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000 20 | 200 0.80 0 10 0.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000 21 | 200 0.90 0 10 0.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000 22 | 400 0.10 0 10 0.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000 23 | 400 0.20 10 0 0.004490 2.004597 10.609263 58.733546 0.037607 0.030522 0.047149 24 | 400 0.30 10 0 0.005717 2.556972 11.268009 64.209036 0.052249 0.024359 0.125676 25 | 400 0.40 10 0 0.007277 3.417715 11.559405 73.115510 0.237340 0.064774 0.809323 26 | 400 0.50 2 8 0.009462 4.508425 11.963432 82.887544 0.659194 0.499083 0.819306 27 | 400 0.60 0 10 0.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000 28 | 400 0.70 0 10 0.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000 29 | 400 0.80 0 10 0.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000 30 | 400 0.90 0 10 0.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000 31 | 800 0.10 0 10 0.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000 32 | 800 0.20 10 0 0.004603 2.010988 10.534520 59.127452 0.075884 0.048925 0.167950 33 | 800 0.30 10 0 0.005418 2.470363 10.927816 63.183693 0.060836 0.038172 0.088143 34 | 800 0.40 10 0 0.006632 2.953197 11.427582 68.837183 0.142038 0.038462 0.349492 35 | 800 0.50 8 2 0.007211 3.166619 11.851741 71.512852 0.503559 0.154425 0.902826 36 | 800 0.60 5 5 0.008191 3.599524 11.915994 74.767293 0.617545 0.259649 0.893708 37 | 800 0.70 0 10 0.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000 38 | 800 0.80 0 10 0.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000 39 | 800 0.90 0 10 0.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000 40 | 1600 0.10 0 10 0.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000 41 | 1600 0.20 10 0 0.004218 1.837291 10.568520 55.267916 0.117327 0.088431 0.205582 42 | 1600 0.30 10 0 0.004888 2.138605 10.834710 61.145458 0.086561 0.061620 0.110443 43 | 1600 0.40 10 0 0.006314 2.800537 11.384974 67.452361 0.109471 0.050127 0.205379 44 | 1600 0.50 9 1 0.007078 3.080746 11.788639 71.503295 0.380032 0.102299 0.762263 45 | 1600 0.60 4 6 0.008302 3.814861 11.867271 76.757577 0.575033 0.115944 0.842909 46 | 1600 0.70 1 9 0.010231 4.681419 12.212642 87.418107 0.696528 0.696528 0.696528 47 | 1600 0.80 1 9 0.007387 3.314479 12.367211 73.265532 0.646785 0.646785 0.646785 48 | 1600 0.90 0 10 0.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000 49 | 3200 0.10 0 10 0.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000 50 | 3200 0.20 10 0 0.003567 1.569704 10.186178 52.378055 0.187835 0.156345 0.219419 51 | 3200 0.30 10 0 0.004629 2.053585 10.623789 59.355019 0.150787 0.111130 0.280088 52 | 3200 0.40 10 0 0.006792 3.172256 11.446258 70.830143 0.139504 0.089458 0.227738 53 | 3200 0.50 10 0 0.006808 3.096094 11.546639 70.228761 0.250672 0.111749 0.432220 54 | 3200 0.60 6 4 0.007458 3.233250 11.688981 73.578540 0.357352 0.156261 0.664762 55 | 3200 0.70 3 7 0.008156 3.544932 12.013037 77.466865 0.297109 0.141762 0.396314 56 | 3200 0.80 0 10 0.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000 57 | 3200 0.90 0 10 0.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000 58 | 6400 0.10 0 10 0.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000 59 | 6400 0.20 10 0 0.003675 1.615677 10.097017 54.080871 0.359456 0.271409 0.474580 60 | 6400 0.30 10 0 0.004456 1.914160 10.629009 58.506961 0.246495 0.182091 0.315048 61 | 6400 0.40 10 0 0.005352 2.377174 10.960651 62.404131 0.231080 0.167989 0.308332 62 | 6400 0.50 10 0 0.006628 3.014188 11.462904 68.802850 0.261141 0.134521 0.364668 63 | 6400 0.60 6 4 0.006811 3.057080 11.544854 69.665975 0.586839 0.287708 0.926820 64 | 6400 0.70 4 6 0.008144 3.606687 12.076527 77.352481 0.405133 0.172557 0.697516 65 | 6400 0.80 1 9 0.009695 4.535159 12.174485 81.344714 0.556497 0.556497 0.556497 66 | 6400 0.90 0 10 0.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000 67 | 12800 0.10 0 10 0.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000 68 | 12800 0.20 10 0 0.003741 1.686823 10.109314 52.529478 0.569952 0.493316 0.672564 69 | 12800 0.30 10 0 0.004628 2.076351 10.537178 59.673019 0.425427 0.353004 0.521931 70 | 12800 0.40 10 0 0.004845 2.093298 10.830089 59.428607 0.431268 0.304106 0.564247 71 | 12800 0.50 9 1 0.006827 2.997711 11.414800 70.924932 0.436638 0.300314 0.653665 72 | 12800 0.60 7 3 0.006372 2.755026 11.381569 67.079492 0.521195 0.240745 0.797036 73 | 12800 0.70 0 10 0.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000 74 | 12800 0.80 1 9 0.009007 3.745444 12.587546 77.186903 0.607024 0.607024 0.607024 75 | 12800 0.90 0 10 0.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000 76 | 25600 0.10 0 10 0.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000 77 | 25600 0.20 1 9 0.004934 2.419827 10.741028 59.044980 0.953684 0.953684 0.953684 78 | 25600 0.30 9 1 0.004242 1.898621 10.432594 57.178759 0.793758 0.705954 0.953083 79 | 25600 0.40 9 1 0.004972 2.211460 10.746869 60.282092 0.803839 0.671989 0.975141 80 | 25600 0.50 8 2 0.006300 2.883597 11.094431 67.860794 0.722632 0.538966 0.863149 81 | 25600 0.60 5 5 0.006859 3.081415 11.366696 72.270675 0.769857 0.544769 0.974984 82 | 25600 0.70 5 5 0.007282 3.079658 11.696242 73.285021 0.720758 0.479368 0.865608 83 | 25600 0.80 1 9 0.007011 3.006743 11.472812 70.310491 0.833916 0.833916 0.833916 84 | 25600 0.90 1 9 0.008750 3.922192 11.885666 79.040352 0.421431 0.421431 0.421431 85 | -------------------------------------------------------------------------------- /graph/mppi_ipddp.txt: -------------------------------------------------------------------------------- 1 | Target = MPPI-IPDDP 2 | Simulate 10 times for each 3 | N S_u P F a_msc_x a_msc_u a_tv_x a_tv_u avg_time min_time max_time 4 | 100 0.10 0 10 0.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000 5 | 100 0.20 3 7 0.000020 0.002009 8.408971 2.892563 0.832685 0.775504 0.919115 6 | 100 0.30 9 1 0.000091 0.034550 8.467030 5.169320 0.586240 0.458275 0.910621 7 | 100 0.40 10 0 0.000188 0.075581 8.573893 7.675681 0.458942 0.350352 0.631798 8 | 100 0.50 10 0 0.000099 0.037270 8.660014 5.694912 0.499318 0.387399 0.700492 9 | 100 0.60 9 1 0.000036 0.009093 8.511055 3.899797 0.502253 0.301119 0.793011 10 | 100 0.70 3 7 0.000021 0.002131 8.495531 3.014921 0.489968 0.348027 0.748451 11 | 100 0.80 1 9 0.000031 0.004520 8.934170 3.667051 0.544633 0.544633 0.544633 12 | 100 0.90 0 10 0.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000 13 | 200 0.10 0 10 0.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000 14 | 200 0.20 4 6 0.000020 0.001968 8.466903 2.995929 0.705625 0.631619 0.800669 15 | 200 0.30 10 0 0.000026 0.004786 8.472839 3.561240 0.575110 0.389193 0.783344 16 | 200 0.40 10 0 0.000032 0.007811 8.439648 3.719991 0.427910 0.397787 0.488684 17 | 200 0.50 10 0 0.000020 0.002130 8.555949 3.013074 0.429027 0.325481 0.584198 18 | 200 0.60 10 0 0.000020 0.001483 8.572715 3.035039 0.360328 0.250695 0.477820 19 | 200 0.70 7 3 0.000106 0.039023 8.548982 5.853702 0.563051 0.327428 0.866506 20 | 200 0.80 4 6 0.000036 0.006707 8.709847 4.137896 0.463312 0.306096 0.746182 21 | 200 0.90 1 9 0.000025 0.003020 8.507974 3.206474 0.774575 0.774575 0.774575 22 | 400 0.10 0 10 0.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000 23 | 400 0.20 5 5 0.000026 0.004085 8.429015 3.066116 0.711110 0.533448 0.912689 24 | 400 0.30 10 0 0.000019 0.002352 8.401051 3.037011 0.488688 0.371834 0.626283 25 | 400 0.40 10 0 0.000022 0.003094 8.399701 3.052088 0.401469 0.347659 0.458753 26 | 400 0.50 10 0 0.000086 0.031818 8.556811 5.223675 0.351885 0.294409 0.429666 27 | 400 0.60 10 0 0.000022 0.003159 8.443232 2.998982 0.332537 0.281439 0.396617 28 | 400 0.70 9 1 0.000025 0.003378 8.515083 3.194511 0.468151 0.307731 0.656106 29 | 400 0.80 8 2 0.000025 0.003208 8.636210 3.327636 0.512401 0.246022 0.940289 30 | 400 0.90 7 3 0.000033 0.005840 8.622378 3.593090 0.639591 0.480245 0.966711 31 | 800 0.10 0 10 0.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000 32 | 800 0.20 4 6 0.000021 0.002760 8.406917 2.979798 0.688137 0.591892 0.781129 33 | 800 0.30 10 0 0.000049 0.016705 8.422144 4.830772 0.489526 0.430245 0.567409 34 | 800 0.40 10 0 0.000092 0.033817 8.586699 5.261969 0.403572 0.332786 0.471525 35 | 800 0.50 10 0 0.000067 0.020165 8.536955 4.860436 0.333480 0.280525 0.412049 36 | 800 0.60 9 1 0.000070 0.022393 8.573944 4.832644 0.367270 0.284744 0.642402 37 | 800 0.70 10 0 0.000092 0.034950 8.544810 5.458489 0.383166 0.305683 0.483488 38 | 800 0.80 10 0 0.000022 0.002723 8.545308 3.090507 0.452548 0.275675 0.758929 39 | 800 0.90 8 2 0.000132 0.051434 8.631655 6.082611 0.559833 0.426506 0.778090 40 | 1600 0.10 0 10 0.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000 41 | 1600 0.20 7 3 0.000019 0.002385 8.395372 2.906732 0.803541 0.719871 0.930279 42 | 1600 0.30 10 0 0.000084 0.031740 8.428496 4.968050 0.536098 0.447399 0.635681 43 | 1600 0.40 10 0 0.000019 0.001855 8.443339 2.935759 0.407994 0.328387 0.503734 44 | 1600 0.50 10 0 0.000202 0.085154 8.518965 7.930676 0.378817 0.280804 0.486173 45 | 1600 0.60 10 0 0.000026 0.003953 8.508653 3.352538 0.404013 0.316743 0.506418 46 | 1600 0.70 10 0 0.000020 0.002383 8.502470 3.072065 0.384805 0.262103 0.561993 47 | 1600 0.80 10 0 0.000023 0.002811 8.523181 3.152777 0.422721 0.292085 0.713277 48 | 1600 0.90 10 0 0.000036 0.009385 8.425335 3.853834 0.455373 0.323211 0.661979 49 | 3200 0.10 0 10 0.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000 50 | 3200 0.20 3 7 0.000018 0.002289 8.356076 2.918536 0.879329 0.831019 0.971638 51 | 3200 0.30 10 0 0.000018 0.002355 8.367903 2.841594 0.664119 0.573069 0.814283 52 | 3200 0.40 10 0 0.000062 0.021469 8.470352 4.734770 0.497219 0.416182 0.690932 53 | 3200 0.50 10 0 0.000023 0.003180 8.476817 3.035168 0.421262 0.368823 0.525240 54 | 3200 0.60 10 0 0.000020 0.001884 8.483429 2.960348 0.418154 0.343408 0.494772 55 | 3200 0.70 10 0 0.000039 0.010667 8.581896 4.135296 0.405973 0.328767 0.565317 56 | 3200 0.80 10 0 0.000025 0.004221 8.439254 3.092956 0.393985 0.326793 0.533459 57 | 3200 0.90 10 0 0.000040 0.007925 8.472785 3.573917 0.450715 0.314413 0.700851 58 | 6400 0.10 0 10 0.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000 59 | 6400 0.20 0 10 0.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000 60 | 6400 0.30 8 2 0.000060 0.020564 8.429949 4.633413 0.826625 0.727642 0.925479 61 | 6400 0.40 10 0 0.000046 0.014561 8.455139 4.345835 0.703160 0.594921 0.821866 62 | 6400 0.50 10 0 0.000179 0.072742 8.593504 7.447720 0.600911 0.515289 0.691922 63 | 6400 0.60 10 0 0.000025 0.004236 8.442252 3.271415 0.560653 0.415670 0.756662 64 | 6400 0.70 10 0 0.000097 0.037484 8.613348 5.799487 0.615481 0.478227 0.907236 65 | 6400 0.80 10 0 0.000418 0.185257 8.782012 8.351479 0.495850 0.381112 0.727907 66 | 6400 0.90 10 0 0.000217 0.094690 8.524872 6.972015 0.662261 0.485697 0.970834 67 | 12800 0.10 0 10 0.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000 68 | 12800 0.20 0 10 0.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000 69 | 12800 0.30 0 10 0.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000 70 | 12800 0.40 1 9 0.000022 0.002717 8.437724 2.898120 0.969459 0.969459 0.969459 71 | 12800 0.50 9 1 0.000103 0.042142 8.475145 5.375617 0.863126 0.823558 0.992739 72 | 12800 0.60 10 0 0.000022 0.002692 8.450850 3.060802 0.831714 0.796799 0.953953 73 | 12800 0.70 10 0 0.000180 0.066259 8.532175 6.298185 0.781411 0.657482 0.938770 74 | 12800 0.80 8 2 0.000026 0.003980 8.507977 3.233362 0.758158 0.631965 0.946361 75 | 12800 0.90 10 0 0.000022 0.002565 8.509973 3.118293 0.731301 0.619160 0.886195 76 | 25600 0.10 0 10 0.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000 77 | 25600 0.20 0 10 0.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000 78 | 25600 0.30 0 10 0.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000 79 | 25600 0.40 0 10 0.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000 80 | 25600 0.50 0 10 0.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000 81 | 25600 0.60 0 10 0.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000 82 | 25600 0.70 0 10 0.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000 83 | 25600 0.80 0 10 0.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000 84 | 25600 0.90 0 10 0.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000 85 | -------------------------------------------------------------------------------- /graph/other_mppi.xlsx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MC00614/mppi_ipddp_cpp/009aecadaa7e46aec6a25f1fdfc73d56f480a0a6/graph/other_mppi.xlsx -------------------------------------------------------------------------------- /graph/smooth_mppi.txt: -------------------------------------------------------------------------------- 1 | Target = Smooth-MPPI 2 | Simulate 10 times for each 3 | N S_u P F a_msc_x a_msc_u a_tv_x a_tv_u avg_time min_time max_time 4 | 100 0.10 10 0 0.004674 1.996101 11.267287 54.806079 0.066254 0.013260 0.152470 5 | 100 0.20 0 10 0.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000 6 | 100 0.30 0 10 0.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000 7 | 100 0.40 0 10 0.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000 8 | 100 0.50 0 10 0.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000 9 | 100 0.60 0 10 0.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000 10 | 100 0.70 0 10 0.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000 11 | 100 0.80 0 10 0.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000 12 | 100 0.90 0 10 0.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000 13 | 200 0.10 10 0 0.005592 2.473568 11.452953 59.193240 0.089744 0.011433 0.254885 14 | 200 0.20 7 3 0.005338 2.350370 11.243303 58.411255 0.379583 0.103449 0.626047 15 | 200 0.30 0 10 0.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000 16 | 200 0.40 0 10 0.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000 17 | 200 0.50 0 10 0.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000 18 | 200 0.60 0 10 0.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000 19 | 200 0.70 0 10 0.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000 20 | 200 0.80 0 10 0.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000 21 | 200 0.90 0 10 0.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000 22 | 400 0.10 10 0 0.005634 2.417266 11.657927 59.835033 0.163579 0.028626 0.673442 23 | 400 0.20 10 0 0.003029 1.233318 10.655399 43.662284 0.092764 0.025822 0.297432 24 | 400 0.30 0 10 0.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000 25 | 400 0.40 0 10 0.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000 26 | 400 0.50 0 10 0.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000 27 | 400 0.60 0 10 0.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000 28 | 400 0.70 0 10 0.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000 29 | 400 0.80 0 10 0.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000 30 | 400 0.90 0 10 0.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000 31 | 800 0.10 6 4 0.005500 2.389195 11.351590 59.193265 0.157839 0.050411 0.296062 32 | 800 0.20 10 0 0.004143 1.681519 11.265113 50.035882 0.091221 0.029289 0.244080 33 | 800 0.30 7 3 0.004096 1.740977 11.007078 52.465245 0.459708 0.104619 0.895531 34 | 800 0.40 0 10 0.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000 35 | 800 0.50 0 10 0.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000 36 | 800 0.60 0 10 0.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000 37 | 800 0.70 0 10 0.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000 38 | 800 0.80 0 10 0.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000 39 | 800 0.90 0 10 0.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000 40 | 1600 0.10 6 4 0.005823 2.458787 11.643074 59.337328 0.330104 0.100525 0.970091 41 | 1600 0.20 10 0 0.003977 1.681116 11.358134 47.649622 0.244149 0.064259 0.798621 42 | 1600 0.30 10 0 0.003112 1.260089 10.919609 43.446671 0.140122 0.056989 0.262127 43 | 1600 0.40 2 8 0.006722 2.977948 11.472839 69.641894 0.598550 0.541464 0.655637 44 | 1600 0.50 0 10 0.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000 45 | 1600 0.60 0 10 0.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000 46 | 1600 0.70 0 10 0.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000 47 | 1600 0.80 0 10 0.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000 48 | 1600 0.90 0 10 0.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000 49 | 3200 0.10 4 6 0.005048 2.110702 11.607639 55.222916 0.407182 0.271583 0.530853 50 | 3200 0.20 9 1 0.004169 1.712949 11.485084 48.518574 0.294374 0.131514 0.873306 51 | 3200 0.30 10 0 0.002269 0.848576 10.572061 36.755558 0.164467 0.114271 0.355691 52 | 3200 0.40 9 1 0.003387 1.367844 10.784333 46.955073 0.330137 0.081322 0.804388 53 | 3200 0.50 0 10 0.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000 54 | 3200 0.60 0 10 0.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000 55 | 3200 0.70 0 10 0.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000 56 | 3200 0.80 0 10 0.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000 57 | 3200 0.90 0 10 0.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000 58 | 6400 0.10 2 8 0.005557 2.351086 11.770015 57.155597 0.941172 0.882585 0.999759 59 | 6400 0.20 9 1 0.003959 1.681214 11.259891 47.081618 0.426020 0.222506 0.663954 60 | 6400 0.30 10 0 0.003813 1.554044 11.222244 45.786527 0.274315 0.174803 0.513715 61 | 6400 0.40 10 0 0.002341 0.856836 10.659162 35.978164 0.284678 0.108027 0.633066 62 | 6400 0.50 3 7 0.006161 2.607998 11.838435 63.273392 0.377844 0.105902 0.886136 63 | 6400 0.60 0 10 0.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000 64 | 6400 0.70 0 10 0.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000 65 | 6400 0.80 0 10 0.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000 66 | 6400 0.90 0 10 0.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000 67 | 12800 0.10 0 10 0.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000 68 | 12800 0.20 8 2 0.005218 2.318049 11.324513 55.774838 0.547217 0.340764 0.981431 69 | 12800 0.30 10 0 0.003719 1.476942 11.335202 44.463505 0.415041 0.266389 0.558652 70 | 12800 0.40 9 1 0.004295 1.849070 11.228931 49.994888 0.345158 0.224836 0.606181 71 | 12800 0.50 7 3 0.003270 1.315917 10.911881 44.023168 0.387475 0.251751 0.602068 72 | 12800 0.60 2 8 0.004871 2.152483 11.516360 57.566082 0.227071 0.209886 0.244256 73 | 12800 0.70 0 10 0.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000 74 | 12800 0.80 1 9 0.007567 2.978168 12.803598 68.469207 0.461747 0.461747 0.461747 75 | 12800 0.90 0 10 0.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000 76 | 25600 0.10 0 10 0.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000 77 | 25600 0.20 2 8 0.004244 1.761101 11.298538 47.889592 0.957709 0.936711 0.978708 78 | 25600 0.30 10 0 0.003242 1.296141 11.156907 41.000866 0.829092 0.644698 0.962732 79 | 25600 0.40 10 0 0.002388 0.910940 10.681906 35.276619 0.710420 0.419974 0.827269 80 | 25600 0.50 8 2 0.001480 0.529877 10.175081 29.926682 0.743460 0.541256 0.911220 81 | 25600 0.60 3 7 0.003112 1.287182 10.663293 46.018160 0.556667 0.418451 0.643797 82 | 25600 0.70 2 8 0.005820 2.678583 11.615351 62.663924 0.597565 0.414053 0.781078 83 | 25600 0.80 0 10 0.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000 84 | 25600 0.90 0 10 0.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000 85 | -------------------------------------------------------------------------------- /model/model_base.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | using namespace autodiff; 6 | 7 | #include 8 | 9 | #include 10 | 11 | class ModelBase { 12 | public: 13 | ModelBase(); 14 | ~ModelBase(); 15 | 16 | int N; 17 | int dim_x; 18 | int dim_u; 19 | int dim_c; 20 | int center_point; 21 | Eigen::MatrixXd X; 22 | Eigen::MatrixXd U; 23 | Eigen::MatrixXd Y; 24 | Eigen::MatrixXd S; 25 | 26 | // Discrete Time System 27 | std::function f; 28 | std::vector> fs; 29 | // Stage Cost Function 30 | std::function q; 31 | // Terminal Cost Function 32 | std::function p; 33 | // Constraint 34 | std::function c; 35 | // Projection 36 | std::function)> h; 37 | }; 38 | 39 | ModelBase::ModelBase() { 40 | f = [this](const VectorXdual2nd& x, const VectorXdual2nd& u) -> VectorXdual2nd { 41 | VectorXdual2nd x_n(dim_x); 42 | for (int i = 0; i < dim_x; ++i) { 43 | x_n(i) = fs[i](x,u); 44 | } 45 | return x_n; 46 | }; 47 | }; 48 | 49 | ModelBase::~ModelBase() { 50 | }; 51 | 52 | 53 | -------------------------------------------------------------------------------- /model/wmrobot.h: -------------------------------------------------------------------------------- 1 | #include "model_base.h" 2 | 3 | class WMRobot : public ModelBase { 4 | public: 5 | WMRobot(); 6 | ~WMRobot(); 7 | 8 | const int consts = 4; 9 | const double dt = 0.1; 10 | }; 11 | 12 | WMRobot::WMRobot() { 13 | // Stage Count 14 | N = 50; 15 | 16 | // Dimensions 17 | dim_x = 3; 18 | dim_u = 2; 19 | dim_c = consts + 1; 20 | 21 | center_point = 2; 22 | 23 | // Status Setting 24 | X = Eigen::MatrixXd::Zero(dim_x, N+1); 25 | X(0,0) = 0.0; 26 | X(1,0) = 0.0; 27 | X(2,0) = M_PI_2; 28 | 29 | // U = 0.02*Eigen::MatrixXd::Random(dim_u, N) - Eigen::MatrixXd::Constant(dim_u, N, 0.01); 30 | U = Eigen::MatrixXd::Zero(dim_u, N); 31 | U(0,0) = 0.0; 32 | 33 | Y = 0.01*Eigen::MatrixXd::Ones(dim_c, N); 34 | 35 | S = 0.01*Eigen::MatrixXd::Ones(dim_c, N); 36 | 37 | // Discrete Time System 38 | auto f0 = [this](const VectorXdual2nd& x, const VectorXdual2nd& u) -> dual2nd { 39 | return x(0) + (u(0) * cos(x(2)) * dt); 40 | }; 41 | fs.push_back(f0); 42 | auto f1 = [this](const VectorXdual2nd& x, const VectorXdual2nd& u) -> dual2nd { 43 | return x(1) + (u(0) * sin(x(2)) * dt); 44 | }; 45 | fs.push_back(f1); 46 | auto f2 = [this](const VectorXdual2nd& x, const VectorXdual2nd& u) -> dual2nd { 47 | return x(2) + (u(1) * dt); 48 | }; 49 | fs.push_back(f2); 50 | 51 | // Stage Cost Function 52 | q = [this](const VectorXdual2nd& x, const VectorXdual2nd& u) -> dual2nd { 53 | return (u.squaredNorm()) + 0.01 * (x(0)*x(0) + (x(1)-6.0)*(x(1)-6.0) + (x(2)-M_PI_2)*(x(2)-M_PI_2)); 54 | }; 55 | 56 | // Terminal Cost Function 57 | p = [this](const VectorXdual2nd& x) -> dual2nd { 58 | return 300.0 * (x(0)*x(0) + (x(1)-6.0)*(x(1)-6.0) + (x(2)-M_PI_2)*(x(2)-M_PI_2)); 59 | }; 60 | 61 | // Constraint 62 | c = [this](const VectorXdual2nd& x, const VectorXdual2nd& u, const VectorXdual2nd& C, const dual2nd& R) -> VectorXdual2nd { 63 | VectorXdual2nd c_n(dim_c); 64 | c_n(0) = u(0) - 1.5; 65 | c_n(1) = - u(0); 66 | c_n(2) = u(1) - 1.5; 67 | c_n(3) = - u(1) - 1.5; 68 | c_n(consts) = (x.topRows(center_point) - C).norm() - (R); 69 | return c_n; 70 | }; 71 | 72 | h = [&](Eigen::Ref U) -> void { 73 | U.row(0) = U.row(0).cwiseMax(0.0).cwiseMin(1.5); 74 | U.row(1) = U.row(1).cwiseMax(-1.5).cwiseMin(1.5); 75 | return; 76 | }; 77 | } 78 | 79 | WMRobot::~WMRobot() { 80 | } -------------------------------------------------------------------------------- /model/wmrobot_map.h: -------------------------------------------------------------------------------- 1 | #include "model_base.h" 2 | 3 | class WMRobotMap : public ModelBase { 4 | public: 5 | WMRobotMap(); 6 | ~WMRobotMap(); 7 | 8 | const int consts = 4; 9 | const double dt = 0.1; 10 | }; 11 | 12 | WMRobotMap::WMRobotMap() { 13 | // Stage Count 14 | N = 100; 15 | 16 | // Dimensions 17 | dim_x = 3; 18 | dim_u = 2; 19 | dim_c = consts + 1; 20 | 21 | center_point = 2; 22 | 23 | // Status Setting 24 | X = Eigen::MatrixXd::Zero(dim_x, N+1); 25 | X(0,0) = 1.5; 26 | X(1,0) = 0.0; 27 | X(2,0) = M_PI_2; 28 | 29 | // U = 0.02*Eigen::MatrixXd::Random(dim_u, N) - Eigen::MatrixXd::Constant(dim_u, N, 0.01); 30 | U = Eigen::MatrixXd::Zero(dim_u, N); 31 | U(0,0) = 0.0; 32 | 33 | Y = 0.01*Eigen::MatrixXd::Ones(dim_c, N); 34 | 35 | S = 0.01*Eigen::MatrixXd::Ones(dim_c, N); 36 | 37 | // Discrete Time System 38 | auto f0 = [this](const VectorXdual2nd& x, const VectorXdual2nd& u) -> dual2nd { 39 | return x(0) + (u(0) * cos(x(2)) * dt); 40 | }; 41 | fs.push_back(f0); 42 | auto f1 = [this](const VectorXdual2nd& x, const VectorXdual2nd& u) -> dual2nd { 43 | return x(1) + (u(0) * sin(x(2)) * dt); 44 | }; 45 | fs.push_back(f1); 46 | auto f2 = [this](const VectorXdual2nd& x, const VectorXdual2nd& u) -> dual2nd { 47 | return x(2) + (u(1) * dt); 48 | }; 49 | fs.push_back(f2); 50 | 51 | // Stage Cost Function 52 | q = [this](const VectorXdual2nd& x, const VectorXdual2nd& u) -> dual2nd { 53 | return (u.squaredNorm()); 54 | }; 55 | 56 | // Terminal Cost Function 57 | p = [this](const VectorXdual2nd& x) -> dual2nd { 58 | return 300.0 * ((x(0)-1.5)*(x(0)-1.5) + (x(1)-5.0)*(x(1)-5.0) + (x(2)-M_PI_2)*(x(2)-M_PI_2)); 59 | }; 60 | 61 | // Constraint 62 | c = [this](const VectorXdual2nd& x, const VectorXdual2nd& u, const VectorXdual2nd& C, const dual2nd& R) -> VectorXdual2nd { 63 | VectorXdual2nd c_n(dim_c); 64 | c_n(0) = u(0) - 1.0; 65 | c_n(1) = - u(0); 66 | c_n(2) = u(1) - 1.5; 67 | c_n(3) = - u(1) - 1.5; 68 | c_n(consts) = (x.topRows(center_point) - C).norm() - (R); 69 | return c_n; 70 | }; 71 | 72 | h = [&](Eigen::Ref U) -> void { 73 | U.row(0) = U.row(0).cwiseMax(0.0).cwiseMin(1.0); 74 | U.row(1) = U.row(1).cwiseMax(-1.5).cwiseMin(1.5); 75 | return; 76 | }; 77 | } 78 | 79 | WMRobotMap::~WMRobotMap() { 80 | } -------------------------------------------------------------------------------- /mppi_ipddp/collision_checker.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "model_base.h" 4 | 5 | #include 6 | 7 | #include 8 | #include 9 | #include 10 | 11 | class CollisionChecker { 12 | public: 13 | CollisionChecker(); 14 | ~CollisionChecker(); 15 | 16 | void loadMap(const std::string& file_path, double resolution); 17 | 18 | // x, y, r, r^2 19 | std::vector> circles; 20 | // x1, x2, y1, y2 21 | std::vector> rectangles; 22 | 23 | void addCircle(double x, double y, double r); 24 | void addRectangle(double x, double y, double w, double h); 25 | 26 | bool getCollisionGrid(const Eigen::VectorXd &x); 27 | bool getCollisionCircle(const Eigen::VectorXd &z); 28 | bool getCollisionGrid_polygon(const Eigen::VectorXd &x); 29 | bool getCollisionCircle_polygon(const Eigen::VectorXd &z); 30 | bool getCollisionGrid_map(const Eigen::VectorXd &x); 31 | bool getCollisionCircle_map(const Eigen::VectorXd &z); 32 | 33 | private: 34 | bool with_map; 35 | std::vector> map; 36 | double resolution; 37 | int max_row; 38 | int max_col; 39 | }; 40 | 41 | CollisionChecker::CollisionChecker() { 42 | circles.clear(); 43 | rectangles.clear(); 44 | with_map = false; 45 | } 46 | 47 | CollisionChecker::~CollisionChecker() { 48 | } 49 | 50 | void CollisionChecker::loadMap(const std::string& file_path, double resolution) { 51 | map.clear(); 52 | std::ifstream file(file_path); 53 | std::string line; 54 | 55 | while (std::getline(file, line)) { 56 | std::vector row; 57 | std::string::size_type sz = 0; 58 | 59 | if (!file.is_open()) { 60 | throw std::runtime_error("Error opening file: " + file_path); 61 | } 62 | 63 | while (sz < line.length()) { 64 | double value = std::stod(line, &sz); 65 | row.push_back(value); 66 | line = line.substr(sz); 67 | } 68 | 69 | map.push_back(row); 70 | } 71 | 72 | file.close(); 73 | 74 | with_map = true; 75 | this->resolution = resolution; 76 | max_row = map.size(); 77 | max_col = map[0].size(); 78 | } 79 | 80 | void CollisionChecker::addCircle(double x, double y, double r) { 81 | circles.push_back({x, y, r, r*r}); 82 | } 83 | 84 | void CollisionChecker::addRectangle(double x, double y, double w, double h) { 85 | rectangles.push_back({x, x + w, y, y + h}); 86 | } 87 | 88 | bool CollisionChecker::getCollisionGrid(const Eigen::VectorXd &x) { 89 | if (with_map) {return getCollisionGrid_map(x);} 90 | else {return getCollisionGrid_polygon(x);} 91 | } 92 | 93 | bool CollisionChecker::getCollisionCircle(const Eigen::VectorXd &x) { 94 | if (with_map) {return getCollisionCircle_map(x);} 95 | else {return getCollisionCircle_polygon(x);} 96 | } 97 | 98 | bool CollisionChecker::getCollisionGrid_polygon(const Eigen::VectorXd &x) { 99 | double dx; 100 | double dy; 101 | double distance_2; 102 | // Circle 103 | for (int i = 0; i < circles.size(); ++i) { 104 | dx = circles[i][0] - x(0); 105 | dy = circles[i][1] - x(1); 106 | distance_2 = (dx * dx) + (dy * dy); 107 | if (distance_2 <= circles[i][3]) {return true;} 108 | else {continue;} 109 | } 110 | // Rectangle 111 | for (int i = 0; i < rectangles.size(); ++i) { 112 | if (x(0) < rectangles[i][0]) {continue;} 113 | else if (rectangles[i][1] < x(0)) {continue;} 114 | else if (x(1) < rectangles[i][2]) {continue;} 115 | else if (rectangles[i][3] < x(1)) {continue;} 116 | else {return true;} 117 | } 118 | return false; 119 | } 120 | 121 | bool CollisionChecker::getCollisionCircle_polygon(const Eigen::VectorXd &z) { 122 | Eigen::VectorXd zj; 123 | double dx; 124 | double dy; 125 | double distance_2; 126 | double dc; 127 | // Circle 128 | for (int i = 0; i < circles.size(); ++i) { 129 | dx = circles[i][0] - z(0); 130 | dy = circles[i][1] - z(1); 131 | distance_2 = (dx * dx) + (dy * dy); 132 | dc = circles[i][2] + z(2); 133 | if (distance_2 <= (dc*dc)) {return true;} 134 | else {continue;} 135 | } 136 | // Rectangle 137 | for (int i = 0; i < rectangles.size(); ++i) { 138 | if ((z(0) + z(2)) < rectangles[i][0]) {continue;} 139 | else if (rectangles[i][1] < (z(0) - z(2))) {continue;} 140 | else if ((z(1) + z(2)) < rectangles[i][2]) {continue;} 141 | else if (rectangles[i][3] < (z(1) - z(2))) {continue;} 142 | else {return true;} 143 | } 144 | return false; 145 | } 146 | 147 | bool CollisionChecker::getCollisionGrid_map(const Eigen::VectorXd &x) { 148 | // Need to check comparison double 149 | int nx = round(x(0)/resolution); 150 | int ny = round(x(1)/resolution); 151 | if (nx < 0 || max_row <= nx) {return false;} 152 | if (nx < 0 || max_col <= ny) {return false;} 153 | if (map[nx][ny] == 10) {return true;} 154 | return false; 155 | } 156 | 157 | bool CollisionChecker::getCollisionCircle_map(const Eigen::VectorXd &z) { 158 | // Need to check comparison double 159 | int cx = round(z(0)/resolution); 160 | int cy = round(z(1)/resolution); 161 | double r = z(2)/resolution + 0.5; 162 | if (cx < 0 || max_row <= cx) {return true;} 163 | if (cy < 0 || max_col <= cy) {return true;} 164 | if (map[cx][cy] == 10) {return true;} 165 | if (map[cx][cy] == 5) {return false;} 166 | if (map[cx][cy] < r) {return true;} 167 | return false; 168 | } -------------------------------------------------------------------------------- /mppi_ipddp/mppi_ipddp.h: -------------------------------------------------------------------------------- 1 | #include "mppi.h" 2 | #include "corridor.h" 3 | #include "ipddp.h" 4 | 5 | #include "model_base.h" 6 | 7 | #include 8 | 9 | class MPPI_IPDDP { 10 | public: 11 | MPPI_IPDDP(ModelBase model); 12 | ~MPPI_IPDDP(); 13 | 14 | void init(MPPIParam mppi_param, CorridorParam corridor_param, Param ipddp_param); 15 | void setCollisionChecker(CollisionChecker *collision_checker); 16 | void solve(int iter); 17 | void move(); 18 | 19 | Eigen::MatrixXd X; 20 | Eigen::MatrixXd U; 21 | Eigen::MatrixXd C; 22 | Eigen::VectorXd R; 23 | 24 | // TEMP 25 | Eigen::MatrixXd mppi_X; 26 | Eigen::MatrixXd mppi_U; 27 | double mppi_duration; 28 | double corridor_duration; 29 | double ipddp_duration; 30 | 31 | private: 32 | MPPI mppi; 33 | Corridor corridor; 34 | IPDDP ipddp; 35 | 36 | std::function f; 37 | 38 | int N; 39 | int dim_x; 40 | int dim_u; 41 | int center_point; 42 | }; 43 | 44 | MPPI_IPDDP::MPPI_IPDDP(ModelBase model) 45 | : mppi(model), ipddp(model), corridor(model) { 46 | N = model.N; 47 | dim_x = model.dim_x; 48 | dim_u = model.dim_u; 49 | center_point = model.center_point; 50 | 51 | f = model.f; 52 | 53 | X = model.X; 54 | U = model.U; 55 | } 56 | 57 | MPPI_IPDDP::~MPPI_IPDDP() { 58 | } 59 | 60 | void MPPI_IPDDP::init(MPPIParam mppi_param, CorridorParam corridor_param, Param ipddp_param) { 61 | mppi.init(mppi_param); 62 | corridor.init(corridor_param); 63 | ipddp.init(ipddp_param); 64 | 65 | C.resize(center_point, N); 66 | R.resize(N); 67 | } 68 | 69 | void MPPI_IPDDP::setCollisionChecker(CollisionChecker *collision_checker) { 70 | mppi.setCollisionChecker(collision_checker); 71 | corridor.setCollisionChecker(collision_checker); 72 | } 73 | 74 | void MPPI_IPDDP::solve(int iter) { 75 | mppi_duration = 0.0; 76 | corridor_duration = 0.0; 77 | ipddp_duration = 0.0; 78 | 79 | auto start = std::chrono::high_resolution_clock::now(); 80 | auto finish = std::chrono::high_resolution_clock::now(); 81 | std::chrono::duration elapsed; 82 | 83 | for (int i = 0; i < iter; ++i) { 84 | start = std::chrono::high_resolution_clock::now(); 85 | // std::cout<<"mppi"<(); 116 | } -------------------------------------------------------------------------------- /mppi_ipddp/parameter.h: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | 5 | Eigen::MatrixXd firstDerivative(const Eigen::MatrixXd &matrix) { 6 | int rows = matrix.rows(); 7 | int cols = matrix.cols(); 8 | Eigen::MatrixXd derivative(rows, cols - 1); 9 | 10 | for (int i = 0; i < cols - 1; ++i) { 11 | derivative.col(i) = matrix.col(i + 1) - matrix.col(i); 12 | } 13 | 14 | return derivative; 15 | } 16 | 17 | Eigen::MatrixXd secondDerivative(const Eigen::MatrixXd &matrix) { 18 | Eigen::MatrixXd first_deriv = firstDerivative(matrix); 19 | return firstDerivative(first_deriv); 20 | } 21 | 22 | double meanSquaredCurvature(const Eigen::MatrixXd &matrix) { 23 | Eigen::MatrixXd second_deriv = secondDerivative(matrix); 24 | double mean_squared_curvature = second_deriv.array().square().mean(); 25 | return mean_squared_curvature; 26 | } 27 | 28 | double totalVariation(const Eigen::MatrixXd &matrix) { 29 | Eigen::MatrixXd first_deriv = firstDerivative(matrix); 30 | double total_variation = first_deriv.array().abs().sum(); 31 | return total_variation; 32 | } -------------------------------------------------------------------------------- /mppi_ipddp/show.h: -------------------------------------------------------------------------------- 1 | #include "matplotlibcpp.h" 2 | 3 | namespace plt = matplotlibcpp; 4 | 5 | inline void show2D(const Eigen::MatrixXd &mppi_X, const Eigen::MatrixXd &mppi_U, const std::string& file_path, double resolution) { 6 | std::vector> X_MPPI(mppi_X.rows(), std::vector(mppi_X.cols())); 7 | for (int i = 0; i < mppi_X.rows(); ++i) { 8 | for (int j = 0; j < mppi_X.cols(); ++j) { 9 | X_MPPI[i][j] = mppi_X(i, j); 10 | } 11 | } 12 | std::vector> U_MPPI(mppi_U.rows(), std::vector(mppi_U.cols())); 13 | for (int i = 0; i < mppi_U.rows(); ++i) { 14 | for (int j = 0; j < mppi_U.cols(); ++j) { 15 | U_MPPI[i][j] = mppi_U(i, j); 16 | } 17 | } 18 | 19 | std::vector> map; 20 | std::ifstream file(file_path); 21 | std::string line; 22 | 23 | while (std::getline(file, line)) { 24 | std::vector row; 25 | std::string::size_type sz = 0; 26 | 27 | if (!file.is_open()) { 28 | throw std::runtime_error("Error opening file: " + file_path); 29 | } 30 | 31 | while (sz < line.length()) { 32 | double value = std::stod(line, &sz); 33 | row.push_back(value); 34 | line = line.substr(sz); 35 | } 36 | 37 | map.push_back(row); 38 | } 39 | file.close(); 40 | 41 | plt::scatter(X_MPPI[0], X_MPPI[1], 10.0, {{"color", "b"}}); 42 | plt::plot(X_MPPI[0], X_MPPI[1], "b"); 43 | 44 | double hl = 0.05; 45 | for (int i = 0; i < map.size(); ++i) { 46 | for (int j = 0; j < map[0].size(); ++j) { 47 | if (map[i][j] == 10) { 48 | double mx = i*resolution; 49 | double my = j*resolution; 50 | std::vector oX = {mx-hl, mx+hl, mx+hl, mx-hl, mx-hl}; 51 | std::vector oY = {my-hl,my-hl,my+hl,my+hl,my-hl}; 52 | plt::plot(oX, oY, "k"); 53 | } 54 | } 55 | } 56 | 57 | plt::xlim(0, 3); 58 | plt::ylim(0, 5); 59 | plt::grid(true); 60 | plt::show(); 61 | } 62 | 63 | inline void show2D(const Eigen::MatrixXd &mppi_X, const Eigen::MatrixXd &mppi_U, const Eigen::MatrixXd &X, const Eigen::MatrixXd &U, const Eigen::MatrixXd &C, const Eigen::VectorXd &R, 64 | const std::vector> &circles, const std::vector> &rectangles) { 65 | 66 | plt::subplot(2,2,1); 67 | std::vector> X_MPPI(X.rows(), std::vector(X.cols())); 68 | for (int i = 0; i < X.rows(); ++i) { 69 | for (int j = 0; j < X.cols(); ++j) { 70 | X_MPPI[i][j] = mppi_X(i, j); 71 | } 72 | } 73 | std::vector> U_MPPI(U.rows(), std::vector(U.cols())); 74 | for (int i = 0; i < U.rows(); ++i) { 75 | for (int j = 0; j < U.cols(); ++j) { 76 | U_MPPI[i][j] = mppi_U(i, j); 77 | } 78 | } 79 | std::vector> X_RES(X.rows(), std::vector(X.cols())); 80 | for (int i = 0; i < X.rows(); ++i) { 81 | for (int j = 0; j < X.cols(); ++j) { 82 | X_RES[i][j] = X(i, j); 83 | } 84 | } 85 | std::vector> U_RES(U.rows(), std::vector(U.cols())); 86 | for (int i = 0; i < U.rows(); ++i) { 87 | for (int j = 0; j < U.cols(); ++j) { 88 | U_RES[i][j] = U(i, j); 89 | } 90 | } 91 | 92 | for (int i = 0; i < circles.size(); ++i) { 93 | std::vector cx; 94 | std::vector cy; 95 | for (int t = 0; t < 361; ++t) { 96 | cx.push_back(circles[i][0] + circles[0][2]*cos(t/180.0*(M_PI))); 97 | cy.push_back(circles[i][1] + circles[0][2]*sin(t/180.0*M_PI)); 98 | } 99 | plt::plot(cx, cy, "k"); 100 | } 101 | 102 | for (int i = 0; i < rectangles.size(); ++i) { 103 | std::vector cx = {rectangles[i][0], rectangles[i][1], rectangles[i][1], rectangles[i][0], rectangles[i][0]}; 104 | std::vector cy = {rectangles[i][2], rectangles[i][2], rectangles[i][3], rectangles[i][3], rectangles[i][2]}; 105 | plt::plot(cx, cy, "k"); 106 | } 107 | 108 | for (int i = 0; i < C.cols(); ++i) { 109 | std::vector cx; 110 | std::vector cy; 111 | for (int t = 0; t < 361; ++t) { 112 | cx.push_back(C.col(i)(0) + R(i)*cos(t/180.0*M_PI)); 113 | cy.push_back(C.col(i)(1) + R(i)*sin(t/180.0*M_PI)); 114 | } 115 | plt::plot(cx, cy, "silver"); 116 | } 117 | 118 | std::vector> C_RES(C.rows(), std::vector(C.cols())); 119 | for (int i = 0; i < C.rows(); ++i) { 120 | for (int j = 0; j < C.cols(); ++j) { 121 | C_RES[i][j] = C(i, j); 122 | } 123 | } 124 | 125 | // plt::plot(C_RES[0], C_RES[1], "g"); 126 | plt::named_plot("MPPI", X_MPPI[0], X_MPPI[1], "b"); 127 | plt::named_plot("MPPI-IPDDP", X_RES[0], X_RES[1], "r"); 128 | plt::scatter(X_MPPI[0], X_MPPI[1], 10.0, {{"color", "b"}}); 129 | plt::scatter(X_RES[0], X_RES[1], 10.0, {{"color", "r"}}); 130 | plt::xlim(0, 3); 131 | plt::ylim(0, 5); 132 | // plt::xlim(-4, 4); 133 | // plt::ylim(-1, 7); 134 | plt::grid(true); 135 | plt::legend({{"fontsize", "20"}}); 136 | 137 | 138 | plt::subplot(2,2,3); 139 | 140 | for (int i = 0; i < circles.size(); ++i) { 141 | std::vector cx; 142 | std::vector cy; 143 | for (int t = 0; t < 361; ++t) { 144 | cx.push_back(circles[i][0] + circles[0][2]*cos(t/180.0*(M_PI))); 145 | cy.push_back(circles[i][1] + circles[0][2]*sin(t/180.0*M_PI)); 146 | } 147 | plt::plot(cx, cy, "k"); 148 | } 149 | 150 | for (int i = 0; i < rectangles.size(); ++i) { 151 | std::vector cx = {rectangles[i][0], rectangles[i][1], rectangles[i][1], rectangles[i][0], rectangles[i][0]}; 152 | std::vector cy = {rectangles[i][2], rectangles[i][2], rectangles[i][3], rectangles[i][3], rectangles[i][2]}; 153 | plt::plot(cx, cy, "k"); 154 | } 155 | 156 | // plt::named_plot("MPPI", X_MPPI[0], X_MPPI[1], "r"); 157 | // plt::named_plot("MPPI-IPDDP", X_RES[0], X_RES[1], "b"); 158 | plt::scatter(X_MPPI[0], X_MPPI[1], 10.0, {{"color", "b"}}); 159 | plt::scatter(X_RES[0], X_RES[1], 10.0, {{"color", "r"}}); 160 | plt::xlim(0, 3); 161 | plt::ylim(0, 5); 162 | // plt::xlim(-4, 4); 163 | // plt::ylim(-1, 7); 164 | plt::grid(true); 165 | // plt::legend({{"fontsize", "20"}}); 166 | 167 | plt::subplot(2,2,2); 168 | plt::named_plot("MPPI", U_MPPI[0]); 169 | plt::named_plot("MPPI-IPDDP", U_RES[0]); 170 | plt::ylim(-0.1, 1.6); 171 | plt::grid(true); 172 | plt::legend({{"fontsize", "20"}}); 173 | // plt::title("U Dimension " + std::to_string(0), {{"fontsize", "20"}}); 174 | plt::title("U Dimension 0 (v)", {{"fontsize", "20"}}); 175 | 176 | plt::subplot(2,2,4); 177 | plt::named_plot("MPPI", U_MPPI[1]); 178 | plt::named_plot("MPPI-IPDDP", U_RES[1]); 179 | plt::ylim(-1.6, 1.6); 180 | // plt::title("U Dimension " + std::to_string(1), {{"fontsize", "20"}}); 181 | plt::title("U Dimension 1 (w)", {{"fontsize", "20"}}); 182 | plt::grid(true); 183 | plt::legend({{"fontsize", "20"}}); 184 | plt::show(); 185 | } -------------------------------------------------------------------------------- /src/example.cpp: -------------------------------------------------------------------------------- 1 | #include "wmrobot.h" 2 | 3 | #include "collision_checker.h" 4 | #include "mppi_ipddp.h" 5 | #include "parameter.h" 6 | 7 | #include 8 | 9 | int main() { 10 | // Model 11 | auto model = WMRobot(); 12 | 13 | // Collision Checker 14 | CollisionChecker collision_checker; 15 | collision_checker.addRectangle(-2.5, 2.0, 3.0, 2.0); 16 | collision_checker.addRectangle(1.0, 2.0, 3.0, 2.0); 17 | collision_checker.addCircle(0.5, 1.0, 0.25); 18 | 19 | Eigen::VectorXd final_state(model.dim_x); 20 | final_state << 0.0, 6.0, M_PI_2; 21 | Eigen::MatrixXd res_X, res_U; 22 | int max_iter = 100; 23 | 24 | // PARAMETERS // PARAMETERS // PARAMETERS // 25 | // MPPI Parameter 26 | MPPIParam mppi_param; 27 | mppi_param.Nu = 300; 28 | mppi_param.gamma_u = 100.0; 29 | Eigen::VectorXd sigma_u(model.dim_u); 30 | sigma_u << 0.5, 0.5; 31 | mppi_param.sigma_u = sigma_u.asDiagonal(); 32 | 33 | // Corridor Parameter 34 | CorridorParam corridor_param; 35 | corridor_param.max_iter = 3; 36 | corridor_param.Nz = 1000; 37 | corridor_param.gamma_z = 1000.0; 38 | Eigen::VectorXd sigma_z(model.center_point + 1); 39 | sigma_z << 0.3, 0.3, 0.1; 40 | corridor_param.sigma_z = sigma_z.asDiagonal(); 41 | corridor_param.lambda_c = 35.0; 42 | corridor_param.lambda_r = 35.0; 43 | corridor_param.r_max = 0.5; 44 | 45 | // IPDDP Parameter 46 | Param ipddp_param; 47 | ipddp_param.tolerance = 1e-7; 48 | ipddp_param.max_iter = 100; 49 | ipddp_param.mu = 0.01; 50 | ipddp_param.infeasible = true; 51 | ipddp_param.q = 0.001; 52 | // PARAMETERS // PARAMETERS // PARAMETERS // 53 | 54 | // MPPI_IPDDP 55 | MPPI_IPDDP mppi_ipddp(model); 56 | mppi_ipddp.init(mppi_param, corridor_param, ipddp_param); 57 | mppi_ipddp.setCollisionChecker(&collision_checker); 58 | 59 | for (int i = 0; i < max_iter; ++i) { 60 | // MPPI_IPDDP 61 | mppi_ipddp.solve(1); 62 | 63 | // double mppi_ipddp_duration = mppi_ipddp.mppi_duration + mppi_ipddp.corridor_duration + mppi_ipddp.ipddp_duration; 64 | 65 | res_X = mppi_ipddp.X; 66 | res_U = mppi_ipddp.U; 67 | std::cout<<"\nX_last = "< 9 | 10 | int main() { 11 | // Model 12 | auto model = WMRobot(); 13 | 14 | // Collision Checker 15 | CollisionChecker collision_checker; 16 | collision_checker.addRectangle(-2.5, 2.0, 3.0, 2.0); 17 | collision_checker.addRectangle(1.0, 2.0, 3.0, 2.0); 18 | collision_checker.addCircle(0.5, 1.0, 0.25); 19 | 20 | clock_t start; 21 | clock_t finish; 22 | 23 | Eigen::VectorXd final_state(model.dim_x); 24 | final_state << 0.0, 6.0, M_PI_2; 25 | Eigen::MatrixXd res_X, res_U; 26 | int max_iter = 10000; 27 | double total_duration = 0.0; 28 | bool is_failed; 29 | int fail = 0; 30 | int sim_maxiter = 100; 31 | 32 | double total_msc_x = 0.0; 33 | double total_msc_u = 0.0; 34 | double total_tv_x = 0.0; 35 | double total_tv_u = 0.0; 36 | double msc_x = 0.0; 37 | double msc_u = 0.0; 38 | double tv_x = 0.0; 39 | double tv_u = 0.0; 40 | 41 | std::cout << "Log-MPPI (" << sim_maxiter << " simulations)" << std::endl; 42 | std::cout << "iter_duration\titer\tfinal_error\tfailed\tmsc_x\t\tmsc_u\t\ttv_x\t\ttv_u" << std::endl; 43 | 44 | for (int t = 0; t < sim_maxiter; ++t) { 45 | is_failed = false; 46 | // Log_MPPI 47 | LogMPPI log_mppi(model); 48 | MPPIParam log_mppi_param1; 49 | log_mppi_param1.Nu = 300; 50 | log_mppi_param1.gamma_u = 100.0; 51 | Eigen::VectorXd log_mppi_sigma_u(model.dim_u); 52 | log_mppi_sigma_u << 0.1, 0.1; 53 | log_mppi_param1.sigma_u = log_mppi_sigma_u.asDiagonal(); 54 | log_mppi.init(log_mppi_param1); 55 | log_mppi.setCollisionChecker(&collision_checker); 56 | 57 | double log_mppi_duration = 0.0; 58 | int i; 59 | for (i = 0; i < max_iter; ++i) { 60 | // Log_MPPI 61 | auto start = std::chrono::high_resolution_clock::now(); 62 | log_mppi.solve(); 63 | auto finish = std::chrono::high_resolution_clock::now(); 64 | std::chrono::duration elapsed = finish - start; 65 | log_mppi_duration += elapsed.count(); 66 | 67 | res_X = log_mppi.getResX(); 68 | res_U = log_mppi.getResU(); 69 | if ((final_state - res_X.col(model.N)).norm() < 0.1) { 70 | bool is_collision = false; 71 | for (int j = 0; j < model.N; ++j) { 72 | if (collision_checker.getCollisionGrid(res_X.col(j))) { 73 | is_collision = true; 74 | break; 75 | } 76 | } 77 | if (!is_collision) { 78 | break; 79 | } 80 | } 81 | if (i + 1 == max_iter) { 82 | is_failed = true; 83 | } 84 | } 85 | if (!is_failed) { 86 | total_duration += log_mppi_duration; 87 | msc_x = meanSquaredCurvature(res_X); 88 | msc_u = meanSquaredCurvature(res_U); 89 | tv_x = totalVariation(res_X); 90 | tv_u = totalVariation(res_U); 91 | } 92 | else {fail++;} 93 | double fs_error = (final_state - res_X.col(model.N)).norm(); 94 | total_msc_x += msc_x; 95 | total_msc_u += msc_u; 96 | total_tv_x += tv_x; 97 | total_tv_u += tv_u; 98 | std::cout << std::fixed << std::setprecision(6); 99 | std::cout.fill('0'); 100 | std::cout.width(8); 101 | std::cout< 12 | 13 | int main(int argc, char* argv[]) { 14 | std::string target; 15 | int sim_maxiter; 16 | 17 | if (argc > 2) { 18 | target = argv[1]; 19 | sim_maxiter = std::stoi(argv[2]); 20 | } 21 | else { 22 | std::cerr << "Error: No target specified.\n"; 23 | std::cerr << "Usage: " << argv[0] << " " << " \n"; 24 | return 1; 25 | } 26 | 27 | // Model 28 | auto model = WMRobot(); 29 | 30 | // PARAMETERS // PARAMETERS // PARAMETERS // PARAMETERS // 31 | // MPPI 32 | MPPIParam mppi_param; 33 | 34 | // Log_MPPI 35 | MPPIParam log_mppi_param; 36 | 37 | // Smooth_MPPI 38 | MPPIParam smooth_mppi_param1; 39 | SmoothMPPIParam smooth_mppi_param2; 40 | 41 | //MPPI_IPDDP 42 | MPPIParam mi_mppi_param; 43 | CorridorParam corridor_param; 44 | // Corridor Parameter 45 | corridor_param.max_iter = 3; 46 | corridor_param.Nz = 1000; 47 | corridor_param.gamma_z = 1000.0; 48 | Eigen::VectorXd sigma_z(model.center_point + 1); 49 | sigma_z << 0.3, 0.3, 0.1; 50 | corridor_param.sigma_z = sigma_z.asDiagonal(); 51 | corridor_param.lambda_c = 35.0; 52 | corridor_param.lambda_r = 35.0; 53 | corridor_param.r_max = 0.5; 54 | 55 | Param ipddp_param; 56 | // IPDDP Parameter 57 | ipddp_param.tolerance = 1e-7; 58 | ipddp_param.max_iter = 100; 59 | ipddp_param.mu = 0.01; 60 | ipddp_param.infeasible = true; 61 | ipddp_param.q = 0.001; 62 | // PARAMETERS // PARAMETERS // PARAMETERS // PARAMETERS // 63 | 64 | // Collision Checker 65 | CollisionChecker collision_checker; 66 | collision_checker.addRectangle(-2.5, 2.0, 3.0, 2.0); 67 | collision_checker.addRectangle(1.0, 2.0, 3.0, 2.0); 68 | collision_checker.addCircle(0.5, 1.0, 0.25); 69 | 70 | clock_t start; 71 | clock_t finish; 72 | 73 | Eigen::VectorXd final_state(model.dim_x); 74 | final_state << 0.0, 6.0, M_PI_2; 75 | Eigen::MatrixXd res_X, res_U; 76 | double max_sim_duration = 10.0; 77 | bool is_failed; 78 | 79 | // 100 200 400 800 1600 3200 6400 12800 25600 80 | std::vector N; 81 | for (int n = 0; n < 9; ++n) { 82 | N.push_back(100 * std::pow(2, n)); 83 | } 84 | std::vector SIGMA_U; 85 | for (int n = 1; n < 10; ++n) { 86 | SIGMA_U.push_back(0.1 * n); 87 | } 88 | 89 | std::cout<<"Target = "< elapsed = finish - start; 180 | iter_duration += elapsed.count(); 181 | res_X = mppi.getResX(); 182 | res_U = mppi.getResU(); 183 | } 184 | else if (target == "Log-MPPI") { 185 | auto start = std::chrono::high_resolution_clock::now(); 186 | log_mppi.solve(); 187 | auto finish = std::chrono::high_resolution_clock::now(); 188 | std::chrono::duration elapsed = finish - start; 189 | iter_duration += elapsed.count(); 190 | res_X = log_mppi.getResX(); 191 | res_U = log_mppi.getResU(); 192 | } 193 | else if (target == "Smooth-MPPI") { 194 | auto start = std::chrono::high_resolution_clock::now(); 195 | smooth_mppi.solve(); 196 | auto finish = std::chrono::high_resolution_clock::now(); 197 | std::chrono::duration elapsed = finish - start; 198 | iter_duration += elapsed.count(); 199 | res_X = smooth_mppi.getResX(); 200 | res_U = smooth_mppi.getResU(); 201 | } 202 | else if (target == "MPPI-IPDDP") { 203 | mppi_ipddp.solve(1); 204 | iter_duration += mppi_ipddp.mppi_duration+mppi_ipddp.corridor_duration+mppi_ipddp.ipddp_duration; 205 | res_X = mppi_ipddp.X; 206 | res_U = mppi_ipddp.U; 207 | } 208 | 209 | if (max_sim_duration < iter_duration) { 210 | is_failed = true; 211 | break; 212 | } 213 | 214 | if ((final_state - res_X.col(model.N)).norm() < 0.1) { 215 | bool is_collision = false; 216 | for (int j = 0; j < model.N; ++j) { 217 | if (collision_checker.getCollisionGrid(res_X.col(j))) { 218 | is_collision = true; 219 | break; 220 | } 221 | } 222 | if (!is_collision) { 223 | break; 224 | } 225 | } 226 | } 227 | if (!is_failed) { 228 | total_duration += iter_duration; 229 | min_duration = std::min(min_duration, iter_duration); 230 | max_duration = std::max(max_duration, iter_duration); 231 | msc_x = meanSquaredCurvature(res_X); 232 | msc_u = meanSquaredCurvature(res_U); 233 | tv_x = totalVariation(res_X); 234 | tv_u = totalVariation(res_U); 235 | } 236 | else {fail++;} 237 | double fs_error = (final_state - res_X.col(model.N)).norm(); 238 | total_msc_x += msc_x; 239 | total_msc_u += msc_u; 240 | total_tv_x += tv_x; 241 | total_tv_u += tv_u; 242 | } 243 | std::cout << std::fixed << std::setprecision(2); 244 | // std::cout.fill(' '); 245 | // std::cout.width(8); 246 | int success = std::max(1, sim_maxiter - fail); 247 | std::cout< 9 | 10 | int main() { 11 | // Model 12 | auto model = WMRobot(); 13 | 14 | // Collision Checker 15 | CollisionChecker collision_checker; 16 | collision_checker.addRectangle(-2.5, 2.0, 3.0, 2.0); 17 | collision_checker.addRectangle(1.0, 2.0, 3.0, 2.0); 18 | collision_checker.addCircle(0.5, 1.0, 0.25); 19 | 20 | clock_t start; 21 | clock_t finish; 22 | 23 | Eigen::VectorXd final_state(model.dim_x); 24 | final_state << 0.0, 6.0, M_PI_2; 25 | Eigen::MatrixXd res_X, res_U; 26 | int max_iter = 10000; 27 | double total_duration = 0.0; 28 | bool is_failed; 29 | int fail = 0; 30 | int sim_maxiter = 100; 31 | 32 | double total_msc_x = 0.0; 33 | double total_msc_u = 0.0; 34 | double total_tv_x = 0.0; 35 | double total_tv_u = 0.0; 36 | double msc_x = 0.0; 37 | double msc_u = 0.0; 38 | double tv_x = 0.0; 39 | double tv_u = 0.0; 40 | 41 | std::cout << "MPPI (" << sim_maxiter << " simulations)" << std::endl; 42 | std::cout << "iter_duration\titer\tfinal_error\tfailed\tmsc_x\t\tmsc_u\t\ttv_x\t\ttv_u" << std::endl; 43 | 44 | for (int t = 0; t < sim_maxiter; ++t) { 45 | is_failed = false; 46 | // MPPI 47 | MPPI mppi(model); 48 | MPPIParam mppi_param; 49 | mppi_param.Nu = 300; 50 | mppi_param.gamma_u = 100.0; 51 | Eigen::VectorXd sigma_u(model.dim_u); 52 | sigma_u << 0.3, 0.3; 53 | mppi_param.sigma_u = sigma_u.asDiagonal(); 54 | mppi.init(mppi_param); 55 | mppi.setCollisionChecker(&collision_checker); 56 | 57 | double mppi_duration = 0.0; 58 | int i; 59 | for (i = 0; i < max_iter; ++i) { 60 | // MPPI 61 | auto start = std::chrono::high_resolution_clock::now(); 62 | mppi.solve(); 63 | auto finish = std::chrono::high_resolution_clock::now(); 64 | std::chrono::duration elapsed = finish - start; 65 | mppi_duration += elapsed.count(); 66 | 67 | res_X = mppi.getResX(); 68 | res_U = mppi.getResU(); 69 | if ((final_state - res_X.col(model.N)).norm() < 0.1) { 70 | bool is_collision = false; 71 | for (int j = 0; j < model.N; ++j) { 72 | if (collision_checker.getCollisionGrid(res_X.col(j))) { 73 | is_collision = true; 74 | break; 75 | } 76 | } 77 | if (!is_collision) { 78 | break; 79 | } 80 | } 81 | if (i + 1 == max_iter) { 82 | is_failed = true; 83 | } 84 | } 85 | if (!is_failed) { 86 | total_duration += mppi_duration; 87 | msc_x = meanSquaredCurvature(res_X); 88 | msc_u = meanSquaredCurvature(res_U); 89 | tv_x = totalVariation(res_X); 90 | tv_u = totalVariation(res_U); 91 | } 92 | else {fail++;} 93 | double fs_error = (final_state - res_X.col(model.N)).norm(); 94 | total_msc_x += msc_x; 95 | total_msc_u += msc_u; 96 | total_tv_x += tv_x; 97 | total_tv_u += tv_u; 98 | std::cout << std::fixed << std::setprecision(6); 99 | std::cout.fill('0'); 100 | std::cout.width(8); 101 | std::cout< 9 | 10 | int main() { 11 | // Model 12 | auto model = WMRobot(); 13 | 14 | // Collision Checker 15 | CollisionChecker collision_checker; 16 | collision_checker.addRectangle(-2.5, 2.0, 3.0, 2.0); 17 | collision_checker.addRectangle(1.0, 2.0, 3.0, 2.0); 18 | collision_checker.addCircle(0.5, 1.0, 0.25); 19 | 20 | clock_t start; 21 | clock_t finish; 22 | 23 | Eigen::VectorXd final_state(model.dim_x); 24 | final_state << 0.0, 6.0, M_PI_2; 25 | Eigen::MatrixXd res_X, res_U; 26 | int max_iter = 100; 27 | double total_duration = 0.0; 28 | bool is_failed; 29 | int fail = 0; 30 | int sim_maxiter = 100; 31 | 32 | double total_msc_x = 0.0; 33 | double total_msc_u = 0.0; 34 | double total_tv_x = 0.0; 35 | double total_tv_u = 0.0; 36 | double msc_x = 0.0; 37 | double msc_u = 0.0; 38 | double tv_x = 0.0; 39 | double tv_u = 0.0; 40 | 41 | std::cout << "MPPI-IPDDP (" << sim_maxiter << " simulations)" << std::endl; 42 | std::cout << "iter_duration\titer\tfinal_error\tfailed\tmsc_x\t\tmsc_u\t\ttv_x\t\ttv_u" << std::endl; 43 | 44 | for (int t = 0; t < sim_maxiter; ++t) { 45 | is_failed = false; 46 | // MPPI Parameter 47 | MPPIParam mppi_param; 48 | mppi_param.Nu = 300; 49 | mppi_param.gamma_u = 100.0; 50 | Eigen::VectorXd sigma_u(model.dim_u); 51 | sigma_u << 0.5, 0.5; 52 | mppi_param.sigma_u = sigma_u.asDiagonal(); 53 | 54 | // Corridor Parameter 55 | CorridorParam corridor_param; 56 | corridor_param.max_iter = 3; 57 | corridor_param.Nz = 1000; 58 | corridor_param.gamma_z = 1000.0; 59 | Eigen::VectorXd sigma_z(model.center_point + 1); 60 | sigma_z << 0.3, 0.3, 0.1; 61 | corridor_param.sigma_z = sigma_z.asDiagonal(); 62 | corridor_param.lambda_c = 35.0; 63 | corridor_param.lambda_r = 35.0; 64 | corridor_param.r_max = 0.5; 65 | 66 | // IPDDP Parameter 67 | Param ipddp_param; 68 | ipddp_param.tolerance = 1e-7; 69 | ipddp_param.max_iter = 100; 70 | ipddp_param.mu = 0.01; 71 | ipddp_param.infeasible = true; 72 | ipddp_param.q = 0.001; 73 | 74 | // MPPI_IPDDP 75 | MPPI_IPDDP mppi_ipddp(model); 76 | mppi_ipddp.init(mppi_param, corridor_param, ipddp_param); 77 | mppi_ipddp.setCollisionChecker(&collision_checker); 78 | 79 | double mppi_ipddp_duration = 0.0; 80 | double mppi_ipddp_duration1 = 0.0; 81 | double mppi_ipddp_duration2 = 0.0; 82 | double mppi_ipddp_duration3 = 0.0; 83 | int i; 84 | for (i = 0; i < max_iter; ++i) { 85 | // MPPI_IPDDP 86 | mppi_ipddp.solve(1); 87 | mppi_ipddp_duration1 += mppi_ipddp.mppi_duration; 88 | mppi_ipddp_duration2 += mppi_ipddp.corridor_duration; 89 | mppi_ipddp_duration3 += mppi_ipddp.ipddp_duration; 90 | 91 | res_X = mppi_ipddp.X; 92 | res_U = mppi_ipddp.U; 93 | if ((final_state - res_X.col(model.N)).norm() < 0.1) { 94 | bool is_collision = false; 95 | for (int j = 0; j < model.N; ++j) { 96 | if (collision_checker.getCollisionGrid(res_X.col(j))) { 97 | is_collision = true; 98 | break; 99 | } 100 | } 101 | if (!is_collision) { 102 | break; 103 | } 104 | } 105 | if (i + 1 == max_iter) { 106 | is_failed = true; 107 | } 108 | } 109 | if (!is_failed) { 110 | mppi_ipddp_duration = mppi_ipddp_duration1 + mppi_ipddp_duration2 + mppi_ipddp_duration3; 111 | total_duration += mppi_ipddp_duration; 112 | msc_x = meanSquaredCurvature(res_X); 113 | msc_u = meanSquaredCurvature(res_U); 114 | tv_x = totalVariation(res_X); 115 | tv_u = totalVariation(res_U); 116 | } 117 | else {fail++;} 118 | double fs_error = (final_state - res_X.col(model.N)).norm(); 119 | total_msc_x += msc_x; 120 | total_msc_u += msc_u; 121 | total_tv_x += tv_x; 122 | total_tv_u += tv_u; 123 | std::cout << std::fixed << std::setprecision(6); 124 | std::cout.fill('0'); 125 | std::cout.width(8); 126 | std::cout< 8 | 9 | int main() { 10 | // Model 11 | auto model = WMRobot(); 12 | 13 | // Collision Checker 14 | CollisionChecker collision_checker; 15 | // collision_checker.addRectangle(-2.5, 2.0, 3.0, 2.0); 16 | // collision_checker.addRectangle(1.0, 2.0, 3.0, 2.0); 17 | // collision_checker.addCircle(0.5, 1.0, 0.25); 18 | collision_checker.addCircle(0.0, 1.0, 0.3); 19 | collision_checker.addCircle(-0.85, 3.5, 1.6); 20 | collision_checker.addCircle(2.0, 3.5, 1.0); 21 | 22 | clock_t start; 23 | clock_t finish; 24 | 25 | Eigen::VectorXd final_state(model.dim_x); 26 | final_state << 0.0, 6.0, M_PI_2; 27 | Eigen::MatrixXd res_X = model.X.col(0); 28 | Eigen::MatrixXd res_U = model.U.col(0); 29 | int max_iter = 100; 30 | double total_duration = 0.0; 31 | bool is_failed; 32 | int fail = 0; 33 | int sim_maxiter = 1; 34 | 35 | double total_msc_x = 0.0; 36 | double total_msc_u = 0.0; 37 | double total_tv_x = 0.0; 38 | double total_tv_u = 0.0; 39 | double msc_x = 0.0; 40 | double msc_u = 0.0; 41 | double tv_x = 0.0; 42 | double tv_u = 0.0; 43 | 44 | std::cout << "MPPI-IPDDP (" << sim_maxiter << " simulations)" << std::endl; 45 | std::cout << "iter_duration\titer\tfinal_error\tfailed\tmsc_x\t\tmsc_u\t\ttv_x\t\ttv_u" << std::endl; 46 | 47 | for (int t = 0; t < sim_maxiter; ++t) { 48 | is_failed = false; 49 | // MPPI Parameter 50 | MPPIParam mppi_param; 51 | mppi_param.Nu = 300; 52 | mppi_param.gamma_u = 100.0; 53 | Eigen::VectorXd sigma_u(model.dim_u); 54 | sigma_u << 0.5, 0.5; 55 | mppi_param.sigma_u = sigma_u.asDiagonal(); 56 | 57 | // Corridor Parameter 58 | CorridorParam corridor_param; 59 | corridor_param.max_iter = 3; 60 | corridor_param.Nz = 1000; 61 | corridor_param.gamma_z = 1000.0; 62 | Eigen::VectorXd sigma_z(model.center_point + 1); 63 | sigma_z << 0.3, 0.3, 0.1; 64 | corridor_param.sigma_z = sigma_z.asDiagonal(); 65 | corridor_param.lambda_c = 35.0; 66 | corridor_param.lambda_r = 35.0; 67 | corridor_param.r_max = 0.5; 68 | 69 | // IPDDP Parameter 70 | Param ipddp_param; 71 | ipddp_param.tolerance = 1e-7; 72 | ipddp_param.max_iter = 100; 73 | ipddp_param.mu = 0.01; 74 | ipddp_param.infeasible = true; 75 | ipddp_param.q = 0.001; 76 | 77 | // MPPI_IPDDP 78 | MPPI_IPDDP mppi_ipddp(model); 79 | mppi_ipddp.init(mppi_param, corridor_param, ipddp_param); 80 | mppi_ipddp.setCollisionChecker(&collision_checker); 81 | 82 | double mppi_ipddp_duration = 0.0; 83 | double mppi_ipddp_duration1 = 0.0; 84 | double mppi_ipddp_duration2 = 0.0; 85 | double mppi_ipddp_duration3 = 0.0; 86 | int i; 87 | for (i = 0; i < max_iter; ++i) { 88 | // MPPI_IPDDP 89 | mppi_ipddp.solve(1); 90 | 91 | res_X.conservativeResize(res_X.rows(), res_X.cols()+1); 92 | res_X.col(res_X.cols()-1) = mppi_ipddp.X.col(0); 93 | res_U.conservativeResize(res_U.rows(), res_U.cols()+1); 94 | res_U.col(res_U.cols()-1) = mppi_ipddp.U.col(0); 95 | std::cout< 9 | 10 | int main() { 11 | // Model 12 | auto model = WMRobot(); 13 | 14 | // Collision Checker 15 | CollisionChecker collision_checker; 16 | collision_checker.addRectangle(-2.5, 2.0, 3.0, 2.0); 17 | collision_checker.addRectangle(1.0, 2.0, 3.0, 2.0); 18 | collision_checker.addCircle(0.5, 1.0, 0.25); 19 | 20 | clock_t start; 21 | clock_t finish; 22 | 23 | Eigen::VectorXd final_state(model.dim_x); 24 | final_state << 0.0, 6.0, M_PI_2; 25 | Eigen::MatrixXd res_X, res_U; 26 | int max_iter = 10000; 27 | double total_duration = 0.0; 28 | bool is_failed; 29 | int fail = 0; 30 | int sim_maxiter = 100; 31 | 32 | double total_msc_x = 0.0; 33 | double total_msc_u = 0.0; 34 | double total_tv_x = 0.0; 35 | double total_tv_u = 0.0; 36 | double msc_x = 0.0; 37 | double msc_u = 0.0; 38 | double tv_x = 0.0; 39 | double tv_u = 0.0; 40 | 41 | std::cout << "Smooth-MPPI (" << sim_maxiter << " simulations)" << std::endl; 42 | std::cout << "iter_duration\titer\tfinal_error\tfailed\tmsc_x\t\tmsc_u\t\ttv_x\t\ttv_u" << std::endl; 43 | 44 | for (int t = 0; t < sim_maxiter; ++t) { 45 | is_failed = false; 46 | // Smooth_MPPI 47 | SmoothMPPI smooth_mppi(model); 48 | MPPIParam smooth_mppi_param1; 49 | smooth_mppi_param1.Nu = 300; 50 | smooth_mppi_param1.gamma_u = 10.0; 51 | Eigen::VectorXd sigma_u_1(model.dim_u); 52 | sigma_u_1 << 0.3, 0.3; 53 | smooth_mppi_param1.sigma_u = sigma_u_1.asDiagonal(); 54 | SmoothMPPIParam smooth_mppi_param2; 55 | smooth_mppi_param2.dt = 1.0; 56 | smooth_mppi_param2.lambda = 15.0; 57 | Eigen::VectorXd w(model.dim_u); 58 | w << 0.8, 0.8; 59 | smooth_mppi_param2.w = w.asDiagonal(); 60 | 61 | smooth_mppi.init2(smooth_mppi_param1, smooth_mppi_param2); 62 | smooth_mppi.setCollisionChecker(&collision_checker); 63 | 64 | double smooth_mppi_duration = 0.0; 65 | int i; 66 | for (i = 0; i < max_iter; ++i) { 67 | // Smooth_MPPI 68 | auto start = std::chrono::high_resolution_clock::now(); 69 | smooth_mppi.solve(); 70 | auto finish = std::chrono::high_resolution_clock::now(); 71 | std::chrono::duration elapsed = finish - start; 72 | smooth_mppi_duration += elapsed.count(); 73 | 74 | res_X = smooth_mppi.getResX(); 75 | res_U = smooth_mppi.getResU(); 76 | if ((final_state - res_X.col(model.N)).norm() < 0.1) { 77 | bool is_collision = false; 78 | for (int j = 0; j < model.N; ++j) { 79 | if (collision_checker.getCollisionGrid(res_X.col(j))) { 80 | is_collision = true; 81 | break; 82 | } 83 | } 84 | if (!is_collision) { 85 | break; 86 | } 87 | break; 88 | } 89 | if (i + 1 == max_iter) { 90 | is_failed = true; 91 | } 92 | } 93 | if (!is_failed) { 94 | total_duration += smooth_mppi_duration; 95 | msc_x = meanSquaredCurvature(res_X); 96 | msc_u = meanSquaredCurvature(res_U); 97 | tv_x = totalVariation(res_X); 98 | tv_u = totalVariation(res_U); 99 | } 100 | else {fail++;} 101 | double fs_error = (final_state - res_X.col(model.N)).norm(); 102 | total_msc_x += msc_x; 103 | total_msc_u += msc_u; 104 | total_tv_x += tv_x; 105 | total_tv_u += tv_u; 106 | std::cout << std::fixed << std::setprecision(6); 107 | std::cout.fill('0'); 108 | std::cout.width(8); 109 | std::cout< 12 | 13 | int main(int argc, char* argv[]) { 14 | std::string target; 15 | 16 | if (argc > 1) { 17 | target = argv[1]; 18 | } 19 | else { 20 | std::cerr << "Error: No target specified.\n"; 21 | std::cerr << "Usage: " << argv[0] << " \n"; 22 | return 1; 23 | } 24 | 25 | // Model 26 | auto model = WMRobotMap(); 27 | 28 | // PARAMETERS // PARAMETERS // PARAMETERS // PARAMETERS // 29 | // MPPI 30 | MPPIParam mppi_param; 31 | 32 | // Log_MPPI 33 | MPPIParam log_mppi_param; 34 | 35 | // Smooth_MPPI 36 | MPPIParam smooth_mppi_param1; 37 | SmoothMPPIParam smooth_mppi_param2; 38 | 39 | //MPPI_IPDDP 40 | MPPIParam mi_mppi_param; 41 | CorridorParam corridor_param; 42 | // Corridor Parameter 43 | corridor_param.max_iter = 3; 44 | corridor_param.Nz = 1000; 45 | corridor_param.gamma_z = 1000.0; 46 | Eigen::VectorXd sigma_z(model.center_point + 1); 47 | sigma_z << 0.3, 0.3, 0.1; 48 | corridor_param.sigma_z = sigma_z.asDiagonal(); 49 | corridor_param.lambda_c = 35.0; 50 | corridor_param.lambda_r = 35.0; 51 | corridor_param.r_max = 0.5; 52 | 53 | Param ipddp_param; 54 | // IPDDP Parameter 55 | ipddp_param.tolerance = 1e-7; 56 | ipddp_param.max_iter = 100; 57 | ipddp_param.mu = 0.01; 58 | ipddp_param.infeasible = true; 59 | ipddp_param.q = 0.001; 60 | // PARAMETERS // PARAMETERS // PARAMETERS // PARAMETERS // 61 | 62 | clock_t start; 63 | clock_t finish; 64 | 65 | Eigen::VectorXd final_state(model.dim_x); 66 | final_state << 1.5, 5.0, M_PI_2; 67 | Eigen::MatrixXd res_X, res_U; 68 | double max_sim_duration = 1.0; 69 | bool is_failed; 70 | 71 | std::cout<<"Target = "< elapsed = finish - start; 162 | iter_duration += elapsed.count(); 163 | res_X = mppi.getResX(); 164 | res_U = mppi.getResU(); 165 | } 166 | else if (target == "Log-MPPI") { 167 | auto start = std::chrono::high_resolution_clock::now(); 168 | log_mppi.solve(); 169 | auto finish = std::chrono::high_resolution_clock::now(); 170 | std::chrono::duration elapsed = finish - start; 171 | iter_duration += elapsed.count(); 172 | res_X = log_mppi.getResX(); 173 | res_U = log_mppi.getResU(); 174 | } 175 | else if (target == "Smooth-MPPI") { 176 | auto start = std::chrono::high_resolution_clock::now(); 177 | smooth_mppi.solve(); 178 | auto finish = std::chrono::high_resolution_clock::now(); 179 | std::chrono::duration elapsed = finish - start; 180 | iter_duration += elapsed.count(); 181 | res_X = smooth_mppi.getResX(); 182 | res_U = smooth_mppi.getResU(); 183 | } 184 | else if (target == "MPPI-IPDDP") { 185 | mppi_ipddp.solve(1); 186 | iter_duration += mppi_ipddp.mppi_duration+mppi_ipddp.corridor_duration+mppi_ipddp.ipddp_duration; 187 | res_X = mppi_ipddp.X; 188 | res_U = mppi_ipddp.U; 189 | } 190 | 191 | if (max_sim_duration < iter_duration) { 192 | is_failed = true; 193 | break; 194 | } 195 | 196 | if ((final_state - res_X.col(model.N)).norm() < 0.1) { 197 | bool is_collision = false; 198 | for (int j = 0; j < model.N; ++j) { 199 | if (collision_checker.getCollisionGrid(res_X.col(j))) { 200 | is_collision = true; 201 | break; 202 | } 203 | } 204 | if (!is_collision) { 205 | break; 206 | } 207 | } 208 | } 209 | if (!is_failed) { 210 | total_duration += iter_duration; 211 | min_duration = std::min(min_duration, iter_duration); 212 | max_duration = std::max(max_duration, iter_duration); 213 | msc_x = meanSquaredCurvature(res_X); 214 | msc_u = meanSquaredCurvature(res_U); 215 | tv_x = totalVariation(res_X); 216 | tv_u = totalVariation(res_U); 217 | } 218 | else {fail++;} 219 | double fs_error = (final_state - res_X.col(model.N)).norm(); 220 | total_msc_x += msc_x; 221 | total_msc_u += msc_u; 222 | total_tv_x += tv_x; 223 | total_tv_u += tv_u; 224 | std::cout << std::fixed << std::setprecision(6); 225 | std::cout.fill('0'); 226 | std::cout.width(8); 227 | std::cout<