Motion Planning, Gym Wrapper, Compose Agents, Hab2 Bug fixes (#685)

The next version of the Habitat 2.0 release, this PR adds motion planning, an OpenAI Gym wrapper, bug fixes, and the initial set of features necessary to compose agent policies to record demonstrations with motion planning.

Author: ASzot

configs/tasks/rearrange_reach_replica_cad.yaml

@@ -0,0 +1,104 @@
+ENVIRONMENT:
+    MAX_EPISODE_STEPS: 50
+DATASET:
+    TYPE: RearrangeDataset-v0
+    SPLIT: train
+    DATA_PATH: data/datasets/rearrange_pick/replica_cad/v0/{split}/{split}_counter_L_analysis_5000_500.json.gz
+    SCENES_DIR: "data/replica_cad/"
+TASK:
+    TYPE: RearrangeReachTask-v0
+    RENDER_TARGET: True
+    EE_SAMPLE_FACTOR: 0.5
+
+    JOINT_SENSOR:
+        TYPE: "JointSensor"
+        DIMENSIONALITY: 7
+    RELATIVE_RESTING_POS_SENSOR:
+        TYPE: "RelativeRestingPositionSensor"
+    RESTING_POS_SENSOR:
+        TYPE: "RestingPositionSensor"
+    END_EFFECTOR_SENSOR:
+        TYPE: "EEPositionSensor"
+    SENSORS: ["JOINT_SENSOR", "RELATIVE_RESTING_POS_SENSOR", 'END_EFFECTOR_SENSOR', "RESTING_POS_SENSOR"]
+
+    END_EFFECTOR_TO_REST_DISTANCE:
+        TYPE: "EndEffectorToRestDistance"
+    REARRANGE_REACH_REWARD:
+        TYPE: "RearrangeReachReward"
+        SCALE: 1.0
+        DIFF_REWARD: True
+    REARRANGE_REACH_SUCCESS:
+        TYPE: "RearrangeReachSuccess"
+        SUCC_THRESH: 0.1
+    MEASUREMENTS:
+        - "END_EFFECTOR_TO_REST_DISTANCE"
+        - "REARRANGE_REACH_REWARD"
+        - "REARRANGE_REACH_SUCCESS"
+    ACTIONS:
+        ARM_ACTION:
+            TYPE: "ArmAction"
+            ARM_CONTROLLER: "ArmRelPosAction"
+            ARM_JOINT_DIMENSIONALITY: 7
+            GRIP_CONTROLLER: null
+            EE_CTRL_LIM: 0.025
+            GRASP_THRESH_DIST: 0.15
+            DELTA_POS_LIMIT: 0.025
+            RENDER_EE_TARGET: True
+    POSSIBLE_ACTIONS:
+        - ARM_ACTION
+
+SIMULATOR:
+    ACTION_SPACE_CONFIG: v0
+    AGENTS: ['AGENT_0']
+    AGENT_0:
+        ANGULAR_ACCELERATION: 12.56
+        ANGULAR_FRICTION: 1.0
+        COEFFICIENT_OF_RESTITUTION: 0.0
+        HEIGHT: 1.5
+        IS_SET_START_STATE: False
+        LINEAR_ACCELERATION: 20.0
+        LINEAR_FRICTION: 0.5
+        MASS: 32.0
+        RADIUS: 0.1
+        SENSORS: ['HEAD_RGB_SENSOR', 'THIRD_RGB_SENSOR']
+        START_POSITION: [0, 0, 0]
+        START_ROTATION: [0, 0, 0, 1]
+    HEAD_RGB_SENSOR:
+        WIDTH: 128
+        HEIGHT: 128
+    THIRD_RGB_SENSOR:
+        WIDTH: 512
+        HEIGHT: 512
+
+    # Agent setup
+    ARM_REST: [0.6, 0.0, 0.9]
+    CTRL_FREQ: 120.0
+    AC_FREQ_RATIO: 4
+    ROBOT_URDF: ./data/robots/hab_fetch/robots/hab_fetch.urdf
+    FORWARD_STEP_SIZE: 0.25
+    LAG_OBSERVATIONS: 0
+    IK_ARM_URDF: null
+    LOAD_OBJS: False
+
+    # Grasping
+    HOLD_THRESH: 0.09
+    GRASP_IMPULSE: 1000.0
+
+    DEFAULT_AGENT_ID: 0
+    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
+    SEMANTIC_SENSOR:
+        HEIGHT: 480
+        HFOV: 90
+        ORIENTATION: [0.0, 0.0, 0.0]
+        POSITION: [0, 1.25, 0]
+        TYPE: HabitatSimSemanticSensor
+        WIDTH: 640
+    TILT_ANGLE: 15
+    TURN_ANGLE: 10
+    TYPE: RearrangeSim-v0

configs/tasks/rearrangepick_replica_cad.yaml

@@ -37,13 +37,16 @@ TASK:
         DIMENSIONALITY: 3
     JOINT_SENSOR:
         TYPE: "JointSensor"
-    EE_SENSOR:
-        TYPE: "EeSensor"
+        DIMENSIONALITY: 7
+    END_EFFECTOR_SENSOR:
+        TYPE: "EEPositionSensor"
     IS_HOLDING_SENSOR:
         TYPE: "IsHoldingSensor"
+    RELATIVE_RESTING_POS_SENSOR:
+        TYPE: "RelativeRestingPositionSensor"
     LOCAL_SENSOR:
         TYPE: "LocalizationSensor"
-    SENSORS: ["TARGET_START_SENSOR", "GOAL_SENSOR", "JOINT_SENSOR", "IS_HOLDING_SENSOR", "EE_SENSOR", "LOCAL_SENSOR"]
+    SENSORS: ["TARGET_START_SENSOR", "GOAL_SENSOR", "JOINT_SENSOR", "IS_HOLDING_SENSOR", "END_EFFECTOR_SENSOR", "LOCAL_SENSOR", "RELATIVE_RESTING_POS_SENSOR"]
     ROBOT_FORCE:
         TYPE: "RobotForce"
         MIN_FORCE: 20.0
@@ -53,6 +56,8 @@ TASK:
         TYPE: "ObjectToGoalDistance"
     END_EFFECTOR_TO_OBJECT_DISTANCE:
         TYPE: "EndEffectorToObjectDistance"
+    END_EFFECTOR_TO_REST_DISTANCE:
+        TYPE: "EndEffectorToRestDistance"
     REARRANGE_PICK_REWARD:
         TYPE: "RearrangePickReward"
         DIST_REWARD: 20.0
@@ -81,17 +86,19 @@ TASK:
         - "OBJECT_TO_GOAL_DISTANCE"
         - "ROBOT_FORCE"
         - "ROBOT_COLLS"
+        - "END_EFFECTOR_TO_REST_DISTANCE"
         - "END_EFFECTOR_TO_OBJECT_DISTANCE"
         - "REARRANGE_PICK_SUCCESS"
         - "REARRANGE_PICK_REWARD"
     ACTIONS:
         ARM_ACTION:
             TYPE: "ArmAction"
-            ARM_CONTROLLER: "ArmVelAction"
+            ARM_CONTROLLER: "ArmRelPosAction"
             GRIP_CONTROLLER: "MagicGraspAction"
+            ARM_JOINT_DIMENSIONALITY: 7
             GRASP_THRESH_DIST: 0.15
             DISABLE_GRIP: False
-            VEL_CTRL_LIM: 0.0125
+            DELTA_POS_LIMIT: 0.0125
     POSSIBLE_ACTIONS:
         - ARM_ACTION
 
@@ -137,7 +144,6 @@ SIMULATOR:
     ROBOT_URDF: ./data/robots/hab_fetch/robots/hab_fetch.urdf
     FORWARD_STEP_SIZE: 0.25
     LAG_OBSERVATIONS: 1
-    HIGH_RES_DIM: 512
 
     # Grasping
     HOLD_THRESH: 0.09

configs/tasks/rearrangepick_replica_cad_example.yaml

@@ -29,6 +29,10 @@ TASK:
         TYPE: "GoalSensor"
         GOAL_FORMAT: "CARTESIAN"
         DIMENSIONALITY: 3
+    REL_RESTING_POSITION_SENSOR:
+        TYPE: "RelativeRestingPositionSensor"
+        GOAL_FORMAT: "CARTESIAN"
+        DIMENSIONALITY: 3
     ABS_TARGET_START_SENSOR:
         TYPE: "AbsTargetStartSensor"
         GOAL_FORMAT: "CARTESIAN"
@@ -39,13 +43,14 @@ TASK:
         DIMENSIONALITY: 3
     JOINT_SENSOR:
         TYPE: "JointSensor"
-    EE_SENSOR:
-        TYPE: "EeSensor"
+        DIMENSIONALITY: 7
+    END_EFFECTOR_SENSOR:
+        TYPE: "EEPositionSensor"
     IS_HOLDING_SENSOR:
         TYPE: "IsHoldingSensor"
     LOCAL_SENSOR:
         TYPE: "LocalizationSensor"
-    SENSORS: ["TARGET_START_SENSOR", "GOAL_SENSOR", "JOINT_SENSOR", "IS_HOLDING_SENSOR", "EE_SENSOR", "LOCAL_SENSOR"]
+    SENSORS: ["TARGET_START_SENSOR", "REL_RESTING_POSITION_SENSOR", "GOAL_SENSOR", "JOINT_SENSOR", "IS_HOLDING_SENSOR", "END_EFFECTOR_SENSOR", "LOCAL_SENSOR"]
     ROBOT_FORCE:
         TYPE: "RobotForce"
         MIN_FORCE: 20.0
@@ -55,6 +60,8 @@ TASK:
         TYPE: "ObjectToGoalDistance"
     END_EFFECTOR_TO_OBJECT_DISTANCE:
         TYPE: "EndEffectorToObjectDistance"
+    END_EFFECTOR_TO_REST_DISTANCE:
+        TYPE: "EndEffectorToRestDistance"
     REARRANGE_PICK_REWARD:
         TYPE: "RearrangePickReward"
         DIST_REWARD: 20.0
@@ -83,21 +90,24 @@ TASK:
         - "OBJECT_TO_GOAL_DISTANCE"
         - "ROBOT_FORCE"
         - "ROBOT_COLLS"
+        - "END_EFFECTOR_TO_REST_DISTANCE"
         - "END_EFFECTOR_TO_OBJECT_DISTANCE"
         - "REARRANGE_PICK_SUCCESS"
         - "REARRANGE_PICK_REWARD"
     ACTIONS:
         ARM_ACTION:
             TYPE: "ArmAction"
-            ARM_CONTROLLER: "ArmVelAction"
+            # Uncomment to run with velocity control
+            ARM_CONTROLLER: "ArmRelPosAction"
 
             # Uncomment to run with end-effector control
             #ARM_CONTROLLER: "ArmEEAction"
 
             GRIP_CONTROLLER: "MagicGraspAction"
+            ARM_JOINT_DIMENSIONALITY: 7
             GRASP_THRESH_DIST: 0.15
             DISABLE_GRIP: False
-            VEL_CTRL_LIM: 0.0125
+            DELTA_POS_LIMIT: 0.0125
             EE_CTRL_LIM: 0.015
         BASE_VELOCITY:
             TYPE: "BaseVelAction"
@@ -165,12 +175,9 @@ SIMULATOR:
         GPU_DEVICE_ID: 0
         GPU_GPU: False
         PHYSICS_CONFIG_FILE: ./data/default.physics_config.json
-    HIGH_RES_DIM: 512
     LAG_OBSERVATIONS: 1
-    SHOULD_RENDER_DEBUG: True
     ROBOT_URDF: ./data/robots/hab_fetch/robots/hab_fetch.urdf
-    # Uncomment to use IK control
-    #IK_ARM_URDF: ./data/robots/hab_fetch/robots/fetch_onlyarm.urdf
+    IK_ARM_URDF: ./data/robots/hab_fetch/robots/fetch_onlyarm.urdf
     SEED: 100
     SEMANTIC_SENSOR:
         HEIGHT: 480

configs/tasks/rearrangepick_replica_cad_example_ik.yaml

@@ -10,6 +10,10 @@ TASK:
     COUNT_OBJ_COLLISIONS: True
     COUNT_ROBOT_OBJ_COLLS: False
 
+    # Reach task config
+    RENDER_TARGET: True
+    EE_SAMPLE_FACTOR: 0.8
+
     # In radians
     #BASE_ANGLE_NOISE: 0.15
     #BASE_NOISE: 0.05
@@ -39,13 +43,16 @@ TASK:
         DIMENSIONALITY: 3
     JOINT_SENSOR:
         TYPE: "JointSensor"
-    EE_SENSOR:
-        TYPE: "EeSensor"
+        DIMENSIONALITY: 7
+    END_EFFECTOR_SENSOR:
+        TYPE: "EEPositionSensor"
     IS_HOLDING_SENSOR:
         TYPE: "IsHoldingSensor"
     LOCAL_SENSOR:
         TYPE: "LocalizationSensor"
-    SENSORS: ["TARGET_START_SENSOR", "GOAL_SENSOR", "JOINT_SENSOR", "IS_HOLDING_SENSOR", "EE_SENSOR", "LOCAL_SENSOR"]
+    RELATIVE_RESTING_POS_SENSOR:
+        TYPE: "RelativeRestingPositionSensor"
+    SENSORS: ["TARGET_START_SENSOR", "GOAL_SENSOR", "JOINT_SENSOR", "IS_HOLDING_SENSOR", "END_EFFECTOR_SENSOR", "LOCAL_SENSOR", "RELATIVE_RESTING_POS_SENSOR"]
     ROBOT_FORCE:
         TYPE: "RobotForce"
         MIN_FORCE: 20.0
@@ -55,6 +62,8 @@ TASK:
         TYPE: "ObjectToGoalDistance"
     END_EFFECTOR_TO_OBJECT_DISTANCE:
         TYPE: "EndEffectorToObjectDistance"
+    END_EFFECTOR_TO_REST_DISTANCE:
+        TYPE: "EndEffectorToRestDistance"
     REARRANGE_PICK_REWARD:
         TYPE: "RearrangePickReward"
         DIST_REWARD: 20.0
@@ -84,16 +93,18 @@ TASK:
         - "ROBOT_FORCE"
         - "ROBOT_COLLS"
         - "END_EFFECTOR_TO_OBJECT_DISTANCE"
+        - "END_EFFECTOR_TO_REST_DISTANCE"
         - "REARRANGE_PICK_SUCCESS"
         - "REARRANGE_PICK_REWARD"
     ACTIONS:
         ARM_ACTION:
             TYPE: "ArmAction"
             ARM_CONTROLLER: "ArmEEAction"
             GRIP_CONTROLLER: "MagicGraspAction"
+            ARM_JOINT_DIMENSIONALITY: 7
             GRASP_THRESH_DIST: 0.15
             DISABLE_GRIP: False
-            VEL_CTRL_LIM: 0.0125
+            DELTA_POS_LIMIT: 0.0125
             EE_CTRL_LIM: 0.015
         BASE_VELOCITY:
             TYPE: "BaseVelAction"
@@ -161,9 +172,7 @@ SIMULATOR:
         GPU_DEVICE_ID: 0
         GPU_GPU: False
         PHYSICS_CONFIG_FILE: ./data/default.physics_config.json
-    HIGH_RES_DIM: 512
     LAG_OBSERVATIONS: 1
-    SHOULD_RENDER_DEBUG: True
     ROBOT_URDF: ./data/robots/hab_fetch/robots/hab_fetch.urdf
     IK_ARM_URDF: ./data/robots/hab_fetch/robots/fetch_onlyarm.urdf
     SEED: 100