[SKILLs] Spot open drawer

habitat-lab/habitat/articulated_agents/robots/spot_robot.py

@@ -2,11 +2,12 @@
 # This source code is licensed under the MIT license found in the
 # LICENSE file in the root directory of this source tree.
 
-from typing import Dict, List, Optional, Set
+from typing import Dict, List, Optional, Set, Tuple
 
 import attr
 import magnum as mn
 import numpy as np
+import quaternion
 
 from habitat.articulated_agents.mobile_manipulator import (
     ArticulatedAgentCameraParams,
@@ -107,7 +108,7 @@ def _get_spot_params(self):
                 0.7,
                 -1.5,
             ],
-            ee_offset=[mn.Vector3(0.08, 0, 0)],
+            ee_offset=[mn.Vector3(0.0, 0, -0.1)],
             ee_links=[7],
             ee_constraint=np.array([[[0.4, 1.2], [-0.7, 0.7], [0.25, 1.5]]]),
             cameras={
@@ -152,6 +153,24 @@ def _get_spot_params(self):
                     cam_look_at_pos=mn.Vector3(1, 0.0, 0.75),
                     attached_link_id=-1,
                 ),
+                "articulated_agent_jaw_depth": ArticulatedAgentCameraParams(
+                    cam_offset_pos=mn.Vector3(0.166, 0.0, -0.107),
+                    cam_orientation=mn.Vector3(0, -1.571, 0.0),
+                    attached_link_id=6,
+                    relative_transform=mn.Matrix4.rotation_z(mn.Deg(-90)),
+                ),
+                "articulated_agent_jaw_rgb": ArticulatedAgentCameraParams(
+                    cam_offset_pos=mn.Vector3(0.166, 0.023, -0.095),
+                    cam_orientation=mn.Vector3(0, -1.571, 0.0),
+                    attached_link_id=6,
+                    relative_transform=mn.Matrix4.rotation_z(mn.Deg(-90)),
+                ),
+                "articulated_agent_jaw_panoptic": ArticulatedAgentCameraParams(
+                    cam_offset_pos=mn.Vector3(0.166, 0.0, -0.107),
+                    cam_orientation=mn.Vector3(0, -1.571, 0.0),
+                    attached_link_id=6,
+                    relative_transform=mn.Matrix4.rotation_z(mn.Deg(-90)),
+                ),
             },
             gripper_closed_state=[0.0],
             gripper_open_state=[-1.56],
@@ -175,6 +194,33 @@ def base_transformation(self):
         )
         return self.sim_obj.transformation @ add_rot
 
+    def get_ee_local_pose(
+        self, ee_index: int = 0
+    ) -> Tuple[np.ndarray, np.ndarray]:
+        """Get the local pose of the end-effector.
+
+        :param ee_index: the end effector index for which we want the link transform
+        """
+        if ee_index >= len(self.params.ee_links):
+            raise ValueError(
+                "The current manipulator does not have enough end effectors"
+            )
+
+        ee_transform = self.ee_transform()
+        base_transform = self.base_transformation
+        # Get transformation
+        base_T_ee_transform = base_transform.inverted() @ ee_transform
+
+        # Get the local ee location (x,y,z)
+        local_ee_location = base_T_ee_transform.translation
+
+        # Get the local ee orientation (roll, pitch, yaw)
+        local_ee_quat = quaternion.from_rotation_matrix(
+            base_T_ee_transform.rotation()
+        )
+
+        return np.array(local_ee_location), local_ee_quat
+
     def __init__(
         self, agent_cfg, sim, limit_robo_joints=True, fixed_base=True
     ):

habitat-lab/habitat/config/benchmark/rearrange/skills/open_cab_spot.yaml

