From 8f84f0f5518797c1c11f4cee204c0a5c6bffbe82 Mon Sep 17 00:00:00 2001 From: noob3-3 Date: Sat, 27 Apr 2024 20:51:19 +0800 Subject: [PATCH] fix::Control PathFinderController bug (#1000) --- Control/move_to_pose/move_to_pose.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Control/move_to_pose/move_to_pose.py b/Control/move_to_pose/move_to_pose.py index cc29666297..279ba0625b 100644 --- a/Control/move_to_pose/move_to_pose.py +++ b/Control/move_to_pose/move_to_pose.py @@ -74,7 +74,7 @@ def calc_control_command(self, x_diff, y_diff, theta, theta_goal): alpha = angle_mod(np.arctan2(y_diff, x_diff) - theta) beta = angle_mod(theta_goal - theta - alpha) v = self.Kp_rho * rho - w = self.Kp_alpha * alpha - controller.Kp_beta * beta + w = self.Kp_alpha * alpha - self.Kp_beta * beta if alpha > np.pi / 2 or alpha < -np.pi / 2: v = -v