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

spot_rl_experiments/configs/waypoints_apartment.yaml

@@ -112,6 +112,7 @@ kitchen_island:
 
 
 nav_targets:
+  dock: [1.5, 0.0, 0.0]
   hall_table:
     - 4.973838631994881
     - 3.1629789195672372

spot_rl_experiments/configs/waypoints_microkitchen.yaml

@@ -72,3 +72,31 @@ trash:
 - 6.184285849819101
 - -0.7815312691658676
 - 179.66070876534218
+dock: [1.5, 0.0, 0.0]
+
+nav_targets:
+  dock: [1.5, 0.0, 0.0]
+  table1:
+  - 3.299641079508338
+  - -2.543085839668663
+  - -166.60155928991725
+  couch:
+  - 2.23906958193421
+  - -0.56702771730329
+  - 88.29794273106572
+  table2:
+  - 0.8877704664933876
+  - -5.601562538611034
+  - -149.53198213686227
+  sink:
+  - 6.174181697206729
+  - -3.563298461337695
+  - -3.3523308120884794
+  coffee_counter:
+  - 5.967649279863249
+  - -1.2784570676598292
+  - 2.5817470348872544
+  trash:
+  - 6.184285849819101
+  - -0.7815312691658676
+  - 179.66070876534218

spot_rl_experiments/spot_rl/envs/lang_env.py

@@ -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/spot_rl/envs/mobile_manipulation_env.py

@@ -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/utils/generate_place_goal.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)

spot_rl_experiments/spot_rl/utils/utils.py

@@ -18,15 +18,18 @@
 configs_dir = osp.join(spot_rl_experiments_dir, "configs")
 DEFAULT_CONFIG = osp.join(configs_dir, "config.yaml")
 WAYPOINTS_YAML = osp.join(configs_dir, "waypoints.yaml")
-with open(WAYPOINTS_YAML) as f:
-    WAYPOINTS = yaml.safe_load(f)
 
 ROS_TOPICS = osp.join(configs_dir, "ros_topic_names.yaml")
 ros_topics = CN()
 ros_topics.set_new_allowed(True)
 ros_topics.merge_from_file(ROS_TOPICS)
 
 
+def get_waypoint_yaml(waypoint_file=WAYPOINTS_YAML):
+    with open(waypoint_file) as f:
+        return yaml.safe_load(f)
+
+
 def get_default_parser():
     parser = argparse.ArgumentParser()
     parser.add_argument("-o", "--opts", nargs="*", default=[])
@@ -54,20 +57,23 @@ def construct_config(opts=None):
 
 
 def nav_target_from_waypoints(waypoint):
-    goal_x, goal_y, goal_heading = WAYPOINTS[waypoint]
+    waypoints_yaml = get_waypoint_yaml(WAYPOINTS_YAML)
+    goal_x, goal_y, goal_heading = waypoints_yaml["nav_targets"][waypoint]
     return goal_x, goal_y, np.deg2rad(goal_heading)
 
 
 def place_target_from_waypoints(waypoint):
-    return np.array(WAYPOINTS["place_targets"][waypoint])
+    waypoints_yaml = get_waypoint_yaml(WAYPOINTS_YAML)
+    return np.array(waypoints_yaml["place_targets"][waypoint])
 
 
 def closest_clutter(x, y, clutter_blacklist=None):
+    waypoints_yaml = get_waypoint_yaml(WAYPOINTS_YAML)
     if clutter_blacklist is None:
         clutter_blacklist = []
     clutter_locations = [
         (np.array(nav_target_from_waypoints(w)[:2]), w)
-        for w in WAYPOINTS["clutter"]
+        for w in waypoints_yaml["clutter"]
         if w not in clutter_blacklist
     ]
     xy = np.array([x, y])
@@ -77,23 +83,26 @@ def closest_clutter(x, y, clutter_blacklist=None):
 
 
 def object_id_to_nav_waypoint(object_id):
+    waypoints_yaml = get_waypoint_yaml(WAYPOINTS_YAML)
     if isinstance(object_id, str):
-        for k, v in WAYPOINTS["object_targets"].items():
+        for k, v in waypoints_yaml["object_targets"].items():
             if v[0] == object_id:
                 object_id = int(k)
                 break
         if isinstance(object_id, str):
             KeyError(f"{object_id} not a valid class name!")
-    place_nav_target_name = WAYPOINTS["object_targets"][object_id][1]
+    place_nav_target_name = waypoints_yaml["object_targets"][object_id][1]
     return place_nav_target_name, nav_target_from_waypoints(place_nav_target_name)
 
 
 def object_id_to_object_name(object_id):
-    return WAYPOINTS["object_targets"][object_id][0]
+    waypoints_yaml = get_waypoint_yaml(WAYPOINTS_YAML)
+    return waypoints_yaml["object_targets"][object_id][0]
 
 
 def get_clutter_amounts():
-    return WAYPOINTS["clutter_amounts"]
+    waypoints_yaml = get_waypoint_yaml(WAYPOINTS_YAML)
+    return waypoints_yaml["clutter_amounts"]
 
 
 def arr2str(arr):