From e754e360a5faa32f0716efca60a6488f0bbb6037 Mon Sep 17 00:00:00 2001 From: Buggy Date: Wed, 20 Mar 2024 18:33:20 -0400 Subject: [PATCH] minor changes to MPC parameters set path planning curb margin to 1m --- rb_ws/src/buggy/launch/sim_2d_2buggies.launch | 11 +++---- rb_ws/src/buggy/scripts/2d_sim/engine.py | 19 +++++++++--- .../auton/model_predictive_controller.py | 29 ++++++++++--------- rb_ws/src/buggy/scripts/auton/path_planner.py | 2 +- 4 files changed, 37 insertions(+), 24 deletions(-) diff --git a/rb_ws/src/buggy/launch/sim_2d_2buggies.launch b/rb_ws/src/buggy/launch/sim_2d_2buggies.launch index 61eeb2e1..51bd1955 100755 --- a/rb_ws/src/buggy/launch/sim_2d_2buggies.launch +++ b/rb_ws/src/buggy/launch/sim_2d_2buggies.launch @@ -1,7 +1,7 @@ - + @@ -9,7 +9,7 @@ - + @@ -39,12 +39,13 @@ - - + + + diff --git a/rb_ws/src/buggy/scripts/2d_sim/engine.py b/rb_ws/src/buggy/scripts/2d_sim/engine.py index 9f44b86a..dde65855 100755 --- 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 index 9f8355a4..ee95ba01 100644 --- 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,10 +52,10 @@ 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 = { @@ -63,8 +63,9 @@ class ModelPredictiveController(Controller): "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 index 8d5f6e37..9251eb26 100755 --- 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