diff --git a/social_mpc/controller.py b/social_mpc/controller.py
index 9ef853b7ab1988ecacb0a90a0979d77e318ff962..e758c96f9e9e4c1977ce0e7cd83a1387f1d0c743 100644
--- a/social_mpc/controller.py
+++ b/social_mpc/controller.py
@@ -354,7 +354,9 @@ class RobotController():
             if target[-1]:
                 self.goto_target_flag = True
                 if target[-2]:
-                    new_target = [*local_to_global(state.robot_pose, target[:2]), constraint_angle(state.robot_pose[-1] + np.pi/2 + 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])]
                     self.set_target_pose(new_target)
                 else:
                     self.set_target_pose(target[:-1])
diff --git a/social_mpc/utils.py b/social_mpc/utils.py
index 410dbc8445d39303bc4838a9b56271b092940037..381d82419e91054db830b0dd0f949bbb6dd1e9b6 100644
--- a/social_mpc/utils.py
+++ b/social_mpc/utils.py
@@ -72,7 +72,7 @@ def rotmat_2d_np(angle):
 
 def local_to_global(robot_position, x):
     angle = robot_position[-1]
-    y = rotmat_2d(angle).dot(x) + robot_position[:2]
+    y = rotmat_2d(-angle).dot(x) + robot_position[:2]
     return np.array(y).reshape(x.shape)