From 9f4a476de891d05e8150a61fac7446b2c37fe56d Mon Sep 17 00:00:00 2001 From: oscardegroot Date: Wed, 11 Dec 2024 15:12:13 +0100 Subject: [PATCH] Connect scenario_module to mpc_planner, tested on jackalsimulator and rosnavigation. --- .../config/settings.yaml | 2 +- .../launch/ros1_jackalsimulator.launch | 1 + mpc_planner_jackalsimulator/rviz/ros1.rviz | 82 ++++++++++-------- .../generate_jackalsimulator_solver.py | 11 ++- .../scenario_constraints.h | 11 ++- .../scripts/scenario_constraints.py | 4 +- .../src/scenario_constraints.cpp | 60 +++++++------ .../launch/ros1_rosnavigation.launch | 1 + mpc_planner_rosnavigation/rviz/ros1_3d.rviz | 30 ++++--- .../scripts/generate_rosnavigation_solver.py | 9 +- .../acados_solver_interface.h | 7 ++ .../forces_solver_interface.h | 8 ++ .../src/acados_solver_interface.cpp | 86 +++++++++++-------- .../src/forces_solver_interface.cpp | 17 ++++ solver_generator/solver_model.py | 4 +- 15 files changed, 208 insertions(+), 125 deletions(-) diff --git a/mpc_planner_jackalsimulator/config/settings.yaml b/mpc_planner_jackalsimulator/config/settings.yaml index 5c3ea52..e6c63e9 100644 --- a/mpc_planner_jackalsimulator/config/settings.yaml +++ b/mpc_planner_jackalsimulator/config/settings.yaml @@ -43,7 +43,7 @@ linearized_constraints: add_halfspaces: 0 # Add static constraints in T-MPC (e.g., for road boundaries) scenario_constraints: - parallel_solvers: 1 + parallel_solvers: 4 road: two_way: false # Does road go two ways? diff --git a/mpc_planner_jackalsimulator/launch/ros1_jackalsimulator.launch b/mpc_planner_jackalsimulator/launch/ros1_jackalsimulator.launch index 5cec3cc..0b3b480 100644 --- a/mpc_planner_jackalsimulator/launch/ros1_jackalsimulator.launch +++ b/mpc_planner_jackalsimulator/launch/ros1_jackalsimulator.launch @@ -12,6 +12,7 @@ + diff --git a/mpc_planner_jackalsimulator/rviz/ros1.rviz b/mpc_planner_jackalsimulator/rviz/ros1.rviz index 0f50fc4..6435cb3 100644 --- a/mpc_planner_jackalsimulator/rviz/ros1.rviz +++ b/mpc_planner_jackalsimulator/rviz/ros1.rviz @@ -6,13 +6,11 @@ Panels: Expanded: - /Status1 - /Planner1 - - /Planner1/T-MPC1 - - /Planner1/SH-MPC1 - /Lab1 - /Lab1/Robot1/Shape1 - /Odometry1/Shape1 Splitter Ratio: 0.6411764621734619 - Tree Height: 725 + Tree Height: 752 - Class: rviz/Selection Name: Selection - Class: rviz/Tool Properties @@ -198,16 +196,24 @@ Visualization Manager: {} Queue Size: 100 Value: false + - Class: rviz/MarkerArray + Enabled: false + Marker Topic: /jackalsimulator_planner/contouring/road_constraints + Name: Road Constraints + Namespaces: + {} + Queue Size: 100 + Value: false - Class: rviz/Group Displays: - Class: rviz/MarkerArray - Enabled: true + Enabled: false Marker Topic: /jackalsimulator_planner/guidance_planner/start_and_goals Name: Start and Goals Namespaces: {} Queue Size: 100 - Value: true + Value: false - Class: rviz/MarkerArray Enabled: false Marker Topic: /jackalsimulator_planner/guidance_planner/graph @@ -253,7 +259,7 @@ Visualization Manager: Marker Topic: /jackalsimulator_planner/guidance_constraints/optimized_trajectories Name: Optimized Trajectories Namespaces: - {} + "": true Queue Size: 100 Value: true - Class: rviz/MarkerArray @@ -266,32 +272,48 @@ Visualization Manager: Value: false Enabled: true Name: T-MPC - - Class: rviz/MarkerArray - Enabled: false - Marker Topic: /jackalsimulator_planner/contouring/road_constraints - Name: Road Constraints - Namespaces: - {} - Queue Size: 100 - Value: false - Class: rviz/Group Displays: - Class: rviz/MarkerArray - Enabled: true + Enabled: false Marker Topic: /jackalsimulator_planner/scenario_constraints/optimized_trajectories Name: Optimized Scenario Trajectories Namespaces: {} Queue Size: 100 + Value: false + - Class: rviz/MarkerArray + Enabled: true + Marker Topic: /jackalsimulator_planner/scenario_constraints/support + Name: Support + Namespaces: + {} + Queue Size: 100 Value: true - Class: rviz/MarkerArray Enabled: true - Marker Topic: /jackalsimulator_planner/visuals_0 - Name: Safe Horizon + Marker Topic: /jackalsimulator_planner/scenario_constraints/polygons + Name: Polygons Namespaces: {} Queue Size: 100 Value: true + - Class: rviz/MarkerArray + Enabled: false + Marker Topic: /jackalsimulator_planner/scenario_constraints/scenarios + Name: All Scenarios + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: rviz/MarkerArray + Enabled: false + Marker Topic: /jackalsimulator_planner/scenario_constraints/polygon_scenarios + Name: Polygon Scenarios + Namespaces: + {} + Queue Size: 100 + Value: false Enabled: true Name: SH-MPC Enabled: true @@ -378,14 +400,6 @@ Visualization Manager: Value: true Enabled: false Name: Lab - - Class: rviz/MarkerArray - Enabled: true - Marker Topic: /jackalsimulator_planner/visuals_0 - Name: MarkerArray - Namespaces: - {} - Queue Size: 100 - Value: true - Class: rviz/MarkerArray Enabled: true Marker Topic: /jackalsimulator_planner/contouring/path @@ -480,7 +494,7 @@ Visualization Manager: Value: true Views: Current: - Angle: 0.004999999888241291 + Angle: 0 Class: rviz/TopDownOrtho Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 @@ -490,18 +504,18 @@ Visualization Manager: Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Scale: 32.99473190307617 + Scale: 28.81907844543457 Target Frame: - X: 15.15782356262207 - Y: 11.719367980957031 + X: 10.229796409606934 + Y: 13.108591079711914 Saved: ~ Window Geometry: Displays: collapsed: true - Height: 1016 + Height: 1043 Hide Left Dock: true Hide Right Dock: true - QMainWindow State: 000000ff00000000fd0000000400000000000002360000035efc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073000000003b0000035e000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c000002610000000100000112000004ddfc0200000003fb0000000a00560069006500770073000000003d000004dd000000a000fffffffb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000001200530065006c0065006300740069006f006e010000025a000000b2000000000000000000000002000006b8000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007380000003efc0100000002fb0000000800540069006d00650100000000000007380000030700fffffffb0000000800540069006d00650100000000000004500000000000000000000007380000035e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd00000004000000000000026f00000379fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073000000003b00000379000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000011200000379fc0200000003fb0000000a00560069006500770073000000003b00000379000000a000fffffffb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000001200530065006c0065006300740069006f006e010000025a000000b2000000000000000000000002000006b8000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000003efc0100000002fb0000000800540069006d00650100000000000007800000030700fffffffb0000000800540069006d00650100000000000004500000000000000000000007800000037900000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: @@ -510,6 +524,6 @@ Window Geometry: collapsed: false Views: collapsed: true - Width: 1848 - X: 72 - Y: 27 + Width: 1920 + X: 0 + Y: 0 diff --git a/mpc_planner_jackalsimulator/scripts/generate_jackalsimulator_solver.py b/mpc_planner_jackalsimulator/scripts/generate_jackalsimulator_solver.py index e119312..905e862 100644 --- a/mpc_planner_jackalsimulator/scripts/generate_jackalsimulator_solver.py +++ b/mpc_planner_jackalsimulator/scripts/generate_jackalsimulator_solver.py @@ -78,9 +78,14 @@ def configuration_safe_horizon(settings): base_module.weigh_variable(var_name="w", weight_names="angular_velocity") base_module.weigh_variable(var_name="slack", weight_names="slack", rqt_max_value=10000.0) - # modules.add_module(GoalModule(settings)) # Track a goal - modules.add_module(ContouringModule(settings)) - modules.add_module(PathReferenceVelocityModule(settings)) + if not settings["contouring"]["dynamic_velocity_reference"]: + base_module.weigh_variable(var_name="v", + weight_names=["velocity", "reference_velocity"], + cost_function=lambda x, w: w[0] * (x-w[1])**2) # w_v * ||v - v_ref||_2^2 + + modules.add_module(ContouringModule(settings)) # Contouring costs + if settings["contouring"]["dynamic_velocity_reference"]: + modules.add_module(PathReferenceVelocityModule(settings)) # Possibly adaptive v_ref modules.add_module(ScenarioConstraintModule(settings)) return model, modules diff --git a/mpc_planner_modules/include/mpc_planner_modules/scenario_constraints.h b/mpc_planner_modules/include/mpc_planner_modules/scenario_constraints.h index cfbd3e8..d0a993e 100644 --- a/mpc_planner_modules/include/mpc_planner_modules/scenario_constraints.h +++ b/mpc_planner_modules/include/mpc_planner_modules/scenario_constraints.h @@ -3,9 +3,7 @@ #include -#include - -#include +#include namespace MPCPlanner { @@ -22,12 +20,13 @@ namespace MPCPlanner void onDataReceived(RealTimeData &data, std::string &&data_name) override; bool isDataReady(const RealTimeData &data, std::string &missing_data) override; - int optimize(State &state, const RealTimeData &data, ModuleData &module_data) override; // Default: no custom optimization + // Optimizes multiple trajectories in parallel + int optimize(State &state, const RealTimeData &data, ModuleData &module_data) override; void visualize(const RealTimeData &data, const ModuleData &module_data) override; private: - // std::unique_ptr _scenario_module; + double _planning_time; /** @brief a struct to collect parallel scenario optimizations */ struct ScenarioSolver @@ -44,7 +43,7 @@ namespace MPCPlanner }; std::vector> _scenario_solvers; - // ScenarioSolveStatus solve_status_; + ScenarioSolver *_best_solver; int sequentialScenarioIterations(); }; diff --git a/mpc_planner_modules/scripts/scenario_constraints.py b/mpc_planner_modules/scripts/scenario_constraints.py index c157e12..c226da8 100644 --- a/mpc_planner_modules/scripts/scenario_constraints.py +++ b/mpc_planner_modules/scripts/scenario_constraints.py @@ -20,7 +20,7 @@ def __init__(self, settings): super().__init__() self.module_name = "ScenarioConstraints" # Needs to correspond to the c++ name of the module self.import_name = "scenario_constraints.h" - self.dependencies.append("lmpcc_scenario_module") + self.dependencies.append("scenario_module") self.description = "Avoid dynamic obstacles under motion uncertainty using scenario optimization." self.constraints.append(LinearConstraints(n_discs=settings["n_discs"], n_constraints=24, use_slack=True)) @@ -39,7 +39,7 @@ def __init__(self, n_discs, n_constraints, use_slack): def define_parameters(self, params): for disc_id in range(self.n_discs): - params.add(f"ego_disc_{disc_id}_offset") + params.add(f"ego_disc_{disc_id}_offset", bundle_name="ego_disc_offset") for index in range(self.n_constraints): params.add(self.constraint_name(index, disc_id) + "_a1") diff --git a/mpc_planner_modules/src/scenario_constraints.cpp b/mpc_planner_modules/src/scenario_constraints.cpp index 8341d6a..3810dc6 100644 --- a/mpc_planner_modules/src/scenario_constraints.cpp +++ b/mpc_planner_modules/src/scenario_constraints.cpp @@ -5,6 +5,7 @@ #include #include +#include #include @@ -17,7 +18,6 @@ namespace MPCPlanner { solver = std::make_shared(id); scenario_module.initialize(solver); - // std::make_unique(solver); } ScenarioConstraints::ScenarioConstraints(std::shared_ptr solver) @@ -25,6 +25,8 @@ namespace MPCPlanner { LOG_INITIALIZE("Scenario Constraints"); + _planning_time = 1. / CONFIG["control_frequency"].as(); + _SCENARIO_CONFIG.Init(); for (int i = 0; i < CONFIG["scenario_constraints"]["parallel_solvers"].as(); i++) { @@ -40,7 +42,7 @@ namespace MPCPlanner #pragma omp parallel for num_threads(4) for (auto &solver : _scenario_solvers) { - *solver->solver = *_solver; // Copy the main solver + *solver->solver = *_solver; // Copy the main solver, including its initial guess solver->scenario_module.update(data, module_data); } @@ -51,10 +53,6 @@ namespace MPCPlanner (void)module_data; (void)data; (void)k; - // for (auto &solver : _scenario_solvers) - // { - // solver->scenario_module.setParameters(data, k); - // } } int ScenarioConstraints::optimize(State &state, const RealTimeData &data, ModuleData &module_data) @@ -62,15 +60,16 @@ namespace MPCPlanner (void)state; (void)module_data; - // if (!config_->use_trajectory_sampling_) // To test regular optimization with slack - // return SimpleSequentialScenarioIterations(solver_interface); // S-MPCC (SQP) + omp_set_nested(1); + omp_set_max_active_levels(2); + omp_set_dynamic(0); #pragma omp parallel for num_threads(4) for (auto &solver : _scenario_solvers) { - - // auto &solver = _scenario_solvers[s]; - solver->solver->_params.solver_timeout = 1. / CONFIG["control_frequency"].as(); + // Set the planning timeout + std::chrono::duration used_time = std::chrono::system_clock::now() - data.planning_start_time; + solver->solver->_params.solver_timeout = _planning_time - used_time.count() - 0.008; // Copy solver parameters and initial guess *solver->solver = *_solver; // Copy the main solver @@ -81,33 +80,31 @@ namespace MPCPlanner solver->scenario_module.setParameters(data, k); } - // int exit_code = _solver->solve(); - // int exit_code = _scenario_module->optimize(data); // Safe Horizon MPC - solver->solver->loadWarmstart(); + solver->solver->loadWarmstart(); // Load the previous solution solver->exit_code = solver->scenario_module.optimize(data); // Safe Horizon MPC } + omp_set_dynamic(1); + // Retrieve the lowest cost solution double lowest_cost = 1e9; - ScenarioSolver *best_solver = nullptr; + _best_solver = nullptr; for (auto &solver : _scenario_solvers) { if (solver->exit_code == 1 && solver->solver->_info.pobj < lowest_cost) { lowest_cost = solver->solver->_info.pobj; - best_solver = solver.get(); + _best_solver = solver.get(); } } - if (best_solver == nullptr) // No feasible solution + if (_best_solver == nullptr) // No feasible solution return _scenario_solvers.front()->exit_code; - // auto &best_solver = *_scenario_solvers.front().solver; - _solver->_output = best_solver->solver->_output; // Load the solution into the main lmpcc solver - _solver->_info = best_solver->solver->_info; - _solver->_params = best_solver->solver->_params; - _solver->copySolverMemory(*(best_solver->solver)); /** @todo: Verify whether this is necessary */ + _solver->_output = _best_solver->solver->_output; // Load the solution into the main lmpcc solver + _solver->_info = _best_solver->solver->_info; + _solver->_params = _best_solver->solver->_params; - return best_solver->exit_code; + return _best_solver->exit_code; } void ScenarioConstraints::onDataReceived(RealTimeData &data, std::string &&data_name) @@ -119,7 +116,7 @@ namespace MPCPlanner if (_SCENARIO_CONFIG.enable_safe_horizon_) { #pragma omp parallel for num_threads(4) - for (auto &solver : _scenario_solvers) + for (auto &solver : _scenario_solvers) // Draw different samples for all solvers { solver->scenario_module.GetSampler().IntegrateAndTranslateToMeanAndVariance(data.dynamic_obstacles, _solver->dt); } @@ -132,7 +129,6 @@ namespace MPCPlanner if (data.dynamic_obstacles.size() != CONFIG["max_obstacles"].as()) { - missing_data += "Obstacles "; return false; } @@ -161,14 +157,22 @@ namespace MPCPlanner void ScenarioConstraints::visualize(const RealTimeData &data, const ModuleData &module_data) { (void)module_data; + bool visualize_all = false; LOG_MARK("ScenarioConstraints::visualize"); - for (auto &solver : _scenario_solvers) - solver->scenario_module.visualize(data); + if (visualize_all) + { + for (auto &solver : _scenario_solvers) + solver->scenario_module.visualize(data); + } + else if (_best_solver != nullptr) + { + + _best_solver->scenario_module.visualize(data); + } // Visualize optimized trajectories - // Visualize the optimized trajectory for (auto &solver : _scenario_solvers) { if (solver->exit_code == 1) diff --git a/mpc_planner_rosnavigation/launch/ros1_rosnavigation.launch b/mpc_planner_rosnavigation/launch/ros1_rosnavigation.launch index 6fda889..69267a0 100644 --- a/mpc_planner_rosnavigation/launch/ros1_rosnavigation.launch +++ b/mpc_planner_rosnavigation/launch/ros1_rosnavigation.launch @@ -10,6 +10,7 @@ + diff --git a/mpc_planner_rosnavigation/rviz/ros1_3d.rviz b/mpc_planner_rosnavigation/rviz/ros1_3d.rviz index a343ea8..55272a8 100644 --- a/mpc_planner_rosnavigation/rviz/ros1_3d.rviz +++ b/mpc_planner_rosnavigation/rviz/ros1_3d.rviz @@ -8,11 +8,12 @@ Panels: - /Planner1 - /Planner1/T-MPC1 - /Planner1/SH-MPC1 + - /Planner1/SH-MPC1/Optimized Scenario Trajectories1 - /Lab1 - /Lab1/Robot1/Shape1 - /Odometry1/Shape1 - Splitter Ratio: 0.6411764621734619 - Tree Height: 746 + Splitter Ratio: 0.772009015083313 + Tree Height: 752 - Class: rviz/Selection Name: Selection - Class: rviz/Tool Properties @@ -28,7 +29,6 @@ Panels: Name: Views Splitter Ratio: 0.5 - Class: rviz/Time - Experimental: false Name: Time SyncMode: 0 SyncSource: "" @@ -195,7 +195,7 @@ Visualization Manager: Value: false - Class: rviz/MarkerArray Enabled: false - Marker Topic: /rosnavigation_planner/obstacle_predictions + Marker Topic: /move_base/obstacle_predictions Name: Planner Obstacle Predictions Namespaces: {} @@ -256,7 +256,7 @@ Visualization Manager: Marker Topic: /move_base/guidance_constraints/optimized_trajectories Name: Optimized Trajectories Namespaces: - "": true + {} Queue Size: 100 Value: true - Class: rviz/MarkerArray @@ -281,18 +281,26 @@ Visualization Manager: Displays: - Class: rviz/MarkerArray Enabled: true - Marker Topic: /rosnavigation_planner/scenario_constraints/optimized_trajectories + Marker Topic: /move_base/scenario_constraints/optimized_trajectories Name: Optimized Scenario Trajectories Namespaces: - {} + "": true Queue Size: 100 Value: true - Class: rviz/MarkerArray Enabled: true - Marker Topic: /rosnavigation_planner/visuals_0 - Name: Safe Horizon + Marker Topic: /move_base/scenario_constraints/support + Name: Support Namespaces: - {} + "": true + Queue Size: 100 + Value: true + - Class: rviz/MarkerArray + Enabled: true + Marker Topic: /move_base/scenario_constraints/polygons + Name: Polygons + Namespaces: + "": true Queue Size: 100 Value: true Enabled: true @@ -530,7 +538,7 @@ Window Geometry: Height: 1043 Hide Left Dock: true Hide Right Dock: true - QMainWindow State: 000000ff00000000fd00000004000000000000023600000375fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073000000003d00000375000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000011200000375fc0200000003fb0000000a00560069006500770073000000003d00000375000000a400fffffffb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000001200530065006c0065006300740069006f006e010000025a000000b2000000000000000000000002000006b8000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000003efc0100000002fb0000000800540069006d0065010000000000000780000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000007800000037500000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd0000000400000000000001bd00000379fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073000000003b00000379000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000011200000375fc0200000003fb0000000a00560069006500770073000000003d00000375000000a000fffffffb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000001200530065006c0065006300740069006f006e010000025a000000b2000000000000000000000002000006b8000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000003efc0100000002fb0000000800540069006d00650100000000000007800000030700fffffffb0000000800540069006d00650100000000000004500000000000000000000007800000037900000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: diff --git a/mpc_planner_rosnavigation/scripts/generate_rosnavigation_solver.py b/mpc_planner_rosnavigation/scripts/generate_rosnavigation_solver.py index cb5126d..917c629 100644 --- a/mpc_planner_rosnavigation/scripts/generate_rosnavigation_solver.py +++ b/mpc_planner_rosnavigation/scripts/generate_rosnavigation_solver.py @@ -74,10 +74,17 @@ def configuration_safe_horizon(settings): base_module.weigh_variable(var_name="slack", weight_names="slack", rqt_max_value=10000.0) # modules.add_module(GoalModule(settings)) # Track a goal + if not settings["contouring"]["dynamic_velocity_reference"]: + base_module.weigh_variable(var_name="v", + weight_names=["velocity", "reference_velocity"], + cost_function=lambda x, w: w[0] * (x-w[1])**2) + modules.add_module(ContouringModule(settings)) - modules.add_module(PathReferenceVelocityModule(settings)) + if settings["contouring"]["dynamic_velocity_reference"]: + modules.add_module(PathReferenceVelocityModule(settings)) modules.add_module(ScenarioConstraintModule(settings)) + modules.add_module(DecompConstraintModule(settings)) return model, modules diff --git a/mpc_planner_solver/include/mpc_planner_solver/acados_solver_interface.h b/mpc_planner_solver/include/mpc_planner_solver/acados_solver_interface.h index ae26847..57c007b 100644 --- a/mpc_planner_solver/include/mpc_planner_solver/acados_solver_interface.h +++ b/mpc_planner_solver/include/mpc_planner_solver/acados_solver_interface.h @@ -156,6 +156,8 @@ namespace MPCPlanner ocp_nlp_solver *_nlp_solver; void *_nlp_opts; + int _exit_code_one_iter{-1}; + public: int _solver_id; @@ -185,6 +187,11 @@ namespace MPCPlanner int solve(); + // One iteration a time interface + void initializeOneIteration(); + int solveOneIteration(); + int completeOneIteration(); + // PARAMETERS // bool hasParameter(std::string &¶meter); void setParameter(int k, std::string &¶meter, double value); diff --git a/mpc_planner_solver/include/mpc_planner_solver/forces_solver_interface.h b/mpc_planner_solver/include/mpc_planner_solver/forces_solver_interface.h index adef32c..bf0e6fc 100644 --- a/mpc_planner_solver/include/mpc_planner_solver/forces_solver_interface.h +++ b/mpc_planner_solver/include/mpc_planner_solver/forces_solver_interface.h @@ -55,6 +55,8 @@ namespace MPCPlanner YAML::Node _config, _parameter_map, _model_map; + int _exit_code_one_iter{-1}; + Solver(int solver_id = 0); void reset(); ~Solver(); @@ -88,6 +90,12 @@ namespace MPCPlanner /** @brief Solve the optimization */ int solve(); + + // One iteration a time interface + void initializeOneIteration(); + int solveOneIteration(); + int completeOneIteration(); + double getOutput(int k, std::string &&state_name) const; // Debugging utilities diff --git a/mpc_planner_solver/src/acados_solver_interface.cpp b/mpc_planner_solver/src/acados_solver_interface.cpp index 40f7b5a..c2cc375 100644 --- a/mpc_planner_solver/src/acados_solver_interface.cpp +++ b/mpc_planner_solver/src/acados_solver_interface.cpp @@ -93,6 +93,33 @@ namespace MPCPlanner // _params.printParameters(_parameter_map); + initializeOneIteration(); + double iteration_time = 0.; + + for (int iteration = 0; iteration < _num_iterations; iteration++) + { + iteration_timer.start(); + + solveOneIteration(); + + if (status != ACADOS_SUCCESS && _info.qp_status != 0) + break; + + iteration_time += iteration_timer.stop(); + double avg_iteration_time = iteration_time / ((double)(iteration + 1)); + + // Stop iterating if we ran out of time + if (timeout_timer.currentDuration() + avg_iteration_time >= _params.solver_timeout) + { + LOG_WARN_THROTTLE(15000., "Timeout is enabled. Stopping after " << iteration + 1 << " iterations because planning time is exceeded"); + break; + } + } + return completeOneIteration(); + } + + void Solver::initializeOneIteration() + { // Set initial state ocp_nlp_constraints_model_set(_nlp_config, _nlp_dims, _nlp_in, 0, "lbx", _params.xinit); ocp_nlp_constraints_model_set(_nlp_config, _nlp_dims, _nlp_in, 0, "ubx", _params.xinit); @@ -112,37 +139,28 @@ namespace MPCPlanner int rti_phase = 0; // 1 = prep, 2 = feedback, 0 = both ocp_nlp_solver_opts_set(_nlp_config, _nlp_opts, "rti_phase", &rti_phase); - // ocp_nlp_solver_opts_update(_nlp_config, _nlp_dims, _nlp_opts); - ocp_nlp_precompute(_nlp_solver, _nlp_in, _nlp_out); - double iteration_time = 0.; - - for (int iteration = 0; iteration < _num_iterations; iteration++) - { - iteration_timer.start(); + } - status = Solver_acados_solve(_acados_ocp_capsule); + int Solver::solveOneIteration() + { + int status = -1; - ocp_nlp_get(_nlp_solver, "time_tot", &_info.elapsed_time); - _info.solvetime += _info.elapsed_time; - _info.min_time = MIN(_info.elapsed_time, _info.min_time); + status = Solver_acados_solve(_acados_ocp_capsule); - ocp_nlp_get(_nlp_solver, "qp_status", &_info.qp_status); + ocp_nlp_get(_nlp_solver, "time_tot", &_info.elapsed_time); + _info.solvetime += _info.elapsed_time; + _info.min_time = MIN(_info.elapsed_time, _info.min_time); - if (status != ACADOS_SUCCESS && _info.qp_status != 0) - break; + ocp_nlp_get(_nlp_solver, "qp_status", &_info.qp_status); - iteration_time += iteration_timer.stop(); - double avg_iteration_time = iteration_time / ((double)(iteration + 1)); + _exit_code_one_iter = status; - // Stop iterating if we ran out of time - if (timeout_timer.currentDuration() + avg_iteration_time >= _params.solver_timeout) - { - LOG_WARN_THROTTLE(15000., "Timeout is enabled. Stopping iterations when planning time is exceeded"); - break; - } - } + return status; + } + int Solver::completeOneIteration() + { ocp_nlp_get(_nlp_solver, "nlp_res", &_info.nlp_res); // Compute and retrieve the cost @@ -157,14 +175,12 @@ namespace MPCPlanner double res_stat, res_eq, res_ineq, res_comp; ocp_nlp_get(_nlp_solver, "res_eq", &res_eq); - if (res_eq > 1e-2 && status == ACADOS_SUCCESS) + if (res_eq > 1e-2 && _exit_code_one_iter == ACADOS_SUCCESS) { - // LOG_WARN("Acados gave back success, but the solution is infeasible"); - status = ACADOS_QP_FAILURE; + _exit_code_one_iter = ACADOS_QP_FAILURE; } - // _output.print(); - if (status == ACADOS_SUCCESS) + if (_exit_code_one_iter == ACADOS_SUCCESS) { LOG_MARK("Solver_acados_solve(): SUCCESS!"); } @@ -172,23 +188,19 @@ namespace MPCPlanner { Solver_acados_reset(_acados_ocp_capsule, 1); ocp_nlp_solver_reset_qp_memory(_nlp_solver, _nlp_in, _nlp_out); - - // LOG_WARN(explainExitFlag(status)); } // Get INFO ocp_nlp_out_get(_nlp_config, _nlp_dims, _nlp_out, 0, "kkt_norm_inf", &_info.kkt_norm_inf); ocp_nlp_get(_nlp_solver, "sqp_iter", &_info.sqp_iter); - // _info.print(_acados_ocp_capsule); - // Map to FORCES output - if (status == ACADOS_SUCCESS) // || status == 2) // Success - status = 1; - else if (status == 1) - status = 0; + if (_exit_code_one_iter == ACADOS_SUCCESS) // Success + _exit_code_one_iter = 1; + else if (_exit_code_one_iter == 1) + _exit_code_one_iter = 0; - return status; + return _exit_code_one_iter; } // PARAMETERS // diff --git a/mpc_planner_solver/src/forces_solver_interface.cpp b/mpc_planner_solver/src/forces_solver_interface.cpp index 60850ae..e3cb83b 100644 --- a/mpc_planner_solver/src/forces_solver_interface.cpp +++ b/mpc_planner_solver/src/forces_solver_interface.cpp @@ -221,6 +221,23 @@ namespace MPCPlanner return exit_code; } + void Solver::initializeOneIteration() + { + setReinitialize(true); + } + + int Solver::solveOneIteration() + { + _exit_code_one_iter = solve(); + setReinitialize(false); // Disable reinitialization after each iteration + return _exit_code_one_iter; + } + + int Solver::completeOneIteration() + { + return _exit_code_one_iter; + } + double Solver::getOutput(int k, std::string &&state_name) const { return getForcesOutput(_output, k, _model_map[state_name][1].as()); diff --git a/solver_generator/solver_model.py b/solver_generator/solver_model.py index e28f17f..bfd5982 100644 --- a/solver_generator/solver_model.py +++ b/solver_generator/solver_model.py @@ -281,8 +281,8 @@ def __init__(self): self.states = ["x", "y", "psi", "v", "spline", "slack"] self.inputs = ["a", "w"] - self.lower_bound = [-4.0, -2.0, -2000.0, -2000.0, -np.pi * 4, -3.0, -1.0, 0.0] # v -0.01 - self.upper_bound = [4.0, 2.0, 2000.0, 2000.0, np.pi * 4, 3.0, 10000.0, 5000.0] # w 0.8 + self.lower_bound = [-2.0, -0.8, -2000.0, -2000.0, -np.pi * 4, -0.01, -1.0, 0.0] # v -0.01 + self.upper_bound = [2.0, 0.8, 2000.0, 2000.0, np.pi * 4, 3.0, 10000.0, 5000.0] # w 0.8 def continuous_model(self, x, u):