Open drawer nyc backup vector machine

bd_spot_wrapper/spot_wrapper/spot.py

@@ -277,6 +277,19 @@ def rotate_gripper_with_delta(self, wrist_yaw=0.0, wrist_roll=0.0):
         self.set_arm_joint_positions(new_arm_joint_states)
         time.sleep(0.5)
 
+    def get_ee_rotation_in_body_frame_quat(self):
+        """
+        return ee rotation in quaternion
+        """
+        vision_T_hand = get_a_tform_b(
+            self.robot_state_client.get_robot_state().kinematic_state.transforms_snapshot,
+            "body",
+            "hand",
+        )
+        quat = vision_T_hand.rotation
+        quat = quaternion.quaternion(quat.w, quat.x, quat.y, quat.z)
+        return quat
+
     def move_gripper_to_point(
         self, point, rotation, seconds_to_goal=3.0, timeout_sec=10
     ):

spot_rl_experiments/configs/config.yaml

@@ -13,6 +13,9 @@ WEIGHTS_TORCHSCRIPT:
   # Static place
   PLACE: "weights/torchscript/place_10deg_32_seed300_1649709235_ckpt.75_combined_net.torchscript"
 
+  # Open close drawer
+  OPEN_CLOSE_DRAWER: "weights/open_close_drawer/od14lr_30_ckpt.144.combined_net.torchscript"
+
 WEIGHTS:
   # Regular Nav
   NAV: "weights/CUTOUT_WT_True_SD_200_ckpt.99.pvp.pth"
@@ -25,6 +28,9 @@ WEIGHTS:
 
   # Static place
   PLACE: "weights/final_paper/place_10deg_32_seed300_1649709235_ckpt.75.pth"
+
+  # Open close drawer
+  OPEN_CLOSE_DRAWER: "weights/open_close_drawer/od14lr_30_ckpt.144.combined_net.torchscript"
 
   # ASC (weight of ASC will always remain in pytorch)
   MIXER: "weights/final_paper/final_moe_rnn_60_1.0_SD_100_1652120928_ckpt.16_copy.pth"
@@ -70,19 +76,27 @@ SUCC_XY_DIST: 0.1
 SUCC_Z_DIST: 0.20
 PLACE_ACTION_SPACE_LENGTH: 4
 
+# Open Close Drawer env
+OPEN_CLOSE_DRAWER_ACTION_SPACE_LENGTH: 8
+OPEM_CLOSE_DRAWER_DISTANCE_BETWEEN_EE_HANDLE: 0.3
+
 # Base action params
 MAX_LIN_DIST: 0.25  # meters
 MAX_ANG_DIST: 15.0  # degrees
 
 # Arm action params
 MAX_JOINT_MOVEMENT: 0.0872665  # Gaze arm speed (5 deg)
 MAX_JOINT_MOVEMENT_2: 0.174533  # Place arm speed (10 deg)
+MAX_JOINT_MOVEMENT_OPEN_CLOSE_DRAWER: 0.06
 INITIAL_ARM_JOINT_ANGLES: [0, -170, 120, 0, 75, 0]
 GAZE_ARM_JOINT_ANGLES: [0, -160, 100, 0, 90, 0]
 PLACE_ARM_JOINT_ANGLES: [0, -170, 120, 0, 75, 0]
 ARM_LOWER_LIMITS: [-45, -180, 0, 0, -90, 0]
 ARM_UPPER_LIMITS: [45, -45, 180, 0, 90, 0]
+ARM_LOWER_LIMITS_OPEN_CLOSE_DRAWER: [-90, -180, 0, 0, -90, 0]
+ARM_UPPER_LIMITS_OPEN_CLOSE_DRAWER: [90, 0, 180, 0, 90, 0]
 JOINT_BLACKLIST: [3, 5] # joints we can't control "arm0.el0", "arm0.wr1"
+JOINT_BLACKLIST_OPEN_CLOSE_DRAWER: [3]
 ACTUALLY_MOVE_ARM: True
 GRASP_EVERY_STEP: False
 TERMINATE_ON_GRASP: False

spot_rl_experiments/experiments/skill_test/test_open_close_drawer.py

@@ -0,0 +1,26 @@
+# Copyright (c) Meta Platforms, Inc. and its affiliates.
+# This source code is licensed under the MIT license found in the
+# LICENSE file in the root directory of this source tree.
+
+
+import numpy as np
+from spot_rl.envs.skill_manager import SpotSkillManager
+
+if __name__ == "__main__":
+    from perception_and_utils.utils.generic_utils import map_user_input_to_boolean
+
+    # Init the skill
+    spotskillmanager = SpotSkillManager()
+
+    # Using while loop
+    contnue = True
+    while contnue:
+        spotskillmanager.openclosedrawer(open_mode=True)
+        close_drawer = map_user_input_to_boolean(
+            "Do you want to close the drawer ? Y/N "
+        )
+        if close_drawer:
+            spotskillmanager.openclosedrawer(open_mode=False)
+        contnue = map_user_input_to_boolean(
+            "Do you want to open the drawer again ? Y/N "
+        )

spot_rl_experiments/spot_rl/envs/base_env.py

@@ -386,7 +386,7 @@ def step(  # noqa
                 )
             elif arm_action is not None:
                 self.spot.set_arm_joint_positions(
-                    positions=arm_action, travel_time=1 / self.ctrl_hz * 0.9
+                    positions=arm_action, travel_time=1 / self.ctrl_hz * 1.5
                 )
 
         if self.prev_base_moved and base_action is None:
@@ -499,14 +499,11 @@ def get_nav_observation(self, goal_xy, goal_heading):
 
         return observations
 
-    def get_arm_joints(self):
+    def get_arm_joints(self, black_list=None):
         # Get proprioception inputs
+        black_list = self.config.JOINT_BLACKLIST if black_list is None else black_list
         joints = np.array(
-            [
-                j
-                for idx, j in enumerate(self.current_arm_pose)
-                if idx not in self.config.JOINT_BLACKLIST
-            ],
+            [j for idx, j in enumerate(self.current_arm_pose) if idx not in black_list],
             dtype=np.float32,
         )