go_to_position checked

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])