clean start

bdaiinstitute · Jun 5, 2024 · a4a41a9 · a4a41a9
1 parent a039d14
commit a4a41a9
Show file tree

Hide file tree

Showing 5 changed files with 31 additions and 9 deletions.
diff --git a/predicators/spot_utils/water_plant.py → predicators/spot_utils/andi-corl.py b/predicators/spot_utils/water_plant.py → predicators/spot_utils/andi-corl.py
@@ -30,6 +30,7 @@
 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.skills.spot_place import place_at_relative_position
 from predicators.spot_utils.utils import get_spot_home_pose
 
 TEST_CAMERAS = [
@@ -40,9 +41,11 @@
         "frontright_fisheye_image",
     ]
 TEST_LANGUAGE_DESCRIPTIONS = [
-    "potted plant",
-    "green apple/tennis ball",
-    "elephant watering can"
+    "plant pot",
+    "green apple",
+    "tennis ball",
+    "elephant watering can",
+    "green block",
 ]
 
 args = utils.parse_args(env_required=False,
@@ -83,7 +86,7 @@ def pour_at_relative_pose(robot: Robot, rel_pos: math_helpers.Vec3) -> None:
                                            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_rot = init_rot * math_helpers.Quat.from_yaw(np.pi / 4.0) # rotation for pour (if need greater rotation, make angle of pour higher)
     pouring_pose = math_helpers.SE3Pose(x=rel_pos.x,
                                            y=rel_pos.y,
                                            z=rel_pos.z,
@@ -136,6 +139,15 @@ def reward_function(input_traj: List[Tuple[float, float]]) -> float:
 #     navigate_to_absolute_pose(robot, localizer, math_helpers.SE2Pose(waypoint[0], waypoint[1], 0.0))
 #     time.sleep(0.5)
 
+# WATER PLANT SEQUENCE
+stow_arm(robot)
+time.sleep(0.5)
+navigate_to_absolute_pose(robot, localizer, math_helpers.SE2Pose(2.163, 0.360, math.radians(-74.0))) # sampled location 
+time.sleep(0.5)
+downward_angle = np.pi / 2.5
+target_pos = math_helpers.Vec3(0.8, 0.0, 0.25)
+
+# WATER PLANT (FULL) SEQUENCE
 # stow_arm(robot)
 # time.sleep(0.5)
 # navigate_to_absolute_pose(robot, localizer, math_helpers.SE2Pose(2.163, 0.360, math.radians(-74.0)))
@@ -154,11 +166,15 @@ def reward_function(input_traj: List[Tuple[float, float]]) -> float:
 # 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)))
+# move_hand_to_relative_pose(robot, math_helpers.SE3Pose(0.834, 0.103, 0.565, math_helpers.Quat(0.6142, -0.0020, 0.7859, 0.0716)))
 # 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)))
+# # 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)))
+
+# PICK UP CAN SEQUENCE
+
+# THROW AWAY RAG SEQUENCE
diff --git a/predicators/spot_utils/example_skills.py b/predicators/spot_utils/example_skills.py
@@ -37,4 +37,7 @@
 
 grasp_at_pixel(robot, rgbds[hand_camera], pixel)
 stow_arm(robot)
-time.sleep(0.5)
+time.sleep(0.5)
+
+# DROP
+place_at_relative_position(robot, body_to_position: math_helpers.Vec3, downward_angle: float = np.pi / 2.5)
diff --git a/...s/spot_utils/perception/demo/no_detections_image_2024-05-31 09:52:37.354489.png b/...s/spot_utils/perception/demo/no_detections_image_2024-05-31 09:52:37.354489.png
diff --git a/...ot_utils/perception/demo/object_detections_image_2024-05-31 09:52:37.354489.png b/...ot_utils/perception/demo/object_detections_image_2024-05-31 09:52:37.354489.png
diff --git a/predicators/spot_utils/perception/demo/state_2024-05-31 09:52:37.354489.txt b/predicators/spot_utils/perception/demo/state_2024-05-31 09:52:37.354489.txt
@@ -0,0 +1,3 @@
+Object LanguageID(green apple/tennis ball) at position -- X: 5.632 Y: -1.220 Z: -0.366 rotation -- W: 1.0000 X: 0.0000 Y: 0.0000 Z: 0.0000
+Robot pose: position -- X: 2.934 Y: -0.974 Yaw: -78.3 deg
+Robot hand pose: position -- X: 0.834 Y: 0.103 Z: 0.363 rotation -- W: 0.6142 X: -0.0020 Y: 0.7859 Z: 0.0716