Port spot

.circleci/config.yml

@@ -206,6 +206,8 @@ jobs:
       - run:
           name: Download test data
           command: |
+              export PATH=$HOME/miniconda/bin:/usr/local/cuda/bin:$PATH
+              . activate habitat
               if [ ! -f ./habitat-sim/data/scene_datasets/habitat-test-scenes/van-gogh-room.glb ]
               then
                 cd habitat-sim
@@ -216,14 +218,10 @@ jobs:
               fi
               if [ ! -f ./habitat-sim/data/robots/franka_panda/panda_arm.urdf ]
               then
-                export PATH=$HOME/miniconda/bin:/usr/local/cuda/bin:$PATH
-                . activate habitat
                 python -m habitat_sim.utils.datasets_download --uids franka_panda --data-path habitat-sim/data/
               fi
               if [ ! -f ./habitat-sim/data/robots/hab_spot_arm/urdf/hab_spot_arm.urdf ]
               then
-                export PATH=$HOME/miniconda/bin:/usr/local/cuda/bin:$PATH
-                . activate habitat
                 python -m habitat_sim.utils.datasets_download --uids hab_spot_arm --data-path habitat-sim/data/
               fi
       - run:

habitat-lab/habitat/config/default.py

@@ -707,6 +707,8 @@ def __init__(self, *args, **kwargs):
     []
 )  # a list of directory or config paths to search in addition to the dataset for object configs. Should match the generated episodes for the task.
 _C.habitat.simulator.seed = _C.habitat.seed
+_C.habitat.simulator.contact_test = False
+_C.habitat.simulator.nav_mesh = True
 _C.habitat.simulator.turn_angle = (
     10  # angle to rotate left or right in degrees
 )
@@ -830,7 +832,56 @@ def __init__(self, *args, **kwargs):
     _C.habitat.simulator.depth_sensor.clone()
 )
 _C.habitat.simulator.third_depth_sensor.uuid = "robot_third_rgb"
-
+# -----------------------------------------------------------------------------
+# HEAD STEREO LEFT RGB SENSOR
+# -----------------------------------------------------------------------------
+_C.habitat.simulator.head_stereo_left_rgb_sensor = (
+    _C.habitat.simulator.rgb_sensor.clone()
+)
+_C.habitat.simulator.head_stereo_left_rgb_sensor.uuid = (
+    "robot_head_stereo_left_rgb"
+)
+_C.habitat.simulator.head_stereo_left_rgb_sensor.type = (
+    "HabitatSimHeadStereoLeftRGBSensor"
+)
+# -----------------------------------------------------------------------------
+# HEAD STEREO LEFT DEPTH SENSOR
+# -----------------------------------------------------------------------------
+_C.habitat.simulator.head_stereo_left_depth_sensor = (
+    _C.habitat.simulator.depth_sensor.clone()
+)
+_C.habitat.simulator.head_stereo_left_depth_sensor.uuid = (
+    "robot_head_stereo_left_depth"
+)
+_C.habitat.simulator.head_stereo_left_depth_sensor.type = (
+    "HabitatSimHeadStereoLeftDepthSensor"
+)
+_C.habitat.simulator.head_stereo_left_depth_sensor.max_zero = False
+# -----------------------------------------------------------------------------
+# HEAD STEREO RIGHT RGB SENSOR
+# -----------------------------------------------------------------------------
+_C.habitat.simulator.head_stereo_right_rgb_sensor = (
+    _C.habitat.simulator.rgb_sensor.clone()
+)
+_C.habitat.simulator.head_stereo_right_rgb_sensor.uuid = (
+    "robot_head_stereo_right_rgb"
+)
+_C.habitat.simulator.head_stereo_right_rgb_sensor.type = (
+    "HabitatSimHeadStereoRightRGBSensor"
+)
+# -----------------------------------------------------------------------------
+# HEAD STEREO RIGHT DEPTH SENSOR
+# -----------------------------------------------------------------------------
+_C.habitat.simulator.head_stereo_right_depth_sensor = (
+    _C.habitat.simulator.depth_sensor.clone()
+)
+_C.habitat.simulator.head_stereo_right_depth_sensor.uuid = (
+    "robot_head_stereo_right_depth"
+)
+_C.habitat.simulator.head_stereo_right_depth_sensor.type = (
+    "HabitatSimHeadStereoRightDepthSensor"
+)
+_C.habitat.simulator.head_stereo_right_depth_sensor.max_zero = False
 # The default value (alpha, xi) is set to match the lens "GoPro" found in Table 3 of this paper:
 # Vladyslav Usenko, Nikolaus Demmel and Daniel Cremers: The Double Sphere
 # Camera Model, The International Conference on 3D Vision (3DV), 2018

habitat-lab/habitat/config/tasks/rearrange/play_spot.yaml

