From 7eacf6840cf0c36cc7af6895d30e8486485bc414 Mon Sep 17 00:00:00 2001 From: Martin Huynh Date: Sun, 4 Feb 2024 14:10:52 +0100 Subject: [PATCH] Fixed plotting points, removed ivansim --- .../los_guidance/los_guidance/los_guidance.py | 1 - .../lqr_controller/lqr_controller.py | 74 ------------------- .../lqr_controller/lqr_controller_node.py | 1 - .../vessel_simulator/vessel_simulator_node.py | 22 +++--- 4 files changed, 9 insertions(+), 89 deletions(-) diff --git a/guidance/los_guidance/los_guidance/los_guidance.py b/guidance/los_guidance/los_guidance/los_guidance.py index 500c8059..b8855d51 100644 --- a/guidance/los_guidance/los_guidance/los_guidance.py +++ b/guidance/los_guidance/los_guidance/los_guidance.py @@ -4,7 +4,6 @@ class LOSGuidance: def __init__(self, p0: list[float], p1: list[float], p_next: list[float]): self.set_path(p0, p1) self.heading_ref = 50*np.pi/180 # magic init number!!! - self.p_next = [np.array([40, 40]), np.array([90.0, -20.0]), np.array([120.0, 50.0])]#, np.array([160, 0.0]), np.array([60.0, 60.0])] self.p_next = [np.array([50, -40]), np.array([20.0, -80.0]), np.array([120.0, -60.0]), np.array([160, 0.0]), np.array([60.0, 60.0])] self.acceptance_radius = 0.5 diff --git a/motion/lqr_controller/lqr_controller/lqr_controller.py b/motion/lqr_controller/lqr_controller/lqr_controller.py index 7c977f8a..2d02230d 100644 --- a/motion/lqr_controller/lqr_controller/lqr_controller.py +++ b/motion/lqr_controller/lqr_controller/lqr_controller.py @@ -9,7 +9,6 @@ def ssa(angle): class LQRController: def __init__(self, m: float, D: list[float], Q: list[float], R: list[float]): - #self.heading_ref = 30*np.pi/180 # magic init number!!! self.heading_ref = 50*np.pi/180 self.M = np.diag([m, m, m]) @@ -39,79 +38,6 @@ def calculate_control_input(self, x, x_ref, K_LQR, K_r): # print("K_r:", K_r) u = -K_LQR @ x + K_r @ x_ref return u - - # def run_ivan_sim(self): - # x_init = np.array([10, -20, 40*np.pi/180, 0, 0, 0]) - # x_ref = np.array([0, 0, self.heading_ref]) - - # look_ahead = 5 # Look ahead distance - - # T = 69.0 # Total simulation time - # dt = 0.01 # Time step - # num_steps = int(T / dt) # Number of time steps - - # # Initialize arrays to store data - # time = np.linspace(0, T, num_steps+1) - # x_history = np.zeros((num_steps+1, self.A.shape[0])) - # u_history = np.zeros((num_steps+1, self.B.shape[1])) - # x_ref_history = np.zeros((num_steps+1, np.shape(x_ref)[0])) - - # cross_track_error_history = np.zeros(num_steps+1) - - # # Simulation loop - # x = x_init # Initial state - # for i in range(num_steps+1): - # #x_ref[i] = reference_function_const(i * dt) # Update the reference at each time step - # # x_ref_history[i, :] = x_ref # Update the reference at each time step - - # # generate reference at the look-ahead distance - # p_asv = np.array([x[0], x[1]]) - # errors = self.R_Pi_p.T @ (p_asv - self.p0) - # along_track_error = errors[0] - # p_los_world = self.R_Pi_p @ np.array([along_track_error + look_ahead, 0]) + self.p0 - - # x_ref[:2] = p_los_world # Update the position reference at each time step - # x_ref_history[i, :] = x_ref - - # u = self.calculate_control_input(x, x_ref, self.K_LQR, self.K_r) # Calculate control input 'u' at each time step - # x = self.asv.RK4_integration_step(x, u, dt) - - # x_history[i] = x # Store state history - # u_history[i] = u # Store control input history - - # # Plot results - # plt.figure(figsize=(12, 8)) - # plt.subplot(3, 1, 1) - # for j in range(3): - # plt.plot(time, x_history[:, j], label=f'State (x_{j+1})') - # plt.plot(time, x_ref_history[:, j], linestyle='--', label=f'Reference (x_ref_{j+1})') - # plt.xlabel('Time') - # plt.ylabel('State Value') - # plt.legend() - - # plt.subplot(3, 1, 2) - # for j in range(self.B.shape[1]): - # plt.plot(time, u_history[:, j], label=f'Control Input (u_{j+1})') - # plt.xlabel('Time') - # plt.ylabel('Control Input Value') - # plt.legend() - - # plt.subplot(3, 1, 3) - # plt.scatter(x_history[:,0], x_history[:,1], label=f'Position') - # plt.plot([self.p0[0], self.p1[0]], [self.p0[1], self.p1[1]], 'r-', label='Path') - # plt.xlabel('x') - # plt.ylabel('y') - # plt.legend() - - # plt.tight_layout() - # plt.show() - # plt.figure(1) - # plt.plot(time, cross_track_error_history, label="cross track error") - # plt.axis("equal") - # plt.plot(time, np.zeros_like(time)) - # plt.legend() - # plt.show() - \ No newline at end of file diff --git a/motion/lqr_controller/lqr_controller/lqr_controller_node.py b/motion/lqr_controller/lqr_controller/lqr_controller_node.py index d1d6246d..5f180f37 100644 --- a/motion/lqr_controller/lqr_controller/lqr_controller_node.py +++ b/motion/lqr_controller/lqr_controller/lqr_controller_node.py @@ -35,7 +35,6 @@ def __init__(self): self.get_logger().info(f"R: {R}") self.lqr_controller = LQRController(m, D, Q, R) - #self.lqr_controller.run_ivan_sim() # Using x, y, yaw as reference (1x3) self.x_ref = [0, 0, 0] diff --git a/motion/vessel_simulator/vessel_simulator/vessel_simulator_node.py b/motion/vessel_simulator/vessel_simulator/vessel_simulator_node.py index 09a5c340..1595c00f 100644 --- a/motion/vessel_simulator/vessel_simulator/vessel_simulator_node.py +++ b/motion/vessel_simulator/vessel_simulator/vessel_simulator_node.py @@ -26,7 +26,6 @@ def __init__(self): self.x_ref = np.array([0, 0, 50*np.pi/180]) # Note to self: make x_ref have 6 states self.num_steps_simulated = 0 - m = 50.0 self.M = np.diag([m, m, m]) @@ -60,7 +59,7 @@ def __init__(self): ," ## / U ," ## / - Waiting for simulation to finish...""" + str(self.T) + """ secs approximated waiting time on Martin's Thinkpad, will probably go faster for everybody else :) + Waiting for simulation to finish...""" + str(self.T) + """ secs approximated waiting time :) """) self.state_publisher_.publish(self.state_to_odometrymsg(self.state)) @@ -101,14 +100,11 @@ def state_to_odometrymsg(self, state): def wrench_input_cb(self, msg): u = np.array([msg.force.x, msg.force.y, msg.torque.z]) - x_next = self.RK4_integration_step(self.state, u, self.dt) - self.x_history[self.num_steps_simulated] = x_next self.x_ref_history[self.num_steps_simulated, : ] = self.x_ref self.u_history[self.num_steps_simulated] = u - - print(f"self.x_ref_history[{self.num_steps_simulated}]: {self.x_ref_history[self.num_steps_simulated]}") + # print(f"self.x_ref_history[{self.num_steps_simulated}]: {self.x_ref_history[self.num_steps_simulated]}") if (self.num_steps_simulated >= self.num_steps): self.plot_history() @@ -122,7 +118,7 @@ def wrench_input_cb(self, msg): self.state_publisher_.publish(odometry_msg) self.num_steps_simulated += 1 - #self.get_logger().info(str(self.num_steps_simulated) + " lol xD") # heh DO NOT REMOVE + self.get_logger().info(str(self.num_steps_simulated) + " lol xD") # heh DO NOT REMOVE def state_dot(self, state: np.ndarray, tau_actuation: np.ndarray, V_current: np.ndarray = np.zeros(2)) -> np.ndarray: """ @@ -190,18 +186,18 @@ def plot_history(self): plt.scatter(self.x_history[:, 0], self.x_history[:, 1], label='Position', s=5) p0 = [0.0, 0.0] - p1 = [40.0, 40.0] - p2 = [90.0, -20.0] - p3 = [120.0, 50.0] + p1 = [20.0, 20.0] + p2 = [50.0, -40.0] + p3 = [20.0, -80.0] p4 = [120.0, -60.0] p5 = [160, 0.0] p6 = [60.0, 60.0] plt.plot([p0[0], p1[0]], [p0[1], p1[1]], 'r-', label='Path') plt.plot([p1[0], p2[0]], [p1[1], p2[1]], 'g-', label='Path') plt.plot([p2[0], p3[0]], [p2[1], p3[1]], 'b-', label='Path') - # plt.plot([p3[0], p4[0]], [p3[1], p4[1]], 'y-', label='Path') - # plt.plot([p4[0], p5[0]], [p4[1], p5[1]], 'm-', label='Path') - # plt.plot([p5[0], p6[0]], [p5[1], p6[1]], 'c-', label='Path') + plt.plot([p3[0], p4[0]], [p3[1], p4[1]], 'y-', label='Path') + plt.plot([p4[0], p5[0]], [p4[1], p5[1]], 'm-', label='Path') + plt.plot([p5[0], p6[0]], [p5[1], p6[1]], 'c-', label='Path') plt.xlabel('x') plt.ylabel('y')