From 2a9400603ce14326359875e1549fe990bf34c8d9 Mon Sep 17 00:00:00 2001
From: Alex AUTERNAUD <alex.auternaud@inria.fr>
Date: Wed, 31 May 2023 19:31:02 +0200
Subject: [PATCH] go_to_position checked
---
social_mpc/controller.py | 3 +--
1 file changed, 1 insertion(+), 2 deletions(-)
diff --git a/social_mpc/controller.py b/social_mpc/controller.py
index 348b199..69e5aaf 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])
--
GitLab