[hab3_merge] Add Skills for the robot to move forward/backward and tu…

…rn (#1508)

* Add Skills to move forward/backward and turn

* update skills

* fix pytest

habitat-baselines/habitat_baselines/config/default_structured_configs.py

@@ -259,6 +259,8 @@ class HrlDefinedSkillConfig(HabitatBaselinesBaseConfig):
     # map to this skill. If not specified,the name of the skill must match the
     # PDDL action name.
     pddl_action_names: Optional[List[str]] = None
+    turn_power_x: float = 0.0
+    turn_power_y: float = 0.0
 
 
 @dataclass

habitat-baselines/habitat_baselines/config/habitat_baselines/rl/policy/hierarchical_policy/defined_skills/oracle_skills.yaml

@@ -67,3 +67,32 @@ reset_arm:
   max_skill_steps: 50
   reset_joint_state: [-4.50e-01, -1.07e00, 9.95e-02, 9.38e-01, -7.88e-04, 1.57e00, 4.62e-03]
   force_end_on_timeout: False
+
+
+turn_left:
+  skill_name: "MoveSkillPolicy"
+  force_end_on_timeout: False
+  max_skill_steps: 1
+  turn_power_y: 1.0
+  apply_postconds: True
+
+turn_right:
+  skill_name: "MoveSkillPolicy"
+  force_end_on_timeout: False
+  max_skill_steps: 1
+  turn_power_y: -1.0
+  apply_postconds: True
+
+move_forward:
+  skill_name: "MoveSkillPolicy"
+  force_end_on_timeout: False
+  max_skill_steps: 1
+  turn_power_x: 1.0
+  apply_postconds: True
+
+move_backward:
+  skill_name: "MoveSkillPolicy"
+  force_end_on_timeout: False
+  max_skill_steps: 1
+  turn_power_x: -1.0
+  apply_postconds: True

habitat-baselines/habitat_baselines/rl/hrl/skills/__init__.py

@@ -3,6 +3,7 @@
 # LICENSE file in the root directory of this source tree.
 
 from habitat_baselines.rl.hrl.skills.art_obj import ArtObjSkillPolicy
+from habitat_baselines.rl.hrl.skills.ll_nav import MoveSkillPolicy
 from habitat_baselines.rl.hrl.skills.nav import NavSkillPolicy
 from habitat_baselines.rl.hrl.skills.nn_skill import NnSkillPolicy
 from habitat_baselines.rl.hrl.skills.noop import NoopSkillPolicy
@@ -15,6 +16,7 @@
 
 __all__ = [
     "ArtObjSkillPolicy",
+    "MoveSkillPolicy",
     "NavSkillPolicy",
     "NnSkillPolicy",
     "OracleNavPolicy",

habitat-baselines/habitat_baselines/rl/hrl/skills/ll_nav.py

@@ -0,0 +1,41 @@
+import torch
+
+from habitat_baselines.rl.hrl.skills.skill import SkillPolicy
+from habitat_baselines.rl.hrl.utils import find_action_range
+from habitat_baselines.rl.ppo.policy import PolicyActionData
+
+
+class MoveSkillPolicy(SkillPolicy):
+    def __init__(
+        self,
+        config,
+        action_space,
+        batch_size,
+    ):
+        super().__init__(config, action_space, batch_size, True)
+        self._turn_power_fwd = config.turn_power_x
+        self._turn_power_side = config.turn_power_y
+        self._nav_ac_start, _ = find_action_range(
+            action_space, "base_velocity"
+        )
+
+    def _internal_act(
+        self,
+        observations,
+        rnn_hidden_states,
+        prev_actions,
+        masks,
+        cur_batch_idx,
+        full_action,
+        deterministic=False,
+    ):
+        full_action = torch.zeros(
+            (masks.shape[0], self._full_ac_size), device=masks.device
+        )
+        if self._turn_power_fwd != 0:
+            full_action[:, self._nav_ac_start] = self._turn_power_fwd
+        if self._turn_power_side != 0:
+            full_action[:, self._nav_ac_start + 1] = self._turn_power_side
+        return PolicyActionData(
+            actions=full_action, rnn_hidden_states=rnn_hidden_states
+        )