minor changes to MPC parameters

set path planning curb margin to 1m
CMU-Robotics-Club · Mar 20, 2024 · e754e36 · e754e36
1 parent 50f20cd
commit e754e36
Show file tree

Hide file tree

Showing 4 changed files with 37 additions and 24 deletions.
diff --git a/rb_ws/src/buggy/launch/sim_2d_2buggies.launch b/rb_ws/src/buggy/launch/sim_2d_2buggies.launch
@@ -1,15 +1,15 @@
 <launch>
     <arg name="sc_controller"     default="mpc" />
     <arg name="sc_start_pos"    default="Hill1_SC" />
-    <arg name="sc_velocity"         default="15.0" />
+    <arg name="sc_velocity"         default="12.0" />
 
     <arg name="nand_controller"     default="stanley" />
     <arg name="nand_path"           default="buggycourse_safe_1.json" />
     <arg name="sc_path"           default="buggycourse_raceline.json" />
     <arg name="nand_start_pos"     default="Hill1_NAND" />
     <arg name="nand_velocity"       default="10.0" />
 
-    <arg name="manual_vel"          default="true" />
+    <arg name="manual_vel"          default="false" />
     <arg name="auto_vel"            default="false" />
 
     <group if="$(arg auto_vel)">
@@ -39,12 +39,13 @@
     <node name="nand_auton_system" pkg="buggy" type="autonsystem.py" output="screen"
         args="$(arg nand_autonsystem_args)"/>
 
-    <node name="sc_sim_2d_engine" pkg="buggy" type="engine.py" output="screen"
-        args="$(arg sc_start_pos) $(arg sc_velocity) SC"/>
-
     <arg name="sc_autonsystem_args" default="--controller $(arg sc_controller) --dist 0.0 --traj $(arg sc_path) --self_name SC --other_name NAND" />
     <node name="sc_auton_system" pkg="buggy" type="autonsystem.py" output="screen"
         args="$(arg sc_autonsystem_args)"/>
 
+    <node name="sc_sim_2d_engine" pkg="buggy" type="engine.py" output="screen"
+        args="$(arg sc_start_pos) $(arg sc_velocity) SC"/>
+
+
 
 </launch>
diff --git a/rb_ws/src/buggy/scripts/2d_sim/engine.py b/rb_ws/src/buggy/scripts/2d_sim/engine.py
@@ -115,7 +115,7 @@ def __init__(self, start_pos: str, velocity: float, buggy_name: str):
 
         self.steering_angle = 0  # degrees
         self.rate = 200  # Hz
-        self.pub_skip = 1  # publish every pub_skip ticks
+        self.pub_skip = 2  # publish every pub_skip ticks
 
         self.lock = threading.Lock()
 
@@ -196,6 +196,7 @@ def step(self):
             self.e_utm = e_utm_new
             self.n_utm = n_utm_new
             self.heading = heading_new
+
     def publish(self):
         """Publishes the pose the arrow in visualizer should be at"""
         p = Pose()
@@ -231,8 +232,8 @@ def publish(self):
         heading_noisy = heading
 
         if (Simulator.NOISE):
-            lat_noisy = lat + np.random.normal(0, 1e-8)  # ~.1cm error
-            long_noisy = long + np.random.normal(0, 1e-8)  # ~.1cm error
+            lat_noisy = lat + np.random.normal(0, 1e-6)  # ~10cm error
+            long_noisy = long + np.random.normal(0, 1e-6)  # ~10cm error
             velocity_noisy = velocity + np.random.normal(0, 0.01)
             heading_noisy = heading + np.random.normal(0, 0.01)
 
@@ -304,7 +305,6 @@ def publish(self):
             0.01,
         ]
 
-
         self.pose_publisher.publish(odom)
 
     def loop(self):
@@ -335,5 +335,16 @@ def loop(self):
     buggy_name = sys.argv[3]
 
     sim = Simulator(start_pos, velocity, buggy_name)
