Improve mechanism of saving waypoints

facebookresearch · Jul 27, 2023 · 3e85868 · 3e85868
1 parent a90ba18
commit 3e85868
Show file tree

Hide file tree

Showing 8 changed files with 420 additions and 313 deletions.
diff --git a/README.md b/README.md
@@ -51,8 +51,24 @@ git rev-parse --abbrev-ref HEAD
 git checkout main
 ```
 
+## :light_rail: Try teleoperating the robot using keyboard
+### :rotating_light: Running Emergency Stop
+* Since we do not have a physical emergency stop button (like the large red push buttons), we need to run an e-stop node.
+    ```bash
+    python -m spot_wrapper.estop
+    ```
+
+- Keep this window open at all the times, if the robot starts misbehaving you should be able to quickly press `s` or `space_bar` to kill the robot
+
+### :musical_keyboard: Running keyboard teleop
+* Ensure you have the Estop up and running in one terminal. [Follow these instructions for e-stop](/README.md#rotating_light-running-emergency-stop)
+* Run keyboard teleop with this command in a new terminal
+    ```bash
+    spot_keyboard_teleop
+    ```
+
 ## :video_game: Instructions to record waypoints (use joystick to move robot around)
-- Before running scripts on the robot, waypoints should be recording. These waypoints exist inside file `spot-sim2real/spot_rl_experiments/configs/waypoints.yaml`
+- Before running scripts on the robot, waypoints should be recorded. These waypoints exist inside file `spot-sim2real/spot_rl_experiments/configs/waypoints.yaml`
 
 - Before recording receptacles, make the robot sit at home position then run following command
     ```bash
@@ -75,21 +91,6 @@ git checkout main
 
 
 ## :rocket: Running instructions
