From d388a95285628a0d81fcf8354921776357652c4c Mon Sep 17 00:00:00 2001
From: Alex AUTERNAUD <alex.auternaud@inria.fr>
Date: Tue, 4 Apr 2023 10:24:16 +0200
Subject: [PATCH] local_to_global look_at and go_to
---
social_mpc/controller.py | 4 +++-
social_mpc/utils.py | 2 +-
2 files changed, 4 insertions(+), 2 deletions(-)
diff --git a/social_mpc/controller.py b/social_mpc/controller.py
index 9ef853b..e758c96 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 410dbc8..381d824 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)
--
GitLab