andi arm pose

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

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

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

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"

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

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

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