From c5c18ee2760eb7a01d5c18598d9e74cc64112fd0 Mon Sep 17 00:00:00 2001 From: Junyao Shi Date: Fri, 27 Aug 2021 18:54:46 -0700 Subject: [PATCH] added igibson environment files --- .../igibson/configs/turtlebot_clip.yaml | 76 ++ .../configs/turtlebot_object_nav_stadium.yaml | 78 ++ .../configs/turtlebot_point_nav_stadium.yaml | 74 ++ .../igibson/docker/10_nvidia.json | 6 + .../igibson/docker/Dockerfile.igibson | 169 ++++ .../igibson/docker/build_igibson_docker.sh | 12 + .../igibson/docker/nvidia_icd.json | 7 + .../igibson/docker/run_igibson_docker.sh | 10 + alf/environments/igibson/docker/set_env.sh | 26 + alf/environments/igibson/docker/sources.list | 10 + alf/environments/igibson/igibson_env.py | 104 +++ alf/environments/igibson/igibson_object.py | 48 ++ alf/environments/igibson/igibson_tasks.py | 816 ++++++++++++++++++ alf/environments/igibson/manual_control.py | 132 +++ .../igibson/manual_control_with_clip.py | 246 ++++++ alf/environments/igibson/suite_igibson.py | 384 +++++++++ .../igibson/suite_igibson_test.py | 135 +++ 17 files changed, 2333 insertions(+) create mode 100644 alf/environments/igibson/configs/turtlebot_clip.yaml create mode 100644 alf/environments/igibson/configs/turtlebot_object_nav_stadium.yaml create mode 100644 alf/environments/igibson/configs/turtlebot_point_nav_stadium.yaml create mode 100644 alf/environments/igibson/docker/10_nvidia.json create mode 100644 alf/environments/igibson/docker/Dockerfile.igibson create mode 100755 alf/environments/igibson/docker/build_igibson_docker.sh create mode 100644 alf/environments/igibson/docker/nvidia_icd.json create mode 100755 alf/environments/igibson/docker/run_igibson_docker.sh create mode 100755 alf/environments/igibson/docker/set_env.sh create mode 100644 alf/environments/igibson/docker/sources.list create mode 100644 alf/environments/igibson/igibson_env.py create mode 100644 alf/environments/igibson/igibson_object.py create mode 100644 alf/environments/igibson/igibson_tasks.py create mode 100644 alf/environments/igibson/manual_control.py create mode 100644 alf/environments/igibson/manual_control_with_clip.py create mode 100644 alf/environments/igibson/suite_igibson.py create mode 100644 alf/environments/igibson/suite_igibson_test.py diff --git a/alf/environments/igibson/configs/turtlebot_clip.yaml b/alf/environments/igibson/configs/turtlebot_clip.yaml new file mode 100644 index 000000000..5984ce2a2 --- /dev/null +++ b/alf/environments/igibson/configs/turtlebot_clip.yaml @@ -0,0 +1,76 @@ +# scene +scene: igibson +scene_id: Rs_int +build_graph: true +load_texture: true +pybullet_load_texture: true +trav_map_type: no_obj +trav_map_resolution: 0.1 +trav_map_erosion: 2 +should_open_all_doors: true + +# domain randomization +texture_randomization_freq: null +object_randomization_freq: null + +# robot +robot: Turtlebot +is_discrete: false +velocity: 1.0 +robot_scale: 1.0 + +# task +task: point_nav_random +target_dist_min: 1.0 +target_dist_max: 10.0 +goal_format: polar +task_obs_dim: 4 + +# reward +reward_type: geodesic +success_reward: 10.0 +potential_reward_weight: 1.0 +collision_reward_weight: -0.1 + +# discount factor +discount_factor: 0.99 + +# termination condition +dist_tol: 0.36 # body width +max_step: 500 +max_collisions_allowed: 500 + +# misc config +initial_pos_z_offset: 0.1 +collision_ignore_link_a_ids: [1, 2, 3, 4] # ignore collisions with these robot links + +# sensor spec +output: [task_obs, rgb, depth] # scan +# image +# ASUS Xtion PRO LIVE +# https://www.asus.com/us/3D-Sensor/Xtion_PRO_LIVE +fisheye: false +image_width: 320 # 160 +image_height: 320 # 120 +vertical_fov: 75 +# depth +depth_low: 0.8 +depth_high: 3.5 +# scan +# Hokuyo URG-04LX-UG01 +# https://www.hokuyo-aut.jp/search/single.php?serial=166 +# n_horizontal_rays is originally 683, sub-sampled 1/3 +n_horizontal_rays: 228 +n_vertical_beams: 1 +laser_linear_range: 5.6 +laser_angular_range: 240.0 +min_laser_dist: 0.05 +laser_link_name: scan_link + +# sensor noise +depth_noise_rate: 0.0 +scan_noise_rate: 0.0 + +# visual objects +visual_object_at_initial_target_pos: true +target_visual_object_visible_to_agent: false diff --git a/alf/environments/igibson/configs/turtlebot_object_nav_stadium.yaml b/alf/environments/igibson/configs/turtlebot_object_nav_stadium.yaml new file mode 100644 index 000000000..bc2db2008 --- /dev/null +++ b/alf/environments/igibson/configs/turtlebot_object_nav_stadium.yaml @@ -0,0 +1,78 @@ +# scene +scene: stadium +build_graph: false +load_texture: true +pybullet_load_texture: true +trav_map_type: no_obj +trav_map_resolution: 0.1 +trav_map_erosion: 2 +should_open_all_doors: true + +# domain randomization +texture_randomization_freq: null +object_randomization_freq: null + +# robot +robot: Turtlebot +is_discrete: false +velocity: 1.0 + +# task +task: visual_object_nav +object_dist_min: 4.0 +object_dist_max: 12.0 +object_dist_keepout: 3.0 # minimum distances between objects +object_keepout_buffer_dist: 0.5 # object_keepout_buffer_dist + max object radius * 2 = actual object keepout dist +num_objects: 5 +goal_format: polar +task_obs_dim: 5 + +# reward +reward_type: l2 +success_reward: 10.0 # 10.0 +potential_reward_weight: 5.0 # 1.0 +collision_reward_weight: -0.5 # -0.1 + +# discount factor +discount_factor: 0.99 + +# termination condition +dist_tol: 1.0 # 0.5 # 0.36 # body width +max_step: 600 # 500 +max_collisions_allowed: 600 # 500 +goal_buffer_dist: 1.2 # goal_buffer_dist + max object radius = actual dist_tol + +# misc config +initial_pos_z_offset: 0.1 +collision_ignore_link_a_ids: [1, 2, 3, 4] # ignore collisions with these robot links + +# sensor spec +output: [task_obs, rgb, depth] # , scan +# image +# ASUS Xtion PRO LIVE +# https://www.asus.com/us/3D-Sensor/Xtion_PRO_LIVE +fisheye: true +image_width: 84 # 160 +image_height: 84 # 120 +vertical_fov: 45 +# depth +depth_low: 0.8 +depth_high: 3.5 +# scan +# Hokuyo URG-04LX-UG01 +# https://www.hokuyo-aut.jp/search/single.php?serial=166 +# n_horizontal_rays is originally 683, sub-sampled 1/3 +n_horizontal_rays: 228 +n_vertical_beams: 1 +laser_linear_range: 5.6 +laser_angular_range: 240.0 +min_laser_dist: 0.05 +laser_link_name: scan_link + +# sensor noise +depth_noise_rate: 0.0 +scan_noise_rate: 0.0 + +# visual objects +visual_object_at_initial_pos: true +target_visual_object_visible_to_agent: true diff --git a/alf/environments/igibson/configs/turtlebot_point_nav_stadium.yaml b/alf/environments/igibson/configs/turtlebot_point_nav_stadium.yaml new file mode 100644 index 000000000..7fae75cb2 --- /dev/null +++ b/alf/environments/igibson/configs/turtlebot_point_nav_stadium.yaml @@ -0,0 +1,74 @@ +# scene +scene: stadium +build_graph: false +load_texture: true +pybullet_load_texture: true +trav_map_type: no_obj +trav_map_resolution: 0.1 +trav_map_erosion: 2 +should_open_all_doors: true + +# domain randomization +texture_randomization_freq: null +object_randomization_freq: null + +# robot +robot: Turtlebot +is_discrete: false +velocity: 1.0 + +# task +task: None # point_nav_random +target_dist_min: 5.0 +target_dist_max: 10.0 # 10.0 +goal_format: polar +task_obs_dim: 4 + +# reward +reward_type: l2 +success_reward: 10.0 # 10.0 +potential_reward_weight: 5.0 # 1.0 +collision_reward_weight: 0.0 # -0.1 + +# discount factor +discount_factor: 0.99 + +# termination condition +dist_tol: 0.5 # 0.5 # 0.36 # body width +max_step: 500 +max_collisions_allowed: 500 + +# misc config +initial_pos_z_offset: 0.1 +collision_ignore_link_a_ids: [1, 2, 3, 4] # ignore collisions with these robot links + +# sensor spec +output: [rgb, depth] # task_obs, , scan +# image +# ASUS Xtion PRO LIVE +# https://www.asus.com/us/3D-Sensor/Xtion_PRO_LIVE +fisheye: true +image_width: 84 # 160 +image_height: 84 # 120 +vertical_fov: 45 +# depth +depth_low: 0.8 +depth_high: 3.5 +# scan +# Hokuyo URG-04LX-UG01 +# https://www.hokuyo-aut.jp/search/single.php?serial=166 +# n_horizontal_rays is originally 683, sub-sampled 1/3 +n_horizontal_rays: 228 +n_vertical_beams: 1 +laser_linear_range: 5.6 +laser_angular_range: 240.0 +min_laser_dist: 0.05 +laser_link_name: scan_link + +# sensor noise +depth_noise_rate: 0.0 +scan_noise_rate: 0.0 + +# visual objects +visual_object_at_initial_target_pos: true +target_visual_object_visible_to_agent: true diff --git a/alf/environments/igibson/docker/10_nvidia.json b/alf/environments/igibson/docker/10_nvidia.json new file mode 100644 index 000000000..2bfcca059 --- /dev/null +++ b/alf/environments/igibson/docker/10_nvidia.json @@ -0,0 +1,6 @@ +{ + "file_format_version" : "1.0.0", + "ICD" : { + "library_path" : "libEGL_nvidia.so.0" + } +} diff --git a/alf/environments/igibson/docker/Dockerfile.igibson b/alf/environments/igibson/docker/Dockerfile.igibson new file mode 100644 index 000000000..60c183eca --- /dev/null +++ b/alf/environments/igibson/docker/Dockerfile.igibson @@ -0,0 +1,169 @@ +ARG CUDA_BASE=docker.hobot.cc/dlp/tf:runtime-py3.6-cudnn7.4-cuda10.0-ubuntu18.04 + +########################## Build Vulkan ################################# +# Adapted from: +# https://gitlab.com/nvidia/container-images/opengl/-/blob/ubuntu18.04/base/Dockerfile +# https://gitlab.com/nvidia/container-images/opengl/-/blob/ubuntu18.04/glvnd/runtime/Dockerfile +# https://gitlab.com/nvidia/container-images/vulkan/-/blob/master/docker/Dockerfile.ubuntu18.04 + +FROM ubuntu:18.04 as vulkan-khronos + +RUN apt-get update && apt-get install -y --no-install-recommends \ + build-essential \ + ca-certificates \ + cmake \ + git \ + libegl1-mesa-dev \ + libwayland-dev \ + libx11-xcb-dev \ + libxkbcommon-dev \ + libxrandr-dev \ + python3 \ + python3-distutils \ + wget && \ + rm -rf /var/lib/apt/lists/* + +ARG VULKAN_VERSION=sdk-1.1.121.0 + +# Download and compile vulkan components +RUN ln -s /usr/bin/python3 /usr/bin/python && \ + git clone https://github.com/KhronosGroup/Vulkan-ValidationLayers.git /opt/vulkan && \ + cd /opt/vulkan && git checkout "${VULKAN_VERSION}" && \ + mkdir build && cd build && ../scripts/update_deps.py && \ + cmake -C helper.cmake -DCMAKE_BUILD_TYPE=Release .. && \ + cmake --build . && make install && ldconfig && \ + mkdir -p /usr/local/lib && cp -a Vulkan-Loader/build/install/lib/* /usr/local/lib && \ + mkdir -p /usr/local/include/vulkan && cp -r Vulkan-Headers/build/install/include/vulkan/* /usr/local/include/vulkan && \ + mkdir -p /usr/local/share/vulkan/registry && \ + cp -r Vulkan-Headers/build/install/share/vulkan/registry/* /usr/local/share/vulkan/registry && \ + rm -rf /opt/vulkan + +FROM ${CUDA_BASE} + +ENV NVIDIA_DRIVER_CAPABILITIES compute,graphics,utility +RUN echo "/usr/local/nvidia/lib" >> /etc/ld.so.conf.d/nvidia.conf \ + && echo "/usr/local/nvidia/lib64" >> /etc/ld.so.conf.d/nvidia.conf +ENV LD_LIBRARY_PATH=/usr/lib/x86_64-linux-gnu:/usr/lib/i386-linux-gnu:/usr/local/nvidia/lib:/usr/local/nvidia/lib64:/usr/local/nvidia/lib:/usr/local/nvidia/lib64 + +RUN dpkg --add-architecture i386 && apt-get update \ + && apt-get install -y --no-install-recommends --allow-change-held-packages \ + libxau6 libxau6:i386 \ + libxdmcp6 libxdmcp6:i386 \ + libxcb1 libxcb1:i386 \ + libxext6 libxext6:i386 \ + libx11-6 libx11-6:i386 \ + libglvnd0 libglvnd0:i386 \ + libgl1 libgl1:i386 \ + libglx0 libglx0:i386 \ + libegl1 libegl1:i386 \ + libgles2 libgles2:i386 \ + pkg-config \ + libglvnd-dev libglvnd-dev:i386 \ + libgl1-mesa-dev libgl1-mesa-dev:i386 \ + libegl1-mesa-dev libegl1-mesa-dev:i386 \ + libgles2-mesa-dev libgles2-mesa-dev:i386 \ + libx11-xcb-dev \ + libxkbcommon-dev \ + libwayland-dev \ + libxrandr-dev \ + libegl1-mesa-dev + +COPY --from=vulkan-khronos /usr/local/bin /usr/local/bin +COPY --from=vulkan-khronos /usr/local/lib /usr/local/lib +COPY --from=vulkan-khronos /usr/local/include/vulkan /usr/local/include/vulkan +COPY --from=vulkan-khronos /usr/local/share/vulkan /usr/local/share/vulkan +COPY ./nvidia_icd.json /etc/vulkan/icd.d/nvidia_icd.json +COPY ./10_nvidia.json /usr/share/glvnd/egl_vendor.d/10_nvidia.json + +# install packages as written in nvidia/cudagl:10.0-base-ubuntu18.04 +RUN apt-get update && apt-get install -y --no-install-recommends --allow-change-held-packages \ + pkg-config \ + libglvnd-dev libglvnd-dev:i386 \ + libgl1-mesa-dev libgl1-mesa-dev:i386 \ + libegl1-mesa-dev libegl1-mesa-dev:i386 \ + libgles2-mesa-dev libgles2-mesa-dev:i386 && \ + rm -rf /var/lib/apt/lists/* + +# replace source list +COPY ./sources.list /etc/apt/sources.list +ARG CUDA=10.0 +ARG CUDNN=7.6.2.24-1 + +# install packages based on https://github.com/StanfordVL/iGibson/blob/master/docker/base/Dockerfile +RUN apt-get update && apt-get install -y --no-install-recommends --allow-change-held-packages \ + curl build-essential git cmake \ + cuda-command-line-tools-10-0 \ + cuda-cublas-10-0 \ + cuda-cufft-10-0 \ + cuda-curand-10-0 \ + cuda-cusolver-10-0 \ + cuda-cusparse-10-0 \ + libcudnn7=${CUDNN}+cuda${CUDA} \ + vim \ + tmux \ + libhdf5-dev \ + libsm6 \ + libxext6 \ + libxrender-dev \ + wget + +# basic apt software packages +RUN apt update && apt install -y \ + vim emacs tree wget unzip \ + git cmake \ + libxml2 libxml2-dev libxslt1-dev \ + dirmngr gnupg2 lsb-release \ + xvfb kmod swig patchelf \ + libopenmpi-dev libcups2-dev libssl-dev libosmesa6-dev \ + python3-pip ffmpeg libegl-mesa0 libegl1 +# libegl1-mesa-dev libnvidia-gl-440 + +# Install anaconda3 to /anaconda3 +RUN curl -LO https://repo.anaconda.com/archive/Anaconda3-2021.05-Linux-x86_64.sh +RUN bash Anaconda3-2021.05-Linux-x86_64.sh -p /anaconda3 -b +RUN rm Anaconda3-2021.05-Linux-x86_64.sh +ENV PATH=/anaconda3/bin:${PATH} +RUN conda update -y conda +RUN conda create -y -n igibson python=3.7 + +# Python packages from conda +ENV PATH /anaconda3/envs/igibson/bin:$PATH +RUN pip install --upgrade pip +RUN pip install pytest +RUN pip install tensorflow-gpu==1.15.0 + +# install alf requirements +RUN git clone --branch pytorch https://github.com/HorizonRobotics/alf.git /home/alf +WORKDIR /home/alf +RUN pip install -e . + +# install iGibson +RUN git clone --branch master https://github.com/junyaoshi/iGibson.git /opt/igibson --recursive +WORKDIR /opt/igibson +RUN pip install -e . + +# install agents +RUN git clone https://github.com/StanfordVL/agents/ /opt/agents +WORKDIR /opt/agents +RUN pip install -e . + +# update pybullet to more efficient version +RUN pip uninstall -y pybullet +RUN pip install https://github.com/StanfordVL/bullet3/archive/master.zip + +# download iGibson data +RUN python -m igibson.utils.assets_utils --download_assets +RUN python -m igibson.utils.assets_utils --download_ig_dataset +RUN python -m igibson.utils.assets_utils --download_demo_data + +# clean +RUN rm -rf /var/lib/apt/lists/* \ + && rm -rf /tmp/package \ + && rm -rf /install \ + && rm -rf /home + +RUN apt update && apt install -y rsync + +COPY ./set_env.sh /opt/set_env.sh + +WORKDIR /home diff --git a/alf/environments/igibson/docker/build_igibson_docker.sh b/alf/environments/igibson/docker/build_igibson_docker.sh new file mode 100755 index 000000000..13154cb74 --- /dev/null +++ b/alf/environments/igibson/docker/build_igibson_docker.sh @@ -0,0 +1,12 @@ +#!/bin/bash + +while getopts f:t: flag +do + case "${flag}" in + f) Dockerfile=${OPTARG};; + t) tag=${OPTARG};; + esac +done +echo "Building docker image: $tag from Dockerfile: $Dockerfile"; + +docker build -f $Dockerfile --tag $tag . diff --git a/alf/environments/igibson/docker/nvidia_icd.json b/alf/environments/igibson/docker/nvidia_icd.json new file mode 100644 index 000000000..e36e070e4 --- /dev/null +++ b/alf/environments/igibson/docker/nvidia_icd.json @@ -0,0 +1,7 @@ +{ + "file_format_version" : "1.0.0", + "ICD": { + "library_path": "libGLX_nvidia.so.0", + "api_version" : "1.1.99" + } +} diff --git a/alf/environments/igibson/docker/run_igibson_docker.sh b/alf/environments/igibson/docker/run_igibson_docker.sh new file mode 100755 index 000000000..d4b544fdc --- /dev/null +++ b/alf/environments/igibson/docker/run_igibson_docker.sh @@ -0,0 +1,10 @@ +#!/usr/bin/env bash + +while getopts t: flag +do + case "${flag}" in + t) tag=${OPTARG};; + esac +done +echo "Running image: $tag"; +sudo docker run --gpus all --runtime=nvidia -v $HOME/.bashrc:/home/.bashrc -v $HOME/alf:/home/alf -v $HOME/iGibson/results:/home/results -it $tag /bin/bash diff --git a/alf/environments/igibson/docker/set_env.sh b/alf/environments/igibson/docker/set_env.sh new file mode 100755 index 000000000..597c726d2 --- /dev/null +++ b/alf/environments/igibson/docker/set_env.sh @@ -0,0 +1,26 @@ +#!/bin/bash + +export LC_ALL=C.UTF-8 +export LANG=C.UTF-8 + +if [[ "$NVIDIA_VISIBLE_DEVICES" =~ "GPU" ]]; then + unset IDS + GPUS=(${NVIDIA_VISIBLE_DEVICES//,/ }) + for gpu in ${GPUS[@]} + do + ID=$(nvidia-smi -L|grep $gpu| awk -F ":" '{print $1}' |awk '{print $2}') + DISPLAY=":0.$ID" + if [[ $IDS ]]; then + IDS="$IDS,$ID" + else + IDS=$ID + fi + done + export CUDA_DEVICE_ORDER=PCI_BUS_ID + export CUDA_VISIBLE_DEVICES=$IDS + export DISPLAY=$DISPLAY + echo "export CUDA_DEVICE_ORDER=PCI_BUS_ID" >> ~/.bashrc + echo "export CUDA_VISIBLE_DEVICES=$IDS" >> ~/.bashrc + echo "export DISPLAY=$DISPLAY" >> ~/.bashrc + echo "alias nvidia-smi='nvidia-smi -i $NVIDIA_VISIBLE_DEVICES'" >> ~/.bashrc +fi diff --git a/alf/environments/igibson/docker/sources.list b/alf/environments/igibson/docker/sources.list new file mode 100644 index 000000000..b2d8e354e --- /dev/null +++ b/alf/environments/igibson/docker/sources.list @@ -0,0 +1,10 @@ +deb http://mirrors.aliyun.com/ubuntu/ bionic main restricted universe multiverse +deb http://mirrors.aliyun.com/ubuntu/ bionic-security main restricted universe multiverse +deb http://mirrors.aliyun.com/ubuntu/ bionic-updates main restricted universe multiverse +deb http://mirrors.aliyun.com/ubuntu/ bionic-proposed main restricted universe multiverse +deb http://mirrors.aliyun.com/ubuntu/ bionic-backports main restricted universe multiverse +deb-src http://mirrors.aliyun.com/ubuntu/ bionic main restricted universe multiverse +deb-src http://mirrors.aliyun.com/ubuntu/ bionic-security main restricted universe multiverse +deb-src http://mirrors.aliyun.com/ubuntu/ bionic-updates main restricted universe multiverse +deb-src http://mirrors.aliyun.com/ubuntu/ bionic-proposed main restricted universe multiverse +deb-src http://mirrors.aliyun.com/ubuntu/ bionic-backports main restricted universe multiverse diff --git a/alf/environments/igibson/igibson_env.py b/alf/environments/igibson/igibson_env.py new file mode 100644 index 000000000..33244b2a3 --- /dev/null +++ b/alf/environments/igibson/igibson_env.py @@ -0,0 +1,104 @@ +# Copyright (c) 2021 Horizon Robotics and ALF Contributors. All Rights Reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from igibson.envs.igibson_env import iGibsonEnv +from igibson.tasks.room_rearrangement_task import RoomRearrangementTask +from igibson.tasks.point_nav_fixed_task import PointNavFixedTask +from igibson.tasks.point_nav_random_task import PointNavRandomTask +from igibson.tasks.interactive_nav_random_task import InteractiveNavRandomTask +from igibson.tasks.dynamic_nav_random_task import DynamicNavRandomTask +from igibson.tasks.reaching_random_task import ReachingRandomTask +from alf.environments.igibson.igibson_tasks import VisualObjectNavTask + + +class iGibsonCustomEnv(iGibsonEnv): + """iGibson Environment (OpenAI Gym interface)""" + + def __init__( + self, + config_file, + scene_id=None, + mode='headless', + task=None, + action_timestep=1 / 10.0, + physics_timestep=1 / 240.0, + device_idx=0, + render_to_tensor=False, + automatic_reset=False, + ): + """ + Args: + config_file (str): config_file path + scene_id (str): override scene_id in config file + mode (str): headless, gui, iggui + task: (str) task type + action_timestep (float): environment executes action per action_timestep second + physics_timestep (float): physics timestep for pybullet + device_idx (int): which GPU to run the simulation and rendering on + render_to_tensor (bool): whether to render directly to pytorch tensors + automatic_reset (bool): whether to automatic reset after an episode finishes + """ + self.task = task + super(iGibsonCustomEnv, self).__init__( + config_file=config_file, + scene_id=scene_id, + mode=mode, + action_timestep=action_timestep, + physics_timestep=physics_timestep, + device_idx=device_idx, + render_to_tensor=render_to_tensor) + + def load_task_setup(self): + """Load task setup""" + + self.initial_pos_z_offset = self.config.get('initial_pos_z_offset', + 0.1) + drop_distance = 0.5 * 9.8 * (self.action_timestep**2) + assert drop_distance < self.initial_pos_z_offset, \ + 'initial_pos_z_offset is too small for collision checking' + + # ignore the agent's collision with these body ids + self.collision_ignore_body_b_ids = set( + self.config.get('collision_ignore_body_b_ids', [])) + # ignore the agent's collision with these link ids of itself + self.collision_ignore_link_a_ids = set( + self.config.get('collision_ignore_link_a_ids', [])) + + # discount factor + self.discount_factor = self.config.get('discount_factor', 0.99) + + # domain randomization frequency + self.texture_randomization_freq = self.config.get( + 'texture_randomization_freq', None) + self.object_randomization_freq = self.config.get( + 'object_randomization_freq', None) + + # task + if self.task is None: + if self.config['task'] == 'point_nav_fixed': + self.task = PointNavFixedTask(self) + elif self.config['task'] == 'point_nav_random': + self.task = PointNavRandomTask(self) + elif self.config['task'] == 'interactive_nav_random': + self.task = InteractiveNavRandomTask(self) + elif self.config['task'] == 'dynamic_nav_random': + self.task = DynamicNavRandomTask(self) + elif self.config['task'] == 'reaching_random': + self.task = ReachingRandomTask(self) + elif self.config['task'] == 'room_rearrangement': + self.task = RoomRearrangementTask(self) + elif self.config['task'] == 'visual_object_nav': + self.task = VisualObjectNavTask(self) + else: + self.task = None diff --git a/alf/environments/igibson/igibson_object.py b/alf/environments/igibson/igibson_object.py new file mode 100644 index 000000000..a347cb70f --- /dev/null +++ b/alf/environments/igibson/igibson_object.py @@ -0,0 +1,48 @@ +# Copyright (c) 2021 Horizon Robotics and ALF Contributors. All Rights Reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import os +import igibson +from igibson.objects.articulated_object import ArticulatedObject + + +class iGibsonObject(ArticulatedObject): + """iGibson dataset object from igibson/data/ig_dataset/objects""" + + def __init__(self, name, scale=1.): + """ + Args: + name (str): name of the object, corresponding to the directory + igibson/data/ig_dataset/objects/'name' + scale (float): scale of object + """ + dirname = os.path.join(igibson.ig_dataset_path, 'objects', name) + object_dir = [f.path for f in os.scandir(dirname) if f.is_dir()][0] + object_name = os.path.basename(object_dir) + filename = os.path.join(object_dir, f'{object_name}.urdf') + super(iGibsonObject, self).__init__(filename, scale) + + def load(self): + """Load the object into pybullet. + + _load() will be implemented in the subclasses + Returns: + int: PyBullet object unique id + """ + if self.loaded: + return self.body_id + self.body_id = self._load() + self.loaded = True + + return self.body_id diff --git a/alf/environments/igibson/igibson_tasks.py b/alf/environments/igibson/igibson_tasks.py new file mode 100644 index 000000000..c1ed5eb77 --- /dev/null +++ b/alf/environments/igibson/igibson_tasks.py @@ -0,0 +1,816 @@ +# Copyright (c) 2021 Horizon Robotics and ALF Contributors. All Rights Reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import igibson +from igibson.tasks.task_base import BaseTask +from igibson.scenes.igibson_indoor_scene import InteractiveIndoorScene +from igibson.scenes.gibson_indoor_scene import StaticIndoorScene +from igibson.termination_conditions.max_collision import MaxCollision +from igibson.termination_conditions.timeout import Timeout +from igibson.termination_conditions.out_of_bound import OutOfBound +from igibson.termination_conditions.point_goal import PointGoal +from igibson.reward_functions.potential_reward import PotentialReward +from igibson.reward_functions.collision_reward import CollisionReward +from igibson.reward_functions.point_goal_reward import PointGoalReward +from igibson.utils.utils import l2_distance, rotate_vector_3d, cartesian_to_polar +from igibson.objects.visual_marker import VisualMarker + +import numpy as np +import os +import time +import logging +import pybullet as p + +from alf.environments.igibson.igibson_object import iGibsonObject + + +class VisualPointNavFixedTask(BaseTask): + """Fixed Point Navigation Task. + + The goal is to navigate to a fixed highlighted goal position in the stadium scene. + """ + + def __init__(self, env, target_pos=[5, 5, 0], target_pos_vis_obj=None): + """ + Args: + env (igibson.igibson_env.iGibsonCustomEnv): environment instance + target_pos (list[float]): x, y, z coordinates of the target position + target_pos_vis_obj (igibson.objects.object_base.Object): the visual object + at the target position + """ + super(VisualPointNavFixedTask, self).__init__(env) + self.reward_type = self.config.get('reward_type', 'l2') + self.termination_conditions = [ + MaxCollision(self.config), + Timeout(self.config), + OutOfBound(self.config), + PointGoal(self.config), + ] + self.reward_functions = [ + PotentialReward(self.config), + CollisionReward(self.config), + PointGoalReward(self.config), + ] + + self.initial_pos = np.array(self.config.get('initial_pos', [0, 0, 0])) + self.initial_orn = np.array(self.config.get('initial_orn', [0, 0, 0])) + self.target_pos = np.array(self.config.get('target_pos', target_pos)) + self.goal_format = self.config.get('goal_format', 'polar') + self.dist_tol = self.termination_conditions[-1].dist_tol + + self.visual_object_at_initial_target_pos = self.config.get( + 'visual_object_at_initial_target_pos', True) + self.target_visual_object_visible_to_agent = self.config.get( + 'target_visual_object_visible_to_agent', False) + self.target_pos_vis_obj = target_pos_vis_obj + if self.target_visual_object_visible_to_agent: + env.simulator.import_object(self.target_pos_vis_obj) + else: + self.target_pos_vis_obj.load() + + # adjust z position of the object so that it's above ground + self.z_offset = 0 + zmin = p.getAABB(self.target_pos_vis_obj.body_id)[0][2] + if zmin < 0: + self.z_offset = -zmin + x, y, z = self.target_pos + self.target_pos = np.array([x, y, z + self.z_offset]) + self.target_pos_vis_obj.set_position(self.target_pos) + + self.floor_num = 0 + self.load_visualization(env) + + def load_visualization(self, env): + """Load visualization, including the object at the initial position and the shortest path. + + Args: + env (igibson.igibson_env.iGibsonCustomEnv): environment instance + """ + if env.mode != 'gui': + return + + # turn off multiview GUIs + p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0) + + cyl_length = 0.2 + self.initial_pos_vis_obj = VisualMarker( + visual_shape=p.GEOM_CYLINDER, + rgba_color=[1, 0, 0, 0.3], + radius=self.dist_tol, + length=cyl_length, + initial_offset=[0, 0, cyl_length / 2.0]) + self.initial_pos_vis_obj.load() + + if env.scene.build_graph: + self.num_waypoints_vis = 250 + self.waypoints_vis = [ + VisualMarker( + visual_shape=p.GEOM_CYLINDER, + rgba_color=[0, 1, 0, 0.3], + radius=0.1, + length=cyl_length, + initial_offset=[0, 0, cyl_length / 2.0]) + for _ in range(self.num_waypoints_vis) + ] + for waypoint in self.waypoints_vis: + waypoint.load() + + def get_geodesic_potential(self, env): + """Get potential based on geodesic distance. + + Args: + env (igibson.igibson_env.iGibsonCustomEnv): environment instance + + Returns: + float: geodesic distance to the target position. + """ + _, geodesic_dist = self.get_shortest_path(env) + return geodesic_dist + + def get_l2_potential(self, env): + """Get potential based on L2 distance. + + Args: + env (igibson.igibson_env.iGibsonCustomEnv): environment instance + + Returns: + float: L2 distance to the target position + """ + return l2_distance(env.robots[0].get_position()[:2], + self.target_pos[:2]) + + def get_potential(self, env): + """Compute task-specific potential: distance to the goal. + + Args: + env (igibson.igibson_env.iGibsonCustomEnv): environment instance + + Returns: + float: task potential + """ + if self.reward_type == 'l2': + return self.get_l2_potential(env) + elif self.reward_type == 'geodesic': + return self.get_geodesic_potential(env) + + def reset_scene(self, env): + """Task-specific scene reset: reset scene objects or floor plane. + + Args: + env (igibson.igibson_env.iGibsonCustomEnv): environment instance + """ + if isinstance(env.scene, InteractiveIndoorScene): + env.scene.reset_scene_objects() + elif isinstance(env.scene, StaticIndoorScene): + env.scene.reset_floor(floor=self.floor_num) + + def reset_agent(self, env): + """Task-specific agent reset: land the robot to initial pose, compute initial potential. + + Args: + env (igibson.igibson_env.iGibsonCustomEnv): environment instance + """ + env.land(env.robots[0], self.initial_pos, self.initial_orn) + self.path_length = 0.0 + self.robot_pos = self.initial_pos[:2] + self.target_pos_vis_obj.set_position(self.target_pos) + self.geodesic_dist = self.get_geodesic_potential(env) + for reward_function in self.reward_functions: + reward_function.reset(self, env) + + def get_termination(self, env, collision_links=[], action=None, info={}): + """Aggreate termination conditions and fill info. + + Args: + env (igibson.igibson_env.iGibsonCustomEnv): environment instance + collision_links (list[int]): collision links after executing action + action (tuple(float)): the executed action + info (dict): additional info + """ + done, info = super(VisualPointNavFixedTask, self).get_termination( + env, collision_links, action, info) + + info['path_length'] = self.path_length + if done: + info['spl'] = float(info['success']) * \ + min(1.0, self.geodesic_dist / self.path_length) + else: + info['spl'] = 0.0 + + return done, info + + def global_to_local(self, env, pos): + """Convert a 3D point in global frame to agent's local frame. + + Args: + env (igibson.igibson_env.iGibsonCustomEnv): environment instance + pos (numpy.ndarray): a 3D point in global frame + Returns: + numpy.ndarray: the same 3D point in agent's local frame + """ + return rotate_vector_3d(pos - env.robots[0].get_position(), + *env.robots[0].get_rpy()) + + def get_task_obs(self, env): + """Get task-specific observation, including goal position, current velocities, etc. + + Args: + env (igibson.igibson_env.iGibsonCustomEnv): environment instance + Returns: + numpy.ndarray: task-specific observation + """ + task_obs = self.global_to_local(env, self.target_pos)[:2] + if self.goal_format == 'polar': + task_obs = np.array(cartesian_to_polar(task_obs[0], task_obs[1])) + + # linear velocity along the x-axis + linear_velocity = rotate_vector_3d(env.robots[0].get_linear_velocity(), + *env.robots[0].get_rpy())[0] + # angular velocity along the z-axis + angular_velocity = rotate_vector_3d( + env.robots[0].get_angular_velocity(), *env.robots[0].get_rpy())[2] + task_obs = np.append(task_obs, [linear_velocity, angular_velocity]) + + return task_obs + + def get_shortest_path(self, env, from_initial_pos=False, + entire_path=False): + """Get the shortest path and geodesic distance. + + Get the shortest path and geodesic distance from the robot or + the initial position to the target position. + + Args: + env (igibson.igibson_env.iGibsonCustomEnv): environment instance + from_initial_pos (bool): whether source is initial position rather than current position + entire_path (bool): whether to return the entire shortest path + Returns: + shortest path and geodesic distance to the target position: + - shortest path (numpy.ndarray): shortest path as a sequence of waypoints + - geodesic distance (float): geodesic distance + """ + if from_initial_pos: + source = self.initial_pos[:2] + else: + source = env.robots[0].get_position()[:2] + target = self.target_pos[:2] + return env.scene.get_shortest_path( + self.floor_num, source, target, entire_path=entire_path) + + def step_visualization(self, env): + """Step visualization. + + Args: + env (igibson.igibson_env.iGibsonCustomEnv): environment instance + """ + if env.mode != 'gui': + return + + self.initial_pos_vis_obj.set_position(self.initial_pos) + self.target_pos_vis_obj.set_position(self.target_pos) + + if env.scene.build_graph: + shortest_path, _ = self.get_shortest_path(env, entire_path=True) + floor_height = env.scene.get_floor_height(self.floor_num) + num_nodes = min(self.num_waypoints_vis, shortest_path.shape[0]) + for i in range(num_nodes): + self.waypoints_vis[i].set_position( + pos=np.array([ + shortest_path[i][0], shortest_path[i][1], floor_height + ])) + for i in range(num_nodes, self.num_waypoints_vis): + self.waypoints_vis[i].set_position( + pos=np.array([0.0, 0.0, 100.0])) + + def step(self, env): + """Perform task-specific step: step visualization and aggregate path length. + + Args: + env (igibson.igibson_env.iGibsonCustomEnv): environment instance + """ + self.step_visualization(env) + new_robot_pos = env.robots[0].get_position()[:2] + self.path_length += l2_distance(self.robot_pos, new_robot_pos) + self.robot_pos = new_robot_pos + + +class VisualPointNavRandomTask(VisualPointNavFixedTask): + """Random Point Navigation Task. + + The goal is to navigate to a random goal position from a random initial position. + """ + + def __init__(self, env, target_pos_vis_obj=None): + """ + Args: + env (igibson.igibson_env.iGibsonCustomEnv): environment instance + target_pos_vis_obj (igibson.objects.object_base.Object): an instance of igibson.objects + """ + super(VisualPointNavRandomTask, self).__init__( + env=env, target_pos_vis_obj=target_pos_vis_obj) + self.target_dist_min = self.config.get('target_dist_min', 1.0) + self.target_dist_max = self.config.get('target_dist_max', 10.0) + + def sample_initial_pose_and_target_pos(self, env): + """Sample robot initial pose and target position. + + Args: + env (igibson.igibson_env.iGibsonCustomEnv): environment instance + Returns: + initial and target pose information: + - initial_pos (numpy.ndarray): initial position + - initial_orn (numpy.ndarray): initial orientation + - target_pos (numpy.ndarray): target position + """ + _, initial_pos = env.scene.get_random_point(floor=self.floor_num) + max_trials = 100 + dist = 0.0 + for _ in range(max_trials): + _, target_pos = env.scene.get_random_point(floor=self.floor_num) + if env.scene.build_graph: + _, dist = env.scene.get_shortest_path( + self.floor_num, + initial_pos[:2], + target_pos[:2], + entire_path=False) + else: + dist = l2_distance(initial_pos, target_pos) + if self.target_dist_min < dist < self.target_dist_max: + break + if not (self.target_dist_min < dist < self.target_dist_max): + print("WARNING: Failed to sample initial and target positions") + initial_orn = np.array([0, 0, np.random.uniform(0, np.pi * 2)]) + return initial_pos, initial_orn, target_pos + + def reset_scene(self, env): + """Task-specific scene reset: get a random floor number first. + + Args: + env (igibson.igibson_env.iGibsonCustomEnv): environment instance + """ + self.floor_num = env.scene.get_random_floor() + super(VisualPointNavRandomTask, self).reset_scene(env) + + def reset_agent(self, env): + """Reset robot initial pose. + + Sample initial pose and target position, check validity, and land it. + + Args: + env (igibson.igibson_env.iGibsonCustomEnv): environment instance + """ + reset_success = False + max_trials = 100 + + # cache pybullet state + state_id = p.saveState() + for i in range(max_trials): + initial_pos, initial_orn, target_pos = \ + self.sample_initial_pose_and_target_pos(env) + reset_success = env.test_valid_position( + env.robots[0], initial_pos, initial_orn) and \ + env.test_valid_position( + env.robots[0], target_pos) + p.restoreState(state_id) + if reset_success: + break + + if not reset_success: + logging.warning("WARNING: Failed to reset robot without collision") + + p.removeState(state_id) + + x, y, z = target_pos + self.target_pos = np.array([x, y, z + self.z_offset]) + self.initial_pos = initial_pos + self.initial_orn = initial_orn + + super(VisualPointNavRandomTask, self).reset_agent(env) + + +class VisualObjectNavTask(BaseTask): + """Object Navigation Task. + + The goal is to navigate to one of the many loaded objects given object name + from a random initial position. + """ + + def __init__(self, env): + """ + Args: + env (igibson.igibson_env.iGibsonCustomEnv): environment instance + """ + super(VisualObjectNavTask, self).__init__(env) + self.env = env + + # minimum distance between object and initial robot position + self.object_dist_min = self.config.get('object_dist_min', 1.0) + # maximum distance between object and initial robot position + self.object_dist_max = self.config.get('object_dist_max', 10.0) + + self.reward_type = self.config.get('reward_type', 'l2') + self.termination_conditions = [ + MaxCollision(self.config), + Timeout(self.config), + OutOfBound(self.config), + PointGoal(self.config), + ] + self.reward_functions = [ + PotentialReward(self.config), + CollisionReward(self.config), + PointGoalReward(self.config), + ] + + self.initial_pos = np.array(self.config.get('initial_pos', [0, 0, 0])) + self.initial_orn = np.array(self.config.get('initial_orn', [0, 0, 0])) + self.goal_format = self.config.get('goal_format', 'polar') + # additional tolerance for goal checking + self.goal_buffer_dist = self.config.get('goal_buffer_dist', 0.5) + # minimum distance between objects when sampling object position + self.object_keepout_buffer_dist = self.config.get( + 'object_keepout_buffer_dist', 0.5) + self.visual_object_at_initial_pos = self.config.get( + 'visual_object_at_initial_pos', True) + self.floor_num = 0 + self.num_objects = self.config.get('num_objects', 5) + self.object_randomization_freq = self.config.get( + 'object_randomization_freq', None) + self.initialize_scene_objects() + self.load_visualization(env) + self.reset_time_vars() + + def reset_time_vars(self): + """Reset/Initialize time related variables.""" + self.start_time = time.time() + self.reset_time = time.time() + self.episode_time = time.time() + + def initialize_scene_objects(self): + """Initialize objects in the scene.""" + + # get object names + all_object_names = [ + os.path.basename(f.path) for f in os.scandir( + os.path.join(igibson.ig_dataset_path, 'objects')) + if f.is_dir() + ] + assert self.num_objects <= len(all_object_names) + if self.object_randomization_freq is not None: + self.object_names = np.random.choice(all_object_names, + self.num_objects) + else: + if self.num_objects == 5: + self.object_names = [ + 'standing_tv', 'piano', 'office_chair', 'toilet', + 'speaker_system' + ] + else: + self.object_names = all_object_names[-self.num_objects:] + + # load objects into scene and save their info + max_radius = 0.0 + self.object_dict = {} + self.object_pos_dict = {} + self.object_orn_dict = {} + self.object_id_dict = {} + for object_name in self.object_names: + self.object_dict[object_name] = iGibsonObject(name=object_name) + pybullet_id = self.env.simulator.import_object( + self.object_dict[object_name]) + self.object_pos_dict[object_name] = self.object_dict[ + object_name].get_position() + self.object_orn_dict[object_name] = self.object_dict[ + object_name].get_orientation() + self.object_id_dict[object_name] = pybullet_id + # get object max radius from bounding box + (xmax, ymax, _), (xmin, ymin, _) = p.getAABB(pybullet_id) + object_max_radius = max( + abs(xmax - xmin) / 2., + abs(ymax - ymin) / 2.) + max_radius = max(object_max_radius, max_radius) + self.max_radius = max_radius + + # update distance tolerance + self.dist_tol = self.max_radius + self.goal_buffer_dist + self.termination_conditions[-1].dist_tol = self.dist_tol + self.reward_functions[-1].dist_tol = self.dist_tol + self.object_dist_keepout = self.max_radius * 2 + self.object_keepout_buffer_dist + + self.sample_goal_object() + + def sample_goal_object(self): + """Sample goal object and save goal object info.""" + goal_object_idx = np.random.randint(self.num_objects) + self.target_name = self.object_names[goal_object_idx] + self.target_object = self.object_dict[self.target_name] + self.target_pos = self.object_pos_dict[self.target_name] + self.target_orn = self.object_orn_dict[self.target_name] + # one-hot encoding + self.target_obs = np.eye(self.num_objects)[goal_object_idx] + + def sample_initial_pose_and_object_pos(self, env): + """Sample robot initial pose and object positions. + + Args: + env (igibson.igibson_env.iGibsonCustomEnv): environment instance + + Returns: + robot initial pose and object positions + - initial_pos (numpy.ndarray): robot initial position + - initial_orn (numpy.ndarray): robot initial orientation + - object_pos_dict (dict): dictionary of object positions + - object_orn_dict (dict): dictionary of object orientations + """ + object_pos_dict = {} + object_orn_dict = {} + + def placement_is_valid(pos, initial_pos): + """Test if an object position is valid.""" + dist = l2_distance(pos, initial_pos) + if dist < self.object_dist_min or dist > self.object_dist_max: + return False + for object_name, object_pos in object_pos_dict.items(): + dist = l2_distance(pos, object_pos) + if dist < self.object_dist_keepout: + return False + return True + + _, initial_pos = env.scene.get_random_point(floor=self.floor_num) + initial_orn = np.array([0, 0, np.random.uniform(0, np.pi * 2)]) + + max_trials = 500 + for object_name in self.object_names: + object_orn_dict[object_name] = np.array( + [0, 0, np.random.uniform(0, np.pi * 2)]) + for _ in range(max_trials): + _, object_pos = env.scene.get_random_point( + floor=self.floor_num) + valid_pos = placement_is_valid(object_pos, initial_pos) + if valid_pos: + object_pos_dict[object_name] = object_pos + break + if not valid_pos: + print( + f"WARNING: Failed to sample valid position for {object_name}" + ) + + return initial_pos, initial_orn, object_pos_dict, object_orn_dict + + def load_visualization(self, env): + """Load visualization, such as initial and target position, shortest path, etc. + + Args: + env (igibson.igibson_env.iGibsonCustomEnv): environment instance + """ + if env.mode != 'gui': + return + + # Turn off multi-view GUI + p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0) + + # load visual object at initial position + cyl_length = 0.2 + self.initial_pos_vis_obj = VisualMarker( + visual_shape=p.GEOM_CYLINDER, + rgba_color=[1, 0, 0, 0.3], + radius=self.dist_tol, + length=cyl_length, + initial_offset=[0, 0, cyl_length / 2.0]) + self.initial_pos_vis_obj.load() + + # load text at target position + x, y, z = self.target_pos + p.addUserDebugText( + text=f'Target: {self.target_name}', + textPosition=[x, y, z + 2], + textSize=2) + + if env.scene.build_graph: + self.num_waypoints_vis = 250 + self.waypoints_vis = [ + VisualMarker( + visual_shape=p.GEOM_CYLINDER, + rgba_color=[0, 1, 0, 0.3], + radius=0.1, + length=cyl_length, + initial_offset=[0, 0, cyl_length / 2.0]) + for _ in range(self.num_waypoints_vis) + ] + for waypoint in self.waypoints_vis: + waypoint.load() + + def get_geodesic_potential(self, env): + """Get potential based on geodesic distance. + + Args: + env (igibson.igibson_env.iGibsonCustomEnv): environment instance + + Returns: + float: geodesic distance to the target position + """ + _, geodesic_dist = self.get_shortest_path(env) + return geodesic_dist + + def get_l2_potential(self, env): + """Get potential based on L2 distance. + + Args: + env (igibson.igibson_env.iGibsonCustomEnv): environment instance + Returns: + float: L2 distance to the target position + """ + return l2_distance(env.robots[0].get_position()[:2], + self.target_pos[:2]) + + def get_potential(self, env): + """Compute task-specific potential: distance to the goal. + + Args: + env (igibson.igibson_env.iGibsonCustomEnv): environment instance + + Returns: + float: task potential + """ + if self.reward_type == 'l2': + return self.get_l2_potential(env) + else: + raise ValueError(f'Invalid reward type: {self.reward_type}') + + def reset_scene(self, env): + """Task-specific scene reset: get a random floor number first. + + Args: + env (igibson.igibson_env.iGibsonCustomEnv): environment instance + """ + self.floor_num = env.scene.get_random_floor() + if isinstance(env.scene, InteractiveIndoorScene): + env.scene.reset_scene_objects() + elif isinstance(env.scene, StaticIndoorScene): + env.scene.reset_floor(floor=self.floor_num) + + def reset_agent(self, env): + """Reset robot initial pose. + + Sample initial pose and target position, check validity, and land it. + + Args: + env (igibson.igibson_env.iGibsonCustomEnv): environment instance + """ + self.episode_time = time.time() + logging.info( + f'Episode time: {self.episode_time - self.start_time:.5f} | ' + f'Reset time: {self.reset_time - self.start_time:.5f}') + self.reset_time_vars() + + reset_success = False + max_trials = 100 + + # cache pybullet state + state_id = p.saveState() + for i in range(max_trials): + initial_pos, initial_orn, object_pos_dict, object_orn_dict = self.sample_initial_pose_and_object_pos( + env) + reset_success = env.test_valid_position(env.robots[0], initial_pos, + initial_orn) + for object_name, object in self.object_dict.items(): + reset_success &= env.test_valid_position( + object, object_pos_dict[object_name], + object_orn_dict[object_name]) + p.restoreState(state_id) + if reset_success: + break + + if not reset_success: + logging.warning("WARNING: Failed to reset robot without collision") + + p.removeState(state_id) + + self.object_pos_dict = object_pos_dict + self.object_orn_dict = object_orn_dict + self.initial_pos = initial_pos + self.initial_orn = initial_orn + + # land robot and objects + env.land(env.robots[0], self.initial_pos, self.initial_orn) + self.path_length = 0.0 + self.robot_pos = self.initial_pos[:2] + for object_name, object in self.object_dict.items(): + env.land(object, self.object_pos_dict[object_name], + self.object_orn_dict[object_name]) + + self.geodesic_dist = self.get_geodesic_potential(env) + self.sample_goal_object() + for reward_function in self.reward_functions: + reward_function.reset(self, env) + + if env.mode == 'gui': + p.removeAllUserDebugItems() + x, y, z = self.target_pos + p.addUserDebugText( + text=f'Target: {self.target_name}', + textPosition=[x, y, z + 2], + textSize=2) + + self.reset_time = time.time() + + def get_termination(self, env, collision_links=[], action=None, info={}): + """Aggreate termination conditions and fill info. + + Args: + env (igibson.igibson_env.iGibsonCustomEnv): environment instance + collision_links (list[int]): collision links after executing action + action (tuple(float)): the executed action + info (dict): additional info + Returns: + episode termination and info + - done (bool): whether the episode has terminated + - info (dict): additional info + """ + done, info = super(VisualObjectNavTask, self).get_termination( + env, collision_links, action, info) + + info['path_length'] = self.path_length + if done: + info['spl'] = float(info['success']) * \ + min(1.0, self.geodesic_dist / self.path_length) + else: + info['spl'] = 0.0 + + return done, info + + def get_task_obs(self, env): + """Get task-specific observation, including goal position, current velocities, etc. + + Args: + env (igibson.igibson_env.iGibsonCustomEnv): environment instance + Returns: + numpy.ndarray: task-specific observation + """ + return self.target_obs + + def get_shortest_path(self, env, from_initial_pos=False, + entire_path=False): + """Get the shortest path and geodesic distance. + + Args: + env (igibson.igibson_env.iGibsonCustomEnv): environment instance + from_initial_pos (bool): whether source is initial position rather than current position + entire_path: whether to return the entire shortest path + Returns: + shortest path information: + - path_world (numpy.ndarray): shortest path + - geodesic_distance (float): geodesic distance to the target position + """ + if from_initial_pos: + source = self.initial_pos[:2] + else: + source = env.robots[0].get_position()[:2] + target = self.target_pos[:2] + return env.scene.get_shortest_path( + self.floor_num, source, target, entire_path=entire_path) + + def step_visualization(self, env): + """Step visualization. + + Args: + env (igibson.igibson_env.iGibsonCustomEnv): environment instance + """ + if env.mode != 'gui': + return + + self.initial_pos_vis_obj.set_position(self.initial_pos) + + if env.scene.build_graph: + shortest_path, _ = self.get_shortest_path(env, entire_path=True) + floor_height = env.scene.get_floor_height(self.floor_num) + num_nodes = min(self.num_waypoints_vis, shortest_path.shape[0]) + for i in range(num_nodes): + self.waypoints_vis[i].set_position( + pos=np.array([ + shortest_path[i][0], shortest_path[i][1], floor_height + ])) + for i in range(num_nodes, self.num_waypoints_vis): + self.waypoints_vis[i].set_position( + pos=np.array([0.0, 0.0, 100.0])) + + def step(self, env): + """Perform task-specific step: step visualization and aggregate path length. + + Args: + env (igibson.igibson_env.iGibsonCustomEnv): environment instance + """ + self.step_visualization(env) + new_robot_pos = env.robots[0].get_position()[:2] + self.path_length += l2_distance(self.robot_pos, new_robot_pos) + self.robot_pos = new_robot_pos diff --git a/alf/environments/igibson/manual_control.py b/alf/environments/igibson/manual_control.py new file mode 100644 index 000000000..958f55024 --- /dev/null +++ b/alf/environments/igibson/manual_control.py @@ -0,0 +1,132 @@ +# Copyright (c) 2021 Horizon Robotics and ALF Contributors. All Rights Reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import os +import cv2 +import numpy as np +from time import time +import igibson +import pybullet as p + +from alf.environments.igibson.igibson_env import iGibsonCustomEnv + +# env params +yaml_filename = 'turtlebot_clip.yaml' +mode = 'gui' +action_timestep = 1.0 / 10.0 +physics_timestep = 1.0 / 120.0 +device_idx = 1 +fov = 75 + +# data saving +trial_name = '0630_1' +if not os.path.isdir('manual_control_images'): + os.mkdir('manual_control_images') +data_dir = os.path.join('manual_control_images', trial_name) +if not os.path.isdir(data_dir): + os.mkdir(data_dir) + +# keys to action +robot_keys_to_actions = { + 'w': [0.5, 0.5], + 's': [-0.5, -0.5], + 'a': [0.2, -0.2], + 'd': [-0.2, 0.2], + 'f': [0, 0] +} + + +def main(): + config_filename = os.path.join(igibson.vlnav_config_path, yaml_filename) + env = iGibsonCustomEnv( + config_file=config_filename, + mode=mode, + action_timestep=action_timestep, + physics_timestep=physics_timestep, + device_idx=device_idx) + env.reset() + robot = env.config.get('robot') + if robot == 'Turtlebot' or robot == 'Locobot': + keys_to_actions = robot_keys_to_actions + else: + raise ValueError(f'Unknown robot: {robot}') + + categories = list(env.scene.objects_by_category.keys()) + categories_fname = os.path.join( + data_dir, f'{env.config.get("scene_id")}_categories.txt') + with open(categories_fname, 'w') as output: + for name in categories: + output.write(name + '\n') + + def get_key_pressed(): + pressed_keys = [] + events = p.getKeyboardEvents() + key_codes = events.keys() + for key in key_codes: + pressed_keys.append(key) + return pressed_keys + + def get_robot_cam_frame(): + frames = env.simulator.renderer.render_robot_cameras(modes=('rgb')) + if len(frames) > 0: + frame = cv2.cvtColor( + np.concatenate(frames, axis=1), cv2.COLOR_RGB2BGR) + return frame + return None + + running = True + while running: + # detect pressed keys + pressed_keys = get_key_pressed() + if ord('r') in pressed_keys: + print('Reset the environment...') + env.reset() + pressed_keys = [] + if ord('p') in pressed_keys: + print('Shutting down the environment...') + env.close() + running = False + pressed_keys = [] + if ord('c') in pressed_keys: + print('Saving an image from RobotView...') + img_path = os.path.join(data_dir, f'img_{int(time())}.png') + frame = get_robot_cam_frame() + assert frame is not None + frame = (frame[:, :, :3] * 255).astype(np.uint8) + cv2.imwrite(img_path, frame) + pressed_keys = [] + if ord('8') in pressed_keys: + print('Forward...') + env.step(keys_to_actions['w']) + # pressed_keys = [] + if ord('5') in pressed_keys: + print('Backward...') + env.step(keys_to_actions['s']) + # pressed_keys = [] + if ord('6') in pressed_keys: + print('Left...') + env.step(keys_to_actions['a']) + # pressed_keys = [] + if ord('4') in pressed_keys: + print('Right...') + env.step(keys_to_actions['d']) + # pressed_keys = [] + if ord('2') in pressed_keys: + print('Staying still...') + env.step(keys_to_actions['f']) + pressed_keys = [] + + +if __name__ == "__main__": + main() diff --git a/alf/environments/igibson/manual_control_with_clip.py b/alf/environments/igibson/manual_control_with_clip.py new file mode 100644 index 000000000..d46619749 --- /dev/null +++ b/alf/environments/igibson/manual_control_with_clip.py @@ -0,0 +1,246 @@ +# Copyright (c) 2021 Horizon Robotics and ALF Contributors. All Rights Reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import os +import cv2 +import numpy as np +import igibson +import pybullet as p +from alf.environments.igibson.igibson_env import iGibsonCustomEnv + +import clip +import torch +from PIL import Image +from matplotlib import pyplot as plt +import gym +from scipy.stats import entropy +from alf.utils.video_recorder import VideoRecorder + +# clip params +scene_id = "Rs_int" +use_colors = False +target_name = 'grey bed' if use_colors else 'bed' +record_file = f'/home/junyaoshi/Desktop/CLIP_videos/{scene_id}_{target_name.replace(" ", "_")}' \ + f'{"_with_colors_" if use_colors else "_"}0718_3.mp4' +fps = 10 +colors = [ + "blue", "yellow", "green", "white", "brown", "grey", "purple", "red", + "orange", "pink", "black" +] + +# env params +yaml_filename = 'turtlebot_clip.yaml' +mode = 'gui' +action_timestep = 1.0 / 10.0 +physics_timestep = 1.0 / 120.0 +device_idx = 1 +fov = 75 + +# keys to action +robot_keys_to_actions = { + 'w': [0.5, 0.5], + 's': [-0.5, -0.5], + 'a': [0.2, -0.2], + 'd': [-0.2, 0.2], + 'f': [0, 0] +} + + +class ClipRenderWrapper(gym.Wrapper): + """This wrapper makes the env render CLIP statistics real time + + The render() function renders the current image frame on the left, and classes with + best CLIP values, along with their associated values, on the right. + The plot is then converted into a numpy array for video logging. + """ + + def __init__(self, env, classes, target_name): + super().__init__(env) + self.env = env + self.metadata['render.modes'] = ['rgb_array', 'human'] + self.classes = classes + self.target_name = target_name + self.best_values = None + self.best_indices = None + self.frame_array = None + self.target_value = None + + def render(self, mode='rgb_array', fontsize=40, **kwargs): + assert self.best_values is not None and self.best_indices is not None and self.frame_array is not None + assert self.target_value is not None and self.target_name is not None + entropy_val = entropy(self.best_values.cpu().numpy()) / np.log( + len(self.best_values.cpu().numpy())) + fig = plt.figure(figsize=(50, 30)) + + # plot current image frame and entropy + plt.subplot(1, 2, 1) + plt.imshow(self.frame_array) + plt.axis("off") + plt.title( + f'Entropy: {entropy_val:.4f} | {target_name}: {100. * self.target_value.item():.4f}%', + fontsize=fontsize) + + # plot top classes and values determined by CLIP + y = np.arange(self.best_values.shape[-1]) + plt.subplot(1, 2, 2) + plt.grid() + plt.barh(y, self.best_values.cpu().numpy()) + plt.gca().invert_yaxis() + plt.gca().set_axisbelow(True) + plt.yticks( + y, [ + self.classes[index] if len(self.classes[index]) < 25 else + self.classes[index][:20] + '\n' + self.classes[index][20:] + for index in self.best_indices + ], + fontsize=fontsize) + plt.xticks(fontsize=fontsize) + plt.xlim([0., 1.]) + plt.xlabel("probability", fontsize=fontsize) + + plt.tight_layout(rect=[0.01, 0.01, 0.01, 0.01]) + plt.subplots_adjust(wspace=0.5) + + # convert plot into numpy array + fig.canvas.draw() + output = np.frombuffer(fig.canvas.tostring_rgb(), dtype=np.uint8) + output = output.reshape(fig.canvas.get_width_height()[::-1] + (3, )) + return output + + def set_clip_infos(self, best_values, best_indices, frame_array, + target_value): + """Set CLIP related infos""" + self.best_values = best_values + self.best_indices = best_indices + self.frame_array = frame_array + self.target_value = target_value + + +def main(): + # load CLIP model + device = "cuda" if torch.cuda.is_available() else "cpu" + model, preprocess = clip.load("ViT-B/32", device=device, jit=False) + + # load iGibson env + config_filename = os.path.join(igibson.vlnav_config_path, yaml_filename) + env = iGibsonCustomEnv( + config_file=config_filename, + mode=mode, + action_timestep=action_timestep, + physics_timestep=physics_timestep, + device_idx=device_idx) + + # load and process classes + classes = list(env.scene.objects_by_category.keys()) + if use_colors: + classes = [f'{color} {label}' for color in colors for label in classes] + target_index = classes.index(target_name) + text_inputs = torch.cat( + [clip.tokenize(f"a photo of a {c}") for c in classes]).to(device) + env = ClipRenderWrapper(env, classes, target_name=target_name) + + env.reset() + robot = env.config.get('robot') + if robot == 'Turtlebot' or robot == 'Locobot': + keys_to_actions = robot_keys_to_actions + else: + raise ValueError(f'Unknown robot: {robot}') + + recorder = VideoRecorder( + env, append_blank_frames=0, frames_per_sec=fps, path=record_file) + + def get_key_pressed(): + pressed_keys = [] + events = p.getKeyboardEvents() + key_codes = events.keys() + for key in key_codes: + pressed_keys.append(key) + return pressed_keys + + def get_robot_cam_frame(): + frames = env.simulator.renderer.render_robot_cameras(modes=('rgb')) + if len(frames) > 0: + frame = cv2.cvtColor( + np.concatenate(frames, axis=1), cv2.COLOR_RGB2BGR) + return frame + return None + + def process_pressed_keys(pressed_keys, running): + if ord('r') in pressed_keys: + print('Reset the environment...') + env.reset() + pressed_keys = [] + if ord('p') in pressed_keys: + print('Shutting down the environment...') + env.close() + running = False + pressed_keys = [] + if ord('8') in pressed_keys: + print('Forward...') + env.step(keys_to_actions['w']) + if ord('5') in pressed_keys: + print('Backward...') + env.step(keys_to_actions['s']) + if ord('6') in pressed_keys: + print('Left...') + env.step(keys_to_actions['a']) + if ord('4') in pressed_keys: + print('Right...') + env.step(keys_to_actions['d']) + if ord('2') in pressed_keys: + print('Staying still...') + env.step(keys_to_actions['f']) + return running + + running = True + while running: + + # get and process image input + frame = get_robot_cam_frame() + assert frame is not None + frame = (frame[:, :, :3] * 255).astype(np.uint8) + frame = cv2.cvtColor(frame, cv2.COLOR_BGR2RGB) + image = Image.fromarray(frame, 'RGB') + image_input = preprocess(image).unsqueeze(0).to(device) + + # Calculate features + with torch.no_grad(): + image_features = model.encode_image(image_input) + text_features = model.encode_text(text_inputs) + + # Pick the top 5 most similar labels for the image + image_features /= image_features.norm(dim=-1, keepdim=True) + text_features /= text_features.norm(dim=-1, keepdim=True) + similarity = (100.0 * image_features @ text_features.T).softmax(dim=-1) + best_value, best_index = similarity[0].topk(1) + best_values, best_indices = similarity[0].topk(5) + target_value = similarity[:, target_index] + + print(f'{target_name} (Target): {100. * target_value.item():.4f} | ' + f'{classes[best_index]} (Best): {100. * best_value.item():.4f}') + + env.set_clip_infos( + best_values=best_values, + best_indices=best_indices, + frame_array=frame, + target_value=target_value) + recorder.capture_frame() + + # detect and process pressed keys + pressed_keys = get_key_pressed() + running = process_pressed_keys(pressed_keys, running) + + +if __name__ == "__main__": + main() diff --git a/alf/environments/igibson/suite_igibson.py b/alf/environments/igibson/suite_igibson.py new file mode 100644 index 000000000..659acf030 --- /dev/null +++ b/alf/environments/igibson/suite_igibson.py @@ -0,0 +1,384 @@ +# Copyright (c) 2021 Horizon Robotics and ALF Contributors. All Rights Reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import os +import gym +import numpy as np +import pybullet as p + +from alf.environments import suite_gym +from alf.environments.gym_wrappers import BaseObservationWrapper, ImageChannelFirst, FrameGrayScale +import alf + +from igibson.objects.visual_marker import VisualMarker +from alf.environments.igibson.igibson_tasks import VisualPointNavFixedTask, \ + VisualPointNavRandomTask, VisualObjectNavTask +from alf.environments.igibson.igibson_env import iGibsonCustomEnv + + +@alf.configurable() +class FlattenWrapper(BaseObservationWrapper): + """Flatten selected obversation fields of the environment""" + + def __init__(self, env, fields=None): + super().__init__(env, fields=fields) + + def transform_space(self, observation_space): + if isinstance(observation_space, gym.spaces.Box): + if self._need_flatten(observation_space.shape): + low = observation_space.low + high = observation_space.high + if np.isscalar(low) and np.isscalar(high): + shape = observation_space.shape[:-1] + return gym.spaces.Box( + low=low, + high=high, + shape=shape, + dtype=observation_space.dtype) + else: + low = self._flatten(observation_space.low, flatten=True) + high = self._flatten(observation_space.high, flatten=True) + return gym.spaces.Box( + low=low, high=high, dtype=observation_space.dtype) + else: + raise TypeError('Unsupported observation space type') + return observation_space + + def transform_observation(self, observation): + flatten = self._need_flatten(observation.shape) + return self._flatten(observation, flatten) + + def _need_flatten(self, shape): + if len(shape) == 2: + return True + return False + + def _flatten(self, np_array, flatten=False): + """ + Note that the extra copy() after np.transpose is crucial to pickle dump speed + when called by subprocesses. An explanation of the non-contiguous memory caused + by numpy tranpose can be found in the following: + + https://stackoverflow.com/questions/26998223/ + """ + + if flatten: + np_array = np_array.flatten() + return np_array.copy() + + +@alf.configurable() +class StackRGBDWrapper(gym.ObservationWrapper): + """This wrapper stacks rgb and depth observation field together into an array of shape (H, W, 4) + + Note that the observation space must include `rgb` and `depth`, otherwise an error will be raised. + """ + + def __init__(self, env): + super().__init__(env) + observation_space = env.observation_space + self.observation_space = self.transform_space(observation_space) + + def transform_space(self, observation_space): + assert isinstance(observation_space, gym.spaces.dict.Dict) + rgb_space = observation_space.spaces['rgb'] + depth_space = observation_space.spaces['depth'] + try: + assert not np.isscalar(rgb_space.low) and not np.isscalar( + rgb_space.low) + assert not np.isscalar(depth_space.low) and not np.isscalar( + depth_space.low) + except AssertionError as e: + print(e) + + rgbd_low = np.vstack((rgb_space.low, depth_space.low)) + rgbd_high = np.vstack((rgb_space.high, depth_space.high)) + assert rgbd_low.shape == rgbd_high.shape + rgbd_space = gym.spaces.Box( + low=rgbd_low, high=rgbd_high, dtype=np.uint8) + + observation_space.spaces.pop('rgb') + observation_space.spaces.pop('depth') + observation_space.spaces['rgbd'] = rgbd_space + return observation_space + + def observation(self, observation): + return self.transform_observation(observation) + + def transform_observation(self, observation): + rgbd_obs = np.vstack((observation['rgb'], observation['depth'])) + observation.pop('rgb') + observation.pop('depth') + observation['rgbd'] = rgbd_obs + return observation + + +@alf.configurable() +class OneHotIntWrapper(BaseObservationWrapper): + """Transforms the data type of one-hot input from float to int""" + + def __init__(self, env, fields=None): + super().__init__(env, fields=fields) + + def transform_space(self, observation_space): + obs_low = np.full_like( + observation_space.low, fill_value=0, dtype=np.uint8) + obs_high = np.full_like( + observation_space.high, fill_value=255, dtype=np.uint8) + assert obs_low.shape == obs_high.shape + observation_space = gym.spaces.Box( + low=obs_low, high=obs_high, dtype=np.uint8) + return observation_space + + def transform_observation(self, observation): + return (observation * 255).astype(np.uint8) + + +@alf.configurable() +class ImageIntWrapper(BaseObservationWrapper): + """Transforms the data type of image input from float to int""" + + def __init__(self, env, fields=None): + super().__init__(env, fields=fields) + + def transform_space(self, observation_space): + obs_low = np.full_like( + observation_space.low, fill_value=0, dtype=np.uint8) + obs_high = np.full_like( + observation_space.high, fill_value=255, dtype=np.uint8) + assert obs_low.shape == obs_high.shape + observation_space = gym.spaces.Box( + low=obs_low, high=obs_high, dtype=np.uint8) + return observation_space + + def transform_observation(self, observation): + return (observation * 255).astype(np.uint8) + + +@alf.configurable() +class RenderWrapper(gym.Wrapper): + """Implements render() for iGibson env for visualization purposes""" + + def __init__(self, env): + super().__init__(env) + self.env = env + self.metadata['render.modes'] = ['rgb_array', 'human'] + + def render(self, mode='human', **kwargs): + if 'vision' in self.env.sensors: + vision_obs = self.sensors['vision'].get_obs(self) + if 'rgb' in vision_obs: + rgb_obs = vision_obs['rgb'] + rgb_obs = (rgb_obs * 255).astype(np.uint8) + if 'depth' not in vision_obs: + return rgb_obs + else: + depth_obs = vision_obs['depth'] + depth_obs = (depth_obs * 255).astype(np.uint8) + depth_obs_rescaled = np.tile(depth_obs, (1, 1, 3)) + rgbd_obs = np.concatenate((rgb_obs, depth_obs_rescaled), + axis=1) + return rgbd_obs + else: + raise AttributeError("robot doesn't have modality: rgb") + else: + raise AttributeError("robot doesn't have sensor: vision") + + +@alf.configurable() +class GFTTupleInputWrapper(gym.ObservationWrapper): + """Transforms observation space into tuple for GFT""" + + def __init__(self, env): + super().__init__(env) + assert 'rgbd' in self.observation_space.spaces \ + and 'task_obs' in self.observation_space.spaces + self.observation_space = self.transform_space(self.observation_space) + + def transform_space(self, observation_space): + assert isinstance(observation_space, gym.spaces.dict.Dict) + rgbd_space = observation_space.spaces['rgbd'] + task_obs_space = observation_space.spaces['task_obs'] + tuple_space = gym.spaces.Tuple([rgbd_space, task_obs_space]) + + observation_space.spaces.pop('task_obs') + observation_space.spaces['obs'] = tuple_space + return observation_space + + def observation(self, observation): + return self.transform_observation(observation) + + def transform_observation(self, observation): + rgbd_obs = observation['rgbd'] + task_obs = observation['task_obs'] + observation.pop('task_obs') + observation['obs'] = (rgbd_obs, task_obs) + return observation + + +@alf.configurable() +class PyBulletRecorderWrapper(gym.Wrapper): + """Enables pybullet video logging""" + + def __init__(self, env, record_file): + super().__init__(env) + self.p_logging = p.startStateLogging(p.STATE_LOGGING_VIDEO_MP4, + record_file) + + def close(self): + p.stopStateLogging(self.p_logging) + super().close() + + +@alf.configurable +def load(env_name=None, + env_id=None, + discount=1.0, + max_episode_steps=None, + config_file=None, + scene_id=None, + env_mode='headless', + task=None, + target_pos=[5, 5, 0], + fov=45, + action_timestep=1.0 / 10.0, + physics_timestep=1.0 / 120.0, + device_idx=0, + record_pybullet=False, + pybullet_record_file=None, + use_gray_scale=False, + use_gft=False, + gym_env_wrappers=(), + alf_env_wrappers=()): + """Loads the selected environment and wraps it with the specified wrappers. + + Note that by default a ``TimeLimit`` wrapper is used to limit episode lengths + to the default benchmarks defined by the registered environments. + Args: + env_name (str): Name for the environment to load. + env_id (int): A scalar ``Tensor`` of the environment ID of the time step. + discount (float): This argument is not used; discount is defined in the + iGibson config file. + max_episode_steps: If None or 0 the ``max_episode_steps`` will be set to + the default step limit -1 defined in the environment. Otherwise + ``max_episode_steps`` will be set to the smaller value of the two. + config_file (str): Name for the iGibson config file. Only the file name + is needed, not the full path. + scene_id (str): Overrides scene_id in config file. + env_mode (str): Three options for rendering: headless, gui, iggui. + task (str): A string that specifies the iGibson task. + target_pos (list[float]): [x, y, z] used for "visual_point_nav_fixed". + fov (int): Field of view of the robot in degrees. + num_objects: (int) number of objects in the environment, for object_nav + tasks only + action_timestep (float): Environment executes action per action_timestep + second + physics_timestep (float): Physics timestep for pybullet + device_idx (int): Which GPU to run the simulation and rendering on + record_pybullet (bool): Whether to record pybullet rendering video + pybullet_record_file (str): Video record file for storing pybullet recording + use_gray_scale (bool): Whether to use alf.environments.gym_wrappers.FrameGrayScale + to convert RGB to grey scale + use_gft (bool): Whether to use alf.layers.GFT in conf file + render_to_tensor (bool): Whether to render directly to pytorch tensors + automatic_reset (bool): Whether to automatic reset after an episode finishes + gym_env_wrappers (gym.wrappers): Iterable with references to wrapper classes to use + directly on the gym environment. + alf_env_wrappers (alf.environments.gym_wrappers): Iterable with references to wrapper classes to use on + the torch environment. + Returns: + An AlfEnvironment instance. + """ + env = iGibsonCustomEnv( + config_file=config_file, + scene_id=scene_id, + mode=env_mode, + action_timestep=action_timestep, + physics_timestep=physics_timestep, + device_idx=device_idx) + + # object params (for point navigation tasks only) + visual_shape = p.GEOM_CYLINDER + cyl_length = 1.5 + cyl_radius = 0.2 + rgba_color = [0, 0, 1, 1.0] + initial_offset = [0, 0, cyl_length / 2.0] + vis_obj = VisualMarker( + visual_shape=visual_shape, + rgba_color=rgba_color, + radius=cyl_radius, + length=cyl_length, + initial_offset=initial_offset) + if task == 'visual_point_nav_fixed': + env_task = VisualPointNavFixedTask( + env=env, target_pos=target_pos, target_pos_vis_obj=vis_obj) + env.task = env_task + elif task == 'visual_point_nav_random': + env_task = VisualPointNavRandomTask( + env=env, target_pos_vis_obj=vis_obj) + env.task = env_task + elif task == 'visual_object_nav': + pass + else: + raise ValueError(f'Unrecoganized task: {task}') + env.simulator.renderer.set_fov(fov) + + discount = env.config.get('discount_factor') + max_episode_steps = env.config.get('max_step') - 1 + + # apply wrappers + sensors = env.observation_space.spaces.keys() + image_int_wrapper_fields = [ + field for field in ['rgb', 'depth'] if field in sensors + ] + one_hot_int_wrapper_fields = [ + field for field in ['task_obs'] if field in sensors + ] + change_channel_fields = [ + field for field in ['rgb', 'depth'] if field in sensors + ] + flatten_fields = [field for field in ['scan'] if field in sensors] + stack_rgbd = 'rgb' in sensors and 'depth' in sensors + apply_render_wrapper = 'rgb' in sensors + + if image_int_wrapper_fields: + env = ImageIntWrapper(env, fields=image_int_wrapper_fields) + if one_hot_int_wrapper_fields: + env = OneHotIntWrapper(env, fields=one_hot_int_wrapper_fields) + if use_gray_scale: + assert 'rgb' in sensors + env = FrameGrayScale(env, fields=['rgb']) + if change_channel_fields: + env = ImageChannelFirst(env, fields=change_channel_fields) + if stack_rgbd: + env = StackRGBDWrapper(env) + if flatten_fields: + env = FlattenWrapper(env, fields=flatten_fields) + if use_gft: + env = GFTTupleInputWrapper(env) + if apply_render_wrapper: + env = RenderWrapper(env) + if record_pybullet: + assert pybullet_record_file is not None + env = PyBulletRecorderWrapper(env, pybullet_record_file) + + return suite_gym.wrap_env( + env, + env_id=env_id, + discount=discount, + max_episode_steps=max_episode_steps, + gym_env_wrappers=gym_env_wrappers, + alf_env_wrappers=alf_env_wrappers, + # instead of using suite_gym's wrapper, for which we cannot define fields, we apply the wrapper within load() + image_channel_first=False) diff --git a/alf/environments/igibson/suite_igibson_test.py b/alf/environments/igibson/suite_igibson_test.py new file mode 100644 index 000000000..1dd5b9f66 --- /dev/null +++ b/alf/environments/igibson/suite_igibson_test.py @@ -0,0 +1,135 @@ +# Copyright (c) 2020 Horizon Robotics. All Rights Reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import functools +import os.path +import os +import gin + +import alf +from alf.environments import alf_environment, thread_environment, parallel_environment +from alf.environments.igibson import suite_igibson +from alf.data_structures import StepType + +import igibson +from igibson.render.profiler import Profiler +import logging + + +class SuiteGibsonTest(alf.test.TestCase): + def setUp(self): + super().setUp() + gin.clear_config() + + def tearDown(self): + super().tearDown() + self._env.close() + + def test_unwrapped_env(self): + self._env = suite_igibson.load( + config_file=os.path.join('configs', + 'turtlebot_point_nav_stadium.yaml'), + env_mode='headless', + task='visual_point_nav_random', + physics_timestep=1.0 / 120., + ) + + self.assertIsInstance(self._env, alf_environment.AlfEnvironment) + self._env.reset() + for i in range(100): + with Profiler('Environment action step'): + actions = self._env.action_spec().sample() + # unwrapped env (not thread_env or parallel_env) needs to convert + # from tensor to array + timestep = self._env.step(actions.cpu().numpy()) + if timestep.step_type == StepType.LAST: + logging.info( + "Episode finished after {} timesteps".format(i + 1)) + break + + def test_thread_env(self): + self._env = thread_environment.ThreadEnvironment( + lambda: suite_igibson.load( + config_file=os.path.join('configs', + 'turtlebot_point_nav_stadium.yaml'), + env_mode='headless', + task='visual_point_nav_random', + physics_timestep=1.0 / 120., + )) + self.assertIsInstance(self._env, alf_environment.AlfEnvironment) + self._env.reset() + for i in range(100): + with Profiler('Environment action step'): + actions = self._env.action_spec().sample() + timestep = self._env.step(actions) + if timestep.step_type == StepType.LAST: + logging.info( + "Episode finished after {} timesteps".format(i + 1)) + break + + def test_parallel_env(self): + env_num = 4 + + def ctor(env_id=None): + return suite_igibson.load( + config_file=os.path.join('configs', + 'turtlebot_point_nav_stadium.yaml'), + env_mode='headless', + task='visual_point_nav_random', + physics_timestep=1.0 / 120., + ) + + constructor = functools.partial(ctor) + + self._env = parallel_environment.ParallelAlfEnvironment( + [constructor] * env_num) + self.assertTrue(self._env.batched) + self.assertEqual(self._env.batch_size, env_num) + + actions = self._env.action_spec().sample(outer_dims=(env_num, )) + for _ in range(10): + time_step = self._env.step(actions) + + def test_env_info(self): + # test creating multiple envs in the same process + l0_env = suite_igibson.load( + config_file=os.path.join('configs', + 'turtlebot_point_nav_stadium.yaml'), + env_mode='headless', + task='visual_point_nav_random', + physics_timestep=1.0 / 120., + ) + l1_env = suite_igibson.load( + config_file=os.path.join('configs', + 'turtlebot_point_nav_stadium.yaml'), + env_mode='headless', + task='visual_point_nav_random', + physics_timestep=1.0 / 120., + ) + + # level 0 envs don't have costs + actions = l0_env.action_spec().sample() + time_step = l0_env.step(actions.cpu().numpy()) + # ['collision_step', 'done', 'episode_length', 'path_length', 'spl', 'success'] + self.assertEqual(len(time_step.env_info.keys()), 6) + + actions = l1_env.action_spec().sample() + time_step = l1_env.step(actions.cpu().numpy()) + self.assertEqual(len(time_step.env_info.keys()), 6) + l0_env.close() + self._env = l1_env + + +if __name__ == '__main__': + alf.test.main()