diff --git a/predicators/spot_utils/perception/demos/plant-tall/plant1.txt b/predicators/spot_utils/perception/demos/plant-tall/plant1.txt new file mode 100644 index 0000000000..9090e666ed --- /dev/null +++ b/predicators/spot_utils/perception/demos/plant-tall/plant1.txt @@ -0,0 +1,2 @@ +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 new file mode 100644 index 0000000000..ceaa7784ee --- /dev/null +++ b/predicators/spot_utils/perception/demos/plant-tall/plant2.txt @@ -0,0 +1,2 @@ +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 new file mode 100644 index 0000000000..99cea72d16 --- /dev/null +++ b/predicators/spot_utils/perception/demos/plant-tall/plant3.txt @@ -0,0 +1,2 @@ +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 new file mode 100644 index 0000000000..56e69f80cc Binary files /dev/null and b/predicators/spot_utils/perception/no_detection_artifacts.png differ diff --git a/predicators/spot_utils/perception/object_detection.py b/predicators/spot_utils/perception/object_detection.py index fb21cc4951..7d0661c445 100644 --- a/predicators/spot_utils/perception/object_detection.py +++ b/predicators/spot_utils/perception/object_detection.py @@ -33,6 +33,7 @@ import PIL.Image import requests from bosdyn.client import math_helpers +from bosdyn.client.frame_helpers import get_a_tform_b from matplotlib import pyplot as plt from scipy import ndimage from scipy.spatial import Delaunay @@ -725,14 +726,20 @@ def _run_manual_test() -> None: ] detections, artifacts = detect_objects(language_ids, rgbds) + snapshot = robot.get_frame_tree_snapshot() + hand_in_body = get_a_tform_b(snapshot, "hand", "body") + hand_in_world = hand_in_body.mult(localizer.get_last_robot_pose()) + with open(f"state_{datetime.now()}.txt", 'w') as file: for obj_id, detection in detections.items(): file.write(f"Detected {obj_id} at {detection}\n") file.write(f"Robot pose: {localizer.get_last_robot_pose()}\n") + file.write(f"Hand pose: {hand_in_world}\n") 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}") # Visualize the artifacts. detections_outfile = Path(".") / "object_detection_artifacts.png" diff --git a/predicators/spot_utils/perception/object_detection_artifacts.png b/predicators/spot_utils/perception/object_detection_artifacts.png new file mode 100644 index 0000000000..de6062941f Binary files /dev/null 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 new file mode 100644 index 0000000000..2f9d40ffb9 --- /dev/null +++ b/predicators/spot_utils/perception/plant-short/plant1.txt @@ -0,0 +1,2 @@ +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 new file mode 100644 index 0000000000..a9987e70dd --- /dev/null +++ b/predicators/spot_utils/perception/plant-short/plant2.txt @@ -0,0 +1,2 @@ +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/state_2024-05-21 18:03:23.408526.txt b/predicators/spot_utils/perception/state_2024-05-21 18:03:23.408526.txt new file mode 100644 index 0000000000..cf5a2106c0 --- /dev/null +++ b/predicators/spot_utils/perception/state_2024-05-21 18:03:23.408526.txt @@ -0,0 +1,3 @@ +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