@@ -0,0 +1,197 @@
+habitat:
+    # Config for empty task to explore the scene.
+    environment:
+        max_episode_steps: 0
+    dataset:
+        type: RearrangeDataset-v0
+        split: val
+        data_path: data/datasets/replica_cad/rearrange/v1/{split}/all_receptacles_10k_1k.json.gz
+        scenes_dir: "data/replica_cad/"
+    task:
+        type: RearrangeEmptyTask-v0
+        count_obj_collisions: True
+        desired_resting_position: [0.5, 0.0, 1.0]
+
+        # Reach task config
+        render_target: True
+        ee_sample_factor: 0.8
+
+        # In radians
+        base_angle_noise: 0.0
+        base_noise: 0.0
+        constraint_violation_ends_episode: False
+
+        # If true, does not care about navigability or collisions with objects when spawning
+        # robot
+        easy_init: False
+        force_regenerate: True
+
+        target_start_sensor:
+            type: "TargetStartSensor"
+            goal_format: "CARTESIAN"
+            dimensionality: 3
+        goal_sensor:
+            type: "GoalSensor"
+            goal_format: "CARTESIAN"
+            dimensionality: 3
+        abs_target_start_sensor:
+            type: "AbsTargetStartSensor"
+            goal_format: "CARTESIAN"
+            dimensionality: 3
+        ABS_goal_sensor:
+            type: "AbsGoalSensor"
+            goal_format: "CARTESIAN"
+            dimensionality: 3
+        joint_sensor:
+            type: "JointSensor"
+            dimensionality: 7
+        end_effector_sensor:
+            type: "EEPositionSensor"
+        is_holding_sensor:
+            type: "IsHoldingSensor"
+        sensors: ["joint_sensor"]
+
+        ########################
+        # General measures
+        force_terminate:
+            type: "ForceTerminate"
+            max_accum_force: -1.0
+        robot_force:
+            type: "RobotForce"
+            min_force: 20.0
+        robot_colls:
+            type: "RobotCollisions"
+        object_to_goal_distance:
+            type: "ObjectToGoalDistance"
+        end_effector_to_object_distance:
+            type: "EndEffectorToObjectDistance"
+        end_effector_to_rest_distance:
+            type: "EndEffectorToRestDistance"
+
+        ########################
+        # Pick specific measurements
+        pick_reward:
+            type: "RearrangePickReward"
+            dist_reward: 20.0
+            succ_reward: 10.0
+            pick_reward: 20.0
+            constraint_violate_pen: 10.0
+            drop_pen: 5.0
+            wrong_pick_pen: 5.0
+            max_accum_force: 5000.0
+
+            force_pen: 0.001
+            max_force_pen: 1.0
+            force_end_pen: 10.0
+            use_diff: True
+
+            drop_obj_should_end: False
+            wrong_pick_should_end: False
+        pick_success:
+            type: "RearrangePickSuccess"
+            ee_resting_success_threshold: 0.15
+        ########################
+
+
+        measurements:
+            # General measure
+            - "robot_force"
+        actions:
+            arm_action:
+                type: "ArmAction"
+                arm_controller: "ArmRelPosAction"
+                grip_controller: "MagicGraspAction"
+                arm_joint_dimensionality: 7
+                grasp_thresh_dist: 0.15
+                disable_grip: False
+                delta_pos_limit: 0.0125
+                ee_ctrl_lim: 0.015
+            base_velocity:
+                type: "BaseVelAction"
+                lin_speed: 10.0
+                ang_speed: 10.0
+                allow_dyn_slide: True
+                end_on_stop: False
+                allow_back: True
+                min_abs_lin_speed: 1.0
+                min_abs_ang_speed: 1.0
+            empty:
+                type: "EmptyAction"
+        possible_actions:
+            - arm_action
+            - base_velocity
+            - empty
+
+    simulator:
+        additional_object_paths:
+            - "data/objects/ycb/configs/"
+        action_space_config: v0
+        grasp_impulse: 1000.0
+        hold_thresh: 0.09
+        ac_freq_ratio: 4
+        debug_render: False
+        debug_render_goal: True
+        agents: ['agent_0']
+
+        agent_0:
+            is_set_start_state: False
+            radius: 0.3
+            sensors: ['head_stereo_right_depth_sensor', 'head_stereo_right_rgb_sensor','head_stereo_left_depth_sensor', 'head_stereo_left_rgb_sensor', "arm_rgb_sensor", "arm_depth_sensor"]
+            robot_urdf: data/robots/hab_spot_arm/urdf/hab_spot_arm.urdf
+            robot_type: "SpotRobot"
+            ik_arm_urdf: data/robots/hab_spot_arm/urdf/hab_spot_onlyarm.urdf
+        head_rgb_sensor:
+            width: 128
+            height: 128
+        head_depth_sensor:
+            width: 128
+            height: 128
+            min_depth: 0.0
+            max_depth: 10.0
+            normalize_depth: True
+        arm_depth_sensor:
+            height: 128
+            max_depth: 10.0
+            min_depth: 0.0
+            normalize_depth: True
+            width: 128
+        arm_rgb_sensor:
+            height: 128
+            width: 128
+        head_stereo_left_rgb_sensor:
+            width: 120
+            height: 212
+            hfov: 58
+        head_stereo_right_rgb_sensor:
+            width: 120
+            height: 212
+            hfov: 58
+        head_stereo_left_depth_sensor:
+            width: 120
+            height: 212
+            hfov: 58
+            max_depth: 3.5
+            min_depth: 0.0
+            normalize_depth: True
+            max_zero: False
+        head_stereo_right_depth_sensor:
+            width: 120
+            height: 212
+            hfov: 58
+            max_depth: 3.5
+            min_depth: 0.0
+            normalize_depth: True
+            max_zero: False
+        ctrl_freq: 120.0
+        default_agent_id: 0
+        forward_step_size: 0.25
+        habitat_sim_v0:
+            allow_sliding: True
+            enable_physics: True
+            gpu_device_id: 0
+            gpu_gpu: False
+            physics_config_file: ./data/default.physics_config.json
+        seed: 100
+        type: RearrangeSim-v0
+        contact_test: True
+        nav_mesh: True