@@ -0,0 +1,140 @@
+# @package _global_
+
+defaults:
+  - /habitat: habitat_config_base
+
+  - /habitat/simulator: rearrange_sim
+  - /habitat/simulator/[email protected]_agent: spot_agent
+  - /habitat/simulator/[email protected]_agent: spot
+
+  - /habitat/task: task_config_base
+  - /habitat/task/rearrange/actions: spot_base_arm_empty
+  - /habitat/task/measurements:
+    - articulated_agent_force
+    - force_terminate
+    - articulated_agent_colls
+    - end_effector_to_rest_distance
+    - ee_dist_to_marker
+    - art_obj_at_desired_state
+    - art_obj_state
+    - art_obj_success
+    - art_obj_reward
+    - num_steps
+    - did_violate_hold_constraint
+  - /habitat/task/lab_sensors:
+    - joint_sensor
+    - is_holding_sensor
+    - end_effector_sensor
+    - relative_resting_pos_sensor
+    - handle_bbox_sensor
+    - art_pose_delta_sensor
+
+  - /habitat/dataset/rearrangement: replica_cad
+  - _self_
+
+habitat:
+  gym:
+    obs_keys:
+      - articulated_agent_arm_depth # depth image
+      - joint # joint angles
+      - ee_pos # ee location x,y,z
+      - is_holding # if the robot is holding the object
+      - handle_bbox # the handle bounding box
+      - art_pose_delta_sensor # the angle between the init and current ee orientation
+  task:
+    type: RearrangeOpenCloseDrawerTask-v0
+    reward_measure: art_obj_reward
+    success_measure: art_obj_success
+    actions:
+      arm_action:
+        arm_controller: "ArmRelPosKinematicAction"
+        grip_controller: "MagicGraspAction"
+        arm_joint_mask: [1,1,0,1,0,1,0]
+        arm_joint_dimensionality: 4
+        grasp_thresh_dist: 0.15
+        delta_pos_limit: 0.01667
+        gaze_distance_range: [0.05, 0.2]
+        center_cone_angle_threshold: 10.0
+        center_cone_vector: [0.0, 1.0, 0.0]
+        arm_joint_limit: [[-1.5708,1.5708],[-3.1415,0.0000],[0,3.1415],[-1.5708,1.5708]]
+        auto_grasp: False
+        should_clip: True
+      base_velocity_non_cylinder:
+        lin_speed: 1
+        longitudinal_lin_speed: 1
+        lateral_lin_speed: 0.0
+        ang_speed: 1
+        allow_dyn_slide: False
+        collision_threshold: 1e-5
+        navmesh_offset: [[0.0, 0.0], [0.2, 0.0], [-0.2, 0.0]]
+        enable_lateral_move: False
+    success_reward: 10.0
+    slack_reward: -0.01
+    success_state: 0.45
+    end_on_success: True
+    joint_start_noise: 0.1
+    # For learning in close drawer task, use larger spawn_max_dist_to_obj.
+    # Otherwise, the robot is goint to hit the drawer and cannot see handles
+    spawn_max_dist_to_obj: 3.5
+    base_angle_noise: 0.523599 # 30 degree
+    measurements:
+      art_obj_at_desired_state:
+        use_absolute_distance: False
+        gaze_method: True
+        center_cone_vector: [0.0, 1.0, 0.0]
+        gaze_distance_range: [0.0, 0.1]
+        center_cone_angle_threshold: 15 # in degree
+        pose_angle_threshold: 0.15
+      art_obj_reward:
+        wrong_grasp_end: True
+        wrong_grasp_pen: 5
+        early_grasp_pen: 5
+        grasp_reward: 10.0
+        marker_dist_reward: 20.0
+        ee_orientation_reward: 10.0
+        constraint_violate_pen: 1.0
+        # Collision penality for kinematic simulation
+        count_coll_pen: 0.1
+        max_count_colls: 5
+        count_coll_end_pen: 8
+        # Use special gaze reward
+        gaze_method: True
+        art_at_desired_state_reward: 0.0
+        force_pen: 0.0
+        max_force_pen: 0.0
+        force_end_pen: 0.0
+      art_obj_success:
+        must_call_stop: False
+        rest_dist_threshold: -1
+        gaze_method: True
+      force_terminate:
+        max_accum_force: -1.0
+        max_instant_force: -1.0
+    lab_sensors:
+      # We let robot see 5 joints
+      joint_sensor:
+        dimensionality: 5
+        arm_joint_mask: [1,1,0,1,0,1,1]
+      handle_bbox_sensor:
+        # The size of bbox needs to be the same as the size
+        # of the all the other image sensors due to resnet design
+        height: 240
+        width: 228
+        pixel_size: 2
+        handle_size: [0.02,0.1]
+  environment:
+    max_episode_steps: 1000
+  dataset:
+    data_path: data/datasets/replica_cad/rearrange/v1/{split}/in_drawer_1k_100.json.gz
+  simulator:
+    # We use the kinematic mode to train the policy
+    kinematic_mode: True
+    ac_freq_ratio: 1
+    ctrl_freq: 120 # We change this so that it has the same control frequency as the one in hardware
+    agents:
+      main_agent:
+        joint_start_noise: 0.0
+        joint_that_can_control: [1, 1, 0, 1, 0, 1, 0]
+        # The real world stow location
+        joint_start_override: [0, -3.14, 0, 3.14, 0, 0, 0]
+        joint_start_override_random: [[0, -3.14, 0, 3.14, 0, 0, 0], [0, -3.14, 0, 3.14, 0, 0, 1.5708], [0, -3.14, 0, 3.14, 0, 0, -1.5708]]

habitat-lab/habitat/config/benchmark/rearrange/skills/place.yaml

