From 2ddd24d2c98e71647fa3566b1e8544e679fce535 Mon Sep 17 00:00:00 2001
From: Alex AUTERNAUD <alex.auternaud@inria.fr>
Date: Fri, 26 May 2023 16:37:39 +0200
Subject: [PATCH] np.flip and np.pi/2 removed
---
social_mpc/controller.py | 2 --
social_mpc/goal.py | 9 ++++-----
social_mpc/path.py | 16 ++++++++--------
social_mpc/ssn_model/social_spaces.py | 2 +-
4 files changed, 13 insertions(+), 16 deletions(-)
diff --git a/social_mpc/controller.py b/social_mpc/controller.py
index ecaff16..348b199 100644
--- a/social_mpc/controller.py
+++ b/social_mpc/controller.py
@@ -510,7 +510,6 @@ class RobotController():
global_position)
local_angle = state.humans_pose[i, -
1] - state.robot_pose[-1]
- local_angle = robot_frame_to_ssn_frame_ang(local_angle)
humans[j, :] = * \
local_position, local_angle, state.humans_velocity[i, 0], 0
if n_human > 1:
@@ -523,7 +522,6 @@ class RobotController():
break
f_dsz = ssn.calc_dsz(humans, groups=groups)
- f_dsz = np.flip(f_dsz, axis=0)
rospy.logdebug("Social cost map time : {0}".format(time.time() - tic))
return f_dsz
diff --git a/social_mpc/goal.py b/social_mpc/goal.py
index 8c46600..4ef866c 100644
--- a/social_mpc/goal.py
+++ b/social_mpc/goal.py
@@ -42,7 +42,7 @@ class GoalFinder():
last_time = time.time()
while flags[0]:
with lock:
- global_map = np.flip(np.array([*shared_global_map]), axis=0).squeeze()
+ global_map = (np.array([*shared_global_map])).squeeze()
if self.goal_loop_time:
new_time = time.time()
if (new_time - last_time) < self.goal_loop_time:
@@ -67,7 +67,6 @@ class GoalFinder():
humans = np.zeros((n_human, sh_humans.shape[1]))
for j in range(n_human):
humans[j, :] = sh_humans[j, :]
- humans[j, 2] = utils.robot_frame_to_ssn_frame_ang(humans[j, 2])
human_ids = np.array(sh_humans[sh_humans[:, -1] >= 0, -1], dtype=int).tolist()
n_group = np.sum(sh_groups[:, -1] >= 0)
@@ -113,7 +112,7 @@ class GoalFinder():
group_center=center,
robot=robot_pose)
goal = list(goal)
- goal[2] = np.arctan2(goal[1] - humans[target[1]][1], goal[0] - humans[target[1]][0]) # to face the human
+ goal[2] = np.arctan2(humans[target[1]][1] - goal[1], humans[target[1]][0] - goal[0]) # to face the human
elif target[0] == 'isolated_human':
person = humans[target[1], :]
ts_x = person[0] + np.cos(person[2]) * stride
@@ -127,7 +126,7 @@ class GoalFinder():
group_center=center,
robot=robot_pose)
goal = list(goal)
- goal[2] = np.arctan2(goal[1] - humans[target[1]][1], goal[0] - humans[target[1]][0]) # to face the human
+ goal[2] = np.arctan2(humans[target[1]][1] - goal[1],humans[target[1]][0] - goal[0]) # to face the human
elif target[0] == 'group':
center = [sh_groups[target[1], 0], sh_groups[target[1], 1]]
persons = humans[humans[:, -2] == target[2], :]
@@ -148,4 +147,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
+ shared_target[4] = float(goal[2])
diff --git a/social_mpc/path.py b/social_mpc/path.py
index b27b99f..e506f6d 100644
--- a/social_mpc/path.py
+++ b/social_mpc/path.py
@@ -35,9 +35,10 @@ class PathPlanner():
scale = self.state_config.global_map_scale
x_min = self.state_config.global_map_size[0][0]
y_max = self.state_config.global_map_size[1][1]
+ y_min = self.state_config.global_map_size[1][0]
- i = (y_max - pos[1]) * scale
- j = (pos[0] - x_min) * scale
+ j = (-y_min + pos[1]) * scale
+ i = (pos[0] - x_min) * scale
return int(i), int(j)
def px_coord_grid(self):
@@ -60,11 +61,10 @@ class PathPlanner():
self.distance = skfmm.distance(phi, dx=dx)
def get_next_waypoint(self, pos):
- #x, y = self.px_coord_grid()
x, y = self.ssn.x_coordinates, self.ssn.y_coordinates
dx = 1/self.state_config.global_map_scale
xl, yl, vxl, vyl, dl = utils.optimal_path_2d(
- np.flip(self.distance, axis=0), pos[:2],
+ self.distance, pos[:2],
dx=dx,
coords=(x, y),
#max_distance=self.max_d)
@@ -72,17 +72,17 @@ class PathPlanner():
if len(xl) == 0 or len(yl) == 0 or len(vxl) == 0 or len(vyl) == 0:
return []
- d_angle = utils.constraint_angle(np.arctan2(vxl[0], -vyl[0]) - pos[2])
+ d_angle = utils.constraint_angle(np.arctan2(vyl[0] - pos[1], vxl[0] - pos[0]) - pos[2])
if dl[0] < self.max_d_orientation: # if within range, set waypoint = target
wpt_pos = [self.target[0], self.target[1], self.target[2]]
elif dl[0] < self.max_d:
wpt_pos = [self.target[0],
self.target[1],
- np.arctan2(vxl[0], -vyl[0])]
+ np.arctan2(vyl[0] - pos[1], vxl[0] - pos[0])]
elif np.abs(d_angle) > self.max_target_angle:
- wpt_pos = [xl[0], yl[0], np.arctan2(vxl[0], -vyl[0])]
+ wpt_pos = [xl[0], yl[0], np.arctan2(vyl[0] - pos[1], vxl[0] - pos[0])]
else: # else, point on shortest path with orientation towards target
- wpt_pos = [xl[-1], yl[-1], np.arctan2(vxl[-1], -vyl[-1])]
+ wpt_pos = [xl[-1], yl[-1], np.arctan2(vyl[-1] - pos[1], vxl[-1] - pos[0])]
return wpt_pos
def run(self,
diff --git a/social_mpc/ssn_model/social_spaces.py b/social_mpc/ssn_model/social_spaces.py
index f282093..f035d6c 100644
--- a/social_mpc/ssn_model/social_spaces.py
+++ b/social_mpc/ssn_model/social_spaces.py
@@ -313,5 +313,5 @@ class SocialSpaces():
circle_dsz = dsz_interp.ev(circle_pts[:,1], circle_pts[:,0])
idx = np.argmin(circle_dsz)
x, y = circle_pts[idx, 0], circle_pts[idx, 1]
- theta_q = np.arctan2(y - group_center[1], x - group_center[0])
+ theta_q = np.arctan2(group_center[1] - y, group_center[0] - x)
return x, y, theta_q
\ No newline at end of file
--
GitLab