habitat-lab/habitat/robots/__init__.py

@@ -12,6 +12,7 @@
     RobotCameraParams,
 )
 from habitat.robots.robot_interface import RobotInterface
+from habitat.robots.spot_robot import SpotRobot
 from habitat.robots.static_manipulator import (
     StaticManipulator,
     StaticManipulatorParams,
@@ -23,6 +24,7 @@
     "MobileManipulator",
     "FetchRobot",
     "FetchRobotNoWheels",
+    "SpotRobot",
     "RobotCameraParams",
     "StaticManipulatorParams",
     "StaticManipulator",

habitat-lab/habitat/robots/manipulator.py

@@ -400,23 +400,6 @@ def arm_motor_forces(self, ctrl: List[float]) -> None:
         """Set the desired torques of the arm joint motors"""
         self.sim_obj.joint_forces = ctrl
 
-    def _validate_joint_idx(self, joint):
-        if joint not in self.joint_motors:
-            raise ValueError(
-                f"Requested joint {joint} not in joint motors with indices (keys {self.joint_motors.keys()}) and {self.joint_motors}"
-            )
-
-    def _set_motor_pos(self, joint, ctrl):
-        self._validate_joint_idx(joint)
-        self.joint_motors[joint][1].position_target = ctrl
-        self.sim_obj.update_joint_motor(
-            self.joint_motors[joint][0], self.joint_motors[joint][1]
-        )
-
-    def _get_motor_pos(self, joint):
-        self._validate_joint_idx(joint)
-        return self.joint_motors[joint][1].position_target
-
     def _set_joint_pos(self, joint_idx, angle):
         # NOTE: This is pretty inefficient and should not be used iteratively
         set_pos = self.sim_obj.joint_positions
@@ -444,15 +427,3 @@ def _interpolate_arm_control(
             if get_observations:
                 observations.append(self._sim.get_sensor_observations())
         return observations
-
-    def _update_motor_settings_cache(self):
-        """Updates the JointMotorSettings cache for cheaper future updates"""
-        self.joint_motors = {}
-        for (
-            motor_id,
-            joint_id,
-        ) in self.sim_obj.existing_joint_motor_ids.items():
-            self.joint_motors[joint_id] = (
-                motor_id,
-                self.sim_obj.get_joint_motor_settings(motor_id),
-            )

habitat-lab/habitat/robots/mobile_manipulator.py

@@ -26,8 +26,9 @@ class RobotCameraParams:
     """
 
     attached_link_id: int
-    cam_offset_pos: mn.Vector3
-    cam_look_at_pos: mn.Vector3
+    cam_offset_pos: mn.Vector3 = mn.Vector3(0, 0, 0)
+    cam_look_at_pos: mn.Vector3 = mn.Vector3(0, 0, 0)
+    cam_orientation: mn.Vector3 = mn.Vector3(0, 0, 0)
     relative_transform: mn.Matrix4 = mn.Matrix4.identity_init()
 
 
@@ -106,6 +107,7 @@ def __init__(
         sim: Simulator,
         limit_robo_joints: bool = True,
         fixed_base: bool = True,
+        base_type="mobile",
     ):
         r"""Constructor
         :param params: The parameter of the manipulator robot.
@@ -114,6 +116,7 @@ def __init__(
         :param limit_robo_joints: If true, joint limits of robot are always
             enforced.
         :param fixed_base: If the robot's base is fixed or not.
+        :param base_type: The base type
         """
         # instantiate a manipulator
         Manipulator.__init__(
@@ -132,7 +135,7 @@ def __init__(
             limit_robo_joints=limit_robo_joints,
             fixed_based=fixed_base,
             sim_obj=self.sim_obj,
-            base_type="mobile",
+            base_type=base_type,
         )
 
     def reconfigure(self) -> None: