pour

bdaiinstitute · May 28, 2024 · 8d6d797 · 8d6d797
1 parent 403d32c
commit 8d6d797
Show file tree

Hide file tree

Showing 9 changed files with 41 additions and 12 deletions.
diff --git a/predicators/spot_utils/perception/no_detection_artifacts.png b/predicators/spot_utils/perception/no_detection_artifacts.png
diff --git a/predicators/spot_utils/perception/object_detection_artifacts.png b/predicators/spot_utils/perception/object_detection_artifacts.png
diff --git a/predicators/spot_utils/perception/state_2024-05-28 11:35:16.941320.txt b/predicators/spot_utils/perception/state_2024-05-28 11:35:16.941320.txt
diff --git a/predicators/spot_utils/perception/state_2024-05-28 11:40:56.831690.txt b/predicators/spot_utils/perception/state_2024-05-28 11:40:56.831690.txt
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
@@ -0,0 +1,4 @@
+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
@@ -0,0 +1,5 @@
+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
@@ -0,0 +1,3 @@
+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
@@ -0,0 +1,4 @@
+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
@@ -17,6 +17,7 @@
 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
@@ -154,9 +155,6 @@ def reward_function(input_traj: List[Tuple[float, float]]) -> float:
 # get_grasp_pixel(robot, ) -> returns pixel to grasp 
 # grasp_at_pixel(robot, rgbds[hand_camera], pixel) -> grasps at pixel
 
-
-
-
 # Pouring skill.
 def pour_at_relative_pose(robot: Robot, rel_pos: math_helpers.Vec3) -> None:
     """Assuming the robot is holding something, execute a pour."""
@@ -177,4 +175,27 @@ def pour_at_relative_pose(robot: Robot, rel_pos: math_helpers.Vec3) -> None:
     time.sleep(0.5)
     move_hand_to_relative_pose(robot, init_pose)
 
-pour_at_relative_pose(robot, target_pos)
+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
+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.39, 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)
+pour_at_relative_pose(robot, math_helpers.Vec3(1.086, 0.083, 0.481))