def _est_max_grad_dir(self, goal_pos: np.array) -> np.array:
current_state = self._sim.get_agent_state()
current_pos = current_state.position
if self.mode == "geodesic_path":
points = self._sim.get_straight_shortest_path_points(
self._sim.get_agent_state().position, goal_pos)
# Add a little offset as things get weird if
# points[1] - points[0] is anti-parallel with forward
if len(points) < 2:
return None
max_grad_dir = quaternion_from_two_vectors(
self._sim.forward_vector,
points[1] - points[0] + EPSILON *
np.cross(self._sim.up_vector, self._sim.forward_vector),
)
max_grad_dir.x = 0
max_grad_dir = np.normalized(max_grad_dir)
else:
current_rotation = self._sim.get_agent_state().rotation
current_dist = self._geo_dist(goal_pos)
best_geodesic_delta = -2 * self._max_delta
best_rotation = current_rotation
for _ in range(0, 360, self._sim.config.TURN_ANGLE):
sim_action = SIM_NAME_TO_ACTION[SimulatorActions.FORWARD.value]
self._sim.step(sim_action)
new_delta = current_dist - self._geo_dist(goal_pos)
if new_delta > best_geodesic_delta:
best_rotation = self._sim.get_agent_state().rotation
best_geodesic_delta = new_delta
# If the best delta is within (1 - cos(TURN_ANGLE))% of the
# best delta (the step size), then we almost certainly have
# found the max grad dir and should just exit
if np.isclose(
best_geodesic_delta,
self._max_delta,
rtol=1 -
np.cos(np.deg2rad(self._sim.config.TURN_ANGLE)),
):
break
self._sim.set_agent_state(
current_pos,
self._sim.get_agent_state().rotation,
reset_sensors=False,
)
sim_action = SIM_NAME_TO_ACTION[SimulatorActions.LEFT.value]
self._sim.step(sim_action)
self._reset_agent_state(current_state)
max_grad_dir = quaternion_xyzw_to_wxyz(best_rotation)
return max_grad_dir
def check_state(agent_state, position, rotation):
assert np.allclose(
agent_state.rotation, quaternion_xyzw_to_wxyz(
rotation)), "Agent's rotation diverges from the shortest path."
assert np.allclose(
agent_state.position, position
), "Agent's position position diverges from the shortest path's one."
def _step_along_grad(
self,
grad_dir: np.quaternion) -> Union[SimulatorActions, np.array]:
current_state = self._sim.get_agent_state()
alpha = angle_between_quaternions(
grad_dir, quaternion_xyzw_to_wxyz(current_state.rotation))
if alpha <= np.deg2rad(self._sim.config.TURN_ANGLE) + EPSILON:
return self._get_return_value(SimulatorActions.FORWARD)
else:
sim_action = SIM_NAME_TO_ACTION[SimulatorActions.LEFT.value]
self._sim.step(sim_action)
best_turn = (SimulatorActions.LEFT if (angle_between_quaternions(
grad_dir,
quaternion_xyzw_to_wxyz(self._sim.get_agent_state().rotation),
) < alpha) else SimulatorActions.RIGHT)
self._reset_agent_state(current_state)
return self._get_return_value(best_turn)