diff --git a/social_mpc/config/config.py b/social_mpc/config/config.py
index 466066acd6520aeaddc9817e0615e053985f3d29..8b071919aad1199525a7defe2deceb065ac03a3d 100644
--- a/social_mpc/config/config.py
+++ b/social_mpc/config/config.py
@@ -58,7 +58,8 @@ class ControllerConfig(TemplatedConfig):
"goto_target_dim": int,
"human_target_dim": int,
"pan_target_dim": int,
- "target_pose_dim": int,
+ "target_pose_goto_dim": int,
+ "target_pose_look_dim": int,
"goto_target_id": int,
"goto_target_pose": (np.array, (5,)),
"human_target_id": int,
diff --git a/social_mpc/config/social_mpc.yaml b/social_mpc/config/social_mpc.yaml
index 1cea6cd5f4abbc5462466d7a55b5094bd6260da3..c8c94a312b13bdbc9c111f17dc68b7f22f4f5a85 100644
--- a/social_mpc/config/social_mpc.yaml
+++ b/social_mpc/config/social_mpc.yaml
@@ -11,7 +11,8 @@ max_iter_optim: 5
goto_target_dim: 8 # x, y, ang, v, w, loss, angle flag, flag
human_target_dim: 8 # x, y, ang, v, w, loss, angle flag, flag
pan_target_dim: 5 # x, y, v, w, flag
-target_pose_dim: 5 # x, y, ang, local/global flag, flag
+target_pose_goto_dim: 5 # x, y, ang, local/global flag, flag
+target_pose_look_dim: 4 # x, y, local/global flag, flag
goto_target_id: 14 # id
goto_target_pose: [0., 0., 0., 0., 0.] # pose (x, y, ang),local 1./global 0. flag, flag
human_target_id: -1
diff --git a/social_mpc/config_rl/social_mpc.yaml b/social_mpc/config_rl/social_mpc.yaml
index 12c898fa4cbc281dd7a6be3f4ec10d4c7cfe41fe..3466d2c56608c445ddd454a4682ad001b94180b3 100644
--- a/social_mpc/config_rl/social_mpc.yaml
+++ b/social_mpc/config_rl/social_mpc.yaml
@@ -11,7 +11,8 @@ max_iter_optim: 5
goto_target_dim: 8 # x, y, ang, v, w, loss, angle flag, flag
human_target_dim: 8 # x, y, ang, v, w, loss, angle flag, flag
pan_target_dim: 5 # x, y, v, w, flag
-target_pose_dim: 5 # x, y, ang, local/global flag, flag
+target_pose_goto_dim: 5 # x, y, ang, local/global flag, flag
+target_pose_look_dim: 4 # x, y, local/global flag, flag
goto_target_id: 14 # id
goto_target_pose: [0., 0., 0., 0., 0.] # pose (x, y, ang),local 1./global 0. flag, flag
human_target_id: -1
diff --git a/social_mpc/controller.py b/social_mpc/controller.py
index 7114f504254996f2aa17fb6355dfb007d140d304..5f6a81c8e146c5e0f12760527e5ab4a75d13e719 100644
--- a/social_mpc/controller.py
+++ b/social_mpc/controller.py
@@ -47,6 +47,8 @@ class RobotController():
self.goto_target = False
self.pan_target_id = None
+ self.pan_target_flag = False
+ self.pan_target_pose = None
self.human_target_id = None
self.relative_h_pose = None
self.global_pose_h = None
@@ -212,6 +214,7 @@ class RobotController():
self.shared_robot_pose = list_type([0.0, 0.0, 0.0])
self.shared_goto_target_human_id = list_type([False, 0])
+
def reset_shared_values(self):
with self.lock:
self.shared_waypoint[0] = False
@@ -317,7 +320,7 @@ class RobotController():
self.shared_goto_target_human_id[0] = False
self.shared_goto_target_human_id[1] = 0
- elif len(target) == self.controller_config.target_pose_dim and (
+ 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]:
@@ -353,15 +356,28 @@ class RobotController():
def set_pan_target(self, state, target):
self.pan_target_id = None
+ self.pan_target_pose = None
+ self.pan_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.pan_target_id = target
+ self.pan_target_flag = True
else:
logger.warning('Target id does not exist for pan target')
self.is_failure = True
self.controller_config.pan_target_id = -1
+ elif len(target) == self.controller_config.target_pose_look_dim and (
+ isinstance(target, np.ndarray) or all(
+ isinstance(i, (int, float)) for i in target)):
+ self.controller_config.pan_target_id = -1
+ if target[-1]:
+ self.pan_target_flag = True
+ if target[-2]:
+ self.pan_target_pose = target[:2]
+ else:
+ self.pan_target_pose = utils.global_to_local(state.robot_pose, target[:2])
else:
logger.info('Human target not valid')
self.is_failure = True
@@ -587,7 +603,7 @@ class RobotController():
self.human_features[-2] = 1
self.last_human_features = self.human_features
- if self.mode == 3 or self.pan_target_id is not None:
+ if self.mode == 3 or self.pan_target_flag:
self.pan_goal = np.zeros(self.controller_config.pan_target_dim)
humans_id = state.humans_id.tolist()
if self.pan_target_id in humans_id:
@@ -599,6 +615,8 @@ class RobotController():
self.pan_goal[:] = [
*pos, *vel, 0]
self.pan_goal[-1] = 1
+ if self.pan_target_pose is not None:
+ self.pan_goal[:] = [*self.pan_target_pose, 0, 0, 1]
def set_mode(self):
self.mode = None
@@ -609,7 +627,7 @@ class RobotController():
self.mode = 2 # escort
elif self.human_target_id is not None:
self.mode = 1 # follow
- elif self.pan_target_id is not None:
+ elif self.pan_target_flag:
self.mode = 3 # just look at
else:
logger.warning('No mode was chosen, nothing to do')
@@ -628,7 +646,7 @@ class RobotController():
self.loss_coef = config.loss_coef[self.mode]
self.loss_rad = config.loss_rad[self.mode]
self.reg_parameter = config.reg_parameter.copy()
- elif self.pan_target_id is not None:
+ elif self.pan_target_flag:
# if only pan target id is set, load the join mode weights
for i, weight_name in enumerate(self.weights_description):
self.weights[i] = config.__getattribute__(
@@ -661,7 +679,7 @@ class RobotController():
self.update_goals(state)
if self.do_step():
- if self.pan_target_id != -1 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 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,