diff --git a/guidance/hybridpath_guidance/config/hybridpath_guidance_config.yaml b/guidance/hybridpath_guidance/config/hybridpath_guidance_config.yaml index 99bd951d..042cc781 100644 --- a/guidance/hybridpath_guidance/config/hybridpath_guidance_config.yaml +++ b/guidance/hybridpath_guidance/config/hybridpath_guidance_config.yaml @@ -5,4 +5,4 @@ path_generator_order: 1 # Differentiability order time_to_max_speed: 10.0 # Time to reach maximum speed dt: 0.1 # Time step - u_desired: 0.5 # Desired speed \ No newline at end of file + u_desired: 0.01 # Desired speed \ No newline at end of file diff --git a/motion/hybridpath_controller/hybridpath_controller/adaptive_backstep.py b/motion/hybridpath_controller/hybridpath_controller/adaptive_backstep.py index 74667ea5..bfc40b26 100644 --- a/motion/hybridpath_controller/hybridpath_controller/adaptive_backstep.py +++ b/motion/hybridpath_controller/hybridpath_controller/adaptive_backstep.py @@ -8,14 +8,15 @@ def __init__(self): self.init_system() def init_system(self) -> None: - self.K_1 = np.diag([10, 10, 10]) # First gain matrix - self.K_2 = np.diag([60, 60, 30]) # Second gain matrix + self.K_1 = np.diag([0.2, 0.2, 0.2]) # First gain matrix + self.K_2 = np.diag([1, 1, 1]) # Second gain matrix self.tau_max = np.array([41.0, 50.0, 55.0]) # Må tilpasses thrusterne ## Forenklet modell ## Bør også endres m = 50 self.M = np.diag([m, m, m]) self.D = np.diag([10, 10, 5]) + self.tau_max = np.array([100, 100, 100]) def control_law(self, state: Odometry, reference: HybridpathReference) -> np.ndarray: """ @@ -63,11 +64,11 @@ def control_law(self, state: Odometry, reference: HybridpathReference) -> np.nda tau = -self.K_2 @ z2 + self.D @ nu + self.M @ sigma1 + self.M @ ds_alpha1 * (v_s + w) # Add constraints to tau # This should be improved - # for i in range(len(tau)): - # if tau[i] > self.tau_max[i]: - # tau[i] = self.tau_max[i] - # elif tau[i] < -self.tau_max[i]: - # tau[i] = -self.tau_max[i] + for i in range(len(tau)): + if tau[i] > self.tau_max[i]: + tau[i] = self.tau_max[i] + elif tau[i] < -self.tau_max[i]: + tau[i] = -self.tau_max[i] return tau diff --git a/motion/lqr_controller/lqr_controller/lqr_controller.py b/motion/lqr_controller/lqr_controller/lqr_controller.py index 2d02230d..d3c4ab11 100644 --- a/motion/lqr_controller/lqr_controller/lqr_controller.py +++ b/motion/lqr_controller/lqr_controller/lqr_controller.py @@ -1,5 +1,4 @@ import numpy as np -import matplotlib.pyplot as plt from scipy.linalg import solve_continuous_are from lqr_controller.asv_model import ASV diff --git a/motion/lqr_controller/lqr_controller/lqr_controller_node.py b/motion/lqr_controller/lqr_controller/lqr_controller_node.py index 5f180f37..c8d59337 100644 --- a/motion/lqr_controller/lqr_controller/lqr_controller_node.py +++ b/motion/lqr_controller/lqr_controller/lqr_controller_node.py @@ -5,6 +5,11 @@ from nav_msgs.msg import Odometry from transforms3d.euler import quat2euler from lqr_controller.lqr_controller import LQRController +from time import sleep + +from rclpy.qos import QoSProfile, qos_profile_sensor_data, QoSHistoryPolicy, QoSReliabilityPolicy +qos_profile = QoSProfile(depth=1, history=qos_profile_sensor_data.history, + reliability=QoSReliabilityPolicy.BEST_EFFORT) class LQRControllerNode(Node): def __init__(self): @@ -21,18 +26,18 @@ def __init__(self): ]) self.wrench_publisher_ = self.create_publisher(Wrench, "thrust/wrench_input", 1) - self.state_subscriber_ = self.create_subscription(Odometry, "/sensor/seapath/odometry/ned", self.state_cb, 1) - self.guidance_subscriber_ = self.create_subscription(Odometry, "controller/lqr/reference", self.guidance_cb, 1) + self.state_subscriber_ = self.create_subscription(Odometry, "/sensor/seapath/odom/ned", self.state_cb, qos_profile=qos_profile) + # self.guidance_subscriber_ = self.create_subscription(Odometry, "controller/lqr/reference", self.guidance_cb, 1) m = self.get_parameter('lqr_controller.mass').get_parameter_value().double_value D = self.get_parameter('lqr_controller.D').get_parameter_value().double_array_value Q = self.get_parameter('lqr_controller.Q').get_parameter_value().double_array_value R = self.get_parameter('lqr_controller.R').get_parameter_value().double_array_value - self.get_logger().info(f"Mass: {m}") - self.get_logger().info(f"D: {D}") - self.get_logger().info(f"Q: {Q}") - self.get_logger().info(f"R: {R}") + # self.get_logger().info(f"Mass: {m}") + # self.get_logger().info(f"D: {D}") + # self.get_logger().info(f"Q: {Q}") + # self.get_logger().info(f"R: {R}") self.lqr_controller = LQRController(m, D, Q, R) @@ -40,8 +45,22 @@ def __init__(self): self.x_ref = [0, 0, 0] self.state = [0, 0, 0, 0, 0, 0] + self.enabled = False + + controller_period = 0.1 + self.controller_timer_ = self.create_timer(controller_period, self.controller_callback) + + self.test_oneMeterAhead() + self.get_logger().info("lqr_controller_node started") + def test_oneMeterAhead(self): + self.get_logger().info("Sleeping 3 secs to get odom state") + sleep(3) + self.x_ref = [self.state[0] + 1, self.state[1], self.state[2]] + self.enabled = True + self.get_logger().info("LQR enabled") + def odometrymsg_to_state(self, msg): x = msg.pose.pose.position.x y = msg.pose.pose.position.y @@ -60,18 +79,23 @@ def odometrymsg_to_state(self, msg): state = np.array([x, y, yaw, vx, vy, vyaw]) return state - def guidance_cb(self, msg): - self.x_ref = self.odometrymsg_to_state(msg)[:3] + # def guidance_cb(self, msg): + # self.x_ref = self.odometrymsg_to_state(msg)[:3] - #wrench = self.run_lqr_to_wrench() - #self.wrench_publisher_.publish(wrench) + # #wrench = self.run_lqr_to_wrench() + # #self.wrench_publisher_.publish(wrench) def state_cb(self, msg): self.state = self.odometrymsg_to_state(msg) - wrench = self.run_lqr_to_wrench() - self.wrench_publisher_.publish(wrench) - + def controller_callback(self): + """ + Callback function for the controller timer. This function calculates the control input and publishes the control input. + """ + if self.enabled: + wrench = self.run_lqr_to_wrench() + self.wrench_publisher_.publish(wrench) + def run_lqr_to_wrench(self): self.lqr_controller.linearize_model(self.state[2]) u = self.lqr_controller.calculate_control_input(self.state, self.x_ref, self.lqr_controller.K_LQR, self.lqr_controller.K_r)