diff --git a/.github/workflows/main.yml b/.github/workflows/main.yml index 636a94b..7968f88 100644 --- a/.github/workflows/main.yml +++ b/.github/workflows/main.yml @@ -36,6 +36,7 @@ jobs: - name: Install Poetry uses: snok/install-poetry@v1 with: + version: 1.5.1 virtualenvs-create: true virtualenvs-in-project: true installer-parallel: true diff --git a/README.md b/README.md index 8368c09..2399b96 100644 --- a/README.md +++ b/README.md @@ -16,6 +16,12 @@ This package implements Model Predictive Control (MPC) for motion planning in 2D This repository includes our implementation of **Topology-Driven MPC** (**T-MPC++**) that computes multiple distinct trajectories in parallel, each passing dynamic obstacles differently. For a brief overview of the method, see the [Paper website](https://autonomousrobots.nl/paper_websites/topology-driven-mpc). +--- +>**Update:** This repository now also contains our code for Safe Horizon MPC (SH-MPC) associated with the following publication: + +**Journal Paper:** O. de Groot, L. Ferranti, D. M. Gavrila, and J. Alonso-Mora, *Scenario-Based Trajectory Optimization with Bounded Probability of Collision.* **International Journal of Robotics Research (IJRR)**, 2024. Preprint: https://arxiv.org/pdf/2307.01070 + +SH-MPC can handle non Gaussian uncertainty in the motion predictions of obstacles. For more details on how to use it in `mpc_planner`, see https://github.com/oscardegroot/scenario_module. Simulated Mobile Robot | Real-World Mobile Robot | Static and Dynamic Obstacles | | -------------- | -------------- | -------------- | @@ -59,7 +65,7 @@ Implemented MPC **modules** include: - Linearized constraints - **Chance Constrained Obstacle Avoidance Constraints** - Incorporating uncertainty in the future motion of humans - Avoiding obstacle predictions modeled as Gaussians ([CC-MPC](https://ieeexplore.ieee.org/abstract/document/8613928)) - - Avoiding obstacle predictions modeled as Gaussian Mixtures ([Safe Horizon MPC](https://arxiv.org/pdf/2307.01070) *publication pending*) + - Avoiding obstacle predictions modeled as Gaussian Mixtures ([Safe Horizon MPC](https://arxiv.org/pdf/2307.01070)) - **Static Obstacle Avoidance Constraints** - Avoiding non-moving obstacles in the environment - Using `decomp_util` (see [original paper](https://ieeexplore.ieee.org/abstract/document/7839930) and [modified implementation](https://arxiv.org/pdf/2406.11506)) @@ -67,6 +73,8 @@ These functionalities can be stacked to implement the desired behavior (see [Con The [**Topology-Driven MPC**](https://arxiv.org/pdf/2401.06021) [1] module parallelizes the above functionality over multiple distinct initial guesses, computing several trajectories that pass the obstacles differently. +For more implementation details on [**SH-MPC**](https://arxiv.org/pdf/2307.01070) [2], see https://github.com/oscardegroot/scenario_module. + ## Installation We recommend to use the complete VSCode containerized environment provided here https://github.com/tud-amr/mpc_planner_ws, if you can, to automatically install the planner and its requirements. @@ -255,7 +263,7 @@ You should see output indicating that the solver is being generated. ### Step 5: Build the planner -Source the ROS workspace `source devel/setup.sh`. Then build with: +Source your ROS distribution (e.g., `source /opt/ros/noetic/setup.bash`). Then build with: ```bash catkin config --cmake-args -DCMAKE_BUILD_TYPE=Release # Release, Debug, RelWithDebInfo, MinSizeRel @@ -353,6 +361,9 @@ If you found this repository useful, please cite our paper! - [1] **Journal Paper:** O. de Groot, L. Ferranti, D. M. Gavrila, and J. Alonso-Mora, *Topology-Driven Parallel Trajectory Optimization in Dynamic Environments.* **IEEE Transactions on Robotics (T-RO)** 2024. Available: https://doi.org/10.1109/TRO.2024.3475047 +If you use SH-MPC, please cite: + +- [2] **Journal Paper:** O. de Groot, L. Ferranti, D. M. Gavrila, and J. Alonso-Mora, *Scenario-Based Trajectory Optimization with Bounded Probability of Collision.* **International Journal of Robotics Research (IJRR)**, 2024. Preprint: https://arxiv.org/pdf/2307.01070 ## Contributing We welcome issues and contributions! If you encounter any bugs, have suggestions for new features, or want to propose improvements, feel free to open an issue or pull request. diff --git a/mpc_planner_jackal/README.md b/mpc_planner_jackal/README.md index 56f8b6c..436b301 100644 --- a/mpc_planner_jackal/README.md +++ b/mpc_planner_jackal/README.md @@ -9,6 +9,12 @@ Several additional packages are required to run the planner in the lab. These ar --- +### Demo Instructions + +1. Connect to `mrl-wifi-5g`. +2. `ip a`, set `ROS_IP` in `connect_to_jackal.sh`. +3. `Ctrl + Shift + B` -> `Jackal: Run Real-World Jackal` + ### Notes - The launch file configures the number of pedestrians, which needs to be correct for any pedestrian to be detected. - The launch file also allows static obstacles with markers to be added (as dynamic obstacle with zero velocity). diff --git a/mpc_planner_jackal/config/guidance_planner.yaml b/mpc_planner_jackal/config/guidance_planner.yaml index 499cbb7..db72f3e 100644 --- a/mpc_planner_jackal/config/guidance_planner.yaml +++ b/mpc_planner_jackal/config/guidance_planner.yaml @@ -9,10 +9,10 @@ guidance_planner: homotopy: n_paths: 4 # Number of guidance trajectories - comparison_function: Homology # Homology (default) Winding or UVD + comparison_function: Winding # Homology (default) Winding or UVD winding: pass_threshold: 0.87 #1.74 # half of pi - use_non_passing: true + use_non_passing: false use_learning: false track_selected_homology_only: false @@ -27,8 +27,8 @@ guidance_planner: timeout: 10 # Timeout for PRM sampling [ms] margin: 0.0 # [m] sampled outside of goals - max_velocity: 7.0 # Maximum velocity of connections between nodes - max_acceleration: 7.0 # Maximum velocity of connections between nodes + max_velocity: 3.0 # Maximum velocity of connections between nodes + max_acceleration: 3.0 # Maximum velocity of connections between nodes connection_filters: forward: true velocity: true diff --git a/mpc_planner_jackal/config/settings.yaml b/mpc_planner_jackal/config/settings.yaml index a0924c7..98e20f9 100644 --- a/mpc_planner_jackal/config/settings.yaml +++ b/mpc_planner_jackal/config/settings.yaml @@ -4,8 +4,8 @@ integrator_step: 0.2 n_discs: 1 solver_settings: - solver: "acados" # acados or forces - # solver: "forces" # acados or forces + # solver: "acados" # acados or forces + solver: "forces" # acados or forces acados: iterations: 10 solver_type: SQP_RTI # SQP_RTI (default) or SQP @@ -70,16 +70,16 @@ probabilistic: weights: goal_weight: 10. - velocity: 0.3 + velocity: 0.1 acceleration: 0.25 #0.15 - angular_velocity: 0.5 #0.25 + angular_velocity: 0.1 #0.25 reference_velocity: 1.5 #2.0 - contour: 0.05 #0.01 #0.05 + contour: 0.01 #0.01 #0.05 preview: 0.0 lag: 0.1 slack: 10000. - terminal_angle: 100.0 # 0.0 - terminal_contouring: 10.0 # 0.0 + terminal_angle: 0.0 #100.0 # 0.0 + terminal_contouring: 0.0 #10.0 # 0.0 visualization: draw_every: 5 # stages \ No newline at end of file diff --git a/mpc_planner_jackal/include/mpc_planner_jackal/ros1_jackal.h b/mpc_planner_jackal/include/mpc_planner_jackal/ros1_jackal.h index dde7316..eb91f57 100644 --- a/mpc_planner_jackal/include/mpc_planner_jackal/ros1_jackal.h +++ b/mpc_planner_jackal/include/mpc_planner_jackal/ros1_jackal.h @@ -70,8 +70,8 @@ class JackalPlanner double _measured_velocity{0.}; - double y_max{1.6}; // 2.6 when the blocks are not at the wall - double y_min{-1.6}; + double y_max{2.4}; // 2.6 when the blocks are not at the wall + double y_min{-2.0}; double x_max{3.5}; double x_min{-3.5}; diff --git a/mpc_planner_jackal/launch/ros1_jackal.launch b/mpc_planner_jackal/launch/ros1_jackal.launch index dfba98a..3b1b049 100644 --- a/mpc_planner_jackal/launch/ros1_jackal.launch +++ b/mpc_planner_jackal/launch/ros1_jackal.launch @@ -28,11 +28,13 @@ --> - + + + @@ -43,6 +45,7 @@ + diff --git a/mpc_planner_jackal/rviz/ros1.rviz b/mpc_planner_jackal/rviz/ros1.rviz index a05ba62..1c5f367 100644 --- a/mpc_planner_jackal/rviz/ros1.rviz +++ b/mpc_planner_jackal/rviz/ros1.rviz @@ -87,31 +87,16 @@ Visualization Manager: Expand Link Details: false Expand Tree: false Link Tree Style: Links in Alphabetic Order - VLP16: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - VLP16_base_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true base_link: Alpha: 1 Show Axes: false Show Trail: false - baseplate_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true chassis_link: Alpha: 1 Show Axes: false Show Trail: false Value: true - fenders_link: + front_fender_link: Alpha: 1 Show Axes: false Show Trail: false @@ -130,15 +115,6 @@ Visualization Manager: Show Axes: false Show Trail: false Value: true - hokuyo_base_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - hokuyo_base_scan: - Alpha: 1 - Show Axes: false - Show Trail: false imu_link: Alpha: 1 Show Axes: false @@ -152,6 +128,11 @@ Visualization Manager: Show Axes: false Show Trail: false Value: true + rear_fender_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true rear_left_wheel_link: Alpha: 1 Show Axes: false @@ -175,13 +156,13 @@ Visualization Manager: - Class: rviz/Group Displays: - Class: rviz/MarkerArray - Enabled: true + Enabled: false Marker Topic: /jackal_planner/planned_trajectory Name: Planned Trajectory Namespaces: - "": true + {} Queue Size: 100 - Value: true + Value: false - Class: rviz/MarkerArray Enabled: true Marker Topic: /jackal_planner/contouring/path @@ -217,21 +198,21 @@ Visualization Manager: - Class: rviz/Group Displays: - Class: rviz/MarkerArray - Enabled: true + Enabled: false Marker Topic: /jackal_planner/guidance_planner/start_and_goals Name: Start and Goals Namespaces: - "": true + {} Queue Size: 100 - Value: true + Value: false - Class: rviz/MarkerArray - Enabled: true + Enabled: false Marker Topic: /jackal_planner/guidance_planner/graph Name: Graph Namespaces: - "": true + {} Queue Size: 100 - Value: true + Value: false - Class: rviz/MarkerArray Enabled: false Marker Topic: /lmpcc/homotopy/geometric_paths @@ -391,7 +372,7 @@ Visualization Manager: Scale: 1 Value: true Value: false - Enabled: true + Enabled: false Keep: 100 Name: Past Trajectory Position Tolerance: 0.10000000149011612 @@ -408,23 +389,23 @@ Visualization Manager: Value: Arrow Topic: /odometry/filtered Unreliable: false - Value: true + Value: false - Class: rviz/MarkerArray - Enabled: true + Enabled: false Marker Topic: /jackal_planner/robot_area Name: Robot Area Namespaces: - "": true + {} Queue Size: 100 - Value: true + Value: false - Class: rviz/MarkerArray - Enabled: true + Enabled: false Marker Topic: /jackal_planner/goal Name: Goals Namespaces: - "": true + {} Queue Size: 100 - Value: true + Value: false - Class: rviz/MarkerArray Enabled: true Marker Topic: /jackal_planner/goal_module @@ -479,7 +460,7 @@ Visualization Manager: Near Clip Distance: 0.009999999776482582 Pitch: 0.8647974729537964 Target Frame: - Yaw: 3.101998805999756 + Yaw: 3.140000104904175 Saved: ~ Window Geometry: Displays: @@ -487,7 +468,7 @@ Window Geometry: Height: 1016 Hide Left Dock: false Hide Right Dock: true - QMainWindow State: 000000ff00000000fd0000000400000000000001df0000035efc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b0000035e000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000011100000375fc0200000003fb0000000a00560069006500770073000000003d00000375000000a000fffffffb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000001200530065006c0065006300740069006f006e010000025a000000b2000000000000000000000002000006b8000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007380000003efc0100000002fb0000000800540069006d00650100000000000007380000030700fffffffb0000000800540069006d00650100000000000004500000000000000000000005530000035e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd0000000400000000000001df0000035efc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b0000035e000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c0000026100000001000001110000035efc0200000003fb0000000a00560069006500770073000000003b0000035e000000a000fffffffb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000001200530065006c0065006300740069006f006e010000025a000000b2000000000000000000000002000006b8000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007380000003efc0100000002fb0000000800540069006d00650100000000000007380000030700fffffffb0000000800540069006d00650100000000000004500000000000000000000005530000035e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: diff --git a/mpc_planner_jackal/scripts/generate_jackal_solver.py b/mpc_planner_jackal/scripts/generate_jackal_solver.py index 9e728c3..d06f009 100644 --- a/mpc_planner_jackal/scripts/generate_jackal_solver.py +++ b/mpc_planner_jackal/scripts/generate_jackal_solver.py @@ -58,8 +58,14 @@ def configuration_tmpc(settings): base_module.weigh_variable(var_name="a", weight_names="acceleration") base_module.weigh_variable(var_name="w", weight_names="angular_velocity") + 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)) + # modules.add_module(PathReferenceVelocityModule(settings)) # modules.add_module(GuidanceConstraintModule(settings, constraint_submodule=EllipsoidConstraintModule)) modules.add_module(GuidanceConstraintModule(settings, constraint_submodule=GaussianConstraintModule)) @@ -75,9 +81,14 @@ def configuration_lmpcc(settings): base_module = modules.add_module(MPCBaseModule(settings)) base_module.weigh_variable(var_name="a", weight_names="acceleration") base_module.weigh_variable(var_name="w", weight_names="angular_velocity") + 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)) + # modules.add_module(PathReferenceVelocityModule(settings)) modules.add_module(EllipsoidConstraintModule(settings)) @@ -86,6 +97,7 @@ def configuration_lmpcc(settings): settings = load_settings() +# model, modules = configuration_lmpcc(settings) model, modules = configuration_tmpc(settings) generate_solver(modules, model, settings) 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..e8ee993 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..39e29a7 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 @@ -121,9 +126,16 @@ def configuration_lmpcc(settings): # model, modules = configuration_basic(settings) # model, modules = configuration_no_obstacles(settings) -# model, modules = configuration_safe_horizon(settings) + +# NOTE: LMPCC - basic MPC with deterministic obstacle avoidance # model, modules = configuration_lmpcc(settings) + +# NOTE: T-MPC - Parallelized MPC optimizing trajectories with several distinct passing behaviors. model, modules = configuration_tmpc(settings) +# NOTE: SH-MPC - MPC incorporating non Gaussian uncertainty in obstacle motion. +# More configuration parameters in `scenario_module/config/params.yaml` +# model, modules = configuration_safe_horizon(settings) + generate_solver(modules, model, settings) exit(0) diff --git a/mpc_planner_modules/include/mpc_planner_modules/gaussian_constraints.h b/mpc_planner_modules/include/mpc_planner_modules/gaussian_constraints.h index a124d17..4a2267a 100644 --- a/mpc_planner_modules/include/mpc_planner_modules/gaussian_constraints.h +++ b/mpc_planner_modules/include/mpc_planner_modules/gaussian_constraints.h @@ -19,6 +19,7 @@ namespace MPCPlanner void visualize(const RealTimeData &data, const ModuleData &module_data) override; private: + double _dummy_x{0.}, _dummy_y{0.}; }; } #endif // __GAUSSIAN_CONSTRAINTS_H_ 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/gaussian_constraints.cpp b/mpc_planner_modules/src/gaussian_constraints.cpp index cd67ac6..d54814e 100644 --- a/mpc_planner_modules/src/gaussian_constraints.cpp +++ b/mpc_planner_modules/src/gaussian_constraints.cpp @@ -24,6 +24,8 @@ namespace MPCPlanner (void)state; (void)data; (void)module_data; + _dummy_x = state.get("x") + 100.; + _dummy_y = state.get("y") + 100.; } void GaussianConstraints::setParameters(const RealTimeData &data, const ModuleData &module_data, int k) @@ -34,6 +36,21 @@ namespace MPCPlanner for (int d = 0; d < CONFIG["n_discs"].as(); d++) setSolverParameterEgoDiscOffset(k, _solver->_params, data.robot_area[d].offset, d); + if (k == 0) // Dummies + { + + for (size_t i = 0; i < data.dynamic_obstacles.size(); i++) + { + setSolverParameterGaussianObstX(k, _solver->_params, _dummy_x, i); + setSolverParameterGaussianObstY(k, _solver->_params, _dummy_y, i); + setSolverParameterGaussianObstMajor(k, _solver->_params, 0.1, i); + setSolverParameterGaussianObstMinor(k, _solver->_params, 0.1, i); + setSolverParameterGaussianObstRisk(k, _solver->_params, 0.05, i); + setSolverParameterGaussianObstR(k, _solver->_params, 0.1, i); + } + return; + } + std::vector copied_obstacles = data.dynamic_obstacles; for (size_t i = 0; i < copied_obstacles.size(); i++) diff --git a/mpc_planner_modules/src/scenario_constraints.cpp b/mpc_planner_modules/src/scenario_constraints.cpp index 8341d6a..a828ede 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) @@ -116,10 +113,15 @@ namespace MPCPlanner if (data_name == "dynamic obstacles") { + // Check if uncertainty was provided + for (auto &obs : data.dynamic_obstacles) + { + ROSTOOLS_ASSERT(obs.prediction.type != PredictionType::DETERMINISTIC, "When using Scenario Constraints, the predictions should have a non-zero uncertainty. If you are using pedestrian_simulator, set `process_noise` in config/configuration.yaml to a non-zero value to add uncertainty."); + } 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 +134,6 @@ namespace MPCPlanner if (data.dynamic_obstacles.size() != CONFIG["max_obstacles"].as()) { - missing_data += "Obstacles "; return false; } @@ -161,14 +162,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 97d2b51..c2cc375 100644 --- a/mpc_planner_solver/src/acados_solver_interface.cpp +++ b/mpc_planner_solver/src/acados_solver_interface.cpp @@ -2,6 +2,8 @@ #include +#include + namespace MPCPlanner { Solver::Solver(int solver_id) @@ -85,8 +87,39 @@ namespace MPCPlanner { int status = 1; + RosTools::Benchmarker iteration_timer("iteration"); + RosTools::Timer timeout_timer(_params.solver_timeout); + timeout_timer.start(); + // _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); @@ -106,24 +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); - for (int iteration = 0; iteration < _num_iterations; iteration++) - { + } - 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); + + _exit_code_one_iter = status; + return status; + } + + int Solver::completeOneIteration() + { ocp_nlp_get(_nlp_solver, "nlp_res", &_info.nlp_res); // Compute and retrieve the cost @@ -136,9 +173,14 @@ namespace MPCPlanner for (int k = 0; k < _nlp_dims->N; k++) ocp_nlp_out_get(_nlp_config, _nlp_dims, _nlp_out, k, "u", &_output.utraj[k * nu]); - // _output.print(); + double res_stat, res_eq, res_ineq, res_comp; + ocp_nlp_get(_nlp_solver, "res_eq", &res_eq); + if (res_eq > 1e-2 && _exit_code_one_iter == ACADOS_SUCCESS) + { + _exit_code_one_iter = ACADOS_QP_FAILURE; + } - if (status == ACADOS_SUCCESS) + if (_exit_code_one_iter == ACADOS_SUCCESS) { LOG_MARK("Solver_acados_solve(): SUCCESS!"); } @@ -146,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):