diff --git a/social_mpc/controller.py b/social_mpc/controller.py
index 348b1994576282dcda61ec58bbd73b402f4bc776..69e5aafd81b85102a5f09bcaafef1ae4010a41d2 100644
--- a/social_mpc/controller.py
+++ b/social_mpc/controller.py
@@ -355,8 +355,7 @@ class RobotController():
                 self.goto_target_flag = True
                 if target[-2]:
                     robot_pose = np.copy(state.robot_pose)
-                    robot_pose[-1] = constraint_angle(robot_pose[-1] + np.pi/2)
-                    new_target = [*local_to_global(robot_pose, target[:2]), constraint_angle(state.robot_pose[-1] + np.pi/2 + target[2])]
+                    new_target = [*local_to_global(robot_pose, target[:2]), constraint_angle(state.robot_pose[-1] + target[2])]
                     self.set_target_pose(new_target)
                 else:
                     self.set_target_pose(target[:-1])