From 22bf29aef65d566a5f7159e0b5c7117ba80c6434 Mon Sep 17 00:00:00 2001
From: Alex AUTERNAUD <alex.auternaud@inria.fr>
Date: Wed, 24 Aug 2022 15:44:03 +0200
Subject: [PATCH] group_detector to MotorController beginning (should not break
the current way of controlling the robot)
---
social_mpc/controller.py | 38 +++++++++++++++++++++++++++++++-------
social_mpc/goal.py | 37 +++++++++++++++++++++++++------------
2 files changed, 56 insertions(+), 19 deletions(-)
diff --git a/social_mpc/controller.py b/social_mpc/controller.py
index 5f6a81c..3a9124a 100644
--- a/social_mpc/controller.py
+++ b/social_mpc/controller.py
@@ -45,7 +45,7 @@ class RobotController():
self.shared_global_map = None
self.shared_humans = None
- self.goto_target = False
+ self.goto_target_flag = False
self.pan_target_id = None
self.pan_target_flag = False
self.pan_target_pose = None
@@ -196,7 +196,7 @@ class RobotController():
# 6 = pos + angle + velocity + id
if (state.humans_pose.shape[0] > 0):
- humans = np.zeros((state.humans_pose.shape[0], 6))
+ humans = np.zeros((state.humans_pose.shape[0], 7)) # x, y, orientation, lin vel, ang vel, group id, id
self.shm_humans = self.smm.SharedMemory(size=humans.nbytes)
self.shared_humans = np.ndarray(shape=humans.shape,
dtype=humans.dtype,
@@ -213,6 +213,7 @@ class RobotController():
0.0]) # theta
self.shared_robot_pose = list_type([0.0, 0.0, 0.0])
self.shared_goto_target_human_id = list_type([False, 0])
+ self.shared_goto_target_group_id = list_type([False, 0])
def reset_shared_values(self):
@@ -228,6 +229,8 @@ class RobotController():
self.shared_target[4] = 0.0
self.shared_goto_target_human_id[0] = False
self.shared_goto_target_human_id[1] = 0
+ self.shared_goto_target_group_id[0] = False
+ self.shared_goto_target_group_id[1] = 0
config = self.controller_config
self.goto_goal = np.zeros(config.goto_target_dim)
self.pan_goal = np.zeros(config.pan_target_dim)
@@ -280,6 +283,7 @@ class RobotController():
idx = np.where(state.humans_visible == 0)[0]
n = len(idx)
self.shared_humans[:n, -1] = state.humans_id[idx]
+ self.shared_humans[:n, -2] = state.humans_group_id[idx]
self.shared_humans[:n, :3] = state.humans_pose[idx, :]
self.shared_humans[:n, 3:5] = state.humans_velocity[idx, :]
# if self.shared_goto_target_human_id[0]:
@@ -294,6 +298,7 @@ class RobotController():
def set_target_pose(self, pose):
with self.lock:
self.shared_goto_target_human_id[0] = False # disable joining a person
+ self.shared_goto_target_group_id[0] = False # disable joining a group
self.shared_target[1] = True
self.shared_target[2] = float(pose[0])
self.shared_target[3] = float(pose[1])
@@ -304,14 +309,23 @@ class RobotController():
self.shared_goto_target_human_id[0] = True
self.shared_goto_target_human_id[1] = human_id
+ def set_target_group_id(self, group_id):
+ with self.lock:
+ self.shared_goto_target_group_id[0] = True
+ self.shared_goto_target_group_id[1] = group_id
+
def set_goto_target(self, state, target):
- self.goto_target = False
+ self.goto_target_flag = False
self.is_failure = False
if isinstance(target, int):
if len(state.humans_id) > 0 and target in state.humans_id:
if target >= 0:
- self.goto_target = True
+ self.goto_target_flag = True
self.set_target_human_id(target)
+ elif len(state.humans_group_id) > 0 and target in state.humans_group_id:
+ if target >= 0:
+ self.goto_target_flag = True
+ self.set_target_group_id(target)
else:
logger.warning('Target id does not exist for goto target')
self.is_failure = True
@@ -319,12 +333,14 @@ class RobotController():
with self.lock:
self.shared_goto_target_human_id[0] = False
self.shared_goto_target_human_id[1] = 0
+ self.shared_goto_target_group_id[0] = False
+ self.shared_goto_target_group_id[1] = 0
elif len(target) == self.controller_config.target_pose_goto_dim and (
isinstance(target, np.ndarray) or all(
isinstance(i, (int, float)) for i in target)):
if target[-1]:
- self.goto_target = True
+ self.goto_target_flag = True
if target[-2]:
new_target = [*utils.local_to_global(state.robot_pose, target[:2]), target[2]]
self.set_target_pose(new_target)
@@ -337,6 +353,8 @@ class RobotController():
with self.lock:
self.shared_goto_target_human_id[0] = False
self.shared_goto_target_human_id[1] = 0
+ self.shared_goto_target_group_id[0] = False
+ self.shared_goto_target_group_id[1] = 0
def set_human_target(self, state, target):
self.human_target_id = None
@@ -364,6 +382,11 @@ class RobotController():
if target >= 0:
self.pan_target_id = target
self.pan_target_flag = True
+ elif len(state.humans_group_id) > 0 and target in state.humans_group_id:
+ if target >= 0:
+ pass # to integrate look at group
+ # self.pan_target_id = target
+ # self.pan_target_flag = True
else:
logger.warning('Target id does not exist for pan target')
self.is_failure = True
@@ -421,6 +444,7 @@ class RobotController():
self.shared_target,
self.shared_robot_pose,
self.shared_goto_target_human_id,
+ self.shared_goto_target_group_id,
self.shared_global_map,
self.flags,
self.lock))
@@ -620,7 +644,7 @@ class RobotController():
def set_mode(self):
self.mode = None
- if self.goto_target:
+ if self.goto_target_flag:
if self.human_target_id is None:
self.mode = 0 # join
else:
@@ -679,7 +703,7 @@ class RobotController():
self.update_goals(state)
if self.do_step():
- if not self.pan_target_flag and not self.goto_target and (self.human_target_id == -1 or self.human_target_id == None):
+ if not self.pan_target_flag and not self.goto_target_flag and (self.human_target_id == -1 or self.human_target_id == None):
self.reg_parameter[-1] = 100000
self.actions = self.mpc.step(state=state.joint_angles,
actions=self.actions + self.initial_action,
diff --git a/social_mpc/goal.py b/social_mpc/goal.py
index 303e47d..674cba4 100644
--- a/social_mpc/goal.py
+++ b/social_mpc/goal.py
@@ -38,6 +38,7 @@ class GoalFinder():
shared_target,
shared_robot_pose,
shared_goto_target_human_id,
+ shared_goto_target_group_id,
shared_global_map,
flags,
lock):
@@ -55,41 +56,53 @@ class GoalFinder():
with lock:
robot_pose = (shared_robot_pose[0], shared_robot_pose[1])
sh_goto_target_human_id = np.array([*shared_goto_target_human_id])
+ sh_goto_target_group_id = np.array([*shared_goto_target_group_id])
if shared_humans is not None:
sh_humans = np.array([*shared_humans])
else:
continue
- if not sh_goto_target_human_id[0]:
+ if not sh_goto_target_human_id[0] and not sh_goto_target_group_id[0]:
logger.debug("No target set")
continue
n_human = np.sum(sh_humans[:, -1] >= 0)
+ if n_human < 1:
+ continue
humans = np.zeros((n_human, 5))
for j in range(n_human):
- humans[j, :] = sh_humans[j, :5]
+ humans[j, :] = sh_humans[j, :-1]
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()
logger.debug("human_ids : {}, {}, {}".format(human_ids, humans, n_human))
- target_id = sh_goto_target_human_id[1]
- if target_id not in human_ids:
- logger.warning("Target human not in sight")
- continue
+ target_id = None
+
+ if sh_goto_target_human_id[0]:
+ target_id = sh_goto_target_human_id[1]
+ if target_id not in human_ids:
+ logger.warning("Target human not in sight")
+ continue
+ if sh_goto_target_group_id[0]:
+ pass
- if n_human >= 1:
+ groups = gd.gcff.identify_groups(humans, stride=self.controller_config.group_detection_stride)
+ if target_id:
idx = human_ids.index(target_id)
- groups = gd.gcff.identify_groups(humans, stride=self.controller_config.group_detection_stride)
for group in groups:
if idx in group.person_ids:
group = group
break
humans_in_group = humans[group.person_ids, :]
- else:
- groups = None
- humans_in_group = humans
f_dsz = ssn.calc_dsz(humans, groups=groups)
- goal = ssn.calc_goal_pos(f_dsz, global_map, humans_in_group, group_center=group.center, robot=robot_pose)
+ # 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[2] = np.arctan2(goal[1] - humans[idx][1], goal[0] - humans[idx][0])
+ else:
+ center = [0., 0.] # TODO
+ humans_in_group = humans[:, :] # TODO
+ goal = ssn.calc_goal_pos(f_dsz, global_map, humans_in_group, group_center=center, robot=robot_pose)
logger.debug("goal : {}".format(goal))
with lock:
--
GitLab