Local planner

habitat_sim/nav/greedy_geodesic_follower.py

@@ -5,52 +5,104 @@
 
 from habitat_sim import agent, errors, scene
 from habitat_sim.nav import GreedyFollowerCodes, GreedyGeodesicFollowerImpl, PathFinder
-from habitat_sim.utils.common import quat_to_coeffs
+from habitat_sim.utils.common import quat_to_magnum
 
 
-@attr.s(auto_attribs=True)
+@attr.s(auto_attribs=True, init=False)
 class GreedyGeodesicFollower(object):
-    r"""Greedily fits actions to follow the geodesic shortest path
-
-    :property pathfinder: Instance of the pathfinder that has the correct
-        navmesh already loaded
-    :property agent: Agent to fit actions for. This agent's current
-        configuration is used to specify the actions. The fitted actions will
-        also correspond to keys in the agents action_space. :py:`None` is used
-        to signify that the goal location has been reached
-    :property goal_radius: Specifies how close the agent must get to the goal
-        in order for it to be considered reached.  If :py:`None`, :py:`0.75`
-        times the agents step size is used.
+    r"""Planner that greedily fits actions to follow the geodesic shortest path.
+
+    The planner plans on perfect actions (assumes actuation noise is unbiased) and thus
+    requires the ``move_forward``, ``turn_left``, and ``turn_right`` actions to be
+    present in the agents action space.  If you would like to use different actions
+    (i.e noisy actions), you can override the action key ommited for a given action.
+
+    Planner code heavily inspired by
+    https://github.com/s-gupta/map-plan-baseline
+
     """
 
     pathfinder: PathFinder
     agent: agent.Agent
-    goal_radius: Optional[float] = attr.ib(default=None)
-    action_mapping: Dict[GreedyFollowerCodes, Any] = attr.ib(
-        init=False, factory=dict, repr=False
-    )
-    impl: GreedyGeodesicFollowerImpl = attr.ib(init=False, default=None, repr=False)
-    forward_spec: agent.ActuationSpec = attr.ib(init=False, default=None, repr=False)
-    left_spec: agent.ActuationSpec = attr.ib(init=False, default=None, repr=False)
-    right_spec: agent.ActuationSpec = attr.ib(init=False, default=None, repr=False)
-
-    def __attrs_post_init__(self):
-        self.action_mapping[GreedyFollowerCodes.STOP] = None
+    goal_radius: Optional[float]
+    action_mapping: Dict[GreedyFollowerCodes, Any]
+    impl: GreedyGeodesicFollowerImpl
+    forward_spec: agent.ActuationSpec
+    left_spec: agent.ActuationSpec
+    right_spec: agent.ActuationSpec
+    last_goal: Optional[np.ndarray]
+
+    def __init__(
+        self,
+        pathfinder: PathFinder,
+        agent: agent.Agent,
+        goal_radius: Optional[float] = None,
+        *,
+        stop_key: Optional[Any] = None,
+        forward_key: Optional[Any] = None,
+        left_key: Optional[Any] = None,
+        right_key: Optional[Any] = None,
+        fix_thrashing: bool = True,
+        thrashing_threshold: int = 16,
+    ):
+        r"""Constructor
+
+        :param pathfinder: Instance of the pathfinder that has the correct
+            navmesh already loaded
+        :param agent: Agent to fit actions for. This agent's current
+            configuration is used to specify the actions. The fitted actions will
+            also correspond to keys in the agents action_space. :py:`None` is used
+            to signify that the goal location has been reached
+        :param goal_radius: Specifies how close the agent must get to the goal
+            in order for it to be considered reached.  If :py:`None`, :py:`0.75`
+            times the agents step size is used.
+        :param stop_key: The action key to emit when the agent should stop.
+                            Default :py:`None`
+        :param forward_key: The action key to emit when the agent should
+                               take the move_forward action
+                               Default: The key of the action that calls
+                               the move_forward actuation spec
+        :param left_key: The action key to emit when the agent should
+                               take the turn_left action
+                               Default: The key of the action that calls
+                               the turn_left actuation spec
+        :param right_key: The action key to emit when the agent should
+                               take the turn_right action
+                               Default: The key of the action that calls
+                               the turn_right actuation spec
+        :param fix_thrashing: Whether or not to attempt to fix thrashing
+        :param thrashing_threshold: The number of actions in a left -> right -> left -> ..
+                                       sequence needed to be considered thrashing
+        """
+
+        self.pathfinder = pathfinder
+        self.agent = agent
+        self.last_goal = None
+
+        self.action_mapping = {}
+        self.action_mapping[GreedyFollowerCodes.STOP] = stop_key
 
         key, spec = self._find_action("move_forward")
         self.forward_spec = spec
-        self.action_mapping[GreedyFollowerCodes.FORWARD] = key
+        self.action_mapping[GreedyFollowerCodes.FORWARD] = (
+            key if forward_key is None else forward_key
+        )
 
         key, spec = self._find_action("turn_left")
         self.left_spec = spec
-        self.action_mapping[GreedyFollowerCodes.LEFT] = key
+        self.action_mapping[GreedyFollowerCodes.LEFT] = (
+            key if left_key is None else left_key
+        )
 
         key, spec = self._find_action("turn_right")
         self.right_spec = spec
-        self.action_mapping[GreedyFollowerCodes.RIGHT] = key
+        self.action_mapping[GreedyFollowerCodes.RIGHT] = (
+            key if right_key is None else right_key
+        )
 
-        if self.goal_radius is None:
-            self.goal_radius = 0.75 * self.forward_spec.amount
+        self.goal_radius = (
+            0.75 * self.forward_spec.amount if goal_radius is None else goal_radius
+        )
 
         self.impl = GreedyGeodesicFollowerImpl(
             self.pathfinder,
@@ -60,12 +112,14 @@ def __attrs_post_init__(self):
             self.goal_radius,
             self.forward_spec.amount,
             np.deg2rad(self.left_spec.amount),
+            fix_thrashing,
+            thrashing_threshold,
         )
 
     def _find_action(self, name):
         candidates = list(
             filter(
-                lambda v: v[1].name == name,
+                lambda kv: kv[1].name == name,
                 self.agent.agent_config.action_space.items(),
             )
         )
@@ -76,14 +130,14 @@ def _find_action(self, name):
 
         return candidates[0][0], candidates[0][1].actuation
 
-    def _move_forward(self, obj: scene.SceneNode):
-        self.agent.controls(obj, "move_forward", self.forward_spec, True)
+    def _move_forward(self, obj: scene.SceneNode) -> bool:
+        return self.agent.controls(obj, "move_forward", self.forward_spec, True)
 
-    def _turn_left(self, obj: scene.SceneNode):
-        self.agent.controls(obj, "turn_left", self.left_spec, True)
+    def _turn_left(self, obj: scene.SceneNode) -> bool:
+        return self.agent.controls(obj, "turn_left", self.left_spec, True)
 
-    def _turn_right(self, obj: scene.SceneNode):
-        self.agent.controls(obj, "turn_right", self.right_spec, True)
+    def _turn_right(self, obj: scene.SceneNode) -> bool:
+        return self.agent.controls(obj, "turn_right", self.right_spec, True)
 
     def next_action_along(self, goal_pos: np.ndarray) -> Any:
         r"""Find the next action to greedily follow the geodesic shortest path
@@ -92,9 +146,13 @@ def next_action_along(self, goal_pos: np.ndarray) -> Any:
         :param goal_pos: The position of the goal
         :return: The action to take
         """
+        if self.last_goal is None or not np.allclose(goal_pos, self.last_goal):
+            self.reset()
+            self.last_goal = goal_pos
+
         state = self.agent.state
         next_act = self.impl.next_action_along(
-            state.position, quat_to_coeffs(state.rotation), goal_pos
+            quat_to_magnum(state.rotation), state.position, goal_pos
         )
 
         if next_act == GreedyFollowerCodes.ERROR:
@@ -111,10 +169,18 @@ def find_path(self, goal_pos: np.ndarray) -> List[Any]:
 
         This is roughly equivilent to just calling `next_action_along()` until
         it returns :py:`None`, but is faster.
+
+        .. note-warning::
+
+            Do not use this method if the agent has actuation noise.
+            Instead, use :ref:`next_action_along` to find the action
+            to take in a given state, then take that action, and repeat!
         """
+        self.reset()
+
         state = self.agent.state
         path = self.impl.find_path(
-            state.position, quat_to_coeffs(state.rotation), goal_pos
+            quat_to_magnum(state.rotation), state.position, goal_pos
         )
 
         if len(path) == 0:
@@ -123,3 +189,7 @@ def find_path(self, goal_pos: np.ndarray) -> List[Any]:
         path = list(map(lambda v: self.action_mapping[v], path))
 
         return path
+
+    def reset(self):
+        self.impl.reset()
+        self.last_goal = None

habitat_sim/simulator.py

@@ -6,7 +6,7 @@
 
 import os.path as osp
 import time
-from typing import Dict, List, Optional
+from typing import Any, Dict, List, Optional
 
 import attr
 import magnum as mn
@@ -271,9 +271,28 @@ def step(self, action, dt=1.0 / 60.0):
 
         return observations
 
-    def make_greedy_follower(self, agent_id: int = 0, goal_radius: float = None):
+    def make_greedy_follower(
+        self,
+        agent_id: int = 0,
+        goal_radius: float = None,
+        *,
+        stop_key: Optional[Any] = None,
+        forward_key: Optional[Any] = None,
+        left_key: Optional[Any] = None,
+        right_key: Optional[Any] = None,
+        fix_thrashing: bool = True,
+        thrashing_threshold: int = 16,
+    ):
         return GreedyGeodesicFollower(
-            self.pathfinder, self.get_agent(agent_id), goal_radius
+            self.pathfinder,
+            self.get_agent(agent_id),
+            goal_radius,
+            stop_key=stop_key,
+            forward_key=forward_key,
+            left_key=left_key,
+            right_key=right_key,
+            fix_thrashing=fix_thrashing,
+            thrashing_threshold=thrashing_threshold,
         )
 
     def _step_filter(self, start_pos, end_pos):

src/esp/bindings/PhysicsBindings.cpp

