Changes 2.0

spot_rl_experiments/spot_rl/envs/lang_env.py

@@ -26,7 +26,7 @@
     get_waypoint_yaml,
     nav_target_from_waypoint,
     object_id_to_nav_waypoint,
-    place_target_from_waypoints,
+    place_target_from_waypoint,
 )
 from spot_rl.utils.whisper_translator import WhisperTranslator
 from spot_wrapper.spot import Spot
@@ -321,7 +321,9 @@ def step(self, base_action, arm_action, *args, **kwargs):
             waypoint = nav_target_from_waypoint(waypoint_name, get_waypoint_yaml())
 
             self.say("Navigating to " + waypoint_name)
-            self.place_target = place_target_from_waypoints(waypoint_name)
+            self.place_target = place_target_from_waypoint(
+                waypoint_name, get_waypoint_yaml()
+            )
             self.goal_xy, self.goal_heading = (waypoint[:2], waypoint[2])
             self.navigating_to_place = True
             info["grasp_success"] = True

spot_rl_experiments/spot_rl/envs/mobile_manipulation_env.py

@@ -22,7 +22,7 @@
     get_waypoint_yaml,
     nav_target_from_waypoint,
     object_id_to_nav_waypoint,
-    place_target_from_waypoints,
+    place_target_from_waypoint,
 )
 from spot_wrapper.spot import Spot
 
@@ -323,7 +323,9 @@ def step(self, base_action, arm_action, *args, **kwargs):
             self.say("Navigating to " + waypoint_name)
             rospy.set_param("viz_object", self.target_obj_name)
             rospy.set_param("viz_place", waypoint_name)
-            self.place_target = place_target_from_waypoints(waypoint_name)
+            self.place_target = place_target_from_waypoint(
+                waypoint_name, get_waypoint_yaml()
+            )  # can you make this cleaner????????
             self.goal_xy, self.goal_heading = (waypoint[:2], waypoint[2])
             self.navigating_to_place = True
             info["grasp_success"] = True

spot_rl_experiments/spot_rl/envs/place_env.py

@@ -10,10 +10,12 @@
 import numpy as np
 from spot_rl.envs.base_env import SpotBaseEnv
 from spot_rl.real_policy import PlacePolicy
+from spot_rl.utils.generate_place_goal import get_global_place_target
 from spot_rl.utils.utils import (
     construct_config,
     get_default_parser,
-    place_target_from_waypoints,
+    get_waypoint_yaml,
+    place_target_from_waypoint,
 )
 from spot_wrapper.spot import Spot
 
