Improve mechanism of saving waypoints

Author: KavitShah1998

README.md

@@ -52,7 +52,7 @@ git checkout main
 ```
 
 ## :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
@@ -155,7 +155,7 @@ 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
+#### Step4. Emergency stop
 - Follow the steps described in [e-stop section](/README.md#running-emergency-stop)
 
 

installation/SETUP_INSTRUCTIONS.md

@@ -193,7 +193,20 @@ source ~/.bash_profile
 
 For assistance with finding the right ip of your computer, [please follow these steps](/installation/ISSUES.md#how-to-find-ip-address-of-local-computer).
 
-
+### 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)

spot_rl_experiments/configs/waypoints_apartment.yaml

@@ -20,11 +20,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,
@@ -65,6 +65,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"):
@@ -87,10 +90,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)
 

spot_rl_experiments/configs/waypoints_microkitchen.yaml

@@ -16,11 +16,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,9 +64,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()
@@ -78,7 +81,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

spot_rl_experiments/spot_rl/envs/lang_env.py

@@ -16,6 +16,8 @@ def get_global_place_target(spot: Spot):
     position, rotation = spot.get_base_transform_to("link_wr1")
     position = [position.x, position.y, position.z]
     rotation = [rotation.x, rotation.y, rotation.z, rotation.w]
+
+    # Spot2Habitat transform SHOULD NOT BE A PART OF SpotBaseEnv Class. It is an unnecessary dependency.
     wrist_T_base = SpotBaseEnv.spot2habitat_transform(position, rotation)
     gripper_T_base = wrist_T_base @ mn.Matrix4.translation(
         mn.Vector3(EE_GRIPPER_OFFSET)