├── .gitattributes ├── .gitignore ├── README.md ├── cpp_implementation ├── CMakeLists.txt ├── Pure_Pursuit │ ├── PurePursuit.cpp │ ├── PurePursuit.h │ └── main.cpp ├── Rear_Wheel_Feedback │ ├── RearWheelFeedback.cpp │ ├── RearWheelFeedback.h │ └── main.cpp ├── Stanley │ ├── Stanley.cpp │ ├── Stanley.h │ └── main.cpp └── utils │ ├── ReferenceLine.cpp │ ├── ReferenceLine.h │ ├── VehicleModel.cpp │ ├── VehicleModel.h │ └── matplotlibcpp.h ├── model ├── 横向LQR │ ├── lqr_demo.slx │ ├── lqr_demo.slxc │ ├── lqr_offline.m │ └── routing_planning.m ├── 横纵向联合 │ ├── lqr+pid.mat │ ├── planning_control.slx │ └── planning_control.slxc └── 纵向PID │ ├── biaoding.mat │ ├── brake_calibration.m │ ├── generate_calibration.m │ ├── pid_demo.slx │ ├── pid_demo.slxc │ ├── test2.m │ └── thr_calibration.m ├── modeling_process ├── PID纵向控制.md ├── VS Visualizer - CarSim - Baseline __ Quick Start Guide Example_ 2022-11-03 22-29-50.mp4 ├── 横向LQR.md └── 横纵向综合控制.md └── note ├── notebook ├── 自动驾驶控制算法第一讲2.pdf ├── 自动驾驶控制算法第七讲2.pdf ├── 自动驾驶控制算法第三讲2.pdf ├── 自动驾驶控制算法第九讲.pdf ├── 自动驾驶控制算法第二讲2.pdf ├── 自动驾驶控制算法第五讲2.pdf ├── 自动驾驶控制算法第八讲(四).pdf ├── 自动驾驶控制算法第八讲4.pdf ├── 自动驾驶控制算法第六讲2.pdf ├── 自动驾驶控制算法第十二讲(完结).pdf ├── 自动驾驶控制算法第十讲.pdf └── 自动驾驶控制算法第四讲2.pdf ├── 横向控制.md ├── 纵向控制.md ├── 转向相关知识点.md └── 运动学+动力学模型.md /.gitattributes: -------------------------------------------------------------------------------- 1 | # Auto detect text files and perform LF normalization 2 | * text=auto 3 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | /cpp_implementation/build -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # 简介 2 | 3 | 自动驾驶的控制算法实现。 4 | 5 | 6 | 7 | **TO_DO**： 8 | 9 | 后续计划使用C++ 通过carla-ros-bridge在carla上实现。 10 | 11 | 12 | 13 | # 目录 14 | 15 | ## [cpp_implementation](./cpp_implementation) 16 | 17 | 此部分参考[PythonRobotics](https://github.com/AtsushiSakai/PythonRobotics#pythonrobotics),使用C++ 实现了部分横向控制算法。 18 | 19 | ### 20 | 21 | ### 依赖 22 | 23 | 推荐在Ubuntu 18.04/20.04 环境下运行 24 | 25 | - **cmake** 26 | 27 | 在Ubuntu中安装cmake： 28 | 29 | ``` 30 | sudo apt install cmake 31 | ``` 32 | 33 | - **Eigen** 34 | 35 | 在Ubuntu中安装Eigen： 36 | 37 | ``` 38 | sudo apt-get install libeigen3-dev 39 | ``` 40 | 41 | - **python3** 42 | 43 | 44 | 45 | ### 编译 46 | 47 | 在当前目录下输入： 48 | 49 | ```shell 50 | mkdir build 51 | cd build 52 | cmake ../ 53 | make 54 | ``` 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | ### 控制效果图 65 | 66 | 蓝色为参考轨迹 67 | 68 | 红色为车辆行驶轨迹 69 | 70 | 71 | 72 | #### Pure_Pursuit 73 | 74 | ![pure_pursuit_demo](https://gitee.com/czjaixuexi/typora_pictures/raw/master/img/202305180229501.png) 75 | 76 | #### Rear_Wheel_Feedback 77 | 78 | ![rear_demo](https://gitee.com/czjaixuexi/typora_pictures/raw/master/img/202305180229504.png) 79 | 80 | 81 | 82 | #### Stanley 83 | 84 | ![stanley_demo](https://gitee.com/czjaixuexi/typora_pictures/raw/master/img/202305180229505.png) 85 | 86 | 87 | 88 | 89 | 90 | ## [model](./model) 91 | 92 | 此目录存放matlab代码与模型。 93 | 94 | 此部分参考B站老王，使用carsim2019.1与Matlab2020a实现自动驾驶的横向+纵向控制算法。 95 | 96 | 97 | 98 | ## [modeling_process](./modeling_process) 99 | 100 | 此目录存放建模过程笔记： 101 | 102 | 1. [PID纵向控制建模](./modeling_process/PID纵向控制.md) 103 | 2. [LQR横向控制建模](./modeling_process/横向LQR.md) 104 | 3. [横纵向综合控制建模](./modeling_process/横纵向综合控制.md) 105 | 106 | 107 | 108 | 109 | 110 | ## [note](./note) 111 | 112 | 此目录存放横纵向控制相关知识笔记： 113 | 114 | 1. [运动学+动力学模型](./note/运动学+动力学模型.md) 115 | 2. [纵向控制](./note/纵向控制.md) 116 | 3. [横向控制](./note/横向控制.md) 117 | 4. [转向相关知识点](./note/转向相关知识点.md) 118 | -------------------------------------------------------------------------------- /cpp_implementation/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.21) 2 | project(control) 3 | 4 | set(CMAKE_CXX_STANDARD 14) 5 | 6 | 7 | # include(GNUInstallDirs) 8 | set(PACKAGE_NAME control) 9 | 10 | # output 11 | set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}/bin) 12 | 13 | 14 | # 创建control库 15 | add_library(control INTERFACE) 16 | find_package(Python COMPONENTS Interpreter Development NumPy REQUIRED) 17 | target_link_libraries(control INTERFACE 18 | Python::Python 19 | Python::Module 20 | Python::NumPy 21 | ) 22 | install( 23 | TARGETS control 24 | EXPORT install_targets 25 | ) 26 | 27 | 28 | find_package(Eigen3 REQUIRED) 29 | include_directories(${EIGEN3_INCLUDE_DIR}) 30 | 31 | 32 | #pure_pursuit 33 | add_executable(pure_pursuit_demo Pure_Pursuit/main.cpp Pure_Pursuit/PurePursuit.cpp utils/VehicleModel.cpp utils/ReferenceLine.cpp) 34 | target_link_libraries(pure_pursuit_demo PRIVATE control) 35 | 36 | #stanley 37 | add_executable(stanley_demo Stanley/main.cpp Stanley/Stanley.cpp utils/VehicleModel.cpp utils/ReferenceLine.cpp) 38 | target_link_libraries(stanley_demo PRIVATE control) 39 | 40 | 41 | #rear_wheel_feedback 42 | add_executable(rear_wheel_feedback_demo Rear_Wheel_Feedback/main.cpp Rear_Wheel_Feedback/RearWheelFeedback.cpp utils/ReferenceLine.cpp utils/VehicleModel.cpp) 43 | target_link_libraries(rear_wheel_feedback_demo PRIVATE control) -------------------------------------------------------------------------------- /cpp_implementation/Pure_Pursuit/PurePursuit.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file PurePursuit.cpp 3 | * @author czj 4 | * @brief 5 | * @version 0.1 6 | * @date 2023-05-11 7 | * 8 | * @copyright Copyright (c) 2023 9 | * 10 | */ 11 | 12 | #include "PurePursuit.h" 13 | 14 | /** 15 | * @brief 计算车辆后轴与参考点的距离 16 | * 17 | * @param state 车辆状态 18 | * @param refer 参考轨迹 19 | * @param index 参考点下标 20 | * @return double 21 | */ 22 | double PurePursuit::calc_distance(State state, std::vector refer, int index) 23 | { 24 | return std::sqrt(std::pow(state.rear_x - refer[index].x, 2) + std::pow(state.rear_y - refer[index].y, 2)); 25 | } 26 | 27 | 28 | /** 29 | * @brief 寻找预瞄点下标 30 | * 31 | * @param state 车辆状态 32 | * @param referx 参考轨迹 33 | * @param l_d 预瞄距离 34 | * @return double 35 | */ 36 | double PurePursuit::search_target_index(State state, std::vector refer, double l_d) 37 | { 38 | std::vector dists; 39 | double min_index; 40 | //std::cout << old_nearest_point_index << std::endl; 41 | // 寻找匹配点 42 | // 若是第一次搜寻，则遍历所有参考点寻找最小值下标 43 | if (old_nearest_point_index == -1) { 44 | for (int i = 0; i < refer.size();i++) { 45 | double dist = calc_distance(state, refer, i); 46 | dists.push_back(dist); 47 | } 48 | min_index = std::min_element(dists.begin(), dists.end()) - dists.begin(); //返回vector最小元素的下标 49 | old_nearest_point_index = min_index; 50 | } 51 | // 否则直接从上一个匹配点开始查找（找到第一个距离开始增大的点，类似梯度下降） 52 | else { 53 | min_index = old_nearest_point_index; 54 | for (int i = min_index; i < refer.size() - 1; i++) { 55 | double distance_this_index = calc_distance(state, refer, i); 56 | double distance_next_index = calc_distance(state, refer, i + 1); 57 | if (distance_this_index < distance_next_index) { 58 | break; 59 | } 60 | min_index = i + 1; 61 | std::cout << old_nearest_point_index << std::endl; 62 | } 63 | old_nearest_point_index = min_index; 64 | } 65 | // 从匹配点开始寻找预瞄点 66 | while (l_d > calc_distance(state, refer, min_index) && min_index < refer.size()) { 67 | min_index += 1; 68 | } 69 | return min_index; 70 | } 71 | 72 | 73 | /** 74 | * @brief PP控制输出转角 75 | * 76 | * @param state 车辆状态 77 | * @param refer 参考轨迹 78 | * @param min_index 预瞄点下标 79 | * @param l_d 预瞄距离 80 | * @param L 轴距 81 | * @return double 82 | */ 83 | double PurePursuit::pure_pursuit_steer_control(State state, std::vector refer, int min_index, double l_d, double L) 84 | { 85 | double alpha = std::atan2(refer[min_index].y - state.rear_y, refer[min_index].x - state.rear_x) - state.yaw; 86 | double delta = std::atan2(2 * L * std::sin(alpha), l_d); 87 | return delta; 88 | } 89 | 90 | 91 | 92 | 93 | 94 | -------------------------------------------------------------------------------- /cpp_implementation/Pure_Pursuit/PurePursuit.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file PurePursuit.h 3 | * @author czj 4 | * @brief 5 | * @version 0.1 6 | * @date 2023-05-11 7 | * 8 | * @copyright Copyright (c) 2023 9 | * 10 | */ 11 | 12 | #ifndef PP_H 13 | #define PP_H 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include "../utils/VehicleModel.h" 19 | #include "../utils/ReferenceLine.h" 20 | class PurePursuit { 21 | public: 22 | int old_nearest_point_index = -1; // 上一个最近点的index 23 | 24 | public: 25 | double calc_distance(State state, std::vector refer, int index); 26 | 27 | double search_target_index(State state, std::vector refer, double l_d); 28 | 29 | double pure_pursuit_steer_control(State state, std::vector refer, int min_index, double l_d, double L); 30 | }; 31 | 32 | 33 | 34 | #endif //PP_H 35 | -------------------------------------------------------------------------------- /cpp_implementation/Pure_Pursuit/main.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file main.cpp 3 | * @author czj 4 | * @brief 5 | * @version 0.1 6 | * @date 2023-05-13 7 | * 8 | * @copyright Copyright (c) 2023 9 | * 10 | */ 11 | 12 | #include "PurePursuit.h" 13 | #include "../utils/matplotlibcpp.h" 14 | 15 | namespace plt = matplotlibcpp; 16 | 17 | #define PI 3.1415926 18 | 19 | using std::vector; 20 | using std::sin; 21 | using std::cos; 22 | 23 | int main() { 24 | double x = 0.0, y = 0.0, yaw = 0.0, v = 30.0 / 3.6, l = 2.9, dt = 0.1; 25 | // 车辆状态 26 | State state(x, y, yaw, v, l); 27 | // 车辆模型 28 | VehicleModel vehicle(state, dt); 29 | double lam = 0.31; // 前视距离系数 30 | double c = 1.5; // 前视距离 31 | // 生成参考轨迹 32 | ReferenceLine refer_line; 33 | //后面画图用 34 | vector refer_x(refer_line.refer.size()); 35 | vector refer_y(refer_line.refer.size()); 36 | for (int i = 0;i < refer_line.refer.size();i++) { 37 | refer_x[i] = refer_line.refer[i].x; 38 | refer_y[i] = refer_line.refer[i].y; 39 | } 40 | 41 | // 车辆运动轨迹 42 | vector trajectory_x, trajectory_y; 43 | PurePursuit pp; 44 | while (true) { 45 | plt::clf(); 46 | //更新预瞄距离 47 | double l_d = lam * vehicle.state.v + c; 48 | //寻找预瞄点下标 49 | double min_index = pp.search_target_index(vehicle.state, refer_line.refer, l_d); 50 | //std::cout << min_index << std::endl; 51 | //计算转角控制量 52 | double delta = pp.pure_pursuit_steer_control(vehicle.state, refer_line.refer, min_index, l_d, l); 53 | //std::cout << delta << std::endl; 54 | //控制车辆，更新状态 55 | vehicle.updateState(0, delta); 56 | 57 | // 记录车辆运行轨迹，用于绘图 58 | trajectory_x.push_back(vehicle.state.x); 59 | trajectory_y.push_back(vehicle.state.y); 60 | 61 | //std::cout << vehicle.state.x << " " << vehicle.state.y << std::endl; 62 | 63 | //画图 64 | plt::plot(refer_x, refer_y, "b--"); 65 | plt::plot(trajectory_x, trajectory_y, "r"); 66 | plt::grid(true); 67 | plt::ylim(-8, 8); 68 | plt::pause(0.01); 69 | //控制结束跳出 70 | if (pp.calc_distance(vehicle.state, refer_line.refer, refer_line.refer.size() - 1) < 0.5) { 71 | std::cout << "Control completion" << std::endl; 72 | break; 73 | } 74 | } 75 | // 保存图像 76 | const char* filename = "./pure_pursuit_demo.png"; 77 | std::cout << "Saving result to " << filename << std::endl; 78 | plt::save(filename); 79 | plt::show(); 80 | return 0; 81 | } -------------------------------------------------------------------------------- /cpp_implementation/Rear_Wheel_Feedback/RearWheelFeedback.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file RearWheelFeedback.cpp 3 | * @author czj 4 | * @brief 5 | * @version 0.1 6 | * @date 2023-05-15 7 | * 8 | * @copyright Copyright (c) 2023 9 | * 10 | */ 11 | 12 | #include "RearWheelFeedback.h" 13 | 14 | /** 15 | * @brief 计算后轴与参考轨迹某点距离 16 | * 17 | * @param state 车辆状态 18 | * @param refer 参考轨迹 19 | * @param index 轨迹下标 20 | * @return double 21 | */ 22 | double RearWheelFeedback::calc_distance(State state, std::vector refer, int index) 23 | { 24 | return std::sqrt(std::pow(state.rear_x - refer[index].x, 2) + std::pow(state.rear_y - refer[index].y, 2)); 25 | } 26 | 27 | 28 | /** 29 | * @brief 寻找匹配点下标 30 | * @param state 车辆状态 31 | * @param refer 参考轨迹 32 | * @return double 33 | */ 34 | double RearWheelFeedback::search_target_index(State state, std::vector refer) 35 | { 36 | std::vector dists; 37 | double min_index; 38 | //std::cout << old_nearest_point_index << std::endl; 39 | // 寻找匹配点 40 | // 若是第一次搜寻，则遍历所有参考点寻找最小值下标 41 | if (old_nearest_point_index == -1) { 42 | for (int i = 0; i < refer.size();i++) { 43 | double dist = calc_distance(state, refer, i); 44 | dists.push_back(dist); 45 | } 46 | min_index = std::min_element(dists.begin(), dists.end()) - dists.begin(); //返回vector最小元素的下标 47 | old_nearest_point_index = min_index; 48 | } 49 | // 否则直接从上一个匹配点开始查找（找到第一个距离开始增大的点，类似梯度下降） 50 | else { 51 | min_index = old_nearest_point_index; 52 | for (int i = min_index; i < refer.size() - 1; i++) { 53 | double distance_this_index = calc_distance(state, refer, i); 54 | double distance_next_index = calc_distance(state, refer, i + 1); 55 | if (distance_this_index < distance_next_index) { 56 | break; 57 | } 58 | min_index = i + 1; 59 | //std::cout << old_nearest_point_index << std::endl; 60 | } 61 | old_nearest_point_index = min_index; 62 | } 63 | return min_index; 64 | } 65 | 66 | void RearWheelFeedback::compute_errors(State state, std::vector refer, double& e_y, double& theta_e, double& cur) 67 | { 68 | int match_index = search_target_index(state, refer); // 获取匹配点下标 69 | std::cout << "匹配点" << match_index << std::endl; 70 | if (match_index >= refer.size()) { 71 | match_index = refer.size() - 1; 72 | } 73 | cur = refer[match_index].cur; //匹配点曲率 74 | e_y = calc_distance(state, refer, match_index); //计算横向误差 75 | 76 | // 将位置误差转换为前轮转角的时候：需要将路径上距离车辆最近的点从世界坐标系变换到车辆坐标系下，根据路径点在车辆坐标系下的横坐标的正负决定前轮转角的方向 77 | double match_point_y_in_vehicle_coordinate = -(refer[match_index].x - state.x) * std::sin(state.yaw) + 78 | (refer[match_index].y - state.y) * std::cos(state.yaw); 79 | // 车辆坐标系：X轴沿着车辆纵向，向前为正，Y沿着车辆横向，向左为正（从车头往前看的视角） 80 | if (match_point_y_in_vehicle_coordinate > 0) 81 | { 82 | e_y = -e_y; 83 | } 84 | else if (match_point_y_in_vehicle_coordinate < 0) { 85 | e_y = e_y; 86 | } 87 | theta_e = normalize_angle(state.yaw - refer[match_index].heading); // 航向角误差 88 | } 89 | 90 | 91 | /** 92 | * @brief 角度归一化 93 | * 94 | * @param angle 95 | * @return double 96 | */ 97 | double RearWheelFeedback::normalize_angle(double angle) { 98 | while (angle > PI) { 99 | angle -= 2.0 * PI; 100 | } 101 | while (angle < -PI) { 102 | angle += 2.0 * PI; 103 | } 104 | return angle; 105 | } 106 | 107 | double RearWheelFeedback::rear_wheel_feedback_control(State state, std::vector refer, double K_psi, double K2) 108 | { 109 | double e_y; 110 | double theta_e; 111 | double cur; 112 | // 计算误差 113 | compute_errors(state, refer, e_y, theta_e, cur); 114 | // 由李雅普诺夫方程求得角速度 115 | double psi_dot = state.v * cur * std::cos(theta_e) / (1.0 - cur * e_y) - 116 | K2 * state.v * e_y * std::sin(theta_e) / theta_e - K_psi * std::abs(state.v) * theta_e; 117 | 118 | if (theta_e == 0.0 || psi_dot == 0.0) { 119 | return 0.0; 120 | } 121 | double delta = std::atan2(state.l * psi_dot, state.v); 122 | std::cout << "控制量:" << delta << std::endl; 123 | return delta; 124 | } 125 | 126 | 127 | 128 | 129 | -------------------------------------------------------------------------------- /cpp_implementation/Rear_Wheel_Feedback/RearWheelFeedback.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file RearWheelFeedback.h 3 | * @author czj 4 | * @brief 5 | * @version 0.1 6 | * @date 2023-05-14 7 | * 8 | * @copyright Copyright (c) 2023 9 | * 10 | */ 11 | 12 | #ifndef RW_H 13 | #define RW_H 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include "../utils/ReferenceLine.h" 19 | #include "../utils/VehicleModel.h" 20 | 21 | #define PI 3.1415926 22 | 23 | class RearWheelFeedback { 24 | private: 25 | int old_nearest_point_index = -1; // 上一个最近点的index 26 | public: 27 | double calc_distance(State state, std::vector refer, int index); 28 | 29 | double search_target_index(State state, std::vector refer); 30 | 31 | void compute_errors(State state, std::vector refer, double& e_y, double& theta_e, double& cur); 32 | 33 | double normalize_angle(double angle); 34 | 35 | double rear_wheel_feedback_control(State state, std::vector refer, double K_psi, double K2); 36 | }; 37 | 38 | 39 | #endif //RW_H 40 | -------------------------------------------------------------------------------- /cpp_implementation/Rear_Wheel_Feedback/main.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file main.cpp 3 | * @author czj 4 | * @brief 5 | * @version 0.1 6 | * @date 2023-05-14 7 | * 8 | * @copyright Copyright (c) 2023 9 | * 10 | */ 11 | #include "RearWheelFeedback.h" 12 | #include "../utils/matplotlibcpp.h" 13 | namespace plt = matplotlibcpp; 14 | 15 | int main() { 16 | double x = 0.0, y = 0.0, yaw = 0.0, v = 10.0 / 3.6, l = 2.9, dt = 0.1; 17 | // 车辆状态 18 | State state(x, y, yaw, v, l); 19 | // 车辆模型 20 | VehicleModel vehicle(state, dt); 21 | // 生成参考轨迹 22 | ReferenceLine refer_line; 23 | //后面画图用 24 | std::vector refer_x(refer_line.refer.size()); 25 | std::vector refer_y(refer_line.refer.size()); 26 | for (int i = 0;i < refer_line.refer.size();i++) { 27 | refer_x[i] = refer_line.refer[i].x; 28 | refer_y[i] = refer_line.refer[i].y; 29 | } 30 | double K_psi = 2.1, K2 = 0.5;//利亚普诺夫系数 31 | RearWheelFeedback rwf; 32 | 33 | // 车辆运动轨迹 34 | std::vector trajectory_x, trajectory_y; 35 | 36 | while (true) { 37 | plt::clf(); 38 | // 返回控制量 39 | double delta = rwf.rear_wheel_feedback_control(vehicle.state, refer_line.refer, K_psi, K2); 40 | // 输出控制量 41 | vehicle.updateState(0, delta); 42 | 43 | trajectory_x.push_back(vehicle.state.x); 44 | trajectory_y.push_back(vehicle.state.y); 45 | 46 | //画图 47 | plt::plot(refer_x, refer_y, "b--"); 48 | plt::plot(trajectory_x, trajectory_y, "r"); 49 | plt::grid(true); 50 | plt::ylim(-8, 8); 51 | plt::pause(0.01); 52 | // 到达终点，退出控制 53 | if (rwf.calc_distance(vehicle.state, refer_line.refer, refer_line.refer.size() - 1) < 0.5) { 54 | std::cout << "Control completion" << std::endl; 55 | break; 56 | } 57 | } 58 | 59 | // 保存图片 60 | const char* filename = "./rear_demo.png"; 61 | std::cout << "Saving result to " << filename << std::endl; 62 | plt::save(filename); 63 | plt::show(); 64 | return 0; 65 | } 66 | -------------------------------------------------------------------------------- /cpp_implementation/Stanley/Stanley.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file Stanley.cpp 3 | * @author czj 4 | * @brief 5 | * @version 0.1 6 | * @date 2023-05-13 7 | * 8 | * @copyright Copyright (c) 2023 9 | * 10 | */ 11 | #include "Stanley.h" 12 | using std::vector; 13 | 14 | 15 | /** 16 | * @brief 计算车辆前轴与参考点的距离 17 | * 18 | * @param state 车辆状态 19 | * @param refer 参考轨迹 20 | * @param index 参考点下标 21 | * @return double 22 | */ 23 | double Stanley::calc_distance(State state, std::vector refer, int index) 24 | { 25 | return std::sqrt(std::pow(state.front_x - refer[index].x, 2) + std::pow(state.front_y - refer[index].y, 2)); 26 | } 27 | 28 | 29 | /** 30 | * @brief 寻找匹配点下标 31 | * 32 | * @param state 车辆状态 33 | * @param refer 参考轨迹 34 | * @return double 35 | */ 36 | double Stanley::search_target_index(State state, std::vector refer) 37 | { 38 | std::vector dists; 39 | double min_index; 40 | //std::cout << old_nearest_point_index << std::endl; 41 | // 寻找匹配点 42 | // 若是第一次搜寻，则遍历所有参考点寻找最小值下标 43 | if (old_nearest_point_index == -1) { 44 | for (int i = 0; i < refer.size();i++) { 45 | double dist = calc_distance(state, refer, i); 46 | dists.push_back(dist); 47 | } 48 | min_index = std::min_element(dists.begin(), dists.end()) - dists.begin(); //返回vector最小元素的下标 49 | old_nearest_point_index = min_index; 50 | } 51 | // 否则直接从上一个匹配点开始查找（找到第一个距离开始增大的点，类似梯度下降） 52 | else { 53 | min_index = old_nearest_point_index; 54 | for (int i = min_index; i < refer.size() - 1; i++) { 55 | double distance_this_index = calc_distance(state, refer, i); 56 | double distance_next_index = calc_distance(state, refer, i + 1); 57 | if (distance_this_index < distance_next_index) { 58 | break; 59 | } 60 | min_index = i + 1; 61 | //std::cout << old_nearest_point_index << std::endl; 62 | } 63 | old_nearest_point_index = min_index; 64 | } 65 | return min_index; 66 | } 67 | 68 | 69 | /** 70 | * @brief 角度归一化 71 | * @param angle 72 | * @return 73 | */ 74 | double Stanley::normalize_angle(double angle) { 75 | while (angle > PI) { 76 | angle -= 2.0 * PI; 77 | } 78 | while (angle < -PI) { 79 | angle += 2.0 * PI; 80 | } 81 | return angle; 82 | } 83 | 84 | /** 85 | * @brief 计算误差 86 | * 87 | * @param state 车辆当前状态 88 | * @param refer 参考轨迹 89 | * @param e_y 横向误差 90 | * @param e_theta 航向角误差 91 | */ 92 | 93 | void Stanley::compute_errors(State state, std::vector refer, double& e_y, double& theta_e) { 94 | 95 | int match_index = search_target_index(state, refer); // 获取匹配点下标 96 | std::cout << "匹配点" << match_index << std::endl; 97 | if (match_index >= refer.size()) { 98 | match_index = refer.size() - 1; 99 | } 100 | e_y = calc_distance(state, refer, match_index); //计算横向误差 101 | 102 | // 将位置误差转换为前轮转角的时候：需要将路径上距离车辆最近的点从世界坐标系变换到车辆坐标系下，根据路径点在车辆坐标系下的横坐标的正负决定前轮转角的方向 103 | double match_point_y_in_vehicle_coordinate = -(refer[match_index].x - state.x) * std::sin(state.yaw) + (refer[match_index].y - state.y) * std::cos(state.yaw); 104 | // 车辆坐标系：X轴沿着车辆纵向，向前为正，Y沿着车辆横向，向左为正（从车头往前看的视角） 105 | if (match_point_y_in_vehicle_coordinate > 0) 106 | { 107 | e_y = e_y; 108 | std::cout << "目标位于车辆左侧:" << e_y << std::endl; 109 | } 110 | else if (match_point_y_in_vehicle_coordinate < 0) { 111 | e_y = -e_y; 112 | std::cout << "目标位于车辆右侧:" << e_y << std::endl; 113 | } 114 | 115 | theta_e = (refer[match_index].heading - state.yaw); // 航向角误差 116 | } 117 | 118 | 119 | /** 120 | * @brief stanley控制 121 | * 122 | * @param state 车辆状态 123 | * @param refer 参考轨迹 124 | * @param k 增益系数 125 | * @return double 126 | */ 127 | double Stanley::stanley_control(State state, std::vector refer, double k) { 128 | double e_y; //横向误差 129 | double theta_e; //航向角误差 130 | compute_errors(state, refer, e_y, theta_e); //计算误差 131 | std::cout << "航向角误差" << theta_e << std::endl; 132 | // 计算转角控制量 133 | double delta_e = atan2(k * e_y, state.v); 134 | std::cout << "横向误差控制转角" << delta_e << std::endl; 135 | double delta = normalize_angle(delta_e + theta_e); 136 | std::cout << "控制量" << delta << std::endl; 137 | return delta; 138 | } 139 | 140 | 141 | -------------------------------------------------------------------------------- /cpp_implementation/Stanley/Stanley.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file Stanley.h 3 | * @author czj 4 | * @brief 5 | * @version 0.1 6 | * @date 2023-05-13 7 | * 8 | * @copyright Copyright (c) 2023 9 | * 10 | */ 11 | 12 | #ifndef STANLEY_H 13 | #define STANLEY_H 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include "../utils/VehicleModel.h" 19 | #include "../utils/ReferenceLine.h" 20 | 21 | #define PI 3.1415926 22 | 23 | class Stanley { 24 | private: 25 | int old_nearest_point_index = -1; // 上一个最近点的index 26 | public: 27 | 28 | double calc_distance(State state, std::vector refer, int index); 29 | 30 | double search_target_index(State state, std::vector refer); 31 | 32 | void compute_errors(State state, std::vector refer, double& e_y, double& theta_e); 33 | 34 | double normalize_angle(double angle); 35 | 36 | double stanley_control(State state, std::vector refer, double k); 37 | 38 | }; 39 | 40 | 41 | #endif //STANLEY_H 42 | -------------------------------------------------------------------------------- /cpp_implementation/Stanley/main.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file main.cpp 3 | * @author czj 4 | * @brief 5 | * @version 0.1 6 | * @date 2023-05-13 7 | * 8 | * @copyright Copyright (c) 2023 9 | * 10 | */ 11 | 12 | #include "Stanley.h" 13 | #include "../utils/matplotlibcpp.h" 14 | 15 | namespace plt = matplotlibcpp; 16 | 17 | #define PI 3.1415926 18 | 19 | 20 | int main() { 21 | double x = 0.0, y = 0.0, yaw = 0.0, v = 30.0 / 3.6, l = 2.9, dt = 0.1; 22 | // 车辆状态 23 | State state(x, y, yaw, v, l); 24 | // 车辆模型 25 | VehicleModel vehicle(state, dt); 26 | // 生成参考轨迹 27 | ReferenceLine refer_line; 28 | //后面画图用 29 | std::vector refer_x(refer_line.refer.size()); 30 | std::vector refer_y(refer_line.refer.size()); 31 | for (int i = 0;i < refer_line.refer.size();i++) { 32 | refer_x[i] = refer_line.refer[i].x; 33 | refer_y[i] = refer_line.refer[i].y; 34 | } 35 | 36 | double k = 2.0; //增益系数 37 | // 车辆运动轨迹 38 | std::vector trajectory_x, trajectory_y; 39 | Stanley stanley; 40 | while (true) { 41 | plt::clf(); 42 | double delta = stanley.stanley_control(vehicle.state, refer_line.refer, k); 43 | vehicle.updateState(0, delta); 44 | trajectory_x.push_back(vehicle.state.x); 45 | trajectory_y.push_back(vehicle.state.y); 46 | //画图 47 | plt::plot(refer_x, refer_y, "b--"); 48 | plt::plot(trajectory_x, trajectory_y, "r"); 49 | plt::grid(true); 50 | plt::pause(0.01); 51 | // 到达终点，退出控制 52 | if (stanley.calc_distance(vehicle.state, refer_line.refer, refer_line.refer.size() - 1) < 0.5) { 53 | std::cout << "Control completion" << std::endl; 54 | break; 55 | } 56 | } 57 | 58 | // 保存图片 59 | const char* filename = "./stanley_demo.png"; 60 | std::cout << "Saving result to " << filename << std::endl; 61 | plt::save(filename); 62 | plt::show(); 63 | return 0; 64 | } -------------------------------------------------------------------------------- /cpp_implementation/utils/ReferenceLine.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file ReferenceLine.cpp 3 | * @author czj 4 | * @brief 5 | * @version 0.1 6 | * @date 2023-05-15 7 | * 8 | * @copyright Copyright (c) 2023 9 | * 10 | */ 11 | 12 | #include "ReferenceLine.h" 13 | /** 14 | * 构造函数，求解出参考轨迹点上的曲率等信息 15 | */ 16 | ReferenceLine::ReferenceLine() { 17 | refer = std::vector(500); 18 | // 生成参考轨迹 19 | for (int i = 0; i < refer.size(); i++) { 20 | refer[i].x = 0.5 * i; 21 | refer[i].y = 2.0 * std::sin(refer[i].x / 10.0) + 3.0 * std::cos(refer[i].x / 9.0); 22 | } 23 | double x_delta = 0.0; 24 | double y_delta = 0.0; 25 | double x_delta_2 = 0.0; 26 | double y_delta_2 = 0.0; 27 | for (int i = 0; i < refer.size(); i++) { 28 | if (i == 0) { 29 | x_delta = (refer[i + 1].x - refer[i].x); 30 | y_delta = (refer[i + 1].y - refer[i].y); 31 | x_delta_2 = (refer[i + 2].x - refer[i + 1].x) - (refer[i + 1].x - refer[i].x); 32 | y_delta_2 = (refer[i + 2].y - refer[i + 1].y) - (refer[i + 1].y - refer[i].y); 33 | } 34 | else if (i == refer.size() - 1) { 35 | x_delta = (refer[i].x - refer[i - 1].x); 36 | y_delta = (refer[i].y - refer[i - 1].y); 37 | x_delta_2 = (refer[i].x - refer[i - 1].x) - (refer[i - 1].x - refer[i - 2].x); 38 | y_delta_2 = (refer[i].y - refer[i - 1].y) - (refer[i - 1].y - refer[i - 2].y); 39 | } 40 | else { 41 | x_delta = 0.5 * (refer[i + 1].x - refer[i - 1].x); 42 | y_delta = 0.5 * (refer[i + 1].y - refer[i - 1].y); 43 | x_delta_2 = (refer[i + 1].x - refer[i].x) - (refer[i].x - refer[i - 1].x); 44 | y_delta_2 = (refer[i + 1].y - refer[i].y) - (refer[i].y - refer[i - 1].y); 45 | } 46 | refer[i].heading = std::atan2(y_delta, x_delta); 47 | // 参数方程曲率计算 48 | refer[i].cur = std::abs(y_delta_2 * x_delta - x_delta_2 * y_delta) / std::pow((x_delta * x_delta + y_delta * y_delta), 3 / 2); 49 | } 50 | } 51 | 52 | 53 | 54 | -------------------------------------------------------------------------------- /cpp_implementation/utils/ReferenceLine.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file ReferenceLine.h 3 | * @author czj 4 | * @brief 5 | * @version 0.1 6 | * @date 2023-05-14 7 | * 8 | * @copyright Copyright (c) 2023 9 | * 10 | */ 11 | 12 | #ifndef REFERENCELINE_H 13 | #define REFERENCELINE_H 14 | #include 15 | #include 16 | #include 17 | #include 18 | #define PI 3.1415926 19 | 20 | // 参考点 21 | class ReferPoint { 22 | public: 23 | double x; 24 | double y; 25 | double heading; // 航向角 26 | double cur; //曲率 27 | }; 28 | 29 | // 参考轨迹 30 | class ReferenceLine { 31 | public: 32 | std::vector refer; 33 | public: 34 | ReferenceLine(); 35 | }; 36 | 37 | 38 | #endif //REFERENCELINE_H 39 | -------------------------------------------------------------------------------- /cpp_implementation/utils/VehicleModel.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file VehicleModel.cpp 3 | * @author czj 4 | * @brief 5 | * @version 0.1 6 | * @date 2023-05-13 7 | * 8 | * @copyright Copyright (c) 2023 9 | * 10 | */ 11 | 12 | #include "VehicleModel.h" 13 | 14 | using std::vector; 15 | 16 | State::State(double x, double y, double yaw, double v, double l) { 17 | this->x = x; 18 | this->y = y; 19 | this->yaw = yaw; 20 | this->v = v; 21 | this->l = l; 22 | this->rear_x = x - std::cos(yaw); 23 | this->rear_y = y - std::sin(yaw); 24 | this->front_x = x + std::cos(yaw); 25 | this->front_y = y + std::sin(yaw); 26 | } 27 | 28 | 29 | VehicleModel::VehicleModel(State& state, double dt) : state(state), dt(dt) {} 30 | 31 | /** 32 | * @brief 更新车辆位置 33 | * @param accel 加速度 34 | * @param delta 转向角 35 | */ 36 | void VehicleModel::updateState(double accel, double delta) { 37 | state.x = state.x + state.v * std::cos(state.yaw) * dt; 38 | state.y = state.y + state.v * std::sin(state.yaw) * dt; 39 | state.yaw = state.yaw + state.v / state.l * std::tan(delta) * dt; 40 | state.v = state.v + accel * dt; 41 | // 默认车辆前后轴距比例为1:1 42 | state.rear_x = state.x - state.l / 2 * std::cos(state.yaw); 43 | state.rear_y = state.y - state.l / 2 * std::sin(state.yaw); 44 | state.front_x = state.x + state.l / 2 * std::cos(state.yaw); 45 | state.front_y = state.y + state.l / 2 * std::sin(state.yaw); 46 | } 47 | 48 | /** 49 | * @brief 获取车辆状态 50 | * @return 51 | */ 52 | State VehicleModel::getState() { 53 | return { state }; 54 | } 55 | 56 | 57 | -------------------------------------------------------------------------------- /cpp_implementation/utils/VehicleModel.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file VehicleModel.h 3 | * @author czj 4 | * @brief 5 | * @version 0.1 6 | * @date 2023-05-12 7 | * 8 | * @copyright Copyright (c) 2023 9 | * 10 | */ 11 | 12 | #ifndef VehicleModel_H 13 | #define VehicleModel_H 14 | #include 15 | #include 16 | #include 17 | #include 18 | 19 | 20 | // 车辆状态 21 | class State { 22 | public: 23 | double x; // 质心x位置 24 | double y; // 质心y位置 25 | double yaw; // 航向角 26 | double v; // 速度 27 | double l; // 轴距 28 | double rear_x; //后轴x 29 | double rear_y; //后轴y 30 | double front_x; //前轴x 31 | double front_y; //前轴y 32 | public: 33 | State(double x, double y, double yaw, double v, double l); 34 | }; 35 | 36 | // 车辆运动学模型 37 | class VehicleModel { 38 | public: 39 | double dt; //采样时间 40 | State state; //车辆状态 41 | public: 42 | VehicleModel(State& state, double dt); 43 | 44 | void updateState(double accel, double delta); 45 | 46 | State getState(); 47 | 48 | //vector stateSpace(double ref_delta, double ref_yaw); 49 | 50 | }; 51 | 52 | 53 | #endif //VehicleModel_H 54 | -------------------------------------------------------------------------------- /cpp_implementation/utils/matplotlibcpp.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | // Python headers must be included before any system headers, since 4 | // they define _POSIX_C_SOURCE 5 | #include 6 | 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include // requires c++11 support 15 | #include 16 | #include // std::stod 17 | 18 | #ifndef WITHOUT_NUMPY 19 | # define NPY_NO_DEPRECATED_API NPY_1_7_API_VERSION 20 | # include 21 | 22 | # ifdef WITH_OPENCV 23 | # include 24 | # endif // WITH_OPENCV 25 | 26 | /* 27 | * A bunch of constants were removed in OpenCV 4 in favour of enum classes, so 28 | * define the ones we need here. 29 | */ 30 | # if CV_MAJOR_VERSION > 3 31 | # define CV_BGR2RGB cv::COLOR_BGR2RGB 32 | # define CV_BGRA2RGBA cv::COLOR_BGRA2RGBA 33 | # endif 34 | #endif // WITHOUT_NUMPY 35 | 36 | #if PY_MAJOR_VERSION >= 3 37 | # define PyString_FromString PyUnicode_FromString 38 | # define PyInt_FromLong PyLong_FromLong 39 | # define PyString_FromString PyUnicode_FromString 40 | #endif 41 | 42 | 43 | namespace matplotlibcpp { 44 | namespace detail { 45 | 46 | static std::string s_backend; 47 | 48 | struct _interpreter { 49 | PyObject* s_python_function_arrow; 50 | PyObject *s_python_function_show; 51 | PyObject *s_python_function_close; 52 | PyObject *s_python_function_draw; 53 | PyObject *s_python_function_pause; 54 | PyObject *s_python_function_save; 55 | PyObject *s_python_function_figure; 56 | PyObject *s_python_function_fignum_exists; 57 | PyObject *s_python_function_plot; 58 | PyObject *s_python_function_quiver; 59 | PyObject* s_python_function_contour; 60 | PyObject *s_python_function_semilogx; 61 | PyObject *s_python_function_semilogy; 62 | PyObject *s_python_function_loglog; 63 | PyObject *s_python_function_fill; 64 | PyObject *s_python_function_fill_between; 65 | PyObject *s_python_function_hist; 66 | PyObject *s_python_function_imshow; 67 | PyObject *s_python_function_scatter; 68 | PyObject *s_python_function_boxplot; 69 | PyObject *s_python_function_subplot; 70 | PyObject *s_python_function_subplot2grid; 71 | PyObject *s_python_function_legend; 72 | PyObject *s_python_function_xlim; 73 | PyObject *s_python_function_ion; 74 | PyObject *s_python_function_ginput; 75 | PyObject *s_python_function_ylim; 76 | PyObject *s_python_function_title; 77 | PyObject *s_python_function_axis; 78 | PyObject *s_python_function_axhline; 79 | PyObject *s_python_function_axvline; 80 | PyObject *s_python_function_axvspan; 81 | PyObject *s_python_function_xlabel; 82 | PyObject *s_python_function_ylabel; 83 | PyObject *s_python_function_gca; 84 | PyObject *s_python_function_xticks; 85 | PyObject *s_python_function_yticks; 86 | PyObject* s_python_function_margins; 87 | PyObject *s_python_function_tick_params; 88 | PyObject *s_python_function_grid; 89 | PyObject* s_python_function_cla; 90 | PyObject *s_python_function_clf; 91 | PyObject *s_python_function_errorbar; 92 | PyObject *s_python_function_annotate; 93 | PyObject *s_python_function_tight_layout; 94 | PyObject *s_python_colormap; 95 | PyObject *s_python_empty_tuple; 96 | PyObject *s_python_function_stem; 97 | PyObject *s_python_function_xkcd; 98 | PyObject *s_python_function_text; 99 | PyObject *s_python_function_suptitle; 100 | PyObject *s_python_function_bar; 101 | PyObject *s_python_function_barh; 102 | PyObject *s_python_function_colorbar; 103 | PyObject *s_python_function_subplots_adjust; 104 | PyObject *s_python_function_rcparams; 105 | PyObject *s_python_function_spy; 106 | 107 | /* For now, _interpreter is implemented as a singleton since its currently not possible to have 108 | multiple independent embedded python interpreters without patching the python source code 109 | or starting a separate process for each. [1] 110 | Furthermore, many python objects expect that they are destructed in the same thread as they 111 | were constructed. [2] So for advanced usage, a `kill()` function is provided so that library 112 | users can manually ensure that the interpreter is constructed and destroyed within the 113 | same thread. 114 | 115 | 1: http://bytes.com/topic/python/answers/793370-multiple-independent-python-interpreters-c-c-program 116 | 2: https://github.com/lava/matplotlib-cpp/pull/202#issue-436220256 117 | */ 118 | 119 | static _interpreter& get() { 120 | return interkeeper(false); 121 | } 122 | 123 | static _interpreter& kill() { 124 | return interkeeper(true); 125 | } 126 | 127 | // Stores the actual singleton object referenced by `get()` and `kill()`. 128 | static _interpreter& interkeeper(bool should_kill) { 129 | static _interpreter ctx; 130 | if (should_kill) 131 | ctx.~_interpreter(); 132 | return ctx; 133 | } 134 | 135 | PyObject* safe_import(PyObject* module, std::string fname) { 136 | PyObject* fn = PyObject_GetAttrString(module, fname.c_str()); 137 | 138 | if (!fn) 139 | throw std::runtime_error(std::string("Couldn't find required function: ") + fname); 140 | 141 | if (!PyFunction_Check(fn)) 142 | throw std::runtime_error(fname + std::string(" is unexpectedly not a PyFunction.")); 143 | 144 | return fn; 145 | } 146 | 147 | private: 148 | 149 | #ifndef WITHOUT_NUMPY 150 | # if PY_MAJOR_VERSION >= 3 151 | 152 | void *import_numpy() { 153 | import_array(); // initialize C-API 154 | return NULL; 155 | } 156 | 157 | # else 158 | 159 | void import_numpy() { 160 | import_array(); // initialize C-API 161 | } 162 | 163 | # endif 164 | #endif 165 | 166 | _interpreter() { 167 | 168 | // optional but recommended 169 | #if PY_MAJOR_VERSION >= 3 170 | wchar_t name[] = L"plotting"; 171 | #else 172 | char name[] = "plotting"; 173 | #endif 174 | Py_SetProgramName(name); 175 | Py_Initialize(); 176 | 177 | wchar_t const *dummy_args[] = {L"Python", NULL}; // const is needed because literals must not be modified 178 | wchar_t const **argv = dummy_args; 179 | int argc = sizeof(dummy_args)/sizeof(dummy_args[0])-1; 180 | 181 | #if PY_MAJOR_VERSION >= 3 182 | PySys_SetArgv(argc, const_cast(argv)); 183 | #else 184 | PySys_SetArgv(argc, (char **)(argv)); 185 | #endif 186 | 187 | #ifndef WITHOUT_NUMPY 188 | import_numpy(); // initialize numpy C-API 189 | #endif 190 | 191 | PyObject* matplotlibname = PyString_FromString("matplotlib"); 192 | PyObject* pyplotname = PyString_FromString("matplotlib.pyplot"); 193 | PyObject* cmname = PyString_FromString("matplotlib.cm"); 194 | PyObject* pylabname = PyString_FromString("pylab"); 195 | if (!pyplotname || !pylabname || !matplotlibname || !cmname) { 196 | throw std::runtime_error("couldnt create string"); 197 | } 198 | 199 | PyObject* matplotlib = PyImport_Import(matplotlibname); 200 | 201 | Py_DECREF(matplotlibname); 202 | if (!matplotlib) { 203 | PyErr_Print(); 204 | throw std::runtime_error("Error loading module matplotlib!"); 205 | } 206 | 207 | // matplotlib.use() must be called *before* pylab, matplotlib.pyplot, 208 | // or matplotlib.backends is imported for the first time 209 | if (!s_backend.empty()) { 210 | PyObject_CallMethod(matplotlib, const_cast("use"), const_cast("s"), s_backend.c_str()); 211 | } 212 | 213 | 214 | 215 | PyObject* pymod = PyImport_Import(pyplotname); 216 | Py_DECREF(pyplotname); 217 | if (!pymod) { throw std::runtime_error("Error loading module matplotlib.pyplot!"); } 218 | 219 | s_python_colormap = PyImport_Import(cmname); 220 | Py_DECREF(cmname); 221 | if (!s_python_colormap) { throw std::runtime_error("Error loading module matplotlib.cm!"); } 222 | 223 | PyObject* pylabmod = PyImport_Import(pylabname); 224 | Py_DECREF(pylabname); 225 | if (!pylabmod) { throw std::runtime_error("Error loading module pylab!"); } 226 | 227 | s_python_function_arrow = safe_import(pymod, "arrow"); 228 | s_python_function_show = safe_import(pymod, "show"); 229 | s_python_function_close = safe_import(pymod, "close"); 230 | s_python_function_draw = safe_import(pymod, "draw"); 231 | s_python_function_pause = safe_import(pymod, "pause"); 232 | s_python_function_figure = safe_import(pymod, "figure"); 233 | s_python_function_fignum_exists = safe_import(pymod, "fignum_exists"); 234 | s_python_function_plot = safe_import(pymod, "plot"); 235 | s_python_function_quiver = safe_import(pymod, "quiver"); 236 | s_python_function_contour = safe_import(pymod, "contour"); 237 | s_python_function_semilogx = safe_import(pymod, "semilogx"); 238 | s_python_function_semilogy = safe_import(pymod, "semilogy"); 239 | s_python_function_loglog = safe_import(pymod, "loglog"); 240 | s_python_function_fill = safe_import(pymod, "fill"); 241 | s_python_function_fill_between = safe_import(pymod, "fill_between"); 242 | s_python_function_hist = safe_import(pymod,"hist"); 243 | s_python_function_scatter = safe_import(pymod,"scatter"); 244 | s_python_function_boxplot = safe_import(pymod,"boxplot"); 245 | s_python_function_subplot = safe_import(pymod, "subplot"); 246 | s_python_function_subplot2grid = safe_import(pymod, "subplot2grid"); 247 | s_python_function_legend = safe_import(pymod, "legend"); 248 | s_python_function_xlim = safe_import(pymod, "xlim"); 249 | s_python_function_ylim = safe_import(pymod, "ylim"); 250 | s_python_function_title = safe_import(pymod, "title"); 251 | s_python_function_axis = safe_import(pymod, "axis"); 252 | s_python_function_axhline = safe_import(pymod, "axhline"); 253 | s_python_function_axvline = safe_import(pymod, "axvline"); 254 | s_python_function_axvspan = safe_import(pymod, "axvspan"); 255 | s_python_function_xlabel = safe_import(pymod, "xlabel"); 256 | s_python_function_ylabel = safe_import(pymod, "ylabel"); 257 | s_python_function_gca = safe_import(pymod, "gca"); 258 | s_python_function_xticks = safe_import(pymod, "xticks"); 259 | s_python_function_yticks = safe_import(pymod, "yticks"); 260 | s_python_function_margins = safe_import(pymod, "margins"); 261 | s_python_function_tick_params = safe_import(pymod, "tick_params"); 262 | s_python_function_grid = safe_import(pymod, "grid"); 263 | s_python_function_ion = safe_import(pymod, "ion"); 264 | s_python_function_ginput = safe_import(pymod, "ginput"); 265 | s_python_function_save = safe_import(pylabmod, "savefig"); 266 | s_python_function_annotate = safe_import(pymod,"annotate"); 267 | s_python_function_cla = safe_import(pymod, "cla"); 268 | s_python_function_clf = safe_import(pymod, "clf"); 269 | s_python_function_errorbar = safe_import(pymod, "errorbar"); 270 | s_python_function_tight_layout = safe_import(pymod, "tight_layout"); 271 | s_python_function_stem = safe_import(pymod, "stem"); 272 | s_python_function_xkcd = safe_import(pymod, "xkcd"); 273 | s_python_function_text = safe_import(pymod, "text"); 274 | s_python_function_suptitle = safe_import(pymod, "suptitle"); 275 | s_python_function_bar = safe_import(pymod,"bar"); 276 | s_python_function_barh = safe_import(pymod, "barh"); 277 | s_python_function_colorbar = PyObject_GetAttrString(pymod, "colorbar"); 278 | s_python_function_subplots_adjust = safe_import(pymod,"subplots_adjust"); 279 | s_python_function_rcparams = PyObject_GetAttrString(pymod, "rcParams"); 280 | s_python_function_spy = PyObject_GetAttrString(pymod, "spy"); 281 | #ifndef WITHOUT_NUMPY 282 | s_python_function_imshow = safe_import(pymod, "imshow"); 283 | #endif 284 | s_python_empty_tuple = PyTuple_New(0); 285 | } 286 | 287 | ~_interpreter() { 288 | Py_Finalize(); 289 | } 290 | }; 291 | 292 | } // end namespace detail 293 | 294 | /// Select the backend 295 | /// 296 | /// **NOTE:** This must be called before the first plot command to have 297 | /// any effect. 298 | /// 299 | /// Mainly useful to select the non-interactive 'Agg' backend when running 300 | /// matplotlibcpp in headless mode, for example on a machine with no display. 301 | /// 302 | /// See also: https://matplotlib.org/2.0.2/api/matplotlib_configuration_api.html#matplotlib.use 303 | inline void backend(const std::string& name) 304 | { 305 | detail::s_backend = name; 306 | } 307 | 308 | inline bool annotate(std::string annotation, double x, double y) 309 | { 310 | detail::_interpreter::get(); 311 | 312 | PyObject * xy = PyTuple_New(2); 313 | PyObject * str = PyString_FromString(annotation.c_str()); 314 | 315 | PyTuple_SetItem(xy,0,PyFloat_FromDouble(x)); 316 | PyTuple_SetItem(xy,1,PyFloat_FromDouble(y)); 317 | 318 | PyObject* kwargs = PyDict_New(); 319 | PyDict_SetItemString(kwargs, "xy", xy); 320 | 321 | PyObject* args = PyTuple_New(1); 322 | PyTuple_SetItem(args, 0, str); 323 | 324 | PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_annotate, args, kwargs); 325 | 326 | Py_DECREF(args); 327 | Py_DECREF(kwargs); 328 | 329 | if(res) Py_DECREF(res); 330 | 331 | return res; 332 | } 333 | 334 | namespace detail { 335 | 336 | #ifndef WITHOUT_NUMPY 337 | // Type selector for numpy array conversion 338 | template struct select_npy_type { const static NPY_TYPES type = NPY_NOTYPE; }; //Default 339 | template <> struct select_npy_type { const static NPY_TYPES type = NPY_DOUBLE; }; 340 | template <> struct select_npy_type { const static NPY_TYPES type = NPY_FLOAT; }; 341 | template <> struct select_npy_type { const static NPY_TYPES type = NPY_BOOL; }; 342 | template <> struct select_npy_type { const static NPY_TYPES type = NPY_INT8; }; 343 | template <> struct select_npy_type { const static NPY_TYPES type = NPY_SHORT; }; 344 | template <> struct select_npy_type { const static NPY_TYPES type = NPY_INT; }; 345 | template <> struct select_npy_type { const static NPY_TYPES type = NPY_INT64; }; 346 | template <> struct select_npy_type { const static NPY_TYPES type = NPY_UINT8; }; 347 | template <> struct select_npy_type { const static NPY_TYPES type = NPY_USHORT; }; 348 | template <> struct select_npy_type { const static NPY_TYPES type = NPY_ULONG; }; 349 | template <> struct select_npy_type { const static NPY_TYPES type = NPY_UINT64; }; 350 | 351 | // Sanity checks; comment them out or change the numpy type below if you're compiling on 352 | // a platform where they don't apply 353 | static_assert(sizeof(long long) == 8); 354 | template <> struct select_npy_type { const static NPY_TYPES type = NPY_INT64; }; 355 | static_assert(sizeof(unsigned long long) == 8); 356 | template <> struct select_npy_type { const static NPY_TYPES type = NPY_UINT64; }; 357 | 358 | template 359 | PyObject* get_array(const std::vector& v) 360 | { 361 | npy_intp vsize = v.size(); 362 | NPY_TYPES type = select_npy_type::type; 363 | if (type == NPY_NOTYPE) { 364 | size_t memsize = v.size()*sizeof(double); 365 | double* dp = static_cast(::malloc(memsize)); 366 | for (size_t i=0; i(varray), NPY_ARRAY_OWNDATA); 370 | return varray; 371 | } 372 | 373 | PyObject* varray = PyArray_SimpleNewFromData(1, &vsize, type, (void*)(v.data())); 374 | return varray; 375 | } 376 | 377 | 378 | template 379 | PyObject* get_2darray(const std::vector<::std::vector>& v) 380 | { 381 | if (v.size() < 1) throw std::runtime_error("get_2d_array v too small"); 382 | 383 | npy_intp vsize[2] = {static_cast(v.size()), 384 | static_cast(v[0].size())}; 385 | 386 | PyArrayObject *varray = 387 | (PyArrayObject *)PyArray_SimpleNew(2, vsize, NPY_DOUBLE); 388 | 389 | double *vd_begin = static_cast(PyArray_DATA(varray)); 390 | 391 | for (const ::std::vector &v_row : v) { 392 | if (v_row.size() != static_cast(vsize[1])) 393 | throw std::runtime_error("Missmatched array size"); 394 | std::copy(v_row.begin(), v_row.end(), vd_begin); 395 | vd_begin += vsize[1]; 396 | } 397 | 398 | return reinterpret_cast(varray); 399 | } 400 | 401 | #else // fallback if we don't have numpy: copy every element of the given vector 402 | 403 | template 404 | PyObject* get_array(const std::vector& v) 405 | { 406 | PyObject* list = PyList_New(v.size()); 407 | for(size_t i = 0; i < v.size(); ++i) { 408 | PyList_SetItem(list, i, PyFloat_FromDouble(v.at(i))); 409 | } 410 | return list; 411 | } 412 | 413 | #endif // WITHOUT_NUMPY 414 | 415 | // sometimes, for labels and such, we need string arrays 416 | inline PyObject * get_array(const std::vector& strings) 417 | { 418 | PyObject* list = PyList_New(strings.size()); 419 | for (std::size_t i = 0; i < strings.size(); ++i) { 420 | PyList_SetItem(list, i, PyString_FromString(strings[i].c_str())); 421 | } 422 | return list; 423 | } 424 | 425 | // not all matplotlib need 2d arrays, some prefer lists of lists 426 | template 427 | PyObject* get_listlist(const std::vector>& ll) 428 | { 429 | PyObject* listlist = PyList_New(ll.size()); 430 | for (std::size_t i = 0; i < ll.size(); ++i) { 431 | PyList_SetItem(listlist, i, get_array(ll[i])); 432 | } 433 | return listlist; 434 | } 435 | 436 | } // namespace detail 437 | 438 | /// Plot a line through the given x and y data points.. 439 | /// 440 | /// See: https://matplotlib.org/3.2.1/api/_as_gen/matplotlib.pyplot.plot.html 441 | template 442 | bool plot(const std::vector &x, const std::vector &y, const std::map& keywords) 443 | { 444 | assert(x.size() == y.size()); 445 | 446 | detail::_interpreter::get(); 447 | 448 | // using numpy arrays 449 | PyObject* xarray = detail::get_array(x); 450 | PyObject* yarray = detail::get_array(y); 451 | 452 | // construct positional args 453 | PyObject* args = PyTuple_New(2); 454 | PyTuple_SetItem(args, 0, xarray); 455 | PyTuple_SetItem(args, 1, yarray); 456 | 457 | // construct keyword args 458 | PyObject* kwargs = PyDict_New(); 459 | for(std::map::const_iterator it = keywords.begin(); it != keywords.end(); ++it) 460 | { 461 | PyDict_SetItemString(kwargs, it->first.c_str(), PyString_FromString(it->second.c_str())); 462 | } 463 | 464 | PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_plot, args, kwargs); 465 | 466 | Py_DECREF(args); 467 | Py_DECREF(kwargs); 468 | if(res) Py_DECREF(res); 469 | 470 | return res; 471 | } 472 | 473 | // TODO - it should be possible to make this work by implementing 474 | // a non-numpy alternative for `detail::get_2darray()`. 475 | #ifndef WITHOUT_NUMPY 476 | template 477 | void plot_surface(const std::vector<::std::vector> &x, 478 | const std::vector<::std::vector> &y, 479 | const std::vector<::std::vector> &z, 480 | const std::map &keywords = 481 | std::map(), 482 | const long fig_number=0) 483 | { 484 | detail::_interpreter::get(); 485 | 486 | // We lazily load the modules here the first time this function is called 487 | // because I'm not sure that we can assume "matplotlib installed" implies 488 | // "mpl_toolkits installed" on all platforms, and we don't want to require 489 | // it for people who don't need 3d plots. 490 | static PyObject *mpl_toolkitsmod = nullptr, *axis3dmod = nullptr; 491 | if (!mpl_toolkitsmod) { 492 | detail::_interpreter::get(); 493 | 494 | PyObject* mpl_toolkits = PyString_FromString("mpl_toolkits"); 495 | PyObject* axis3d = PyString_FromString("mpl_toolkits.mplot3d"); 496 | if (!mpl_toolkits || !axis3d) { throw std::runtime_error("couldnt create string"); } 497 | 498 | mpl_toolkitsmod = PyImport_Import(mpl_toolkits); 499 | Py_DECREF(mpl_toolkits); 500 | if (!mpl_toolkitsmod) { throw std::runtime_error("Error loading module mpl_toolkits!"); } 501 | 502 | axis3dmod = PyImport_Import(axis3d); 503 | Py_DECREF(axis3d); 504 | if (!axis3dmod) { throw std::runtime_error("Error loading module mpl_toolkits.mplot3d!"); } 505 | } 506 | 507 | assert(x.size() == y.size()); 508 | assert(y.size() == z.size()); 509 | 510 | // using numpy arrays 511 | PyObject *xarray = detail::get_2darray(x); 512 | PyObject *yarray = detail::get_2darray(y); 513 | PyObject *zarray = detail::get_2darray(z); 514 | 515 | // construct positional args 516 | PyObject *args = PyTuple_New(3); 517 | PyTuple_SetItem(args, 0, xarray); 518 | PyTuple_SetItem(args, 1, yarray); 519 | PyTuple_SetItem(args, 2, zarray); 520 | 521 | // Build up the kw args. 522 | PyObject *kwargs = PyDict_New(); 523 | PyDict_SetItemString(kwargs, "rstride", PyInt_FromLong(1)); 524 | PyDict_SetItemString(kwargs, "cstride", PyInt_FromLong(1)); 525 | 526 | PyObject *python_colormap_coolwarm = PyObject_GetAttrString( 527 | detail::_interpreter::get().s_python_colormap, "coolwarm"); 528 | 529 | PyDict_SetItemString(kwargs, "cmap", python_colormap_coolwarm); 530 | 531 | for (std::map::const_iterator it = keywords.begin(); 532 | it != keywords.end(); ++it) { 533 | if (it->first == "linewidth" || it->first == "alpha") { 534 | PyDict_SetItemString(kwargs, it->first.c_str(), 535 | PyFloat_FromDouble(std::stod(it->second))); 536 | } else { 537 | PyDict_SetItemString(kwargs, it->first.c_str(), 538 | PyString_FromString(it->second.c_str())); 539 | } 540 | } 541 | 542 | PyObject *fig_args = PyTuple_New(1); 543 | PyObject* fig = nullptr; 544 | PyTuple_SetItem(fig_args, 0, PyLong_FromLong(fig_number)); 545 | PyObject *fig_exists = 546 | PyObject_CallObject( 547 | detail::_interpreter::get().s_python_function_fignum_exists, fig_args); 548 | if (!PyObject_IsTrue(fig_exists)) { 549 | fig = PyObject_CallObject(detail::_interpreter::get().s_python_function_figure, 550 | detail::_interpreter::get().s_python_empty_tuple); 551 | } else { 552 | fig = PyObject_CallObject(detail::_interpreter::get().s_python_function_figure, 553 | fig_args); 554 | } 555 | Py_DECREF(fig_exists); 556 | if (!fig) throw std::runtime_error("Call to figure() failed."); 557 | 558 | PyObject *gca_kwargs = PyDict_New(); 559 | PyDict_SetItemString(gca_kwargs, "projection", PyString_FromString("3d")); 560 | 561 | PyObject *gca = PyObject_GetAttrString(fig, "gca"); 562 | if (!gca) throw std::runtime_error("No gca"); 563 | Py_INCREF(gca); 564 | PyObject *axis = PyObject_Call( 565 | gca, detail::_interpreter::get().s_python_empty_tuple, gca_kwargs); 566 | 567 | if (!axis) throw std::runtime_error("No axis"); 568 | Py_INCREF(axis); 569 | 570 | Py_DECREF(gca); 571 | Py_DECREF(gca_kwargs); 572 | 573 | PyObject *plot_surface = PyObject_GetAttrString(axis, "plot_surface"); 574 | if (!plot_surface) throw std::runtime_error("No surface"); 575 | Py_INCREF(plot_surface); 576 | PyObject *res = PyObject_Call(plot_surface, args, kwargs); 577 | if (!res) throw std::runtime_error("failed surface"); 578 | Py_DECREF(plot_surface); 579 | 580 | Py_DECREF(axis); 581 | Py_DECREF(args); 582 | Py_DECREF(kwargs); 583 | if (res) Py_DECREF(res); 584 | } 585 | 586 | template 587 | void contour(const std::vector<::std::vector> &x, 588 | const std::vector<::std::vector> &y, 589 | const std::vector<::std::vector> &z, 590 | const std::map &keywords = {}) 591 | { 592 | detail::_interpreter::get(); 593 | 594 | // using numpy arrays 595 | PyObject *xarray = detail::get_2darray(x); 596 | PyObject *yarray = detail::get_2darray(y); 597 | PyObject *zarray = detail::get_2darray(z); 598 | 599 | // construct positional args 600 | PyObject *args = PyTuple_New(3); 601 | PyTuple_SetItem(args, 0, xarray); 602 | PyTuple_SetItem(args, 1, yarray); 603 | PyTuple_SetItem(args, 2, zarray); 604 | 605 | // Build up the kw args. 606 | PyObject *kwargs = PyDict_New(); 607 | 608 | PyObject *python_colormap_coolwarm = PyObject_GetAttrString( 609 | detail::_interpreter::get().s_python_colormap, "coolwarm"); 610 | 611 | PyDict_SetItemString(kwargs, "cmap", python_colormap_coolwarm); 612 | 613 | for (std::map::const_iterator it = keywords.begin(); 614 | it != keywords.end(); ++it) { 615 | PyDict_SetItemString(kwargs, it->first.c_str(), 616 | PyString_FromString(it->second.c_str())); 617 | } 618 | 619 | PyObject *res = PyObject_Call(detail::_interpreter::get().s_python_function_contour, args, kwargs); 620 | if (!res) 621 | throw std::runtime_error("failed contour"); 622 | 623 | Py_DECREF(args); 624 | Py_DECREF(kwargs); 625 | if (res) Py_DECREF(res); 626 | } 627 | 628 | template 629 | void spy(const std::vector<::std::vector> &x, 630 | const double markersize = -1, // -1 for default matplotlib size 631 | const std::map &keywords = {}) 632 | { 633 | detail::_interpreter::get(); 634 | 635 | PyObject *xarray = detail::get_2darray(x); 636 | 637 | PyObject *kwargs = PyDict_New(); 638 | if (markersize != -1) { 639 | PyDict_SetItemString(kwargs, "markersize", PyFloat_FromDouble(markersize)); 640 | } 641 | for (std::map::const_iterator it = keywords.begin(); 642 | it != keywords.end(); ++it) { 643 | PyDict_SetItemString(kwargs, it->first.c_str(), 644 | PyString_FromString(it->second.c_str())); 645 | } 646 | 647 | PyObject *plot_args = PyTuple_New(1); 648 | PyTuple_SetItem(plot_args, 0, xarray); 649 | 650 | PyObject *res = PyObject_Call( 651 | detail::_interpreter::get().s_python_function_spy, plot_args, kwargs); 652 | 653 | Py_DECREF(plot_args); 654 | Py_DECREF(kwargs); 655 | if (res) Py_DECREF(res); 656 | } 657 | #endif // WITHOUT_NUMPY 658 | 659 | template 660 | void plot3(const std::vector &x, 661 | const std::vector &y, 662 | const std::vector &z, 663 | const std::map &keywords = 664 | std::map(), 665 | const long fig_number=0) 666 | { 667 | detail::_interpreter::get(); 668 | 669 | // Same as with plot_surface: We lazily load the modules here the first time 670 | // this function is called because I'm not sure that we can assume "matplotlib 671 | // installed" implies "mpl_toolkits installed" on all platforms, and we don't 672 | // want to require it for people who don't need 3d plots. 673 | static PyObject *mpl_toolkitsmod = nullptr, *axis3dmod = nullptr; 674 | if (!mpl_toolkitsmod) { 675 | detail::_interpreter::get(); 676 | 677 | PyObject* mpl_toolkits = PyString_FromString("mpl_toolkits"); 678 | PyObject* axis3d = PyString_FromString("mpl_toolkits.mplot3d"); 679 | if (!mpl_toolkits || !axis3d) { throw std::runtime_error("couldnt create string"); } 680 | 681 | mpl_toolkitsmod = PyImport_Import(mpl_toolkits); 682 | Py_DECREF(mpl_toolkits); 683 | if (!mpl_toolkitsmod) { throw std::runtime_error("Error loading module mpl_toolkits!"); } 684 | 685 | axis3dmod = PyImport_Import(axis3d); 686 | Py_DECREF(axis3d); 687 | if (!axis3dmod) { throw std::runtime_error("Error loading module mpl_toolkits.mplot3d!"); } 688 | } 689 | 690 | assert(x.size() == y.size()); 691 | assert(y.size() == z.size()); 692 | 693 | PyObject *xarray = detail::get_array(x); 694 | PyObject *yarray = detail::get_array(y); 695 | PyObject *zarray = detail::get_array(z); 696 | 697 | // construct positional args 698 | PyObject *args = PyTuple_New(3); 699 | PyTuple_SetItem(args, 0, xarray); 700 | PyTuple_SetItem(args, 1, yarray); 701 | PyTuple_SetItem(args, 2, zarray); 702 | 703 | // Build up the kw args. 704 | PyObject *kwargs = PyDict_New(); 705 | 706 | for (std::map::const_iterator it = keywords.begin(); 707 | it != keywords.end(); ++it) { 708 | PyDict_SetItemString(kwargs, it->first.c_str(), 709 | PyString_FromString(it->second.c_str())); 710 | } 711 | 712 | PyObject *fig_args = PyTuple_New(1); 713 | PyObject* fig = nullptr; 714 | PyTuple_SetItem(fig_args, 0, PyLong_FromLong(fig_number)); 715 | PyObject *fig_exists = 716 | PyObject_CallObject(detail::_interpreter::get().s_python_function_fignum_exists, fig_args); 717 | if (!PyObject_IsTrue(fig_exists)) { 718 | fig = PyObject_CallObject(detail::_interpreter::get().s_python_function_figure, 719 | detail::_interpreter::get().s_python_empty_tuple); 720 | } else { 721 | fig = PyObject_CallObject(detail::_interpreter::get().s_python_function_figure, 722 | fig_args); 723 | } 724 | if (!fig) throw std::runtime_error("Call to figure() failed."); 725 | 726 | PyObject *gca_kwargs = PyDict_New(); 727 | PyDict_SetItemString(gca_kwargs, "projection", PyString_FromString("3d")); 728 | 729 | PyObject *gca = PyObject_GetAttrString(fig, "gca"); 730 | if (!gca) throw std::runtime_error("No gca"); 731 | Py_INCREF(gca); 732 | PyObject *axis = PyObject_Call( 733 | gca, detail::_interpreter::get().s_python_empty_tuple, gca_kwargs); 734 | 735 | if (!axis) throw std::runtime_error("No axis"); 736 | Py_INCREF(axis); 737 | 738 | Py_DECREF(gca); 739 | Py_DECREF(gca_kwargs); 740 | 741 | PyObject *plot3 = PyObject_GetAttrString(axis, "plot"); 742 | if (!plot3) throw std::runtime_error("No 3D line plot"); 743 | Py_INCREF(plot3); 744 | PyObject *res = PyObject_Call(plot3, args, kwargs); 745 | if (!res) throw std::runtime_error("Failed 3D line plot"); 746 | Py_DECREF(plot3); 747 | 748 | Py_DECREF(axis); 749 | Py_DECREF(args); 750 | Py_DECREF(kwargs); 751 | if (res) Py_DECREF(res); 752 | } 753 | 754 | template 755 | bool stem(const std::vector &x, const std::vector &y, const std::map& keywords) 756 | { 757 | assert(x.size() == y.size()); 758 | 759 | detail::_interpreter::get(); 760 | 761 | // using numpy arrays 762 | PyObject* xarray = detail::get_array(x); 763 | PyObject* yarray = detail::get_array(y); 764 | 765 | // construct positional args 766 | PyObject* args = PyTuple_New(2); 767 | PyTuple_SetItem(args, 0, xarray); 768 | PyTuple_SetItem(args, 1, yarray); 769 | 770 | // construct keyword args 771 | PyObject* kwargs = PyDict_New(); 772 | for (std::map::const_iterator it = 773 | keywords.begin(); it != keywords.end(); ++it) { 774 | PyDict_SetItemString(kwargs, it->first.c_str(), 775 | PyString_FromString(it->second.c_str())); 776 | } 777 | 778 | PyObject* res = PyObject_Call( 779 | detail::_interpreter::get().s_python_function_stem, args, kwargs); 780 | 781 | Py_DECREF(args); 782 | Py_DECREF(kwargs); 783 | if (res) 784 | Py_DECREF(res); 785 | 786 | return res; 787 | } 788 | 789 | template< typename Numeric > 790 | bool fill(const std::vector& x, const std::vector& y, const std::map& keywords) 791 | { 792 | assert(x.size() == y.size()); 793 | 794 | detail::_interpreter::get(); 795 | 796 | // using numpy arrays 797 | PyObject* xarray = detail::get_array(x); 798 | PyObject* yarray = detail::get_array(y); 799 | 800 | // construct positional args 801 | PyObject* args = PyTuple_New(2); 802 | PyTuple_SetItem(args, 0, xarray); 803 | PyTuple_SetItem(args, 1, yarray); 804 | 805 | // construct keyword args 806 | PyObject* kwargs = PyDict_New(); 807 | for (auto it = keywords.begin(); it != keywords.end(); ++it) { 808 | PyDict_SetItemString(kwargs, it->first.c_str(), PyUnicode_FromString(it->second.c_str())); 809 | } 810 | 811 | PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_fill, args, kwargs); 812 | 813 | Py_DECREF(args); 814 | Py_DECREF(kwargs); 815 | 816 | if (res) Py_DECREF(res); 817 | 818 | return res; 819 | } 820 | 821 | template< typename Numeric > 822 | bool fill_between(const std::vector& x, const std::vector& y1, const std::vector& y2, const std::map& keywords) 823 | { 824 | assert(x.size() == y1.size()); 825 | assert(x.size() == y2.size()); 826 | 827 | detail::_interpreter::get(); 828 | 829 | // using numpy arrays 830 | PyObject* xarray = detail::get_array(x); 831 | PyObject* y1array = detail::get_array(y1); 832 | PyObject* y2array = detail::get_array(y2); 833 | 834 | // construct positional args 835 | PyObject* args = PyTuple_New(3); 836 | PyTuple_SetItem(args, 0, xarray); 837 | PyTuple_SetItem(args, 1, y1array); 838 | PyTuple_SetItem(args, 2, y2array); 839 | 840 | // construct keyword args 841 | PyObject* kwargs = PyDict_New(); 842 | for(std::map::const_iterator it = keywords.begin(); it != keywords.end(); ++it) { 843 | PyDict_SetItemString(kwargs, it->first.c_str(), PyUnicode_FromString(it->second.c_str())); 844 | } 845 | 846 | PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_fill_between, args, kwargs); 847 | 848 | Py_DECREF(args); 849 | Py_DECREF(kwargs); 850 | if(res) Py_DECREF(res); 851 | 852 | return res; 853 | } 854 | 855 | template 856 | bool arrow(Numeric x, Numeric y, Numeric end_x, Numeric end_y, const std::string& fc = "r", 857 | const std::string ec = "k", Numeric head_length = 0.25, Numeric head_width = 0.1625) { 858 | PyObject* obj_x = PyFloat_FromDouble(x); 859 | PyObject* obj_y = PyFloat_FromDouble(y); 860 | PyObject* obj_end_x = PyFloat_FromDouble(end_x); 861 | PyObject* obj_end_y = PyFloat_FromDouble(end_y); 862 | 863 | PyObject* kwargs = PyDict_New(); 864 | PyDict_SetItemString(kwargs, "fc", PyString_FromString(fc.c_str())); 865 | PyDict_SetItemString(kwargs, "ec", PyString_FromString(ec.c_str())); 866 | PyDict_SetItemString(kwargs, "head_width", PyFloat_FromDouble(head_width)); 867 | PyDict_SetItemString(kwargs, "head_length", PyFloat_FromDouble(head_length)); 868 | 869 | PyObject* plot_args = PyTuple_New(4); 870 | PyTuple_SetItem(plot_args, 0, obj_x); 871 | PyTuple_SetItem(plot_args, 1, obj_y); 872 | PyTuple_SetItem(plot_args, 2, obj_end_x); 873 | PyTuple_SetItem(plot_args, 3, obj_end_y); 874 | 875 | PyObject* res = 876 | PyObject_Call(detail::_interpreter::get().s_python_function_arrow, plot_args, kwargs); 877 | 878 | Py_DECREF(plot_args); 879 | Py_DECREF(kwargs); 880 | if (res) 881 | Py_DECREF(res); 882 | 883 | return res; 884 | } 885 | 886 | template< typename Numeric> 887 | bool hist(const std::vector& y, long bins=10,std::string color="b", 888 | double alpha=1.0, bool cumulative=false) 889 | { 890 | detail::_interpreter::get(); 891 | 892 | PyObject* yarray = detail::get_array(y); 893 | 894 | PyObject* kwargs = PyDict_New(); 895 | PyDict_SetItemString(kwargs, "bins", PyLong_FromLong(bins)); 896 | PyDict_SetItemString(kwargs, "color", PyString_FromString(color.c_str())); 897 | PyDict_SetItemString(kwargs, "alpha", PyFloat_FromDouble(alpha)); 898 | PyDict_SetItemString(kwargs, "cumulative", cumulative ? Py_True : Py_False); 899 | 900 | PyObject* plot_args = PyTuple_New(1); 901 | 902 | PyTuple_SetItem(plot_args, 0, yarray); 903 | 904 | 905 | PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_hist, plot_args, kwargs); 906 | 907 | 908 | Py_DECREF(plot_args); 909 | Py_DECREF(kwargs); 910 | if(res) Py_DECREF(res); 911 | 912 | return res; 913 | } 914 | 915 | #ifndef WITHOUT_NUMPY 916 | namespace detail { 917 | 918 | inline void imshow(void *ptr, const NPY_TYPES type, const int rows, const int columns, const int colors, const std::map &keywords, PyObject** out) 919 | { 920 | assert(type == NPY_UINT8 || type == NPY_FLOAT); 921 | assert(colors == 1 || colors == 3 || colors == 4); 922 | 923 | detail::_interpreter::get(); 924 | 925 | // construct args 926 | npy_intp dims[3] = { rows, columns, colors }; 927 | PyObject *args = PyTuple_New(1); 928 | PyTuple_SetItem(args, 0, PyArray_SimpleNewFromData(colors == 1 ? 2 : 3, dims, type, ptr)); 929 | 930 | // construct keyword args 931 | PyObject* kwargs = PyDict_New(); 932 | for(std::map::const_iterator it = keywords.begin(); it != keywords.end(); ++it) 933 | { 934 | PyDict_SetItemString(kwargs, it->first.c_str(), PyUnicode_FromString(it->second.c_str())); 935 | } 936 | 937 | PyObject *res = PyObject_Call(detail::_interpreter::get().s_python_function_imshow, args, kwargs); 938 | Py_DECREF(args); 939 | Py_DECREF(kwargs); 940 | if (!res) 941 | throw std::runtime_error("Call to imshow() failed"); 942 | if (out) 943 | *out = res; 944 | else 945 | Py_DECREF(res); 946 | } 947 | 948 | } // namespace detail 949 | 950 | inline void imshow(const unsigned char *ptr, const int rows, const int columns, const int colors, const std::map &keywords = {}, PyObject** out = nullptr) 951 | { 952 | detail::imshow((void *) ptr, NPY_UINT8, rows, columns, colors, keywords, out); 953 | } 954 | 955 | inline void imshow(const float *ptr, const int rows, const int columns, const int colors, const std::map &keywords = {}, PyObject** out = nullptr) 956 | { 957 | detail::imshow((void *) ptr, NPY_FLOAT, rows, columns, colors, keywords, out); 958 | } 959 | 960 | #ifdef WITH_OPENCV 961 | void imshow(const cv::Mat &image, const std::map &keywords = {}) 962 | { 963 | // Convert underlying type of matrix, if needed 964 | cv::Mat image2; 965 | NPY_TYPES npy_type = NPY_UINT8; 966 | switch (image.type() & CV_MAT_DEPTH_MASK) { 967 | case CV_8U: 968 | image2 = image; 969 | break; 970 | case CV_32F: 971 | image2 = image; 972 | npy_type = NPY_FLOAT; 973 | break; 974 | default: 975 | image.convertTo(image2, CV_MAKETYPE(CV_8U, image.channels())); 976 | } 977 | 978 | // If color image, convert from BGR to RGB 979 | switch (image2.channels()) { 980 | case 3: 981 | cv::cvtColor(image2, image2, CV_BGR2RGB); 982 | break; 983 | case 4: 984 | cv::cvtColor(image2, image2, CV_BGRA2RGBA); 985 | } 986 | 987 | detail::imshow(image2.data, npy_type, image2.rows, image2.cols, image2.channels(), keywords); 988 | } 989 | #endif // WITH_OPENCV 990 | #endif // WITHOUT_NUMPY 991 | 992 | template 993 | bool scatter(const std::vector& x, 994 | const std::vector& y, 995 | const double s=1.0, // The marker size in points**2 996 | const std::map & keywords = {}) 997 | { 998 | detail::_interpreter::get(); 999 | 1000 | assert(x.size() == y.size()); 1001 | 1002 | PyObject* xarray = detail::get_array(x); 1003 | PyObject* yarray = detail::get_array(y); 1004 | 1005 | PyObject* kwargs = PyDict_New(); 1006 | PyDict_SetItemString(kwargs, "s", PyLong_FromLong(s)); 1007 | for (const auto& it : keywords) 1008 | { 1009 | PyDict_SetItemString(kwargs, it.first.c_str(), PyString_FromString(it.second.c_str())); 1010 | } 1011 | 1012 | PyObject* plot_args = PyTuple_New(2); 1013 | PyTuple_SetItem(plot_args, 0, xarray); 1014 | PyTuple_SetItem(plot_args, 1, yarray); 1015 | 1016 | PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_scatter, plot_args, kwargs); 1017 | 1018 | Py_DECREF(plot_args); 1019 | Py_DECREF(kwargs); 1020 | if(res) Py_DECREF(res); 1021 | 1022 | return res; 1023 | } 1024 | 1025 | template 1026 | bool scatter_colored(const std::vector& x, 1027 | const std::vector& y, 1028 | const std::vector& colors, 1029 | const double s=1.0, // The marker size in points**2 1030 | const std::map & keywords = {}) 1031 | { 1032 | detail::_interpreter::get(); 1033 | 1034 | assert(x.size() == y.size()); 1035 | 1036 | PyObject* xarray = detail::get_array(x); 1037 | PyObject* yarray = detail::get_array(y); 1038 | PyObject* colors_array = detail::get_array(colors); 1039 | 1040 | PyObject* kwargs = PyDict_New(); 1041 | PyDict_SetItemString(kwargs, "s", PyLong_FromLong(s)); 1042 | PyDict_SetItemString(kwargs, "c", colors_array); 1043 | 1044 | for (const auto& it : keywords) 1045 | { 1046 | PyDict_SetItemString(kwargs, it.first.c_str(), PyString_FromString(it.second.c_str())); 1047 | } 1048 | 1049 | PyObject* plot_args = PyTuple_New(2); 1050 | PyTuple_SetItem(plot_args, 0, xarray); 1051 | PyTuple_SetItem(plot_args, 1, yarray); 1052 | 1053 | PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_scatter, plot_args, kwargs); 1054 | 1055 | Py_DECREF(plot_args); 1056 | Py_DECREF(kwargs); 1057 | if(res) Py_DECREF(res); 1058 | 1059 | return res; 1060 | } 1061 | 1062 | 1063 | template 1064 | bool scatter(const std::vector& x, 1065 | const std::vector& y, 1066 | const std::vector& z, 1067 | const double s=1.0, // The marker size in points**2 1068 | const std::map & keywords = {}, 1069 | const long fig_number=0) { 1070 | detail::_interpreter::get(); 1071 | 1072 | // Same as with plot_surface: We lazily load the modules here the first time 1073 | // this function is called because I'm not sure that we can assume "matplotlib 1074 | // installed" implies "mpl_toolkits installed" on all platforms, and we don't 1075 | // want to require it for people who don't need 3d plots. 1076 | static PyObject *mpl_toolkitsmod = nullptr, *axis3dmod = nullptr; 1077 | if (!mpl_toolkitsmod) { 1078 | detail::_interpreter::get(); 1079 | 1080 | PyObject* mpl_toolkits = PyString_FromString("mpl_toolkits"); 1081 | PyObject* axis3d = PyString_FromString("mpl_toolkits.mplot3d"); 1082 | if (!mpl_toolkits || !axis3d) { throw std::runtime_error("couldnt create string"); } 1083 | 1084 | mpl_toolkitsmod = PyImport_Import(mpl_toolkits); 1085 | Py_DECREF(mpl_toolkits); 1086 | if (!mpl_toolkitsmod) { throw std::runtime_error("Error loading module mpl_toolkits!"); } 1087 | 1088 | axis3dmod = PyImport_Import(axis3d); 1089 | Py_DECREF(axis3d); 1090 | if (!axis3dmod) { throw std::runtime_error("Error loading module mpl_toolkits.mplot3d!"); } 1091 | } 1092 | 1093 | assert(x.size() == y.size()); 1094 | assert(y.size() == z.size()); 1095 | 1096 | PyObject *xarray = detail::get_array(x); 1097 | PyObject *yarray = detail::get_array(y); 1098 | PyObject *zarray = detail::get_array(z); 1099 | 1100 | // construct positional args 1101 | PyObject *args = PyTuple_New(3); 1102 | PyTuple_SetItem(args, 0, xarray); 1103 | PyTuple_SetItem(args, 1, yarray); 1104 | PyTuple_SetItem(args, 2, zarray); 1105 | 1106 | // Build up the kw args. 1107 | PyObject *kwargs = PyDict_New(); 1108 | 1109 | for (std::map::const_iterator it = keywords.begin(); 1110 | it != keywords.end(); ++it) { 1111 | PyDict_SetItemString(kwargs, it->first.c_str(), 1112 | PyString_FromString(it->second.c_str())); 1113 | } 1114 | PyObject *fig_args = PyTuple_New(1); 1115 | PyObject* fig = nullptr; 1116 | PyTuple_SetItem(fig_args, 0, PyLong_FromLong(fig_number)); 1117 | PyObject *fig_exists = 1118 | PyObject_CallObject(detail::_interpreter::get().s_python_function_fignum_exists, fig_args); 1119 | if (!PyObject_IsTrue(fig_exists)) { 1120 | fig = PyObject_CallObject(detail::_interpreter::get().s_python_function_figure, 1121 | detail::_interpreter::get().s_python_empty_tuple); 1122 | } else { 1123 | fig = PyObject_CallObject(detail::_interpreter::get().s_python_function_figure, 1124 | fig_args); 1125 | } 1126 | Py_DECREF(fig_exists); 1127 | if (!fig) throw std::runtime_error("Call to figure() failed."); 1128 | 1129 | PyObject *gca_kwargs = PyDict_New(); 1130 | PyDict_SetItemString(gca_kwargs, "projection", PyString_FromString("3d")); 1131 | 1132 | PyObject *gca = PyObject_GetAttrString(fig, "gca"); 1133 | if (!gca) throw std::runtime_error("No gca"); 1134 | Py_INCREF(gca); 1135 | PyObject *axis = PyObject_Call( 1136 | gca, detail::_interpreter::get().s_python_empty_tuple, gca_kwargs); 1137 | 1138 | if (!axis) throw std::runtime_error("No axis"); 1139 | Py_INCREF(axis); 1140 | 1141 | Py_DECREF(gca); 1142 | Py_DECREF(gca_kwargs); 1143 | 1144 | PyObject *plot3 = PyObject_GetAttrString(axis, "scatter"); 1145 | if (!plot3) throw std::runtime_error("No 3D line plot"); 1146 | Py_INCREF(plot3); 1147 | PyObject *res = PyObject_Call(plot3, args, kwargs); 1148 | if (!res) throw std::runtime_error("Failed 3D line plot"); 1149 | Py_DECREF(plot3); 1150 | 1151 | Py_DECREF(axis); 1152 | Py_DECREF(args); 1153 | Py_DECREF(kwargs); 1154 | Py_DECREF(fig); 1155 | if (res) Py_DECREF(res); 1156 | return res; 1157 | 1158 | } 1159 | 1160 | template 1161 | bool boxplot(const std::vector>& data, 1162 | const std::vector& labels = {}, 1163 | const std::map & keywords = {}) 1164 | { 1165 | detail::_interpreter::get(); 1166 | 1167 | PyObject* listlist = detail::get_listlist(data); 1168 | PyObject* args = PyTuple_New(1); 1169 | PyTuple_SetItem(args, 0, listlist); 1170 | 1171 | PyObject* kwargs = PyDict_New(); 1172 | 1173 | // kwargs needs the labels, if there are (the correct number of) labels 1174 | if (!labels.empty() && labels.size() == data.size()) { 1175 | PyDict_SetItemString(kwargs, "labels", detail::get_array(labels)); 1176 | } 1177 | 1178 | // take care of the remaining keywords 1179 | for (const auto& it : keywords) 1180 | { 1181 | PyDict_SetItemString(kwargs, it.first.c_str(), PyString_FromString(it.second.c_str())); 1182 | } 1183 | 1184 | PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_boxplot, args, kwargs); 1185 | 1186 | Py_DECREF(args); 1187 | Py_DECREF(kwargs); 1188 | 1189 | if(res) Py_DECREF(res); 1190 | 1191 | return res; 1192 | } 1193 | 1194 | template 1195 | bool boxplot(const std::vector& data, 1196 | const std::map & keywords = {}) 1197 | { 1198 | detail::_interpreter::get(); 1199 | 1200 | PyObject* vector = detail::get_array(data); 1201 | PyObject* args = PyTuple_New(1); 1202 | PyTuple_SetItem(args, 0, vector); 1203 | 1204 | PyObject* kwargs = PyDict_New(); 1205 | for (const auto& it : keywords) 1206 | { 1207 | PyDict_SetItemString(kwargs, it.first.c_str(), PyString_FromString(it.second.c_str())); 1208 | } 1209 | 1210 | PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_boxplot, args, kwargs); 1211 | 1212 | Py_DECREF(args); 1213 | Py_DECREF(kwargs); 1214 | 1215 | if(res) Py_DECREF(res); 1216 | 1217 | return res; 1218 | } 1219 | 1220 | template 1221 | bool bar(const std::vector & x, 1222 | const std::vector & y, 1223 | std::string ec = "black", 1224 | std::string ls = "-", 1225 | double lw = 1.0, 1226 | const std::map & keywords = {}) 1227 | { 1228 | detail::_interpreter::get(); 1229 | 1230 | PyObject * xarray = detail::get_array(x); 1231 | PyObject * yarray = detail::get_array(y); 1232 | 1233 | PyObject * kwargs = PyDict_New(); 1234 | 1235 | PyDict_SetItemString(kwargs, "ec", PyString_FromString(ec.c_str())); 1236 | PyDict_SetItemString(kwargs, "ls", PyString_FromString(ls.c_str())); 1237 | PyDict_SetItemString(kwargs, "lw", PyFloat_FromDouble(lw)); 1238 | 1239 | for (std::map::const_iterator it = 1240 | keywords.begin(); 1241 | it != keywords.end(); 1242 | ++it) { 1243 | PyDict_SetItemString( 1244 | kwargs, it->first.c_str(), PyUnicode_FromString(it->second.c_str())); 1245 | } 1246 | 1247 | PyObject * plot_args = PyTuple_New(2); 1248 | PyTuple_SetItem(plot_args, 0, xarray); 1249 | PyTuple_SetItem(plot_args, 1, yarray); 1250 | 1251 | PyObject * res = PyObject_Call( 1252 | detail::_interpreter::get().s_python_function_bar, plot_args, kwargs); 1253 | 1254 | Py_DECREF(plot_args); 1255 | Py_DECREF(kwargs); 1256 | if (res) Py_DECREF(res); 1257 | 1258 | return res; 1259 | } 1260 | 1261 | template 1262 | bool bar(const std::vector & y, 1263 | std::string ec = "black", 1264 | std::string ls = "-", 1265 | double lw = 1.0, 1266 | const std::map & keywords = {}) 1267 | { 1268 | using T = typename std::remove_reference::type::value_type; 1269 | 1270 | detail::_interpreter::get(); 1271 | 1272 | std::vector x; 1273 | for (std::size_t i = 0; i < y.size(); i++) { x.push_back(i); } 1274 | 1275 | return bar(x, y, ec, ls, lw, keywords); 1276 | } 1277 | 1278 | 1279 | template 1280 | bool barh(const std::vector &x, const std::vector &y, std::string ec = "black", std::string ls = "-", double lw = 1.0, const std::map &keywords = { }) { 1281 | PyObject *xarray = detail::get_array(x); 1282 | PyObject *yarray = detail::get_array(y); 1283 | 1284 | PyObject *kwargs = PyDict_New(); 1285 | 1286 | PyDict_SetItemString(kwargs, "ec", PyString_FromString(ec.c_str())); 1287 | PyDict_SetItemString(kwargs, "ls", PyString_FromString(ls.c_str())); 1288 | PyDict_SetItemString(kwargs, "lw", PyFloat_FromDouble(lw)); 1289 | 1290 | for (std::map::const_iterator it = keywords.begin(); it != keywords.end(); ++it) { 1291 | PyDict_SetItemString(kwargs, it->first.c_str(), PyUnicode_FromString(it->second.c_str())); 1292 | } 1293 | 1294 | PyObject *plot_args = PyTuple_New(2); 1295 | PyTuple_SetItem(plot_args, 0, xarray); 1296 | PyTuple_SetItem(plot_args, 1, yarray); 1297 | 1298 | PyObject *res = PyObject_Call(detail::_interpreter::get().s_python_function_barh, plot_args, kwargs); 1299 | 1300 | Py_DECREF(plot_args); 1301 | Py_DECREF(kwargs); 1302 | if (res) Py_DECREF(res); 1303 | 1304 | return res; 1305 | } 1306 | 1307 | 1308 | inline bool subplots_adjust(const std::map& keywords = {}) 1309 | { 1310 | detail::_interpreter::get(); 1311 | 1312 | PyObject* kwargs = PyDict_New(); 1313 | for (std::map::const_iterator it = 1314 | keywords.begin(); it != keywords.end(); ++it) { 1315 | PyDict_SetItemString(kwargs, it->first.c_str(), 1316 | PyFloat_FromDouble(it->second)); 1317 | } 1318 | 1319 | 1320 | PyObject* plot_args = PyTuple_New(0); 1321 | 1322 | PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_subplots_adjust, plot_args, kwargs); 1323 | 1324 | Py_DECREF(plot_args); 1325 | Py_DECREF(kwargs); 1326 | if(res) Py_DECREF(res); 1327 | 1328 | return res; 1329 | } 1330 | 1331 | template< typename Numeric> 1332 | bool named_hist(std::string label,const std::vector& y, long bins=10, std::string color="b", double alpha=1.0) 1333 | { 1334 | detail::_interpreter::get(); 1335 | 1336 | PyObject* yarray = detail::get_array(y); 1337 | 1338 | PyObject* kwargs = PyDict_New(); 1339 | PyDict_SetItemString(kwargs, "label", PyString_FromString(label.c_str())); 1340 | PyDict_SetItemString(kwargs, "bins", PyLong_FromLong(bins)); 1341 | PyDict_SetItemString(kwargs, "color", PyString_FromString(color.c_str())); 1342 | PyDict_SetItemString(kwargs, "alpha", PyFloat_FromDouble(alpha)); 1343 | 1344 | 1345 | PyObject* plot_args = PyTuple_New(1); 1346 | PyTuple_SetItem(plot_args, 0, yarray); 1347 | 1348 | PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_hist, plot_args, kwargs); 1349 | 1350 | Py_DECREF(plot_args); 1351 | Py_DECREF(kwargs); 1352 | if(res) Py_DECREF(res); 1353 | 1354 | return res; 1355 | } 1356 | 1357 | template 1358 | bool plot(const std::vector& x, const std::vector& y, const std::string& s = "") 1359 | { 1360 | assert(x.size() == y.size()); 1361 | 1362 | detail::_interpreter::get(); 1363 | 1364 | PyObject* xarray = detail::get_array(x); 1365 | PyObject* yarray = detail::get_array(y); 1366 | 1367 | PyObject* pystring = PyString_FromString(s.c_str()); 1368 | 1369 | PyObject* plot_args = PyTuple_New(3); 1370 | PyTuple_SetItem(plot_args, 0, xarray); 1371 | PyTuple_SetItem(plot_args, 1, yarray); 1372 | PyTuple_SetItem(plot_args, 2, pystring); 1373 | 1374 | PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_plot, plot_args); 1375 | 1376 | Py_DECREF(plot_args); 1377 | if(res) Py_DECREF(res); 1378 | 1379 | return res; 1380 | } 1381 | 1382 | template 1383 | bool contour(const std::vector& x, const std::vector& y, 1384 | const std::vector& z, 1385 | const std::map& keywords = {}) { 1386 | assert(x.size() == y.size() && x.size() == z.size()); 1387 | 1388 | PyObject* xarray = detail::get_array(x); 1389 | PyObject* yarray = detail::get_array(y); 1390 | PyObject* zarray = detail::get_array(z); 1391 | 1392 | PyObject* plot_args = PyTuple_New(3); 1393 | PyTuple_SetItem(plot_args, 0, xarray); 1394 | PyTuple_SetItem(plot_args, 1, yarray); 1395 | PyTuple_SetItem(plot_args, 2, zarray); 1396 | 1397 | // construct keyword args 1398 | PyObject* kwargs = PyDict_New(); 1399 | for (std::map::const_iterator it = keywords.begin(); 1400 | it != keywords.end(); ++it) { 1401 | PyDict_SetItemString(kwargs, it->first.c_str(), PyUnicode_FromString(it->second.c_str())); 1402 | } 1403 | 1404 | PyObject* res = 1405 | PyObject_Call(detail::_interpreter::get().s_python_function_contour, plot_args, kwargs); 1406 | 1407 | Py_DECREF(kwargs); 1408 | Py_DECREF(plot_args); 1409 | if (res) 1410 | Py_DECREF(res); 1411 | 1412 | return res; 1413 | } 1414 | 1415 | template 1416 | bool quiver(const std::vector& x, const std::vector& y, const std::vector& u, const std::vector& w, const std::map& keywords = {}) 1417 | { 1418 | assert(x.size() == y.size() && x.size() == u.size() && u.size() == w.size()); 1419 | 1420 | detail::_interpreter::get(); 1421 | 1422 | PyObject* xarray = detail::get_array(x); 1423 | PyObject* yarray = detail::get_array(y); 1424 | PyObject* uarray = detail::get_array(u); 1425 | PyObject* warray = detail::get_array(w); 1426 | 1427 | PyObject* plot_args = PyTuple_New(4); 1428 | PyTuple_SetItem(plot_args, 0, xarray); 1429 | PyTuple_SetItem(plot_args, 1, yarray); 1430 | PyTuple_SetItem(plot_args, 2, uarray); 1431 | PyTuple_SetItem(plot_args, 3, warray); 1432 | 1433 | // construct keyword args 1434 | PyObject* kwargs = PyDict_New(); 1435 | for(std::map::const_iterator it = keywords.begin(); it != keywords.end(); ++it) 1436 | { 1437 | PyDict_SetItemString(kwargs, it->first.c_str(), PyUnicode_FromString(it->second.c_str())); 1438 | } 1439 | 1440 | PyObject* res = PyObject_Call( 1441 | detail::_interpreter::get().s_python_function_quiver, plot_args, kwargs); 1442 | 1443 | Py_DECREF(kwargs); 1444 | Py_DECREF(plot_args); 1445 | if (res) 1446 | Py_DECREF(res); 1447 | 1448 | return res; 1449 | } 1450 | 1451 | template 1452 | bool quiver(const std::vector& x, const std::vector& y, const std::vector& z, const std::vector& u, const std::vector& w, const std::vector& v, const std::map& keywords = {}) 1453 | { 1454 | //set up 3d axes stuff 1455 | static PyObject *mpl_toolkitsmod = nullptr, *axis3dmod = nullptr; 1456 | if (!mpl_toolkitsmod) { 1457 | detail::_interpreter::get(); 1458 | 1459 | PyObject* mpl_toolkits = PyString_FromString("mpl_toolkits"); 1460 | PyObject* axis3d = PyString_FromString("mpl_toolkits.mplot3d"); 1461 | if (!mpl_toolkits || !axis3d) { throw std::runtime_error("couldnt create string"); } 1462 | 1463 | mpl_toolkitsmod = PyImport_Import(mpl_toolkits); 1464 | Py_DECREF(mpl_toolkits); 1465 | if (!mpl_toolkitsmod) { throw std::runtime_error("Error loading module mpl_toolkits!"); } 1466 | 1467 | axis3dmod = PyImport_Import(axis3d); 1468 | Py_DECREF(axis3d); 1469 | if (!axis3dmod) { throw std::runtime_error("Error loading module mpl_toolkits.mplot3d!"); } 1470 | } 1471 | 1472 | //assert sizes match up 1473 | assert(x.size() == y.size() && x.size() == u.size() && u.size() == w.size() && x.size() == z.size() && x.size() == v.size() && u.size() == v.size()); 1474 | 1475 | //set up parameters 1476 | detail::_interpreter::get(); 1477 | 1478 | PyObject* xarray = detail::get_array(x); 1479 | PyObject* yarray = detail::get_array(y); 1480 | PyObject* zarray = detail::get_array(z); 1481 | PyObject* uarray = detail::get_array(u); 1482 | PyObject* warray = detail::get_array(w); 1483 | PyObject* varray = detail::get_array(v); 1484 | 1485 | PyObject* plot_args = PyTuple_New(6); 1486 | PyTuple_SetItem(plot_args, 0, xarray); 1487 | PyTuple_SetItem(plot_args, 1, yarray); 1488 | PyTuple_SetItem(plot_args, 2, zarray); 1489 | PyTuple_SetItem(plot_args, 3, uarray); 1490 | PyTuple_SetItem(plot_args, 4, warray); 1491 | PyTuple_SetItem(plot_args, 5, varray); 1492 | 1493 | // construct keyword args 1494 | PyObject* kwargs = PyDict_New(); 1495 | for(std::map::const_iterator it = keywords.begin(); it != keywords.end(); ++it) 1496 | { 1497 | PyDict_SetItemString(kwargs, it->first.c_str(), PyUnicode_FromString(it->second.c_str())); 1498 | } 1499 | 1500 | //get figure gca to enable 3d projection 1501 | PyObject *fig = 1502 | PyObject_CallObject(detail::_interpreter::get().s_python_function_figure, 1503 | detail::_interpreter::get().s_python_empty_tuple); 1504 | if (!fig) throw std::runtime_error("Call to figure() failed."); 1505 | 1506 | PyObject *gca_kwargs = PyDict_New(); 1507 | PyDict_SetItemString(gca_kwargs, "projection", PyString_FromString("3d")); 1508 | 1509 | PyObject *gca = PyObject_GetAttrString(fig, "gca"); 1510 | if (!gca) throw std::runtime_error("No gca"); 1511 | Py_INCREF(gca); 1512 | PyObject *axis = PyObject_Call( 1513 | gca, detail::_interpreter::get().s_python_empty_tuple, gca_kwargs); 1514 | 1515 | if (!axis) throw std::runtime_error("No axis"); 1516 | Py_INCREF(axis); 1517 | Py_DECREF(gca); 1518 | Py_DECREF(gca_kwargs); 1519 | 1520 | //plot our boys bravely, plot them strongly, plot them with a wink and clap 1521 | PyObject *plot3 = PyObject_GetAttrString(axis, "quiver"); 1522 | if (!plot3) throw std::runtime_error("No 3D line plot"); 1523 | Py_INCREF(plot3); 1524 | PyObject* res = PyObject_Call( 1525 | plot3, plot_args, kwargs); 1526 | if (!res) throw std::runtime_error("Failed 3D plot"); 1527 | Py_DECREF(plot3); 1528 | Py_DECREF(axis); 1529 | Py_DECREF(kwargs); 1530 | Py_DECREF(plot_args); 1531 | if (res) 1532 | Py_DECREF(res); 1533 | 1534 | return res; 1535 | } 1536 | 1537 | template 1538 | bool stem(const std::vector& x, const std::vector& y, const std::string& s = "") 1539 | { 1540 | assert(x.size() == y.size()); 1541 | 1542 | detail::_interpreter::get(); 1543 | 1544 | PyObject* xarray = detail::get_array(x); 1545 | PyObject* yarray = detail::get_array(y); 1546 | 1547 | PyObject* pystring = PyString_FromString(s.c_str()); 1548 | 1549 | PyObject* plot_args = PyTuple_New(3); 1550 | PyTuple_SetItem(plot_args, 0, xarray); 1551 | PyTuple_SetItem(plot_args, 1, yarray); 1552 | PyTuple_SetItem(plot_args, 2, pystring); 1553 | 1554 | PyObject* res = PyObject_CallObject( 1555 | detail::_interpreter::get().s_python_function_stem, plot_args); 1556 | 1557 | Py_DECREF(plot_args); 1558 | if (res) 1559 | Py_DECREF(res); 1560 | 1561 | return res; 1562 | } 1563 | 1564 | template 1565 | bool semilogx(const std::vector& x, const std::vector& y, const std::string& s = "") 1566 | { 1567 | assert(x.size() == y.size()); 1568 | 1569 | detail::_interpreter::get(); 1570 | 1571 | PyObject* xarray = detail::get_array(x); 1572 | PyObject* yarray = detail::get_array(y); 1573 | 1574 | PyObject* pystring = PyString_FromString(s.c_str()); 1575 | 1576 | PyObject* plot_args = PyTuple_New(3); 1577 | PyTuple_SetItem(plot_args, 0, xarray); 1578 | PyTuple_SetItem(plot_args, 1, yarray); 1579 | PyTuple_SetItem(plot_args, 2, pystring); 1580 | 1581 | PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_semilogx, plot_args); 1582 | 1583 | Py_DECREF(plot_args); 1584 | if(res) Py_DECREF(res); 1585 | 1586 | return res; 1587 | } 1588 | 1589 | template 1590 | bool semilogy(const std::vector& x, const std::vector& y, const std::string& s = "") 1591 | { 1592 | assert(x.size() == y.size()); 1593 | 1594 | detail::_interpreter::get(); 1595 | 1596 | PyObject* xarray = detail::get_array(x); 1597 | PyObject* yarray = detail::get_array(y); 1598 | 1599 | PyObject* pystring = PyString_FromString(s.c_str()); 1600 | 1601 | PyObject* plot_args = PyTuple_New(3); 1602 | PyTuple_SetItem(plot_args, 0, xarray); 1603 | PyTuple_SetItem(plot_args, 1, yarray); 1604 | PyTuple_SetItem(plot_args, 2, pystring); 1605 | 1606 | PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_semilogy, plot_args); 1607 | 1608 | Py_DECREF(plot_args); 1609 | if(res) Py_DECREF(res); 1610 | 1611 | return res; 1612 | } 1613 | 1614 | template 1615 | bool loglog(const std::vector& x, const std::vector& y, const std::string& s = "") 1616 | { 1617 | assert(x.size() == y.size()); 1618 | 1619 | detail::_interpreter::get(); 1620 | 1621 | PyObject* xarray = detail::get_array(x); 1622 | PyObject* yarray = detail::get_array(y); 1623 | 1624 | PyObject* pystring = PyString_FromString(s.c_str()); 1625 | 1626 | PyObject* plot_args = PyTuple_New(3); 1627 | PyTuple_SetItem(plot_args, 0, xarray); 1628 | PyTuple_SetItem(plot_args, 1, yarray); 1629 | PyTuple_SetItem(plot_args, 2, pystring); 1630 | 1631 | PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_loglog, plot_args); 1632 | 1633 | Py_DECREF(plot_args); 1634 | if(res) Py_DECREF(res); 1635 | 1636 | return res; 1637 | } 1638 | 1639 | template 1640 | bool errorbar(const std::vector &x, const std::vector &y, const std::vector &yerr, const std::map &keywords = {}) 1641 | { 1642 | assert(x.size() == y.size()); 1643 | 1644 | detail::_interpreter::get(); 1645 | 1646 | PyObject* xarray = detail::get_array(x); 1647 | PyObject* yarray = detail::get_array(y); 1648 | PyObject* yerrarray = detail::get_array(yerr); 1649 | 1650 | // construct keyword args 1651 | PyObject* kwargs = PyDict_New(); 1652 | for(std::map::const_iterator it = keywords.begin(); it != keywords.end(); ++it) 1653 | { 1654 | PyDict_SetItemString(kwargs, it->first.c_str(), PyString_FromString(it->second.c_str())); 1655 | } 1656 | 1657 | PyDict_SetItemString(kwargs, "yerr", yerrarray); 1658 | 1659 | PyObject *plot_args = PyTuple_New(2); 1660 | PyTuple_SetItem(plot_args, 0, xarray); 1661 | PyTuple_SetItem(plot_args, 1, yarray); 1662 | 1663 | PyObject *res = PyObject_Call(detail::_interpreter::get().s_python_function_errorbar, plot_args, kwargs); 1664 | 1665 | Py_DECREF(kwargs); 1666 | Py_DECREF(plot_args); 1667 | 1668 | if (res) 1669 | Py_DECREF(res); 1670 | else 1671 | throw std::runtime_error("Call to errorbar() failed."); 1672 | 1673 | return res; 1674 | } 1675 | 1676 | template 1677 | bool named_plot(const std::string& name, const std::vector& y, const std::string& format = "") 1678 | { 1679 | detail::_interpreter::get(); 1680 | 1681 | PyObject* kwargs = PyDict_New(); 1682 | PyDict_SetItemString(kwargs, "label", PyString_FromString(name.c_str())); 1683 | 1684 | PyObject* yarray = detail::get_array(y); 1685 | 1686 | PyObject* pystring = PyString_FromString(format.c_str()); 1687 | 1688 | PyObject* plot_args = PyTuple_New(2); 1689 | 1690 | PyTuple_SetItem(plot_args, 0, yarray); 1691 | PyTuple_SetItem(plot_args, 1, pystring); 1692 | 1693 | PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_plot, plot_args, kwargs); 1694 | 1695 | Py_DECREF(kwargs); 1696 | Py_DECREF(plot_args); 1697 | if (res) Py_DECREF(res); 1698 | 1699 | return res; 1700 | } 1701 | 1702 | template 1703 | bool named_plot(const std::string& name, const std::vector& x, const std::vector& y, const std::string& format = "") 1704 | { 1705 | detail::_interpreter::get(); 1706 | 1707 | PyObject* kwargs = PyDict_New(); 1708 | PyDict_SetItemString(kwargs, "label", PyString_FromString(name.c_str())); 1709 | 1710 | PyObject* xarray = detail::get_array(x); 1711 | PyObject* yarray = detail::get_array(y); 1712 | 1713 | PyObject* pystring = PyString_FromString(format.c_str()); 1714 | 1715 | PyObject* plot_args = PyTuple_New(3); 1716 | PyTuple_SetItem(plot_args, 0, xarray); 1717 | PyTuple_SetItem(plot_args, 1, yarray); 1718 | PyTuple_SetItem(plot_args, 2, pystring); 1719 | 1720 | PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_plot, plot_args, kwargs); 1721 | 1722 | Py_DECREF(kwargs); 1723 | Py_DECREF(plot_args); 1724 | if (res) Py_DECREF(res); 1725 | 1726 | return res; 1727 | } 1728 | 1729 | template 1730 | bool named_semilogx(const std::string& name, const std::vector& x, const std::vector& y, const std::string& format = "") 1731 | { 1732 | detail::_interpreter::get(); 1733 | 1734 | PyObject* kwargs = PyDict_New(); 1735 | PyDict_SetItemString(kwargs, "label", PyString_FromString(name.c_str())); 1736 | 1737 | PyObject* xarray = detail::get_array(x); 1738 | PyObject* yarray = detail::get_array(y); 1739 | 1740 | PyObject* pystring = PyString_FromString(format.c_str()); 1741 | 1742 | PyObject* plot_args = PyTuple_New(3); 1743 | PyTuple_SetItem(plot_args, 0, xarray); 1744 | PyTuple_SetItem(plot_args, 1, yarray); 1745 | PyTuple_SetItem(plot_args, 2, pystring); 1746 | 1747 | PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_semilogx, plot_args, kwargs); 1748 | 1749 | Py_DECREF(kwargs); 1750 | Py_DECREF(plot_args); 1751 | if (res) Py_DECREF(res); 1752 | 1753 | return res; 1754 | } 1755 | 1756 | template 1757 | bool named_semilogy(const std::string& name, const std::vector& x, const std::vector& y, const std::string& format = "") 1758 | { 1759 | detail::_interpreter::get(); 1760 | 1761 | PyObject* kwargs = PyDict_New(); 1762 | PyDict_SetItemString(kwargs, "label", PyString_FromString(name.c_str())); 1763 | 1764 | PyObject* xarray = detail::get_array(x); 1765 | PyObject* yarray = detail::get_array(y); 1766 | 1767 | PyObject* pystring = PyString_FromString(format.c_str()); 1768 | 1769 | PyObject* plot_args = PyTuple_New(3); 1770 | PyTuple_SetItem(plot_args, 0, xarray); 1771 | PyTuple_SetItem(plot_args, 1, yarray); 1772 | PyTuple_SetItem(plot_args, 2, pystring); 1773 | 1774 | PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_semilogy, plot_args, kwargs); 1775 | 1776 | Py_DECREF(kwargs); 1777 | Py_DECREF(plot_args); 1778 | if (res) Py_DECREF(res); 1779 | 1780 | return res; 1781 | } 1782 | 1783 | template 1784 | bool named_loglog(const std::string& name, const std::vector& x, const std::vector& y, const std::string& format = "") 1785 | { 1786 | detail::_interpreter::get(); 1787 | 1788 | PyObject* kwargs = PyDict_New(); 1789 | PyDict_SetItemString(kwargs, "label", PyString_FromString(name.c_str())); 1790 | 1791 | PyObject* xarray = detail::get_array(x); 1792 | PyObject* yarray = detail::get_array(y); 1793 | 1794 | PyObject* pystring = PyString_FromString(format.c_str()); 1795 | 1796 | PyObject* plot_args = PyTuple_New(3); 1797 | PyTuple_SetItem(plot_args, 0, xarray); 1798 | PyTuple_SetItem(plot_args, 1, yarray); 1799 | PyTuple_SetItem(plot_args, 2, pystring); 1800 | PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_loglog, plot_args, kwargs); 1801 | 1802 | Py_DECREF(kwargs); 1803 | Py_DECREF(plot_args); 1804 | if (res) Py_DECREF(res); 1805 | 1806 | return res; 1807 | } 1808 | 1809 | template 1810 | bool plot(const std::vector& y, const std::string& format = "") 1811 | { 1812 | std::vector x(y.size()); 1813 | for(size_t i=0; i 1818 | bool plot(const std::vector& y, const std::map& keywords) 1819 | { 1820 | std::vector x(y.size()); 1821 | for(size_t i=0; i 1826 | bool stem(const std::vector& y, const std::string& format = "") 1827 | { 1828 | std::vector x(y.size()); 1829 | for (size_t i = 0; i < x.size(); ++i) x.at(i) = i; 1830 | return stem(x, y, format); 1831 | } 1832 | 1833 | template 1834 | void text(Numeric x, Numeric y, const std::string& s = "") 1835 | { 1836 | detail::_interpreter::get(); 1837 | 1838 | PyObject* args = PyTuple_New(3); 1839 | PyTuple_SetItem(args, 0, PyFloat_FromDouble(x)); 1840 | PyTuple_SetItem(args, 1, PyFloat_FromDouble(y)); 1841 | PyTuple_SetItem(args, 2, PyString_FromString(s.c_str())); 1842 | 1843 | PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_text, args); 1844 | if(!res) throw std::runtime_error("Call to text() failed."); 1845 | 1846 | Py_DECREF(args); 1847 | Py_DECREF(res); 1848 | } 1849 | 1850 | inline void colorbar(PyObject* mappable = NULL, const std::map& keywords = {}) 1851 | { 1852 | if (mappable == NULL) 1853 | throw std::runtime_error("Must call colorbar with PyObject* returned from an image, contour, surface, etc."); 1854 | 1855 | detail::_interpreter::get(); 1856 | 1857 | PyObject* args = PyTuple_New(1); 1858 | PyTuple_SetItem(args, 0, mappable); 1859 | 1860 | PyObject* kwargs = PyDict_New(); 1861 | for(std::map::const_iterator it = keywords.begin(); it != keywords.end(); ++it) 1862 | { 1863 | PyDict_SetItemString(kwargs, it->first.c_str(), PyFloat_FromDouble(it->second)); 1864 | } 1865 | 1866 | PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_colorbar, args, kwargs); 1867 | if(!res) throw std::runtime_error("Call to colorbar() failed."); 1868 | 1869 | Py_DECREF(args); 1870 | Py_DECREF(kwargs); 1871 | Py_DECREF(res); 1872 | } 1873 | 1874 | 1875 | inline long figure(long number = -1) 1876 | { 1877 | detail::_interpreter::get(); 1878 | 1879 | PyObject *res; 1880 | if (number == -1) 1881 | res = PyObject_CallObject(detail::_interpreter::get().s_python_function_figure, detail::_interpreter::get().s_python_empty_tuple); 1882 | else { 1883 | assert(number > 0); 1884 | 1885 | // Make sure interpreter is initialised 1886 | detail::_interpreter::get(); 1887 | 1888 | PyObject *args = PyTuple_New(1); 1889 | PyTuple_SetItem(args, 0, PyLong_FromLong(number)); 1890 | res = PyObject_CallObject(detail::_interpreter::get().s_python_function_figure, args); 1891 | Py_DECREF(args); 1892 | } 1893 | 1894 | if(!res) throw std::runtime_error("Call to figure() failed."); 1895 | 1896 | PyObject* num = PyObject_GetAttrString(res, "number"); 1897 | if (!num) throw std::runtime_error("Could not get number attribute of figure object"); 1898 | const long figureNumber = PyLong_AsLong(num); 1899 | 1900 | Py_DECREF(num); 1901 | Py_DECREF(res); 1902 | 1903 | return figureNumber; 1904 | } 1905 | 1906 | inline bool fignum_exists(long number) 1907 | { 1908 | detail::_interpreter::get(); 1909 | 1910 | PyObject *args = PyTuple_New(1); 1911 | PyTuple_SetItem(args, 0, PyLong_FromLong(number)); 1912 | PyObject *res = PyObject_CallObject(detail::_interpreter::get().s_python_function_fignum_exists, args); 1913 | if(!res) throw std::runtime_error("Call to fignum_exists() failed."); 1914 | 1915 | bool ret = PyObject_IsTrue(res); 1916 | Py_DECREF(res); 1917 | Py_DECREF(args); 1918 | 1919 | return ret; 1920 | } 1921 | 1922 | inline void figure_size(size_t w, size_t h) 1923 | { 1924 | detail::_interpreter::get(); 1925 | 1926 | const size_t dpi = 100; 1927 | PyObject* size = PyTuple_New(2); 1928 | PyTuple_SetItem(size, 0, PyFloat_FromDouble((double)w / dpi)); 1929 | PyTuple_SetItem(size, 1, PyFloat_FromDouble((double)h / dpi)); 1930 | 1931 | PyObject* kwargs = PyDict_New(); 1932 | PyDict_SetItemString(kwargs, "figsize", size); 1933 | PyDict_SetItemString(kwargs, "dpi", PyLong_FromSize_t(dpi)); 1934 | 1935 | PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_figure, 1936 | detail::_interpreter::get().s_python_empty_tuple, kwargs); 1937 | 1938 | Py_DECREF(kwargs); 1939 | 1940 | if(!res) throw std::runtime_error("Call to figure_size() failed."); 1941 | Py_DECREF(res); 1942 | } 1943 | 1944 | inline void legend() 1945 | { 1946 | detail::_interpreter::get(); 1947 | 1948 | PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_legend, detail::_interpreter::get().s_python_empty_tuple); 1949 | if(!res) throw std::runtime_error("Call to legend() failed."); 1950 | 1951 | Py_DECREF(res); 1952 | } 1953 | 1954 | inline void legend(const std::map& keywords) 1955 | { 1956 | detail::_interpreter::get(); 1957 | 1958 | // construct keyword args 1959 | PyObject* kwargs = PyDict_New(); 1960 | for(std::map::const_iterator it = keywords.begin(); it != keywords.end(); ++it) 1961 | { 1962 | PyDict_SetItemString(kwargs, it->first.c_str(), PyString_FromString(it->second.c_str())); 1963 | } 1964 | 1965 | PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_legend, detail::_interpreter::get().s_python_empty_tuple, kwargs); 1966 | if(!res) throw std::runtime_error("Call to legend() failed."); 1967 | 1968 | Py_DECREF(kwargs); 1969 | Py_DECREF(res); 1970 | } 1971 | 1972 | template 1973 | inline void set_aspect(Numeric ratio) 1974 | { 1975 | detail::_interpreter::get(); 1976 | 1977 | PyObject* args = PyTuple_New(1); 1978 | PyTuple_SetItem(args, 0, PyFloat_FromDouble(ratio)); 1979 | PyObject* kwargs = PyDict_New(); 1980 | 1981 | PyObject *ax = 1982 | PyObject_CallObject(detail::_interpreter::get().s_python_function_gca, 1983 | detail::_interpreter::get().s_python_empty_tuple); 1984 | if (!ax) throw std::runtime_error("Call to gca() failed."); 1985 | Py_INCREF(ax); 1986 | 1987 | PyObject *set_aspect = PyObject_GetAttrString(ax, "set_aspect"); 1988 | if (!set_aspect) throw std::runtime_error("Attribute set_aspect not found."); 1989 | Py_INCREF(set_aspect); 1990 | 1991 | PyObject *res = PyObject_Call(set_aspect, args, kwargs); 1992 | if (!res) throw std::runtime_error("Call to set_aspect() failed."); 1993 | Py_DECREF(set_aspect); 1994 | 1995 | Py_DECREF(ax); 1996 | Py_DECREF(args); 1997 | Py_DECREF(kwargs); 1998 | } 1999 | 2000 | inline void set_aspect_equal() 2001 | { 2002 | // expect ratio == "equal". Leaving error handling to matplotlib. 2003 | detail::_interpreter::get(); 2004 | 2005 | PyObject* args = PyTuple_New(1); 2006 | PyTuple_SetItem(args, 0, PyString_FromString("equal")); 2007 | PyObject* kwargs = PyDict_New(); 2008 | 2009 | PyObject *ax = 2010 | PyObject_CallObject(detail::_interpreter::get().s_python_function_gca, 2011 | detail::_interpreter::get().s_python_empty_tuple); 2012 | if (!ax) throw std::runtime_error("Call to gca() failed."); 2013 | Py_INCREF(ax); 2014 | 2015 | PyObject *set_aspect = PyObject_GetAttrString(ax, "set_aspect"); 2016 | if (!set_aspect) throw std::runtime_error("Attribute set_aspect not found."); 2017 | Py_INCREF(set_aspect); 2018 | 2019 | PyObject *res = PyObject_Call(set_aspect, args, kwargs); 2020 | if (!res) throw std::runtime_error("Call to set_aspect() failed."); 2021 | Py_DECREF(set_aspect); 2022 | 2023 | Py_DECREF(ax); 2024 | Py_DECREF(args); 2025 | Py_DECREF(kwargs); 2026 | } 2027 | 2028 | template 2029 | void ylim(Numeric left, Numeric right) 2030 | { 2031 | detail::_interpreter::get(); 2032 | 2033 | PyObject* list = PyList_New(2); 2034 | PyList_SetItem(list, 0, PyFloat_FromDouble(left)); 2035 | PyList_SetItem(list, 1, PyFloat_FromDouble(right)); 2036 | 2037 | PyObject* args = PyTuple_New(1); 2038 | PyTuple_SetItem(args, 0, list); 2039 | 2040 | PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_ylim, args); 2041 | if(!res) throw std::runtime_error("Call to ylim() failed."); 2042 | 2043 | Py_DECREF(args); 2044 | Py_DECREF(res); 2045 | } 2046 | 2047 | template 2048 | void xlim(Numeric left, Numeric right) 2049 | { 2050 | detail::_interpreter::get(); 2051 | 2052 | PyObject* list = PyList_New(2); 2053 | PyList_SetItem(list, 0, PyFloat_FromDouble(left)); 2054 | PyList_SetItem(list, 1, PyFloat_FromDouble(right)); 2055 | 2056 | PyObject* args = PyTuple_New(1); 2057 | PyTuple_SetItem(args, 0, list); 2058 | 2059 | PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_xlim, args); 2060 | if(!res) throw std::runtime_error("Call to xlim() failed."); 2061 | 2062 | Py_DECREF(args); 2063 | Py_DECREF(res); 2064 | } 2065 | 2066 | 2067 | inline std::array xlim() 2068 | { 2069 | PyObject* args = PyTuple_New(0); 2070 | PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_xlim, args); 2071 | 2072 | if(!res) throw std::runtime_error("Call to xlim() failed."); 2073 | 2074 | Py_DECREF(res); 2075 | 2076 | PyObject* left = PyTuple_GetItem(res,0); 2077 | PyObject* right = PyTuple_GetItem(res,1); 2078 | return { PyFloat_AsDouble(left), PyFloat_AsDouble(right) }; 2079 | } 2080 | 2081 | 2082 | inline std::array ylim() 2083 | { 2084 | PyObject* args = PyTuple_New(0); 2085 | PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_ylim, args); 2086 | 2087 | if(!res) throw std::runtime_error("Call to ylim() failed."); 2088 | 2089 | Py_DECREF(res); 2090 | 2091 | PyObject* left = PyTuple_GetItem(res,0); 2092 | PyObject* right = PyTuple_GetItem(res,1); 2093 | return { PyFloat_AsDouble(left), PyFloat_AsDouble(right) }; 2094 | } 2095 | 2096 | template 2097 | inline void xticks(const std::vector &ticks, const std::vector &labels = {}, const std::map& keywords = {}) 2098 | { 2099 | assert(labels.size() == 0 || ticks.size() == labels.size()); 2100 | 2101 | detail::_interpreter::get(); 2102 | 2103 | // using numpy array 2104 | PyObject* ticksarray = detail::get_array(ticks); 2105 | 2106 | PyObject* args; 2107 | if(labels.size() == 0) { 2108 | // construct positional args 2109 | args = PyTuple_New(1); 2110 | PyTuple_SetItem(args, 0, ticksarray); 2111 | } else { 2112 | // make tuple of tick labels 2113 | PyObject* labelstuple = PyTuple_New(labels.size()); 2114 | for (size_t i = 0; i < labels.size(); i++) 2115 | PyTuple_SetItem(labelstuple, i, PyUnicode_FromString(labels[i].c_str())); 2116 | 2117 | // construct positional args 2118 | args = PyTuple_New(2); 2119 | PyTuple_SetItem(args, 0, ticksarray); 2120 | PyTuple_SetItem(args, 1, labelstuple); 2121 | } 2122 | 2123 | // construct keyword args 2124 | PyObject* kwargs = PyDict_New(); 2125 | for(std::map::const_iterator it = keywords.begin(); it != keywords.end(); ++it) 2126 | { 2127 | PyDict_SetItemString(kwargs, it->first.c_str(), PyString_FromString(it->second.c_str())); 2128 | } 2129 | 2130 | PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_xticks, args, kwargs); 2131 | 2132 | Py_DECREF(args); 2133 | Py_DECREF(kwargs); 2134 | if(!res) throw std::runtime_error("Call to xticks() failed"); 2135 | 2136 | Py_DECREF(res); 2137 | } 2138 | 2139 | template 2140 | inline void xticks(const std::vector &ticks, const std::map& keywords) 2141 | { 2142 | xticks(ticks, {}, keywords); 2143 | } 2144 | 2145 | template 2146 | inline void yticks(const std::vector &ticks, const std::vector &labels = {}, const std::map& keywords = {}) 2147 | { 2148 | assert(labels.size() == 0 || ticks.size() == labels.size()); 2149 | 2150 | detail::_interpreter::get(); 2151 | 2152 | // using numpy array 2153 | PyObject* ticksarray = detail::get_array(ticks); 2154 | 2155 | PyObject* args; 2156 | if(labels.size() == 0) { 2157 | // construct positional args 2158 | args = PyTuple_New(1); 2159 | PyTuple_SetItem(args, 0, ticksarray); 2160 | } else { 2161 | // make tuple of tick labels 2162 | PyObject* labelstuple = PyTuple_New(labels.size()); 2163 | for (size_t i = 0; i < labels.size(); i++) 2164 | PyTuple_SetItem(labelstuple, i, PyUnicode_FromString(labels[i].c_str())); 2165 | 2166 | // construct positional args 2167 | args = PyTuple_New(2); 2168 | PyTuple_SetItem(args, 0, ticksarray); 2169 | PyTuple_SetItem(args, 1, labelstuple); 2170 | } 2171 | 2172 | // construct keyword args 2173 | PyObject* kwargs = PyDict_New(); 2174 | for(std::map::const_iterator it = keywords.begin(); it != keywords.end(); ++it) 2175 | { 2176 | PyDict_SetItemString(kwargs, it->first.c_str(), PyString_FromString(it->second.c_str())); 2177 | } 2178 | 2179 | PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_yticks, args, kwargs); 2180 | 2181 | Py_DECREF(args); 2182 | Py_DECREF(kwargs); 2183 | if(!res) throw std::runtime_error("Call to yticks() failed"); 2184 | 2185 | Py_DECREF(res); 2186 | } 2187 | 2188 | template 2189 | inline void yticks(const std::vector &ticks, const std::map& keywords) 2190 | { 2191 | yticks(ticks, {}, keywords); 2192 | } 2193 | 2194 | template inline void margins(Numeric margin) 2195 | { 2196 | // construct positional args 2197 | PyObject* args = PyTuple_New(1); 2198 | PyTuple_SetItem(args, 0, PyFloat_FromDouble(margin)); 2199 | 2200 | PyObject* res = 2201 | PyObject_CallObject(detail::_interpreter::get().s_python_function_margins, args); 2202 | if (!res) 2203 | throw std::runtime_error("Call to margins() failed."); 2204 | 2205 | Py_DECREF(args); 2206 | Py_DECREF(res); 2207 | } 2208 | 2209 | template inline void margins(Numeric margin_x, Numeric margin_y) 2210 | { 2211 | // construct positional args 2212 | PyObject* args = PyTuple_New(2); 2213 | PyTuple_SetItem(args, 0, PyFloat_FromDouble(margin_x)); 2214 | PyTuple_SetItem(args, 1, PyFloat_FromDouble(margin_y)); 2215 | 2216 | PyObject* res = 2217 | PyObject_CallObject(detail::_interpreter::get().s_python_function_margins, args); 2218 | if (!res) 2219 | throw std::runtime_error("Call to margins() failed."); 2220 | 2221 | Py_DECREF(args); 2222 | Py_DECREF(res); 2223 | } 2224 | 2225 | 2226 | inline void tick_params(const std::map& keywords, const std::string axis = "both") 2227 | { 2228 | detail::_interpreter::get(); 2229 | 2230 | // construct positional args 2231 | PyObject* args; 2232 | args = PyTuple_New(1); 2233 | PyTuple_SetItem(args, 0, PyString_FromString(axis.c_str())); 2234 | 2235 | // construct keyword args 2236 | PyObject* kwargs = PyDict_New(); 2237 | for (std::map::const_iterator it = keywords.begin(); it != keywords.end(); ++it) 2238 | { 2239 | PyDict_SetItemString(kwargs, it->first.c_str(), PyString_FromString(it->second.c_str())); 2240 | } 2241 | 2242 | 2243 | PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_tick_params, args, kwargs); 2244 | 2245 | Py_DECREF(args); 2246 | Py_DECREF(kwargs); 2247 | if (!res) throw std::runtime_error("Call to tick_params() failed"); 2248 | 2249 | Py_DECREF(res); 2250 | } 2251 | 2252 | inline void subplot(long nrows, long ncols, long plot_number) 2253 | { 2254 | detail::_interpreter::get(); 2255 | 2256 | // construct positional args 2257 | PyObject* args = PyTuple_New(3); 2258 | PyTuple_SetItem(args, 0, PyFloat_FromDouble(nrows)); 2259 | PyTuple_SetItem(args, 1, PyFloat_FromDouble(ncols)); 2260 | PyTuple_SetItem(args, 2, PyFloat_FromDouble(plot_number)); 2261 | 2262 | PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_subplot, args); 2263 | if(!res) throw std::runtime_error("Call to subplot() failed."); 2264 | 2265 | Py_DECREF(args); 2266 | Py_DECREF(res); 2267 | } 2268 | 2269 | inline void subplot2grid(long nrows, long ncols, long rowid=0, long colid=0, long rowspan=1, long colspan=1) 2270 | { 2271 | detail::_interpreter::get(); 2272 | 2273 | PyObject* shape = PyTuple_New(2); 2274 | PyTuple_SetItem(shape, 0, PyLong_FromLong(nrows)); 2275 | PyTuple_SetItem(shape, 1, PyLong_FromLong(ncols)); 2276 | 2277 | PyObject* loc = PyTuple_New(2); 2278 | PyTuple_SetItem(loc, 0, PyLong_FromLong(rowid)); 2279 | PyTuple_SetItem(loc, 1, PyLong_FromLong(colid)); 2280 | 2281 | PyObject* args = PyTuple_New(4); 2282 | PyTuple_SetItem(args, 0, shape); 2283 | PyTuple_SetItem(args, 1, loc); 2284 | PyTuple_SetItem(args, 2, PyLong_FromLong(rowspan)); 2285 | PyTuple_SetItem(args, 3, PyLong_FromLong(colspan)); 2286 | 2287 | PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_subplot2grid, args); 2288 | if(!res) throw std::runtime_error("Call to subplot2grid() failed."); 2289 | 2290 | Py_DECREF(shape); 2291 | Py_DECREF(loc); 2292 | Py_DECREF(args); 2293 | Py_DECREF(res); 2294 | } 2295 | 2296 | inline void title(const std::string &titlestr, const std::map &keywords = {}) 2297 | { 2298 | detail::_interpreter::get(); 2299 | 2300 | PyObject* pytitlestr = PyString_FromString(titlestr.c_str()); 2301 | PyObject* args = PyTuple_New(1); 2302 | PyTuple_SetItem(args, 0, pytitlestr); 2303 | 2304 | PyObject* kwargs = PyDict_New(); 2305 | for (auto it = keywords.begin(); it != keywords.end(); ++it) { 2306 | PyDict_SetItemString(kwargs, it->first.c_str(), PyUnicode_FromString(it->second.c_str())); 2307 | } 2308 | 2309 | PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_title, args, kwargs); 2310 | if(!res) throw std::runtime_error("Call to title() failed."); 2311 | 2312 | Py_DECREF(args); 2313 | Py_DECREF(kwargs); 2314 | Py_DECREF(res); 2315 | } 2316 | 2317 | inline void suptitle(const std::string &suptitlestr, const std::map &keywords = {}) 2318 | { 2319 | detail::_interpreter::get(); 2320 | 2321 | PyObject* pysuptitlestr = PyString_FromString(suptitlestr.c_str()); 2322 | PyObject* args = PyTuple_New(1); 2323 | PyTuple_SetItem(args, 0, pysuptitlestr); 2324 | 2325 | PyObject* kwargs = PyDict_New(); 2326 | for (auto it = keywords.begin(); it != keywords.end(); ++it) { 2327 | PyDict_SetItemString(kwargs, it->first.c_str(), PyUnicode_FromString(it->second.c_str())); 2328 | } 2329 | 2330 | PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_suptitle, args, kwargs); 2331 | if(!res) throw std::runtime_error("Call to suptitle() failed."); 2332 | 2333 | Py_DECREF(args); 2334 | Py_DECREF(kwargs); 2335 | Py_DECREF(res); 2336 | } 2337 | 2338 | inline void axis(const std::string &axisstr) 2339 | { 2340 | detail::_interpreter::get(); 2341 | 2342 | PyObject* str = PyString_FromString(axisstr.c_str()); 2343 | PyObject* args = PyTuple_New(1); 2344 | PyTuple_SetItem(args, 0, str); 2345 | 2346 | PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_axis, args); 2347 | if(!res) throw std::runtime_error("Call to title() failed."); 2348 | 2349 | Py_DECREF(args); 2350 | Py_DECREF(res); 2351 | } 2352 | 2353 | inline void axhline(double y, double xmin = 0., double xmax = 1., const std::map& keywords = std::map()) 2354 | { 2355 | detail::_interpreter::get(); 2356 | 2357 | // construct positional args 2358 | PyObject* args = PyTuple_New(3); 2359 | PyTuple_SetItem(args, 0, PyFloat_FromDouble(y)); 2360 | PyTuple_SetItem(args, 1, PyFloat_FromDouble(xmin)); 2361 | PyTuple_SetItem(args, 2, PyFloat_FromDouble(xmax)); 2362 | 2363 | // construct keyword args 2364 | PyObject* kwargs = PyDict_New(); 2365 | for(std::map::const_iterator it = keywords.begin(); it != keywords.end(); ++it) 2366 | { 2367 | PyDict_SetItemString(kwargs, it->first.c_str(), PyString_FromString(it->second.c_str())); 2368 | } 2369 | 2370 | PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_axhline, args, kwargs); 2371 | 2372 | Py_DECREF(args); 2373 | Py_DECREF(kwargs); 2374 | 2375 | if(res) Py_DECREF(res); 2376 | } 2377 | 2378 | inline void axvline(double x, double ymin = 0., double ymax = 1., const std::map& keywords = std::map()) 2379 | { 2380 | detail::_interpreter::get(); 2381 | 2382 | // construct positional args 2383 | PyObject* args = PyTuple_New(3); 2384 | PyTuple_SetItem(args, 0, PyFloat_FromDouble(x)); 2385 | PyTuple_SetItem(args, 1, PyFloat_FromDouble(ymin)); 2386 | PyTuple_SetItem(args, 2, PyFloat_FromDouble(ymax)); 2387 | 2388 | // construct keyword args 2389 | PyObject* kwargs = PyDict_New(); 2390 | for(std::map::const_iterator it = keywords.begin(); it != keywords.end(); ++it) 2391 | { 2392 | PyDict_SetItemString(kwargs, it->first.c_str(), PyString_FromString(it->second.c_str())); 2393 | } 2394 | 2395 | PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_axvline, args, kwargs); 2396 | 2397 | Py_DECREF(args); 2398 | Py_DECREF(kwargs); 2399 | 2400 | if(res) Py_DECREF(res); 2401 | } 2402 | 2403 | inline void axvspan(double xmin, double xmax, double ymin = 0., double ymax = 1., const std::map& keywords = std::map()) 2404 | { 2405 | // construct positional args 2406 | PyObject* args = PyTuple_New(4); 2407 | PyTuple_SetItem(args, 0, PyFloat_FromDouble(xmin)); 2408 | PyTuple_SetItem(args, 1, PyFloat_FromDouble(xmax)); 2409 | PyTuple_SetItem(args, 2, PyFloat_FromDouble(ymin)); 2410 | PyTuple_SetItem(args, 3, PyFloat_FromDouble(ymax)); 2411 | 2412 | // construct keyword args 2413 | PyObject* kwargs = PyDict_New(); 2414 | for (auto it = keywords.begin(); it != keywords.end(); ++it) { 2415 | if (it->first == "linewidth" || it->first == "alpha") { 2416 | PyDict_SetItemString(kwargs, it->first.c_str(), 2417 | PyFloat_FromDouble(std::stod(it->second))); 2418 | } else { 2419 | PyDict_SetItemString(kwargs, it->first.c_str(), 2420 | PyString_FromString(it->second.c_str())); 2421 | } 2422 | } 2423 | 2424 | PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_axvspan, args, kwargs); 2425 | Py_DECREF(args); 2426 | Py_DECREF(kwargs); 2427 | 2428 | if(res) Py_DECREF(res); 2429 | } 2430 | 2431 | inline void xlabel(const std::string &str, const std::map &keywords = {}) 2432 | { 2433 | detail::_interpreter::get(); 2434 | 2435 | PyObject* pystr = PyString_FromString(str.c_str()); 2436 | PyObject* args = PyTuple_New(1); 2437 | PyTuple_SetItem(args, 0, pystr); 2438 | 2439 | PyObject* kwargs = PyDict_New(); 2440 | for (auto it = keywords.begin(); it != keywords.end(); ++it) { 2441 | PyDict_SetItemString(kwargs, it->first.c_str(), PyUnicode_FromString(it->second.c_str())); 2442 | } 2443 | 2444 | PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_xlabel, args, kwargs); 2445 | if(!res) throw std::runtime_error("Call to xlabel() failed."); 2446 | 2447 | Py_DECREF(args); 2448 | Py_DECREF(kwargs); 2449 | Py_DECREF(res); 2450 | } 2451 | 2452 | inline void ylabel(const std::string &str, const std::map& keywords = {}) 2453 | { 2454 | detail::_interpreter::get(); 2455 | 2456 | PyObject* pystr = PyString_FromString(str.c_str()); 2457 | PyObject* args = PyTuple_New(1); 2458 | PyTuple_SetItem(args, 0, pystr); 2459 | 2460 | PyObject* kwargs = PyDict_New(); 2461 | for (auto it = keywords.begin(); it != keywords.end(); ++it) { 2462 | PyDict_SetItemString(kwargs, it->first.c_str(), PyUnicode_FromString(it->second.c_str())); 2463 | } 2464 | 2465 | PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_ylabel, args, kwargs); 2466 | if(!res) throw std::runtime_error("Call to ylabel() failed."); 2467 | 2468 | Py_DECREF(args); 2469 | Py_DECREF(kwargs); 2470 | Py_DECREF(res); 2471 | } 2472 | 2473 | inline void set_zlabel(const std::string &str, const std::map& keywords = {}) 2474 | { 2475 | detail::_interpreter::get(); 2476 | 2477 | // Same as with plot_surface: We lazily load the modules here the first time 2478 | // this function is called because I'm not sure that we can assume "matplotlib 2479 | // installed" implies "mpl_toolkits installed" on all platforms, and we don't 2480 | // want to require it for people who don't need 3d plots. 2481 | static PyObject *mpl_toolkitsmod = nullptr, *axis3dmod = nullptr; 2482 | if (!mpl_toolkitsmod) { 2483 | PyObject* mpl_toolkits = PyString_FromString("mpl_toolkits"); 2484 | PyObject* axis3d = PyString_FromString("mpl_toolkits.mplot3d"); 2485 | if (!mpl_toolkits || !axis3d) { throw std::runtime_error("couldnt create string"); } 2486 | 2487 | mpl_toolkitsmod = PyImport_Import(mpl_toolkits); 2488 | Py_DECREF(mpl_toolkits); 2489 | if (!mpl_toolkitsmod) { throw std::runtime_error("Error loading module mpl_toolkits!"); } 2490 | 2491 | axis3dmod = PyImport_Import(axis3d); 2492 | Py_DECREF(axis3d); 2493 | if (!axis3dmod) { throw std::runtime_error("Error loading module mpl_toolkits.mplot3d!"); } 2494 | } 2495 | 2496 | PyObject* pystr = PyString_FromString(str.c_str()); 2497 | PyObject* args = PyTuple_New(1); 2498 | PyTuple_SetItem(args, 0, pystr); 2499 | 2500 | PyObject* kwargs = PyDict_New(); 2501 | for (auto it = keywords.begin(); it != keywords.end(); ++it) { 2502 | PyDict_SetItemString(kwargs, it->first.c_str(), PyUnicode_FromString(it->second.c_str())); 2503 | } 2504 | 2505 | PyObject *ax = 2506 | PyObject_CallObject(detail::_interpreter::get().s_python_function_gca, 2507 | detail::_interpreter::get().s_python_empty_tuple); 2508 | if (!ax) throw std::runtime_error("Call to gca() failed."); 2509 | Py_INCREF(ax); 2510 | 2511 | PyObject *zlabel = PyObject_GetAttrString(ax, "set_zlabel"); 2512 | if (!zlabel) throw std::runtime_error("Attribute set_zlabel not found."); 2513 | Py_INCREF(zlabel); 2514 | 2515 | PyObject *res = PyObject_Call(zlabel, args, kwargs); 2516 | if (!res) throw std::runtime_error("Call to set_zlabel() failed."); 2517 | Py_DECREF(zlabel); 2518 | 2519 | Py_DECREF(ax); 2520 | Py_DECREF(args); 2521 | Py_DECREF(kwargs); 2522 | if (res) Py_DECREF(res); 2523 | } 2524 | 2525 | inline void grid(bool flag) 2526 | { 2527 | detail::_interpreter::get(); 2528 | 2529 | PyObject* pyflag = flag ? Py_True : Py_False; 2530 | Py_INCREF(pyflag); 2531 | 2532 | PyObject* args = PyTuple_New(1); 2533 | PyTuple_SetItem(args, 0, pyflag); 2534 | 2535 | PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_grid, args); 2536 | if(!res) throw std::runtime_error("Call to grid() failed."); 2537 | 2538 | Py_DECREF(args); 2539 | Py_DECREF(res); 2540 | } 2541 | 2542 | inline void show(const bool block = true) 2543 | { 2544 | detail::_interpreter::get(); 2545 | 2546 | PyObject* res; 2547 | if(block) 2548 | { 2549 | res = PyObject_CallObject( 2550 | detail::_interpreter::get().s_python_function_show, 2551 | detail::_interpreter::get().s_python_empty_tuple); 2552 | } 2553 | else 2554 | { 2555 | PyObject *kwargs = PyDict_New(); 2556 | PyDict_SetItemString(kwargs, "block", Py_False); 2557 | res = PyObject_Call( detail::_interpreter::get().s_python_function_show, detail::_interpreter::get().s_python_empty_tuple, kwargs); 2558 | Py_DECREF(kwargs); 2559 | } 2560 | 2561 | 2562 | if (!res) throw std::runtime_error("Call to show() failed."); 2563 | 2564 | Py_DECREF(res); 2565 | } 2566 | 2567 | inline void close() 2568 | { 2569 | detail::_interpreter::get(); 2570 | 2571 | PyObject* res = PyObject_CallObject( 2572 | detail::_interpreter::get().s_python_function_close, 2573 | detail::_interpreter::get().s_python_empty_tuple); 2574 | 2575 | if (!res) throw std::runtime_error("Call to close() failed."); 2576 | 2577 | Py_DECREF(res); 2578 | } 2579 | 2580 | inline void xkcd() { 2581 | detail::_interpreter::get(); 2582 | 2583 | PyObject* res; 2584 | PyObject *kwargs = PyDict_New(); 2585 | 2586 | res = PyObject_Call(detail::_interpreter::get().s_python_function_xkcd, 2587 | detail::_interpreter::get().s_python_empty_tuple, kwargs); 2588 | 2589 | Py_DECREF(kwargs); 2590 | 2591 | if (!res) 2592 | throw std::runtime_error("Call to show() failed."); 2593 | 2594 | Py_DECREF(res); 2595 | } 2596 | 2597 | inline void draw() 2598 | { 2599 | detail::_interpreter::get(); 2600 | 2601 | PyObject* res = PyObject_CallObject( 2602 | detail::_interpreter::get().s_python_function_draw, 2603 | detail::_interpreter::get().s_python_empty_tuple); 2604 | 2605 | if (!res) throw std::runtime_error("Call to draw() failed."); 2606 | 2607 | Py_DECREF(res); 2608 | } 2609 | 2610 | template 2611 | inline void pause(Numeric interval) 2612 | { 2613 | detail::_interpreter::get(); 2614 | 2615 | PyObject* args = PyTuple_New(1); 2616 | PyTuple_SetItem(args, 0, PyFloat_FromDouble(interval)); 2617 | 2618 | PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_pause, args); 2619 | if(!res) throw std::runtime_error("Call to pause() failed."); 2620 | 2621 | Py_DECREF(args); 2622 | Py_DECREF(res); 2623 | } 2624 | 2625 | inline void save(const std::string& filename, const int dpi=0) 2626 | { 2627 | detail::_interpreter::get(); 2628 | 2629 | PyObject* pyfilename = PyString_FromString(filename.c_str()); 2630 | 2631 | PyObject* args = PyTuple_New(1); 2632 | PyTuple_SetItem(args, 0, pyfilename); 2633 | 2634 | PyObject* kwargs = PyDict_New(); 2635 | 2636 | if(dpi > 0) 2637 | { 2638 | PyDict_SetItemString(kwargs, "dpi", PyLong_FromLong(dpi)); 2639 | } 2640 | 2641 | PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_save, args, kwargs); 2642 | if (!res) throw std::runtime_error("Call to save() failed."); 2643 | 2644 | Py_DECREF(args); 2645 | Py_DECREF(kwargs); 2646 | Py_DECREF(res); 2647 | } 2648 | 2649 | inline void rcparams(const std::map& keywords = {}) { 2650 | detail::_interpreter::get(); 2651 | PyObject* args = PyTuple_New(0); 2652 | PyObject* kwargs = PyDict_New(); 2653 | for (auto it = keywords.begin(); it != keywords.end(); ++it) { 2654 | if ("text.usetex" == it->first) 2655 | PyDict_SetItemString(kwargs, it->first.c_str(), PyLong_FromLong(std::stoi(it->second.c_str()))); 2656 | else PyDict_SetItemString(kwargs, it->first.c_str(), PyString_FromString(it->second.c_str())); 2657 | } 2658 | 2659 | PyObject * update = PyObject_GetAttrString(detail::_interpreter::get().s_python_function_rcparams, "update"); 2660 | PyObject * res = PyObject_Call(update, args, kwargs); 2661 | if(!res) throw std::runtime_error("Call to rcParams.update() failed."); 2662 | Py_DECREF(args); 2663 | Py_DECREF(kwargs); 2664 | Py_DECREF(update); 2665 | Py_DECREF(res); 2666 | } 2667 | 2668 | inline void clf() { 2669 | detail::_interpreter::get(); 2670 | 2671 | PyObject *res = PyObject_CallObject( 2672 | detail::_interpreter::get().s_python_function_clf, 2673 | detail::_interpreter::get().s_python_empty_tuple); 2674 | 2675 | if (!res) throw std::runtime_error("Call to clf() failed."); 2676 | 2677 | Py_DECREF(res); 2678 | } 2679 | 2680 | inline void cla() { 2681 | detail::_interpreter::get(); 2682 | 2683 | PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_cla, 2684 | detail::_interpreter::get().s_python_empty_tuple); 2685 | 2686 | if (!res) 2687 | throw std::runtime_error("Call to cla() failed."); 2688 | 2689 | Py_DECREF(res); 2690 | } 2691 | 2692 | inline void ion() { 2693 | detail::_interpreter::get(); 2694 | 2695 | PyObject *res = PyObject_CallObject( 2696 | detail::_interpreter::get().s_python_function_ion, 2697 | detail::_interpreter::get().s_python_empty_tuple); 2698 | 2699 | if (!res) throw std::runtime_error("Call to ion() failed."); 2700 | 2701 | Py_DECREF(res); 2702 | } 2703 | 2704 | inline std::vector> ginput(const int numClicks = 1, const std::map& keywords = {}) 2705 | { 2706 | detail::_interpreter::get(); 2707 | 2708 | PyObject *args = PyTuple_New(1); 2709 | PyTuple_SetItem(args, 0, PyLong_FromLong(numClicks)); 2710 | 2711 | // construct keyword args 2712 | PyObject* kwargs = PyDict_New(); 2713 | for(std::map::const_iterator it = keywords.begin(); it != keywords.end(); ++it) 2714 | { 2715 | PyDict_SetItemString(kwargs, it->first.c_str(), PyUnicode_FromString(it->second.c_str())); 2716 | } 2717 | 2718 | PyObject* res = PyObject_Call( 2719 | detail::_interpreter::get().s_python_function_ginput, args, kwargs); 2720 | 2721 | Py_DECREF(kwargs); 2722 | Py_DECREF(args); 2723 | if (!res) throw std::runtime_error("Call to ginput() failed."); 2724 | 2725 | const size_t len = PyList_Size(res); 2726 | std::vector> out; 2727 | out.reserve(len); 2728 | for (size_t i = 0; i < len; i++) { 2729 | PyObject *current = PyList_GetItem(res, i); 2730 | std::array position; 2731 | position[0] = PyFloat_AsDouble(PyTuple_GetItem(current, 0)); 2732 | position[1] = PyFloat_AsDouble(PyTuple_GetItem(current, 1)); 2733 | out.push_back(position); 2734 | } 2735 | Py_DECREF(res); 2736 | 2737 | return out; 2738 | } 2739 | 2740 | // Actually, is there any reason not to call this automatically for every plot? 2741 | inline void tight_layout() { 2742 | detail::_interpreter::get(); 2743 | 2744 | PyObject *res = PyObject_CallObject( 2745 | detail::_interpreter::get().s_python_function_tight_layout, 2746 | detail::_interpreter::get().s_python_empty_tuple); 2747 | 2748 | if (!res) throw std::runtime_error("Call to tight_layout() failed."); 2749 | 2750 | Py_DECREF(res); 2751 | } 2752 | 2753 | // Support for variadic plot() and initializer lists: 2754 | 2755 | namespace detail { 2756 | 2757 | template 2758 | using is_function = typename std::is_function>>::type; 2759 | 2760 | template 2761 | struct is_callable_impl; 2762 | 2763 | template 2764 | struct is_callable_impl 2765 | { 2766 | typedef is_function type; 2767 | }; // a non-object is callable iff it is a function 2768 | 2769 | template 2770 | struct is_callable_impl 2771 | { 2772 | struct Fallback { void operator()(); }; 2773 | struct Derived : T, Fallback { }; 2774 | 2775 | template struct Check; 2776 | 2777 | template 2778 | static std::true_type test( ... ); // use a variadic function to make sure (1) it accepts everything and (2) its always the worst match 2779 | 2780 | template 2781 | static std::false_type test( Check* ); 2782 | 2783 | public: 2784 | typedef decltype(test(nullptr)) type; 2785 | typedef decltype(&Fallback::operator()) dtype; 2786 | static constexpr bool value = type::value; 2787 | }; // an object is callable iff it defines operator() 2788 | 2789 | template 2790 | struct is_callable 2791 | { 2792 | // dispatch to is_callable_impl or is_callable_impl depending on whether T is of class type or not 2793 | typedef typename is_callable_impl::value, T>::type type; 2794 | }; 2795 | 2796 | template 2797 | struct plot_impl { }; 2798 | 2799 | template<> 2800 | struct plot_impl 2801 | { 2802 | template 2803 | bool operator()(const IterableX& x, const IterableY& y, const std::string& format) 2804 | { 2805 | detail::_interpreter::get(); 2806 | 2807 | // 2-phase lookup for distance, begin, end 2808 | using std::distance; 2809 | using std::begin; 2810 | using std::end; 2811 | 2812 | auto xs = distance(begin(x), end(x)); 2813 | auto ys = distance(begin(y), end(y)); 2814 | assert(xs == ys && "x and y data must have the same number of elements!"); 2815 | 2816 | PyObject* xlist = PyList_New(xs); 2817 | PyObject* ylist = PyList_New(ys); 2818 | PyObject* pystring = PyString_FromString(format.c_str()); 2819 | 2820 | auto itx = begin(x), ity = begin(y); 2821 | for(size_t i = 0; i < xs; ++i) { 2822 | PyList_SetItem(xlist, i, PyFloat_FromDouble(*itx++)); 2823 | PyList_SetItem(ylist, i, PyFloat_FromDouble(*ity++)); 2824 | } 2825 | 2826 | PyObject* plot_args = PyTuple_New(3); 2827 | PyTuple_SetItem(plot_args, 0, xlist); 2828 | PyTuple_SetItem(plot_args, 1, ylist); 2829 | PyTuple_SetItem(plot_args, 2, pystring); 2830 | 2831 | PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_plot, plot_args); 2832 | 2833 | Py_DECREF(plot_args); 2834 | if(res) Py_DECREF(res); 2835 | 2836 | return res; 2837 | } 2838 | }; 2839 | 2840 | template<> 2841 | struct plot_impl 2842 | { 2843 | template 2844 | bool operator()(const Iterable& ticks, const Callable& f, const std::string& format) 2845 | { 2846 | if(begin(ticks) == end(ticks)) return true; 2847 | 2848 | // We could use additional meta-programming to deduce the correct element type of y, 2849 | // but all values have to be convertible to double anyways 2850 | std::vector y; 2851 | for(auto x : ticks) y.push_back(f(x)); 2852 | return plot_impl()(ticks,y,format); 2853 | } 2854 | }; 2855 | 2856 | } // end namespace detail 2857 | 2858 | // recursion stop for the above 2859 | template 2860 | bool plot() { return true; } 2861 | 2862 | template 2863 | bool plot(const A& a, const B& b, const std::string& format, Args... args) 2864 | { 2865 | return detail::plot_impl::type>()(a,b,format) && plot(args...); 2866 | } 2867 | 2868 | /* 2869 | * This group of plot() functions is needed to support initializer lists, i.e. calling 2870 | * plot( {1,2,3,4} ) 2871 | */ 2872 | inline bool plot(const std::vector& x, const std::vector& y, const std::string& format = "") { 2873 | return plot(x,y,format); 2874 | } 2875 | 2876 | inline bool plot(const std::vector& y, const std::string& format = "") { 2877 | return plot(y,format); 2878 | } 2879 | 2880 | inline bool plot(const std::vector& x, const std::vector& y, const std::map& keywords) { 2881 | return plot(x,y,keywords); 2882 | } 2883 | 2884 | /* 2885 | * This class allows dynamic plots, ie changing the plotted data without clearing and re-plotting 2886 | */ 2887 | class Plot 2888 | { 2889 | public: 2890 | // default initialization with plot label, some data and format 2891 | template 2892 | Plot(const std::string& name, const std::vector& x, const std::vector& y, const std::string& format = "") { 2893 | detail::_interpreter::get(); 2894 | 2895 | assert(x.size() == y.size()); 2896 | 2897 | PyObject* kwargs = PyDict_New(); 2898 | if(name != "") 2899 | PyDict_SetItemString(kwargs, "label", PyString_FromString(name.c_str())); 2900 | 2901 | PyObject* xarray = detail::get_array(x); 2902 | PyObject* yarray = detail::get_array(y); 2903 | 2904 | PyObject* pystring = PyString_FromString(format.c_str()); 2905 | 2906 | PyObject* plot_args = PyTuple_New(3); 2907 | PyTuple_SetItem(plot_args, 0, xarray); 2908 | PyTuple_SetItem(plot_args, 1, yarray); 2909 | PyTuple_SetItem(plot_args, 2, pystring); 2910 | 2911 | PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_plot, plot_args, kwargs); 2912 | 2913 | Py_DECREF(kwargs); 2914 | Py_DECREF(plot_args); 2915 | 2916 | if(res) 2917 | { 2918 | line= PyList_GetItem(res, 0); 2919 | 2920 | if(line) 2921 | set_data_fct = PyObject_GetAttrString(line,"set_data"); 2922 | else 2923 | Py_DECREF(line); 2924 | Py_DECREF(res); 2925 | } 2926 | } 2927 | 2928 | // shorter initialization with name or format only 2929 | // basically calls line, = plot([], []) 2930 | Plot(const std::string& name = "", const std::string& format = "") 2931 | : Plot(name, std::vector(), std::vector(), format) {} 2932 | 2933 | template 2934 | bool update(const std::vector& x, const std::vector& y) { 2935 | assert(x.size() == y.size()); 2936 | if(set_data_fct) 2937 | { 2938 | PyObject* xarray = detail::get_array(x); 2939 | PyObject* yarray = detail::get_array(y); 2940 | 2941 | PyObject* plot_args = PyTuple_New(2); 2942 | PyTuple_SetItem(plot_args, 0, xarray); 2943 | PyTuple_SetItem(plot_args, 1, yarray); 2944 | 2945 | PyObject* res = PyObject_CallObject(set_data_fct, plot_args); 2946 | if (res) Py_DECREF(res); 2947 | return res; 2948 | } 2949 | return false; 2950 | } 2951 | 2952 | // clears the plot but keep it available 2953 | bool clear() { 2954 | return update(std::vector(), std::vector()); 2955 | } 2956 | 2957 | // definitely remove this line 2958 | void remove() { 2959 | if(line) 2960 | { 2961 | auto remove_fct = PyObject_GetAttrString(line,"remove"); 2962 | PyObject* args = PyTuple_New(0); 2963 | PyObject* res = PyObject_CallObject(remove_fct, args); 2964 | if (res) Py_DECREF(res); 2965 | } 2966 | decref(); 2967 | } 2968 | 2969 | ~Plot() { 2970 | decref(); 2971 | } 2972 | private: 2973 | 2974 | void decref() { 2975 | if(line) 2976 | Py_DECREF(line); 2977 | if(set_data_fct) 2978 | Py_DECREF(set_data_fct); 2979 | } 2980 | 2981 | 2982 | PyObject* line = nullptr; 2983 | PyObject* set_data_fct = nullptr; 2984 | }; 2985 | 2986 | } // end namespace matplotlibcpp 2987 | -------------------------------------------------------------------------------- /model/横向LQR/lqr_demo.slx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/czjaixuexi/Control/567bfc4dc9ad5f0b7420a94579600bccaf4033c2/model/横向LQR/lqr_demo.slx -------------------------------------------------------------------------------- /model/横向LQR/lqr_demo.slxc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/czjaixuexi/Control/567bfc4dc9ad5f0b7420a94579600bccaf4033c2/model/横向LQR/lqr_demo.slxc -------------------------------------------------------------------------------- /model/横向LQR/lqr_offline.m: -------------------------------------------------------------------------------- 1 | cf=-110000; 2 | cr=cf; 3 | m=1412; 4 | Iz=1536.7; 5 | a=1.015; 6 | b=2.910-1.015; 7 | k=zeros(5000,4); 8 | for i=1:5000 9 | vx=0.01*i; 10 | A=[0,1,0,0; 11 | 0,(cf+cr)/(m*vx),-(cf+cr)/m,(a*cf-b*cr)/(m*vx); 12 | 0,0,0,1; 13 | 0,(a*cf-b*cr)/(Iz*vx),-(a*cf-b*cr)/Iz,(a*a*cf+b*b*cr)/(Iz*vx)]; 14 | B=[0; 15 | -cf/m; 16 | 0; 17 | -a*cf/Iz]; 18 | Q=1*eye(4); 19 | R=10; 20 | k(i,:)=lqr(A,B,Q,R); 21 | end 22 | k1=k(:,1)'; 23 | k2=k(:,2)'; 24 | k3=k(:,3)'; 25 | k4=k(:,4)'; 26 | 27 | 28 | 29 | 30 | 31 | 32 | -------------------------------------------------------------------------------- /model/横向LQR/routing_planning.m: -------------------------------------------------------------------------------- 1 | %绘制一个规划的路径 2 | count=100; 3 | [x1,y1,theta1,kr1]=straight([0,0],[20,0],0,count); 4 | [x2,y2,theta2,kr2]=arc([20,0],[30,10],0,pi/2,count); 5 | [x3,y3,theta3,kr3]=arc([30,10],[40,20],pi/2,0,count); 6 | [x4,y4,theta4,kr4]=arc([40,20],[40,40],0,pi,count); 7 | [x5,y5,theta5,kr5]=arc([40,40],[35,35],pi,3*pi/2,count); 8 | [x6,y6,theta6,kr6]=arc([35,35],[25,35],3*pi/2,pi/2,count); 9 | [x7,y7,theta7,kr7]=arc([25,35],[15,35],pi/2,3*pi/2,count); 10 | [x8,y8,theta8,kr8]=arc([15,35],[5,35],3*pi/2,pi/2,count); 11 | [x9,y9,theta9,kr9]=arc([5,35],[-15,35],pi/2,3*pi/2,count); 12 | [x10,y10,theta10,kr10]=straight([-15,35],[-15,15],3*pi/2,count); 13 | [x11,y11,theta11,kr11]=arc([-15,15],[0,0],3*pi/2,2*pi,count); 14 | xr=[x1,x2,x3,x4,x5,x6,x7,x8,x9,x10,x11]; 15 | yr=[y1,y2,y3,y4,y5,y6,y7,y8,y9,y10,y11]; 16 | thetar=[theta1,theta2,theta3,theta4,theta5,theta6,theta7,theta8,theta9,theta10,theta11]; 17 | kappar=[kr1,kr2,kr3,kr4,kr5,kr6,kr7,kr8,kr9,kr10,kr11]; 18 | 19 | 20 | scatter(xr,yr); 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | function[xr,yr,thetar,kr]=straight(init_coord,end_coord,init_angle,count) 29 | delta_x=(end_coord(1)-init_coord(1))/(count-1); 30 | delta_y=(end_coord(2)-init_coord(2))/(count-1); 31 | for i=1:count 32 | xr(i)=init_coord(1)+delta_x*i; 33 | yr(i)=init_coord(2)+delta_y*i; 34 | thetar(i)=init_angle; 35 | kr(i)=0; 36 | end 37 | end 38 | 39 | function[xr,yr,thetar,kr]=arc(init_coord,end_coord,init_angle,end_angle,count) 40 | L=sqrt((init_coord(1)-end_coord(1))^2+(init_coord(2)-end_coord(2))^2); 41 | R=L/sqrt(2*(1-cos(end_angle-init_angle))); 42 | delta_angle=(end_angle-init_angle)/(count-1) ; 43 | 44 | for i=1:count 45 | if delta_angle>0 46 | xr(i)=init_coord(1)-R*sin(init_angle)+R*sin(init_angle+delta_angle*(i-1)); 47 | yr(i)=init_coord(2)+R*cos(init_angle)-R*cos(init_angle+delta_angle*(i-1)); 48 | thetar(i)=init_angle+delta_angle*i; 49 | kr(i)=1/R; 50 | else 51 | xr(i)=init_coord(1)+R*sin(init_angle)-R*sin(init_angle+delta_angle*(i-1)); 52 | 53 | yr(i)=init_coord(2)-R*cos(init_angle)+R*cos(init_angle+delta_angle*(i-1)); 54 | thetar(i)=init_angle+delta_angle*i; 55 | kr(i)=-1/R; 56 | end 57 | end 58 | end 59 | 60 | 61 | -------------------------------------------------------------------------------- /model/横纵向联合/lqr+pid.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/czjaixuexi/Control/567bfc4dc9ad5f0b7420a94579600bccaf4033c2/model/横纵向联合/lqr+pid.mat -------------------------------------------------------------------------------- /model/横纵向联合/planning_control.slx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/czjaixuexi/Control/567bfc4dc9ad5f0b7420a94579600bccaf4033c2/model/横纵向联合/planning_control.slx -------------------------------------------------------------------------------- /model/横纵向联合/planning_control.slxc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/czjaixuexi/Control/567bfc4dc9ad5f0b7420a94579600bccaf4033c2/model/横纵向联合/planning_control.slxc -------------------------------------------------------------------------------- /model/纵向PID/biaoding.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/czjaixuexi/Control/567bfc4dc9ad5f0b7420a94579600bccaf4033c2/model/纵向PID/biaoding.mat -------------------------------------------------------------------------------- /model/纵向PID/brake_calibration.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/czjaixuexi/Control/567bfc4dc9ad5f0b7420a94579600bccaf4033c2/model/纵向PID/brake_calibration.m -------------------------------------------------------------------------------- /model/纵向PID/generate_calibration.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/czjaixuexi/Control/567bfc4dc9ad5f0b7420a94579600bccaf4033c2/model/纵向PID/generate_calibration.m -------------------------------------------------------------------------------- /model/纵向PID/pid_demo.slx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/czjaixuexi/Control/567bfc4dc9ad5f0b7420a94579600bccaf4033c2/model/纵向PID/pid_demo.slx -------------------------------------------------------------------------------- /model/纵向PID/pid_demo.slxc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/czjaixuexi/Control/567bfc4dc9ad5f0b7420a94579600bccaf4033c2/model/纵向PID/pid_demo.slxc -------------------------------------------------------------------------------- /model/纵向PID/test2.m: -------------------------------------------------------------------------------- 1 | thr =1; 2 | sim('calibration'); 3 | 4 | -------------------------------------------------------------------------------- /model/纵向PID/thr_calibration.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/czjaixuexi/Control/567bfc4dc9ad5f0b7420a94579600bccaf4033c2/model/纵向PID/thr_calibration.m -------------------------------------------------------------------------------- /modeling_process/PID纵向控制.md: -------------------------------------------------------------------------------- 1 | # 纵向控制 2 | 3 | ## 一、更改carsim相关设置 4 | 5 | ##### 1.设置新的输入输出 6 | 7 | ##### 输入： 8 | 9 | ![image-20230509212215758](https://gitee.com/czjaixuexi/typora_pictures/raw/master/img/202305092141284.png) 10 | 11 | ##### 输出： 12 | 13 | ![image-20230509212224329](https://gitee.com/czjaixuexi/typora_pictures/raw/master/img/202305092141286.png) 14 | 15 | 16 | 17 | ##### 2.设置换挡策略，把油车模拟成电车 18 | 19 | ![image-20230509212231279](https://gitee.com/czjaixuexi/typora_pictures/raw/master/img/202305092141287.png) 20 | 21 | ![image-20230509212237240](https://gitee.com/czjaixuexi/typora_pictures/raw/master/img/202305092141288.png) 22 | 23 | ##### 3.更改道路为直线 24 | 25 | ![image-20230509212243879](https://gitee.com/czjaixuexi/typora_pictures/raw/master/img/202305092141289.png) 26 | 27 | ##### 4.send to simulink 搭建一个简单的测试模型测试输入输出 28 | 29 | ![image-20230509212251062](https://gitee.com/czjaixuexi/typora_pictures/raw/master/img/202305092141290.png) 30 | 31 | 电机的策略： 32 | 33 | ![image-20230509212258636](https://gitee.com/czjaixuexi/typora_pictures/raw/master/img/202305092141291.png) 34 | 35 | ![image-20230509212307323](https://gitee.com/czjaixuexi/typora_pictures/raw/master/img/202305092141292.png) 36 | 37 | ```matlab 38 | function torque = fcn(thr,rpm) 39 | Tmax=380*thr; 40 | if (rpm<=4523) 41 | torque=Tmax; 42 | else 43 | torque=Tmax*4523/rpm; 44 | end 45 | end 46 | ``` 47 | 48 | ## 二、油门/刹车标定表的制作 49 | 50 | #### 注：标定后的数据已经保存在biaoding.mat中，后续可以直接加载该数据，无需再重复标定。 51 | 52 | ![image-20230509212322473](https://gitee.com/czjaixuexi/typora_pictures/raw/master/img/202305092141293.png) 53 | 54 | 55 | 56 | 通过实验，得到大量的(v,a,thr)的三维点，从而拟合出thr = f(v,a) 57 | 58 | 在apollo中通过深度学习完成标定。 59 | 60 | #### 油门标定 61 | 62 | ##### 1.调整模型输入为thr，保存模型calibration.m,通过to workspace添加输入输出vx,ax 63 | 64 | 65 | 66 | ![image-20230509212333791](https://gitee.com/czjaixuexi/typora_pictures/raw/master/img/202305092141294.png) 67 | 68 | ##### 2.在matlab中编写脚本 69 | 70 | 先用简单的脚本测试一下 71 | 72 | ```matlab 73 | thr =1; 74 | sim('calibration'); 75 | ``` 76 | 77 | ###### 运行后发现数据成功加载到工作区 78 | 79 | ![image-20230509212341709](https://gitee.com/czjaixuexi/typora_pictures/raw/master/img/202305092141295.png) 80 | 81 | ###### 编写油门标定脚本thr_calibration.m： 82 | 83 | ```matlab 84 | thr=0;%初始化油门 85 | for i=1:21 86 | %该程序非常耗时，如果需要更多更密集的数据，请先测试 87 | sim('calibration'); 88 | v_temp(:,i)=vx.data; 89 | a_temp(:,i)=ax.data; 90 | thr_temp(:,i)=ones(length(vx.data),1)*thr; 91 | thr=thr+0.01; 92 | end 93 | 94 | %合并,一定要转成行向量再合并，否则会导致合并失败 95 | v=v_temp(:,1)'; 96 | a=a_temp(:,1)'; 97 | tr=thr_temp(:,1)'; 98 | for i=2:length(a_temp(1,:)) 99 | v=[v,v_temp(:,i)']; 100 | a=[a,a_temp(:,i)']; 101 | tr=[tr,thr_temp(:,i)']; 102 | end 103 | %拟合 104 | F=scatteredInterpolant(v',a',tr');%转成列向量 105 | vu=0:0.1:50; 106 | au=0:0.1:5; 107 | table=zeros(length(vu),length(au)); 108 | for i=1:length(vu) 109 | for j=1:length(au) 110 | table(i,j)=F(vu(i),au(j)); 111 | end 112 | end 113 | ``` 114 | 115 | ###### 运行后看到工作区已经加载了数据 116 | 117 | ![image-20230509212347724](https://gitee.com/czjaixuexi/typora_pictures/raw/master/img/202305092141296.png) 118 | 119 | ##### 3.回到simulink，添加2d lookup模块用于查找工作区的数据，并将工作区的数据填入： 120 | 121 | ![image-20230509212352668](https://gitee.com/czjaixuexi/typora_pictures/raw/master/img/202305092141297.png) 122 | 123 | ![image-20230509212358688](https://gitee.com/czjaixuexi/typora_pictures/raw/master/img/202305092141298.png) 124 | 125 | #### 刹车标定 126 | 127 | ##### 1.在Simulink模型中将油门设置为0，刹车添加一个brake变量 128 | 129 | ![image-20230509212404496](https://gitee.com/czjaixuexi/typora_pictures/raw/master/img/202305092141299.png) 130 | 131 | ##### 2.在carsim中将车辆的速度设置为180km/h，然后send to simulink 132 | 133 | ![image-20230509212409475](https://gitee.com/czjaixuexi/typora_pictures/raw/master/img/202305092141300.png) 134 | 135 | ##### 3.编写标定脚本brake_calibration 136 | 137 | ```matlab 138 | brake=0.1;%初始化刹车 139 | for i=1:80 140 | %该程序非常耗时，如果需要更多更密集的数据，请先测试 141 | sim('calibration'); 142 | v_temp(:,i)=vx.data; 143 | a_temp(:,i)=ax.data; 144 | brake_temp(:,i)=ones(length(vx.data),1)*brake; 145 | brake=brake+0.1; 146 | 147 | end 148 | 149 | 150 | %合并,一定要转成行向量再合并，否则会导致合并失败 151 | vbr=v_temp(:,1)'; 152 | abr=a_temp(:,1)'; 153 | br=brake_temp(:,1)'; 154 | for i=2:length(a_temp(1,:)) 155 | vbr=[vbr,v_temp(:,i)']; 156 | abr=[abr,a_temp(:,i)']; 157 | br=[br,brake_temp(:,i)']; 158 | end 159 | % 160 | %拟合 161 | F=scatteredInterpolant(vbr',abr',br');%转成列向量 162 | vubr=0:0.05:50; 163 | aubr=-8:0.05:0; 164 | tablebr=zeros(length(vubr),length(aubr)); 165 | for i=1:length(vubr) 166 | for j=1:length(aubr) 167 | tablebr(i,j)=F(vubr(i),aubr(j)); 168 | end 169 | end 170 | ``` 171 | 172 | ##### 4.回到simulink，添加2d lookup模块用于查找工作区的数据，并将工作区的数据填入： 173 | 174 | ![image-20230509212414378](https://gitee.com/czjaixuexi/typora_pictures/raw/master/img/202305092141301.png) 175 | 176 | ##### 5.将表接入模型，做一下简单测试 177 | 178 | ![image-20230509212418948](https://gitee.com/czjaixuexi/typora_pictures/raw/master/img/202305092141302.png) 179 | 180 | vx图像: 181 | 182 | ![image-20230509212425224](https://gitee.com/czjaixuexi/typora_pictures/raw/master/img/202305092141303.png) 183 | 184 | 185 | 186 | ## 三、双PID控制 187 | 188 | ##### 标定，将油门和刹车标定表合成一张表。 189 | 190 | **注：标定后的数据已经保存在biaoding.mat中，后续可以直接加载该数据，无需再重复标定。** 191 | 192 | 标定模型： 193 | 194 | ![image-20230509212430939](https://gitee.com/czjaixuexi/typora_pictures/raw/master/img/202305092141304.png) 195 | 196 | 其中biaoding moudle根据x的正负，判断输入是油门还是刹车： 197 | 198 | ```matlab 199 | function [thr,brake] = fcn(x) 200 | %正数代表油门，负数代表刹车 201 | %不允许同时踩油门和刹车 202 | if x>=0 203 | thr=x; 204 | brake=0; 205 | else 206 | thr=0; 207 | brake=-x; 208 | end 209 | ``` 210 | 211 | ###### 油门标定脚本thr_calibration.m: 212 | 213 | 启动前查看carsim车辆车速是否为0 214 | 215 | ```matlab 216 | %%%启动前检查carsim的初速度是否为0 217 | x=0;%初始化油门 218 | for i=1:21 219 | %该程序非常耗时，如果需要更多更密集的数据，请先测试 220 | sim('calibration'); 221 | v_temp(:,i)=vx.data; 222 | a_temp(:,i)=ax.data; 223 | thr_temp(:,i)=ones(length(vx.data),1)*x; 224 | x=x+0.05; 225 | end 226 | 227 | %合并,一定要转成行向量再合并，否则会导致合并失败 228 | v=v_temp(:,1)'; 229 | a=a_temp(:,1)'; 230 | tr=thr_temp(:,1)'; 231 | for i=2:length(v_temp(1,:)) 232 | v=[v,v_temp(:,i)']; 233 | a=[a,a_temp(:,i)']; 234 | tr=[tr,thr_temp(:,i)']; 235 | end 236 | ``` 237 | 238 | ###### 刹车标定脚本brake_calibration.m: 239 | 240 | 启动前查看carsim车辆车速是否为180 241 | 242 | ```matlab 243 | %启动前检查车的初速度是否为180 244 | x=0;%初始化刹车 245 | %%刹车的初速度一定要比较高，180km/h、144km/h 246 | for i=1:81 247 | %该程序非常耗时，如果需要更多更密集的数据，请先测试 248 | sim('calibration'); 249 | v_temp1(:,i)=vx.data; 250 | a_temp1(:,i)=ax.data; 251 | brake_temp1(:,i)=ones(length(vx.data),1)*x; 252 | %%%%这里是消除奇异性，因为无论brake=1还是2，最后都会导致车的v，a=0；这将导致多值性 253 | for j=1:length(v_temp1(:,i)) 254 | if v_temp1(j,i)<0.01 255 | brake_temp1(j,i)=0; 256 | end 257 | end 258 | 259 | x=x-0.1; 260 | end 261 | a_temp1(1,:)=a_temp1(2,:); 262 | 263 | %合并,一定要转成行向量再合并，否则会导致合并失败 264 | vbr=v_temp1(:,1)'; 265 | abr=a_temp1(:,1)'; 266 | br=brake_temp1(:,1)'; 267 | for i=2:length(v_temp1(1,:)) 268 | vbr=[vbr,v_temp1(:,i)']; 269 | abr=[abr,a_temp1(:,i)']; 270 | br=[br,brake_temp1(:,i)']; 271 | end 272 | ``` 273 | 274 | ###### 将油门、刹车标定表合成为一个表的脚本generate_calibration.m: 275 | 276 | ```matlab 277 | v2=[v,vbr]; 278 | a2=[a,abr]; 279 | br2=[tr,br]; 280 | 281 | 282 | F=scatteredInterpolant(v2',a2',br2');%转成列向量 283 | vubr=0:0.05:50; 284 | aubr=-8:0.05:5; 285 | tablebr=zeros(length(vubr),length(aubr)); 286 | for i=1:length(vubr) 287 | for j=1:length(aubr) 288 | tablebr(i,j)=F(vubr(i),aubr(j)); 289 | end 290 | end 291 | ``` 292 | 293 | 294 | 295 | ###### 在simulink中添加2D look up： 296 | 297 | ![image-20230509212437640](https://gitee.com/czjaixuexi/typora_pictures/raw/master/img/202305092141305.png) 298 | 299 | 300 | 301 | ![image-20230509212443368](https://gitee.com/czjaixuexi/typora_pictures/raw/master/img/202305092141306.png) 302 | 303 | ##### Simulink模型： 304 | 305 | ###### 将下面3个模块合为一个subsystem 306 | 307 | ![image-20230509212449399](https://gitee.com/czjaixuexi/typora_pictures/raw/master/img/202305092141307.png) 308 | 309 | ###### 合成后： 310 | 311 | ![image-20230509212453512](https://gitee.com/czjaixuexi/typora_pictures/raw/master/img/202305092141308.png) 312 | 313 | ###### 输入期望速度，不加PID观察效果： 314 | 315 | ![image-20230509212501782](https://gitee.com/czjaixuexi/typora_pictures/raw/master/img/202305092141309.png) 316 | 317 | 速度存在稳态误差： 318 | 319 | ![image-20230509212510493](https://gitee.com/czjaixuexi/typora_pictures/raw/master/img/202305092141310.png) 320 | 321 | ###### 添加pid模块： 322 | 323 | ![image-20230509212516352](https://gitee.com/czjaixuexi/typora_pictures/raw/master/img/202305092141311.png) 324 | 325 | 速度控制得到优化： 326 | 327 | ![image-20230509212522003](https://gitee.com/czjaixuexi/typora_pictures/raw/master/img/202305092141312.png) 328 | 329 | ##### 双PID控制： 330 | 331 | ###### 首先添加一个随时间变化的规划模块，用来提供期望的位置、速度、加速度： 332 | 333 | ![image-20230509212526455](https://gitee.com/czjaixuexi/typora_pictures/raw/master/img/202305092141313.png) 334 | 335 | planning: 336 | 337 | 提供提供期望的位置、速度、加速度 338 | 339 | ```matlab 340 | function [s,v,a] = fcn(t) 341 | if t<10 342 | s=0.1*t^3/3; 343 | v=0.1*t^2; 344 | a=0.2*t; 345 | else 346 | a=2-0.1*(t-10); 347 | v=2*t-0.05*(t-10)^2-10; 348 | s=t^2-0.05*(t-10)^3/3-10*t+100/3; 349 | end 350 | ``` 351 | 352 | ###### 在carsim的输出中添加车的纵向位置信息： 353 | 354 | ![image-20230509212530738](https://gitee.com/czjaixuexi/typora_pictures/raw/master/img/202305092141314.png) 355 | 356 | ###### 添加双PID控制： 357 | 358 | 原理：通过控制油门/刹车来控制车辆的加速度，实现加速度、速度、位置的控制。 359 | 360 | 控制流程图： 先对位置进行PID，给出速度补偿，再对速度进行PID，给出加速度补偿。 361 | 362 | 加速度=期望加速度+加速度补偿。 363 | 364 | ![image-20230509212544821](https://gitee.com/czjaixuexi/typora_pictures/raw/master/img/202305092141315.png) 365 | 366 | 367 | 368 | 369 | 370 | ###### 打包： 371 | 372 | ![image-20230509212552835](https://gitee.com/czjaixuexi/typora_pictures/raw/master/img/202305092141316.png) 373 | 374 | 运行模型后发现，加速器、速度、位置曲线与期望值拟合较好，纵向双PID控制完成。 375 | 376 | ![image-20230509212559811](https://gitee.com/czjaixuexi/typora_pictures/raw/master/img/202305092141317.png) -------------------------------------------------------------------------------- /modeling_process/VS Visualizer - CarSim - Baseline __ Quick Start Guide Example_ 2022-11-03 22-29-50.mp4: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/czjaixuexi/Control/567bfc4dc9ad5f0b7420a94579600bccaf4033c2/modeling_process/VS Visualizer - CarSim - Baseline __ Quick Start Guide Example_ 2022-11-03 22-29-50.mp4 -------------------------------------------------------------------------------- /modeling_process/横向LQR.md: -------------------------------------------------------------------------------- 1 | # 横向LQR Carsim与Simulink建模步骤 2 | 3 | ### 一.carsim选择数据库为CarSim2019.1_Data 进入后配置 4 | 5 | 6 | 7 | ### 二.配置carsim对的simulink输入输出 8 | 9 | #### 1.点击此处进入配置 10 | 11 | 在advanced settings中输入opt_steer_ext(1) 4 车辆可以由车轮转角控制方向 12 | 13 | ![image-20221102203901891](https://gitee.com/czjaixuexi/typora_pictures/raw/master/img/image-20221102203901891.png) 14 | 15 | #### 2.选择模型（该模型路线需要在CarSim2019.1_Data中） 16 | 17 | ![image-20230505001805998](https://gitee.com/czjaixuexi/typora_pictures/raw/master/img/image-20230505001805998.png) 18 | 19 | #### 3.配置输入输出 20 | 21 | ![image-20221104095011138](https://gitee.com/czjaixuexi/typora_pictures/raw/master/img/image-20221104095011138.png) 22 | 23 | ##### 输入： 24 | 25 | ![image-20221102204040341](https://gitee.com/czjaixuexi/typora_pictures/raw/master/img/image-20221102204040341.png) 26 | 27 | ##### 输出： 28 | 29 | ![image-20221103153028519](https://gitee.com/czjaixuexi/typora_pictures/raw/master/img/image-20221103153028519.png) 30 | 31 | #### 4.回到主界面，点击 send to simulink，进入Simulink界面 32 | 33 | ![image-20221102204337906](https://gitee.com/czjaixuexi/typora_pictures/raw/master/img/image-20221102204337906.png) 34 | 35 | ### 三.编辑Simulink模型 36 | 37 | **tips:D:\CarSim2019.1\Programs\solvers\Matlab84+ （安装路径下） 有一个SolverSF可以从这里复制S-Function模块** 38 | 39 | **在carsim中点击send to simulink后，目录中会自动生产一个simfile.sim文件(可以自己改名)，S-funciton填写该文件名。** 40 | 41 | ![image-20221102205200877](https://gitee.com/czjaixuexi/typora_pictures/raw/master/img/image-20221102205200877.png) 42 | 43 | #### 1.配置输入输出，查看carsim导入是否成功 44 | 45 | ![image-20221103135416819](https://gitee.com/czjaixuexi/typora_pictures/raw/master/img/image-20221103135416819.png) 46 | 47 | 有了输出的结果，说明导入成功，接下来进行开始lqr模块的建模 48 | 49 | #### 2.设置道路 50 | 51 | 1.在carsim中设置道路 52 | 53 | ![image-20221103140457321](https://gitee.com/czjaixuexi/typora_pictures/raw/master/img/image-20221103140457321.png) 54 | 55 | ![image-20221103140508441](https://gitee.com/czjaixuexi/typora_pictures/raw/master/img/image-20221103140508441.png) 56 | 57 | ![image-20221103140514457](https://gitee.com/czjaixuexi/typora_pictures/raw/master/img/image-20221103140514457.png) 58 | 59 | 2.在matlab中，先跑一遍routing_planning.m生成路径 60 | 61 | ```matlab 62 | %绘制一个规划的路径 63 | count=50; 64 | [x1,y1,theta1,kr1]=straight([0,0],[20,0],0,count); 65 | [x2,y2,theta2,kr2]=arc([20,0],[30,10],0,pi/2,count); 66 | [x3,y3,theta3,kr3]=arc([30,10],[40,20],pi/2,0,count); 67 | [x4,y4,theta4,kr4]=arc([40,20],[40,40],0,pi,count); 68 | [x5,y5,theta5,kr5]=arc([40,40],[35,35],pi,3*pi/2,count); 69 | [x6,y6,theta6,kr6]=arc([35,35],[25,35],3*pi/2,pi/2,count); 70 | [x7,y7,theta7,kr7]=arc([25,35],[15,35],pi/2,3*pi/2,count); 71 | [x8,y8,theta8,kr8]=arc([15,35],[5,35],3*pi/2,pi/2,count); 72 | [x9,y9,theta9,kr9]=arc([5,35],[-15,35],pi/2,3*pi/2,count); 73 | [x10,y10,theta10,kr10]=straight([-15,35],[-15,15],3*pi/2,count); 74 | [x11,y11,theta11,kr11]=arc([-15,15],[0,0],3*pi/2,2*pi,count); 75 | xr=[x1,x2,x3,x4,x5,x6,x7,x8,x9,x10,x11]; 76 | yr=[y1,y2,y3,y4,y5,y6,y7,y8,y9,y10,y11]; 77 | thetar=[theta1,theta2,theta3,theta4,theta5,theta6,theta7,theta8,theta9,theta10,theta11]; 78 | kappar=[kr1,kr2,kr3,kr4,kr5,kr6,kr7,kr8,kr9,kr10,kr11]; 79 | 80 | 81 | scatter(xr,yr); 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | function[xr,yr,thetar,kr]=straight(init_coord,end_coord,init_angle,count) 90 | delta_x=(end_coord(1)-init_coord(1))/(count-1); 91 | delta_y=(end_coord(2)-init_coord(2))/(count-1); 92 | for i=1:count 93 | xr(i)=init_coord(1)+delta_x*i; 94 | yr(i)=init_coord(2)+delta_y*i; 95 | thetar(i)=init_angle; 96 | kr(i)=0; 97 | end 98 | end 99 | 100 | function[xr,yr,thetar,kr]=arc(init_coord,end_coord,init_angle,end_angle,count) 101 | L=sqrt((init_coord(1)-end_coord(1))^2+(init_coord(2)-end_coord(2))^2); 102 | R=L/sqrt(2*(1-cos(end_angle-init_angle))); 103 | delta_angle=(end_angle-init_angle)/(count-1) ; 104 | 105 | for i=1:count 106 | if delta_angle>0 107 | xr(i)=init_coord(1)-R*sin(init_angle)+R*sin(init_angle+delta_angle*(i-1)); 108 | yr(i)=init_coord(2)+R*cos(init_angle)-R*cos(init_angle+delta_angle*(i-1)); 109 | thetar(i)=init_angle+delta_angle*i; 110 | kr(i)=1/R; 111 | else 112 | xr(i)=init_coord(1)+R*sin(init_angle)-R*sin(init_angle+delta_angle*(i-1)); 113 | 114 | yr(i)=init_coord(2)-R*cos(init_angle)+R*cos(init_angle+delta_angle*(i-1)); 115 | thetar(i)=init_angle+delta_angle*i; 116 | kr(i)=-1/R; 117 | end 118 | end 119 | end 120 | 121 | ``` 122 | 123 | ![image-20221103141057593](https://gitee.com/czjaixuexi/typora_pictures/raw/master/img/image-20221103141057593.png) 124 | 125 | #### 3.模块编写 126 | 127 | 控制模型图: 128 | 129 | ![image-20221103141207259](https://gitee.com/czjaixuexi/typora_pictures/raw/master/img/image-20221103141207259.png) 130 | 131 | ##### 1.预测模块的编写: 132 | 133 | 公式： 134 | 135 | ![image-20221103141349437](https://gitee.com/czjaixuexi/typora_pictures/raw/master/img/image-20221103141349437.png) 136 | 137 | predict_module: 138 | 139 | ![image-20221103141644314](https://gitee.com/czjaixuexi/typora_pictures/raw/master/img/image-20221103141644314.png) 140 | 141 | ```matlab 142 | %预测模块 143 | function [pre_x,pre_y,pre_phi,pre_vx,pre_vy,pre_phi_dot] = fcn(x,y,phi,vx,vy,phi_dot,ts) 144 | pre_x=x+vx*ts*cos(phi)-vy*ts*sin(phi); 145 | pre_y=y+vy*ts*cos(phi)+vx*ts*sin(phi); 146 | pre_phi=phi+phi_dot*ts; 147 | pre_vx=vx; 148 | pre_vy=vy; 149 | pre_phi_dot=phi_dot; 150 | end 151 | ``` 152 | 153 | ##### 2.误差，曲率计算模块： 154 | 155 | 公式： 156 | 157 | ![image-20221103142604682](https://gitee.com/czjaixuexi/typora_pictures/raw/master/img/image-20221103142604682.png) 158 | 159 | err_kappa_calculate_module： 160 | 161 | **xr,yr,thetar,kappar由上一步的路径规划文件得到，使用from模块从工作区中导入** 162 | 163 | ![image-20221103142842192](https://gitee.com/czjaixuexi/typora_pictures/raw/master/img/image-20221103142842192.png) 164 | 165 | ```matlab 166 | %误差，曲率计算模块 167 | %xr,yr,thetar,kappar表示规划的路径 168 | %x,y,phi,vx,vy,phi_dot为预测模块提供的车辆当前状态 169 | function [kr,err] = fcn(x,y,phi,vx,vy,phi_dot,xr,yr,thetar,kappar) 170 | %先遍历寻找距离最近的点作为匹配点 171 | n=length(xr); 172 | d_min=(x-xr(1))^2+(y-yr(1))^2; 173 | min=1; 174 | for i=1:n 175 | d=(x-xr(i))^2+(y-yr(i))^2; 176 | if d 309 | 310 | -------------------------------------------------------------------------------- /modeling_process/横纵向综合控制.md: -------------------------------------------------------------------------------- 1 | # 横纵向综合控制 2 | 3 | #### 更新carsim中的输入输出： 4 | 5 | **输入：** 6 | 7 | ![image-20221105211123106](https://gitee.com/czjaixuexi/typora_pictures/raw/master/img/image-20221105211123106.png) 8 | 9 | **输出：** 10 | 11 | ![image-20221105211212244](https://gitee.com/czjaixuexi/typora_pictures/raw/master/img/image-20221105211212244.png) 12 | 13 | #### 建立新的模型planning_control.slx: 14 | 15 | ![image-20221105211359671](https://gitee.com/czjaixuexi/typora_pictures/raw/master/img/image-20221105211359671.png) 16 | 17 | #### 标定油门、刹车表，然后合成，获得标定表： 18 | 19 | 具体步骤参考纵向控制。可以直接加载保存好的标定表到工作区。 20 | 21 | 数据在pid+lqr.mat中 22 | 23 | #### 编辑Simulink模型： 24 | 25 | ##### 1.根据carsim的输入输出，设置好输入输出接口 26 | 27 | ![image-20221105212453840](https://gitee.com/czjaixuexi/typora_pictures/raw/master/img/image-20221105212453840.png) 28 | 29 | ##### 2.将先前纵向控制的PID模型复制过来： 30 | 31 | ![image-20221105212602998](https://gitee.com/czjaixuexi/typora_pictures/raw/master/img/image-20221105212602998.png) 32 | 33 | ##### 3.设置规划轨迹 34 | 35 | ![image-20221105212823977](https://gitee.com/czjaixuexi/typora_pictures/raw/master/img/image-20221105212823977.png) 36 | 37 | planning trajectory: 38 | 39 | ![image-20221105230835518](https://gitee.com/czjaixuexi/typora_pictures/raw/master/img/image-20221105230835518.png) 40 | 41 | ```matlab 42 | function [vp,ap,xr,yr,thetar,kr] = fcn(t) 43 | dx=100; 44 | dy=10; 45 | T=30; 46 | xstart=[0,0,0]; 47 | xend=[dx,0,0]; 48 | ystart=[0,0,0]; 49 | yend=[dy,0,0]; 50 | a=zeros(1,6); 51 | b=zeros(1,6); 52 | 53 | a(1)=xstart(1); 54 | a(2)=xstart(2); 55 | a(3)=xstart(3)/2; 56 | A1=[T^3,T^4,T^5; 57 | 3*T^2,4*T^3,5*T^4; 58 | 6*T,12*T^2,20*T^3]; 59 | B1=[xend(1)-a(1)-a(2)*T-a(3)*T^2; 60 | xend(2)-a(2)-2*a(3)*T; 61 | xend(3)-2*a(3)]; 62 | xs=inv(A1)*B1; 63 | a(4)=xs(1); 64 | a(5)=xs(2); 65 | a(6)=xs(3); 66 | b(1)=ystart(1); 67 | b(2)=ystart(2); 68 | b(3)=ystart(3)/2; 69 | A2=[dx^3,dx^4,dx^5; 70 | 3*dx^2,4*dx^3,5*dx^4; 71 | 6*dx,12*dx^2,20*dx^3]; 72 | B2=[yend(1)-b(1)-b(2)*dx-b(3)*dx^2; 73 | yend(2)-b(2)-2*b(3)*dx; 74 | yend(3)-2*b(3)]; 75 | ys=inv(A2)*B2; 76 | b(4)=ys(1); 77 | b(5)=ys(2); 78 | b(6)=ys(3); 79 | xr=a(1)+a(2)*t+a(3)*t^2+a(4)*t^3+a(5)*t^4+a(6)*t^5; 80 | yr=b(1)+b(2)*xr+b(3)*xr^2+b(4)*xr^3+b(5)*xr^4+b(6)*xr^5; 81 | xr_dot=a(2)+2*a(3)*t+3*a(4)*t^2+4*a(5)*t^3+5*a(6)*t^4; 82 | yr_dx=b(2)+2*b(3)*xr+3*b(4)*xr^2+4*b(5)*xr^3+5*b(6)*xr^4; 83 | yr_dot=yr_dx*xr_dot; 84 | thetar=atan(yr_dx); 85 | xr_dot2=2*a(3)+6*a(4)*t+12*a(5)*t^2+20*a(6)*t^3; 86 | yr_dx2=2*b(3)+6*b(4)*xr+12*b(5)*xr^2+20*b(6)*xr^3; 87 | yr_dot2=yr_dx2*xr_dot^2+yr_dx*xr_dot2; 88 | kr=yr_dx2/((1+yr_dx^2)^1.5); 89 | vp=sqrt(xr_dot^2+yr_dot^2); 90 | if xr_dot2>=0 91 | ap=sqrt(xr_dot2^2+yr_dot2^2); 92 | else 93 | ap=-sqrt(xr_dot2^2+yr_dot2^2); 94 | end 95 | ``` 96 | 97 | ##### 4.将先前横向控制的LQR模型复制过来 98 | 99 | ![image-20221105231414824](https://gitee.com/czjaixuexi/typora_pictures/raw/master/img/image-20221105231414824.png) 100 | 101 | ![image-20221105231445892](https://gitee.com/czjaixuexi/typora_pictures/raw/master/img/image-20221105231445892.png) 102 | 103 | ###### 其中误差计算模块err_kappa_calculate_module需要修改 104 | 105 | err_kappa_calculate_module： 106 | 107 | 由于轨迹是由规划模块直接提供的，不需要遍历寻找匹配点，时间相同的点即为匹配点 108 | 109 | ```matlab 110 | function [kr,err,es,s_dot] = fcn(x,y,phi,vx,vy,phi_dot,xr,yr,thetar,kappar) 111 | 112 | tor=[cos(thetar);sin(thetar)]; 113 | nor=[-sin(thetar);cos(thetar)]; 114 | d_err=[x-xr;y-yr]; 115 | ed=nor'*d_err; 116 | es=tor'*d_err; 117 | %projection_point_thetar=thetar(dmin);%apollo 118 | projection_point_thetar=thetar+kappar*es; 119 | ed_dot=vy*cos(phi-projection_point_thetar)+vx*sin(phi-projection_point_thetar); 120 | %%%%%%%%% 121 | ephi=sin(phi-projection_point_thetar); 122 | %%%%%%%%% 123 | ss_dot=vx*cos(phi-projection_point_thetar)-vy*sin(phi-projection_point_thetar); 124 | s_dot=ss_dot/(1-kappar*ed); 125 | ephi_dot=phi_dot-kappar*s_dot; 126 | kr=kappar; 127 | err=[ed;ed_dot;ephi;ephi_dot]; 128 | 129 | end 130 | ``` 131 | 132 | ##### 整体模型： 133 | 134 | ![image-20221105235210693](https://gitee.com/czjaixuexi/typora_pictures/raw/master/img/image-20221105235210693.png) 135 | 136 | ##### 转向不足： 137 | 138 | ![image-20221105235252187](https://gitee.com/czjaixuexi/typora_pictures/raw/master/img/image-20221105235252187.png) 139 | 140 | 在横向误差处添加PID模块，来修正转向角度不足的偏差。 141 | 142 | ![image-20221106001934342](https://gitee.com/czjaixuexi/typora_pictures/raw/master/img/image-20221106001934342.png) -------------------------------------------------------------------------------- /note/notebook/自动驾驶控制算法第一讲2.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/czjaixuexi/Control/567bfc4dc9ad5f0b7420a94579600bccaf4033c2/note/notebook/自动驾驶控制算法第一讲2.pdf -------------------------------------------------------------------------------- /note/notebook/自动驾驶控制算法第七讲2.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/czjaixuexi/Control/567bfc4dc9ad5f0b7420a94579600bccaf4033c2/note/notebook/自动驾驶控制算法第七讲2.pdf -------------------------------------------------------------------------------- /note/notebook/自动驾驶控制算法第三讲2.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/czjaixuexi/Control/567bfc4dc9ad5f0b7420a94579600bccaf4033c2/note/notebook/自动驾驶控制算法第三讲2.pdf -------------------------------------------------------------------------------- /note/notebook/自动驾驶控制算法第九讲.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/czjaixuexi/Control/567bfc4dc9ad5f0b7420a94579600bccaf4033c2/note/notebook/自动驾驶控制算法第九讲.pdf -------------------------------------------------------------------------------- /note/notebook/自动驾驶控制算法第二讲2.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/czjaixuexi/Control/567bfc4dc9ad5f0b7420a94579600bccaf4033c2/note/notebook/自动驾驶控制算法第二讲2.pdf -------------------------------------------------------------------------------- /note/notebook/自动驾驶控制算法第五讲2.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/czjaixuexi/Control/567bfc4dc9ad5f0b7420a94579600bccaf4033c2/note/notebook/自动驾驶控制算法第五讲2.pdf -------------------------------------------------------------------------------- /note/notebook/自动驾驶控制算法第八讲(四).pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/czjaixuexi/Control/567bfc4dc9ad5f0b7420a94579600bccaf4033c2/note/notebook/自动驾驶控制算法第八讲(四).pdf -------------------------------------------------------------------------------- /note/notebook/自动驾驶控制算法第八讲4.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/czjaixuexi/Control/567bfc4dc9ad5f0b7420a94579600bccaf4033c2/note/notebook/自动驾驶控制算法第八讲4.pdf -------------------------------------------------------------------------------- /note/notebook/自动驾驶控制算法第六讲2.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/czjaixuexi/Control/567bfc4dc9ad5f0b7420a94579600bccaf4033c2/note/notebook/自动驾驶控制算法第六讲2.pdf -------------------------------------------------------------------------------- /note/notebook/自动驾驶控制算法第十二讲(完结).pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/czjaixuexi/Control/567bfc4dc9ad5f0b7420a94579600bccaf4033c2/note/notebook/自动驾驶控制算法第十二讲(完结).pdf -------------------------------------------------------------------------------- /note/notebook/自动驾驶控制算法第十讲.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/czjaixuexi/Control/567bfc4dc9ad5f0b7420a94579600bccaf4033c2/note/notebook/自动驾驶控制算法第十讲.pdf -------------------------------------------------------------------------------- /note/notebook/自动驾驶控制算法第四讲2.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/czjaixuexi/Control/567bfc4dc9ad5f0b7420a94579600bccaf4033c2/note/notebook/自动驾驶控制算法第四讲2.pdf -------------------------------------------------------------------------------- /note/横向控制.md: -------------------------------------------------------------------------------- 1 | # 横向控制 2 | 3 | ## PID控制 4 | 5 | 常见控制方法： 6 | 7 | 模仿驾驶员的操作习惯， 8 | 9 | 1. 选取一个离车辆较近的点来计算横向距离误差，并基于这个误差计算根据PID计算一个目标角度； 10 | 2. 然后选取一个远一点的轨迹点来计算航向误差，然后根据PID来计算一个目标角度； 11 | 3. 然后二者相加或者通过一些其他规则融合出一个最终的目标转角。 12 | 13 | 这种控制方式比较符合人的思考模式，但是亲身经历过，性能全靠调试调整，特别是两个预瞄点的位置，距离误差跟踪和航向角跟踪的PID参数等等，调参过程比较痛苦。 14 | 15 | 16 | 17 | ## 基于运动学模型 18 | 19 | ### Pure Pursuit 20 | 21 | #### 自行车模型 22 | 23 | 阿克曼转向模型可得进一步简化为车辆单轨模型——自行车模型。 24 | 25 | ![在这里插入图片描述](https://gitee.com/czjaixuexi/typora_pictures/raw/master/img/watermark,type_ZmFuZ3poZW5naGVpdGk,shadow_10,text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3UwMTM0Njg2MTQ=,size_16,color_FFFFFF,t_70.png) 26 | 27 | 该模型中无人车前轮转向角与曲率之间的几何关系，其关系如下式： 28 | 29 | 采用自行车模型的好处是它简化了无人车前轮转向角与曲率之间的几何关系，其关系如下式： 30 | $$ 31 | tan(\delta)=\frac{R}{L} \tag{1} 32 | $$ 33 | 34 | 35 | 其中$\delta$为前轮的转向角，$L$为轴距，$R$则为无人车在该转向角下的运动形成圆的半径（曲率的倒数）。 36 | 37 | 38 | 39 | #### Pure Pursuit 算法原理 40 | 41 | 纯追踪算法的原理很简单，就是单车模型通过调整前轮转向$\delta$运动，使得车辆后轴中心刚好可以经过当前规划的路点。换句话说，此时的后轴中心为圆弧切点，车辆纵向车身为切线。通过控制前轮转角$\delta$， 使车辆可以沿着一条经过目标路点（goal point）（或者叫预瞄点）的圆弧行驶。 42 | 43 | 基于当前车辆后轴中心位置,在参考路径上向$l_d$的距离匹配一个预瞄点。假设车辆后轴中心点可以按照一定的转弯半径$R$行驶抵达该预瞄点,然后根据预瞄距离$l_d$、转弯半径$R$、车辆坐标系下预瞄点的朝向角$2\alpha$之间的几何关系确定前轮转角。 44 | 45 | 46 | 47 | 48 | ![在这里插入图片描述](https://gitee.com/czjaixuexi/typora_pictures/raw/master/img/d5537f3dde3b4a459d7a015615d72d15.png) 49 | 50 | 51 | 52 | $\delta$即为$\delta_f$。$\alpha$为路点与车后轴中心连成的向量的角度与车辆偏航角的差值，当路点在车的左边时，$\alpha>0$，反之则$\alpha<0$；$l_d$为车后轴离前视路点的距离，又被称为前视距离，它决定了将预瞄路点放置多远。 53 | 54 | 根据上图，由正弦定理得 55 | $$ 56 | \frac{l_{d}}{\sin (2 \alpha)}=\frac{R}{\sin \left(\frac{\pi}{2}-\alpha\right)} \tag{2} 57 | $$ 58 | 59 | 60 | 根据三角函数性质，对等式(2)化简得 61 | $$ 62 | \frac{l_{d}}{2 \sin (\alpha) \cos (\alpha)}=\frac{R}{\cos (\alpha)} \tag{3} 63 | $$ 64 | 由于$\cos (\alpha) \neq 0$，对等式(3)进一步化简得 65 | $$ 66 | R=\frac{l_{d}}{2 \sin (\alpha)} \tag{4} 67 | $$ 68 | 故圆弧的曲率表示为 69 | $$ 70 | k=\frac{2 \sin (\alpha)}{l_{d}} \tag{5} 71 | $$ 72 | 根据等式(1)所示的车辆几何关系得 73 | $$ 74 | \delta=\arctan (k \cdot L) \tag{6} 75 | $$ 76 | 将等式(5)带入等式(6)得纯追踪算法控制量的的最终表达式： 77 | $$ 78 | \delta=\arctan \left(\frac{2 L\sin {\alpha} }{l_{d}}\right) \tag{7} 79 | $$ 80 | 当然，通过上图可知 81 | $$ 82 | \sin \alpha = \frac{e_y}{l_d} \tag{8} 83 | $$ 84 | 将式(8)代入式(7)得: 85 | $$ 86 | \delta = \arctan \left(\frac{2 L e_y}{l_d^2}\right) \tag{9} 87 | $$ 88 | 分析式(9)，利用小角度近似，我们有: 89 | $$ 90 | \delta \approx \frac{2L}{l_d^2} \cdot e_y \tag{10} 91 | $$ 92 | 把 $\frac{2L}{l_d^2}$ 看作比例控制器的参数，$e_y$ 作为系统误差，那么这就相当于一个以横向跟踪误差CTE作为系统误差的比例控制器。 93 | 94 | 在pure pursuit方法中，$l_d$ 表示成无人车纵向线速度的形式，即 $l_d = \lambda v_x + c$，$c$ 为常数，其中参数 $\lambda$与c是需要调整的。 95 | 96 | 97 | #### 总结 98 | 99 | Pure Pursuit近似P控制，效果强依赖于预瞄距离的选择，可根据车速动态的调整预瞄距离，从而增强系统的控制鲁棒性。 100 | 101 | 其控制的关键在于**对最佳前向预瞄距离的确定** 102 | 103 | 1，前视距离变短（相当于增大P），那么控制精度越高，但车辆控制会不稳定甚至震荡； 104 | 105 | 2，前视距离越长（相当于减小P），那么控制效果趋于平滑，稳定性提高，震荡减弱，但路径跟踪性能降低及稳态误差增大，表现出**转弯内切现象（提早转弯）**，在某些急剧的转角处，可能会转向不足的问题。 106 | 107 | 108 | 109 | 下图能够直观的看到弯道处转弯内切的现象：绿色为预设线路，橙色为汽车在PurePursuit控制下真实行驶线路 110 | 111 | 112 | 113 | image-20230509214415222 114 | 115 | 116 | 117 | 118 | 119 | 因此Pure Pursuit 虽然在大的跟踪误差和非连续的路径场景下鲁棒性较好，但不太适合在弯道比较多的场景，**适合直线多、弯道少、精度不太高的低速场景。** 120 | 121 | 122 | 123 | ### Stanley（前轮反馈） 124 | 125 | #### 算法推导 126 | 127 | 前轮反馈控制(Front wheel feedback)也就是常说的Stanley方法，其**核心思想**是基于车辆前轴中心点的[路径跟踪](https://so.csdn.net/so/search?q=路径跟踪&spm=1001.2101.3001.7020)偏差量对方向盘转向控制量进行计算。 128 | 129 | 前轮转角控制变量$\delta$由两部分构成: 130 | $$ 131 | \delta(t)=\delta_e(t)+\delta_{\theta_{e}}(t) 132 | $$ 133 | 134 | - 一部分是航向误差引起的转角,即当前车身方向与参考轨迹最近点的切线方向的夹角$\theta_e$; 135 | 136 | - 另一部分是横向误差引起的转角,即前轮速度方向与参考轨迹最近点的切线方向所成的夹角$\delta_e$. 137 | 138 | 139 | 140 | image-20230509212714981 141 | 142 | 143 | 144 | 145 | 146 | 在不考虑横向跟踪误差的情况下，前轮偏角和给定路径切线方向一致，如图所示。其中$\theta_e$表示车辆航向与最近路径点切线方向之间的夹角，在没有任何横向误差的情况下，前轮方向与所在路径点的方向相同： 147 | $$ 148 | \delta_{\theta_{e}}(t)=\theta_e(t) 149 | $$ 150 | 在不考虑航向跟踪误差$\theta_e$的情况下，车辆预期轨迹在距离前轮$d(t)$处与给定路径上最近点切线相交，向跟踪误差越大，前轮转向角越大，根据几何关系得出如下非线性比例函数： 151 | $$ 152 | \delta e(t) = \arctan\left(\frac{e_y(t)}{d(t)}\right)=\arctan\left(\frac{ke_y(t)}{ v(t)}\right) 153 | $$ 154 | 其中 $d(t)$ 与车速 $v(t)$ 相关，增益参数 $k$，$d=\frac{v}{k}$。 155 | 156 | 157 | 158 | 另外，根据几何关系，$\theta_e$ 满足： 159 | $$ 160 | \begin{equation} \theta_e = \psi_t(t) - \psi(t) \end{equation} 161 | $$ 162 | 其中，$\psi$ 表示车辆航向角，$\psi_t$ 表示离车辆前轴中心最近目标路径点处的航向角。 163 | 164 | 前轮转角控制量 $\delta$ 的最终表达式为： 165 | $$ 166 | \begin{equation} \delta(t) = \psi_t(t) - \psi(t) + \operatorname{arctan} \frac{ke_y(t)}{v(t)} \end{equation} 167 | $$ 168 | 其中，$e_y$ 为横向误差，$v$ 为车速，$k$ 为增益参数。 169 | 170 | 171 | 172 | #### 横向误差收敛 173 | 174 | 使用线性自行车运动模型，可以得到横向误差的变化率： 175 | $$ 176 | \begin{equation} \dot{e_y}(t) = -v(t) \sin \delta_{e}(t) \end{equation} 177 | $$ 178 | 179 | ``` 180 | 带负号是因为在控制过程中，横向误差会越来越小，因此横向偏差变化率会有负号。 181 | ``` 182 | 183 | 其中 $\sin \delta_e(t)$ 根据几何关系可知： 184 | $$ 185 | \begin{equation} \sin \delta_{e}(t) = \frac{e_y(t)}{\sqrt{d(t)^{2}+(e_y(t))^{2}}} = \frac{ke_y(t)}{\sqrt{v(t)^{2}+(ke_y(t))^{2}}} \end{equation} 186 | $$ 187 | 故有: 188 | $$ 189 | \dot{e_y}(t) = \frac{-v(t)ke_y(t)}{\sqrt{v(t)^{2}+(ke_y(t))^{2}}} = \frac{-ke_y(t)}{\sqrt{1+\Bigg(\frac{ke_y(t)}{v(t)}\Bigg)^{2}}} 190 | $$ 191 | 当横向跟踪误差$e_y(t)$很小时，$(\frac{ke_y(t)}{v(t)})^2\rightarrow 0$： 192 | $$ 193 | \dot{e_y}(t) \approx -ke_y(t) 194 | $$ 195 | 根据一阶线性微分方程的解法，对上式进行积分得到 196 | $$ 197 | e_y(t) = e_y(0) \times e^{-kt} 198 | $$ 199 | 因此，当 $t\rightarrow\infty$ 时，横向误差以指数形式收敛于 0，参数 $k$ 决定了收敛速度。 200 | 201 | 202 | 203 | #### 计算步骤 204 | 205 | - 输入：当前车辆位置、航向角 $\psi$、速度 $v$，当前目标路点和离车辆前轴中心最近目标路径点处的航向角 $\psi_t$。 206 | - 计算： 207 | 1. 计算横向误差 $e_y$。 208 | 2. 计算前轮转角控制量 $\delta = \psi_t - \psi + \arctan{\frac{ke_y}{v}}$。 209 | - 输出：前轮转角控制量 $\delta$。 210 | 211 | 212 | 213 | #### 总结 214 | 215 | **适合曲率连续的低速场景。** 216 | 217 | 相比于 Pure Pursuit 算法，Stanley 前轮反馈算法还额外考虑了横摆角偏差 218 | 因此在大多数场景下，跟踪性能更佳 219 | 然而，由于没有设置前向预瞄，Stanley 算法鲁棒性差，在曲率不连续的路径上，会出现震荡。 220 | 221 | 222 | 223 | 224 | 225 | ### 后轮反馈 226 | 227 | 参考 228 | 229 | [后轮位置反馈实现轨迹跟踪](https://blog.csdn.net/weixin_42301220/article/details/125003918?spm=1001.2014.3001.5501) 230 | 231 | [控制算法 - 后轮位置反馈](https://zgh551.github.io/2020/02/26/%E6%8E%A7%E5%88%B6%E7%AE%97%E6%B3%95-%E5%90%8E%E8%BD%AE%E4%BD%8D%E7%BD%AE%E5%8F%8D%E9%A6%88/#%E6%80%BB%E7%BB%93) 232 | 233 | 与 Pure Pursuit 和 Stanley 算法相比，后轮反馈控制算法计算更加复杂， 对路径的平滑性要求更高 234 | 在中等速度下的跟踪性能及鲁棒性与 Stanley 方法近似 235 | 然而在速度较大时，稳态误差也会变大，从而导致控制效果不佳 236 | 237 | 238 | 239 | 240 | 241 | ## 基于动力学模型 242 | 243 | ### LQR控制 244 | 245 | [动力学模型](notebook/自动驾驶控制算法第三讲2.pdf) 246 | 247 | [动力学横向控制误差模型](notebook/自动驾驶控制算法第四讲2.pdf) 248 | 249 | [LQR离散化+求解](notebook/自动驾驶控制算法第五讲2.pdf) 250 | 251 | [前馈控制与航向误差](notebook/自动驾驶控制算法第六讲2.pdf) 252 | 253 | [离散规划轨迹的误差计算](notebook/自动驾驶控制算法第七讲2.pdf) 254 | 255 | [整体模型](notebook/自动驾驶控制算法第八讲4.pdf) 256 | 257 | [转向角突变原因与优化方式](notebook/自动驾驶控制算法第八讲(四).pdf) -------------------------------------------------------------------------------- /note/纵向控制.md: -------------------------------------------------------------------------------- 1 | # PID控制 2 | 3 | 4 | 5 | ![image-20230428233746318](https://gitee.com/czjaixuexi/typora_pictures/raw/master/img/202305092142480.png) 6 | 7 | 8 | 9 | ## 增量式PID与位置式PID 10 | 11 | - 位置式：输出控制量，位置式算法较为简单，直接输入当前的偏差e(k) ，即可得到输出u(k) ； 12 | - 增量式：输出控制增量，增量式算法需要保存历史偏差e(k-1),e(k-2),即在第k次控制周期时，需要使用第k-1和第k-2次控制所输入的偏差，最终计算得到$\Delta u(k)$，此时，**这还不是我们所需要的PID输出量**；所以需要进行累加； 13 | 14 | $$ 15 | u(k) = u(k-1)+\Delta u(k) 16 | $$ 17 | 18 | 通过对比位置式PID公式和增量式PID公式，我们会发现，位置式PID公式带有积分项，而增量式PID公式没有带积分项，而什么时候用位置式PID，什么时候用增量式PID，是根据执行元件是否带积分部件来决定的，若带积分部件则使用增量式PID（增量式PID没有积分项），若不带积分部件，则使用位置式PID（位置式PID带有积分项）。 19 | 20 | 那什么是带有积分部件，什么是不带积分部件？ 21 | 22 | 带有积分部件就是指执行元件有记忆性，能够记录此次的运行状态，比如步进电机，当给步进电机一个脉冲后，在下一个脉冲到来前，它都会保持在这个位置。而直流电机就是不带积分部件的执行元件，它没有记忆性，给一个脉冲，它的位置不会保持在一个固定位置，它会因为惯性继续转动。 23 | 24 | 25 | 26 | **自动驾驶中常用位置式PID。** 27 | 28 | ### 增量式PID 29 | 30 | 31 | 32 | 33 | 34 | 1 35 | 36 | 37 | 38 | 39 | 40 | 增量式PID控制的主要优点为： 41 | 42 | ①控制增量Δu(k)的确定仅与最近3次的采样值有关，容易通过加权处理获得比较好的控制效果； 43 | 44 | ②计算机每次只输出控制增量，即对应执行机构位置的变化量，故机器发生故障时影响范围小、不会严重影响生产过程； 45 | 46 | ③对于一些工业控制上的机器，手动到自动切换时冲击小。当控制从手动向自动切换时，可以作到无扰动切换。 47 | 48 | 49 | 50 | ### 位置式PID 51 | 52 | 53 | 54 | 2 55 | 56 | 57 | 58 | 59 | 60 | 3 61 | 62 | 63 | 64 | 65 | 66 | ## PID参数调整（位置式） 67 | 68 | 69 | 70 | 4 71 | 72 | 73 | 74 | 75 | 76 | 5 77 | 78 | 79 | 80 | **参数整定找最佳，从小到大顺序查，** 81 | 82 | **先是比例后积分，最后再把微分加，** 83 | 84 | **曲线振荡很频繁，比例度盘要放大，** 85 | 86 | **曲线漂浮绕大湾，比例度盘往小扳，** 87 | 88 | **曲线偏离回复慢，积分时间往下降，** 89 | 90 | **曲线波动周期长，积分时间再加长，** 91 | 92 | **曲线振荡频率快，先把微分降下来，** 93 | 94 | **动差大来波动慢，微分时间应加长，** 95 | 96 | **理想曲线两个波，前高后低四比一**， 97 | 98 | **一看二调多分析，调节质量不会低。** 99 | 100 | 比例是回复力，积分是惯性力（有滞后性），微分是阻尼力（但是会放大噪声，可能会导致稳定性变差） 101 | 102 | 快速响应系统变化的控制任务，如速度控制，偏向D些。更慢的则偏向I些。泛泛地说，偏运动控制的更快，更偏向D些。偏过程控制的更慢，更偏向I些。中间的则如机械、动力、汽车类的，ID比较平衡。 103 | 104 | 105 | 106 | ## 纵向控制PID调参经验： 107 | 108 | **P比例系数，对误差进行比例项的控制，P很大的话，可以加快当前值到达目标值的速度（上升时间）**，但P过大也会带来超调，实际中，由于信号的延迟，比例项P会引起超调，在仿真时可以在信号后添加延时模块，模拟真实情况。**当Kp越小时，稳态误差就越大**。通过增大Kp来减小稳态误差理论上是可行的，但是由于过大的Kp会引起系统的超调。所以我们并不能无限制的增大Kp，因此我们就需要引入一个新的控制器。**当比较接近目标时，P的控制作用就比较小了。越接近目标，P的作用越温柔**。有很多内在的或者外部的因素，使控制量发生小范围的摆动 109 | 110 | **I积分项，为了消除稳态误差**，但是会有超调，因为一般不要求准确到达目标值，只需要满足一个精度就可以，可以添加一个很小的积分项，如果时间足够长的话，会使得稳态误差收敛到0 111 | 112 | **D微分项，抑制超调**，在实际中，微分项很重要，降低由积分项I引起的超调会比较难，又要通过降低P来降低超调，但此时响应就会比较慢，数值微分，有提前控制的作用，比如当前的速度是6，目标的速度是10，3s后的速度是12，微分项充当10-12=-2的作用， 113 | 114 | 经验：**先调P，再调D，最后调I，一般不使用积分项，使用PD控制只要控制的精度属于一个范围就可以，因为过分的追求稳态误差就会引起超调量，为了抑制超调，就要增大D，但同时也会放大噪声，因为噪声是高频低幅的，微分项会使得伯德图的对数幅频曲线往上翘，会放大高频噪声** 115 | -------------------------------------------------------------------------------- /note/转向相关知识点.md: -------------------------------------------------------------------------------- 1 | # 阿克曼转角 2 | 3 | 参考：https://zhuanlan.zhihu.com/p/408058383 4 | 5 | 使内转角大于外轮转角，使得四轮转弯时的轴线相交于同一点，此时汽车绕着该点做纯滚动。（阻力小+稳定） 6 | 7 | 8 | 9 | ![image-20221108210259424](https://gitee.com/czjaixuexi/typora_pictures/raw/master/img/image-20221108210259424.png) 10 | 11 | 12 | 13 | 14 | 15 | ## 校正率： 16 | 17 | ![image-20221108210639360](https://gitee.com/czjaixuexi/typora_pictures/raw/master/img/image-20221108210639360.png) 18 | 19 | 20 | 21 | ![image-20221108210728672](https://gitee.com/czjaixuexi/typora_pictures/raw/master/img/image-20221108210728672.png) 22 | 23 | 车辆在低速转弯行驶时，侧向加速度很小（认为0），理想的转向方式第一张图，也被称为理想阿克曼转向。 24 | 25 | 当车辆高速行驶转弯时，由于侧向角速度较大（不能忽略），根据轮胎特性（车轮侧偏角越大越容易达到侧滑极限），内轮更容易侧滑。**侧偏力导致车辆转向角增大，所以为了增加车辆的转弯极限能力，因此需要适当减小内轮转角和外轮转角的差值。** 26 | 27 | 通常家用车为了权衡转向性能和减少轮胎磨损，阿克曼率设置在60%-80%。 28 | 29 | 在一些赛车上，可能会用到反阿克曼（轮胎特性，外轮轮荷大，更大的侧偏角可以提供更大的侧向力）。 30 | 31 | 32 | 33 | # ABS最佳滑移率 34 | 35 | 汽车从纯滚动到抱死拖滑的制动过程是一个渐进的过程，经历了纯滚动、边滚边滑和纯滑动三个阶段。为了评价汽车车轮滑移成分所占比例的多少，常用滑移率s来表示，其定义如下： 36 | 37 | image-20221108212519447 38 | 39 | u为车速； uw为车轮速度；ω为车轮滚动角速度；r为车轮半径。 40 | 41 | image-20221108212835104 42 | 43 | 滑移率大约在20%左右时制动纵向附着系数ϕx最大，同时保证有足够的横向附着系数ϕ y 44 | 45 | ABS把车轮的滑移率保持在10%～30%的范围内，以保证车轮与路面有良好的纵向、侧向附着力，有效防止制动时汽车侧滑、甩尾、失去转向等现象发生 46 | 47 | 48 | 49 | # 汽车稳态转向特性 50 | 51 | 参考https://www.docin.com/p-1180549897.html 52 | 53 | ![image-20221109000828334](https://gitee.com/czjaixuexi/typora_pictures/raw/master/img/image-20221109000828334.png) 54 | 55 | ## 不足转向 56 | 57 | ![image-20221109001220169](https://gitee.com/czjaixuexi/typora_pictures/raw/master/img/image-20221109001220169.png) 58 | 59 | ## 中性转向 60 | 61 | ![image-20221109001237920](https://gitee.com/czjaixuexi/typora_pictures/raw/master/img/image-20221109001237920.png) 62 | 63 | ## 过度转向 64 | 65 | ![image-20221109001259857](https://gitee.com/czjaixuexi/typora_pictures/raw/master/img/image-20221109001259857.png) 66 | 67 | ![image-20221109001324961](https://gitee.com/czjaixuexi/typora_pictures/raw/master/img/image-20221109001324961.png) 68 | 69 | ## 表征稳态相应的参数 70 | 71 | ### 1、前后轮侧偏角之差 72 | 73 | ![image-20221109001403484](https://gitee.com/czjaixuexi/typora_pictures/raw/master/img/image-20221109001403484.png) 74 | 75 | ![image-20221109001430337](https://gitee.com/czjaixuexi/typora_pictures/raw/master/img/image-20221109001430337.png) 76 | 77 | ### 2、转向半径比 78 | 79 | ![image-20221109001454840](https://gitee.com/czjaixuexi/typora_pictures/raw/master/img/image-20221109001454840.png) 80 | 81 | 82 | 83 | ### 3、静态储备系数 84 | 85 | ![image-20221109001704900](https://gitee.com/czjaixuexi/typora_pictures/raw/master/img/image-20221109001704900.png) 86 | 87 | ![image-20221109001711050](https://gitee.com/czjaixuexi/typora_pictures/raw/master/img/image-20221109001711050.png) -------------------------------------------------------------------------------- /note/运动学+动力学模型.md: -------------------------------------------------------------------------------- 1 | # 车辆运动学模型 2 | 3 | 4 | 5 | 参考： 6 | 7 | [车辆运动学模型](https://blog.csdn.net/weixin_42301220/article/details/124747072) 8 | 9 | 纠正： 10 | 11 | $\psi$ 是横摆角 12 | 13 | $\beta$是质心侧偏角 14 | 15 | $\psi+\beta$ 为航向角 16 | 17 | 18 | 19 | image-20230509215330579 20 | 21 | 22 | 23 | [运动学模型的线性化+离散化](https://windses.blog.csdn.net/article/details/103463683) 24 | 25 | 26 | 27 | # 车辆动力学模型 28 | 29 | [动力学模型](notebook/自动驾驶控制算法第三讲2.pdf) 30 | 31 | [动力学横向控制误差模型](notebook/自动驾驶控制算法第四讲2.pdf) --------------------------------------------------------------------------------