@@ -29,8 +31,8 @@ def parse_arguments(args=sys.argv[1:]):
     )
     parser.add_argument(
         "-w",
-        "--waypoint",
-        help="input:string -> waypoint (place_target name) where robot needs to place the object",
+        "--waypoints",
+        help="input:string -> place target waypoints (comma separated place_target names) where robot needs to place the object",
     )
     parser.add_argument(
         "-l",
@@ -69,15 +71,35 @@ def __init__(self, config, spot: Spot):
         self.place_env = SpotPlaceEnv(config, spot)
         self.place_env.power_robot()
 
-    def execute(self, place_target, is_local=False):
-        observations = self.place_env.reset(place_target, is_local)
-        done = False
-
-        while not done:
-            action = self.policy.act(observations)
-            observations, _, done, _ = self.place_env.step(arm_action=action)
-
-        return done
+    def execute(self, place_target_list, is_local=False):
+        success_list = []
+        for place_target in place_target_list:
+            # input("Press enter to continue1")
+            observations = self.place_env.reset(place_target, is_local)
+            # input("Press enter to continue2")
+            done = False
+            start_time = time.time()
+
+            while not done:
+                action = self.policy.act(observations)
+                observations, _, done, _ = self.place_env.step(arm_action=action)
+
+            # Get end effector position
+            ee_pos = get_global_place_target(self.spot)
+            print(ee_pos)
+            print(place_target)
+
+            success_list.append(
+                {
+                    "place_object": "place_target",
+                    "time_taken": time.time() - start_time,
+                    "success": self.place_env.place_attempted,
+                    "place_target": place_target,
+                    "ee_pos": ee_pos,
+                }
+            )
+
+        return success_list
 
     def shutdown(self, should_dock=False):
         try:
@@ -97,22 +119,6 @@ def shutdown(self, should_dock=False):
             self.spot.power_off()
 
 
-def main(spot):
-    args = parse_arguments()
-    config = construct_config_for_place(opts=args.opts)
-
-    if args.waypoint is not None:
-        assert not args.target_is_local
-        place_target = place_target_from_waypoints(args.waypoint)
-    else:
-        assert args.place_target is not None
-        place_target = [float(i) for i in args.place_target.split(",")]
-
-    place_controller = PlaceController(config, spot)
-    place_controller.execute(place_target, args.target_is_local)
-    place_controller.shutdown(False)
-
-
 class SpotPlaceEnv(SpotBaseEnv):
     def __init__(self, config, spot: Spot):
         super().__init__(config, spot)
@@ -155,6 +161,37 @@ def get_observations(self):
 
 
 if __name__ == "__main__":
+    args = parse_arguments()
+    config = construct_config_for_place(opts=args.opts)
+    waypoints_yaml_dict = get_waypoint_yaml()
+
+    # Get place_target_list (list) to go and pick from
+    place_target_list = None
+    if args.waypoints is not None:
+        assert not args.target_is_local
+        waypoints = [
+            waypoint
+            for waypoint in args.waypoints.replace(" ,", ",")
+            .replace(", ", ",")
+            .split(",")
+            if waypoint.strip() is not None
+        ]
+        place_target_list = [
+            place_target_from_waypoint(waypoint, waypoints_yaml_dict)
+            for waypoint in waypoints
+        ]
+    else:
+        assert args.place_target is not None
+        place_target_list = [[float(i) for i in args.place_target.split(",")]]
+
     spot = Spot("RealPlaceEnv")
     with spot.get_lease(hijack=True):
-        main(spot)
+        place_controller = PlaceController(config, spot)
+        try:
+            place_result = place_controller.execute(
+                place_target_list, args.target_is_local
+            )
+        finally:
+            place_controller.shutdown(False)
+
+    print(f"Place results - {place_result}")

spot_rl_experiments/spot_rl/utils/utils.py

@@ -72,9 +72,19 @@ def nav_target_from_waypoint(waypoint, waypoints_yaml):
     return goal_x, goal_y, np.deg2rad(goal_heading)
 
 
-def place_target_from_waypoints(waypoint):
-    waypoints_yaml = get_waypoint_yaml(WAYPOINTS_YAML)
-    return np.array(waypoints_yaml["place_targets"][waypoint])
+def place_target_from_waypoint(waypoint, waypoints_yaml):
+    waypoints_yaml_place_target_dict = waypoints_yaml.get("place_targets")
+    if waypoints_yaml_place_target_dict is None:
+        raise Exception(
+            "No `place_targets` found in waypoints.yaml. Please construct waypoints.yaml correctly as per the README.md"
+        )
+
+    place_target = waypoints_yaml_place_target_dict.get(waypoint)
+    if place_target is None:
+        raise Exception(
+            f"Requested waypoint - {waypoint} does not exist inside `place_targets` in file waypoints.yaml. Please construct waypoints.yaml correctly as per the README.md"
+        )
+    return np.array(place_target)
 
 
 def closest_clutter(x, y, clutter_blacklist=None):

tests/hardware_tests/test_place_env.py

@@ -0,0 +1,125 @@
+# Copyright (c) Meta Platforms, 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.
+
+
+import os
+from typing import Dict, List
+
+import numpy as np
+import pytest
+from spot_rl.envs.place_env import PlaceController, construct_config_for_place
+from spot_rl.utils.utils import get_waypoint_yaml, place_target_from_waypoint
+from spot_wrapper.spot import Spot
+
+hardware_tests_dir = os.path.dirname(os.path.abspath(__file__))
+test_configs_dir = os.path.join(hardware_tests_dir, "configs")
+test_data_dir = os.path.join(hardware_tests_dir, "data")
+test_nav_trajectories_dir = os.path.join(test_data_dir, "nav_trajectories")
+test_square_nav_trajectories_dir = os.path.join(
+    test_nav_trajectories_dir, "square_of_side_200cm"
+)
+TEST_WAYPOINTS_YAML = os.path.join(test_configs_dir, "waypoints.yaml")
+TEST_CONFIGS_YAML = os.path.join(test_configs_dir, "config.yaml")
+
+
+def init_config():
+    """
+    Initialize config object for Nav test
+
+    Returns:
+        config: Config object
+    """
+    config = construct_config_for_place(file_path=TEST_CONFIGS_YAML, opts=[])
+
+    return config
+
+
+def test_place():
+    config = init_config()
+    test_waypoints_yaml_dict = get_waypoint_yaml(waypoint_file=TEST_WAYPOINTS_YAML)
+
+    test_waypoints = [
+        "test_cup",
+        "test_plush_lion",
+        "test_plush_ball",
+    ]
+    test_place_targets_list = [
+        place_target_from_waypoint(test_waypoint, test_waypoints_yaml_dict)
+        for test_waypoint in test_waypoints
+    ]
+
+    test_spot = Spot("PlaceEnvHardwareTest")
+    test_DATA = None  # TODO: Rename to something useful after defining return type of the method
+    with test_spot.get_lease(hijack=True):
+        place_controller = PlaceController(config=config, spot=test_spot)
+
+        try:
+            test_DATA = place_controller.execute(
+                place_target_list=test_place_targets_list
+            )
+        except Exception:
+            pytest.fails(
+                "Pytest raised an error while executing PlaceController.execute from test_place_env.py"
+            )
+        finally:
+            place_controller.shutdown(should_dock=True)
+
+    assert test_DATA is not []
+    assert len(test_DATA) == len(test_waypoints)
+
+    # ref_traj_set = load_json_files(test_square_nav_trajectories_dir)
+
+    # avg_time_list_traj, std_time_list_traj = compute_avg_and_std_time(ref_traj_set)
+    # avg_steps_list_traj, std_steps_list_traj = compute_avg_and_std_steps(ref_traj_set)
+    # (
+    #     test_pose_list,
+    #     test_time_list,
+    #     test_steps_list,
+    # ) = extract_goal_poses_timestamps_steps_from_traj(test_traj)
+
+    # print(f"Dataset: Avg. time to reach each waypoint - {avg_time_list_traj}")
+    # print(f"Dataset: Std.Dev in time reach each waypoint - {std_time_list_traj}")
+    # print(f"Test-Nav: Time taken to reach each waypoint - {test_time_list}\n")
+
+    # print(f"Dataset: Avg. steps to reach each waypoint - {avg_steps_list_traj}")
+    # print(f"Dataset: Std.Dev in steps to reach each waypoint - {std_steps_list_traj}")
+    # print(f"Test-Nav: Steps taken to reach each waypoint - {test_steps_list}\n")
+
+    # print(f"Test-Nav: Pose at each of the goal waypoint - {test_pose_list}\n")
+
+    # allowable_std_dev_in_time = 3.0
+    # allowable_std_dev_in_steps = 3.0
+    # for wp_idx in range(len(test_waypoints)):
+    #     # Capture target pose for each waypoint
+    #     target_pose = list(
+    #         nav_target_from_waypoint(test_waypoints[wp_idx], test_waypoints_yaml_dict)
+    #     )
+    #     target_pose[-1] = np.rad2deg(target_pose[-1])
+    #     # Test that robot reached its goal successfully spatially
+    #     assert (
+    #         is_pose_within_bounds(
+    #             test_pose_list[wp_idx],
+    #             target_pose,
+    #             config.SUCCESS_DISTANCE,
+    #             config.SUCCESS_ANGLE_DIST,
+    #         )
+    #         is True
+    #     )
+
+    #     # Test that robot reached its goal successfully temporally (within 1 std dev of mean)
+    #     assert (
+    #         abs(test_time_list[wp_idx] - avg_time_list_traj[wp_idx])
+    #         < allowable_std_dev_in_time * std_time_list_traj[wp_idx]
+    #     )
+
+    #     # Test that test trajectory took similar amount of steps to finish execution
+    #     assert (
+    #         abs(test_steps_list[wp_idx] - avg_steps_list_traj[wp_idx])
+    #         < allowable_std_dev_in_steps * std_steps_list_traj[wp_idx]
+    #     )
+
+    # # Report DTW scores
+    # dtw_score_list = compute_dtw_scores(test_traj, ref_traj_set)
+    # print(f"DTW scores: {dtw_score_list}")