diff --git a/Control/move_to_pose/move_to_pose.py b/Control/move_to_pose/move_to_pose.py index cc29666297a..279ba0625bd 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