add twist.linear.z calculation to align frame controller

itu-auv · Jan 29, 2025 · e491685 · e491685
1 parent f7cfad7
commit e491685
Showing 1 changed file with 1 addition and 1 deletion.
diff --git a/auv_control/auv_control/scripts/align_frame_controller.py b/auv_control/auv_control/scripts/align_frame_controller.py
@@ -140,7 +140,7 @@ def compute_cmd_vel(self, trans, rot):
         # Set linear velocities based on translation differences
         twist.linear.x = self.constrain(trans[0] * kp, self.max_linear_velocity)
         twist.linear.y = self.constrain(trans[1] * kp, self.max_linear_velocity)
-
+        twist.linear.z = self.constrain(trans[2] * kp, self.max_linear_velocity)
         # Convert quaternion to Euler angles and set angular velocity
         _, _, yaw = euler_from_quaternion(rot)
         twist.angular.z = self.constrain(yaw * angle_kp, self.max_angular_velocity)