diff --git a/predicators/spot_utils/move_and_pour.py b/predicators/spot_utils/move_and_pour.py deleted file mode 100644 index d3fbaaf68c..0000000000 --- a/predicators/spot_utils/move_and_pour.py +++ /dev/null @@ -1,139 +0,0 @@ -"""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 -import numpy as np - -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 - -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 -from predicators.spot_utils.perception.object_detection import detect_objects -from predicators.spot_utils.skills.spot_hand_move import move_hand_to_relative_pose -from predicators.spot_utils.utils import get_allowed_map_regions, \ - get_collision_geoms_for_nav, load_spot_metadata, object_to_top_down_geom, \ - sample_move_offset_from_target, spot_pose_to_geom2d, get_relative_se2_from_se3 -from predicators.spot_utils.skills.spot_navigation import navigate_to_relative_pose, go_home, navigate_to_absolute_pose -from predicators.spot_utils.utils import get_robot_state, get_spot_home_pose - - - -def move_and_hand_reach_relative_transform(robot: Robot, localizer: SpotLocalizer, - abs_hand_world_pose: math_helpers.SE3Pose, rng: np.random.Generator) -> None: - """Attempts to get the hand to a specific pose in the world frame. - If this is too far away to reach, the robot will sample a point - closeby to move the body to, then move the hand.""" - # First, compute distance between current robot pose and desired - # pose. - # snapshot = robot.get_frame_tree_snapshot() - # hand_in_body = get_a_tform_b(snapshot, BODY_FRAME_NAME, "hand") - # localizer.localize() - # curr_hand_pose = hand_in_body.mult(localizer.get_last_robot_pose()) - # dist_to_goal = np.linalg.norm(curr_hand_pose.get_translation() - abs_hand_world_pose.get_translation()) - # print(f"Dist to goal: {dist_to_goal}") - # if dist_to_goal > 0.8: - # localizer.localize() - # desired_hand_pos = abs_hand_world_pose.get_translation() - # distance, angle, _ = sample_move_offset_from_target(tuple([desired_hand_pos[0], desired_hand_pos[1]]), spot_pose_to_geom2d(localizer.get_last_robot_pose()), [], rng, 0.25, 0.35, get_allowed_map_regions()) - # print(f"Distance, angle: {(distance, angle)}") - # relative_se2_move = get_relative_se2_from_se3(localizer.get_last_robot_pose(), abs_hand_world_pose, distance, - # angle) - # relative_se2_move = get_relative_se2_from_se3(curr_hand_pose, abs_hand_world_pose, 0.0, - # 0.0) - # navigate_to_relative_pose(robot, relative_se2_move) - # navigate_to_absolute_pose(robot, localizer, abs_hand_world_pose.get_closest_se2_transform()) - # time.sleep(0.5) - localizer.localize() - curr_robot_pose = localizer.get_last_robot_pose() - desired_hand_in_body = abs_hand_world_pose.mult(curr_robot_pose.inverse()) - print(desired_hand_in_body) - move_hand_to_relative_pose(robot, desired_hand_in_body) - time.sleep(0.5) - - - snapshot = robot.get_frame_tree_snapshot() - hand_in_body = get_a_tform_b(snapshot, BODY_FRAME_NAME, "hand") - localizer.localize() - hand_in_world = hand_in_body.mult(localizer.get_last_robot_pose()) - # print(abs_hand_world_pose) - # print(hand_in_world) - - - -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", -] - -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) -rng = np.random.default_rng(0) - -assert path.exists() -# Creating a localizer so the robot knows its position in a map. -localizer = SpotLocalizer(robot, path, lease_client, lease_keepalive) -rgbds = capture_images(robot, localizer, TEST_CAMERAS) -language_ids: List[ObjectDetectionID] = [ - LanguageObjectDetectionID(d) for d in TEST_LANGUAGE_DESCRIPTIONS -] -# detections, artifacts = detect_objects(language_ids, rgbds) -snapshot = robot.get_frame_tree_snapshot() -hand_in_body = get_a_tform_b(snapshot, BODY_FRAME_NAME, "hand") -localizer.localize() -hand_in_world = hand_in_body.mult(localizer.get_last_robot_pose()) - -# for obj_id, detection in detections.items(): -# print(f"Detected {obj_id} at {detection}") -# print(f"Robot pose: {localizer.get_last_robot_pose()}") -# print(f"Hand pose: {hand_in_world}") - -# TESTING. -test_hand_pose = math_helpers.SE3Pose(2.691, -0.848, 0.509, math_helpers.Quat(-0.0994, 0.0791, -0.0041, 0.9919)) -move_and_hand_reach_relative_transform(robot, localizer, test_hand_pose, rng) -# move_and_hand_reach_relative_transform(robot, localizer, hand_in_world, rng) - -# Probably easier to just move the body to some absolute pose in the world, and then move the hand to a relative pose.... -# TODO: sample from map. - -# Simple reward function example. -def reward_function(proposed_pose: math_helpers.SE2Pose) -> float: - spot_home = get_spot_home_pose() - return 1/np.linalg.norm(np.array([spot_home.x, spot_home.y]) - np.array([proposed_pose.x, proposed_pose.y])) diff --git a/predicators/spot_utils/perception/no_detection_artifacts.png b/predicators/spot_utils/perception/no_detection_artifacts.png index 56e69f80cc..8aebcc6176 100644 Binary files a/predicators/spot_utils/perception/no_detection_artifacts.png and b/predicators/spot_utils/perception/no_detection_artifacts.png differ diff --git a/predicators/spot_utils/perception/object_detection_artifacts.png b/predicators/spot_utils/perception/object_detection_artifacts.png index de6062941f..87adac23b6 100644 Binary files a/predicators/spot_utils/perception/object_detection_artifacts.png and b/predicators/spot_utils/perception/object_detection_artifacts.png differ diff --git a/predicators/spot_utils/perception/plant-short/plant1.txt b/predicators/spot_utils/perception/plant-short/plant1.txt deleted file mode 100644 index 2f9d40ffb9..0000000000 --- a/predicators/spot_utils/perception/plant-short/plant1.txt +++ /dev/null @@ -1,2 +0,0 @@ -Detected LanguageID(potted plant) at position -- X: 0.659 Y: 0.359 Z: 0.157 rotation -- W: 1.0000 X: 0.0000 Y: 0.0000 Z: 0.0000 -Robot pose: position -- X: 3.684 Y: -0.340 Z: 0.109 rotation -- W: -0.3051 X: -0.0019 Y: -0.0010 Z: 0.9523 diff --git a/predicators/spot_utils/perception/plant-short/plant2.txt b/predicators/spot_utils/perception/plant-short/plant2.txt deleted file mode 100644 index a9987e70dd..0000000000 --- a/predicators/spot_utils/perception/plant-short/plant2.txt +++ /dev/null @@ -1,2 +0,0 @@ -Detected LanguageID(potted plant) at position -- X: 0.757 Y: 0.284 Z: 0.129 rotation -- W: 1.0000 X: 0.0000 Y: 0.0000 Z: 0.0000 -Robot pose: position -- X: 2.672 Y: -0.919 Z: 0.113 rotation -- W: -0.2937 X: -0.0011 Y: -0.0007 Z: 0.9559 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 new file mode 100644 index 0000000000..e77e9bd424 --- /dev/null +++ b/predicators/spot_utils/perception/plant-short/state_2024-05-21 18:26:35.545072.txt @@ -0,0 +1,3 @@ +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 new file mode 100644 index 0000000000..18d998448c --- /dev/null +++ b/predicators/spot_utils/perception/plant-short/state_2024-05-21 18:27:25.276281.txt @@ -0,0 +1,3 @@ +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 new file mode 100644 index 0000000000..133bc6b175 --- /dev/null +++ b/predicators/spot_utils/perception/plant-short/state_2024-05-21 18:28:31.174064.txt @@ -0,0 +1,3 @@ +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 new file mode 100644 index 0000000000..4d88d9f178 --- /dev/null +++ b/predicators/spot_utils/perception/plant-tall/state_2024-05-21 18:20:55.519938.txt @@ -0,0 +1,3 @@ +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 new file mode 100644 index 0000000000..05975568aa --- /dev/null +++ b/predicators/spot_utils/perception/plant-tall/state_2024-05-21 18:21:36.705204.txt @@ -0,0 +1,3 @@ +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 new file mode 100644 index 0000000000..75638582ca --- /dev/null +++ b/predicators/spot_utils/perception/plant-tall/state_2024-05-21 18:23:43.159150.txt @@ -0,0 +1,3 @@ +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-21 18:03:23.408526.txt b/predicators/spot_utils/perception/state_2024-05-21 18:03:23.408526.txt deleted file mode 100644 index cf5a2106c0..0000000000 --- a/predicators/spot_utils/perception/state_2024-05-21 18:03:23.408526.txt +++ /dev/null @@ -1,3 +0,0 @@ -Detected LanguageID(potted plant) at position -- X: 0.664 Y: 0.473 Z: 0.183 rotation -- W: 1.0000 X: 0.0000 Y: 0.0000 Z: 0.0000 -Robot pose: position -- X: 3.763 Y: -0.060 Z: 0.156 rotation -- W: -0.1298 X: -0.0001 Y: -0.0000 Z: 0.9915 -Hand pose: position -- X: 3.211 Y: -0.062 Z: -0.058 rotation -- W: -0.1295 X: -0.0077 Y: 0.0013 Z: 0.9915 diff --git a/predicators/spot_utils/sample_and_move.py b/predicators/spot_utils/sample_and_move.py index db980f20de..0339ada45d 100644 --- a/predicators/spot_utils/sample_and_move.py +++ b/predicators/spot_utils/sample_and_move.py @@ -81,6 +81,15 @@ def reward_function(input_traj: List[Tuple[float, float]]) -> float: 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 @@ -95,6 +104,10 @@ def reward_function(input_traj: List[Tuple[float, float]]) -> float: 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)