Motion planner improvements and instance memory setup

.gitignore

@@ -86,4 +86,7 @@ projects/habitat_ovmm/configs/agent/generated/*
 # Remove ros2 build, install and log
 /build/*
 /install/*
-/log/*
+/log/*
+
+# VLM output
+vlm_plan.txt

.gitmodules

@@ -23,6 +23,3 @@
 [submodule "src/home_robot/home_robot/perception/detection/grounded_sam/Grounded-Segment-Anything"]
 	path = src/home_robot/home_robot/perception/detection/grounded_sam/Grounded-Segment-Anything
 	url = https://github.com/IDEA-Research/Grounded-Segment-Anything
-[submodule "src/third_party/spot-sim2real"]
-	path = src/third_party/spot-sim2real
-	url = [email protected]:Jdvakil/spot-sim2real.git

install.sh

@@ -0,0 +1,74 @@
+#!/usr/bin/env bash
+# This script (c) 2024 Chris Paxton under the MIT license: https://opensource.org/licenses/MIT
+# This script is designed to install the HomeRobot/StretchPy environment.
+export PYTORCH_VERSION=2.1.2
+export CUDA_VERSION=11.8
+export PYTHON_VERSION=3.10
+ENV_NAME=home-robot
+CUDA_VERSION_NODOT="${CUDA_VERSION//./}"
+export CUDA_HOME=/usr/local/cuda-$CUDA_VERSION
+echo "=============================================="
+echo "         INSTALLING STRETCH AI TOOLS"
+echo "=============================================="
+echo "---------------------------------------------"
+echo "Environment name: $ENV_NAME"
+echo "PyTorch Version: $PYTORCH_VERSION"
+echo "CUDA Version: $CUDA_VERSION"
+echo "Python Version: $PYTHON_VERSION"
+echo "CUDA Version No Dot: $CUDA_VERSION_NODOT"
+echo "---------------------------------------------"
+echo "Notes:"
+echo " - This script will remove the existing environment if it exists."
+echo " - This script will install the following packages:"
+echo "   - pytorch=$PYTORCH_VERSION"
+echo "   - pytorch-cuda=$CUDA_VERSION"
+echo "   - pyg"
+echo "   - torchvision"
+echo "   - python=$PYTHON_VERSION"
+echo " - This script will install the following packages from source:"
+echo "   - pytorch3d"
+echo "   - torch_scatter"
+echo "   - torch_cluster"
+echo " - Python version 3.12 is not supported by Open3d."
+echo "---------------------------------------------"
+echo "Currently:"
+echo " - CUDA_HOME=$CUDA_HOME"
+echo " - python=`which python`"
+echo "---------------------------------------------"
+read -p "Does all this look correct? (y/n) " yn
+case $yn in
+	y ) echo "Starting installation...";;
+	n ) echo "Exiting...";
+		exit;;
+	* ) echo Invalid response!;
+		exit 1;;
+esac
+mamba env remove -n $ENV_NAME -y
+mamba create -n $ENV_NAME -c pyg -c pytorch -c nvidia pytorch=$PYTORCH_VERSION pytorch-cuda=$CUDA_VERSION pyg torchvision python=$PYTHON_VERSION -y 
+source activate $ENV_NAME
+
+# Now install pytorch3d a bit faster
+mamba install -c fvcore -c iopath -c conda-forge fvcore iopath -y
+
+pip install "git+https://github.com/facebookresearch/pytorch3d.git@stable"
+pip install torch_cluster -f https://pytorch-geometric.com/whl/torch-${PYTORCH_VERSION}+${CUDA_VERSION_NODOT}.html
+pip install torch_scatter -f https://pytorch-geometric.com/whl/torch-${PYTORCH_VERSION}+${CUDA_VERSION_NODOT}.html
+pip install torch_geometric
+pip install -e ./src[dev]
+# TODO: should we remove this?
+# Open3d is an optional dependency - not included in setup.py since not supported on 3.12
+# pip install open3d scikit-fmm
+
+echo "=============================================="
+echo "         INSTALLATION COMPLETE"
+echo "Finished setting up the StretchPy environment."
+echo "Environment name: $ENV_NAME"
+echo "CUDA Version: $CUDA_VERSION"
+echo "Python Version: $PYTHON_VERSION"
+echo "CUDA Version No Dot: $CUDA_VERSION_NODOT"
+echo "CUDA_HOME=$CUDA_HOME"
+echo "python=`which python`"
+echo "You can start using it with:"
+echo ""
+echo "     source activate $ENV_NAME"
+echo "=============================================="

projects/habitat_ovmm/configs/env/hssd_eval_robot_agent.yaml

@@ -0,0 +1,32 @@
+NO_GPU: 0                 # 1: ignore IDs above and run on CPU, 0: run on GPUs with IDs above
+NUM_ENVIRONMENTS: 1       # number of environments (per agent process)
+DUMP_LOCATION: datadump   # path to dump models and log
+EXP_NAME: eval_hssd       # experiment name
+VISUALIZE: 0              # 1: render observation and predicted semantic map, 0: no visualization
+PRINT_IMAGES: 1           # 1: save visualization as images, 0: no image saving
+GROUND_TRUTH_SEMANTICS: 1 # 1: use ground-truth semantics (for debugging / ablations)
+seed: 0                   # seed
+SHOW_RL_OBS: False         # whether to show the observations passed to RL policices, for debugging
+
+ENVIRONMENT:
+  forward: 0.25           # forward motion (in meters)
+  turn_angle: 30.0        # agent turn angle (in degrees)
+  frame_height: 640       # first-person frame height (in pixels)
+  frame_width: 480        # first-person frame width (in pixels)
+  camera_height: 1.31     # camera sensor height (in metres)
+  hfov: 42.0              # horizontal field of view (in degrees)
+  min_depth: 0.0          # minimum depth for depth sensor (in metres)
+  max_depth: 10.0         # maximum depth for depth sensor (in metres)
+  num_receptacles: 21
+  category_map_file: projects/real_world_ovmm/configs/example_cat_map.json
+  use_detic_viz: False
+  evaluate_instance_tracking: False # whether to evaluate the built instance map against groundtruth instance ids
+  use_opencv_camera_pose: True # whether to convert camera pose to opencv convention, set False for OVMM challenge baseline and True for voxel code
+
+EVAL_VECTORIZED:
+  simulator_gpu_ids: [1, 2, 3, 4, 5, 6, 7] # IDs of GPUs to use for vectorized environments
+  split: val                # eval split
+  num_episodes_per_env: null # number of eval episodes per environment
+  record_videos: 0          # 1: record videos from printed images, 0: don't
+  record_planner_videos: 0  # 1: record planner videos (if record videos), 0: don't
+  metrics_save_freq: 5      # save metrics after every n episodes

projects/habitat_ovmm/eval_robot_agent.py

@@ -14,13 +14,13 @@
     get_habitat_config,
     get_omega_config,
 )
-from utils.env_utils import create_ovmm_env_fn
 
 from home_robot.agent.multitask import get_parameters
 from home_robot.agent.multitask.robot_agent import RobotAgent
 from home_robot.perception import create_semantic_sensor
 from home_robot.utils.rpc import get_vlm_rpc_stub
 from home_robot_sim.ovmm_sim_client import OvmmSimClient, SimGraspPlanner
+from home_robot_sim.utils.env_utils import create_ovmm_env_fn
 
 os.environ["OPENBLAS_NUM_THREADS"] = "1"
 os.environ["NUMEXPR_NUM_THREADS"] = "1"

projects/habitat_ovmm/test_owlv2.py

@@ -0,0 +1,31 @@
+import requests
+import torch
+from PIL import Image
+from transformers import Owlv2ForObjectDetection, Owlv2Processor
+
+processor = Owlv2Processor.from_pretrained("google/owlv2-base-patch16-ensemble")
+model = Owlv2ForObjectDetection.from_pretrained("google/owlv2-base-patch16-ensemble")
+
+url = "http://images.cocodataset.org/val2017/000000039769.jpg"
+image = Image.open(requests.get(url, stream=True).raw)
+texts = [["a photo of a cat"]]
+inputs = processor(text=texts, images=image, return_tensors="pt")
+outputs = model(**inputs)
+
+# Target image sizes (height, width) to rescale box predictions [batch_size, 2]
+target_sizes = torch.Tensor([image.size[::-1]])
+# Convert outputs (bounding boxes and class logits) to COCO API
+results = processor.post_process_object_detection(
+    outputs=outputs, threshold=0.1, target_sizes=target_sizes
+)
+
+i = 0  # Retrieve predictions for the first image for the corresponding text queries
+text = texts[i]
+boxes, scores, labels = results[i]["boxes"], results[i]["scores"], results[i]["labels"]
+
+# Print detected objects and rescaled box coordinates
+for box, score, label in zip(boxes, scores, labels):
+    box = [round(i, 2) for i in box.tolist()]
+    print(
+        f"Detected {text[label]} with confidence {round(score.item(), 3)} at location {box}"
+    )

projects/habitat_ovmm/test_real_traj.ipynb

@@ -0,0 +1,155 @@
+{
+ "cells": [
+  {
+   "cell_type": "code",
+   "execution_count": null,
+   "metadata": {},
+   "outputs": [],
+   "source": [
+    "root_path = \"/home/xiaohan/accel-cortex/\"\n",
+    "\n",
+    "import pickle\n",
+    "\n",
+    "import numpy as np\n",
+    "\n",
+    "# with open(root_path + \"debug_svm.pkl\", \"rb\") as f:\n",
+    "#     svm = pickle.load(f)\n",
+    "\n",
+    "\n",
+    "# observations = svm.observations\n",
+    "# with open(root_path + \"annotation.pkl\", \"rb\") as f:\n",
+    "#     annotation = pickle.load(f)\n",
+    "with open(root_path + \"stretch_output_2024-03-13_15-23-13.pkl\", \"rb\") as f:\n",
+    "    obs_history = pickle.load(f)\n",
+    "\n",
+    "# print(annotation[\"task\"])\n",
+    "# key_frames = []\n",
+    "# key_obs = []\n",
+    "# for idx, obs in enumerate(observations):\n",
+    "#     perceived_ids = np.unique(obs.obs.task_observations[\"gt_instance_ids\"])\n",
+    "#     for target_id in annotation[\"object_ids\"]:\n",
+    "#         if (target_id + 1) in perceived_ids:\n",
+    "#             print(\"target observation found\")\n",
+    "#             key_frames.append(obs)\n",
+    "#             key_obs.append(obs_history[idx])\n",
+    "# obs = key_frames[-1]\n",
+    "key_obs = obs_history[\"obs\"]\n",
+    "obs = key_obs[-1]\n",
+    "\n",
+    "import time\n",
+    "from pathlib import Path\n",
+    "\n",
+    "import imageio\n",
+    "import yaml\n",
+    "from PIL import Image\n",
+    "\n",
+    "from home_robot.agent.multitask import get_parameters\n",
+    "from home_robot.mapping.voxel import (\n",
+    "    SparseVoxelMap,\n",
+    "    SparseVoxelMapNavigationSpace,\n",
+    "    plan_to_frontier,\n",
+    ")\n",
+    "from home_robot.perception import create_semantic_sensor\n",
+    "from home_robot.perception.encoders import get_encoder\n",
+    "\n",
+    "# image_array = np.array(obs.obs.rgb, dtype=np.uint8)\n",
+    "# print(image_array.shape)\n",
+    "# # image_array = image_array[..., ::-1]\n",
+    "# image = Image.fromarray(image_array)\n",
+    "\n",
+    "\n",
+    "parameters = yaml.safe_load(\n",
+    "    Path(\"/home/xiaohan/home-robot/src/home_robot_sim/configs/gpt4v.yaml\").read_text()\n",
+    ")\n",
+    "config, semantic_sensor = create_semantic_sensor()\n",
+    "semantic_sensor\n",
+    "\n",
+    "# parameters = get_parameters(cfg.agent_parameters)\n",
+    "encoder = get_encoder(parameters[\"encoder\"], parameters[\"encoder_args\"])\n",
+    "\n",
+    "voxel_map = SparseVoxelMap(\n",
+    "    resolution=parameters[\"voxel_size\"],\n",
+    "    local_radius=parameters[\"local_radius\"],\n",
+    "    obs_min_height=parameters[\"obs_min_height\"],\n",
+    "    obs_max_height=parameters[\"obs_max_height\"],\n",
+    "    min_depth=parameters[\"min_depth\"],\n",
+    "    max_depth=parameters[\"max_depth\"],\n",
+    "    pad_obstacles=parameters[\"pad_obstacles\"],\n",
+    "    add_local_radius_points=parameters.get(\"add_local_radius_points\", True),\n",
+    "    remove_visited_from_obstacles=parameters.get(\n",
+    "        \"remove_visited_from_obstacles\", False\n",
+    "    ),\n",
+    "    obs_min_density=parameters[\"obs_min_density\"],\n",
+    "    smooth_kernel_size=parameters[\"smooth_kernel_size\"],\n",
+    "    encoder=encoder,\n",
+    "    use_median_filter=parameters.get(\"use_median_filter\", False),\n",
+    "    median_filter_size=parameters.get(\"median_filter_size\", 5),\n",
+    "    median_filter_max_error=parameters.get(\"median_filter_max_error\", 0.01),\n",
+    "    use_derivative_filter=parameters.get(\"use_derivative_filter\", False),\n",
+    "    derivative_filter_threshold=parameters.get(\"derivative_filter_threshold\", 0.5),\n",
+    "    instance_memory_kwargs={\n",
+    "        \"min_pixels_for_instance_view\": parameters.get(\n",
+    "            \"min_pixels_for_instance_view\", 100\n",
+    "        ),\n",
+    "        \"min_instance_thickness\": parameters.get(\"min_instance_thickness\", 0.05),\n",
+    "        \"min_instance_vol\": parameters.get(\"min_instance_vol\", 1e-6),\n",
+    "        \"max_instance_vol\": parameters.get(\"max_instance_vol\", 10.0),\n",
+    "        \"min_instance_height\": parameters.get(\"min_instance_height\", 0.1),\n",
+    "        \"max_instance_height\": parameters.get(\"max_instance_height\", 1.8),\n",
+    "        \"open_vocab_cat_map_file\": parameters.get(\"open_vocab_cat_map_file\", None),    \n",
+    "    },\n",
+    ")\n",
+    "\n",
+    "voxel_map.reset()\n",
+    "# key_obs = [key_obs[5]]\n",
+    "for idx, obs in enumerate(key_obs):\n",
+    "\n",
+    "    image_array = np.array(obs.rgb, dtype=np.uint8)\n",
+    "    # print(image_array.shape)\n",
+    "    # # image_array = image_array[..., ::-1]\n",
+    "    image = Image.fromarray(image_array)\n",
+    "    image.show()\n",
+    "\n",
+    "    obs = semantic_sensor.predict(obs)\n",
+    "    voxel_map.add_obs(obs)\n",
+    "\n",
+    "voxel_map.show(\n",
+    "    instances=True,\n",
+    "    height=1000,\n",
+    "    boxes_plot_together=False,\n",
+    "    backend=\"pytorch3d\",\n",
+    ")\n"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": null,
+   "metadata": {},
+   "outputs": [],
+   "source": [
+    "voxel_map.get_instances()[3]"
+   ]
+  }
+ ],
+ "metadata": {
+  "kernelspec": {
+   "display_name": "home-robot",
+   "language": "python",
+   "name": "python3"
+  },
+  "language_info": {
+   "codemirror_mode": {
+    "name": "ipython",
+    "version": 3
+   },
+   "file_extension": ".py",
+   "mimetype": "text/x-python",
+   "name": "python",
+   "nbconvert_exporter": "python",
+   "pygments_lexer": "ipython3",
+   "version": "3.9.18"
+  }
+ },
+ "nbformat": 4,
+ "nbformat_minor": 2
+}