+    for i in range(100):
+        sim.publish()
+        rospy.sleep(0.01)
+
+    # publish initial position, then sleep
+    # so that auton stack has time to initialize
+    # before buggy moves
+    print("SIM PUBLISHED", rospy.get_time())
+    if buggy_name == "SC":
+        rospy.sleep(15.0)
+    print("SIM STARTED STEPPING", rospy.get_time())
     sim.loop()
 
diff --git a/rb_ws/src/buggy/scripts/auton/model_predictive_controller.py b/rb_ws/src/buggy/scripts/auton/model_predictive_controller.py
@@ -37,7 +37,7 @@ class ModelPredictiveController(Controller):
     # MPC Params
     WHEELBASE = 1.17 #m
     MASS = 62 #kg
-    MAX_LAT_FORCE = 250 #N, Factor of Safety = 1.4
+    MAX_LAT_FORCE = 350 #N
     MAX_STEER = np.deg2rad(20) #degrees
     MIN_SPEED = 1.0
     MPC_TIMESTEP = 0.02
@@ -52,19 +52,20 @@ class ModelPredictiveController(Controller):
     final_state_cost = 2 * np.array([0.0001, 250, 5, 25])  # x, y, theta, steer
 
     # Control constraints
-    control_lb = np.array([-np.pi * 2])  # d_steer
-    control_ub = np.array([np.pi * 2])  # d_steer
-    # control_lb = np.array([-np.inf])  # d_steer
-    # control_ub = np.array([np.inf])  # d_steer
+    # control_lb = np.array([-np.pi / 2])  # d_steer
+    # control_ub = np.array([np.pi / 2])  # d_steer
+    control_lb = np.array([-np.inf])  # d_steer
+    control_ub = np.array([np.inf])  # d_steer
 
     # Solver params
     solver_settings: dict = {
         "verbose": DEBUG,
         "eps_abs": 1e-4,
         "eps_rel": 1e-4,
         "polish": 1,
-        "time_limit": 5e-2,
+        "time_limit": 2e-2,
         "warm_start": True,
+        # "linsys_solver": "mkl pardiso",
     }
 
     # Precomputed arrays
@@ -552,25 +553,25 @@ def compute_trajectory(self, current_pose: Pose, trajectory: Trajectory, current
 
         lb = np.hstack(
             (
-                -self.state_jacobian(reference_trajectory[0, :])
-                @ (self.state(0, 0, 0, 0) - reference_trajectory[0, :]),
+                -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(state_lb, self.MPC_HORIZON),
-                np.tile(self.control_lb, self.MPC_HORIZON) + reference_control.ravel(),
+                np.tile(state_lb, self.MPC_HORIZON) - reference_trajectory.ravel(),
+                np.tile(self.control_lb, self.MPC_HORIZON) - reference_control.ravel(),
                 # np.tile(n.T @ p, self.MPC_HORIZON),
             )
         )
         ub = np.hstack(
             (
-                -self.state_jacobian(reference_trajectory[0, :])
-                @ (self.state(0, 0, 0, 0) - reference_trajectory[0, :]),
+                -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(state_ub, self.MPC_HORIZON),
-                np.tile(self.control_ub, self.MPC_HORIZON) + reference_control.ravel(),
+                np.tile(state_ub, self.MPC_HORIZON) - reference_trajectory.ravel(),
+                np.tile(self.control_ub, self.MPC_HORIZON) - reference_control.ravel(),
                 # np.tile(np.inf, self.MPC_HORIZON),
             )
         )
 
+        print("bounds", lb[(self.N_STATES * self.MPC_HORIZON):], ub[(self.N_STATES * self.MPC_HORIZON):])
+
         if self.TIME:
             create_mat_time_bounds = 1000.0 * (time.time() - t)
 

diff --git a/rb_ws/src/buggy/scripts/auton/path_planner.py b/rb_ws/src/buggy/scripts/auton/path_planner.py
@@ -12,7 +12,7 @@
 class PathPlanner():
     # move the curb towards the center of the course by CURB_MARGIN meters
     # for a margin of safety
-    CURB_MARGIN = 0 #m
+    CURB_MARGIN = 1#m
 
     # the offset is calculated as a mirrored sigmoid function of distance
     OFFSET_SCALE_CROSS_TRACK = 2 #m