np.flip and np.pi/2 removed

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
 

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

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,

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