Fixed plotting points, removed ivansim

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
 

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()
-
 
 
 

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]

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')