@@ -17,6 +17,9 @@ defaults:
     - end_effector_to_goal_distance
     - object_to_goal_distance
     - obj_at_goal
+    - obj_at_receptacle
+    - end_effector_to_initial_orientation_distance
+    - object_to_target_orientation_distance
     - place_success
     - place_reward
     - num_steps
@@ -25,6 +28,8 @@ defaults:
     - joint_sensor
     - is_holding_sensor
     - relative_resting_pos_sensor
+    - relative_end_effector_orientation_sensor
+    - relative_target_object_orientation_sensor
 
   - /habitat/dataset/rearrangement: replica_cad
   - _self_

habitat-lab/habitat/config/benchmark/rearrange/skills/place_spot.yaml

@@ -0,0 +1,88 @@
+# @package _global_
+defaults:
+  - place
+  - /habitat/task/measurements:
+    - base_to_object_distance
+  - /habitat/task/lab_sensors:
+    - spot_head_stereo_depth_sensor
+  - override /habitat/simulator/[email protected]_agent: spot_agent
+  - override /habitat/simulator/[email protected]_agent: spot
+  - override /habitat/task/rearrange/actions: spot_base_arm_empty
+  - _self_
+
+# This yaml is designed specifically for learning a semantic place policy for the Boston Dynamics Spot robot.
+# It uses the base place yaml to define the basic measurements and sensors,
+# observation keys, and reward function to train the policy.
+# The major changes compared to the base place yaml are the following:
+# (1) obs_keys: we ensure these observations can be obtained from the real robot (Spot)
+# (2) place_reward: place reward considers collisions based on a kinematic simulation
+# (3) actions: Spot uses arm action to place the object
+# (4) simulator: we simulate the environment via the kinematic mode to facilitate sim2real transfer
+habitat:
+  gym:
+    obs_keys:
+      - relative_initial_ee_orientation
+      - articulated_agent_arm_depth
+      - joint
+      - is_holding
+  task:
+    measurements:
+      force_terminate:
+        # We ignore the force here since in kinematic simulation, there is no force
+        max_accum_force: -1.0
+        max_instant_force: -1.0
+      place_reward:
+        wrong_drop_should_end: False
+        drop_pen: 5.0
+        count_coll_pen: 0.05
+        max_count_colls: 100
+        count_coll_end_pen: 5
+        # We want to check if the object is placed on the receptacle
+        obj_at_receptacle_success: True
+        # We want to give the reward for ee orientation to initial orientation
+        use_ee_ori: True
+      place_success:
+        ee_resting_success_threshold: -1.0
+        # We want to check if the object is placed on the receptacle
+        obj_at_receptacle_success: True
+        # We want to check if the robot maintains the same ee's orientation
+        ee_orientation_to_initial_threshold: 0.15
+      obj_at_goal:
+        # We want this number to be as small as possible
+        # since we want the robot to place the object precisely on the receptacle
+        succ_thresh: 0.05
+      obj_at_receptacle:
+        vertical_diff_threshold: 0.10
+        horizontal_diff_threshold: 0.30
+        surface_vertical_diff_threshold: -1.0
+        snap_down_surface_vertical_diff_threshold: 0.10
+    lab_sensors:
+      # We can only control 4 of the joints of Spot's arm
+      joint_sensor:
+        dimensionality: 4
+        arm_joint_mask: [1,1,0,1,0,1,0]
+    actions:
+      arm_action:
+        arm_controller: "ArmRelPosKinematicAction"
+        center_cone_vector: [0.0, 1.0, 0.0]
+        # We limit the joint angles to ensure that this is feasible in the real world
+        arm_joint_limit: [[-0.7853, 0.7853], [-3.1415, -0.7853], [0, 3.1415], [-1.5708, 1.5708]]
+        auto_grasp: False
+        should_clip: True
+    success_reward: 10.0
+    slack_reward: -0.01
+    # Control Spot robot place location
+    spawn_max_dist_to_obj: 1.0
+    base_angle_noise: 0.261799
+  simulator:
+    # We use the kinematic mode to train the policy
+    kinematic_mode: True
+    ac_freq_ratio: 1
+    step_physics: False
+    agents:
+      main_agent:
+        joint_start_noise: 0.0
+        joint_that_can_control: [1, 1, 0, 1, 0, 1, 0]
+        # The real-world gaze ready location
+        joint_start_override: [0, -2.792, 0, 1.745, 0, 1.571, 0]
+        joint_start_override_random: [[0, -3.14, 0, 3.14, 0, 0, 1.57], [0, -3.14, 0, 3.14, 0, 0, -1.57], [-0.016, -1.621, 0, 1.592, 0, 1.571, 0],  [-0.046, -1.920, 0, 2.094, 0, 1.396, 0]]