-### Running Emergency Stop
-* Since we do not have a physical emergency stop button (like the large red push buttons), we need to run an e-stop node.
-    ```bash
-    python -m spot_wrapper.estop
-    ```
-
-- Keep this window open at all the times, if the robot starts misbehaving you should be able to quickly press `s` or `space_bar` to kill the robot
-
-### Running keyboard teleop
-* Ensure you have the Estop up and running in one terminal. [Follow these instructions for e-stop](/README.md#running-emergency-stop)
-* Run keyboard teleop with this command in a new terminal
-    ```bash
-    spot_keyboard_teleop
-    ```
-
 ### Running the demo (ASC/LSC/Seq-Experts)
 #### Step1. Run the local launch executable
 - In a new terminal, run the executable as
@@ -155,8 +156,8 @@ git checkout main
 
 - The waypoints that were recorded are w.r.t the home location. Since the odometry drifts while robot is moving, **it is necessary to reset home before start of every new run**
 
-### Step4. Emergency stop
-- Follow the steps described in [e-stop section](/README.md#running-emergency-stop)
+#### Step4. Emergency stop
+- Follow the steps described in [e-stop section](/README.md#rotating_light-running-emergency-stop)
 
 
 #### Step5. Main demo code **in a new terminal**

diff --git a/installation/SETUP_INSTRUCTIONS.md b/installation/SETUP_INSTRUCTIONS.md
@@ -200,6 +200,21 @@ echo 'export SPOT_IP=<your-spot-ip>' >> ~/.bashrc
 source ~/.bashrc
 ```
 
+### Testing the setup by running simple navigation policy on robot
+1. Create waypoints.yaml file using the following command
+    ```bash
+    spot_rl_waypoint_recorder -x
+    ```
+2. Follow Steps 1,2,3,4 from [README.md](/README.md#running-the-demo-asclscseq-experts)
+3. Go to root of repo, and run simple command to move robot to a new waypoint using the navigation policy. This command will move robot 2.5m in front after undocking. **Ensure there is 2.5m space in front of dock**
+    ```bash
+    python spot_rl_experiments/spot_rl/envs/nav_env.py -w "test_receptacle"
+    ```
+4. Once the robot has moved, you can dock back the robot with the following command
+    ```bash
+    spot_rl_autodock
+    ```
+
 ### For Meta internal users (with Meta account), please check the following link for the ip and the password
 
 [Link](https://docs.google.com/document/d/1u4x4ZMjHDQi33PB5V2aTZ3snUIV9UdkSOV1zFhRq1Do/edit)

diff --git a/spot_rl_experiments/configs/waypoints_apartment.yaml b/spot_rl_experiments/configs/waypoints_apartment.yaml
diff --git a/spot_rl_experiments/configs/waypoints_microkitchen.yaml b/spot_rl_experiments/configs/waypoints_microkitchen.yaml
diff --git a/spot_rl_experiments/spot_rl/envs/lang_env.py b/spot_rl_experiments/spot_rl/envs/lang_env.py
@@ -19,11 +19,11 @@
 from spot_rl.real_policy import GazePolicy, MixerPolicy, NavPolicy, PlacePolicy
 from spot_rl.utils.remote_spot import RemoteSpot
 from spot_rl.utils.utils import (
-    WAYPOINTS,
     closest_clutter,
     construct_config,
     get_clutter_amounts,
     get_default_parser,
+    get_waypoint_yaml,
     nav_target_from_waypoints,
     object_id_to_nav_waypoint,
     place_target_from_waypoints,
@@ -64,6 +64,9 @@ def main(spot, use_mixer, config, out_path=None):
     # Check if robot should return to base
     return_to_base = config.RETURN_TO_BASE
 
+    # Get the waypoints from waypoints.yaml
+    waypoints = get_waypoint_yaml()
+
     audio_to_text = WhisperTranslator()
     sentence_similarity = SentenceSimilarity()
     with initialize(config_path="../llm/src/conf"):
@@ -86,10 +89,10 @@ def main(spot, use_mixer, config, out_path=None):
 
     # Find closest nav_targets to the ones robot knows locations of
     nav_1 = sentence_similarity.get_most_similar_in_list(
-        nav_1, list(WAYPOINTS["nav_targets"].keys())
+        nav_1, list(waypoints["nav_targets"].keys())
     )
     nav_2 = sentence_similarity.get_most_similar_in_list(
-        nav_2, list(WAYPOINTS["nav_targets"].keys())
+        nav_2, list(waypoints["nav_targets"].keys())
     )
     print("MOST SIMILAR: ", nav_1, pick, nav_2)
 

diff --git a/spot_rl_experiments/spot_rl/envs/mobile_manipulation_env.py b/spot_rl_experiments/spot_rl/envs/mobile_manipulation_env.py
@@ -15,11 +15,11 @@
 from spot_rl.real_policy import GazePolicy, MixerPolicy, NavPolicy, PlacePolicy
 from spot_rl.utils.remote_spot import RemoteSpot
 from spot_rl.utils.utils import (
-    WAYPOINTS,
     closest_clutter,
     construct_config,
     get_clutter_amounts,
     get_default_parser,
+    get_waypoint_yaml,
     nav_target_from_waypoints,
     object_id_to_nav_waypoint,
     place_target_from_waypoints,
@@ -63,9 +63,12 @@ def main(spot, use_mixer, config, out_path=None):
     # Check if robot should return to base
     return_to_base = config.RETURN_TO_BASE
 
+    # Get the waypoints from waypoints.yaml
+    waypoints = get_waypoint_yaml()
+
     objects_to_look = []
-    for waypoint in WAYPOINTS["object_targets"]:
-        objects_to_look.append(WAYPOINTS["object_targets"][waypoint][0])
+    for waypoint in waypoints["object_targets"]:
+        objects_to_look.append(waypoints["object_targets"][waypoint][0])
     rospy.set_param("object_target", ",".join(objects_to_look))
 
     env.power_robot()
@@ -77,7 +80,7 @@ def main(spot, use_mixer, config, out_path=None):
         if trip_idx < NUM_OBJECTS:
             # 2 objects per receptacle
             clutter_blacklist = [
-                i for i in WAYPOINTS["clutter"] if count[i] >= CLUTTER_AMOUNTS[i]
+                i for i in waypoints["clutter"] if count[i] >= CLUTTER_AMOUNTS[i]
             ]
             waypoint_name, waypoint = closest_clutter(
                 env.x, env.y, clutter_blacklist=clutter_blacklist