def get_waypoint_dist_dir(self):
angle = get_direction(self.pose6D.squeeze(),
self.waypointPose6D.squeeze(), 0, 0)
dist = get_distance(self.pose6D.squeeze(),
self.waypointPose6D.squeeze())
return torch.cat(
[
dist.view(1, 1),
torch.sin(angle).view(1, 1),
torch.cos(angle).view(1, 1),
],
dim=1,
)
def planner_prediction_to_command(self, p_next):
command = SimulatorActions.STOP.value
p_init = self.pose6D.squeeze()
d_angle_rot_th = self.angle_th
pos_th = self.pos_th
if get_distance(p_init, p_next) <= pos_th:
return command
d_angle = angle_to_pi_2_minus_pi_2(
get_direction(p_init, p_next, ang_th=d_angle_rot_th,
pos_th=pos_th))
if abs(d_angle) < d_angle_rot_th:
command = SimulatorActions.FORWARD.value
else:
if (d_angle > 0) and (d_angle < pi):
command = SimulatorActions.LEFT.value
elif d_angle > pi:
command = SimulatorActions.RIGHT.value
elif (d_angle < 0) and (d_angle > -pi):
command = SimulatorActions.RIGHT.value
else:
command = SimulatorActions.LEFT.value
return command
def planner_prediction_to_command(self, p_next):
command = HabitatSimActions.STOP
p_init = self.pose6D.squeeze()
d_angle_rot_th = self.angle_th
pos_th = self.pos_th
if get_distance(p_init, p_next) <= pos_th:
return command
d_angle = angle_to_pi_2_minus_pi_2(
get_direction(p_init, p_next, ang_th=d_angle_rot_th, pos_th=pos_th)
)
if abs(d_angle) < d_angle_rot_th:
command = HabitatSimActions.MOVE_FORWARD
else:
if (d_angle > 0) and (d_angle < pi):
command = HabitatSimActions.TURN_LEFT
elif d_angle > pi:
command = HabitatSimActions.TURN_RIGHT
elif (d_angle < 0) and (d_angle > -pi):
command = HabitatSimActions.TURN_RIGHT
else:
command = HabitatSimActions.TURN_LEFT
return command
