diff --git a/social_mpc/goal.py b/social_mpc/goal.py index 562678145679247dbebad98c78aa44c324fe4231..93a66398f9b598c32a7712783f015a1edcfc1e23 100644 --- a/social_mpc/goal.py +++ b/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)) diff --git a/social_mpc/ssn_model/social_spaces.py b/social_mpc/ssn_model/social_spaces.py index c5274335821da0aea01e79e8e9102cb3903c655a..f282093143e14ee3db84da3d76c73b3bc9ef88cd 100644 --- a/social_mpc/ssn_model/social_spaces.py +++ b/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)