From a8ab44b857a5b07dea0c8491e79c7d1a0a951507 Mon Sep 17 00:00:00 2001
From: Alex AUTERNAUD <alex.auternaud@inria.fr>
Date: Mon, 29 Aug 2022 17:47:09 +0200
Subject: [PATCH] goal.py issue
---
social_mpc/goal.py | 5 +++--
1 file changed, 3 insertions(+), 2 deletions(-)
diff --git a/social_mpc/goal.py b/social_mpc/goal.py
index 674cba4..73c6f25 100644
--- a/social_mpc/goal.py
+++ b/social_mpc/goal.py
@@ -69,7 +69,7 @@ class GoalFinder():
continue
humans = np.zeros((n_human, 5))
for j in range(n_human):
- humans[j, :] = sh_humans[j, :-1]
+ humans[j, :] = sh_humans[j, :-2]
humans[j, 2] = utils.robot_frame_to_ssn_frame_ang(humans[j, 2])
human_ids = np.array(
sh_humans[sh_humans[:, -1] >= 0, -1],
@@ -98,6 +98,7 @@ class GoalFinder():
# TODO group_id, group center, group members to integrate
if target_id:
goal = ssn.calc_goal_pos(f_dsz, global_map, humans_in_group, group_center=group.center, robot=robot_pose)
+ goal = list(goal)
goal[2] = np.arctan2(goal[1] - humans[idx][1], goal[0] - humans[idx][0])
else:
center = [0., 0.] # TODO
@@ -109,4 +110,4 @@ class GoalFinder():
shared_target[1] = True
shared_target[2] = float(goal[0])
shared_target[3] = float(goal[1])
- shared_target[4] = float(goal[2]) + np.pi/2
\ No newline at end of file
+ shared_target[4] = float(goal[2]) + np.pi/2
--
GitLab