debugging isolated_human

social_mpc/goal.py

@@ -127,11 +127,14 @@ class GoalFinder():
                 goal[2] = np.arctan2(goal[1] - humans[target[1]][1], goal[0] - humans[target[1]][0])  # to face the human
                 print('goal human_in_group after {}'.format(goal))
             elif target[0] == 'isolated_human':
+                print('target', target)
                 person = humans[target[1], :]
                 ts_x = person[0] + np.cos(person[2]) * stride
                 ts_y = person[1] + np.sin(person[2]) * stride
                 center = [ts_x, ts_y]
-                print('isolated_human : center {}, persons{}'.format(center, persons))
+                person = person[np.newaxis, :]
+                print('isolated_human : center {}, person{}'.format(center, person))
+                print(person.shape)
                 goal = ssn.calc_goal_pos(
                     f_dsz=f_dsz,
                     map=global_map,
@@ -143,6 +146,7 @@ class GoalFinder():
                 goal[2] = np.arctan2(goal[1] - humans[target[1]][1], goal[0] - humans[target[1]][0])  # to face the human
                 print('goal isolated_human after {}'.format(goal))
             elif target[0] == 'group':
+                print('target', target)
                 center = [sh_groups[target[1], 0], sh_groups[target[1], 1]]
                 persons = humans[humans[:, -2] == target[1], :]
                 print('group : center {}, persons{}'.format(center, persons))

social_mpc/ssn_model/social_spaces.py

@@ -284,7 +284,11 @@ class SocialSpaces():
         r_max = 5
         n_points = 100
         r_step = 1.1
-        r = self.calc_circle_radius(group_center, persons)
+        if persons.shape[0] == 1:
+            # single person
+            r = self.config.r_p_0
+        else:
+            r = self.calc_circle_radius(group_center, persons)
 
         dsz_interp = interpolate.RectBivariateSpline(self.y_coordinates, self.x_coordinates, f_dsz)
         map_interp = interpolate.RectBivariateSpline(self.y_coordinates, self.x_coordinates, map)