From 85907454a46c58603eb997d16968dacfb41173a6 Mon Sep 17 00:00:00 2001 From: Kotakku Date: Fri, 5 Jan 2024 14:25:05 +0900 Subject: [PATCH 1/6] =?UTF-8?q?=E4=B8=80=E6=97=A6=E3=83=95=E3=82=A1?= =?UTF-8?q?=E3=82=A4=E3=83=AB=E6=95=B4=E7=90=86?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- CMakeLists.txt | 3 +++ benckmark/build_time.cpp | 6 ++++++ include/cpp_robotics/algorithm.hpp | 6 +++--- include/cpp_robotics/arm_ik.hpp | 1 - include/cpp_robotics/controller.hpp | 18 ++++++++++++++---- include/cpp_robotics/core.hpp | 4 ++-- include/cpp_robotics/cpp_robotics.hpp | 1 - include/cpp_robotics/filter.hpp | 4 +++- include/cpp_robotics/optimize.hpp | 2 -- include/cpp_robotics/path_planning.hpp | 2 -- include/cpp_robotics/system.hpp | 4 +++- .../cpp_robotics/system/transfer_function.hpp | 1 - include/cpp_robotics/utility.hpp | 4 +++- include/cpp_robotics/utility/eigen_utils.hpp | 8 +++++--- unsupported/arm_ik/arm_ik.hpp | 3 +++ .../arm_ik/delta_robot.hpp | 0 .../xy_2deg_robot/xy_2deg_robot.hpp | 0 .../xy_2deg_robot/xy_2deg_robot_test.cpp | 0 .../filter_connector.hpp | 0 .../utility => unsupported}/ginac_utils.hpp | 0 .../algorithm => unsupported}/misc.hpp | 0 .../algorithm => unsupported}/ndt.hpp | 0 .../{ => optimize}/fmin_search_test.cpp | 0 .../optimize/interior_point_method.hpp | 0 unsupported/{ => optimize}/ipopt_example.cpp | 0 .../optimize/lesi_doc.hpp | 0 .../optimize/linprog.hpp | 0 .../path_planning/dwa.hpp | 0 .../path_planning/frenet_frame.hpp | 0 .../state_machine.hpp | 0 30 files changed, 45 insertions(+), 22 deletions(-) create mode 100644 benckmark/build_time.cpp delete mode 100644 include/cpp_robotics/arm_ik.hpp create mode 100644 unsupported/arm_ik/arm_ik.hpp rename {include/cpp_robotics => unsupported}/arm_ik/delta_robot.hpp (100%) rename unsupported/{ => arm_ik}/xy_2deg_robot/xy_2deg_robot.hpp (100%) rename unsupported/{ => arm_ik}/xy_2deg_robot/xy_2deg_robot_test.cpp (100%) rename {include/cpp_robotics/filter => unsupported}/filter_connector.hpp (100%) rename {include/cpp_robotics/utility => unsupported}/ginac_utils.hpp (100%) rename {include/cpp_robotics/algorithm => unsupported}/misc.hpp (100%) rename {include/cpp_robotics/algorithm => unsupported}/ndt.hpp (100%) rename unsupported/{ => optimize}/fmin_search_test.cpp (100%) rename {include/cpp_robotics => unsupported}/optimize/interior_point_method.hpp (100%) rename unsupported/{ => optimize}/ipopt_example.cpp (100%) rename {include/cpp_robotics => unsupported}/optimize/lesi_doc.hpp (100%) rename {include/cpp_robotics => unsupported}/optimize/linprog.hpp (100%) rename {include/cpp_robotics => unsupported}/path_planning/dwa.hpp (100%) rename {include/cpp_robotics => unsupported}/path_planning/frenet_frame.hpp (100%) rename {include/cpp_robotics/algorithm => unsupported}/state_machine.hpp (100%) diff --git a/CMakeLists.txt b/CMakeLists.txt index 4c9ee4de5..37e72aaa2 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -129,7 +129,10 @@ if(BUILD_BENCHMARKS) find_package(Python3 COMPONENTS Development NumPy) +# set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -ftime-report") + set(bench_test_files + build_time autodiff eigen_arg_ret ) diff --git a/benckmark/build_time.cpp b/benckmark/build_time.cpp new file mode 100644 index 000000000..3de5465d9 --- /dev/null +++ b/benckmark/build_time.cpp @@ -0,0 +1,6 @@ +#include +#include + +int main() { + return 0; +} \ No newline at end of file diff --git a/include/cpp_robotics/algorithm.hpp b/include/cpp_robotics/algorithm.hpp index f31fef756..038b3a65f 100644 --- a/include/cpp_robotics/algorithm.hpp +++ b/include/cpp_robotics/algorithm.hpp @@ -2,9 +2,9 @@ #include "algorithm/kdtree.hpp" #include "algorithm/icp.hpp" -#include "algorithm/ndt.hpp" #include "algorithm/poly_regression.hpp" -#include "algorithm/state_machine.hpp" #include "algorithm/k_means_method.hpp" #include "algorithm/mahalanobis.hpp" -#include "algorithm/random.hpp" \ No newline at end of file +#include "algorithm/random.hpp" +#include "algorithm/transformation.hpp" +#include "algorithm/auto_diff.hpp" \ No newline at end of file diff --git a/include/cpp_robotics/arm_ik.hpp b/include/cpp_robotics/arm_ik.hpp deleted file mode 100644 index 7b9637ef9..000000000 --- a/include/cpp_robotics/arm_ik.hpp +++ /dev/null @@ -1 +0,0 @@ -#pragma once \ No newline at end of file diff --git a/include/cpp_robotics/controller.hpp b/include/cpp_robotics/controller.hpp index c8e160eed..7849de231 100644 --- a/include/cpp_robotics/controller.hpp +++ b/include/cpp_robotics/controller.hpp @@ -2,9 +2,19 @@ #include "controller/pid.hpp" #include "controller/pid2.hpp" -#include "controller/lqr.hpp" -#include "controller/pure_pursuit.hpp" +#include "controller/psmc.hpp" #include "controller/nctf.hpp" -#include "controller/modern_control.hpp" + #include "controller/pfc.hpp" -#include "controller/vrft.hpp" \ No newline at end of file +#include "controller/pure_pursuit.hpp" + +// ここから時間かかる +#include "controller/lqr.hpp" // + 7.0s +#include "controller/modern_control.hpp" // +1.7s +#include "controller/vrft.hpp" // +4.0s +#include "controller/linear_mpc.hpp" // +2.6s +#include "controller/linear_regulator_mpc.hpp" // +2.6s +#include "controller/ocp_dynamics.hpp" +#include "controller/ocp_cost.hpp" +#include "controller/ocp_constraint.hpp" +#include "controller/optimal_control_problem.hpp" diff --git a/include/cpp_robotics/core.hpp b/include/cpp_robotics/core.hpp index 5f726ea04..299bf9420 100644 --- a/include/cpp_robotics/core.hpp +++ b/include/cpp_robotics/core.hpp @@ -6,6 +6,6 @@ #include "unit.hpp" // control core +#include "system.hpp" #include "controller.hpp" -#include "filter.hpp" -#include "system.hpp" \ No newline at end of file +#include "filter.hpp" \ No newline at end of file diff --git a/include/cpp_robotics/cpp_robotics.hpp b/include/cpp_robotics/cpp_robotics.hpp index 53172c9cd..31f8fd5c7 100644 --- a/include/cpp_robotics/cpp_robotics.hpp +++ b/include/cpp_robotics/cpp_robotics.hpp @@ -3,7 +3,6 @@ #include "core.hpp" #include "path_planning.hpp" -#include "filter.hpp" #include "chassis.hpp" #include "algorithm.hpp" #include "motor.hpp" diff --git a/include/cpp_robotics/filter.hpp b/include/cpp_robotics/filter.hpp index 1880f084b..a4b93cc6c 100644 --- a/include/cpp_robotics/filter.hpp +++ b/include/cpp_robotics/filter.hpp @@ -12,4 +12,6 @@ #include "filter/acceleration_limit_filter.hpp" #include "filter/filter_state_holder.hpp" #include "filter/kalman_filter.hpp" -#include "filter/extended_kalman_filter.hpp" + +// 時間かかる +#include "filter/extended_kalman_filter.hpp" // +2.3s diff --git a/include/cpp_robotics/optimize.hpp b/include/cpp_robotics/optimize.hpp index 21a92d555..279929139 100644 --- a/include/cpp_robotics/optimize.hpp +++ b/include/cpp_robotics/optimize.hpp @@ -7,8 +7,6 @@ #include "optimize/quasi_newton_method.hpp" #include "optimize/penalty_method.hpp" #include "optimize/barrier_method.hpp" -#include "optimize/linprog.hpp" #include "optimize/quadprog.hpp" #include "optimize/sqp.hpp" -#include "optimize/interior_point_method.hpp" #include "optimize/active_set_method.hpp" \ No newline at end of file diff --git a/include/cpp_robotics/path_planning.hpp b/include/cpp_robotics/path_planning.hpp index dd128c121..6ce89a2e3 100644 --- a/include/cpp_robotics/path_planning.hpp +++ b/include/cpp_robotics/path_planning.hpp @@ -2,6 +2,4 @@ #include "path_planning/a_star.hpp" #include "path_planning/wave_propagation.hpp" #include "path_planning/dubins_path.hpp" -#include "path_planning/frenet_frame.hpp" -#include "path_planning/dwa.hpp" #include "path_planning/spline.hpp" \ No newline at end of file diff --git a/include/cpp_robotics/system.hpp b/include/cpp_robotics/system.hpp index 387f26cc7..38ab31ba8 100644 --- a/include/cpp_robotics/system.hpp +++ b/include/cpp_robotics/system.hpp @@ -7,4 +7,6 @@ #include "system/bode.hpp" #include "system/nyquist.hpp" #include "system/time_responce.hpp" -#include "system/siso_system.hpp" \ No newline at end of file +#include "system/siso_system.hpp" +#include "system/dynamics_integrate.hpp" +#include "system/system_transformation.hpp" // +4.0s \ No newline at end of file diff --git a/include/cpp_robotics/system/transfer_function.hpp b/include/cpp_robotics/system/transfer_function.hpp index 0a8c03a8e..ffa5eefbc 100644 --- a/include/cpp_robotics/system/transfer_function.hpp +++ b/include/cpp_robotics/system/transfer_function.hpp @@ -1,6 +1,5 @@ #pragma once -#include #include #include diff --git a/include/cpp_robotics/utility.hpp b/include/cpp_robotics/utility.hpp index 4e1dbf6cc..6899ef89d 100644 --- a/include/cpp_robotics/utility.hpp +++ b/include/cpp_robotics/utility.hpp @@ -4,4 +4,6 @@ #include "utility/angle_range.hpp" #include "utility/space.hpp" #include "utility/singleton.hpp" -#include "utility/cpp_support.hpp" \ No newline at end of file +#include "utility/cpp_support.hpp" +#include "utility/eigen_utils.hpp" +#include "utility/timer.hpp" \ No newline at end of file diff --git a/include/cpp_robotics/utility/eigen_utils.hpp b/include/cpp_robotics/utility/eigen_utils.hpp index 49976f74f..f7e07faa3 100755 --- a/include/cpp_robotics/utility/eigen_utils.hpp +++ b/include/cpp_robotics/utility/eigen_utils.hpp @@ -5,10 +5,12 @@ namespace cpp_robotics { -bool isPositiveDefinite(const Eigen::MatrixXd& matrix) { - Eigen::LLT llt(matrix); // Compute the Cholesky decomposition +// 行列が正定値かどうか +template +bool isPositiveDefinite(const MatrixType& matrix) { + Eigen::LLT llt(matrix); if(llt.info() == Eigen::NumericalIssue) { - return false; // Matrix is not positive definite + return false; } return true; } diff --git a/unsupported/arm_ik/arm_ik.hpp b/unsupported/arm_ik/arm_ik.hpp new file mode 100644 index 000000000..bf292945b --- /dev/null +++ b/unsupported/arm_ik/arm_ik.hpp @@ -0,0 +1,3 @@ +#pragma once + +#include "arm_ik/delta_robot.hpp" \ No newline at end of file diff --git a/include/cpp_robotics/arm_ik/delta_robot.hpp b/unsupported/arm_ik/delta_robot.hpp similarity index 100% rename from include/cpp_robotics/arm_ik/delta_robot.hpp rename to unsupported/arm_ik/delta_robot.hpp diff --git a/unsupported/xy_2deg_robot/xy_2deg_robot.hpp b/unsupported/arm_ik/xy_2deg_robot/xy_2deg_robot.hpp similarity index 100% rename from unsupported/xy_2deg_robot/xy_2deg_robot.hpp rename to unsupported/arm_ik/xy_2deg_robot/xy_2deg_robot.hpp diff --git a/unsupported/xy_2deg_robot/xy_2deg_robot_test.cpp b/unsupported/arm_ik/xy_2deg_robot/xy_2deg_robot_test.cpp similarity index 100% rename from unsupported/xy_2deg_robot/xy_2deg_robot_test.cpp rename to unsupported/arm_ik/xy_2deg_robot/xy_2deg_robot_test.cpp diff --git a/include/cpp_robotics/filter/filter_connector.hpp b/unsupported/filter_connector.hpp similarity index 100% rename from include/cpp_robotics/filter/filter_connector.hpp rename to unsupported/filter_connector.hpp diff --git a/include/cpp_robotics/utility/ginac_utils.hpp b/unsupported/ginac_utils.hpp similarity index 100% rename from include/cpp_robotics/utility/ginac_utils.hpp rename to unsupported/ginac_utils.hpp diff --git a/include/cpp_robotics/algorithm/misc.hpp b/unsupported/misc.hpp similarity index 100% rename from include/cpp_robotics/algorithm/misc.hpp rename to unsupported/misc.hpp diff --git a/include/cpp_robotics/algorithm/ndt.hpp b/unsupported/ndt.hpp similarity index 100% rename from include/cpp_robotics/algorithm/ndt.hpp rename to unsupported/ndt.hpp diff --git a/unsupported/fmin_search_test.cpp b/unsupported/optimize/fmin_search_test.cpp similarity index 100% rename from unsupported/fmin_search_test.cpp rename to unsupported/optimize/fmin_search_test.cpp diff --git a/include/cpp_robotics/optimize/interior_point_method.hpp b/unsupported/optimize/interior_point_method.hpp similarity index 100% rename from include/cpp_robotics/optimize/interior_point_method.hpp rename to unsupported/optimize/interior_point_method.hpp diff --git a/unsupported/ipopt_example.cpp b/unsupported/optimize/ipopt_example.cpp similarity index 100% rename from unsupported/ipopt_example.cpp rename to unsupported/optimize/ipopt_example.cpp diff --git a/include/cpp_robotics/optimize/lesi_doc.hpp b/unsupported/optimize/lesi_doc.hpp similarity index 100% rename from include/cpp_robotics/optimize/lesi_doc.hpp rename to unsupported/optimize/lesi_doc.hpp diff --git a/include/cpp_robotics/optimize/linprog.hpp b/unsupported/optimize/linprog.hpp similarity index 100% rename from include/cpp_robotics/optimize/linprog.hpp rename to unsupported/optimize/linprog.hpp diff --git a/include/cpp_robotics/path_planning/dwa.hpp b/unsupported/path_planning/dwa.hpp similarity index 100% rename from include/cpp_robotics/path_planning/dwa.hpp rename to unsupported/path_planning/dwa.hpp diff --git a/include/cpp_robotics/path_planning/frenet_frame.hpp b/unsupported/path_planning/frenet_frame.hpp similarity index 100% rename from include/cpp_robotics/path_planning/frenet_frame.hpp rename to unsupported/path_planning/frenet_frame.hpp diff --git a/include/cpp_robotics/algorithm/state_machine.hpp b/unsupported/state_machine.hpp similarity index 100% rename from include/cpp_robotics/algorithm/state_machine.hpp rename to unsupported/state_machine.hpp From 422970351e248da15e855db1e16a6447180bfbc4 Mon Sep 17 00:00:00 2001 From: Kotakku Date: Fri, 5 Jan 2024 15:19:59 +0900 Subject: [PATCH 2/6] =?UTF-8?q?=E3=83=93=E3=83=AB=E3=83=89=E9=80=9A?= =?UTF-8?q?=E3=81=97=E3=81=9F?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- CMakeLists.txt | 4 ++-- benckmark/build_time.cpp | 20 ++++++++++++++-- example/algorithm/icp_sample.cpp | 4 ++-- example/algorithm/random_example.cpp | 2 +- example/controller/canonicalize.cpp | 1 + example/controller/linear_mpc.cpp | 5 ++-- example/controller/lqr.cpp | 5 ++-- example/controller/motor_pid_control.cpp | 6 ++--- example/controller/nctf_example.cpp | 7 +++--- example/controller/pid_vs_nctf.cpp | 7 +++--- example/controller/place.cpp | 1 + example/controller/vrft_pid.cpp | 7 +++--- example/hello_world.cpp | 5 +++- .../optimize/active_set_method_example.cpp | 1 - example/optimize/optimize_rosenbrock.cpp | 2 +- example/optimize/optimize_scalar.cpp | 2 +- example/optimize/optimize_with_constraint.cpp | 3 ++- .../optimize/optimize_without_constraint.cpp | 2 +- example/optimize/sqp_example.cpp | 1 - example/path_planning/dubins_path.cpp | 3 ++- example/system/state_space_system.cpp | 2 ++ include/cpp_robotics/algorithm.hpp | 10 -------- .../algorithm/poly_regression.hpp | 2 -- include/cpp_robotics/controller.hpp | 24 +++++++++---------- include/cpp_robotics/core.hpp | 11 --------- include/cpp_robotics/cpp_robotics.hpp | 13 ---------- include/cpp_robotics/filter.hpp | 6 ++--- include/cpp_robotics/matplotlibcpp.hpp | 1 + include/cpp_robotics/optimize.hpp | 12 ---------- .../optimize/no_constraint_method.hpp | 5 ++++ include/cpp_robotics/path_planning.hpp | 5 ---- .../path_planning/dubins_path.hpp | 5 ++-- include/cpp_robotics/system.hpp | 4 ++-- include/cpp_robotics/system/polynomial.hpp | 3 +-- include/cpp_robotics/utility.hpp | 9 ++++--- include/cpp_robotics/utility/cpp_support.hpp | 14 ----------- .../{algorithm => utility}/random.hpp | 2 +- test/algorithm/icp_test.cpp | 2 +- test/common/lib_test.cpp | 9 ------- test/optimize/sqp_test.cpp | 2 -- test/{algorithm => utility}/random_test.cpp | 2 +- unsupported/print.hpp | 18 ++++++++++++++ 42 files changed, 112 insertions(+), 137 deletions(-) delete mode 100644 include/cpp_robotics/algorithm.hpp delete mode 100644 include/cpp_robotics/core.hpp delete mode 100644 include/cpp_robotics/cpp_robotics.hpp create mode 100644 include/cpp_robotics/matplotlibcpp.hpp delete mode 100644 include/cpp_robotics/optimize.hpp create mode 100644 include/cpp_robotics/optimize/no_constraint_method.hpp delete mode 100644 include/cpp_robotics/path_planning.hpp rename include/cpp_robotics/{algorithm => utility}/random.hpp (99%) delete mode 100644 test/common/lib_test.cpp rename test/{algorithm => utility}/random_test.cpp (98%) create mode 100644 unsupported/print.hpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 37e72aaa2..1cf90fd93 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -79,11 +79,11 @@ if(BUILD_TESTS) find_package(GTest REQUIRED) enable_testing() set(test_files - common/lib_test + # common/lib_test utility/singleton_test + utility/random_test algorithm/poly_regression_test - algorithm/random_test algorithm/k_means_test algorithm/auto_diff_test algorithm/icp_test diff --git a/benckmark/build_time.cpp b/benckmark/build_time.cpp index 3de5465d9..e8fe5b63e 100644 --- a/benckmark/build_time.cpp +++ b/benckmark/build_time.cpp @@ -1,5 +1,21 @@ -#include -#include +// utility core +#include "cpp_robotics/utility.hpp" // 1.15 +#include "cpp_robotics/vector.hpp" // 0.26 +#include "cpp_robotics/unit.hpp" // 0.29 + +#include "cpp_robotics/system.hpp" // 2.4 +#include "cpp_robotics/controller.hpp" // 3.0 +#include "cpp_robotics/filter.hpp" // 2.0 + +#include "cpp_robotics/chassis.hpp" // 1.9 +#include "cpp_robotics/motor.hpp" // 2.0 + + +// 廃止 +// #include "cpp_robotics/path_planning.hpp" +// #include "cpp_robotics/algorithm.hpp" +// #include "cpp_robotics/optimize.hpp" + int main() { return 0; diff --git a/example/algorithm/icp_sample.cpp b/example/algorithm/icp_sample.cpp index 5d85359a0..564b91e12 100644 --- a/example/algorithm/icp_sample.cpp +++ b/example/algorithm/icp_sample.cpp @@ -1,9 +1,9 @@ #define CR_USE_MATPLOTLIB #include #include -#include +#include +#include #include -#include #include int main() diff --git a/example/algorithm/random_example.cpp b/example/algorithm/random_example.cpp index da63070a7..2f92705a2 100644 --- a/example/algorithm/random_example.cpp +++ b/example/algorithm/random_example.cpp @@ -1,5 +1,5 @@ #include -#include +#include double random_std() { diff --git a/example/controller/canonicalize.cpp b/example/controller/canonicalize.cpp index 609fb524c..ad55808ea 100644 --- a/example/controller/canonicalize.cpp +++ b/example/controller/canonicalize.cpp @@ -2,6 +2,7 @@ #include #include #include +#include #define PRINT_MAT(MAT) \ std::cout << #MAT << std::endl;\ diff --git a/example/controller/linear_mpc.cpp b/example/controller/linear_mpc.cpp index f4e0bd00a..857f6e341 100644 --- a/example/controller/linear_mpc.cpp +++ b/example/controller/linear_mpc.cpp @@ -1,10 +1,9 @@ #define CR_USE_MATPLOTLIB #include - -#include +#include #include #include -#include +#include int main() diff --git a/example/controller/lqr.cpp b/example/controller/lqr.cpp index 26fbdeec2..3fd3079cf 100644 --- a/example/controller/lqr.cpp +++ b/example/controller/lqr.cpp @@ -1,7 +1,8 @@ #define CR_USE_MATPLOTLIB #include -#include - +#include +#include +#include int main() { diff --git a/example/controller/motor_pid_control.cpp b/example/controller/motor_pid_control.cpp index 54545a3e7..b1416180d 100644 --- a/example/controller/motor_pid_control.cpp +++ b/example/controller/motor_pid_control.cpp @@ -1,8 +1,8 @@ #define CR_USE_MATPLOTLIB #include -#include -#include -#include +#include +#include +#include static std::tuple, std::vector> ref_trajectory(double time, double dt) { diff --git a/example/controller/nctf_example.cpp b/example/controller/nctf_example.cpp index 654215093..1c855e78b 100644 --- a/example/controller/nctf_example.cpp +++ b/example/controller/nctf_example.cpp @@ -1,8 +1,9 @@ #define CR_USE_MATPLOTLIB #include -#include -#include -#include +#include +#include +#include +#include static std::tuple, std::vector> ref_trajectory(double time, double dt) { diff --git a/example/controller/pid_vs_nctf.cpp b/example/controller/pid_vs_nctf.cpp index bfa7262f8..e1a3af556 100644 --- a/example/controller/pid_vs_nctf.cpp +++ b/example/controller/pid_vs_nctf.cpp @@ -1,8 +1,9 @@ #define CR_USE_MATPLOTLIB #include -#include -#include -#include +#include +#include +#include +#include static std::tuple, std::vector> ref_trajectory(double time, double dt) { diff --git a/example/controller/place.cpp b/example/controller/place.cpp index 6892e8d1d..3e6f87b00 100644 --- a/example/controller/place.cpp +++ b/example/controller/place.cpp @@ -3,6 +3,7 @@ #include #include #include +#include #define PRINT_MAT(MAT) \ std::cout << #MAT << std::endl;\ diff --git a/example/controller/vrft_pid.cpp b/example/controller/vrft_pid.cpp index 8fb7d32b7..121c91491 100644 --- a/example/controller/vrft_pid.cpp +++ b/example/controller/vrft_pid.cpp @@ -1,10 +1,11 @@ #define CR_USE_MATPLOTLIB #include #include -#include -#include +#include +#include +#include +#include #include -#include #include #include diff --git a/example/hello_world.cpp b/example/hello_world.cpp index 1a6d7b5b2..0d240bb4b 100644 --- a/example/hello_world.cpp +++ b/example/hello_world.cpp @@ -1,7 +1,10 @@ #define CR_USE_MATPLOTLIB #include #include -#include +#include +#include +#include +#include int main() { diff --git a/example/optimize/active_set_method_example.cpp b/example/optimize/active_set_method_example.cpp index 45ff34ee0..acb847e10 100644 --- a/example/optimize/active_set_method_example.cpp +++ b/example/optimize/active_set_method_example.cpp @@ -1,5 +1,4 @@ #include -#include #include int main() diff --git a/example/optimize/optimize_rosenbrock.cpp b/example/optimize/optimize_rosenbrock.cpp index c320816e4..9448589df 100644 --- a/example/optimize/optimize_rosenbrock.cpp +++ b/example/optimize/optimize_rosenbrock.cpp @@ -1,6 +1,6 @@ #include #include -#include +#include int main() { diff --git a/example/optimize/optimize_scalar.cpp b/example/optimize/optimize_scalar.cpp index 3c025bdc0..1c2aa4229 100644 --- a/example/optimize/optimize_scalar.cpp +++ b/example/optimize/optimize_scalar.cpp @@ -1,6 +1,6 @@ #include #include -#include +#include int main() { diff --git a/example/optimize/optimize_with_constraint.cpp b/example/optimize/optimize_with_constraint.cpp index 6585ac0fe..9c202be3f 100644 --- a/example/optimize/optimize_with_constraint.cpp +++ b/example/optimize/optimize_with_constraint.cpp @@ -1,6 +1,7 @@ #include #include -#include +#include +#include int main() { diff --git a/example/optimize/optimize_without_constraint.cpp b/example/optimize/optimize_without_constraint.cpp index 5e3d0ab36..b6ad0556a 100644 --- a/example/optimize/optimize_without_constraint.cpp +++ b/example/optimize/optimize_without_constraint.cpp @@ -1,6 +1,6 @@ #include #include -#include +#include int main() { diff --git a/example/optimize/sqp_example.cpp b/example/optimize/sqp_example.cpp index 811fd6ad8..872c6c2fc 100644 --- a/example/optimize/sqp_example.cpp +++ b/example/optimize/sqp_example.cpp @@ -1,6 +1,5 @@ #define CR_USE_MATPLOTLIB #include -#include #include #include "cpp_robotics/third_party/matplotlib-cpp/matplotlibcpp.h" diff --git a/example/path_planning/dubins_path.cpp b/example/path_planning/dubins_path.cpp index 4456284b9..d9d340d6a 100644 --- a/example/path_planning/dubins_path.cpp +++ b/example/path_planning/dubins_path.cpp @@ -1,6 +1,7 @@ #define CR_USE_MATPLOTLIB #include -#include "cpp_robotics/cpp_robotics.hpp" +#include +#include "cpp_robotics/path_planning/dubins_path.hpp" int main() { diff --git a/example/system/state_space_system.cpp b/example/system/state_space_system.cpp index a899a0573..0a9784422 100644 --- a/example/system/state_space_system.cpp +++ b/example/system/state_space_system.cpp @@ -1,5 +1,7 @@ +#include #include #include +#include int main() { diff --git a/include/cpp_robotics/algorithm.hpp b/include/cpp_robotics/algorithm.hpp deleted file mode 100644 index 038b3a65f..000000000 --- a/include/cpp_robotics/algorithm.hpp +++ /dev/null @@ -1,10 +0,0 @@ -#pragma once - -#include "algorithm/kdtree.hpp" -#include "algorithm/icp.hpp" -#include "algorithm/poly_regression.hpp" -#include "algorithm/k_means_method.hpp" -#include "algorithm/mahalanobis.hpp" -#include "algorithm/random.hpp" -#include "algorithm/transformation.hpp" -#include "algorithm/auto_diff.hpp" \ No newline at end of file diff --git a/include/cpp_robotics/algorithm/poly_regression.hpp b/include/cpp_robotics/algorithm/poly_regression.hpp index 5f49f92d5..20d28310e 100644 --- a/include/cpp_robotics/algorithm/poly_regression.hpp +++ b/include/cpp_robotics/algorithm/poly_regression.hpp @@ -1,7 +1,5 @@ #pragma once -#include -#include #include #include "cpp_robotics/system/polynomial.hpp" diff --git a/include/cpp_robotics/controller.hpp b/include/cpp_robotics/controller.hpp index 7849de231..88f5bc9a4 100644 --- a/include/cpp_robotics/controller.hpp +++ b/include/cpp_robotics/controller.hpp @@ -5,16 +5,16 @@ #include "controller/psmc.hpp" #include "controller/nctf.hpp" -#include "controller/pfc.hpp" -#include "controller/pure_pursuit.hpp" +// #include "controller/pfc.hpp" +// #include "controller/pure_pursuit.hpp" -// ここから時間かかる -#include "controller/lqr.hpp" // + 7.0s -#include "controller/modern_control.hpp" // +1.7s -#include "controller/vrft.hpp" // +4.0s -#include "controller/linear_mpc.hpp" // +2.6s -#include "controller/linear_regulator_mpc.hpp" // +2.6s -#include "controller/ocp_dynamics.hpp" -#include "controller/ocp_cost.hpp" -#include "controller/ocp_constraint.hpp" -#include "controller/optimal_control_problem.hpp" +// // ここから時間かかる +// #include "controller/lqr.hpp" // + 7.0s +// #include "controller/modern_control.hpp" // +1.7s +// #include "controller/vrft.hpp" // +4.0s +// #include "controller/linear_mpc.hpp" // +2.6s +// #include "controller/linear_regulator_mpc.hpp" // +2.6s +// #include "controller/ocp_dynamics.hpp" +// #include "controller/ocp_cost.hpp" +// #include "controller/ocp_constraint.hpp" +// #include "controller/optimal_control_problem.hpp" diff --git a/include/cpp_robotics/core.hpp b/include/cpp_robotics/core.hpp deleted file mode 100644 index 299bf9420..000000000 --- a/include/cpp_robotics/core.hpp +++ /dev/null @@ -1,11 +0,0 @@ -#pragma once - -// utility core -#include "utility.hpp" -#include "vector.hpp" -#include "unit.hpp" - -// control core -#include "system.hpp" -#include "controller.hpp" -#include "filter.hpp" \ No newline at end of file diff --git a/include/cpp_robotics/cpp_robotics.hpp b/include/cpp_robotics/cpp_robotics.hpp deleted file mode 100644 index 31f8fd5c7..000000000 --- a/include/cpp_robotics/cpp_robotics.hpp +++ /dev/null @@ -1,13 +0,0 @@ -#pragma once - -#include "core.hpp" - -#include "path_planning.hpp" -#include "chassis.hpp" -#include "algorithm.hpp" -#include "motor.hpp" -#include "optimize.hpp" - -#ifdef CR_USE_MATPLOTLIB -#include "third_party/matplotlib-cpp/matplotlibcpp.h" -#endif \ No newline at end of file diff --git a/include/cpp_robotics/filter.hpp b/include/cpp_robotics/filter.hpp index a4b93cc6c..1b147e959 100644 --- a/include/cpp_robotics/filter.hpp +++ b/include/cpp_robotics/filter.hpp @@ -11,7 +11,7 @@ #include "filter/velocity_limit_filter.hpp" #include "filter/acceleration_limit_filter.hpp" #include "filter/filter_state_holder.hpp" -#include "filter/kalman_filter.hpp" +// #include "filter/kalman_filter.hpp" -// 時間かかる -#include "filter/extended_kalman_filter.hpp" // +2.3s +// // 時間かかる +// #include "filter/extended_kalman_filter.hpp" // +2.3s diff --git a/include/cpp_robotics/matplotlibcpp.hpp b/include/cpp_robotics/matplotlibcpp.hpp new file mode 100644 index 000000000..f2a6ee977 --- /dev/null +++ b/include/cpp_robotics/matplotlibcpp.hpp @@ -0,0 +1 @@ +#include \ No newline at end of file diff --git a/include/cpp_robotics/optimize.hpp b/include/cpp_robotics/optimize.hpp deleted file mode 100644 index 279929139..000000000 --- a/include/cpp_robotics/optimize.hpp +++ /dev/null @@ -1,12 +0,0 @@ -#include "optimize/constraint.hpp" -#include "optimize/derivative.hpp" -#include "optimize/bracketing_serach.hpp" -#include "optimize/golden_serach.hpp" -#include "optimize/steepest_descent_method.hpp" -#include "optimize/newton_method.hpp" -#include "optimize/quasi_newton_method.hpp" -#include "optimize/penalty_method.hpp" -#include "optimize/barrier_method.hpp" -#include "optimize/quadprog.hpp" -#include "optimize/sqp.hpp" -#include "optimize/active_set_method.hpp" \ No newline at end of file diff --git a/include/cpp_robotics/optimize/no_constraint_method.hpp b/include/cpp_robotics/optimize/no_constraint_method.hpp new file mode 100644 index 000000000..0a2aebaa9 --- /dev/null +++ b/include/cpp_robotics/optimize/no_constraint_method.hpp @@ -0,0 +1,5 @@ +#include +#include +#include +#include +#include \ No newline at end of file diff --git a/include/cpp_robotics/path_planning.hpp b/include/cpp_robotics/path_planning.hpp deleted file mode 100644 index 6ce89a2e3..000000000 --- a/include/cpp_robotics/path_planning.hpp +++ /dev/null @@ -1,5 +0,0 @@ -#include "path_planning/grid_path_planning_utils.hpp" -#include "path_planning/a_star.hpp" -#include "path_planning/wave_propagation.hpp" -#include "path_planning/dubins_path.hpp" -#include "path_planning/spline.hpp" \ No newline at end of file diff --git a/include/cpp_robotics/path_planning/dubins_path.hpp b/include/cpp_robotics/path_planning/dubins_path.hpp index 3c839f922..24272d371 100644 --- a/include/cpp_robotics/path_planning/dubins_path.hpp +++ b/include/cpp_robotics/path_planning/dubins_path.hpp @@ -1,8 +1,9 @@ #pragma once -#include "../vector/transform.hpp" +#include "cpp_robotics/utility.hpp" +#include "cpp_robotics/vector/transform.hpp" +#include #include -#include "../utility/math_utils.hpp" namespace cpp_robotics { diff --git a/include/cpp_robotics/system.hpp b/include/cpp_robotics/system.hpp index 38ab31ba8..360a4646b 100644 --- a/include/cpp_robotics/system.hpp +++ b/include/cpp_robotics/system.hpp @@ -8,5 +8,5 @@ #include "system/nyquist.hpp" #include "system/time_responce.hpp" #include "system/siso_system.hpp" -#include "system/dynamics_integrate.hpp" -#include "system/system_transformation.hpp" // +4.0s \ No newline at end of file +// #include "system/dynamics_integrate.hpp" +// #include "system/system_transformation.hpp" // +4.0s \ No newline at end of file diff --git a/include/cpp_robotics/system/polynomial.hpp b/include/cpp_robotics/system/polynomial.hpp index d950c2603..ec9ca9eb9 100644 --- a/include/cpp_robotics/system/polynomial.hpp +++ b/include/cpp_robotics/system/polynomial.hpp @@ -1,9 +1,8 @@ #pragma once #include -// #include #include -#include "../utility/cpp_support.hpp" +#include namespace cpp_robotics { diff --git a/include/cpp_robotics/utility.hpp b/include/cpp_robotics/utility.hpp index 6899ef89d..77f85a3ac 100644 --- a/include/cpp_robotics/utility.hpp +++ b/include/cpp_robotics/utility.hpp @@ -1,9 +1,12 @@ #pragma once +#include "utility/cpp_support.hpp" #include "utility/math_utils.hpp" #include "utility/angle_range.hpp" #include "utility/space.hpp" #include "utility/singleton.hpp" -#include "utility/cpp_support.hpp" -#include "utility/eigen_utils.hpp" -#include "utility/timer.hpp" \ No newline at end of file +#include "utility/random.hpp" + +#ifdef CR_USE_MATPLOTLIB +#include "third_party/matplotlib-cpp/matplotlibcpp.h" +#endif \ No newline at end of file diff --git a/include/cpp_robotics/utility/cpp_support.hpp b/include/cpp_robotics/utility/cpp_support.hpp index ea539eb5a..52aee758e 100644 --- a/include/cpp_robotics/utility/cpp_support.hpp +++ b/include/cpp_robotics/utility/cpp_support.hpp @@ -3,7 +3,6 @@ #include #include #include -#include // platform #if defined(linux) || defined(__linux) || defined(__linux__) @@ -123,17 +122,4 @@ std::string c_format(const std::string &format, Args const&... args) #pragma GCC diagnostic warning "-Wformat-security" } -/** - * @brief std::coutにprintfの形式でフォーマットされた文字列を出力する - * - * @tparam Args - * @param format - * @param args - */ -template -void print(const std::string &format, Args const&... args) -{ - std::cout << c_format(format, args...); -} - } \ No newline at end of file diff --git a/include/cpp_robotics/algorithm/random.hpp b/include/cpp_robotics/utility/random.hpp similarity index 99% rename from include/cpp_robotics/algorithm/random.hpp rename to include/cpp_robotics/utility/random.hpp index 4890fee21..2974ae267 100644 --- a/include/cpp_robotics/algorithm/random.hpp +++ b/include/cpp_robotics/utility/random.hpp @@ -2,7 +2,7 @@ #include #include -#include "../utility/singleton.hpp" +#include "cpp_robotics/utility/singleton.hpp" namespace cpp_robotics { diff --git a/test/algorithm/icp_test.cpp b/test/algorithm/icp_test.cpp index efc5fc85f..2e191dd1c 100644 --- a/test/algorithm/icp_test.cpp +++ b/test/algorithm/icp_test.cpp @@ -2,7 +2,7 @@ #include -#include +#include #include // #include diff --git a/test/common/lib_test.cpp b/test/common/lib_test.cpp deleted file mode 100644 index a0db38a4c..000000000 --- a/test/common/lib_test.cpp +++ /dev/null @@ -1,9 +0,0 @@ -#include - -// 一番大きいヘッダをincludeしてビルドが通るかどうかテスト -#include - -TEST(lib_test, test) { - namespace cr = cpp_robotics; - cr::print("hello world: %d, %.3lf\n", 100, 3.141); -} \ No newline at end of file diff --git a/test/optimize/sqp_test.cpp b/test/optimize/sqp_test.cpp index b8288cb74..7bd1136c4 100644 --- a/test/optimize/sqp_test.cpp +++ b/test/optimize/sqp_test.cpp @@ -1,8 +1,6 @@ #include #include -#include -#include #include diff --git a/test/algorithm/random_test.cpp b/test/utility/random_test.cpp similarity index 98% rename from test/algorithm/random_test.cpp rename to test/utility/random_test.cpp index 86c3af3aa..f337fc8cb 100644 --- a/test/algorithm/random_test.cpp +++ b/test/utility/random_test.cpp @@ -1,7 +1,7 @@ #include #include -#include +#include #include TEST(random, constructor) { diff --git a/unsupported/print.hpp b/unsupported/print.hpp new file mode 100644 index 000000000..5c1e7a3cf --- /dev/null +++ b/unsupported/print.hpp @@ -0,0 +1,18 @@ +#pragma once + +#include +#include +#include + +/** + * @brief std::coutにprintfの形式でフォーマットされた文字列を出力する + * + * @tparam Args + * @param format + * @param args + */ +template +void print(const std::string &format, Args const&... args) +{ + std::cout << c_format(format, args...); +} \ No newline at end of file From e4da258c31f0760841ab2a766a72fbf41d176eac Mon Sep 17 00:00:00 2001 From: Kotakku Date: Fri, 5 Jan 2024 15:25:16 +0900 Subject: [PATCH 3/6] typo --- CMakeLists.txt | 4 ++-- {benckmark => benchmark}/autodiff.cpp | 0 {benckmark => benchmark}/build_time.cpp | 7 ------- {benckmark => benchmark}/eigen_arg_ret.cpp | 0 4 files changed, 2 insertions(+), 9 deletions(-) rename {benckmark => benchmark}/autodiff.cpp (100%) rename {benckmark => benchmark}/build_time.cpp (73%) rename {benckmark => benchmark}/eigen_arg_ret.cpp (100%) diff --git a/CMakeLists.txt b/CMakeLists.txt index 1cf90fd93..3bedec2ae 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -138,8 +138,8 @@ set(bench_test_files ) foreach(bench_test_file ${bench_test_files}) -get_filename_component(bench_test_file benckmark/${bench_test_file}.cpp NAME_WE) -add_executable(bench_${bench_test_file} benckmark/${bench_test_file}.cpp) +get_filename_component(bench_test_file benchmark/${bench_test_file}.cpp NAME_WE) +add_executable(bench_${bench_test_file} benchmark/${bench_test_file}.cpp) target_include_directories(bench_${bench_test_file} PRIVATE include) target_link_libraries(bench_${bench_test_file} PRIVATE Eigen3::Eigen Python3::Python Python3::NumPy) endforeach() diff --git a/benckmark/autodiff.cpp b/benchmark/autodiff.cpp similarity index 100% rename from benckmark/autodiff.cpp rename to benchmark/autodiff.cpp diff --git a/benckmark/build_time.cpp b/benchmark/build_time.cpp similarity index 73% rename from benckmark/build_time.cpp rename to benchmark/build_time.cpp index e8fe5b63e..450737e12 100644 --- a/benckmark/build_time.cpp +++ b/benchmark/build_time.cpp @@ -10,13 +10,6 @@ #include "cpp_robotics/chassis.hpp" // 1.9 #include "cpp_robotics/motor.hpp" // 2.0 - -// 廃止 -// #include "cpp_robotics/path_planning.hpp" -// #include "cpp_robotics/algorithm.hpp" -// #include "cpp_robotics/optimize.hpp" - - int main() { return 0; } \ No newline at end of file diff --git a/benckmark/eigen_arg_ret.cpp b/benchmark/eigen_arg_ret.cpp similarity index 100% rename from benckmark/eigen_arg_ret.cpp rename to benchmark/eigen_arg_ret.cpp From a35560d6b9c955d209731e90a921d24a15493d52 Mon Sep 17 00:00:00 2001 From: Kotakku Date: Fri, 5 Jan 2024 15:44:46 +0900 Subject: [PATCH 4/6] =?UTF-8?q?VRFT=E3=81=AEexample=E6=95=B4=E7=90=86?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- example/controller/vrft_pid.cpp | 365 +------------------------------- 1 file changed, 2 insertions(+), 363 deletions(-) diff --git a/example/controller/vrft_pid.cpp b/example/controller/vrft_pid.cpp index 121c91491..ea9413458 100644 --- a/example/controller/vrft_pid.cpp +++ b/example/controller/vrft_pid.cpp @@ -174,370 +174,9 @@ void vrft_pid_with_closed_loop() plt::show(); } -using namespace cpp_robotics; -namespace plt = matplotlibcpp; -Eigen::VectorXd vrft_nctf(const Eigen::VectorXd &u, const Eigen::VectorXd &y, const double Ts, std::function nct, PID::pid_param_t base_param, TransferFunction ref_model, std::optional prefilter, double Td = 0.01) -{ - // PID::pid_param_t p_param = - // { - // .Ts = base_param.Ts, - // .gpd = base_param.gpd, - // .Kp = 1, - // .Ki = 0, - // .Kd = 0, - // .output_limit = std::nullopt - // }; - // PID::pid_param_t i_param = - // { - // .Ts = base_param.Ts, - // .gpd = base_param.gpd, - // .Kp = 0, - // .Ki = 1, - // .Kd = 0, - // .output_limit = std::nullopt - // }; - // std::vector control_base = { - // NctfController(nct, p_param, Ts), - // NctfController(nct, i_param, Ts) - // }; - - // (void) Ts; - - // Eigen::VectorXd uf, ef; - // if(prefilter) - // { - // uf = lsim_y(prefilter.value(), u); - // ef = lsim_y(prefilter.value()*(ref_model.inv()-1.0), y); - // } - // else - // { - // uf = u; - // ef = lsim_y((ref_model.inv()-1.0), y);; - // } - - // Eigen::MatrixXd phi(ef.size(), control_base.size()); - // for(size_t i = 0; i < control_base.size(); ++i) - // { - // phi.col(i) = lsim_y([&](double e){ return control_base[i].control(e); }, Ts, ef); - // } - - // plt::figure(); - // eigen_named_plot("ef", ef); - // eigen_named_plot("y", y); - // eigen_named_plot("p0", phi.col(0)); - // eigen_named_plot("p1", phi.col(1)); - // plt::legend(); - // plt::show(false); - // return leastsq(phi, uf); - - - std::vector control_base = { - TransferFunction({1.0}, {1.0}, Ts), - TransferFunction({1.0}, {1.0, 0.0}, Ts) - }; - - Differentiator vel_diff(base_param.gpd, Ts); - Eigen::VectorXd vel = lsim_y(vel_diff, y); - - plt::figure(); - eigen_named_plot("Vel", vel); - - - Eigen::VectorXd uf, ef; - // prefilter.reset(); - if(prefilter) - { - uf = lsim_y(prefilter.value(), u); - ef = lsim_y(prefilter.value()*(ref_model.inv()-1.0), vel); - } - else - { - uf = u; - ef = lsim_y((ref_model.inv()-1.0), vel); - } - ef = y[y.size()-1]*Eigen::VectorXd::Ones(ef.size()) - y; - - // ud = -NCT(u)+ u*sを求める - Differentiator diff(base_param.gpd, Ts); - diff.reset_test(0, ef[0]); - Eigen::VectorXd ed = lsim_y(diff,ef,true); - for(int i = 0; i < y.size(); i++) - { - ed[i] -= nct(ef[i]); - } - - eigen_named_plot("ef", ef); - eigen_named_plot("ed", ed); - eigen_named_plot("y", y); - plt::legend(); - plt::show(false); - - Eigen::MatrixXd phi(ef.size(), control_base.size()); - for(size_t i = 0; i < control_base.size(); ++i) - { - phi.col(i) = lsim_y(control_base[i], ed); - } - - // ActiveSetMethod asmethod; - // ActiveSetMethod::Problem p; - // std::tie(p.Q, p.c) = lsi2qp(phi, -uf); - // // p.A = -Eigen::Matrix2d::Identity(); - // // p.b = Eigen::Vector2d::Zero(); - // // p.constraint_size = 2; - // auto result = asmethod.solve(p, Eigen::Vector2d::Ones()); - // return result.x_opt; - - return leastsq(phi, uf); - - // return vrft_pi(ed, vel, Ts, ref_model, ref_model); -} - -// まずは評価関数に落とし込むところから考える -Eigen::VectorXd vrft_nctf2(const Eigen::VectorXd &u, const Eigen::VectorXd &y, const double Ts, std::function nct, PID::pid_param_t base_param, TransferFunction ref_model, std::optional prefilter) -{ - namespace cr = cpp_robotics; - std::cout << "vrft_nctf2" << std::endl; - // PID::pid_param_t p_param = - // { - // .Ts = base_param.Ts, - // .gpd = base_param.gpd, - // .Kp = 1, - // .Ki = 0, - // .Kd = 0, - // .output_limit = std::nullopt - // }; - // PID::pid_param_t i_param = - // { - // .Ts = base_param.Ts, - // .gpd = base_param.gpd, - // .Kp = 0, - // .Ki = 1, - // .Kd = 0, - // .output_limit = std::nullopt - // }; - // std::vector control_base = { - // NctfController(nct, p_param, Ts), - // NctfController(nct, i_param, Ts) - // }; - - // std::vector control_base = { - // TransferFunction({1.0}, {1.0}, Ts), - // TransferFunction({1.0}, {1.0, 0.0}, Ts) - // }; - - Differentiator vel_diff(base_param.gpd, Ts); - Eigen::VectorXd vel = lsim_y(vel_diff, y); - - Eigen::VectorXd uf, ef; - // prefilter.reset(); - Eigen::VectorXd vref; - - prefilter = cr::TransferFunction({1.0}, {0.001, 1.0}, base_param.Ts); - if(prefilter) - { - uf = lsim_y(prefilter.value(), u); - ef = lsim_y(prefilter.value()*(ref_model.inv()-1.0), y); - vref = lsim_y(prefilter.value()*(ref_model.inv()), y); - } - else - { - uf = u; - ef = lsim_y((ref_model.inv()), y); - } - - plt::figure(); - eigen_named_plot("ef", ef); - eigen_named_plot("vref", vref); - // eigen_named_plot("ed", ed); - // eigen_named_plot("vel", vel); - eigen_named_plot("y", y); - plt::legend(); - plt::show(false); - - // 最小二乗法で解こうとしてるのが間違い？ - // Eigen::MatrixXd phi(vf.size(), control_base.size()); - // for(size_t i = 0; i < control_base.size(); ++i) - // { - // phi.col(i) = lsim_y([&](double e){ return control_base[i].control(e); }, base_param.Ts, vf); - // } - - // return leastsq(phi, uf); - - auto cost_f = [&](Eigen::VectorXd K) - { - PID::pid_param_t nctf_param = - { - .Ts = base_param.Ts, - .gpd = base_param.gpd, - .Kp = K[0], - .Ki = 3, - .Kd = 0, - .output_limit = base_param.output_limit - }; - NctfController nctf(nct, nctf_param, Ts); - Eigen::VectorXd uc = lsim_y([&](double e){ return nctf.control(e); }, base_param.Ts, ef); - std::cout << "uc = " << uc.squaredNorm() << std::endl; - return (uf-uc).squaredNorm(); - }; - auto cost_grad = [&](const Eigen::VectorXd &K) - { - return cr::derivative(cost_f, K); - }; - auto [ret, rx, rcnt] = cr::quasi_newton_method(cost_f, cost_grad, Eigen::VectorXd::Zero(1)); - if(ret) - { - std::cout << "quasi_newton_method : sucsess" << std::endl; - return rx; - } - else - { - std::cout << "quasi_newton_method : failed" << std::endl; - return Eigen::VectorXd::Zero(2); - } -} - -void check_nct(Eigen::VectorXd y, double Ts, std::function nct) -{ - - namespace plt = matplotlibcpp; - Differentiator diff(600, Ts); - diff.reset_test(0,y[0]); - Eigen::VectorXd vel = lsim_y(diff, y,true); - - // Eigen::VectorXd y2(y.size()*2), vel2(vel.size()*2); - // y2 << Eigen::VectorXd(-y.array().reverse()), y; - // vel2 << Eigen::VectorXd(vel.array().reverse()), -vel; - - Eigen::VectorXd nct_out(y.size()); - for(int i = 0; i < y.size(); i++) - { - nct_out[i] = nct(y[i]); - } - - plt::figure(); - eigen_named_plot("sys", y, (vel).eval()); - eigen_named_plot("NCT", y, nct_out); - plt::xlim(-1.5, 1.5); - plt::ylim(-20, 5); - plt::legend(); - plt::show(false); -} - -void vrft_nctf_with_open_loop() -{ - namespace cr = cpp_robotics; - namespace plt = matplotlibcpp; - - const double Ts = 0.001; // サンプリング周期 - const double sim_time = 2.0; - const double exp_sim_time = 0.5; - // 制御対象 - const double omega = 50.0; - const double zeta = 1.0; - const double max_input = 50; - cr::TransferFunction tf = cr::TransferFunction::make_second_order_system(omega, zeta, Ts) * cr::TransferFunction({0.3}, {1,0}, Ts); - - // 4秒分のステップ応答を計算 - Eigen::VectorXd u = max_input * cr::stepspace(exp_sim_time, Ts); - for(int i = 2.0*u.size()/4; i < u.size(); ++i) - { - u[i] = 0; - } - for(int i = u.size()/2; i < u.size(); ++i) - { - const int size = u.size()/2; - // u[i] *= (double)i/(u.size()/2); - // u[i] = -max_input + std::sin((i-u.size()/2) *0.5*M_PI/size)*max_input; - } - auto [t1, y] = cr::lsim(tf, u); - - const double nct_max_velocity = max_input*0.3; - const double nct_slope = 0.6/0.015; - check_nct(Eigen::VectorXd::Constant(y.size(), y[y.size()-1]) - y, Ts, NctfController::make_simple_nct(nct_max_velocity, nct_slope)); - - // 規範モデル - cr::TransferFunction ref_model = cr::TransferFunction({1.0}, {0.01, 1.0}, Ts); - - // VRFTでPIDゲインを求める - cr::PID::pid_param_t nctf_pid_param = - { - .Ts = Ts, - .gpd = 100*2*M_PI, - .Kp = 0, - .Ki = 0, - .Kd = 0, - .output_limit = std::pair(-max_input, max_input) - }; - - auto K = vrft_nctf2(u, y, Ts, NctfController::make_simple_nct(nct_max_velocity, nct_slope), nctf_pid_param, ref_model, ref_model); - // auto K = cr::vrft_pi(u,y,Ts,ref_model,ref_model); - // Eigen::Vector2d K(5.0, 0.0); - std::cout << "PI gain =" << std::endl; - std::cout << K << std::endl; - nctf_pid_param.Kp = K[0]; - nctf_pid_param.Ki = 0.0; - // nctf_pid_param.Kd = K[2]; - - - cr::NctfController nctf(nct_max_velocity, nct_slope, nctf_pid_param, Ts); - // cr::PID pid(nctf_pid_param); - - // cr::PID pid(pid_param); - // // PIDコントローラとtfからなるフィードバック制御系 - cr::SisoFeedbackSystem siso_sys = cr::make_feedback_system(nctf, tf); - - // VRFTでチューニングしたPIDゲインを用いて閉ループのステップ応答をシミュレーション - const double target_pos = 5.0; - Eigen::VectorXd test_y_ref = Eigen::VectorXd(target_pos * cr::stepspace(sim_time, Ts)); - size_t i = 0; - for(; i < 100; i++) - { - test_y_ref[i] *= 0.02; - } - for(; i < 300; i++) - { - test_y_ref[i] *= 0.1; - } - for(; i < 700; i++) - { - test_y_ref[i] *= 0.5; - } - auto [t2, reu, responce] = cr::lsim(siso_sys, test_y_ref); - - check_nct(Eigen::VectorXd::Constant(responce.size(), responce[responce.size()-1])-responce, Ts, NctfController::make_simple_nct(nct_max_velocity, nct_slope)); - - // 開ループのステップ応答のグラフ表示 - plt::figure_size(1020, 500); - plt::subplot(1, 2, 1); - eigen_named_plot("Input", t1, u); - eigen_named_plot("Responce", t1, y); - plt::xlabel("Time [s]"); - plt::title("Open loop responce"); - // plt::ylim(-0.1, 1.5); - plt::legend(); - plt::grid(true); - - // チューニング後の結果のグラフ表示 - plt::subplot(1, 2, 2); - eigen_named_plot("Target", t2, test_y_ref); - eigen_named_plot("PID feedback responce", t2, responce); - plt::xlabel("Time [s]"); - std::stringstream ss; - ss << std::fixed << std::setprecision(3); - // ss << "(Kp = " << K[0] << ", Ki = " << K[1] << ")"; - plt::title("Closed loop responce\n" + ss.str()); - // plt::ylim(-0.1, 1.5); - plt::legend(); - plt::grid(true); - plt::tight_layout(); - plt::show(); - -} - int main() { - // vrft_pid_with_open_loop(); - // vrft_pid_with_closed_loop(); - vrft_nctf_with_open_loop(); + vrft_pid_with_open_loop(); + vrft_pid_with_closed_loop(); return 0; } \ No newline at end of file From 2d9f7ecc5824398206317a8924ee474a9e86be7b Mon Sep 17 00:00:00 2001 From: Kotakku Date: Fri, 5 Jan 2024 15:44:59 +0900 Subject: [PATCH 5/6] =?UTF-8?q?action=E5=88=86=E5=89=B2?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .github/workflows/build.yaml | 26 ++++++++++++++++++++++---- 1 file changed, 22 insertions(+), 4 deletions(-) diff --git a/.github/workflows/build.yaml b/.github/workflows/build.yaml index 3193d2aae..ea2ca8285 100644 --- a/.github/workflows/build.yaml +++ b/.github/workflows/build.yaml @@ -8,12 +8,12 @@ on: pull_request: workflow_dispatch: jobs: - build: + test: runs-on: ubuntu-latest steps: - uses: actions/checkout@v2 - name: Install dependencies - run: sudo apt install libeigen3-dev libgtest-dev libginac-dev + run: sudo apt install libeigen3-dev libgtest-dev - name: Install Python dependencies run: | python -m pip install --upgrade pip @@ -24,8 +24,26 @@ jobs: working-directory: ${{github.workspace}} run: | JOBS=$(($(grep cpu.cores /proc/cpuinfo | sort -u | sed 's/[^0-9]//g') + 1)) - cmake -S . -B build -DBUILD_EXAMPLES=ON -DBUILD_TESTS=ON -DBUILD_BENCHMARKS=ON + cmake -S . -B build -DBUILD_EXAMPLES=OFF -DBUILD_TESTS=ON -DBUILD_BENCHMARKS=OFF cmake --build build -j${JOBS} cd build ctest - + + build_examples: + runs-on: ubuntu-latest + steps: + - uses: actions/checkout@v2 + - name: Install dependencies + run: sudo apt install libeigen3-dev libgtest-dev + - name: Install Python dependencies + run: | + python -m pip install --upgrade pip + pip install pipenv + pip install numpy + pip install matplotlib + - name: Build & Test + working-directory: ${{github.workspace}} + run: | + JOBS=$(($(grep cpu.cores /proc/cpuinfo | sort -u | sed 's/[^0-9]//g') + 1)) + cmake -S . -B build -DBUILD_EXAMPLES=ON -DBUILD_TESTS=OFF -DBUILD_BENCHMARKS=OFF + cmake --build build -j${JOBS} From f924bd542a751def5ea0631730a7f1150e2b2eae Mon Sep 17 00:00:00 2001 From: Kotakku Date: Fri, 5 Jan 2024 15:46:59 +0900 Subject: [PATCH 6/6] =?UTF-8?q?job=E5=90=8D=E5=A4=89=E6=9B=B4?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .github/workflows/build.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/build.yaml b/.github/workflows/build.yaml index ea2ca8285..397766cbd 100644 --- a/.github/workflows/build.yaml +++ b/.github/workflows/build.yaml @@ -29,7 +29,7 @@ jobs: cd build ctest - build_examples: + example: runs-on: ubuntu-latest steps: - uses: actions/checkout@v2