@@ -16,14 +16,6 @@ void initPhysicsBindings(py::module& m) {
       .value("KINEMATIC", MotionType::KINEMATIC)
       .value("DYNAMIC", MotionType::DYNAMIC);
 
-  // ==== struct RigidState ===
-  py::class_<RigidState, RigidState::ptr>(m, "RigidState")
-      .def(py::init(&RigidState::create<>))
-      .def(py::init(&RigidState::create<const Magnum::Quaternion&,
-                                        const Magnum::Vector3&>))
-      .def_readwrite("rotation", &RigidState::rotation)
-      .def_readwrite("translation", &RigidState::translation);
-
   // ==== struct object VelocityControl ====
   py::class_<VelocityControl, VelocityControl::ptr>(m, "VelocityControl")
       .def(py::init(&VelocityControl::create<>))

src/esp/bindings/ShortestPathBindings.cpp

@@ -16,6 +16,7 @@
 #include "esp/scene/ObjectControls.h"
 
 namespace py = pybind11;
+namespace Mn = Magnum;
 
 using py::literals::operator""_a;
 
@@ -121,19 +122,30 @@ void initShortestPathBindings(py::module& m) {
 
   py::class_<GreedyGeodesicFollowerImpl, GreedyGeodesicFollowerImpl::ptr>(
       m, "GreedyGeodesicFollowerImpl")
-      .def(py::init(
-          &GreedyGeodesicFollowerImpl::create<
-              PathFinder::ptr&, GreedyGeodesicFollowerImpl::MoveFn&,
-              GreedyGeodesicFollowerImpl::MoveFn&,
-              GreedyGeodesicFollowerImpl::MoveFn&, double, double, double>))
+      .def(py::init(&GreedyGeodesicFollowerImpl::create<
+                    PathFinder::ptr&, GreedyGeodesicFollowerImpl::MoveFn&,
+                    GreedyGeodesicFollowerImpl::MoveFn&,
+                    GreedyGeodesicFollowerImpl::MoveFn&, double, double, double,
+                    bool, int>))
       .def("next_action_along",
-           py::overload_cast<const vec3f&, const vec4f&, const vec3f&>(
+           py::overload_cast<const Mn::Quaternion&, const Mn::Vector3&,
+                             const Mn::Vector3&>(
+               &GreedyGeodesicFollowerImpl::nextActionAlong),
+           py::return_value_policy::move)
+      .def("next_action_along",
+           py::overload_cast<const core::RigidState&, const Mn::Vector3&>(
                &GreedyGeodesicFollowerImpl::nextActionAlong),
            py::return_value_policy::move)
       .def("find_path",
-           py::overload_cast<const vec3f&, const vec4f&, const vec3f&>(
+           py::overload_cast<const Mn::Quaternion&, const Mn::Vector3&,
+                             const Mn::Vector3&>(
                &GreedyGeodesicFollowerImpl::findPath),
-           py::return_value_policy::move);
+           py::return_value_policy::move)
+      .def("find_path",
+           py::overload_cast<const core::RigidState&, const Mn::Vector3&>(
+               &GreedyGeodesicFollowerImpl::findPath),
+           py::return_value_policy::move)
+      .def("reset", &GreedyGeodesicFollowerImpl::reset);
 }
 
 }  // namespace nav

src/esp/bindings/bindings.cpp

@@ -5,6 +5,7 @@
 #include "esp/bindings/bindings.h"
 
 #include "esp/core/Configuration.h"
+#include "esp/core/RigidState.h"
 
 namespace py = pybind11;
 using py::literals::operator""_a;
@@ -38,6 +39,14 @@ void initCoreBindings(py::module& m) {
       .def("get_string_group", &Configuration::getStringGroup)
       .def("has_value", &Configuration::hasValue)
       .def("remove_value", &Configuration::removeValue);
+
+  // ==== struct RigidState ===
+  py::class_<RigidState, RigidState::ptr>(m, "RigidState")
+      .def(py::init(&RigidState::create<>))
+      .def(py::init(&RigidState::create<const Magnum::Quaternion&,
+                                        const Magnum::Vector3&>))
+      .def_readwrite("rotation", &RigidState::rotation)
+      .def_readwrite("translation", &RigidState::translation);
 }
 
 }  // namespace core

src/esp/core/RigidState.h

@@ -0,0 +1,30 @@
+// Copyright (c) Facebook, Inc. and its affiliates.
+// This source code is licensed under the MIT license found in the
+// LICENSE file in the root directory of this source tree.
+
+#pragma once
+
+#include <Magnum/Magnum.h>
+#include <Magnum/Math/Quaternion.h>
+
+#include "esp.h"
+
+namespace esp {
+namespace core {
+/**
+ * @brief describes the state of a rigid object as a composition of rotation
+ * (quaternion) and translation.
+ */
+struct RigidState {
+  RigidState(){};
+  RigidState(const Magnum::Quaternion& _rotation,
+             const Magnum::Vector3& _translation)
+      : rotation(_rotation), translation(_translation){};
+
+  Magnum::Quaternion rotation;
+  Magnum::Vector3 translation;
+
+  ESP_SMART_POINTERS(RigidState)
+};
+};  // namespace core
+};  // namespace esp