added lateral force constraint to mpc

CMU-Robotics-Club · Mar 17, 2024 · 50f20cd · 50f20cd
1 parent f6cec88
commit 50f20cd
Show file tree

Hide file tree

Showing 2 changed files with 25 additions and 14 deletions.
diff --git a/rb_ws/src/buggy/launch/sim_2d_2buggies.launch b/rb_ws/src/buggy/launch/sim_2d_2buggies.launch
@@ -1,7 +1,5 @@
 <launch>
-    <arg name="sc_controller"     default="stanley" />
-    <arg name="sc_path"           default="buggycourse_safe_1.json" />
-
+    <arg name="sc_controller"     default="mpc" />
     <arg name="sc_start_pos"    default="Hill1_SC" />
     <arg name="sc_velocity"         default="15.0" />
 
@@ -11,7 +9,7 @@
     <arg name="nand_start_pos"     default="Hill1_NAND" />
     <arg name="nand_velocity"       default="10.0" />
 
-    <arg name="manual_vel"          default="false" />
+    <arg name="manual_vel"          default="true" />
     <arg name="auto_vel"            default="false" />
 
     <group if="$(arg auto_vel)">

diff --git a/rb_ws/src/buggy/scripts/auton/model_predictive_controller.py b/rb_ws/src/buggy/scripts/auton/model_predictive_controller.py
@@ -29,13 +29,16 @@ class ModelPredictiveController(Controller):
     Convex Model Predictive Controller (MPC)
     """
 
-    DEBUG = False
+    DEBUG = True
     PLOT = False
     TIME = False
     ROS = True
 
     # MPC Params
-    WHEELBASE = 1.17
+    WHEELBASE = 1.17 #m
+    MASS = 62 #kg
+    MAX_LAT_FORCE = 250 #N, Factor of Safety = 1.4
+    MAX_STEER = np.deg2rad(20) #degrees
     MIN_SPEED = 1.0
     MPC_TIMESTEP = 0.02
     MPC_HORIZON = 125
@@ -44,14 +47,10 @@ class ModelPredictiveController(Controller):
     N_CONTROLS = 1
 
     # MPC Cost Weights
-    state_cost = np.array([0.0001, 250, 5, 25])  # x, y, theta, steer
+    state_cost = np.array([0.0001, 250, 5, 2500])  # x, y, theta, steer
     control_cost = np.array([5])  # d_steer
     final_state_cost = 2 * np.array([0.0001, 250, 5, 25])  # x, y, theta, steer
 
-    # State constraints (relative to the reference)
-    state_lb = np.array([-np.inf, -np.inf, -np.inf, np.deg2rad(-10)])  # x, y, theta, steer
-    state_ub = np.array([np.inf, np.inf, np.inf, np.deg2rad(10)])  # x, y, theta, steer
-
     # Control constraints
     control_lb = np.array([-np.pi * 2])  # d_steer
     control_ub = np.array([np.pi * 2])  # d_steer
@@ -64,7 +63,7 @@ class ModelPredictiveController(Controller):
         "eps_abs": 1e-4,
         "eps_rel": 1e-4,
         "polish": 1,
-        # "time_limit": 0.01,
+        "time_limit": 5e-2,
         "warm_start": True,
     }
 
@@ -348,6 +347,11 @@ def transform_trajectory(trajectory, transform_matrix):
 
     first_iteration = True
 
+    def compute_steering_limit(self, speed):
+        # in radians
+        max_steer = np.arctan(self.WHEELBASE * self.MAX_LAT_FORCE / (self.MASS * speed ** 2))
+        return max_steer
+
     def compute_trajectory(self, current_pose: Pose, trajectory: Trajectory, current_speed: float):
         """
         Computes the desired control output given the current state and reference trajectory
@@ -538,12 +542,20 @@ def compute_trajectory(self, current_pose: Pose, trajectory: Trajectory, current
         if self.TIME:
             t = time.time()
 
+        steer_limit = min(self.compute_steering_limit(current_speed), self.MAX_STEER)
+        if self.DEBUG:
+            print("max steer:", np.rad2deg(steer_limit))
+
+        # State constraints
+        state_lb = np.array([-np.inf, -np.inf, -np.inf, -steer_limit])  # x, y, theta, steer
+        state_ub = np.array([np.inf, np.inf, np.inf, steer_limit])  # x, y, theta, steer
+
         lb = np.hstack(
             (
                 -self.state_jacobian(reference_trajectory[0, :])
                 @ (self.state(0, 0, 0, 0) - reference_trajectory[0, :]),
                 np.zeros(self.N_STATES * (self.MPC_HORIZON - 1)),
-                np.tile(self.state_lb, self.MPC_HORIZON) + reference_trajectory.ravel(),
+                np.tile(state_lb, self.MPC_HORIZON),
                 np.tile(self.control_lb, self.MPC_HORIZON) + reference_control.ravel(),
                 # np.tile(n.T @ p, self.MPC_HORIZON),
             )
@@ -553,7 +565,7 @@ def compute_trajectory(self, current_pose: Pose, trajectory: Trajectory, current
                 -self.state_jacobian(reference_trajectory[0, :])
                 @ (self.state(0, 0, 0, 0) - reference_trajectory[0, :]),
                 np.zeros(self.N_STATES * (self.MPC_HORIZON - 1)),
-                np.tile(self.state_ub, self.MPC_HORIZON) + reference_trajectory.ravel(),
+                np.tile(state_ub, self.MPC_HORIZON),
                 np.tile(self.control_ub, self.MPC_HORIZON) + reference_control.ravel(),
                 # np.tile(np.inf, self.MPC_HORIZON),
             )
@@ -606,6 +618,7 @@ def compute_trajectory(self, current_pose: Pose, trajectory: Trajectory, current
         state_trajectory = solution_trajectory[:, self.N_CONTROLS:(self.N_CONTROLS + self.N_STATES)]
 
         if not (results.info.status == "solved" or results.info.status == "solved inaccurate"):
+            print("unable to solve!")
             return reference_trajectory
 
         state_trajectory += reference_trajectory