diff --git a/predicators/spot_utils/README.txt b/predicators/spot_utils/README.txt new file mode 100644 index 0000000000..e69de29bb2 diff --git a/predicators/spot_utils/example_skills.py b/predicators/spot_utils/example_skills.py new file mode 100644 index 0000000000..54271adff9 --- /dev/null +++ b/predicators/spot_utils/example_skills.py @@ -0,0 +1,40 @@ +# MOVE TO LOCATION +# X,Y in map, Theta is direction of robot (in radians) +move_hand_to_relative_pose(robot, se3_pose(x, y, z, quaternion)) # x is forward from robot, y is right from robot, and z is up from robot +get_grasp_pixel(robot, ) -> returns pixel to grasp +grasp_at_pixel(robot, rgbds[hand_camera], pixel) -> grasps at pixel + +# MOVE HAND TO RELATIVE POSE +target_pos = math_helpers.Vec3(0.8, 0.0, 0.25) +downward_angle = np.pi / 2.5 +target_pos = math_helpers.Vec3(0.8, 0.0, 0.25) +rot = math_helpers.Quat.from_pitch(downward_angle) +body_tform_goal = math_helpers.SE3Pose(x=target_pos.x, y=target_pos.y, z=target_pos.z, rot=rot) +move_hand_to_relative_pose(robot, body_tform_goal) +# move_hand_to_relative_pose(robot, math_helpers.SE3Pose(0.753, 0.018, 0.89, math_helpers.Quat(0.6870, 0.0664, 0.7200, -0.0722))) +# time.sleep(0.5) + +# GRASP + +# GET PIXEL FROM USER +pixel = get_pixel_from_user(rgbds[hand_camera].rgb) + +# GET PIXEL FROM OBJECT DETECTION +rgbds = capture_images(robot, localizer, TEST_CAMERAS) +language_ids: List[ObjectDetectionID] = [ + LanguageObjectDetectionID(d) for d in TEST_LANGUAGE_DESCRIPTIONS +] +hand_camera = "hand_color_image" +detections, artifacts = detect_objects(language_ids, rgbds) + +detections_outfile = Path(".") / "object_detection_artifacts.png" +no_detections_outfile = Path(".") / "no_detection_artifacts.png" +visualize_all_artifacts(artifacts, detections_outfile, + no_detections_outfile) + +pixel, _ = get_grasp_pixel(rgbds, artifacts, language_ids[-1], + hand_camera, rng) + +grasp_at_pixel(robot, rgbds[hand_camera], pixel) +stow_arm(robot) +time.sleep(0.5) \ No newline at end of file diff --git a/predicators/spot_utils/no_detection_artifacts.png b/predicators/spot_utils/no_detection_artifacts.png deleted file mode 100644 index 5644ab86af..0000000000 Binary files a/predicators/spot_utils/no_detection_artifacts.png and /dev/null differ diff --git a/predicators/spot_utils/object_detection_artifacts.png b/predicators/spot_utils/object_detection_artifacts.png deleted file mode 100644 index d670983415..0000000000 Binary files a/predicators/spot_utils/object_detection_artifacts.png and /dev/null differ diff --git a/predicators/spot_utils/perception/demo/no_detections_image_2024-05-29 21:29:34.094149.png b/predicators/spot_utils/perception/demo/no_detections_image_2024-05-29 21:29:34.094149.png new file mode 100644 index 0000000000..1ea10770bf Binary files /dev/null and b/predicators/spot_utils/perception/demo/no_detections_image_2024-05-29 21:29:34.094149.png differ diff --git a/predicators/spot_utils/perception/demo/no_detections_image_2024-05-29 21:30:33.354505.png b/predicators/spot_utils/perception/demo/no_detections_image_2024-05-29 21:30:33.354505.png new file mode 100644 index 0000000000..54a21d82b1 Binary files /dev/null and b/predicators/spot_utils/perception/demo/no_detections_image_2024-05-29 21:30:33.354505.png differ diff --git a/predicators/spot_utils/perception/demo/state_2024-05-29 21:29:34.094149.txt b/predicators/spot_utils/perception/demo/state_2024-05-29 21:29:34.094149.txt new file mode 100644 index 0000000000..331394a117 --- /dev/null +++ b/predicators/spot_utils/perception/demo/state_2024-05-29 21:29:34.094149.txt @@ -0,0 +1,2 @@ +Robot pose: position -- X: 2.759 Y: 0.978 Yaw: 92.6 deg +Robot hand pose: position -- X: 0.833 Y: 0.150 Z: 0.499 rotation -- W: 0.6156 X: -0.4840 Y: 0.4909 Z: 0.3817 diff --git a/predicators/spot_utils/perception/demo/state_2024-05-29 21:30:33.354505.txt b/predicators/spot_utils/perception/demo/state_2024-05-29 21:30:33.354505.txt new file mode 100644 index 0000000000..3aed7ec083 --- /dev/null +++ b/predicators/spot_utils/perception/demo/state_2024-05-29 21:30:33.354505.txt @@ -0,0 +1,2 @@ +Robot pose: position -- X: 2.560 Y: 0.922 Yaw: 92.6 deg +Robot hand pose: position -- X: 0.644 Y: -0.325 Z: 0.250 rotation -- W: 0.7085 X: -0.2046 Y: 0.6750 Z: 0.0231 diff --git a/predicators/spot_utils/perception/demos/plant-tall/plant1.txt b/predicators/spot_utils/perception/demos/plant-tall/plant1.txt deleted file mode 100644 index 9090e666ed..0000000000 --- a/predicators/spot_utils/perception/demos/plant-tall/plant1.txt +++ /dev/null @@ -1,2 +0,0 @@ -Detected LanguageID(potted plant) at position -- X: 0.758 Y: 0.380 Z: 0.135 rotation -- W: 1.0000 X: 0.0000 Y: 0.0000 Z: 0.0000 -Robot pose: position -- X: 3.998 Y: -0.338 Z: 0.111 rotation -- W: -0.2548 X: -0.0013 Y: -0.0016 Z: 0.9670 diff --git a/predicators/spot_utils/perception/demos/plant-tall/plant2.txt b/predicators/spot_utils/perception/demos/plant-tall/plant2.txt deleted file mode 100644 index ceaa7784ee..0000000000 --- a/predicators/spot_utils/perception/demos/plant-tall/plant2.txt +++ /dev/null @@ -1,2 +0,0 @@ -Detected LanguageID(potted plant) at position -- X: 0.816 Y: 0.319 Z: 0.132 rotation -- W: 1.0000 X: 0.0000 Y: 0.0000 Z: 0.0000 -Robot pose: position -- X: 2.896 Y: -0.858 Z: 0.111 rotation -- W: -0.2481 X: -0.0018 Y: -0.0012 Z: 0.9687 diff --git a/predicators/spot_utils/perception/demos/plant-tall/plant3.txt b/predicators/spot_utils/perception/demos/plant-tall/plant3.txt deleted file mode 100644 index 99cea72d16..0000000000 --- a/predicators/spot_utils/perception/demos/plant-tall/plant3.txt +++ /dev/null @@ -1,2 +0,0 @@ -Detected LanguageID(potted plant) at position -- X: 0.806 Y: 0.279 Z: 0.136 rotation -- W: 1.0000 X: 0.0000 Y: 0.0000 Z: 0.0000 -Robot pose: position -- X: 1.915 Y: -0.453 Z: 0.123 rotation -- W: 0.2849 X: -0.0024 Y: -0.0008 Z: -0.9585 diff --git a/predicators/spot_utils/perception/no_detection_artifacts.png b/predicators/spot_utils/perception/no_detection_artifacts.png deleted file mode 100644 index c0b5997c19..0000000000 Binary files a/predicators/spot_utils/perception/no_detection_artifacts.png and /dev/null differ diff --git a/predicators/spot_utils/perception/object_detection.py b/predicators/spot_utils/perception/object_detection.py index 277867b04b..b52b14bcef 100644 --- a/predicators/spot_utils/perception/object_detection.py +++ b/predicators/spot_utils/perception/object_detection.py @@ -691,7 +691,7 @@ def display_camera_detections(artifacts: Dict[str, Any], TEST_LANGUAGE_DESCRIPTIONS = [ "potted plant", "green apple/tennis ball", - "water bottle" + "elephant watering can" ] def _run_manual_test() -> None: @@ -729,20 +729,24 @@ def _run_manual_test() -> None: snapshot = robot.get_frame_tree_snapshot() hand_in_body = get_a_tform_b(snapshot, BODY_FRAME_NAME, "hand") - with open(f"state_{datetime.now()}.txt", 'w') as file: + save_path = Path("./demo") + save_path.mkdir(parents=True, exist_ok=True) + date = datetime.now() + + with open(f"demo/state_{date}.txt", 'w') as file: for obj_id, detection in detections.items(): - file.write(f"Detected {obj_id} at {detection}\n") + file.write(f"Object {obj_id} at {detection}\n") file.write(f"Robot pose: {localizer.get_last_robot_pose().get_closest_se2_transform()}\n") - file.write(f"Hand pose: {hand_in_body}\n") + file.write(f"Robot hand pose: {hand_in_body}\n") for obj_id, detection in detections.items(): - print(f"Detected {obj_id} at {detection}") + print(f"Object {obj_id} at {detection}") print(f"Robot pose: {localizer.get_last_robot_pose().get_closest_se2_transform()}") - print(f"Hand pose: {hand_in_body}") + print(f"Robot hand pose: {hand_in_body}") # Visualize the artifacts. - detections_outfile = Path(".") / "object_detection_artifacts.png" - no_detections_outfile = Path(".") / "no_detection_artifacts.png" + detections_outfile = f"./demo/object_detections_image_{date}.png" + no_detections_outfile = f"./demo/no_detections_image_{date}.png" visualize_all_artifacts(artifacts, detections_outfile, no_detections_outfile) diff --git a/predicators/spot_utils/perception/object_detection_artifacts.png b/predicators/spot_utils/perception/object_detection_artifacts.png deleted file mode 100644 index ee8cf81483..0000000000 Binary files a/predicators/spot_utils/perception/object_detection_artifacts.png and /dev/null differ diff --git a/predicators/spot_utils/perception/plant-short/state_2024-05-21 18:26:35.545072.txt b/predicators/spot_utils/perception/plant-short/state_2024-05-21 18:26:35.545072.txt deleted file mode 100644 index e77e9bd424..0000000000 --- a/predicators/spot_utils/perception/plant-short/state_2024-05-21 18:26:35.545072.txt +++ /dev/null @@ -1,3 +0,0 @@ -Detected LanguageID(potted plant) at position -- X: 0.220 Y: -0.410 Z: 0.179 rotation -- W: 1.0000 X: 0.0000 Y: 0.0000 Z: 0.0000 -Robot pose: position -- X: 3.858 Y: -0.051 Z: 0.151 rotation -- W: -0.1692 X: -0.0010 Y: -0.0006 Z: 0.9856 -Hand pose: position -- X: 3.306 Y: -0.049 Z: -0.069 rotation -- W: -0.1694 X: -0.0076 Y: 0.0005 Z: 0.9855 diff --git a/predicators/spot_utils/perception/plant-short/state_2024-05-21 18:27:25.276281.txt b/predicators/spot_utils/perception/plant-short/state_2024-05-21 18:27:25.276281.txt deleted file mode 100644 index 18d998448c..0000000000 --- a/predicators/spot_utils/perception/plant-short/state_2024-05-21 18:27:25.276281.txt +++ /dev/null @@ -1,3 +0,0 @@ -Detected LanguageID(potted plant) at position -- X: 0.923 Y: -0.274 Z: 0.134 rotation -- W: 1.0000 X: 0.0000 Y: 0.0000 Z: 0.0000 -Robot pose: position -- X: 2.464 Y: -0.468 Z: 0.123 rotation -- W: -0.4515 X: -0.0017 Y: -0.0006 Z: 0.8923 -Hand pose: position -- X: -0.145 Y: -0.394 Z: 1.741 rotation -- W: -0.3294 X: -0.7136 Y: 0.3309 Z: 0.5222 diff --git a/predicators/spot_utils/perception/plant-short/state_2024-05-21 18:28:31.174064.txt b/predicators/spot_utils/perception/plant-short/state_2024-05-21 18:28:31.174064.txt deleted file mode 100644 index 133bc6b175..0000000000 --- a/predicators/spot_utils/perception/plant-short/state_2024-05-21 18:28:31.174064.txt +++ /dev/null @@ -1,3 +0,0 @@ -Detected LanguageID(potted plant) at position -- X: 0.980 Y: -0.268 Z: -0.131 rotation -- W: 1.0000 X: 0.0000 Y: 0.0000 Z: 0.0000 -Robot pose: position -- X: 1.852 Y: -0.317 Z: 0.029 rotation -- W: 0.2490 X: 0.0311 Y: 0.0058 Z: -0.9680 -Hand pose: position -- X: 0.075 Y: 0.240 Z: 0.962 rotation -- W: 0.2966 X: 0.6067 Y: -0.3155 Z: -0.6666 diff --git a/predicators/spot_utils/perception/plant-tall/state_2024-05-21 18:20:55.519938.txt b/predicators/spot_utils/perception/plant-tall/state_2024-05-21 18:20:55.519938.txt deleted file mode 100644 index 4d88d9f178..0000000000 --- a/predicators/spot_utils/perception/plant-tall/state_2024-05-21 18:20:55.519938.txt +++ /dev/null @@ -1,3 +0,0 @@ -Detected LanguageID(potted plant) at position -- X: 0.994 Y: -0.582 Z: 0.143 rotation -- W: 1.0000 X: 0.0000 Y: 0.0000 Z: 0.0000 -Robot pose: position -- X: 3.652 Y: 0.111 Z: -0.088 rotation -- W: -0.1436 X: -0.0009 Y: -0.0005 Z: 0.9896 -Hand pose: position -- X: 3.104 Y: 0.112 Z: -0.307 rotation -- W: -0.1438 X: -0.0079 Y: 0.0005 Z: 0.9896 diff --git a/predicators/spot_utils/perception/plant-tall/state_2024-05-21 18:21:36.705204.txt b/predicators/spot_utils/perception/plant-tall/state_2024-05-21 18:21:36.705204.txt deleted file mode 100644 index 05975568aa..0000000000 --- a/predicators/spot_utils/perception/plant-tall/state_2024-05-21 18:21:36.705204.txt +++ /dev/null @@ -1,3 +0,0 @@ -Detected LanguageID(potted plant) at position -- X: 0.504 Y: 1.954 Z: 0.843 rotation -- W: 1.0000 X: 0.0000 Y: 0.0000 Z: 0.0000 -Robot pose: position -- X: 2.543 Y: -0.501 Z: 0.124 rotation -- W: -0.2786 X: -0.0007 Y: -0.0019 Z: 0.9604 -Hand pose: position -- X: 1.992 Y: -0.501 Z: -0.111 rotation -- W: -0.2787 X: -0.0075 Y: 0.0002 Z: 0.9604 diff --git a/predicators/spot_utils/perception/plant-tall/state_2024-05-21 18:23:43.159150.txt b/predicators/spot_utils/perception/plant-tall/state_2024-05-21 18:23:43.159150.txt deleted file mode 100644 index 75638582ca..0000000000 --- a/predicators/spot_utils/perception/plant-tall/state_2024-05-21 18:23:43.159150.txt +++ /dev/null @@ -1,3 +0,0 @@ -Detected LanguageID(potted plant) at position -- X: 0.501 Y: 2.020 Z: 0.769 rotation -- W: 1.0000 X: 0.0000 Y: 0.0000 Z: 0.0000 -Robot pose: position -- X: 1.910 Y: -0.327 Z: 0.189 rotation -- W: 0.3682 X: -0.0260 Y: -0.0118 Z: -0.9293 -Hand pose: position -- X: -0.155 Y: 0.098 Z: 1.046 rotation -- W: 0.2218 X: 0.6180 Y: -0.4710 Z: -0.5891 diff --git a/predicators/spot_utils/perception/state_2024-05-28 11:56:45.948311.txt b/predicators/spot_utils/perception/state_2024-05-28 11:56:45.948311.txt deleted file mode 100644 index 6545602430..0000000000 --- a/predicators/spot_utils/perception/state_2024-05-28 11:56:45.948311.txt +++ /dev/null @@ -1,4 +0,0 @@ -Detected LanguageID(potted plant) at position -- X: 2.763 Y: -1.996 Z: 0.569 rotation -- W: 1.0000 X: 0.0000 Y: 0.0000 Z: 0.0000 -Detected LanguageID(green apple/tennis ball) at position -- X: 5.595 Y: -1.316 Z: -0.349 rotation -- W: 1.0000 X: 0.0000 Y: 0.0000 Z: 0.0000 -Robot pose: position -- X: 2.163 Y: 0.360 Yaw: -74.0 deg -Hand pose: position -- X: 0.554 Y: 0.000 Z: 0.263 rotation -- W: 1.0000 X: 0.0004 Y: 0.0078 Z: 0.0003 diff --git a/predicators/spot_utils/perception/state_2024-05-28 11:57:08.547399.txt b/predicators/spot_utils/perception/state_2024-05-28 11:57:08.547399.txt deleted file mode 100644 index f7c0663912..0000000000 --- a/predicators/spot_utils/perception/state_2024-05-28 11:57:08.547399.txt +++ /dev/null @@ -1,5 +0,0 @@ -Detected LanguageID(potted plant) at position -- X: 2.749 Y: -1.895 Z: 0.190 rotation -- W: 1.0000 X: 0.0000 Y: 0.0000 Z: 0.0000 -Detected LanguageID(water bottle) at position -- X: 3.002 Y: -2.249 Z: 0.260 rotation -- W: 1.0000 X: 0.0000 Y: 0.0000 Z: 0.0000 -Detected LanguageID(green apple/tennis ball) at position -- X: 5.473 Y: -1.264 Z: -0.561 rotation -- W: 1.0000 X: 0.0000 Y: 0.0000 Z: 0.0000 -Robot pose: position -- X: 3.271 Y: -0.077 Yaw: -78.6 deg -Hand pose: position -- X: 0.554 Y: 0.000 Z: 0.263 rotation -- W: 1.0000 X: 0.0004 Y: 0.0078 Z: 0.0002 diff --git a/predicators/spot_utils/perception/state_2024-05-28 11:58:00.452985.txt b/predicators/spot_utils/perception/state_2024-05-28 11:58:00.452985.txt deleted file mode 100644 index 6a947bae97..0000000000 --- a/predicators/spot_utils/perception/state_2024-05-28 11:58:00.452985.txt +++ /dev/null @@ -1,3 +0,0 @@ -Detected LanguageID(green apple/tennis ball) at position -- X: 5.581 Y: -1.269 Z: -0.358 rotation -- W: 1.0000 X: 0.0000 Y: 0.0000 Z: 0.0000 -Robot pose: position -- X: 2.808 Y: -0.908 Yaw: -78.6 deg -Hand pose: position -- X: 1.086 Y: 0.083 Z: 0.481 rotation -- W: 0.9983 X: 0.0005 Y: 0.0286 Z: 0.0516 diff --git a/predicators/spot_utils/perception/state_2024-05-28 12:06:19.916744.txt b/predicators/spot_utils/perception/state_2024-05-28 12:06:19.916744.txt deleted file mode 100644 index bdd7c2bdc0..0000000000 --- a/predicators/spot_utils/perception/state_2024-05-28 12:06:19.916744.txt +++ /dev/null @@ -1,4 +0,0 @@ -Detected LanguageID(potted plant) at position -- X: 2.825 Y: -1.850 Z: 0.407 rotation -- W: 1.0000 X: 0.0000 Y: 0.0000 Z: 0.0000 -Detected LanguageID(green apple/tennis ball) at position -- X: 5.647 Y: -1.245 Z: -0.362 rotation -- W: 1.0000 X: 0.0000 Y: 0.0000 Z: 0.0000 -Robot pose: position -- X: 2.798 Y: -0.378 Yaw: -78.9 deg -Hand pose: position -- X: 0.753 Y: 0.018 Z: 0.391 rotation -- W: 0.6870 X: 0.0664 Y: 0.7200 Z: -0.0722 diff --git a/predicators/spot_utils/sample_and_move.py b/predicators/spot_utils/sample_and_move.py index 3f1149fd0e..4495a34ee2 100644 --- a/predicators/spot_utils/sample_and_move.py +++ b/predicators/spot_utils/sample_and_move.py @@ -194,7 +194,7 @@ def pour_at_relative_pose(robot: Robot, rel_pos: math_helpers.Vec3) -> None: pixel = get_pixel_from_user(rgbds[hand_camera].rgb) grasp_at_pixel(robot, rgbds[hand_camera], pixel) time.sleep(0.5) -move_hand_to_relative_pose(robot, math_helpers.SE3Pose(0.753, 0.018, 0.39, math_helpers.Quat(0.6870, 0.0664, 0.7200, -0.0722))) +move_hand_to_relative_pose(robot, math_helpers.SE3Pose(0.753, 0.018, 0.89, math_helpers.Quat(0.6870, 0.0664, 0.7200, -0.0722))) time.sleep(0.5) navigate_to_absolute_pose(robot, localizer, math_helpers.SE2Pose(2.808, -0.908, math.radians(-78.6))) time.sleep(0.5) diff --git a/predicators/spot_utils/water_plant.py b/predicators/spot_utils/water_plant.py new file mode 100644 index 0000000000..999d1bf0ad --- /dev/null +++ b/predicators/spot_utils/water_plant.py @@ -0,0 +1,164 @@ +"""Special script for Andi CoRL experiments""" + +from predicators import utils +from predicators.settings import CFG +from bosdyn.client.frame_helpers import get_a_tform_b, BODY_FRAME_NAME +from typing import List, Tuple +import numpy as np +from pathlib import Path + +from predicators.settings import CFG +from predicators.spot_utils.perception.perception_structs import \ + LanguageObjectDetectionID, ObjectDetectionID +from predicators.spot_utils.utils import get_graph_nav_dir +from bosdyn.client import create_standard_sdk +from bosdyn.client.lease import LeaseClient, LeaseKeepAlive +from bosdyn.client.util import authenticate +from bosdyn.client import math_helpers +from bosdyn.client.sdk import Robot +import time +import math + +from predicators import utils +from predicators.spot_utils.perception.spot_cameras import capture_images +from predicators.spot_utils.spot_localization import SpotLocalizer +from predicators.spot_utils.utils import verify_estop, get_pixel_from_user +from predicators.spot_utils.perception.object_detection import detect_objects, get_grasp_pixel, visualize_all_artifacts +from predicators.spot_utils.skills.spot_hand_move import move_hand_to_relative_pose +from predicators.spot_utils.utils import get_allowed_map_regions, \ + sample_move_offset_from_target, spot_pose_to_geom2d +from predicators.spot_utils.skills.spot_stow_arm import stow_arm +from predicators.spot_utils.skills.spot_navigation import navigate_to_absolute_pose +from predicators.spot_utils.skills.spot_grasp import grasp_at_pixel +from predicators.spot_utils.utils import get_spot_home_pose + +TEST_CAMERAS = [ + "hand_color_image", + "frontleft_fisheye_image", + "left_fisheye_image", + "right_fisheye_image", + "frontright_fisheye_image", + ] +TEST_LANGUAGE_DESCRIPTIONS = [ + "potted plant", + "green apple/tennis ball", + "elephant watering can" +] + +args = utils.parse_args(env_required=False, + seed_required=False, + approach_required=False) +utils.update_config(args) + +# Get constants. +hostname = CFG.spot_robot_ip +path = get_graph_nav_dir() +# Setup. +sdk = create_standard_sdk('SpotCameraTestClient') +robot = sdk.create_robot(hostname) +authenticate(robot) +verify_estop(robot) +lease_client = robot.ensure_client(LeaseClient.default_service_name) +lease_client.take() +lease_client = robot.ensure_client(LeaseClient.default_service_name) +lease_client.take() +lease_keepalive = LeaseKeepAlive(lease_client, + must_acquire=True, + return_at_exit=True) + +assert path.exists() +# Creating a localizer so the robot knows its position in a map. +localizer = SpotLocalizer(robot, path, lease_client, lease_keepalive) +localizer.localize() +robot_pose = localizer.get_last_robot_pose() + +# Pouring skill. +def pour_at_relative_pose(robot: Robot, rel_pos: math_helpers.Vec3) -> None: + """Assuming the robot is holding something, execute a pour.""" + # First move the hand to the target pose. + init_rot = math_helpers.Quat.from_pitch(np.pi/2) + init_pose = math_helpers.SE3Pose(x=rel_pos.x, + y=rel_pos.y, + z=rel_pos.z, + rot=init_rot) + move_hand_to_relative_pose(robot, init_pose) + time.sleep(0.5) + pouring_rot = init_rot * math_helpers.Quat.from_yaw(np.pi / 4.0) + pouring_pose = math_helpers.SE3Pose(x=rel_pos.x, + y=rel_pos.y, + z=rel_pos.z, + rot=pouring_rot) + move_hand_to_relative_pose(robot, pouring_pose) + time.sleep(0.5) + move_hand_to_relative_pose(robot, init_pose) + +# Simple reward function example. +def reward_function(input_traj: List[Tuple[float, float]]) -> float: + assert len(input_traj) == 3 + desired_trajectory = [(3.5, 0.45), (3.0, 0.45), (3.0, 0.0)] + reward = 0.0 + for i, waypoint in enumerate(input_traj): + reward += -np.linalg.norm(np.array([desired_trajectory[i][0], desired_trajectory[i][1]]) - np.array([waypoint[0], waypoint[1]])) + return reward + +# ANDI to modify +#def reward_network(input_traj: List[Tuple[float, float]]) -> float: +# self.cost_nn = MLP(input_dim, 1, [], output_activation=None).to(self.device) +# self.optimizer = optim.Adam(self.cost_nn.parameters(), lr=params["lr"]) + +#def calc_cost(self, traj): +# traj = torch.tensor(traj, dtype=torch.float32).to(self.device) +# return self.cost_nn(traj).item() + +# Example sampler. +# spot_home_pose = get_spot_home_pose() +# max_reward = -np.inf +# max_reward_traj = None +rng = np.random.default_rng(0) +# for _ in range(10000): +# curr_traj = [] +# for _ in range(3): +# distance, angle, _ = sample_move_offset_from_target((spot_home_pose.x, spot_home_pose.y), spot_pose_to_geom2d(robot_pose), [], rng, 0.0, 2.5, get_allowed_map_regions()) +# x, y = spot_home_pose.x + np.cos(angle) * distance, spot_home_pose.y + np.sin(angle) * distance +# curr_traj.append((x, y)) +# if reward_function(curr_traj) > max_reward: +# max_reward = reward_function(curr_traj) +# max_reward_traj = curr_traj +# # ANDI TO MODIFY + ADD +# # if calc_cost(curr_traj) < max_cost: +# # min_cost = calc_cost(curr_traj) +# # min_cost_traj = curr_traj +# assert max_reward_traj is not None + +# print(max_reward_traj) + +# for waypoint in max_reward_traj: +# navigate_to_absolute_pose(robot, localizer, math_helpers.SE2Pose(waypoint[0], waypoint[1], 0.0)) +# time.sleep(0.5) + +# stow_arm(robot) +# time.sleep(0.5) +# navigate_to_absolute_pose(robot, localizer, math_helpers.SE2Pose(2.163, 0.360, math.radians(-74.0))) +# time.sleep(0.5) +# navigate_to_absolute_pose(robot, localizer, math_helpers.SE2Pose(3.271, -0.077, math.radians(-78.6))) +# time.sleep(0.5) +# downward_angle = np.pi / 2.5 +# target_pos = math_helpers.Vec3(0.8, 0.0, 0.25) +# rot = math_helpers.Quat.from_pitch(downward_angle) +# body_tform_goal = math_helpers.SE3Pose(x=target_pos.x, y=target_pos.y, z=target_pos.z, rot=rot) +# move_hand_to_relative_pose(robot, body_tform_goal) +# time.sleep(0.5) +# rgbds = capture_images(robot, localizer, TEST_CAMERAS) +# hand_camera = "hand_color_image" +# pixel = get_pixel_from_user(rgbds[hand_camera].rgb) +# grasp_at_pixel(robot, rgbds[hand_camera], pixel) +# time.sleep(0.5) +# #move_hand_to_relative_pose(robot, math_helpers.SE3Pose(0.753, 0.018, 0.89, math_helpers.Quat(0.6870, 0.0664, 0.7200, -0.0722))) +# move_hand_to_relative_pose(robot, math_helpers.SE3Pose(0.644, -0.008, 0.734, math_helpers.Quat(0.8861, 0.1572, 0.4129, -0.1398))) +# time.sleep(0.5) +# navigate_to_absolute_pose(robot, localizer, math_helpers.SE2Pose(2.808, -0.908, math.radians(-78.6))) +# time.sleep(0.5) +# pour_at_relative_pose(robot, math_helpers.Vec3(1.086, 0.083, 0.481)) +move_hand_to_relative_pose(robot, math_helpers.SE3Pose(0.833, 0.150, 0.499, math_helpers.Quat(0.6156, -0.4840, 0.4909, 0.3817))) +time.sleep(0.5) +move_hand_to_relative_pose(robot, math_helpers.SE3Pose(0.644, -0.325, 0.250, math_helpers.Quat(0.7085, -0.2046, 0.6750, 0.0231)))