{
 "cells": [
  {
   "cell_type": "markdown",
   "id": "371659c1",
   "metadata": {},
   "source": [
    "# 键盘采集示教数据\n",
    "\n",
    "在给定环境中采集示教数据。\n",
    "任务是抓起杯子并放到盘子上。当杯子在盘子上、夹爪打开且末端执行器位于杯子上方时，环境会判定成功。\n",
    "\n",
    "<img src=\"./media/teleop_v2.gif\" width=\"480\" height=\"360\">\n",
    "\n",
    "键位说明：WASD 控制 x-y 平面移动，R/F 控制 z 轴，Q/E 控制倾斜，方向键控制其余旋转。\n",
    "\n",
    "空格键切换夹爪开合状态，Z 键会重置环境并丢弃当前回合缓存数据。\n",
    "\n",
    "叠加图像说明：\n",
    "- 右上：Agent 视角\n",
    "- 右下：腕部（第一人称）视角\n",
    "- 左上：侧视图\n",
    "- 左下：俯视图\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 1,
   "id": "a8a4fa1a",
   "metadata": {},
   "outputs": [
    {
     "name": "stderr",
     "output_type": "stream",
     "text": [
      "/home/jeongeun/.pyenv/versions/3.10.2/envs/lerobot/lib/python3.10/site-packages/tqdm/auto.py:21: TqdmWarning: IProgress not found. Please update jupyter and ipywidgets. See https://ipywidgets.readthedocs.io/en/stable/user_install.html\n",
      "  from .autonotebook import tqdm as notebook_tqdm\n"
     ]
    }
   ],
   "source": [
    "import sys\n",
    "import random\n",
    "import numpy as np\n",
    "import os\n",
    "from PIL import Image\n",
    "from mujoco_env.y_env2 import SimpleEnv2\n",
    "from lerobot.common.datasets.lerobot_dataset import LeRobotDataset"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "id": "7f7ecde8",
   "metadata": {},
   "outputs": [],
   "source": [
    "# If you want to randomize the object positions, set this to None\n",
    "# If you fix the seed, the object positions will be the same every time\n",
    "SEED = 0 \n",
    "# SEED = None <- Uncomment this line to randomize the object positions\n",
    "\n",
    "REPO_NAME = 'datawhale_eai_pnp_language'\n",
    "NUM_DEMO = 20 # Number of demonstrations to collect\n",
    "ROOT = \"./demo_data_language\" # The root directory to save the demonstrations"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "id": "00198f05",
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "\n",
      "-----------------------------------------------------------------------------\n",
      "name:[Tabletop] dt:[0.002] HZ:[500]\n",
      " n_qpos:[31] n_qvel:[28] n_qacc:[28] n_ctrl:[10]\n",
      " integrator:[IMPLICITFAST]\n",
      "\n",
      "n_body:[23]\n",
      " [0/23] [world] mass:[0.00]kg\n",
      " [1/23] [front_object_table] mass:[1.00]kg\n",
      " [2/23] [camera] mass:[0.00]kg\n",
      " [3/23] [camera2] mass:[0.00]kg\n",
      " [4/23] [camera3] mass:[0.00]kg\n",
      " [5/23] [link1] mass:[2.06]kg\n",
      " [6/23] [link2] mass:[3.68]kg\n",
      " [7/23] [link3] mass:[2.39]kg\n",
      " [8/23] [link4] mass:[1.40]kg\n",
      " [9/23] [link5] mass:[1.40]kg\n",
      " [10/23] [link6] mass:[0.65]kg\n",
      " [11/23] [camera_center] mass:[0.00]kg\n",
      " [12/23] [tcp_link] mass:[0.32]kg\n",
      " [13/23] [rh_p12_rn_r1] mass:[0.07]kg\n",
      " [14/23] [rh_p12_rn_r2] mass:[0.02]kg\n",
      " [15/23] [rh_p12_rn_l1] mass:[0.07]kg\n",
      " [16/23] [rh_p12_rn_l2] mass:[0.02]kg\n",
      " [17/23] [body_obj_mug_5] mass:[0.00]kg\n",
      " [18/23] [object_mug_5] mass:[0.08]kg\n",
      " [19/23] [body_obj_plate_11] mass:[0.00]kg\n",
      " [20/23] [object_plate_11] mass:[0.10]kg\n",
      " [21/23] [body_obj_mug_6] mass:[0.00]kg\n",
      " [22/23] [object_mug_6] mass:[0.08]kg\n",
      "body_total_mass:[13.35]kg\n",
      "\n",
      "n_geom:[116]\n",
      "geom_names:['floor', None, 'front_object_table', None, None, None, None, None, None, None, None, None, None, None, None, None, None, None, None, None, None, None, None, None, None, None, None, None, None, None, None, None, None, None, None, None, None, None, None, None, None, None, None, None, None, None, None, None, None, None, None, None, None, None, None, None, None, None, None, None, None, None, None, None, None, None, None, None, None, None, None, None, None, None, None, None, None, None, None, None, None, None, None, None, None, None, None, None, None, None, None, None, None, None, None, None, None, None, None, None, None, None, None, None, None, None, None, None, None, None, None, None, None, None, None, None]\n",
      "\n",
      "n_mesh:[112]\n",
      "mesh_names:['base_unit', 'link1', 'link2', 'link3', 'link4', 'link5', 'link6', 'flange', 'base', 'r1', 'r2', 'l1', 'l2', 'mug_5_normalized_0_vis', 'mug_5_normalized_collision_22._coll', 'mug_5_normalized_collision_23._coll', 'mug_5_normalized_collision_21._coll', 'mug_5_normalized_collision_20._coll', 'mug_5_normalized_collision_24._coll', 'mug_5_normalized_collision_30._coll', 'mug_5_normalized_collision_18._coll', 'mug_5_normalized_collision_19._coll', 'mug_5_normalized_collision_31._coll', 'mug_5_normalized_collision_25._coll', 'mug_5_normalized_collision_27._coll', 'mug_5_normalized_collision_26._coll', 'mug_5_normalized_collision_9._coll', 'mug_5_normalized_collision_8._coll', 'mug_5_normalized_collision_6._coll', 'mug_5_normalized_collision_7._coll', 'mug_5_normalized_collision_5._coll', 'mug_5_normalized_collision_4._coll', 'mug_5_normalized_collision_0._coll', 'mug_5_normalized_collision_1._coll', 'mug_5_normalized_collision_3._coll', 'mug_5_normalized_collision_2._coll', 'mug_5_normalized_collision_17._coll', 'mug_5_normalized_collision_16._coll', 'mug_5_normalized_collision_28._coll', 'mug_5_normalized_collision_14._coll', 'mug_5_normalized_collision_15._coll', 'mug_5_normalized_collision_29._coll', 'mug_5_normalized_collision_11._coll', 'mug_5_normalized_collision_10._coll', 'mug_5_normalized_collision_12._coll', 'mug_5_normalized_collision_13._coll', 'plate_11_normalized_0_vis', 'plate_11_normalized_collision_22._coll', 'plate_11_normalized_collision_23._coll', 'plate_11_normalized_collision_21._coll', 'plate_11_normalized_collision_20._coll', 'plate_11_normalized_collision_24._coll', 'plate_11_normalized_collision_30._coll', 'plate_11_normalized_collision_18._coll', 'plate_11_normalized_collision_19._coll', 'plate_11_normalized_collision_31._coll', 'plate_11_normalized_collision_25._coll', 'plate_11_normalized_collision_27._coll', 'plate_11_normalized_collision_26._coll', 'plate_11_normalized_collision_9._coll', 'plate_11_normalized_collision_8._coll', 'plate_11_normalized_collision_6._coll', 'plate_11_normalized_collision_7._coll', 'plate_11_normalized_collision_5._coll', 'plate_11_normalized_collision_4._coll', 'plate_11_normalized_collision_0._coll', 'plate_11_normalized_collision_1._coll', 'plate_11_normalized_collision_3._coll', 'plate_11_normalized_collision_2._coll', 'plate_11_normalized_collision_17._coll', 'plate_11_normalized_collision_16._coll', 'plate_11_normalized_collision_28._coll', 'plate_11_normalized_collision_14._coll', 'plate_11_normalized_collision_15._coll', 'plate_11_normalized_collision_29._coll', 'plate_11_normalized_collision_11._coll', 'plate_11_normalized_collision_10._coll', 'plate_11_normalized_collision_12._coll', 'plate_11_normalized_collision_13._coll', 'mug_6_normalized_0_vis', 'mug_6_normalized_collision_22._coll', 'mug_6_normalized_collision_23._coll', 'mug_6_normalized_collision_21._coll', 'mug_6_normalized_collision_20._coll', 'mug_6_normalized_collision_24._coll', 'mug_6_normalized_collision_30._coll', 'mug_6_normalized_collision_18._coll', 'mug_6_normalized_collision_19._coll', 'mug_6_normalized_collision_31._coll', 'mug_6_normalized_collision_25._coll', 'mug_6_normalized_collision_27._coll', 'mug_6_normalized_collision_26._coll', 'mug_6_normalized_collision_9._coll', 'mug_6_normalized_collision_8._coll', 'mug_6_normalized_collision_6._coll', 'mug_6_normalized_collision_7._coll', 'mug_6_normalized_collision_5._coll', 'mug_6_normalized_collision_4._coll', 'mug_6_normalized_collision_0._coll', 'mug_6_normalized_collision_1._coll', 'mug_6_normalized_collision_3._coll', 'mug_6_normalized_collision_2._coll', 'mug_6_normalized_collision_17._coll', 'mug_6_normalized_collision_16._coll', 'mug_6_normalized_collision_28._coll', 'mug_6_normalized_collision_14._coll', 'mug_6_normalized_collision_15._coll', 'mug_6_normalized_collision_29._coll', 'mug_6_normalized_collision_11._coll', 'mug_6_normalized_collision_10._coll', 'mug_6_normalized_collision_12._coll', 'mug_6_normalized_collision_13._coll']\n",
      "\n",
      "n_joint:[13]\n",
      " [0/13] [joint1] axis:[0. 0. 1.]\n",
      " [1/13] [joint2] axis:[0. 1. 0.]\n",
      " [2/13] [joint3] axis:[0. 1. 0.]\n",
      " [3/13] [joint4] axis:[0. 1. 0.]\n",
      " [4/13] [joint5] axis:[0. 0. 1.]\n",
      " [5/13] [joint6] axis:[0. 1. 0.]\n",
      " [6/13] [rh_r1] axis:[1. 0. 0.]\n",
      " [7/13] [rh_r2] axis:[-1.  0.  0.]\n",
      " [8/13] [rh_l1] axis:[-1.  0.  0.]\n",
      " [9/13] [rh_l2] axis:[1. 0. 0.]\n",
      " [10/13] [None] axis:[0. 0. 1.]\n",
      " [11/13] [None] axis:[0. 0. 1.]\n",
      " [12/13] [None] axis:[0. 0. 1.]\n",
      "\n",
      "n_dof:[28] (=number of rows of Jacobian)\n",
      " [0/28] [None] attached joint:[joint1] body:[link1]\n",
      " [1/28] [None] attached joint:[joint2] body:[link2]\n",
      " [2/28] [None] attached joint:[joint3] body:[link3]\n",
      " [3/28] [None] attached joint:[joint4] body:[link4]\n",
      " [4/28] [None] attached joint:[joint5] body:[link5]\n",
      " [5/28] [None] attached joint:[joint6] body:[link6]\n",
      " [6/28] [None] attached joint:[rh_r1] body:[rh_p12_rn_r1]\n",
      " [7/28] [None] attached joint:[rh_r2] body:[rh_p12_rn_r2]\n",
      " [8/28] [None] attached joint:[rh_l1] body:[rh_p12_rn_l1]\n",
      " [9/28] [None] attached joint:[rh_l2] body:[rh_p12_rn_l2]\n",
      " [10/28] [None] attached joint:[None] body:[body_obj_mug_5]\n",
      " [11/28] [None] attached joint:[None] body:[body_obj_mug_5]\n",
      " [12/28] [None] attached joint:[None] body:[body_obj_mug_5]\n",
      " [13/28] [None] attached joint:[None] body:[body_obj_mug_5]\n",
      " [14/28] [None] attached joint:[None] body:[body_obj_mug_5]\n",
      " [15/28] [None] attached joint:[None] body:[body_obj_mug_5]\n",
      " [16/28] [None] attached joint:[None] body:[body_obj_plate_11]\n",
      " [17/28] [None] attached joint:[None] body:[body_obj_plate_11]\n",
      " [18/28] [None] attached joint:[None] body:[body_obj_plate_11]\n",
      " [19/28] [None] attached joint:[None] body:[body_obj_plate_11]\n",
      " [20/28] [None] attached joint:[None] body:[body_obj_plate_11]\n",
      " [21/28] [None] attached joint:[None] body:[body_obj_plate_11]\n",
      " [22/28] [None] attached joint:[None] body:[body_obj_mug_6]\n",
      " [23/28] [None] attached joint:[None] body:[body_obj_mug_6]\n",
      " [24/28] [None] attached joint:[None] body:[body_obj_mug_6]\n",
      " [25/28] [None] attached joint:[None] body:[body_obj_mug_6]\n",
      " [26/28] [None] attached joint:[None] body:[body_obj_mug_6]\n",
      " [27/28] [None] attached joint:[None] body:[body_obj_mug_6]\n",
      "\n",
      "Free joint information. n_free_joint:[3]\n",
      " [0/3] [None] body_name_attached:[body_obj_mug_5]\n",
      " [1/3] [None] body_name_attached:[body_obj_plate_11]\n",
      " [2/3] [None] body_name_attached:[body_obj_mug_6]\n",
      "\n",
      "Revolute joint information. n_rev_joint:[10]\n",
      " [0/10] [joint1] range:[-6.283]~[6.283]\n",
      " [1/10] [joint2] range:[-6.283]~[6.283]\n",
      " [2/10] [joint3] range:[-6.283]~[6.283]\n",
      " [3/10] [joint4] range:[-6.283]~[6.283]\n",
      " [4/10] [joint5] range:[-6.283]~[6.283]\n",
      " [5/10] [joint6] range:[-6.283]~[6.283]\n",
      " [6/10] [rh_r1] range:[0.000]~[1.100]\n",
      " [7/10] [rh_r2] range:[0.000]~[1.000]\n",
      " [8/10] [rh_l1] range:[0.000]~[1.100]\n",
      " [9/10] [rh_l2] range:[0.000]~[1.000]\n",
      "\n",
      "Prismatic joint information. n_pri_joint:[0]\n",
      "\n",
      "Control information. n_ctrl:[10]\n",
      " [0/10] [actuator_joint1] range:[-6.283]~[6.283] gear:[1.00] type:[JOINT]\n",
      " [1/10] [actuator_joint2] range:[-6.283]~[6.283] gear:[1.00] type:[JOINT]\n",
      " [2/10] [actuator_joint3] range:[-6.283]~[6.283] gear:[1.00] type:[JOINT]\n",
      " [3/10] [actuator_joint4] range:[-6.283]~[6.283] gear:[1.00] type:[JOINT]\n",
      " [4/10] [actuator_joint5] range:[-6.283]~[6.283] gear:[1.00] type:[JOINT]\n",
      " [5/10] [actuator_joint6] range:[-6.283]~[6.283] gear:[1.00] type:[JOINT]\n",
      " [6/10] [actuator_rh_r1] range:[0.000]~[1.100] gear:[1.00] type:[JOINT]\n",
      " [7/10] [actuator_rh_r2] range:[0.000]~[1.000] gear:[1.00] type:[JOINT]\n",
      " [8/10] [actuator_rh_l1] range:[0.000]~[1.100] gear:[1.00] type:[JOINT]\n",
      " [9/10] [actuator_rh_l2] range:[0.000]~[1.000] gear:[1.00] type:[JOINT]\n",
      "\n",
      "Camera information. n_cam:[4]\n",
      " [0/4] [agentview] fov:[60.0]\n",
      " [1/4] [topview] fov:[90.0]\n",
      " [2/4] [sideview] fov:[90.0]\n",
      " [3/4] [egocentric] fov:[90.0]\n",
      "\n",
      "n_sensor:[0]\n",
      "sensor_names:[]\n",
      "n_site:[9]\n",
      "site_names:['bottom_site_mug_5', 'top_site_mug_5', 'horizontal_radius_site_mug_5', 'bottom_site_plate_11', 'top_site_plate_11', 'horizontal_radius_site_plate_11', 'bottom_site_mug_6', 'top_site_mug_6', 'horizontal_radius_site_mug_6']\n",
      "-----------------------------------------------------------------------------\n",
      "env:[Tabletop] reset\n",
      "env:[Tabletop] reset\n",
      "env:[Tabletop] initalize viewer\n",
      "DONE INITIALIZATION\n"
     ]
    }
   ],
   "source": [
    "xml_path = './asset/example_scene_y2.xml'\n",
    "# Define the environment\n",
    "PnPEnv = SimpleEnv2(xml_path, seed = SEED, state_type = 'joint_angle')"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "b3d1da1e",
   "metadata": {},
   "source": [
    "## 定义数据集特征并创建你的数据集\n",
    "数据集结构如下：\n",
    "```\n",
    "fps = 20,\n",
    "features={\n",
    "    \"observation.image\": {\n",
    "        \"dtype\": \"image\",\n",
    "        \"shape\": (256, 256, 3),\n",
    "        \"names\": [\"height\", \"width\", \"channels\"],\n",
    "    },\n",
    "    \"observation.wrist_image\": {\n",
    "        \"dtype\": \"image\",\n",
    "        \"shape\": (256, 256, 3),\n",
    "        \"names\": [\"height\", \"width\", \"channel\"],\n",
    "    },\n",
    "    \"observation.state\": {\n",
    "        \"dtype\": \"float32\",\n",
    "        \"shape\": (6,),\n",
    "        \"names\": [\"state\"], # x, y, z, roll, pitch, yaw\n",
    "    },\n",
    "    \"action\": {\n",
    "        \"dtype\": \"float32\",\n",
    "        \"shape\": (7,),\n",
    "        \"names\": [\"action\"], # 6 joint angles and 1 gripper\n",
    "    },\n",
    "    \"obj_init\": {\n",
    "        \"dtype\": \"float32\",\n",
    "        \"shape\": (6,),\n",
    "        \"names\": [\"obj_init\"], # just the initial position of the object. Not used in training.\n",
    "    },\n",
    "},\n",
    "```\n",
    "\n",
    "数据将保存在 `./demo_data` 目录中。\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "id": "c68153b9",
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Directory ./demo_data_language already exists.\n"
     ]
    }
   ],
   "source": [
    "create_new = True\n",
    "if os.path.exists(ROOT):\n",
    "    print(f\"Directory {ROOT} already exists.\")\n",
    "    ans = input(\"Do you want to delete it? (y/n) \")\n",
    "    if ans == 'y':\n",
    "        import shutil\n",
    "        shutil.rmtree(ROOT)\n",
    "    else:\n",
    "        create_new = False\n",
    "\n",
    "\n",
    "if create_new:\n",
    "    dataset = LeRobotDataset.create(\n",
    "                repo_id=REPO_NAME,\n",
    "                root = ROOT, \n",
    "                robot_type=\"omy\",\n",
    "                fps=20, # 20 frames per second\n",
    "                features={\n",
    "                    \"observation.image\": {\n",
    "                        \"dtype\": \"image\",\n",
    "                        \"shape\": (256, 256, 3),\n",
    "                        \"names\": [\"height\", \"width\", \"channels\"],\n",
    "                    },\n",
    "                    \"observation.wrist_image\": {\n",
    "                        \"dtype\": \"image\",\n",
    "                        \"shape\": (256, 256, 3),\n",
    "                        \"names\": [\"height\", \"width\", \"channel\"],\n",
    "                    },\n",
    "                    \"observation.state\": {\n",
    "                        \"dtype\": \"float32\",\n",
    "                        \"shape\": (6,),\n",
    "                        \"names\": [\"state\"], # x, y, z, roll, pitch, yaw\n",
    "                    },\n",
    "                    \"action\": {\n",
    "                        \"dtype\": \"float32\",\n",
    "                        \"shape\": (7,),\n",
    "                        \"names\": [\"action\"], # 6 joint angles and 1 gripper\n",
    "                    },\n",
    "                    \"obj_init\": {\n",
    "                        \"dtype\": \"float32\",\n",
    "                        \"shape\": (9,),\n",
    "                        \"names\": [\"obj_init\"], # just the initial position of the object. Not used in training.\n",
    "                    },\n",
    "                },\n",
    "                image_writer_threads=10,\n",
    "                image_writer_processes=5,\n",
    "        )\n",
    "else:\n",
    "    print(\"Load from previous dataset\")\n",
    "    dataset = LeRobotDataset(REPO_NAME, root=ROOT)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "84fbab41",
   "metadata": {},
   "source": [
    "## 键盘控制\n",
    "你可以用键盘遥操作机械臂并采集数据\n",
    "```\n",
    "---------     -----------------------\n",
    "   w       ->        backward\n",
    "s  a  d        left   forward   right\n",
    "---------      -----------------------\n",
    "In x, y plane\n",
    "\n",
    "---------\n",
    "R: Moving Up\n",
    "F: Moving Down\n",
    "---------\n",
    "In z axis\n",
    "\n",
    "---------\n",
    "Q: Tilt left\n",
    "E: Tilt right\n",
    "UP: Look Upward\n",
    "Down: Look Donward\n",
    "Right: Turn right\n",
    "Left: Turn left\n",
    "---------\n",
    "For rotation\n",
    "\n",
    "---------\n",
    "SPACEBAR: Toggle Gripper\n",
    "--------\n",
    "\n",
    "---------\n",
    "z: reset\n",
    "--------\n",
    "```\n",
    "重置环境会删除当前示教回合的缓存数据，并重新开始采集。\n"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "436f8b99",
   "metadata": {},
   "source": [
    "### 现在开始遥操作并采集数据\n",
    "\n",
    "**要收到成功信号，你需要松开夹爪并将末端执行器上移到杯子上方。**\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "5b080011",
   "metadata": {},
   "outputs": [],
   "source": [
    "action = np.zeros(7)\n",
    "episode_id = 0\n",
    "record_flag = False # Start recording when the robot starts moving\n",
    "while PnPEnv.env.is_viewer_alive() and episode_id < NUM_DEMO:\n",
    "    PnPEnv.step_env()\n",
    "    if PnPEnv.env.loop_every(HZ=20):\n",
    "        # check if the episode is done\n",
    "        done = PnPEnv.check_success()\n",
    "        if done and record_flag:\n",
    "            # Save the episode data and reset the environment\n",
    "            dataset.save_episode()\n",
    "            episode_id += 1\n",
    "            record_flag = False\n",
    "            if episode_id >= NUM_DEMO:\n",
    "                break\n",
    "            PnPEnv.reset()\n",
    "            continue\n",
    "        # Teleoperate the robot and get delta end-effector pose with gripper\n",
    "        action, reset  = PnPEnv.teleop_robot()\n",
    "        if not record_flag and np.any(action != 0):\n",
    "            record_flag = True\n",
    "            print(\"Start recording\")\n",
    "        if reset:\n",
    "            # Reset the environment and clear the episode buffer\n",
    "            # This can be done by pressing 'z' key\n",
    "            # PnPEnv.reset(seed=SEED)\n",
    "            PnPEnv.reset()\n",
    "            dataset.clear_episode_buffer()\n",
    "            record_flag = False\n",
    "            continue\n",
    "        # Step the environment\n",
    "        # Get the end-effector pose and images\n",
    "        joint_state = PnPEnv.get_joint_state()[:6]\n",
    "        agent_image,wrist_image = PnPEnv.grab_image()\n",
    "        # # resize to 256x256\n",
    "        agent_image = Image.fromarray(agent_image)\n",
    "        wrist_image = Image.fromarray(wrist_image)\n",
    "        agent_image = agent_image.resize((256, 256))\n",
    "        wrist_image = wrist_image.resize((256, 256))\n",
    "        agent_image = np.array(agent_image)\n",
    "        wrist_image = np.array(wrist_image)\n",
    "        PnPEnv.step(action)\n",
    "        joint_action = PnPEnv.get_commanded_joint_action()\n",
    "        if record_flag:\n",
    "            # Add the frame to the dataset\n",
    "            dataset.add_frame( {\n",
    "                    \"observation.image\": agent_image,\n",
    "                    \"observation.wrist_image\": wrist_image,\n",
    "                    \"observation.state\": joint_state,\n",
    "                    \"action\": joint_action,\n",
    "                    \"obj_init\": PnPEnv.obj_init_pose,\n",
    "                    # \"task\": PnPEnv.instruction,\n",
    "                }, task = PnPEnv.instruction\n",
    "            )\n",
    "        PnPEnv.render(teleop=True, idx=episode_id)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 8,
   "id": "e8e2b219",
   "metadata": {},
   "outputs": [],
   "source": [
    "PnPEnv.env.close_viewer()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 9,
   "id": "3ce04f3f",
   "metadata": {},
   "outputs": [
    {
     "ename": "FileNotFoundError",
     "evalue": "[Errno 2] No such file or directory: 'demo_data_language/images'",
     "output_type": "error",
     "traceback": [
      "\u001b[0;31m---------------------------------------------------------------------------\u001b[0m",
      "\u001b[0;31mFileNotFoundError\u001b[0m                         Traceback (most recent call last)",
      "Cell \u001b[0;32mIn[9], line 3\u001b[0m\n\u001b[1;32m      1\u001b[0m \u001b[38;5;66;03m# Clean up the images folder\u001b[39;00m\n\u001b[1;32m      2\u001b[0m \u001b[38;5;28;01mimport\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;21;01mshutil\u001b[39;00m\n\u001b[0;32m----> 3\u001b[0m \u001b[43mshutil\u001b[49m\u001b[38;5;241;43m.\u001b[39;49m\u001b[43mrmtree\u001b[49m\u001b[43m(\u001b[49m\u001b[43mdataset\u001b[49m\u001b[38;5;241;43m.\u001b[39;49m\u001b[43mroot\u001b[49m\u001b[43m \u001b[49m\u001b[38;5;241;43m/\u001b[39;49m\u001b[43m \u001b[49m\u001b[38;5;124;43m'\u001b[39;49m\u001b[38;5;124;43mimages\u001b[39;49m\u001b[38;5;124;43m'\u001b[39;49m\u001b[43m)\u001b[49m\n",
      "File \u001b[0;32m~/.pyenv/versions/3.10.2/lib/python3.10/shutil.py:708\u001b[0m, in \u001b[0;36mrmtree\u001b[0;34m(path, ignore_errors, onerror)\u001b[0m\n\u001b[1;32m    706\u001b[0m     orig_st \u001b[38;5;241m=\u001b[39m os\u001b[38;5;241m.\u001b[39mlstat(path)\n\u001b[1;32m    707\u001b[0m \u001b[38;5;28;01mexcept\u001b[39;00m \u001b[38;5;167;01mException\u001b[39;00m:\n\u001b[0;32m--> 708\u001b[0m     \u001b[43monerror\u001b[49m\u001b[43m(\u001b[49m\u001b[43mos\u001b[49m\u001b[38;5;241;43m.\u001b[39;49m\u001b[43mlstat\u001b[49m\u001b[43m,\u001b[49m\u001b[43m \u001b[49m\u001b[43mpath\u001b[49m\u001b[43m,\u001b[49m\u001b[43m \u001b[49m\u001b[43msys\u001b[49m\u001b[38;5;241;43m.\u001b[39;49m\u001b[43mexc_info\u001b[49m\u001b[43m(\u001b[49m\u001b[43m)\u001b[49m\u001b[43m)\u001b[49m\n\u001b[1;32m    709\u001b[0m     \u001b[38;5;28;01mreturn\u001b[39;00m\n\u001b[1;32m    710\u001b[0m \u001b[38;5;28;01mtry\u001b[39;00m:\n",
      "File \u001b[0;32m~/.pyenv/versions/3.10.2/lib/python3.10/shutil.py:706\u001b[0m, in \u001b[0;36mrmtree\u001b[0;34m(path, ignore_errors, onerror)\u001b[0m\n\u001b[1;32m    703\u001b[0m \u001b[38;5;66;03m# Note: To guard against symlink races, we use the standard\u001b[39;00m\n\u001b[1;32m    704\u001b[0m \u001b[38;5;66;03m# lstat()/open()/fstat() trick.\u001b[39;00m\n\u001b[1;32m    705\u001b[0m \u001b[38;5;28;01mtry\u001b[39;00m:\n\u001b[0;32m--> 706\u001b[0m     orig_st \u001b[38;5;241m=\u001b[39m \u001b[43mos\u001b[49m\u001b[38;5;241;43m.\u001b[39;49m\u001b[43mlstat\u001b[49m\u001b[43m(\u001b[49m\u001b[43mpath\u001b[49m\u001b[43m)\u001b[49m\n\u001b[1;32m    707\u001b[0m \u001b[38;5;28;01mexcept\u001b[39;00m \u001b[38;5;167;01mException\u001b[39;00m:\n\u001b[1;32m    708\u001b[0m     onerror(os\u001b[38;5;241m.\u001b[39mlstat, path, sys\u001b[38;5;241m.\u001b[39mexc_info())\n",
      "\u001b[0;31mFileNotFoundError\u001b[0m: [Errno 2] No such file or directory: 'demo_data_language/images'"
     ]
    }
   ],
   "source": [
    "# Clean up the images folder\n",
    "import shutil\n",
    "shutil.rmtree(dataset.root / 'images')"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "41c0e170",
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3 (ipykernel)",
   "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.12.3"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 5
}
