From 35318d4897a900d2769ecc6424c2af4f593848c5 Mon Sep 17 00:00:00 2001 From: Cyprian Date: Sun, 29 Sep 2024 15:58:39 +0200 Subject: [PATCH] [435-task-velocity-controller] Implemented stable PI control for pitch and yaw, still working on surge and heave. --- .../scripts/velocity_controller_c.py | 85 ++++++++++++------- 1 file changed, 55 insertions(+), 30 deletions(-) diff --git a/control/velocity_controller_c/scripts/velocity_controller_c.py b/control/velocity_controller_c/scripts/velocity_controller_c.py index aa00ddd7a..2724da7c5 100755 --- a/control/velocity_controller_c/scripts/velocity_controller_c.py +++ b/control/velocity_controller_c/scripts/velocity_controller_c.py @@ -9,7 +9,6 @@ from std_msgs.msg import Float32MultiArray - #A function to convert orientation quaternions to euler angles def quaternion_to_euler_angle(w, x, y, z): ysqr = y * y @@ -37,6 +36,18 @@ def ssa(angle): angle += 2 * np.pi return angle +def rotate_point(x, y, z , theta, phi, psi): + # Rotation matrix + R = np.array([[np.cos(theta)*np.cos(psi), np.sin(phi)*np.sin(theta)*np.cos(psi) - np.cos(phi)*np.sin(psi), np.cos(phi)*np.sin(theta)*np.cos(psi) + np.sin(phi)*np.sin(psi)], + [np.cos(theta)*np.sin(psi), np.sin(phi)*np.sin(theta)*np.sin(psi) + np.cos(phi)*np.cos(psi), np.cos(phi)*np.sin(theta)*np.sin(psi) - np.sin(phi)*np.cos(psi)], + [-np.sin(theta), np.sin(phi)*np.cos(theta), np.cos(phi)*np.cos(theta)]]) + # Point to be rotated + P = np.array([x, y, z]) + # Rotated point + P_rot = np.dot(R, P) + + return round(P_rot[0],12) , round(P_rot[1],12), round(P_rot[2],12) + class VelocityNode(Node): def __init__(self): @@ -46,7 +57,7 @@ def __init__(self): ) self.guidance_subscriber = self.create_subscription( - Float32MultiArray, "velocity_points", self.guidance_callback, 10 + Float32MultiArray, "/guidance/los", self.guidance_callback, 10 ) self.publisher_controller = self.create_publisher(Wrench, "/thrust/wrench_input", 10) @@ -54,7 +65,7 @@ def __init__(self): self.control_timer = self.create_timer(0.1, self.publish_callback) self.state_timer = self.create_timer(0.3, self.publish_states) self.topic_subscriber = self.create_subscription( - Odometry, "/nucleus/odom", self.subscribe_callback, 10) + Odometry, "/nucleus/odom", self.nucleus_callback, 10) self.publisher = self.create_publisher(Wrench, "/thrust/wrench_input", 10) self.timer = self.create_timer(0.1, self.publish_callback) @@ -72,24 +83,29 @@ def __init__(self): self.position = np.array([0.0, 0.0, 0.0]) self.twist = np.array([0.0, 0.0, 0.0]) self.orientation = np.array([0.0, 0.0, 0.0]) - self.abu_values = np.array([-0.2, -5.0 , -np.pi/4, np.pi/4]) + self.rotated_point_position = np.array([0.0, 0.0, 0.0]) + self.abu_values = np.array([0.3, -5.0 , np.pi/4, np.pi/4]) - self.Kp_linear = 100.0 - self.Kp_pitch = 2.0 - self.Kp_yaw = 2.0 + self.Kp_surge = 20.0 + self.Kp_heave = 10.0 + self.Kp_pitch = 2.5 + self.Kp_yaw = 2.5 self.Ti_pitch = 0.05 - self.Ti_yaw = 0.05 - self.Ti_surge = 33.0 + self.Ti_yaw = 0.001 + self.Ti_surge = 1.0 + self.Ti_heave = 0.5 self.pitch_integral_sum = 0.0 self.yaw_integral_sum = 0.0 self.surge_integral_sum = 0.0 + self.heave_integral_sum = 0.0 self.heave_error = 0.0 self.pitch_error = 0.0 self.yaw_error = 0.0 self.surge_error = 0.0 - + + self.get_logger().info("Velocity Controller Node has been started") def nucleus_callback(self, msg: Odometry): # callback function self.position = np.array( @@ -114,41 +130,50 @@ def nucleus_callback(self, msg: Odometry): # callback function msg.pose.pose.orientation.z) self.orientation = np.array([X, Y, Z]) + # self.rotated_point_position[0], self.rotated_point_position[1], self.rotated_point_position[2] = rotate_point(self.position[0], self.position[1], self.position[2], Y, X, Z) + def guidance_callback(self, msg: Float32MultiArray): # callback function - self.abu_values = np.array(msg.data[:3]) + self.abu_values = msg.data def publish_callback(self): # The controller function self.surge_error = self.abu_values[0] - self.twist[0] + self.heave_error = self.abu_values[1] - self.rotated_point_position[2] self.pitch_error = ssa(self.abu_values[2] - self.orientation[1]) self.yaw_error = ssa(self.abu_values[3] - self.orientation[2]) msg = Wrench() - if abs(self.surge_error) < 0.005: - self.surge_integral_sum = 0.0 + # if abs(self.pitch_error) < 0.05 and abs(self.yaw_error < 0.05): + + # if abs(self.surge_error) > 0.005: + # self.surge_integral_sum += self.surge_error + # else: + # self.surge_integral_sum += 0.0 + # msg.force.x = self.surge_error*self.Kp_surge + self.surge_integral_sum * self.Ti_surge # SURGE + + # else: + # self.surge_integral_sum = 0.0 + # msg.force.x = 0.0 + + self.pitch_integral_sum += self.pitch_error + msg.torque.y = -(self.pitch_error * self.Kp_pitch + self.pitch_integral_sum * self.Ti_pitch) # PITCH + + if abs(self.yaw_error) > 0.005: + self.yaw_integral_sum += self.yaw_error else: - self.surge_integral_sum += self.surge_error + self.yaw_integral_sum = 0.0 - msg.force.x = self.surge_error*self.Kp_linear + self.surge_integral_sum * self.Ti_surge # SURGE + msg.torque.z = -(self.yaw_error * self.Kp_yaw + self.yaw_integral_sum * self.Ti_yaw) # YAW + # if abs(self.heave_error) > 0.005: + # self.heave_integral_sum += self.heave_error + # else: + # self.heave_integral_sum = 0.0 + # msg.force.z = self.heave_error*self.Kp_heave + self.heave_integral_sum * self.Ti_heave - msg.force.z = 0.0 # HEAVE - msg.torque.z = 0.0 - msg.torque.y = 0.0 - - # if self.pitch_error < 0.4: - # self.pitch_integral_sum += self.pitch_error - - # if self.yaw_error < 0.1: - # self.yaw_integral_sum += self.yaw_error - - # msg.torque.y = 0.0 - # msg.torque.y = -(self.pitch_error * self.Kp_pitch + self.pitch_integral_sum * self.Ti_pitch) # PITCH - - # msg.torque.z = -(self.yaw_error * self.Kp_yaw + self.yaw_integral_sum * self.Ti_yaw)# YAW - # self.publisher_controller.publish(msg) + self.publisher_controller.publish(msg) def publish_states(self): msg = Float32MultiArray()