diff --git a/.gitattributes b/.gitattributes
index a6344aac8c09253b3b630fb776ae94478aa0275b..125a30c2d27ba3b415061182b590bdf3b887f0ae 100644
--- a/.gitattributes
+++ b/.gitattributes
@@ -33,3 +33,10 @@ saved_model/**/* filter=lfs diff=lfs merge=lfs -text
 *.zip filter=lfs diff=lfs merge=lfs -text
 *.zst filter=lfs diff=lfs merge=lfs -text
 *tfevents* filter=lfs diff=lfs merge=lfs -text
+robotiq_2f_85/ filter=lfs diff=lfs merge=lfs -text
+*.stl filter=lfs diff=lfs merge=lfs -text
+*.urdf filter=lfs diff=lfs merge=lfs -text
+*.mtl filter=lfs diff=lfs merge=lfs -text
+*.obj filter=lfs diff=lfs merge=lfs -text
+*.jpg filter=lfs diff=lfs merge=lfs -text
+*.dae filter=lfs diff=lfs merge=lfs -text
diff --git a/.gitignore b/.gitignore
new file mode 100644
index 0000000000000000000000000000000000000000..03617cdafb1b93b713d2146620a2b7a79bcea6b6
--- /dev/null
+++ b/.gitignore
@@ -0,0 +1,164 @@
+# Byte-compiled / optimized / DLL files
+__pycache__/
+*.py[cod]
+*$py.class
+
+# C extensions
+*.so
+
+# Distribution / packaging
+.Python
+build/
+develop-eggs/
+dist/
+downloads/
+eggs/
+.eggs/
+lib/
+lib64/
+parts/
+sdist/
+var/
+wheels/
+share/python-wheels/
+*.egg-info/
+.installed.cfg
+*.egg
+MANIFEST
+
+# PyInstaller
+#  Usually these files are written by a python script from a template
+#  before PyInstaller builds the exe, so as to inject date/other infos into it.
+*.manifest
+*.spec
+
+# Installer logs
+pip-log.txt
+pip-delete-this-directory.txt
+
+# Unit test / coverage reports
+htmlcov/
+.tox/
+.nox/
+.coverage
+.coverage.*
+.cache
+nosetests.xml
+coverage.xml
+*.cover
+*.py,cover
+.hypothesis/
+.pytest_cache/
+cover/
+
+# Translations
+*.mo
+*.pot
+
+# Django stuff:
+*.log
+local_settings.py
+db.sqlite3
+db.sqlite3-journal
+
+# Flask stuff:
+instance/
+.webassets-cache
+
+# Scrapy stuff:
+.scrapy
+
+# Sphinx documentation
+docs/_build/
+
+# PyBuilder
+.pybuilder/
+target/
+
+# Jupyter Notebook
+.ipynb_checkpoints
+
+# IPython
+profile_default/
+ipython_config.py
+
+# pyenv
+#   For a library or package, you might want to ignore these files since the code is
+#   intended to run in multiple environments; otherwise, check them in:
+# .python-version
+
+# pipenv
+#   According to pypa/pipenv#598, it is recommended to include Pipfile.lock in version control.
+#   However, in case of collaboration, if having platform-specific dependencies or dependencies
+#   having no cross-platform support, pipenv may install dependencies that don't work, or not
+#   install all needed dependencies.
+#Pipfile.lock
+
+# poetry
+#   Similar to Pipfile.lock, it is generally recommended to include poetry.lock in version control.
+#   This is especially recommended for binary packages to ensure reproducibility, and is more
+#   commonly ignored for libraries.
+#   https://python-poetry.org/docs/basic-usage/#commit-your-poetrylock-file-to-version-control
+#poetry.lock
+
+# pdm
+#   Similar to Pipfile.lock, it is generally recommended to include pdm.lock in version control.
+#pdm.lock
+#   pdm stores project-wide configurations in .pdm.toml, but it is recommended to not include it
+#   in version control.
+#   https://pdm.fming.dev/#use-with-ide
+.pdm.toml
+
+# PEP 582; used by e.g. github.com/David-OConnor/pyflow and github.com/pdm-project/pdm
+__pypackages__/
+
+# Celery stuff
+celerybeat-schedule
+celerybeat.pid
+
+# SageMath parsed files
+*.sage.py
+
+# Environments
+.env
+.venv
+env/
+venv/
+ENV/
+env.bak/
+venv.bak/
+
+# Spyder project settings
+.spyderproject
+.spyproject
+
+# Rope project settings
+.ropeproject
+
+# mkdocs documentation
+/site
+
+# mypy
+.mypy_cache/
+.dmypy.json
+dmypy.json
+
+# Pyre type checker
+.pyre/
+
+# pytype static type analyzer
+.pytype/
+
+# Cython debug symbols
+cython_debug/
+
+# PyCharm
+#  JetBrains specific template is maintained in a separate JetBrains.gitignore that can
+#  be found at https://github.com/github/gitignore/blob/main/Global/JetBrains.gitignore
+#  and can be added to the global gitignore or merged into this file.  For a more nuclear
+#  option (not recommended) you can uncomment the following to ignore the entire idea folder.
+#.idea/
+
+.DS_Store
+
+.vscode/
\ No newline at end of file
diff --git a/LICENSE.md b/LICENSE.md
new file mode 100644
index 0000000000000000000000000000000000000000..2697cde25676d46a917a2d9362dd0e5495b6d2ca
--- /dev/null
+++ b/LICENSE.md
@@ -0,0 +1,7 @@
+Copyright 2021 Google LLC. SPDX-License-Identifier: Apache-2.0
+
+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
+
+https://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.
\ No newline at end of file
diff --git a/README.md b/README.md
index 3f3b46a42debcdf247ede1af7acf389fbe32a184..a8ce4a83e8f274eeec8f3ad478fa4ab912e8547b 100644
--- a/README.md
+++ b/README.md
@@ -1,12 +1,69 @@
----
-title: VoxPoserExamples
-emoji: 🔥
-colorFrom: red
-colorTo: green
-sdk: gradio
-sdk_version: 3.40.1
-app_file: app.py
-pinned: false
----
-
-Check out the configuration reference at https://huggingface.co/docs/hub/spaces-config-reference
+# VoxPoser API Examples
+
+## Usage
+
+```bash
+python3 app.py
+```
+
+1. 在界面中填写OpenAI API Key，使用的代理地址，选择需要的configuration
+2. 点击Setup/Reset Simulation
+3. 填写自定义Instruction
+4. 点击Run执行（需要等待较长时间）
+
+## Example
+
+### VLM & Perception
+
+1. Open Vocab object detection [owlvit](https://huggingface.co/docs/transformers/model_doc/owlvit)
+2. [SAM](https://github.com/facebookresearch/segment-anything)
+3. Object mask tracking [XMem](https://github.com/hkchengrex/XMem)
+4. 使用realsense获得深度图
+5. 使用深度图获得法向量（抓取位姿）
+
+可替代性：
+- [x] owlvit -> Grounded SAM / YOLO
+- [x] SAM -> FastSAM / YOLO-seg
+- [ ] XMem -> DeepSORT(?) ByteTrack(?)
+
+### LMP语言模型编程
+
+语言模型编程：使用GPT-4
+
+VoxPoser需要三大类LMP:
+1. Planner
+2. Composer
+3. Value map generator
+
+可替代性：
+- [ ] GPT-4 -> LLaMA2 (?)
+
+## LMPs
+
+### Planner
+
+LMP的输出是一系列的编程模型接口，Planner将这些语言描述转化为一系列高层级的规划，每步规划这些动作将被Composer执行。
+
+模拟环境中不使用规划器，因为评估的任务由单个操作阶段组成。
+
+### Composer
+
+Composer LMP 从依次逐渐调用如下模组：
+1. 感知模组调用获得感知结果
+2. [optional] Affordance LMP
+3. [optional] Avoidance LMP
+4. [optional] End Effector Velocity LMP
+5. [optional] End Effector Rotation LMP
+6. [optional] Gripper Action LMP
+7. Execute
+
+### Value Maps
+
+TODO
+
+### Execution
+
+1. Motion Planner: 贪心搜索得到一系列末端位姿，仅适用Affordance Map 和 Avoidance Map
+2. Cost map: $W = -2 * \text{norm}(\text{Affordance}) - \text{norm}(\text{Avoidance})$
+3. 根据离开/接近，调用目标法向量的正/负值方向上的Affordance Map
+4. 根据避障目标的占据栅格occupancy_map，调整Avoidance Map
\ No newline at end of file
diff --git a/README_CaP.md b/README_CaP.md
new file mode 100644
index 0000000000000000000000000000000000000000..0b2df1b05ed8c4e6c3a37924b5800d8d670972d0
--- /dev/null
+++ b/README_CaP.md
@@ -0,0 +1,53 @@
+---
+title: Code As Policies
+emoji: 📈
+colorFrom: purple
+colorTo: indigo
+sdk: gradio
+sdk_version: 3.3.1
+app_file: app.py
+pinned: false
+license: apache-2.0
+---
+
+# Code as Policies Tabletop Manipulation Interactive Demo
+
+This demo is from the paper:
+
+[Code as Policies: Language Model Programs for Embodied Control](https://code-as-policies.github.io/)
+
+Below is an interactive demo for the simulated tabletop manipulation domain, seen in the paper section IV.D
+
+## Preparations
+1. Obtain an [OpenAI API Key](https://openai.com/blog/openai-api/)
+
+## Usage 
+1. Fill in the API Key and how many blocks and bowls to be spawned in the environment. 
+2. Click Setup/Reset Simulation
+3. Based on the new randomly sampled object names, input an instruction and click Run Instruction. If successful, this will render a video and update the simulation environment visualization.
+
+You can run instructions in sequence and refer back to previous instructions (e.g. do the same with other blocks, move the same block to the other bowl, etc). To reset, click Setup/Reset Env, and this will clear the current instruction history.
+
+## Supported Instructions
+* Spatial reasoning (e.g. to the left of the red block, the closest corner, the farthest bowl, the second block from the right)
+* Sequential actions (e.g. put blocks in matching bowls, stack blocks on the bottom right corner)
+* Contextual instructions (e.g. do the same with the blue block, undo that)
+* Language-based reasoning (e.g. put the forest-colored block on the ocean-colored bowl).
+* Simple Q&A (e.g. how many blocks are to the left of the blue bowl?)
+
+## Example Instructions 
+Note object names may need to be changed depending the sampled object names.
+* put the sun-colored block on the bowl closest to it
+* stack the blocks on the bottom most bowl
+* arrange the blocks as a square in the middle
+* move the square 5cm to the right
+* how many blocks are to the right of the orange bowl?
+* pick up the block closest to the top left corner and place it on the bottom right corner
+
+## Known Limitations
+* In simulation we're using ground truth object poses instead of using vision models. This means that instructions the require knowledge of visual apperances (e.g. darkest bowl, largest object, empty bowls) are not supported.
+* Currently, the low-level pick place primitive does not do collision checking, so if there are many objects on the table, placing actions may incur collisions.
+* The pick place primitive is also unable to pick up bowls.
+* Prompt saturation - if too many instructions (10+) are executed in a row, then the LLM may start to ignore examples in the early parts of the prompt.
+* Ambiguous instructions - if a given instruction doesn't lead to the desired actions, try rephrasing it to remove ambiguities (e.g. place the block on the closest bowl -> place the block on its closest bowl)
+* Maximum token length - you may hit the maximum token length if running multiple commands in sequence. Please reset the simulation when this happens.
diff --git a/Real/composer/real_composer_prompt.py b/Real/composer/real_composer_prompt.py
new file mode 100644
index 0000000000000000000000000000000000000000..36e1706e24f4521c0732d7da544dbc9d014897cc
--- /dev/null
+++ b/Real/composer/real_composer_prompt.py
@@ -0,0 +1,120 @@
+import numpy as np
+from env_utils import execute, reset_to_default_pose
+from perception_utils import parse_query_obj
+from plan_utils import get_affordance_map, get_avoidance_map, get_velocity_map, get_rotation_map, get_gripper_map
+
+# Query: move ee forward for 10cm.
+movable = parse_query_obj('gripper')
+affordance_map = get_affordance_map(f'a point 10cm in front of {movable.position}')
+execute(movable, affordance_map)
+
+# Query: go back to default.
+reset_to_default_pose()
+
+# Query: move the gripper behind the bowl, and slow down when near the bowl.
+movable = parse_query_obj('gripper')
+affordance_map = get_affordance_map('a point 15cm behind the bowl')
+avoidance_map = get_avoidance_map('10cm near the bowl')
+velocity_map = get_velocity_map('slow down when near the bowl')
+execute(movable, affordance_map=affordance_map, avoidance_map=avoidance_map, velocity_map=velocity_map)
+
+# Query: move to the back side of the table while staying at least 5cm from the blue block.
+movable = parse_query_obj('gripper')
+affordance_map = get_affordance_map('a point on the back side of the table')
+avoidance_map = get_avoidance_map('5cm from the blue block')
+execute(movable, affordance_map=affordance_map, avoidance_map=avoidance_map)
+
+# Query: move to the top of the plate and face the plate.
+movable = parse_query_obj('gripper')
+affordance_map = get_affordance_map('a point 10cm above the plate')
+rotation_map = get_rotation_map('face the plate')
+execute(movable, affordance_map=affordance_map, rotation_map=rotation_map)
+
+# Query: drop the toy inside container.
+movable = parse_query_obj('gripper')
+affordance_map = get_affordance_map('a point 15cm above the container')
+gripper_map = get_gripper_map('close everywhere but open when on top of the container')
+execute(movable, affordance_map=affordance_map, gripper_map=gripper_map)
+
+# Query: push close the topmost drawer.
+movable = parse_query_obj('topmost drawer handle')
+affordance_map = get_affordance_map('a point 30cm into the topmost drawer handle')
+execute(movable, affordance_map=affordance_map)
+
+# Query: push the second to the left block along the red line.
+movable = parse_query_obj('second to the left block')
+affordance_map = get_affordance_map('the red line')
+execute(movable, affordance_map=affordance_map)
+
+# Query: grasp the blue block from the table at a quarter of the speed.
+movable = parse_query_obj('gripper')
+affordance_map = get_affordance_map('a point at the center of blue block')
+velocity_map = get_velocity_map('quarter of the speed')
+gripper_map = get_gripper_map('open everywhere except 1cm around the blue block')
+execute(movable, affordance_map=affordance_map, velocity_map=velocity_map, gripper_map=gripper_map)
+
+# Query: move to the left of the brown block.
+movable = parse_query_obj('gripper')
+affordance_map = get_affordance_map('a point 10cm to the left of the brown block')
+execute(movable, affordance_map=affordance_map)
+
+# Query: move to the top of the tray that contains the lemon.
+movable = parse_query_obj('gripper')
+affordance_map = get_affordance_map('a point 10cm above the tray that contains the lemon')
+execute(movable, affordance_map=affordance_map)
+
+# Query: close drawer by 5cm.
+movable = parse_query_obj('drawer handle')
+affordance_map = get_affordance_map('a point 5cm into the drawer handle')
+execute(movable, affordance_map=affordance_map)
+
+# Query: move to 5cm on top of the soda can, at 0.5x speed when within 20cm of the wooden mug, and keep at least 15cm away from the wooden mug.
+movable = parse_query_obj('gripper')
+affordance_map = get_affordance_map('a point 5cm above the soda can')
+avoidance_map = get_avoidance_map('15cm from the wooden mug')
+velocity_map = get_velocity_map('0.5x speed when within 20cm of the wooden mug')
+execute(movable, affordance_map=affordance_map, avoidance_map=avoidance_map, velocity_map=velocity_map)
+
+# Query: wipe the red dot but avoid the blue block.
+movable = parse_query_obj('gripper')
+affordance_map = get_affordance_map('the red dot')
+avoidance_map = get_avoidance_map('10cm from the blue block')
+execute(movable, affordance_map=affordance_map, avoidance_map=avoidance_map)
+
+# Query: grasp the mug from the shelf.
+movable = parse_query_obj('gripper')
+affordance_map = get_affordance_map('a point at the center of the mug handle')
+gripper_map = get_gripper_map('open everywhere except 1cm around the mug handle')
+execute(movable, affordance_map=affordance_map, gripper_map=gripper_map)
+
+# Query: move to 10cm on top of the soup bowl, and 5cm to the left of the soup bowl, while away from the glass, at 0.75x speed.
+movable = parse_query_obj('gripper')
+affordance_map = get_affordance_map('a point 10cm above and 5cm to the left of the soup bowl')
+avoidance_map = get_avoidance_map('10cm from the glass')
+velocity_map = get_velocity_map('0.75x speed')
+execute(movable, affordance_map=affordance_map, avoidance_map=avoidance_map, velocity_map=velocity_map)
+
+# Query: open gripper.
+movable = parse_query_obj('gripper')
+gripper_map = get_gripper_map('open everywhere')
+execute(movable, gripper_map=gripper_map)
+
+# Query: turn counter-clockwise by 180 degrees.
+movable = parse_query_obj('gripper')
+rotation_map = get_rotation_map('turn counter-clockwise by 180 degrees')
+execute(movable, rotation_map=rotation_map)
+
+# Query: sweep all particles to the left side of the table.
+particles = parse_query_obj('particles')
+for particle in particles:
+    movable = particle
+    affordance_map = get_affordance_map('a point on the left side of the table')
+    execute(particle, affordance_map=affordance_map)
+
+# Query: grasp the bottom drawer handle while moving at 0.5x speed.
+movable = parse_query_obj('gripper')
+affordance_map = get_affordance_map('a point at the center of the bottom drawer handle')
+velocity_map = get_velocity_map('0.5x speed')
+rotation_map = get_rotation_map('face the bottom drawer handle')
+gripper_map = get_gripper_map('open everywhere except 1cm around the bottom drawer handle')
+execute(movable, affordance_map=affordance_map, velocity_map=velocity_map, rotation_map=rotation_map, gripper_map=gripper_map)
\ No newline at end of file
diff --git a/Real/perception/real_parse_query_obj_prompt.py b/Real/perception/real_parse_query_obj_prompt.py
new file mode 100644
index 0000000000000000000000000000000000000000..996b45abf82cedecabaede1f248e355563412c78
--- /dev/null
+++ b/Real/perception/real_parse_query_obj_prompt.py
@@ -0,0 +1,104 @@
+import numpy as np
+from perception_utils import detect
+
+objects = ['table', 'gripper', 'green block', 'cardboard box']
+# Query: gripper.
+gripper = detect('gripper')[0]
+ret_val = gripper
+
+objects = ['table', 'gripper', 'drawer', 'egg', 'egg', 'plate']
+# Query: topmost handle.
+handles = detect('drawer handle')
+handles = sorted(handles, key=lambda x: x.position[2])
+top_handle = handles[-1]
+ret_val = top_handle
+
+objects = ['table', 'gripper', 'yellow block', 'charging cable', 'cyan block', 'magenta block']
+# Query: second to the left block.
+blocks = detect('block')
+blocks = sorted(blocks, key=lambda x: x.position[1])
+second_left_block = blocks[1]
+ret_val = second_left_block
+
+objects = ['table', 'gripper', 'iPhone', 'ruler', 'pink line', 'blue line']
+# Query: the front most line on the table.
+lines = detect('line')
+lines = sorted(lines, key=lambda x: x.position[0])
+front_most_line = lines[-1]
+ret_val = front_most_line
+
+objects = ['table', 'gripper', 'vase', 'napkin box', 'mask']
+# Query: table.
+table = detect('table')[0]
+ret_val = table
+
+objects = ['table', 'gripper', 'bottle', 'drawer', 'bowl', 'bag']
+# Query: second to the bottom handle.
+handles = detect('drawer handle')
+handles = sorted(handles, key=lambda x: x.position[2])
+second_bottom_handle = handles[1]
+ret_val = second_bottom_handle
+
+objects = ['table', 'gripper', 'brown line', 'red block', 'monitor']
+# Query: brown line.
+brown_line = detect('brown line')[0]
+ret_val = brown_line
+
+objects = ['table', 'gripper', 'green block', 'cup holder', 'black block']
+# Query: block.
+block = detect('green block')[0]
+ret_val = block
+
+objects = ['table', 'gripper', 'mouse', 'yellow bowl', 'brown bowl', 'sticker']
+# Query: bowl closest to the sticker.
+bowls = detect('bowl')
+sticker = detect('sticker')[0]
+closest_bowl = min(bowls, key=lambda x: np.linalg.norm(x.position - sticker.position))
+ret_val = closest_bowl
+
+objects = ['table', 'gripper', 'keyboard', 'brown bag', 'pink bag', 'red tape', 'bottle']
+# Query: bag with the red tape on top.
+bags = detect('bag')
+red_tape = detect('red tape')[0]
+bag_with_red_tape = min(bags, key=lambda x: np.linalg.norm(x.position - red_tape.position))
+ret_val = bag_with_red_tape
+
+objects = ['table', 'gripper', 'grape', 'wood tray', 'strawberry', 'white tray', 'blue tray', 'bread']
+# Query: tray that contains the bread.
+trays = detect('tray')
+bread = detect('bread')[0]
+tray_with_bread = min(trays, key=lambda x: np.linalg.norm(x.position - bread.position))
+ret_val = tray_with_bread
+
+objects = ['table', 'gripper', 'drawer']
+# Query: top drawer handle.
+handles = detect('drawer handle')
+top_drawer_handle = max(handles, key=lambda x: x.position[2])
+ret_val = top_drawer_handle
+
+objects = ['table', 'gripper', 'door']
+# Query: the thing you can open the door with.
+door_handle = detect('door handle')[0]
+ret_val = door_handle
+
+objects = ['table', 'gripper', 'glass', 'vase', 'plastic bottle', 'block', 'phone case']
+# Query: anything fragile.
+fragile_items = []
+for obj in ['glass', 'vase']:
+    item = detect(obj)[0]
+    fragile_items.append(item)
+ret_val = fragile_items
+
+objects = ['table', 'gripper', 'fridge']
+# Query: fridge handle.
+fridge_handle = detect('fridge handle')[0]
+ret_val = fridge_handle
+
+objects = ['table', 'gripper', 'blue block', 'red block']
+# Query: green block.
+ret_val = None
+
+objects = ['table', 'gripper', 'yellow bowl', 'red spoon']
+# Query: gripper.
+gripper = detect('gripper')[0]
+ret_val = gripper
\ No newline at end of file
diff --git a/Real/planner/real_planner_prompt.py b/Real/planner/real_planner_prompt.py
new file mode 100644
index 0000000000000000000000000000000000000000..2c2365664eb2868b09c1a0fe12a52d4e3a6909e0
--- /dev/null
+++ b/Real/planner/real_planner_prompt.py
@@ -0,0 +1,121 @@
+import numpy as np
+from env_utils import execute
+from perception_utils import parse_query_obj
+import action_utils import composer
+
+objects = ['blue block', 'yellow block', 'mug']
+# Query: place the blue block on the yellow block, and avoid the mug at all time.
+composer("grasp the blue block while keeping at least 15cm away from the mug")
+composer("back to default pose")
+composer("move to 5cm on top of the yellow block while keeping at least 15cm away from the mug")
+composer("open gripper")
+# done
+
+objects = ['airpods', 'drawer']
+# Query: Open the drawer slowly.
+composer("grasp the drawer handle, at 0.5x speed")
+composer("move away from the drawer handle by 25cm, at 0.5x speed")
+composer("open gripper, at 0.5x speed")
+# done
+
+objects = ['tissue box', 'tissue', 'bowl']
+# Query: Can you pass me a tissue and place it next to the bowl?
+composer("grasp the tissue")
+composer("back to default pose")
+composer("move to 10cm to the right of the bowl")
+composer("open gripper")
+composer("back to default pose")
+# done
+
+objects = ['charger', 'outlet']
+# Query: unplug the charger from the wall.
+composer("grasp the charger")
+composer("back to default pose")
+# done
+
+objects = ['grape', 'lemon', 'drill', 'router', 'bread', 'tray']
+# Query: put the sweeter fruit in the tray that contains the bread.
+composer("grasp the grape")
+composer("back to default pose")
+composer("move to the top of the tray that contains the bread")
+composer("open gripper")
+# done
+
+objects = ['marbles', 'tray', 'broom']
+# Query: Can you sweep the marbles into the tray?
+composer("grasp the broom")
+composer("back to default pose")
+composer("push the marbles into the tray")
+# done
+
+objects = ['orange', 'QR code', 'lemon', 'drawer']
+# Query: put the sour fruit into the top drawer.
+composer("grasp the top drawer handle")
+composer("move away from the top drawer handle by 25cm")
+composer("open gripper")
+composer("back to default pose")
+composer("grasp the lemon")
+composer("move to 10cm on top of the top drawer")
+composer("open gripper")
+# done
+
+objects = ['fridge', 'hot soup']
+# Query: Open the fridge door and be careful around the hot soup.
+composer("grasp the fridge handle and keep at least 15cm away from the hot soup")
+composer("move away from the fridge handle by 25cm and keep at least 15cm away from the hot soup")
+composer("open gripper")
+# done
+
+objects = ['cyan bowl', 'yellow bowl', 'box', 'ice cream']
+# Query: move to the top of the cyan bowl.
+composer("move to the top of the cyan bowl")
+# done
+
+objects = ['drawer', 'umbrella']
+# Query: close the drawer.
+composer("push close the drawer handle by 25cm")
+# done
+
+objects = ['plate', 'steak', 'fork', 'knife', 'spoon']
+# Query: Could you please set up the fork for the steak for me?
+composer("grasp the fork")
+composer("back to default pose")
+composer("move to 10cm to the right of the plate")
+composer("open gripper")
+composer("back to default pose")
+# done
+
+objects = ['light switch']
+# Query: Press down the switch.
+composer("close the gripper")
+composer("move to the center of the light switch")
+composer("back to default pose")
+# done
+
+objects = ['beer']
+# Query: turn close the beer.
+composer("grasp the beer cap")
+composer("turn clockwise by 180 degrees")
+composer("back to default pose")
+# done
+
+objects = ['steak', 'grill', 'plate']
+# Query: Take the steak out of the grill and put it flat on the plate.
+composer("grasp the steak")
+composer("back to default pose")
+composer("rotate the gripper to be 45 degrees slanted relative to the plate")
+composer("move to 10cm on top of the plate")
+composer("open gripper")
+composer("back to default pose")
+# done
+
+objects = ['trash can', 'napkin']
+# Query: sort napkin into trash can.
+napkin = parse_query_obj("napkin")
+trash_can = parse_query_obj("trash can")
+while np.linalg.norm(napkin.position - trash_can.position) > 0.1:
+    composer("grasp the napkin")
+    composer("move to 10cm on top of the trash can")
+    composer("open gripper")
+    composer("back to default pose")
+# done
diff --git a/Real/value_maps/real_get_affordance_map_prompt.py b/Real/value_maps/real_get_affordance_map_prompt.py
new file mode 100644
index 0000000000000000000000000000000000000000..1688cb926b7afbb3427992b13d28aebf213e92fe
--- /dev/null
+++ b/Real/value_maps/real_get_affordance_map_prompt.py
@@ -0,0 +1,146 @@
+import numpy as np
+from perception_utils import parse_query_obj
+from plan_utils import get_empty_affordance_map, set_voxel_by_radius, cm2index
+
+# Query: a point 10cm in front of [10, 15, 60].
+affordance_map = get_empty_affordance_map()
+# 10cm in front of so we add to x-axis
+x = 10 + cm2index(10, 'x')
+y = 15
+z = 60
+affordance_map[x, y, z] = 1
+ret_val = affordance_map
+
+# Query: a point on the right side of the table.
+affordance_map = get_empty_affordance_map()
+table = parse_query_obj('table')
+(min_x, min_y, min_z), (max_x, max_y, max_z) = table.aabb
+center_x, center_y, center_z = table.position
+# right side so y = max_y
+x = center_x
+y = max_y
+z = center_z
+affordance_map[x, y, z] = 1
+ret_val = affordance_map
+
+# Query: a point 20cm on top of the container.
+affordance_map = get_empty_affordance_map()
+container = parse_query_obj('container')
+(min_x, min_y, min_z), (max_x, max_y, max_z) = container.aabb
+center_x, center_y, center_z = container.position
+# 20cm on top of so we add to z-axis
+x = center_x
+y = center_y
+z = max_z + cm2index(20, 'z')
+affordance_map[x, y, z] = 1
+ret_val = affordance_map
+
+# Query: a point 1cm to the left of the brown block.
+affordance_map = get_empty_affordance_map()
+brown_block = parse_query_obj('brown block')
+(min_x, min_y, min_z), (max_x, max_y, max_z) = brown_block.aabb
+center_x, center_y, center_z = brown_block.position
+# 1cm to the left of so we subtract from y-axis
+x = center_x
+y = min_y - cm2index(1, 'y')
+z = center_z
+affordance_map[x, y, z] = 1
+ret_val = affordance_map
+
+# Query: anywhere within 20cm of the right most block.
+affordance_map = get_empty_affordance_map()
+right_most_block = parse_query_obj('the right most block')
+set_voxel_by_radius(affordance_map, right_most_block.position, radius_cm=20, value=1)
+
+# Query: a point on the back side of the table.
+affordance_map = get_empty_affordance_map()
+table = parse_query_obj('table')
+(min_x, min_y, min_z), (max_x, max_y, max_z) = table.aabb
+center_x, center_y, center_z = table.position
+# back side so x = min_x
+x = min_x
+y = center_y
+z = center_z
+affordance_map[x, y, z] = 1
+ret_val = affordance_map
+
+# Query: a point on the front right corner of the table.
+affordance_map = get_empty_affordance_map()
+table = parse_query_obj('table')
+(min_x, min_y, min_z), (max_x, max_y, max_z) = table.aabb
+center_x, center_y, center_z = table.position
+# front right corner so x = max_x and y = max_y
+x = max_x
+y = max_y
+z = center_z
+affordance_map[x, y, z] = 1
+ret_val = affordance_map
+
+# Query: a point 30cm into the topmost drawer handle.
+affordance_map = get_empty_affordance_map()
+top_handle = parse_query_obj('topmost drawer handle')
+# negative normal because we are moving into the handle.
+moving_dir = -top_handle.normal
+affordance_xyz = top_handle.position + cm2index(30, moving_dir)
+affordance_map[affordance_xyz[0], affordance_xyz[1], affordance_xyz[2]] = 1
+ret_val = affordance_map
+
+# Query: a point 5cm above the blue block.
+affordance_map = get_empty_affordance_map()
+blue_block = parse_query_obj('blue block')
+(min_x, min_y, min_z), (max_x, max_y, max_z) = blue_block.aabb
+center_x, center_y, center_z = blue_block.position
+# 5cm above so we add to z-axis
+x = center_x
+y = center_y
+z = max_z + cm2index(5, 'z')
+affordance_map[x, y, z] = 1
+ret_val = affordance_map
+
+# Query: a point 20cm away from the leftmost block.
+affordance_map = get_empty_affordance_map()
+leftmost_block = parse_query_obj('leftmost block')
+# positive normal because we are moving away from the block.
+moving_dir = leftmost_block.normal
+affordance_xyz = leftmost_block.position + cm2index(20, moving_dir)
+affordance_map[affordance_xyz[0], affordance_xyz[1], affordance_xyz[2]] = 1
+ret_val = affordance_map
+
+# Query: a point 4cm to the left of and 10cm on top of the tray that contains the lemon.
+affordance_map = get_empty_affordance_map()
+tray_with_lemon = parse_query_obj('tray that contains the lemon')
+(min_x, min_y, min_z), (max_x, max_y, max_z) = tray_with_lemon.aabb
+center_x, center_y, center_z = tray_with_lemon.position
+# 4cm to the left of so we subtract from y-axis, and 10cm on top of so we add to z-axis
+x = center_x
+y = min_y - cm2index(4, 'y')
+z = max_z + cm2index(10, 'z')
+affordance_map[x, y, z] = 1
+ret_val = affordance_map
+
+# Query: a point 10cm to the right of [45 49 66], and 5cm above it.
+affordance_map = get_empty_affordance_map()
+# 10cm to the right of so we add to y-axis, and 5cm above it so we add to z-axis
+x = 45
+y = 49 + cm2index(10, 'y')
+z = 66 + cm2index(5, 'z')
+affordance_map[x, y, z] = 1
+ret_val = affordance_map
+
+# Query: the blue circle.
+affordance_map = get_empty_affordance_map()
+blue_circle = parse_query_obj('blue circle')
+affordance_map = blue_circle.occupancy_map
+ret_val = affordance_map
+
+# Query: a point 10cm above and 5cm to the left of the yellow bowl.
+affordance_map = get_empty_affordance_map()
+yellow_bowl = parse_query_obj('yellow bowl')
+(min_x, min_y, min_z), (max_x, max_y, max_z) = yellow_bowl.aabb
+center_x, center_y, center_z = yellow_bowl.position
+# 10cm above so we add to z-axis, and 5cm to the left of so we subtract from y-axis
+x = center_x
+y = min_y - cm2index(5, 'y')
+z = max_z + cm2index(10, 'z')
+affordance_map[x, y, z] = 1
+ret_val = affordance_map
diff --git a/Real/value_maps/real_get_avoidance_map_prompt.py b/Real/value_maps/real_get_avoidance_map_prompt.py
new file mode 100644
index 0000000000000000000000000000000000000000..04aef56a51b4791bc26f9a89aaea6bd7e0681b1d
--- /dev/null
+++ b/Real/value_maps/real_get_avoidance_map_prompt.py
@@ -0,0 +1,36 @@
+import numpy as np
+from perception_utils import parse_query_obj
+from plan_utils import get_empty_avoidance_map, set_voxel_by_radius, cm2index
+
+# Query: 10cm from the bowl.
+avoidance_map = get_empty_avoidance_map()
+bowl = parse_query_obj('bowl')
+set_voxel_by_radius(avoidance_map, bowl.position, radius_cm=10, value=1)
+ret_val = avoidance_map
+
+# Query: 20cm near the mug.
+avoidance_map = get_empty_avoidance_map()
+mug = parse_query_obj('mug')
+set_voxel_by_radius(avoidance_map, mug.position, radius_cm=20, value=1)
+ret_val = avoidance_map
+
+# Query: 20cm around the mug and 10cm around the bowl.
+avoidance_map = get_empty_avoidance_map()
+mug = parse_query_obj('mug')
+set_voxel_by_radius(avoidance_map, mug.position, radius_cm=20, value=1)
+bowl = parse_query_obj('bowl')
+set_voxel_by_radius(avoidance_map, bowl.position, radius_cm=10, value=1)
+ret_val = avoidance_map
+
+# Query: 10cm from anything fragile.
+avoidance_map = get_empty_avoidance_map()
+fragile_objects = parse_query_obj('anything fragile')
+for obj in fragile_objects:
+    set_voxel_by_radius(avoidance_map, obj.position, radius_cm=10, value=1)
+ret_val = avoidance_map
+
+# Query: 10cm from the blue circle.
+avoidance_map = get_empty_avoidance_map()
+blue_circle = parse_query_obj('blue circle')
+set_voxel_by_radius(avoidance_map, blue_circle.position, radius_cm=10, value=1)
+ret_val = avoidance_map
\ No newline at end of file
diff --git a/Real/value_maps/real_get_gripper_map_prompt.py b/Real/value_maps/real_get_gripper_map_prompt.py
new file mode 100644
index 0000000000000000000000000000000000000000..fb5b25a9ef64dd7d042fd74ea1859823c916be3c
--- /dev/null
+++ b/Real/value_maps/real_get_gripper_map_prompt.py
@@ -0,0 +1,48 @@
+import numpy as np
+from perception_utils import parse_query_obj
+from plan_utils import get_empty_gripper_map, set_voxel_by_radius, cm2index
+
+# Query: open everywhere except 1cm around the green block.
+gripper_map = get_empty_gripper_map()
+# open everywhere
+gripper_map[:, :, :] = 0
+# close when 1cm around the green block
+green_block = parse_query_obj('green block')
+set_voxel_by_radius(gripper_map, green_block.position, radius_cm=1, value=1)
+ret_val = gripper_map
+
+# Query: close everywhere but open when on top of the back left corner of the table.
+gripper_map = get_empty_gripper_map()
+# close everywhere
+gripper_map[:, :, :] = 1
+# open when on top of the back left corner of the table
+table = parse_query_obj('table')
+(min_x, min_y, min_z), (max_x, max_y, max_z) = table.aabb
+center_x, center_y, center_z = table.position
+# back so x = min_x, left so y = min_y, top so we add to z
+x = min_x
+y = min_y
+z = max_z + cm2index(15, 'z')
+set_voxel_by_radius(gripper_map, (x, y, z), radius_cm=10, value=0)
+ret_val = gripper_map
+
+# Query: always open except when you are on the right side of the table.
+gripper_map = get_empty_gripper_map()
+# always open
+gripper_map[:, :, :] = 0
+# close when you are on the right side of the table
+table = parse_query_obj('table')
+center_x, center_y, center_z = table.position
+# right side so y is greater than center_y
+gripper_map[:, center_y:, :] = 1
+
+# Query: always close except when you are on the back side of the table.
+gripper_map = get_empty_gripper_map()
+# always close
+gripper_map[:, :, :] = 1
+# open when you are on the back side of the table
+table = parse_query_obj('table')
+center_x, center_y, center_z = table.position
+# back side so x is less than center_x
+gripper_map[:center_x, :, :] = 0
+ret_val = gripper_map
\ No newline at end of file
diff --git a/Real/value_maps/real_get_rotation_map_prompt.py b/Real/value_maps/real_get_rotation_map_prompt.py
new file mode 100644
index 0000000000000000000000000000000000000000..d8fc1f68721b543bbea75269fcec1ec8095c5a41
--- /dev/null
+++ b/Real/value_maps/real_get_rotation_map_prompt.py
@@ -0,0 +1,53 @@
+import numpy as np
+from plan_utils import get_empty_rotation_map, set_voxel_by_radius, cm2index, vec2quat
+from perception_utils import parse_query_obj
+from transforms3d.euler import euler2quat, quat2euler
+from transforms3d.quaternions import qmult, qinverse
+
+# Query: face the support surface of the bowl.
+rotation_map = get_empty_rotation_map()
+bowl = parse_query_obj('bowl')
+target_rotation = vec2quat(-bowl.normal)
+rotation_map[:, :, :] = target_rotation
+ret_val = rotation_map
+
+# Query: face the table when within 30cm from table center.
+rotation_map = get_empty_rotation_map()
+table = parse_query_obj('table')
+table_center = table.position
+target_rotation = vec2quat(-table.normal)
+set_voxel_by_radius(rotation_map, table_center, radius_cm=30, value=target_rotation)
+ret_val = rotation_map
+
+# Query: face the blue bowl.
+rotation_map = get_empty_rotation_map()
+blue_bowl = parse_query_obj('brown block')
+target_rotation = vec2quat(-blue_bowl.normal)
+rotation_map[:, :, :] = target_rotation
+ret_val = rotation_map
+
+# Query: turn clockwise by 45 degrees when at the center of the beer cap.
+rotation_map = get_empty_rotation_map()
+beer_cap = parse_query_obj('beer cap')
+(x, y, z) = beer_cap.position
+curr_rotation = rotation_map[x, y, z]
+rotation_delta = euler2quat(0, 0, np.pi / 4)
+rotation_map[x, y, z] = qmult(curr_rotation, rotation_delta)
+ret_val = rotation_map
+
+# Query: turn counter-clockwise by 30 degrees.
+rotation_map = get_empty_rotation_map()
+curr_rotation = rotation_map[0, 0, 0]
+rotation_delta = euler2quat(0, 0, -np.pi / 6)
+rotation_map[:, :, :] = qmult(curr_rotation, rotation_delta)
+ret_val = rotation_map
+
+# Query: rotate the gripper to be 45 degrees slanted relative to the plate.
+rotation_map = get_empty_rotation_map()
+plate = parse_query_obj('plate')
+face_plate_quat = vec2quat(-plate.normal)
+# rotate 45 degrees around the x-axis
+rotation_delta = euler2quat(-np.pi / 4, 0, 0)
+target_rotation = qmult(face_plate_quat, rotation_delta)
+rotation_map[:, :, :] = target_rotation
+ret_val = rotation_map
\ No newline at end of file
diff --git a/Real/value_maps/real_get_velocity_map_prompt.py b/Real/value_maps/real_get_velocity_map_prompt.py
new file mode 100644
index 0000000000000000000000000000000000000000..d414e030fb184aef4fe76bf4f08a0fa7154988d9
--- /dev/null
+++ b/Real/value_maps/real_get_velocity_map_prompt.py
@@ -0,0 +1,31 @@
+import numpy as np
+from plan_utils import get_empty_velocity_map, set_voxel_by_radius, cm2index
+from perception_utils import parse_query_obj
+
+# Query: faster when on the right side of the table and slower when on the left side of the table.
+velocity_map = get_empty_velocity_map()
+table = parse_query_obj('table')
+center_x, center_y, center_z = table.position
+# faster on right side so 1.5 when y > center_y, slower on left side so 0.5 when y < center_y
+velocity_map[:, center_y:, :] = 1.5
+velocity_map[:, :center_y, :] = 0.5
+ret_val = velocity_map
+
+# Query: slow down by a quarter.
+velocity_map = get_empty_velocity_map()
+velocity_map[:] = 0.75
+ret_val = velocity_map
+
+# Query: slow down by a half when you're near anything fragile (objects: ['block', 'fork', 'mug', 'bowl', 'chips']).
+velocity_map = get_empty_velocity_map()
+mug = parse_query_obj('mug')
+set_voxel_by_radius(velocity_map, mug.position, radius_cm=10, value=0.5)
+bowl = parse_query_obj('bowl')
+set_voxel_by_radius(velocity_map, bowl.position, radius_cm=10, value=0.5)
+ret_val = velocity_map
+
+# Query: quarter of the speed when within 9cm from the yellow line.
+velocity_map = get_empty_velocity_map()
+yellow_line = parse_query_obj('yellow_line')
+set_voxel_by_radius(velocity_map, yellow_line.position, radius_cm=9, value=0.25)
+ret_val = velocity_map
\ No newline at end of file
diff --git a/Sim/composer/sim_composer_prompt.py b/Sim/composer/sim_composer_prompt.py
new file mode 100644
index 0000000000000000000000000000000000000000..eea7bb637cb70930215d09e77ca87f11f82d9b5f
--- /dev/null
+++ b/Sim/composer/sim_composer_prompt.py
@@ -0,0 +1,91 @@
+import numpy as np
+from env_utils import execute, reset_to_default_pose
+from perception_utils import parse_query_obj
+from plan_utils import get_affordance_map, get_avoidance_map, get_velocity_map, get_rotation_map, get_gripper_map
+
+# Query: move ee forward for 7cm.
+movable = parse_query_obj('ee')
+affordance_map = get_affordance_map(f'a point 7cm in front of {movable.position}')
+execute(movable, affordance_map)
+
+# Query: move to the left of the green block.
+movable = parse_query_obj('ee')
+affordance_map = get_affordance_map('a point 11cm to the left of the green block')
+execute(movable, affordance_map=affordance_map)
+
+# Query: move to the yellow block while staying on the front side the pink block.
+movable = parse_query_obj('ee')
+affordance_map = get_affordance_map('a point 11cm on top of the yellow block')
+avoidance_map = get_avoidance_map('anywhere behind the pink block')
+execute(movable, affordance_map=affordance_map, avoidance_map=avoidance_map)
+
+# Query: move to the back side of the table.
+movable = parse_query_obj('ee')
+affordance_map = get_affordance_map('a point on the back side of the table')
+execute(movable, affordance_map=affordance_map, avoidance_map=avoidance_map)
+
+# Query: move to the front right corner of the table while moving at faster speed when within 7cm from the yellow block.
+movable = parse_query_obj('ee')
+affordance_map = get_affordance_map('a point on the front right corner of the table')
+velocity_map = get_velocity_map('faster speed when within 7cm from the yellow block')
+execute(movable, affordance_map=affordance_map, velocity_map=velocity_map)
+
+# Query: move to 15cm on top of the rightmost block while avoiding the blue line and moving at a quarter of the speed on the right side of the table.
+movable = parse_query_obj('ee')
+affordance_map = get_affordance_map('a point 15cm on top of the rightmost block')
+avoidance_map = get_avoidance_map('the blue line')
+velocity_map = get_velocity_map('a quarter of the speed on the right side of the table')
+execute(movable, affordance_map=affordance_map, avoidance_map=avoidance_map, velocity_map=velocity_map)
+
+# Query: close the topmost drawer by pushing.
+movable = parse_query_obj('topmost drawer handle')
+affordance_map = get_affordance_map('a point 21cm into the topmost drawer handle')
+execute(movable, affordance_map=affordance_map)
+
+# Query: push the second to the left block along the brown line.
+movable = parse_query_obj('second to the left block')
+affordance_map = get_affordance_map('the brown line')
+execute(movable, affordance_map=affordance_map)
+
+# Query: grasp the blue block from the table at a quarter of the speed.
+movable = parse_query_obj('ee')
+affordance_map = get_affordance_map('a point at the center of blue block')
+rotation_map = get_rotation_map('face the blue block')
+velocity_map = get_velocity_map('quarter of the speed')
+gripper_map = get_gripper_map('open everywhere except 1cm around the blue block')
+execute(movable, affordance_map=affordance_map, rotation_map=rotation_map, velocity_map=velocity_map, gripper_map=gripper_map)
+
+# Query: move to the left of the brown block.
+movable = parse_query_obj('ee')
+affordance_map = get_affordance_map('a point 11cm to the left of the brown block')
+execute(movable, affordance_map=affordance_map)
+
+# Query: back to default pose.
+reset_to_default_pose()
+
+# Query: open gripper.
+movable = parse_query_obj('gripper')
+gripper_map = get_gripper_map('open everywhere')
+execute(movable, gripper_map=gripper_map)
+
+# Query: drop the blue block to the right side of the table while staying at least 7cm away from the brown block.
+movable = parse_query_obj('ee')
+affordance_map = get_affordance_map('a point on the right side of the table')
+avoidance_map = get_avoidance_map('7cm away from the brown block')
+gripper_map = get_gripper_map('close everywhere except 1cm around the right side of the table')
+execute(movable, affordance_map=affordance_map, avoidance_map=avoidance_map, gripper_map=gripper_map)
+
+# Query: move to the front side of the yellow block.
+movable = parse_query_obj('ee')
+affordance_map = get_affordance_map('a point 11cm in front of the yellow block')
+execute(movable, affordance_map=affordance_map)
+
+# Query: move to the back side of the table.
+movable = parse_query_obj('ee')
+affordance_map = get_affordance_map('a point on the back side of the table')
+execute(movable, affordance_map=affordance_map)
+
+# Query: stay away from the brown block.
+movable = parse_query_obj('ee')
+avoidance_map = get_avoidance_map('3cm away from the brown block')
+execute(movable, avoidance_map=avoidance_map)
\ No newline at end of file
diff --git a/Sim/perception/sim_parse_query_obj_prompt.py b/Sim/perception/sim_parse_query_obj_prompt.py
new file mode 100644
index 0000000000000000000000000000000000000000..0e2220a0328d49d219890f72dde68d8520199ef9
--- /dev/null
+++ b/Sim/perception/sim_parse_query_obj_prompt.py
@@ -0,0 +1,71 @@
+import numpy as np
+from perception_utils import detect
+
+objects = ['green block', 'yellow line']
+# Query: ee.
+ee = detect('ee')[0]
+ret_val = ee
+
+objects = ['drawer', 'blue block', 'yellow block']
+# Query: topmost handle.
+handles = detect('drawer handle')
+# topmost so sort by z, take the last one
+handles = sorted(handles, key=lambda x: x.position[2])
+top_handle = handles[-1]
+ret_val = top_handle
+
+objects = ['yellow block', 'pink block', 'brown line', 'pink block']
+# Query: second to the left block.
+blocks = detect('block')
+# second to the left so sort by y, take the second one
+blocks = sorted(blocks, key=lambda x: x.position[1])
+second_left_block = blocks[1]
+ret_val = second_left_block
+
+objects = ['blue line', 'pink line', 'green block']
+# Query: table.
+table = detect('table')[0]
+ret_val = table
+
+objects = ['green line', 'drawer', 'yellow block']
+# Query: second to the bottom handle.
+handles = detect('drawer handle')
+# second to the bottom so sort by z, take the second one
+handles = sorted(handles, key=lambda x: x.position[2])
+second_bottom_handle = handles[1]
+ret_val = second_bottom_handle
+
+objects = ['brown line', 'brown block']
+# Query: brown line.
+brown_line = detect('brown line')[0]
+ret_val = brown_line
+
+objects = ['green block', 'brown block', 'yellow line']
+# Query: block.
+block = detect('green block')[0]
+ret_val = block
+
+objects = ['pink block', 'pink line', 'blue block']
+# Query: block closest to the pink line.
+blocks = detect('block')
+pink_line = detect('pink line')[0]
+closest_block = min(blocks, key=lambda x: np.linalg.norm(x.position - pink_line.position))
+ret_val = closest_block
+
+objects = ['blue block', 'blue line', 'green block', 'pink block', 'brown block']
+# Query: the block that is on top of the blue block.
+blocks = detect('block')
+blue_block = detect('blue block')[0]
+# find the block that is on top of the blue block
+for block in blocks:
+    if block.position[2] > blue_block.position[2]:
+        ret_val = block
+        break
+
+objects = ['drawer']
+# Query: top drawer handle.
+handles = detect('drawer handle')
+# top drawer handle so sort by z, take the last one
+handles = sorted(handles, key=lambda x: x.position[2])
+top_handle = handles[-1]
+ret_val = top_handle
\ No newline at end of file
diff --git a/Sim/value_maps/sim_get_affordance_map_prompt.py b/Sim/value_maps/sim_get_affordance_map_prompt.py
new file mode 100644
index 0000000000000000000000000000000000000000..cfaf0331ab1df2e20d73e241ba018551028d0f32
--- /dev/null
+++ b/Sim/value_maps/sim_get_affordance_map_prompt.py
@@ -0,0 +1,177 @@
+import numpy as np
+from perception_utils import parse_query_obj
+from plan_utils import get_empty_affordance_map, set_voxel_by_radius, cm2index
+
+# Query: a point 10cm in front of [10, 15, 60].
+affordance_map = get_empty_affordance_map()
+# in front so we add to x
+x = 10 + cm2index(10, 'x')
+y = 15
+z = 60
+affordance_map[x, y, z] = 1
+ret_val = affordance_map
+
+# Query: a point on the back side of the table.
+affordance_map = get_empty_affordance_map()
+table = parse_query_obj('table')
+(min_x, min_y, min_z), (max_x, max_y, max_z) = table.aabb
+center_x, center_y, center_z = table.position
+# back side so x = min_x
+x = min_x
+y = center_y
+z = center_z
+affordance_map[x, y, z] = 1
+ret_val = affordance_map
+
+# Query: a point 21cm on top of the green block.
+affordance_map = get_empty_affordance_map()
+green_block = parse_query_obj('green block')
+(min_x, min_y, min_z), (max_x, max_y, max_z) = green_block.aabb
+center_x, center_y, center_z = green_block.position
+# 21cm on top so z = max_z + 21cm
+x = center_x
+y = center_y
+z = max_z + cm2index(21, 'z')
+affordance_map[x, y, z] = 1
+ret_val = affordance_map
+
+# Query: a point 1cm to the left of the brown block.
+affordance_map = get_empty_affordance_map()
+brown_block = parse_query_obj('brown block')
+(min_x, min_y, min_z), (max_x, max_y, max_z) = brown_block.aabb
+center_x, center_y, center_z = brown_block.position
+# 1cm to the left so y = min_y - 1cm
+x = center_x
+y = min_y - cm2index(1, 'y')
+z = center_z
+affordance_map[x, y, z] = 1
+ret_val = affordance_map
+
+# Query: anywhere within 15cm of the right most block.
+affordance_map = get_empty_affordance_map()
+right_most_block = parse_query_obj('the right most block')
+set_voxel_by_radius(affordance_map, right_most_block.position, radius_cm=15, value=1)
+ret_val = affordance_map
+
+# Query: a point on the back side of the table.
+affordance_map = get_empty_affordance_map()
+table = parse_query_obj('table')
+(min_x, min_y, min_z), (max_x, max_y, max_z) = table.aabb
+center_x, center_y, center_z = table.position
+# back side so x = min_x
+x = min_x
+y = center_y
+z = center_z
+affordance_map[x, y, z] = 1
+ret_val = affordance_map
+
+# Query: a point on the front right corner of the table.
+affordance_map = get_empty_affordance_map()
+table = parse_query_obj('table')
+(min_x, min_y, min_z), (max_x, max_y, max_z) = table.aabb
+center_x, center_y, center_z = table.position
+# front right corner -> front so x = max_x, right so y = max_y
+x = max_x
+y = max_y
+z = center_z
+affordance_map[x, y, z] = 1
+ret_val = affordance_map
+
+# Query: a point 30cm into the topmost drawer handle.
+affordance_map = get_empty_affordance_map()
+top_handle = parse_query_obj('topmost drawer handle')
+# negative normal because we are moving into the handle.
+moving_dir = -top_handle.normal
+affordance_xyz = top_handle.position + cm2index(30, moving_dir)
+affordance_map[affordance_xyz[0], affordance_xyz[1], affordance_xyz[2]] = 1
+ret_val = affordance_map
+
+# Query: a point 9cm to the left of the blue block.
+affordance_map = get_empty_affordance_map()
+blue_block = parse_query_obj('blue block')
+(min_x, min_y, min_z), (max_x, max_y, max_z) = blue_block.aabb
+center_x, center_y, center_z = blue_block.position
+# 9cm to the left so y = min_y - 9cm
+x = center_x
+y = min_y - cm2index(9, 'y')
+z = center_z
+affordance_map[x, y, z] = 1
+ret_val = affordance_map
+
+# Query: a point 21cm away from the leftmost block.
+affordance_map = get_empty_affordance_map()
+leftmost_block = parse_query_obj('leftmost block')
+# positive normal because we are moving away from the block.
+moving_dir = leftmost_block.normal
+affordance_xyz = leftmost_block.position + cm2index(21, moving_dir)
+affordance_map[affordance_xyz[0], affordance_xyz[1], affordance_xyz[2]] = 1
+ret_val = affordance_map
+
+# Query: a point 7cm above the tray that contains the lemon.
+affordance_map = get_empty_affordance_map()
+tray_with_lemon = parse_query_obj('tray that contains the lemon')
+(min_x, min_y, min_z), (max_x, max_y, max_z) = tray_with_lemon.aabb
+center_x, center_y, center_z = tray_with_lemon.position
+# 7cm above so z = max_z + 7cm
+x = center_x
+y = center_y
+z = max_z + cm2index(7, 'z')
+affordance_map[x, y, z] = 1
+ret_val = affordance_map
+
+# Query: a point 3cm to the left of [45, 49, 99].
+affordance_map = get_empty_affordance_map()
+# left so y = 49 - 3cm
+x = 45
+y = 49 - cm2index(3, 'y')
+z = 99
+affordance_map[x, y, z] = 1
+ret_val = affordance_map
+
+# Query: the brown line.
+brown_line = parse_query_obj('brown line')
+affordance_map = brown_line.occupancy_map
+ret_val = affordance_map
+
+# Query: a point 13cm to the left of the gripper.
+affordance_map = get_empty_affordance_map()
+gripper = parse_query_obj('gripper')
+(min_x, min_y, min_z), (max_x, max_y, max_z) = gripper.aabb
+center_x, center_y, center_z = gripper.position
+# 13cm to the left so y = min_y - 13cm
+x = center_x
+y = min_y - cm2index(13, 'y')
+z = center_z
+affordance_map[x, y, z] = 1
+ret_val = affordance_map
+
+# Query: a point 17cm above the back left corner of the table.
+affordance_map = get_empty_affordance_map()
+table = parse_query_obj('table')
+(min_x, min_y, min_z), (max_x, max_y, max_z) = table.aabb
+center_x, center_y, center_z = table.position
+# 17cm on top of back left corner so x = min_x, y = min_y, z = max_z + 17cm
+x = min_x
+y = min_y
+z = max_z + cm2index(17, 'z')
+affordance_map[x, y, z] = 1
+ret_val = affordance_map
+
+# Query: a point at the center of the green block.
+affordance_map = get_empty_affordance_map()
+green_block = parse_query_obj('green block')
+center_x, center_y, center_z = green_block.position
+affordance_map[center_x, center_y, center_z] = 1
+ret_val = affordance_map
+
+# Query: the right side of the table.
+affordance_map = get_empty_affordance_map()
+table = parse_query_obj('table')
+(min_x, min_y, min_z), (max_x, max_y, max_z) = table.aabb
+center_x, center_y, center_z = table.position
+# right side so y = max_y
+x = center_x
+y = max_y
+z = center_z
+affordance_map[x, y, z] = 1
+ret_val = affordance_map
diff --git a/Sim/value_maps/sim_get_avoidance_map_prompt.py b/Sim/value_maps/sim_get_avoidance_map_prompt.py
new file mode 100644
index 0000000000000000000000000000000000000000..05f64b1df547bed9d0abc90e2a4d298b5757bc6f
--- /dev/null
+++ b/Sim/value_maps/sim_get_avoidance_map_prompt.py
@@ -0,0 +1,60 @@
+import numpy as np
+from perception_utils import parse_query_obj
+from plan_utils import get_empty_avoidance_map, set_voxel_by_radius, cm2index
+
+# Query: 11cm from the blue block.
+avoidance_map = get_empty_avoidance_map()
+blue_block = parse_query_obj('blue block')
+set_voxel_by_radius(avoidance_map, blue_block.position, radius_cm=11, value=1)
+ret_val = avoidance_map
+
+# Query: 7cm near the pink block.
+avoidance_map = get_empty_avoidance_map()
+pink_block = parse_query_obj('pink block')
+set_voxel_by_radius(avoidance_map, pink_block.position, radius_cm=7, value=1)
+ret_val = avoidance_map
+
+# Query: 13cm around the brown block and 5cm around the green block.
+avoidance_map = get_empty_avoidance_map()
+brown_block = parse_query_obj('brown block')
+set_voxel_by_radius(avoidance_map, brown_block.position, radius_cm=13, value=1)
+green_block = parse_query_obj('green block')
+set_voxel_by_radius(avoidance_map, green_block.position, radius_cm=5, value=1)
+ret_val = avoidance_map
+
+# Query: the blue line.
+blue_line = parse_query_obj('blue_line')
+avoidance_map = blue_line.occupancy_map
+ret_val = avoidance_map
+
+# Query: anywhere on the front side of the blue block.
+avoidance_map = get_empty_avoidance_map()
+blue_block = parse_query_obj('blue block')
+center_x, center_y, center_z = blue_block.position
+# front side so x > center_x
+avoidance_map[center_x:, :, :] = 1
+ret_val = avoidance_map
+
+# Query: anywhere on the left of the green block.
+avoidance_map = get_empty_avoidance_map()
+green_block = parse_query_obj('green block')
+center_x, center_y, center_z = green_block.position
+# left side so y < center_y
+avoidance_map[:, :center_y, :] = 1
+ret_val = avoidance_map
+
+# Query: anywhere behind the pink block.
+avoidance_map = get_empty_avoidance_map()
+pink_block = parse_query_obj('pink block')
+center_x, center_y, center_z = pink_block.position
+# behind so x < center_x
+avoidance_map[:center_x, :, :] = 1
+ret_val = avoidance_map
+
+# Query: anywhere above the brown block.
+avoidance_map = get_empty_avoidance_map()
+brown_block = parse_query_obj('brown block')
+center_x, center_y, center_z = brown_block.position
+# above so z > center_z
+avoidance_map[:, :, center_z:] = 1
+ret_val = avoidance_map
\ No newline at end of file
diff --git a/Sim/value_maps/sim_get_gripper_map_prompt.py b/Sim/value_maps/sim_get_gripper_map_prompt.py
new file mode 100644
index 0000000000000000000000000000000000000000..4f7145d01cf30a2e92d9a0fbedd73b41eb21cb31
--- /dev/null
+++ b/Sim/value_maps/sim_get_gripper_map_prompt.py
@@ -0,0 +1,57 @@
+import numpy as np
+from perception_utils import parse_query_obj
+from plan_utils import get_empty_gripper_map, set_voxel_by_radius, cm2index
+
+# Query: open everywhere except 1cm around the green block.
+gripper_map = get_empty_gripper_map()
+# open everywhere
+gripper_map[:, :, :] = 0
+# close when 1cm around the green block
+green_block = parse_query_obj('green block')
+set_voxel_by_radius(gripper_map, green_block.position, radius_cm=1, value=1)
+ret_val = gripper_map
+
+# Query: close everywhere but open when on top of the back left corner of the table.
+gripper_map = get_empty_gripper_map()
+# close everywhere
+gripper_map[:, :, :] = 1
+# open when on top of the back left corner of the table
+table = parse_query_obj('table')
+(min_x, min_y, min_z), (max_x, max_y, max_z) = table.aabb
+center_x, center_y, center_z = table.position
+# back so x = min_x, left so y = min_y, top so we add to z
+x = min_x
+y = min_y
+z = max_z + cm2index(10, 'z')
+set_voxel_by_radius(gripper_map, (x, y, z), radius_cm=10, value=0)
+ret_val = gripper_map
+
+# Query: always open except when you are on the right side of the table.
+gripper_map = get_empty_gripper_map()
+# always open
+gripper_map[:, :, :] = 0
+# close when you are on the right side of the table
+table = parse_query_obj('table')
+center_x, center_y, center_z = table.position
+# right side so y is greater than center_y
+gripper_map[:, center_y:, :] = 1
+
+# Query: always close except when you are on the back side of the table.
+gripper_map = get_empty_gripper_map()
+# always close
+gripper_map[:, :, :] = 1
+# open when you are on the back side of the table
+table = parse_query_obj('table')
+center_x, center_y, center_z = table.position
+# back side so x is less than center_x
+gripper_map[:center_x, :, :] = 0
+ret_val = gripper_map
+
+# Query: open everywhere except 1cm around the topmost drawer handle.
+gripper_map = get_empty_gripper_map()
+# open everywhere
+gripper_map[:, :, :] = 0
+# close when 1cm around the topmost drawer handle
+topmost_drawer_handle = parse_query_obj('topmost drawer handle')
+set_voxel_by_radius(gripper_map, topmost_drawer_handle.position, radius_cm=1, value=1)
+ret_val = gripper_map
\ No newline at end of file
diff --git a/Sim/value_maps/sim_get_rotation_map_prompt.py b/Sim/value_maps/sim_get_rotation_map_prompt.py
new file mode 100644
index 0000000000000000000000000000000000000000..ca66bfd81cfe101ce1e5d4515dff921cc2eac7ca
--- /dev/null
+++ b/Sim/value_maps/sim_get_rotation_map_prompt.py
@@ -0,0 +1,25 @@
+import numpy as np
+from plan_utils import get_empty_rotation_map, set_voxel_by_radius, cm2index, vec2quat
+from perception_utils import parse_query_obj
+
+# Query: face the green block.
+rotation_map = get_empty_rotation_map()
+green_block = parse_query_obj('green block')
+target_rotation = vec2quat(-green_block.normal)
+rotation_map[:, :, :] = target_rotation
+ret_val = rotation_map
+
+# Query: face the table when within 30cm from table center.
+rotation_map = get_empty_rotation_map()
+table = parse_query_obj('table')
+table_center = table.position
+target_rotation = vec2quat(-table.normal)
+set_voxel_by_radius(rotation_map, table_center, radius_cm=30, value=target_rotation)
+ret_val = rotation_map
+
+# Query: face the topmost drawer handle.
+rotation_map = get_empty_rotation_map()
+topmost_drawer_handle = parse_query_obj('topmost drawer handle')
+target_rotation = vec2quat(-topmost_drawer_handle.normal)
+rotation_map[:, :, :] = target_rotation
+ret_val = rotation_map
\ No newline at end of file
diff --git a/Sim/value_maps/sim_get_velocity_map_prompt.py b/Sim/value_maps/sim_get_velocity_map_prompt.py
new file mode 100644
index 0000000000000000000000000000000000000000..0be25dc15254729105749b6b298b9ca471a86255
--- /dev/null
+++ b/Sim/value_maps/sim_get_velocity_map_prompt.py
@@ -0,0 +1,46 @@
+import numpy as np
+from plan_utils import get_empty_velocity_map, set_voxel_by_radius, cm2index
+from perception_utils import parse_query_obj
+
+# Query: faster when on the left side of the table and a quarter of the speed when on the other side of the table.
+velocity_map = get_empty_velocity_map()
+table = parse_query_obj('table')
+center_x, center_y, center_z = table.position
+# faster on left side so y < center_y
+velocity_map[:, :center_y, :] = 1.5
+# a quarter of the speed on the other side so y > center_y
+velocity_map[:, center_y:, :] = 0.25
+ret_val = velocity_map
+
+# Query: slow down by a quarter.
+velocity_map = get_empty_velocity_map()
+velocity_map[:] = 0.75
+ret_val = velocity_map
+
+# Query: quarter of the speed when within 9cm from the yellow block.
+velocity_map = get_empty_velocity_map()
+yellow_block = parse_query_obj('yellow block')
+set_voxel_by_radius(velocity_map, yellow_block.position, radius_cm=9, value=0.25)
+ret_val = velocity_map
+
+# Query: quarter of the speed in the back side of the table.
+velocity_map = get_empty_velocity_map()
+table = parse_query_obj('table')
+center_x, center_y, center_z = table.position
+# a quarter of the speed in the back side so x < center_x
+velocity_map[:center_x, :, :] = 0.25
+ret_val = velocity_map
+
+# Query: faster speed in the right side of the table.
+velocity_map = get_empty_velocity_map()
+table = parse_query_obj('table')
+center_x, center_y, center_z = table.position
+# faster in the right side so y > center_y
+velocity_map[:, center_y:, :] = 1.5
+ret_val = velocity_map
+
+# Query: quarter of the speed when within 11cm from the brown block.
+velocity_map = get_empty_velocity_map()
+brown_block = parse_query_obj('brown_block')
+set_voxel_by_radius(velocity_map, brown_block.position, radius_cm=11, value=0.25)
+ret_val = velocity_map
\ No newline at end of file
diff --git a/VoxPoserApiExamples/Real/composer/real_composer_prompt.py b/VoxPoserApiExamples/Real/composer/real_composer_prompt.py
new file mode 100644
index 0000000000000000000000000000000000000000..36e1706e24f4521c0732d7da544dbc9d014897cc
--- /dev/null
+++ b/VoxPoserApiExamples/Real/composer/real_composer_prompt.py
@@ -0,0 +1,120 @@
+import numpy as np
+from env_utils import execute, reset_to_default_pose
+from perception_utils import parse_query_obj
+from plan_utils import get_affordance_map, get_avoidance_map, get_velocity_map, get_rotation_map, get_gripper_map
+
+# Query: move ee forward for 10cm.
+movable = parse_query_obj('gripper')
+affordance_map = get_affordance_map(f'a point 10cm in front of {movable.position}')
+execute(movable, affordance_map)
+
+# Query: go back to default.
+reset_to_default_pose()
+
+# Query: move the gripper behind the bowl, and slow down when near the bowl.
+movable = parse_query_obj('gripper')
+affordance_map = get_affordance_map('a point 15cm behind the bowl')
+avoidance_map = get_avoidance_map('10cm near the bowl')
+velocity_map = get_velocity_map('slow down when near the bowl')
+execute(movable, affordance_map=affordance_map, avoidance_map=avoidance_map, velocity_map=velocity_map)
+
+# Query: move to the back side of the table while staying at least 5cm from the blue block.
+movable = parse_query_obj('gripper')
+affordance_map = get_affordance_map('a point on the back side of the table')
+avoidance_map = get_avoidance_map('5cm from the blue block')
+execute(movable, affordance_map=affordance_map, avoidance_map=avoidance_map)
+
+# Query: move to the top of the plate and face the plate.
+movable = parse_query_obj('gripper')
+affordance_map = get_affordance_map('a point 10cm above the plate')
+rotation_map = get_rotation_map('face the plate')
+execute(movable, affordance_map=affordance_map, rotation_map=rotation_map)
+
+# Query: drop the toy inside container.
+movable = parse_query_obj('gripper')
+affordance_map = get_affordance_map('a point 15cm above the container')
+gripper_map = get_gripper_map('close everywhere but open when on top of the container')
+execute(movable, affordance_map=affordance_map, gripper_map=gripper_map)
+
+# Query: push close the topmost drawer.
+movable = parse_query_obj('topmost drawer handle')
+affordance_map = get_affordance_map('a point 30cm into the topmost drawer handle')
+execute(movable, affordance_map=affordance_map)
+
+# Query: push the second to the left block along the red line.
+movable = parse_query_obj('second to the left block')
+affordance_map = get_affordance_map('the red line')
+execute(movable, affordance_map=affordance_map)
+
+# Query: grasp the blue block from the table at a quarter of the speed.
+movable = parse_query_obj('gripper')
+affordance_map = get_affordance_map('a point at the center of blue block')
+velocity_map = get_velocity_map('quarter of the speed')
+gripper_map = get_gripper_map('open everywhere except 1cm around the blue block')
+execute(movable, affordance_map=affordance_map, velocity_map=velocity_map, gripper_map=gripper_map)
+
+# Query: move to the left of the brown block.
+movable = parse_query_obj('gripper')
+affordance_map = get_affordance_map('a point 10cm to the left of the brown block')
+execute(movable, affordance_map=affordance_map)
+
+# Query: move to the top of the tray that contains the lemon.
+movable = parse_query_obj('gripper')
+affordance_map = get_affordance_map('a point 10cm above the tray that contains the lemon')
+execute(movable, affordance_map=affordance_map)
+
+# Query: close drawer by 5cm.
+movable = parse_query_obj('drawer handle')
+affordance_map = get_affordance_map('a point 5cm into the drawer handle')
+execute(movable, affordance_map=affordance_map)
+
+# Query: move to 5cm on top of the soda can, at 0.5x speed when within 20cm of the wooden mug, and keep at least 15cm away from the wooden mug.
+movable = parse_query_obj('gripper')
+affordance_map = get_affordance_map('a point 5cm above the soda can')
+avoidance_map = get_avoidance_map('15cm from the wooden mug')
+velocity_map = get_velocity_map('0.5x speed when within 20cm of the wooden mug')
+execute(movable, affordance_map=affordance_map, avoidance_map=avoidance_map, velocity_map=velocity_map)
+
+# Query: wipe the red dot but avoid the blue block.
+movable = parse_query_obj('gripper')
+affordance_map = get_affordance_map('the red dot')
+avoidance_map = get_avoidance_map('10cm from the blue block')
+execute(movable, affordance_map=affordance_map, avoidance_map=avoidance_map)
+
+# Query: grasp the mug from the shelf.
+movable = parse_query_obj('gripper')
+affordance_map = get_affordance_map('a point at the center of the mug handle')
+gripper_map = get_gripper_map('open everywhere except 1cm around the mug handle')
+execute(movable, affordance_map=affordance_map, gripper_map=gripper_map)
+
+# Query: move to 10cm on top of the soup bowl, and 5cm to the left of the soup bowl, while away from the glass, at 0.75x speed.
+movable = parse_query_obj('gripper')
+affordance_map = get_affordance_map('a point 10cm above and 5cm to the left of the soup bowl')
+avoidance_map = get_avoidance_map('10cm from the glass')
+velocity_map = get_velocity_map('0.75x speed')
+execute(movable, affordance_map=affordance_map, avoidance_map=avoidance_map, velocity_map=velocity_map)
+
+# Query: open gripper.
+movable = parse_query_obj('gripper')
+gripper_map = get_gripper_map('open everywhere')
+execute(movable, gripper_map=gripper_map)
+
+# Query: turn counter-clockwise by 180 degrees.
+movable = parse_query_obj('gripper')
+rotation_map = get_rotation_map('turn counter-clockwise by 180 degrees')
+execute(movable, rotation_map=rotation_map)
+
+# Query: sweep all particles to the left side of the table.
+particles = parse_query_obj('particles')
+for particle in particles:
+    movable = particle
+    affordance_map = get_affordance_map('a point on the left side of the table')
+    execute(particle, affordance_map=affordance_map)
+
+# Query: grasp the bottom drawer handle while moving at 0.5x speed.
+movable = parse_query_obj('gripper')
+affordance_map = get_affordance_map('a point at the center of the bottom drawer handle')
+velocity_map = get_velocity_map('0.5x speed')
+rotation_map = get_rotation_map('face the bottom drawer handle')
+gripper_map = get_gripper_map('open everywhere except 1cm around the bottom drawer handle')
+execute(movable, affordance_map=affordance_map, velocity_map=velocity_map, rotation_map=rotation_map, gripper_map=gripper_map)
\ No newline at end of file
diff --git a/VoxPoserApiExamples/Real/perception/real_parse_query_obj_prompt.py b/VoxPoserApiExamples/Real/perception/real_parse_query_obj_prompt.py
new file mode 100644
index 0000000000000000000000000000000000000000..996b45abf82cedecabaede1f248e355563412c78
--- /dev/null
+++ b/VoxPoserApiExamples/Real/perception/real_parse_query_obj_prompt.py
@@ -0,0 +1,104 @@
+import numpy as np
+from perception_utils import detect
+
+objects = ['table', 'gripper', 'green block', 'cardboard box']
+# Query: gripper.
+gripper = detect('gripper')[0]
+ret_val = gripper
+
+objects = ['table', 'gripper', 'drawer', 'egg', 'egg', 'plate']
+# Query: topmost handle.
+handles = detect('drawer handle')
+handles = sorted(handles, key=lambda x: x.position[2])
+top_handle = handles[-1]
+ret_val = top_handle
+
+objects = ['table', 'gripper', 'yellow block', 'charging cable', 'cyan block', 'magenta block']
+# Query: second to the left block.
+blocks = detect('block')
+blocks = sorted(blocks, key=lambda x: x.position[1])
+second_left_block = blocks[1]
+ret_val = second_left_block
+
+objects = ['table', 'gripper', 'iPhone', 'ruler', 'pink line', 'blue line']
+# Query: the front most line on the table.
+lines = detect('line')
+lines = sorted(lines, key=lambda x: x.position[0])
+front_most_line = lines[-1]
+ret_val = front_most_line
+
+objects = ['table', 'gripper', 'vase', 'napkin box', 'mask']
+# Query: table.
+table = detect('table')[0]
+ret_val = table
+
+objects = ['table', 'gripper', 'bottle', 'drawer', 'bowl', 'bag']
+# Query: second to the bottom handle.
+handles = detect('drawer handle')
+handles = sorted(handles, key=lambda x: x.position[2])
+second_bottom_handle = handles[1]
+ret_val = second_bottom_handle
+
+objects = ['table', 'gripper', 'brown line', 'red block', 'monitor']
+# Query: brown line.
+brown_line = detect('brown line')[0]
+ret_val = brown_line
+
+objects = ['table', 'gripper', 'green block', 'cup holder', 'black block']
+# Query: block.
+block = detect('green block')[0]
+ret_val = block
+
+objects = ['table', 'gripper', 'mouse', 'yellow bowl', 'brown bowl', 'sticker']
+# Query: bowl closest to the sticker.
+bowls = detect('bowl')
+sticker = detect('sticker')[0]
+closest_bowl = min(bowls, key=lambda x: np.linalg.norm(x.position - sticker.position))
+ret_val = closest_bowl
+
+objects = ['table', 'gripper', 'keyboard', 'brown bag', 'pink bag', 'red tape', 'bottle']
+# Query: bag with the red tape on top.
+bags = detect('bag')
+red_tape = detect('red tape')[0]
+bag_with_red_tape = min(bags, key=lambda x: np.linalg.norm(x.position - red_tape.position))
+ret_val = bag_with_red_tape
+
+objects = ['table', 'gripper', 'grape', 'wood tray', 'strawberry', 'white tray', 'blue tray', 'bread']
+# Query: tray that contains the bread.
+trays = detect('tray')
+bread = detect('bread')[0]
+tray_with_bread = min(trays, key=lambda x: np.linalg.norm(x.position - bread.position))
+ret_val = tray_with_bread
+
+objects = ['table', 'gripper', 'drawer']
+# Query: top drawer handle.
+handles = detect('drawer handle')
+top_drawer_handle = max(handles, key=lambda x: x.position[2])
+ret_val = top_drawer_handle
+
+objects = ['table', 'gripper', 'door']
+# Query: the thing you can open the door with.
+door_handle = detect('door handle')[0]
+ret_val = door_handle
+
+objects = ['table', 'gripper', 'glass', 'vase', 'plastic bottle', 'block', 'phone case']
+# Query: anything fragile.
+fragile_items = []
+for obj in ['glass', 'vase']:
+    item = detect(obj)[0]
+    fragile_items.append(item)
+ret_val = fragile_items
+
+objects = ['table', 'gripper', 'fridge']
+# Query: fridge handle.
+fridge_handle = detect('fridge handle')[0]
+ret_val = fridge_handle
+
+objects = ['table', 'gripper', 'blue block', 'red block']
+# Query: green block.
+ret_val = None
+
+objects = ['table', 'gripper', 'yellow bowl', 'red spoon']
+# Query: gripper.
+gripper = detect('gripper')[0]
+ret_val = gripper
\ No newline at end of file
diff --git a/VoxPoserApiExamples/Real/planner/real_planner_prompt.py b/VoxPoserApiExamples/Real/planner/real_planner_prompt.py
new file mode 100644
index 0000000000000000000000000000000000000000..2c2365664eb2868b09c1a0fe12a52d4e3a6909e0
--- /dev/null
+++ b/VoxPoserApiExamples/Real/planner/real_planner_prompt.py
@@ -0,0 +1,121 @@
+import numpy as np
+from env_utils import execute
+from perception_utils import parse_query_obj
+import action_utils import composer
+
+objects = ['blue block', 'yellow block', 'mug']
+# Query: place the blue block on the yellow block, and avoid the mug at all time.
+composer("grasp the blue block while keeping at least 15cm away from the mug")
+composer("back to default pose")
+composer("move to 5cm on top of the yellow block while keeping at least 15cm away from the mug")
+composer("open gripper")
+# done
+
+objects = ['airpods', 'drawer']
+# Query: Open the drawer slowly.
+composer("grasp the drawer handle, at 0.5x speed")
+composer("move away from the drawer handle by 25cm, at 0.5x speed")
+composer("open gripper, at 0.5x speed")
+# done
+
+objects = ['tissue box', 'tissue', 'bowl']
+# Query: Can you pass me a tissue and place it next to the bowl?
+composer("grasp the tissue")
+composer("back to default pose")
+composer("move to 10cm to the right of the bowl")
+composer("open gripper")
+composer("back to default pose")
+# done
+
+objects = ['charger', 'outlet']
+# Query: unplug the charger from the wall.
+composer("grasp the charger")
+composer("back to default pose")
+# done
+
+objects = ['grape', 'lemon', 'drill', 'router', 'bread', 'tray']
+# Query: put the sweeter fruit in the tray that contains the bread.
+composer("grasp the grape")
+composer("back to default pose")
+composer("move to the top of the tray that contains the bread")
+composer("open gripper")
+# done
+
+objects = ['marbles', 'tray', 'broom']
+# Query: Can you sweep the marbles into the tray?
+composer("grasp the broom")
+composer("back to default pose")
+composer("push the marbles into the tray")
+# done
+
+objects = ['orange', 'QR code', 'lemon', 'drawer']
+# Query: put the sour fruit into the top drawer.
+composer("grasp the top drawer handle")
+composer("move away from the top drawer handle by 25cm")
+composer("open gripper")
+composer("back to default pose")
+composer("grasp the lemon")
+composer("move to 10cm on top of the top drawer")
+composer("open gripper")
+# done
+
+objects = ['fridge', 'hot soup']
+# Query: Open the fridge door and be careful around the hot soup.
+composer("grasp the fridge handle and keep at least 15cm away from the hot soup")
+composer("move away from the fridge handle by 25cm and keep at least 15cm away from the hot soup")
+composer("open gripper")
+# done
+
+objects = ['cyan bowl', 'yellow bowl', 'box', 'ice cream']
+# Query: move to the top of the cyan bowl.
+composer("move to the top of the cyan bowl")
+# done
+
+objects = ['drawer', 'umbrella']
+# Query: close the drawer.
+composer("push close the drawer handle by 25cm")
+# done
+
+objects = ['plate', 'steak', 'fork', 'knife', 'spoon']
+# Query: Could you please set up the fork for the steak for me?
+composer("grasp the fork")
+composer("back to default pose")
+composer("move to 10cm to the right of the plate")
+composer("open gripper")
+composer("back to default pose")
+# done
+
+objects = ['light switch']
+# Query: Press down the switch.
+composer("close the gripper")
+composer("move to the center of the light switch")
+composer("back to default pose")
+# done
+
+objects = ['beer']
+# Query: turn close the beer.
+composer("grasp the beer cap")
+composer("turn clockwise by 180 degrees")
+composer("back to default pose")
+# done
+
+objects = ['steak', 'grill', 'plate']
+# Query: Take the steak out of the grill and put it flat on the plate.
+composer("grasp the steak")
+composer("back to default pose")
+composer("rotate the gripper to be 45 degrees slanted relative to the plate")
+composer("move to 10cm on top of the plate")
+composer("open gripper")
+composer("back to default pose")
+# done
+
+objects = ['trash can', 'napkin']
+# Query: sort napkin into trash can.
+napkin = parse_query_obj("napkin")
+trash_can = parse_query_obj("trash can")
+while np.linalg.norm(napkin.position - trash_can.position) > 0.1:
+    composer("grasp the napkin")
+    composer("move to 10cm on top of the trash can")
+    composer("open gripper")
+    composer("back to default pose")
+# done
diff --git a/VoxPoserApiExamples/Real/value_maps/real_get_affordance_map_prompt.py b/VoxPoserApiExamples/Real/value_maps/real_get_affordance_map_prompt.py
new file mode 100644
index 0000000000000000000000000000000000000000..1688cb926b7afbb3427992b13d28aebf213e92fe
--- /dev/null
+++ b/VoxPoserApiExamples/Real/value_maps/real_get_affordance_map_prompt.py
@@ -0,0 +1,146 @@
+import numpy as np
+from perception_utils import parse_query_obj
+from plan_utils import get_empty_affordance_map, set_voxel_by_radius, cm2index
+
+# Query: a point 10cm in front of [10, 15, 60].
+affordance_map = get_empty_affordance_map()
+# 10cm in front of so we add to x-axis
+x = 10 + cm2index(10, 'x')
+y = 15
+z = 60
+affordance_map[x, y, z] = 1
+ret_val = affordance_map
+
+# Query: a point on the right side of the table.
+affordance_map = get_empty_affordance_map()
+table = parse_query_obj('table')
+(min_x, min_y, min_z), (max_x, max_y, max_z) = table.aabb
+center_x, center_y, center_z = table.position
+# right side so y = max_y
+x = center_x
+y = max_y
+z = center_z
+affordance_map[x, y, z] = 1
+ret_val = affordance_map
+
+# Query: a point 20cm on top of the container.
+affordance_map = get_empty_affordance_map()
+container = parse_query_obj('container')
+(min_x, min_y, min_z), (max_x, max_y, max_z) = container.aabb
+center_x, center_y, center_z = container.position
+# 20cm on top of so we add to z-axis
+x = center_x
+y = center_y
+z = max_z + cm2index(20, 'z')
+affordance_map[x, y, z] = 1
+ret_val = affordance_map
+
+# Query: a point 1cm to the left of the brown block.
+affordance_map = get_empty_affordance_map()
+brown_block = parse_query_obj('brown block')
+(min_x, min_y, min_z), (max_x, max_y, max_z) = brown_block.aabb
+center_x, center_y, center_z = brown_block.position
+# 1cm to the left of so we subtract from y-axis
+x = center_x
+y = min_y - cm2index(1, 'y')
+z = center_z
+affordance_map[x, y, z] = 1
+ret_val = affordance_map
+
+# Query: anywhere within 20cm of the right most block.
+affordance_map = get_empty_affordance_map()
+right_most_block = parse_query_obj('the right most block')
+set_voxel_by_radius(affordance_map, right_most_block.position, radius_cm=20, value=1)
+
+# Query: a point on the back side of the table.
+affordance_map = get_empty_affordance_map()
+table = parse_query_obj('table')
+(min_x, min_y, min_z), (max_x, max_y, max_z) = table.aabb
+center_x, center_y, center_z = table.position
+# back side so x = min_x
+x = min_x
+y = center_y
+z = center_z
+affordance_map[x, y, z] = 1
+ret_val = affordance_map
+
+# Query: a point on the front right corner of the table.
+affordance_map = get_empty_affordance_map()
+table = parse_query_obj('table')
+(min_x, min_y, min_z), (max_x, max_y, max_z) = table.aabb
+center_x, center_y, center_z = table.position
+# front right corner so x = max_x and y = max_y
+x = max_x
+y = max_y
+z = center_z
+affordance_map[x, y, z] = 1
+ret_val = affordance_map
+
+# Query: a point 30cm into the topmost drawer handle.
+affordance_map = get_empty_affordance_map()
+top_handle = parse_query_obj('topmost drawer handle')
+# negative normal because we are moving into the handle.
+moving_dir = -top_handle.normal
+affordance_xyz = top_handle.position + cm2index(30, moving_dir)
+affordance_map[affordance_xyz[0], affordance_xyz[1], affordance_xyz[2]] = 1
+ret_val = affordance_map
+
+# Query: a point 5cm above the blue block.
+affordance_map = get_empty_affordance_map()
+blue_block = parse_query_obj('blue block')
+(min_x, min_y, min_z), (max_x, max_y, max_z) = blue_block.aabb
+center_x, center_y, center_z = blue_block.position
+# 5cm above so we add to z-axis
+x = center_x
+y = center_y
+z = max_z + cm2index(5, 'z')
+affordance_map[x, y, z] = 1
+ret_val = affordance_map
+
+# Query: a point 20cm away from the leftmost block.
+affordance_map = get_empty_affordance_map()
+leftmost_block = parse_query_obj('leftmost block')
+# positive normal because we are moving away from the block.
+moving_dir = leftmost_block.normal
+affordance_xyz = leftmost_block.position + cm2index(20, moving_dir)
+affordance_map[affordance_xyz[0], affordance_xyz[1], affordance_xyz[2]] = 1
+ret_val = affordance_map
+
+# Query: a point 4cm to the left of and 10cm on top of the tray that contains the lemon.
+affordance_map = get_empty_affordance_map()
+tray_with_lemon = parse_query_obj('tray that contains the lemon')
+(min_x, min_y, min_z), (max_x, max_y, max_z) = tray_with_lemon.aabb
+center_x, center_y, center_z = tray_with_lemon.position
+# 4cm to the left of so we subtract from y-axis, and 10cm on top of so we add to z-axis
+x = center_x
+y = min_y - cm2index(4, 'y')
+z = max_z + cm2index(10, 'z')
+affordance_map[x, y, z] = 1
+ret_val = affordance_map
+
+# Query: a point 10cm to the right of [45 49 66], and 5cm above it.
+affordance_map = get_empty_affordance_map()
+# 10cm to the right of so we add to y-axis, and 5cm above it so we add to z-axis
+x = 45
+y = 49 + cm2index(10, 'y')
+z = 66 + cm2index(5, 'z')
+affordance_map[x, y, z] = 1
+ret_val = affordance_map
+
+# Query: the blue circle.
+affordance_map = get_empty_affordance_map()
+blue_circle = parse_query_obj('blue circle')
+affordance_map = blue_circle.occupancy_map
+ret_val = affordance_map
+
+# Query: a point 10cm above and 5cm to the left of the yellow bowl.
+affordance_map = get_empty_affordance_map()
+yellow_bowl = parse_query_obj('yellow bowl')
+(min_x, min_y, min_z), (max_x, max_y, max_z) = yellow_bowl.aabb
+center_x, center_y, center_z = yellow_bowl.position
+# 10cm above so we add to z-axis, and 5cm to the left of so we subtract from y-axis
+x = center_x
+y = min_y - cm2index(5, 'y')
+z = max_z + cm2index(10, 'z')
+affordance_map[x, y, z] = 1
+ret_val = affordance_map
diff --git a/VoxPoserApiExamples/Real/value_maps/real_get_avoidance_map_prompt.py b/VoxPoserApiExamples/Real/value_maps/real_get_avoidance_map_prompt.py
new file mode 100644
index 0000000000000000000000000000000000000000..04aef56a51b4791bc26f9a89aaea6bd7e0681b1d
--- /dev/null
+++ b/VoxPoserApiExamples/Real/value_maps/real_get_avoidance_map_prompt.py
@@ -0,0 +1,36 @@
+import numpy as np
+from perception_utils import parse_query_obj
+from plan_utils import get_empty_avoidance_map, set_voxel_by_radius, cm2index
+
+# Query: 10cm from the bowl.
+avoidance_map = get_empty_avoidance_map()
+bowl = parse_query_obj('bowl')
+set_voxel_by_radius(avoidance_map, bowl.position, radius_cm=10, value=1)
+ret_val = avoidance_map
+
+# Query: 20cm near the mug.
+avoidance_map = get_empty_avoidance_map()
+mug = parse_query_obj('mug')
+set_voxel_by_radius(avoidance_map, mug.position, radius_cm=20, value=1)
+ret_val = avoidance_map
+
+# Query: 20cm around the mug and 10cm around the bowl.
+avoidance_map = get_empty_avoidance_map()
+mug = parse_query_obj('mug')
+set_voxel_by_radius(avoidance_map, mug.position, radius_cm=20, value=1)
+bowl = parse_query_obj('bowl')
+set_voxel_by_radius(avoidance_map, bowl.position, radius_cm=10, value=1)
+ret_val = avoidance_map
+
+# Query: 10cm from anything fragile.
+avoidance_map = get_empty_avoidance_map()
+fragile_objects = parse_query_obj('anything fragile')
+for obj in fragile_objects:
+    set_voxel_by_radius(avoidance_map, obj.position, radius_cm=10, value=1)
+ret_val = avoidance_map
+
+# Query: 10cm from the blue circle.
+avoidance_map = get_empty_avoidance_map()
+blue_circle = parse_query_obj('blue circle')
+set_voxel_by_radius(avoidance_map, blue_circle.position, radius_cm=10, value=1)
+ret_val = avoidance_map
\ No newline at end of file
diff --git a/VoxPoserApiExamples/Real/value_maps/real_get_gripper_map_prompt.py b/VoxPoserApiExamples/Real/value_maps/real_get_gripper_map_prompt.py
new file mode 100644
index 0000000000000000000000000000000000000000..fb5b25a9ef64dd7d042fd74ea1859823c916be3c
--- /dev/null
+++ b/VoxPoserApiExamples/Real/value_maps/real_get_gripper_map_prompt.py
@@ -0,0 +1,48 @@
+import numpy as np
+from perception_utils import parse_query_obj
+from plan_utils import get_empty_gripper_map, set_voxel_by_radius, cm2index
+
+# Query: open everywhere except 1cm around the green block.
+gripper_map = get_empty_gripper_map()
+# open everywhere
+gripper_map[:, :, :] = 0
+# close when 1cm around the green block
+green_block = parse_query_obj('green block')
+set_voxel_by_radius(gripper_map, green_block.position, radius_cm=1, value=1)
+ret_val = gripper_map
+
+# Query: close everywhere but open when on top of the back left corner of the table.
+gripper_map = get_empty_gripper_map()
+# close everywhere
+gripper_map[:, :, :] = 1
+# open when on top of the back left corner of the table
+table = parse_query_obj('table')
+(min_x, min_y, min_z), (max_x, max_y, max_z) = table.aabb
+center_x, center_y, center_z = table.position
+# back so x = min_x, left so y = min_y, top so we add to z
+x = min_x
+y = min_y
+z = max_z + cm2index(15, 'z')
+set_voxel_by_radius(gripper_map, (x, y, z), radius_cm=10, value=0)
+ret_val = gripper_map
+
+# Query: always open except when you are on the right side of the table.
+gripper_map = get_empty_gripper_map()
+# always open
+gripper_map[:, :, :] = 0
+# close when you are on the right side of the table
+table = parse_query_obj('table')
+center_x, center_y, center_z = table.position
+# right side so y is greater than center_y
+gripper_map[:, center_y:, :] = 1
+
+# Query: always close except when you are on the back side of the table.
+gripper_map = get_empty_gripper_map()
+# always close
+gripper_map[:, :, :] = 1
+# open when you are on the back side of the table
+table = parse_query_obj('table')
+center_x, center_y, center_z = table.position
+# back side so x is less than center_x
+gripper_map[:center_x, :, :] = 0
+ret_val = gripper_map
\ No newline at end of file
diff --git a/VoxPoserApiExamples/Real/value_maps/real_get_rotation_map_prompt.py b/VoxPoserApiExamples/Real/value_maps/real_get_rotation_map_prompt.py
new file mode 100644
index 0000000000000000000000000000000000000000..d8fc1f68721b543bbea75269fcec1ec8095c5a41
--- /dev/null
+++ b/VoxPoserApiExamples/Real/value_maps/real_get_rotation_map_prompt.py
@@ -0,0 +1,53 @@
+import numpy as np
+from plan_utils import get_empty_rotation_map, set_voxel_by_radius, cm2index, vec2quat
+from perception_utils import parse_query_obj
+from transforms3d.euler import euler2quat, quat2euler
+from transforms3d.quaternions import qmult, qinverse
+
+# Query: face the support surface of the bowl.
+rotation_map = get_empty_rotation_map()
+bowl = parse_query_obj('bowl')
+target_rotation = vec2quat(-bowl.normal)
+rotation_map[:, :, :] = target_rotation
+ret_val = rotation_map
+
+# Query: face the table when within 30cm from table center.
+rotation_map = get_empty_rotation_map()
+table = parse_query_obj('table')
+table_center = table.position
+target_rotation = vec2quat(-table.normal)
+set_voxel_by_radius(rotation_map, table_center, radius_cm=30, value=target_rotation)
+ret_val = rotation_map
+
+# Query: face the blue bowl.
+rotation_map = get_empty_rotation_map()
+blue_bowl = parse_query_obj('brown block')
+target_rotation = vec2quat(-blue_bowl.normal)
+rotation_map[:, :, :] = target_rotation
+ret_val = rotation_map
+
+# Query: turn clockwise by 45 degrees when at the center of the beer cap.
+rotation_map = get_empty_rotation_map()
+beer_cap = parse_query_obj('beer cap')
+(x, y, z) = beer_cap.position
+curr_rotation = rotation_map[x, y, z]
+rotation_delta = euler2quat(0, 0, np.pi / 4)
+rotation_map[x, y, z] = qmult(curr_rotation, rotation_delta)
+ret_val = rotation_map
+
+# Query: turn counter-clockwise by 30 degrees.
+rotation_map = get_empty_rotation_map()
+curr_rotation = rotation_map[0, 0, 0]
+rotation_delta = euler2quat(0, 0, -np.pi / 6)
+rotation_map[:, :, :] = qmult(curr_rotation, rotation_delta)
+ret_val = rotation_map
+
+# Query: rotate the gripper to be 45 degrees slanted relative to the plate.
+rotation_map = get_empty_rotation_map()
+plate = parse_query_obj('plate')
+face_plate_quat = vec2quat(-plate.normal)
+# rotate 45 degrees around the x-axis
+rotation_delta = euler2quat(-np.pi / 4, 0, 0)
+target_rotation = qmult(face_plate_quat, rotation_delta)
+rotation_map[:, :, :] = target_rotation
+ret_val = rotation_map
\ No newline at end of file
diff --git a/VoxPoserApiExamples/Real/value_maps/real_get_velocity_map_prompt.py b/VoxPoserApiExamples/Real/value_maps/real_get_velocity_map_prompt.py
new file mode 100644
index 0000000000000000000000000000000000000000..d414e030fb184aef4fe76bf4f08a0fa7154988d9
--- /dev/null
+++ b/VoxPoserApiExamples/Real/value_maps/real_get_velocity_map_prompt.py
@@ -0,0 +1,31 @@
+import numpy as np
+from plan_utils import get_empty_velocity_map, set_voxel_by_radius, cm2index
+from perception_utils import parse_query_obj
+
+# Query: faster when on the right side of the table and slower when on the left side of the table.
+velocity_map = get_empty_velocity_map()
+table = parse_query_obj('table')
+center_x, center_y, center_z = table.position
+# faster on right side so 1.5 when y > center_y, slower on left side so 0.5 when y < center_y
+velocity_map[:, center_y:, :] = 1.5
+velocity_map[:, :center_y, :] = 0.5
+ret_val = velocity_map
+
+# Query: slow down by a quarter.
+velocity_map = get_empty_velocity_map()
+velocity_map[:] = 0.75
+ret_val = velocity_map
+
+# Query: slow down by a half when you're near anything fragile (objects: ['block', 'fork', 'mug', 'bowl', 'chips']).
+velocity_map = get_empty_velocity_map()
+mug = parse_query_obj('mug')
+set_voxel_by_radius(velocity_map, mug.position, radius_cm=10, value=0.5)
+bowl = parse_query_obj('bowl')
+set_voxel_by_radius(velocity_map, bowl.position, radius_cm=10, value=0.5)
+ret_val = velocity_map
+
+# Query: quarter of the speed when within 9cm from the yellow line.
+velocity_map = get_empty_velocity_map()
+yellow_line = parse_query_obj('yellow_line')
+set_voxel_by_radius(velocity_map, yellow_line.position, radius_cm=9, value=0.25)
+ret_val = velocity_map
\ No newline at end of file
diff --git a/VoxPoserApiExamples/Sim/composer/sim_composer_prompt.py b/VoxPoserApiExamples/Sim/composer/sim_composer_prompt.py
new file mode 100644
index 0000000000000000000000000000000000000000..eea7bb637cb70930215d09e77ca87f11f82d9b5f
--- /dev/null
+++ b/VoxPoserApiExamples/Sim/composer/sim_composer_prompt.py
@@ -0,0 +1,91 @@
+import numpy as np
+from env_utils import execute, reset_to_default_pose
+from perception_utils import parse_query_obj
+from plan_utils import get_affordance_map, get_avoidance_map, get_velocity_map, get_rotation_map, get_gripper_map
+
+# Query: move ee forward for 7cm.
+movable = parse_query_obj('ee')
+affordance_map = get_affordance_map(f'a point 7cm in front of {movable.position}')
+execute(movable, affordance_map)
+
+# Query: move to the left of the green block.
+movable = parse_query_obj('ee')
+affordance_map = get_affordance_map('a point 11cm to the left of the green block')
+execute(movable, affordance_map=affordance_map)
+
+# Query: move to the yellow block while staying on the front side the pink block.
+movable = parse_query_obj('ee')
+affordance_map = get_affordance_map('a point 11cm on top of the yellow block')
+avoidance_map = get_avoidance_map('anywhere behind the pink block')
+execute(movable, affordance_map=affordance_map, avoidance_map=avoidance_map)
+
+# Query: move to the back side of the table.
+movable = parse_query_obj('ee')
+affordance_map = get_affordance_map('a point on the back side of the table')
+execute(movable, affordance_map=affordance_map, avoidance_map=avoidance_map)
+
+# Query: move to the front right corner of the table while moving at faster speed when within 7cm from the yellow block.
+movable = parse_query_obj('ee')
+affordance_map = get_affordance_map('a point on the front right corner of the table')
+velocity_map = get_velocity_map('faster speed when within 7cm from the yellow block')
+execute(movable, affordance_map=affordance_map, velocity_map=velocity_map)
+
+# Query: move to 15cm on top of the rightmost block while avoiding the blue line and moving at a quarter of the speed on the right side of the table.
+movable = parse_query_obj('ee')
+affordance_map = get_affordance_map('a point 15cm on top of the rightmost block')
+avoidance_map = get_avoidance_map('the blue line')
+velocity_map = get_velocity_map('a quarter of the speed on the right side of the table')
+execute(movable, affordance_map=affordance_map, avoidance_map=avoidance_map, velocity_map=velocity_map)
+
+# Query: close the topmost drawer by pushing.
+movable = parse_query_obj('topmost drawer handle')
+affordance_map = get_affordance_map('a point 21cm into the topmost drawer handle')
+execute(movable, affordance_map=affordance_map)
+
+# Query: push the second to the left block along the brown line.
+movable = parse_query_obj('second to the left block')
+affordance_map = get_affordance_map('the brown line')
+execute(movable, affordance_map=affordance_map)
+
+# Query: grasp the blue block from the table at a quarter of the speed.
+movable = parse_query_obj('ee')
+affordance_map = get_affordance_map('a point at the center of blue block')
+rotation_map = get_rotation_map('face the blue block')
+velocity_map = get_velocity_map('quarter of the speed')
+gripper_map = get_gripper_map('open everywhere except 1cm around the blue block')
+execute(movable, affordance_map=affordance_map, rotation_map=rotation_map, velocity_map=velocity_map, gripper_map=gripper_map)
+
+# Query: move to the left of the brown block.
+movable = parse_query_obj('ee')
+affordance_map = get_affordance_map('a point 11cm to the left of the brown block')
+execute(movable, affordance_map=affordance_map)
+
+# Query: back to default pose.
+reset_to_default_pose()
+
+# Query: open gripper.
+movable = parse_query_obj('gripper')
+gripper_map = get_gripper_map('open everywhere')
+execute(movable, gripper_map=gripper_map)
+
+# Query: drop the blue block to the right side of the table while staying at least 7cm away from the brown block.
+movable = parse_query_obj('ee')
+affordance_map = get_affordance_map('a point on the right side of the table')
+avoidance_map = get_avoidance_map('7cm away from the brown block')
+gripper_map = get_gripper_map('close everywhere except 1cm around the right side of the table')
+execute(movable, affordance_map=affordance_map, avoidance_map=avoidance_map, gripper_map=gripper_map)
+
+# Query: move to the front side of the yellow block.
+movable = parse_query_obj('ee')
+affordance_map = get_affordance_map('a point 11cm in front of the yellow block')
+execute(movable, affordance_map=affordance_map)
+
+# Query: move to the back side of the table.
+movable = parse_query_obj('ee')
+affordance_map = get_affordance_map('a point on the back side of the table')
+execute(movable, affordance_map=affordance_map)
+
+# Query: stay away from the brown block.
+movable = parse_query_obj('ee')
+avoidance_map = get_avoidance_map('3cm away from the brown block')
+execute(movable, avoidance_map=avoidance_map)
\ No newline at end of file
diff --git a/VoxPoserApiExamples/Sim/perception/sim_parse_query_obj_prompt.py b/VoxPoserApiExamples/Sim/perception/sim_parse_query_obj_prompt.py
new file mode 100644
index 0000000000000000000000000000000000000000..0e2220a0328d49d219890f72dde68d8520199ef9
--- /dev/null
+++ b/VoxPoserApiExamples/Sim/perception/sim_parse_query_obj_prompt.py
@@ -0,0 +1,71 @@
+import numpy as np
+from perception_utils import detect
+
+objects = ['green block', 'yellow line']
+# Query: ee.
+ee = detect('ee')[0]
+ret_val = ee
+
+objects = ['drawer', 'blue block', 'yellow block']
+# Query: topmost handle.
+handles = detect('drawer handle')
+# topmost so sort by z, take the last one
+handles = sorted(handles, key=lambda x: x.position[2])
+top_handle = handles[-1]
+ret_val = top_handle
+
+objects = ['yellow block', 'pink block', 'brown line', 'pink block']
+# Query: second to the left block.
+blocks = detect('block')
+# second to the left so sort by y, take the second one
+blocks = sorted(blocks, key=lambda x: x.position[1])
+second_left_block = blocks[1]
+ret_val = second_left_block
+
+objects = ['blue line', 'pink line', 'green block']
+# Query: table.
+table = detect('table')[0]
+ret_val = table
+
+objects = ['green line', 'drawer', 'yellow block']
+# Query: second to the bottom handle.
+handles = detect('drawer handle')
+# second to the bottom so sort by z, take the second one
+handles = sorted(handles, key=lambda x: x.position[2])
+second_bottom_handle = handles[1]
+ret_val = second_bottom_handle
+
+objects = ['brown line', 'brown block']
+# Query: brown line.
+brown_line = detect('brown line')[0]
+ret_val = brown_line
+
+objects = ['green block', 'brown block', 'yellow line']
+# Query: block.
+block = detect('green block')[0]
+ret_val = block
+
+objects = ['pink block', 'pink line', 'blue block']
+# Query: block closest to the pink line.
+blocks = detect('block')
+pink_line = detect('pink line')[0]
+closest_block = min(blocks, key=lambda x: np.linalg.norm(x.position - pink_line.position))
+ret_val = closest_block
+
+objects = ['blue block', 'blue line', 'green block', 'pink block', 'brown block']
+# Query: the block that is on top of the blue block.
+blocks = detect('block')
+blue_block = detect('blue block')[0]
+# find the block that is on top of the blue block
+for block in blocks:
+    if block.position[2] > blue_block.position[2]:
+        ret_val = block
+        break
+
+objects = ['drawer']
+# Query: top drawer handle.
+handles = detect('drawer handle')
+# top drawer handle so sort by z, take the last one
+handles = sorted(handles, key=lambda x: x.position[2])
+top_handle = handles[-1]
+ret_val = top_handle
\ No newline at end of file
diff --git a/VoxPoserApiExamples/Sim/value_maps/sim_get_affordance_map_prompt.py b/VoxPoserApiExamples/Sim/value_maps/sim_get_affordance_map_prompt.py
new file mode 100644
index 0000000000000000000000000000000000000000..cfaf0331ab1df2e20d73e241ba018551028d0f32
--- /dev/null
+++ b/VoxPoserApiExamples/Sim/value_maps/sim_get_affordance_map_prompt.py
@@ -0,0 +1,177 @@
+import numpy as np
+from perception_utils import parse_query_obj
+from plan_utils import get_empty_affordance_map, set_voxel_by_radius, cm2index
+
+# Query: a point 10cm in front of [10, 15, 60].
+affordance_map = get_empty_affordance_map()
+# in front so we add to x
+x = 10 + cm2index(10, 'x')
+y = 15
+z = 60
+affordance_map[x, y, z] = 1
+ret_val = affordance_map
+
+# Query: a point on the back side of the table.
+affordance_map = get_empty_affordance_map()
+table = parse_query_obj('table')
+(min_x, min_y, min_z), (max_x, max_y, max_z) = table.aabb
+center_x, center_y, center_z = table.position
+# back side so x = min_x
+x = min_x
+y = center_y
+z = center_z
+affordance_map[x, y, z] = 1
+ret_val = affordance_map
+
+# Query: a point 21cm on top of the green block.
+affordance_map = get_empty_affordance_map()
+green_block = parse_query_obj('green block')
+(min_x, min_y, min_z), (max_x, max_y, max_z) = green_block.aabb
+center_x, center_y, center_z = green_block.position
+# 21cm on top so z = max_z + 21cm
+x = center_x
+y = center_y
+z = max_z + cm2index(21, 'z')
+affordance_map[x, y, z] = 1
+ret_val = affordance_map
+
+# Query: a point 1cm to the left of the brown block.
+affordance_map = get_empty_affordance_map()
+brown_block = parse_query_obj('brown block')
+(min_x, min_y, min_z), (max_x, max_y, max_z) = brown_block.aabb
+center_x, center_y, center_z = brown_block.position
+# 1cm to the left so y = min_y - 1cm
+x = center_x
+y = min_y - cm2index(1, 'y')
+z = center_z
+affordance_map[x, y, z] = 1
+ret_val = affordance_map
+
+# Query: anywhere within 15cm of the right most block.
+affordance_map = get_empty_affordance_map()
+right_most_block = parse_query_obj('the right most block')
+set_voxel_by_radius(affordance_map, right_most_block.position, radius_cm=15, value=1)
+ret_val = affordance_map
+
+# Query: a point on the back side of the table.
+affordance_map = get_empty_affordance_map()
+table = parse_query_obj('table')
+(min_x, min_y, min_z), (max_x, max_y, max_z) = table.aabb
+center_x, center_y, center_z = table.position
+# back side so x = min_x
+x = min_x
+y = center_y
+z = center_z
+affordance_map[x, y, z] = 1
+ret_val = affordance_map
+
+# Query: a point on the front right corner of the table.
+affordance_map = get_empty_affordance_map()
+table = parse_query_obj('table')
+(min_x, min_y, min_z), (max_x, max_y, max_z) = table.aabb
+center_x, center_y, center_z = table.position
+# front right corner -> front so x = max_x, right so y = max_y
+x = max_x
+y = max_y
+z = center_z
+affordance_map[x, y, z] = 1
+ret_val = affordance_map
+
+# Query: a point 30cm into the topmost drawer handle.
+affordance_map = get_empty_affordance_map()
+top_handle = parse_query_obj('topmost drawer handle')
+# negative normal because we are moving into the handle.
+moving_dir = -top_handle.normal
+affordance_xyz = top_handle.position + cm2index(30, moving_dir)
+affordance_map[affordance_xyz[0], affordance_xyz[1], affordance_xyz[2]] = 1
+ret_val = affordance_map
+
+# Query: a point 9cm to the left of the blue block.
+affordance_map = get_empty_affordance_map()
+blue_block = parse_query_obj('blue block')
+(min_x, min_y, min_z), (max_x, max_y, max_z) = blue_block.aabb
+center_x, center_y, center_z = blue_block.position
+# 9cm to the left so y = min_y - 9cm
+x = center_x
+y = min_y - cm2index(9, 'y')
+z = center_z
+affordance_map[x, y, z] = 1
+ret_val = affordance_map
+
+# Query: a point 21cm away from the leftmost block.
+affordance_map = get_empty_affordance_map()
+leftmost_block = parse_query_obj('leftmost block')
+# positive normal because we are moving away from the block.
+moving_dir = leftmost_block.normal
+affordance_xyz = leftmost_block.position + cm2index(21, moving_dir)
+affordance_map[affordance_xyz[0], affordance_xyz[1], affordance_xyz[2]] = 1
+ret_val = affordance_map
+
+# Query: a point 7cm above the tray that contains the lemon.
+affordance_map = get_empty_affordance_map()
+tray_with_lemon = parse_query_obj('tray that contains the lemon')
+(min_x, min_y, min_z), (max_x, max_y, max_z) = tray_with_lemon.aabb
+center_x, center_y, center_z = tray_with_lemon.position
+# 7cm above so z = max_z + 7cm
+x = center_x
+y = center_y
+z = max_z + cm2index(7, 'z')
+affordance_map[x, y, z] = 1
+ret_val = affordance_map
+
+# Query: a point 3cm to the left of [45, 49, 99].
+affordance_map = get_empty_affordance_map()
+# left so y = 49 - 3cm
+x = 45
+y = 49 - cm2index(3, 'y')
+z = 99
+affordance_map[x, y, z] = 1
+ret_val = affordance_map
+
+# Query: the brown line.
+brown_line = parse_query_obj('brown line')
+affordance_map = brown_line.occupancy_map
+ret_val = affordance_map
+
+# Query: a point 13cm to the left of the gripper.
+affordance_map = get_empty_affordance_map()
+gripper = parse_query_obj('gripper')
+(min_x, min_y, min_z), (max_x, max_y, max_z) = gripper.aabb
+center_x, center_y, center_z = gripper.position
+# 13cm to the left so y = min_y - 13cm
+x = center_x
+y = min_y - cm2index(13, 'y')
+z = center_z
+affordance_map[x, y, z] = 1
+ret_val = affordance_map
+
+# Query: a point 17cm above the back left corner of the table.
+affordance_map = get_empty_affordance_map()
+table = parse_query_obj('table')
+(min_x, min_y, min_z), (max_x, max_y, max_z) = table.aabb
+center_x, center_y, center_z = table.position
+# 17cm on top of back left corner so x = min_x, y = min_y, z = max_z + 17cm
+x = min_x
+y = min_y
+z = max_z + cm2index(17, 'z')
+affordance_map[x, y, z] = 1
+ret_val = affordance_map
+
+# Query: a point at the center of the green block.
+affordance_map = get_empty_affordance_map()
+green_block = parse_query_obj('green block')
+center_x, center_y, center_z = green_block.position
+affordance_map[center_x, center_y, center_z] = 1
+ret_val = affordance_map
+
+# Query: the right side of the table.
+affordance_map = get_empty_affordance_map()
+table = parse_query_obj('table')
+(min_x, min_y, min_z), (max_x, max_y, max_z) = table.aabb
+center_x, center_y, center_z = table.position
+# right side so y = max_y
+x = center_x
+y = max_y
+z = center_z
+affordance_map[x, y, z] = 1
+ret_val = affordance_map
diff --git a/VoxPoserApiExamples/Sim/value_maps/sim_get_avoidance_map_prompt.py b/VoxPoserApiExamples/Sim/value_maps/sim_get_avoidance_map_prompt.py
new file mode 100644
index 0000000000000000000000000000000000000000..05f64b1df547bed9d0abc90e2a4d298b5757bc6f
--- /dev/null
+++ b/VoxPoserApiExamples/Sim/value_maps/sim_get_avoidance_map_prompt.py
@@ -0,0 +1,60 @@
+import numpy as np
+from perception_utils import parse_query_obj
+from plan_utils import get_empty_avoidance_map, set_voxel_by_radius, cm2index
+
+# Query: 11cm from the blue block.
+avoidance_map = get_empty_avoidance_map()
+blue_block = parse_query_obj('blue block')
+set_voxel_by_radius(avoidance_map, blue_block.position, radius_cm=11, value=1)
+ret_val = avoidance_map
+
+# Query: 7cm near the pink block.
+avoidance_map = get_empty_avoidance_map()
+pink_block = parse_query_obj('pink block')
+set_voxel_by_radius(avoidance_map, pink_block.position, radius_cm=7, value=1)
+ret_val = avoidance_map
+
+# Query: 13cm around the brown block and 5cm around the green block.
+avoidance_map = get_empty_avoidance_map()
+brown_block = parse_query_obj('brown block')
+set_voxel_by_radius(avoidance_map, brown_block.position, radius_cm=13, value=1)
+green_block = parse_query_obj('green block')
+set_voxel_by_radius(avoidance_map, green_block.position, radius_cm=5, value=1)
+ret_val = avoidance_map
+
+# Query: the blue line.
+blue_line = parse_query_obj('blue_line')
+avoidance_map = blue_line.occupancy_map
+ret_val = avoidance_map
+
+# Query: anywhere on the front side of the blue block.
+avoidance_map = get_empty_avoidance_map()
+blue_block = parse_query_obj('blue block')
+center_x, center_y, center_z = blue_block.position
+# front side so x > center_x
+avoidance_map[center_x:, :, :] = 1
+ret_val = avoidance_map
+
+# Query: anywhere on the left of the green block.
+avoidance_map = get_empty_avoidance_map()
+green_block = parse_query_obj('green block')
+center_x, center_y, center_z = green_block.position
+# left side so y < center_y
+avoidance_map[:, :center_y, :] = 1
+ret_val = avoidance_map
+
+# Query: anywhere behind the pink block.
+avoidance_map = get_empty_avoidance_map()
+pink_block = parse_query_obj('pink block')
+center_x, center_y, center_z = pink_block.position
+# behind so x < center_x
+avoidance_map[:center_x, :, :] = 1
+ret_val = avoidance_map
+
+# Query: anywhere above the brown block.
+avoidance_map = get_empty_avoidance_map()
+brown_block = parse_query_obj('brown block')
+center_x, center_y, center_z = brown_block.position
+# above so z > center_z
+avoidance_map[:, :, center_z:] = 1
+ret_val = avoidance_map
\ No newline at end of file
diff --git a/VoxPoserApiExamples/Sim/value_maps/sim_get_gripper_map_prompt.py b/VoxPoserApiExamples/Sim/value_maps/sim_get_gripper_map_prompt.py
new file mode 100644
index 0000000000000000000000000000000000000000..4f7145d01cf30a2e92d9a0fbedd73b41eb21cb31
--- /dev/null
+++ b/VoxPoserApiExamples/Sim/value_maps/sim_get_gripper_map_prompt.py
@@ -0,0 +1,57 @@
+import numpy as np
+from perception_utils import parse_query_obj
+from plan_utils import get_empty_gripper_map, set_voxel_by_radius, cm2index
+
+# Query: open everywhere except 1cm around the green block.
+gripper_map = get_empty_gripper_map()
+# open everywhere
+gripper_map[:, :, :] = 0
+# close when 1cm around the green block
+green_block = parse_query_obj('green block')
+set_voxel_by_radius(gripper_map, green_block.position, radius_cm=1, value=1)
+ret_val = gripper_map
+
+# Query: close everywhere but open when on top of the back left corner of the table.
+gripper_map = get_empty_gripper_map()
+# close everywhere
+gripper_map[:, :, :] = 1
+# open when on top of the back left corner of the table
+table = parse_query_obj('table')
+(min_x, min_y, min_z), (max_x, max_y, max_z) = table.aabb
+center_x, center_y, center_z = table.position
+# back so x = min_x, left so y = min_y, top so we add to z
+x = min_x
+y = min_y
+z = max_z + cm2index(10, 'z')
+set_voxel_by_radius(gripper_map, (x, y, z), radius_cm=10, value=0)
+ret_val = gripper_map
+
+# Query: always open except when you are on the right side of the table.
+gripper_map = get_empty_gripper_map()
+# always open
+gripper_map[:, :, :] = 0
+# close when you are on the right side of the table
+table = parse_query_obj('table')
+center_x, center_y, center_z = table.position
+# right side so y is greater than center_y
+gripper_map[:, center_y:, :] = 1
+
+# Query: always close except when you are on the back side of the table.
+gripper_map = get_empty_gripper_map()
+# always close
+gripper_map[:, :, :] = 1
+# open when you are on the back side of the table
+table = parse_query_obj('table')
+center_x, center_y, center_z = table.position
+# back side so x is less than center_x
+gripper_map[:center_x, :, :] = 0
+ret_val = gripper_map
+
+# Query: open everywhere except 1cm around the topmost drawer handle.
+gripper_map = get_empty_gripper_map()
+# open everywhere
+gripper_map[:, :, :] = 0
+# close when 1cm around the topmost drawer handle
+topmost_drawer_handle = parse_query_obj('topmost drawer handle')
+set_voxel_by_radius(gripper_map, topmost_drawer_handle.position, radius_cm=1, value=1)
+ret_val = gripper_map
\ No newline at end of file
diff --git a/VoxPoserApiExamples/Sim/value_maps/sim_get_rotation_map_prompt.py b/VoxPoserApiExamples/Sim/value_maps/sim_get_rotation_map_prompt.py
new file mode 100644
index 0000000000000000000000000000000000000000..ca66bfd81cfe101ce1e5d4515dff921cc2eac7ca
--- /dev/null
+++ b/VoxPoserApiExamples/Sim/value_maps/sim_get_rotation_map_prompt.py
@@ -0,0 +1,25 @@
+import numpy as np
+from plan_utils import get_empty_rotation_map, set_voxel_by_radius, cm2index, vec2quat
+from perception_utils import parse_query_obj
+
+# Query: face the green block.
+rotation_map = get_empty_rotation_map()
+green_block = parse_query_obj('green block')
+target_rotation = vec2quat(-green_block.normal)
+rotation_map[:, :, :] = target_rotation
+ret_val = rotation_map
+
+# Query: face the table when within 30cm from table center.
+rotation_map = get_empty_rotation_map()
+table = parse_query_obj('table')
+table_center = table.position
+target_rotation = vec2quat(-table.normal)
+set_voxel_by_radius(rotation_map, table_center, radius_cm=30, value=target_rotation)
+ret_val = rotation_map
+
+# Query: face the topmost drawer handle.
+rotation_map = get_empty_rotation_map()
+topmost_drawer_handle = parse_query_obj('topmost drawer handle')
+target_rotation = vec2quat(-topmost_drawer_handle.normal)
+rotation_map[:, :, :] = target_rotation
+ret_val = rotation_map
\ No newline at end of file
diff --git a/VoxPoserApiExamples/Sim/value_maps/sim_get_velocity_map_prompt.py b/VoxPoserApiExamples/Sim/value_maps/sim_get_velocity_map_prompt.py
new file mode 100644
index 0000000000000000000000000000000000000000..0be25dc15254729105749b6b298b9ca471a86255
--- /dev/null
+++ b/VoxPoserApiExamples/Sim/value_maps/sim_get_velocity_map_prompt.py
@@ -0,0 +1,46 @@
+import numpy as np
+from plan_utils import get_empty_velocity_map, set_voxel_by_radius, cm2index
+from perception_utils import parse_query_obj
+
+# Query: faster when on the left side of the table and a quarter of the speed when on the other side of the table.
+velocity_map = get_empty_velocity_map()
+table = parse_query_obj('table')
+center_x, center_y, center_z = table.position
+# faster on left side so y < center_y
+velocity_map[:, :center_y, :] = 1.5
+# a quarter of the speed on the other side so y > center_y
+velocity_map[:, center_y:, :] = 0.25
+ret_val = velocity_map
+
+# Query: slow down by a quarter.
+velocity_map = get_empty_velocity_map()
+velocity_map[:] = 0.75
+ret_val = velocity_map
+
+# Query: quarter of the speed when within 9cm from the yellow block.
+velocity_map = get_empty_velocity_map()
+yellow_block = parse_query_obj('yellow block')
+set_voxel_by_radius(velocity_map, yellow_block.position, radius_cm=9, value=0.25)
+ret_val = velocity_map
+
+# Query: quarter of the speed in the back side of the table.
+velocity_map = get_empty_velocity_map()
+table = parse_query_obj('table')
+center_x, center_y, center_z = table.position
+# a quarter of the speed in the back side so x < center_x
+velocity_map[:center_x, :, :] = 0.25
+ret_val = velocity_map
+
+# Query: faster speed in the right side of the table.
+velocity_map = get_empty_velocity_map()
+table = parse_query_obj('table')
+center_x, center_y, center_z = table.position
+# faster in the right side so y > center_y
+velocity_map[:, center_y:, :] = 1.5
+ret_val = velocity_map
+
+# Query: quarter of the speed when within 11cm from the brown block.
+velocity_map = get_empty_velocity_map()
+brown_block = parse_query_obj('brown_block')
+set_voxel_by_radius(velocity_map, brown_block.position, radius_cm=11, value=0.25)
+ret_val = velocity_map
\ No newline at end of file
diff --git a/agibot_prompts/agibot_composer.py b/agibot_prompts/agibot_composer.py
new file mode 100644
index 0000000000000000000000000000000000000000..a165fcc8767e50851f22881c7560a798974f543a
--- /dev/null
+++ b/agibot_prompts/agibot_composer.py
@@ -0,0 +1,139 @@
+import numpy as np
+from env_utils import execute, reset_to_default_pose
+from perception_utils import search_object
+from plan_utils import get_target_pose, get_operation
+
+# Query: move ee forward for 10cm.
+while True:
+    movable = search_object('gripper')
+    target_pose = get_target_pose(f'a point 10cm in front of {movable.position}')
+    success = execute(movable, target_pose)
+    if success: break
+
+# Query: go back to default.
+reset_to_default_pose()
+
+# Query: move the gripper behind the bowl.
+while True:
+    movable = search_object('gripper')
+    target_pose = get_target_pose('a point 15cm behind the bowl')
+    success = execute(movable, target_pose=target_pose)
+    if success: break
+
+# Query: move to the back side of the table.
+while True:
+    movable = search_object('gripper')
+    target_pose = get_target_pose('a point on the back side of the table')
+    success = execute(movable, target_pose=target_pose)
+    if success: break
+
+# Query: move to the top of the plate.
+while True:
+    movable = search_object('gripper')
+    target_pose = get_target_pose('a point 10cm above the plate')
+    success = execute(movable, target_pose=target_pose)
+    if success: break
+
+# Query: drop the toy inside container.
+while True:
+    movable = search_object('gripper')
+    target_pose = get_target_pose('a point 15cm above the container')
+    operation = get_operation('close everywhere but open when on top of the container')
+    success = execute(movable, target_pose=target_pose, operation=operation)
+    if success: break
+
+# Query: push close the topmost drawer.
+while True:
+    movable = search_object('topmost drawer handle')
+    target_pose = get_target_pose('a point 30cm into the topmost drawer handle')
+    success = execute(movable, target_pose=target_pose)
+    if success: break
+
+# Query: push the second to the left block along the red line.
+while True:
+    movable = search_object('second to the left block')
+    target_pose = get_target_pose('the red line')
+    success = execute(movable, target_pose=target_pose)
+    if success: break
+
+# Query: grasp the blue block from the table.
+while True:
+    movable = search_object('gripper')
+    target_pose = get_target_pose('a point at the center of blue block')
+    operation = get_operation('open everywhere except 1cm around the blue block')
+    success = execute(movable, target_pose=target_pose, operation=operation)
+    if success: break
+
+# Query: move to the left of the brown block.
+while True:
+    movable = search_object('gripper')
+    target_pose = get_target_pose('a point 10cm to the left of the brown block')
+    success = execute(movable, target_pose=target_pose)
+    if success: break
+
+# Query: move to the top of the tray that contains the lemon.
+while True:
+    movable = search_object('gripper')
+    target_pose = get_target_pose('a point 10cm above the tray that contains the lemon')
+    success = execute(movable, target_pose=target_pose)
+    if success: break
+
+# Query: close drawer by 5cm.
+while True:
+    movable = search_object('drawer handle')
+    target_pose = get_target_pose('a point 5cm into the drawer handle')
+    success = execute(movable, target_pose=target_pose)
+    if success: break
+
+# Query: move to 5cm on top of the soda can, when within 20cm of the wooden mug.
+while True:
+    movable = search_object('gripper')
+    target_pose = get_target_pose('a point 5cm above the soda can')
+    success = execute(movable, target_pose=target_pose)
+    if success: break
+
+# Query: wipe the red dot.
+while True:
+    movable = search_object('gripper')
+    target_pose = get_target_pose('the red dot')
+    success = execute(movable, target_pose=target_pose)
+    if success: break
+
+# Query: grasp the mug from the shelf.
+while True:
+    movable = search_object('gripper')
+    target_pose = get_target_pose('a point at the center of the mug handle')
+    operation = get_operation('open everywhere except 1cm around the mug handle')
+    success = execute(movable, target_pose=target_pose, operation=operation)
+    if success: break
+
+# Query: move to 10cm on top of the soup bowl, and 5cm to the left of the soup bowl.
+while True:
+    movable = search_object('gripper')
+    target_pose = get_target_pose('a point 10cm above and 5cm to the left of the soup bowl')
+    success = execute(movable, target_pose=target_pose)
+    if success: break
+
+# Query: open gripper.
+while True:
+    movable = search_object('gripper')
+    operation = get_operation('open everywhere')
+    success = execute(movable, operation=operation)
+    if success: break
+
+# Query: sweep all particles to the left side of the table.
+particles = search_object('particles')
+for particle in particles:
+    while True:
+        movable = particle
+        target_pose = get_target_pose('a point on the left side of the table')
+        success = execute(particle, target_pose=target_pose)
+        if success: break
+
+# Query: grasp the bottom drawer handle.
+while True:
+    movable = search_object('gripper')
+    target_pose = get_target_pose('a point at the center of the bottom drawer handle')
+    operation = get_operation('open everywhere except 1cm around the bottom drawer handle')
+    success = execute(movable, target_pose=target_pose, operation=operation)
+    if success: break
\ No newline at end of file
diff --git a/agibot_prompts/agibot_perceptor.py b/agibot_prompts/agibot_perceptor.py
new file mode 100644
index 0000000000000000000000000000000000000000..d88286f3b3a815aaeb315359d0f0b15307af558a
--- /dev/null
+++ b/agibot_prompts/agibot_perceptor.py
@@ -0,0 +1,104 @@
+import numpy as np
+from perception_utils import track
+
+objects = ['table', 'gripper', 'green block', 'cardboard box']
+# Query: gripper.
+gripper = track('gripper')[0]
+ret_val = gripper
+
+objects = ['table', 'gripper', 'drawer', 'egg', 'egg', 'plate']
+# Query: topmost handle.
+handles = track('drawer handle')
+handles = sorted(handles, key=lambda x: x.position[2])
+top_handle = handles[-1]
+ret_val = top_handle
+
+objects = ['table', 'gripper', 'yellow block', 'charging cable', 'cyan block', 'magenta block']
+# Query: second to the left block.
+blocks = track('block')
+blocks = sorted(blocks, key=lambda x: x.position[1])
+second_left_block = blocks[1]
+ret_val = second_left_block
+
+objects = ['table', 'gripper', 'iPhone', 'ruler', 'pink line', 'blue line']
+# Query: the front most line on the table.
+lines = track('line')
+lines = sorted(lines, key=lambda x: x.position[0])
+front_most_line = lines[-1]
+ret_val = front_most_line
+
+objects = ['table', 'gripper', 'vase', 'napkin box', 'mask']
+# Query: table.
+table = track('table')[0]
+ret_val = table
+
+objects = ['table', 'gripper', 'bottle', 'drawer', 'bowl', 'bag']
+# Query: second to the bottom handle.
+handles = track('drawer handle')
+handles = sorted(handles, key=lambda x: x.position[2])
+second_bottom_handle = handles[1]
+ret_val = second_bottom_handle
+
+objects = ['table', 'gripper', 'brown line', 'red block', 'monitor']
+# Query: brown line.
+brown_line = track('brown line')[0]
+ret_val = brown_line
+
+objects = ['table', 'gripper', 'green block', 'cup holder', 'black block']
+# Query: block.
+block = track('green block')[0]
+ret_val = block
+
+objects = ['table', 'gripper', 'mouse', 'yellow bowl', 'brown bowl', 'sticker']
+# Query: bowl closest to the sticker.
+bowls = track('bowl')
+sticker = track('sticker')[0]
+closest_bowl = min(bowls, key=lambda x: np.linalg.norm(x.position - sticker.position))
+ret_val = closest_bowl
+
+objects = ['table', 'gripper', 'keyboard', 'brown bag', 'pink bag', 'red tape', 'bottle']
+# Query: bag with the red tape on top.
+bags = track('bag')
+red_tape = track('red tape')[0]
+bag_with_red_tape = min(bags, key=lambda x: np.linalg.norm(x.position - red_tape.position))
+ret_val = bag_with_red_tape
+
+objects = ['table', 'gripper', 'grape', 'wood tray', 'strawberry', 'white tray', 'blue tray', 'bread']
+# Query: tray that contains the bread.
+trays = track('tray')
+bread = track('bread')[0]
+tray_with_bread = min(trays, key=lambda x: np.linalg.norm(x.position - bread.position))
+ret_val = tray_with_bread
+
+objects = ['table', 'gripper', 'drawer']
+# Query: top drawer handle.
+handles = track('drawer handle')
+top_drawer_handle = max(handles, key=lambda x: x.position[2])
+ret_val = top_drawer_handle
+
+objects = ['table', 'gripper', 'door']
+# Query: the thing you can open the door with.
+door_handle = track('door handle')[0]
+ret_val = door_handle
+
+objects = ['table', 'gripper', 'glass', 'vase', 'plastic bottle', 'block', 'phone case']
+# Query: anything fragile.
+fragile_items = []
+for obj in ['glass', 'vase']:
+    item = track(obj)[0]
+    fragile_items.append(item)
+ret_val = fragile_items
+
+objects = ['table', 'gripper', 'fridge']
+# Query: fridge handle.
+fridge_handle = track('fridge handle')[0]
+ret_val = fridge_handle
+
+objects = ['table', 'gripper', 'blue block', 'red block']
+# Query: green block.
+ret_val = None
+
+objects = ['table', 'gripper', 'yellow bowl', 'red spoon']
+# Query: gripper.
+gripper = track('gripper')[0]
+ret_val = gripper
\ No newline at end of file
diff --git a/agibot_prompts/agibot_planner.py b/agibot_prompts/agibot_planner.py
new file mode 100644
index 0000000000000000000000000000000000000000..2c2365664eb2868b09c1a0fe12a52d4e3a6909e0
--- /dev/null
+++ b/agibot_prompts/agibot_planner.py
@@ -0,0 +1,121 @@
+import numpy as np
+from env_utils import execute
+from perception_utils import parse_query_obj
+import action_utils import composer
+
+objects = ['blue block', 'yellow block', 'mug']
+# Query: place the blue block on the yellow block, and avoid the mug at all time.
+composer("grasp the blue block while keeping at least 15cm away from the mug")
+composer("back to default pose")
+composer("move to 5cm on top of the yellow block while keeping at least 15cm away from the mug")
+composer("open gripper")
+# done
+
+objects = ['airpods', 'drawer']
+# Query: Open the drawer slowly.
+composer("grasp the drawer handle, at 0.5x speed")
+composer("move away from the drawer handle by 25cm, at 0.5x speed")
+composer("open gripper, at 0.5x speed")
+# done
+
+objects = ['tissue box', 'tissue', 'bowl']
+# Query: Can you pass me a tissue and place it next to the bowl?
+composer("grasp the tissue")
+composer("back to default pose")
+composer("move to 10cm to the right of the bowl")
+composer("open gripper")
+composer("back to default pose")
+# done
+
+objects = ['charger', 'outlet']
+# Query: unplug the charger from the wall.
+composer("grasp the charger")
+composer("back to default pose")
+# done
+
+objects = ['grape', 'lemon', 'drill', 'router', 'bread', 'tray']
+# Query: put the sweeter fruit in the tray that contains the bread.
+composer("grasp the grape")
+composer("back to default pose")
+composer("move to the top of the tray that contains the bread")
+composer("open gripper")
+# done
+
+objects = ['marbles', 'tray', 'broom']
+# Query: Can you sweep the marbles into the tray?
+composer("grasp the broom")
+composer("back to default pose")
+composer("push the marbles into the tray")
+# done
+
+objects = ['orange', 'QR code', 'lemon', 'drawer']
+# Query: put the sour fruit into the top drawer.
+composer("grasp the top drawer handle")
+composer("move away from the top drawer handle by 25cm")
+composer("open gripper")
+composer("back to default pose")
+composer("grasp the lemon")
+composer("move to 10cm on top of the top drawer")
+composer("open gripper")
+# done
+
+objects = ['fridge', 'hot soup']
+# Query: Open the fridge door and be careful around the hot soup.
+composer("grasp the fridge handle and keep at least 15cm away from the hot soup")
+composer("move away from the fridge handle by 25cm and keep at least 15cm away from the hot soup")
+composer("open gripper")
+# done
+
+objects = ['cyan bowl', 'yellow bowl', 'box', 'ice cream']
+# Query: move to the top of the cyan bowl.
+composer("move to the top of the cyan bowl")
+# done
+
+objects = ['drawer', 'umbrella']
+# Query: close the drawer.
+composer("push close the drawer handle by 25cm")
+# done
+
+objects = ['plate', 'steak', 'fork', 'knife', 'spoon']
+# Query: Could you please set up the fork for the steak for me?
+composer("grasp the fork")
+composer("back to default pose")
+composer("move to 10cm to the right of the plate")
+composer("open gripper")
+composer("back to default pose")
+# done
+
+objects = ['light switch']
+# Query: Press down the switch.
+composer("close the gripper")
+composer("move to the center of the light switch")
+composer("back to default pose")
+# done
+
+objects = ['beer']
+# Query: turn close the beer.
+composer("grasp the beer cap")
+composer("turn clockwise by 180 degrees")
+composer("back to default pose")
+# done
+
+objects = ['steak', 'grill', 'plate']
+# Query: Take the steak out of the grill and put it flat on the plate.
+composer("grasp the steak")
+composer("back to default pose")
+composer("rotate the gripper to be 45 degrees slanted relative to the plate")
+composer("move to 10cm on top of the plate")
+composer("open gripper")
+composer("back to default pose")
+# done
+
+objects = ['trash can', 'napkin']
+# Query: sort napkin into trash can.
+napkin = parse_query_obj("napkin")
+trash_can = parse_query_obj("trash can")
+while np.linalg.norm(napkin.position - trash_can.position) > 0.1:
+    composer("grasp the napkin")
+    composer("move to 10cm on top of the trash can")
+    composer("open gripper")
+    composer("back to default pose")
+# done
diff --git a/app.py b/app.py
new file mode 100644
index 0000000000000000000000000000000000000000..cffdb6ba84e4a48e64c08ef5feee3fa84cc80416
--- /dev/null
+++ b/app.py
@@ -0,0 +1,202 @@
+import openai
+import numpy as np
+from tempfile import NamedTemporaryFile
+import copy
+import shapely
+from shapely.geometry import *
+from shapely.affinity import *
+from omegaconf import OmegaConf
+from moviepy.editor import ImageSequenceClip
+import gradio as gr
+
+from lmp import LMP, LMPFGen
+from sim import PickPlaceEnv, LMP_wrapper, VoxPoserWrapper, AgibotWrapper
+from consts import ALL_BLOCKS, ALL_BOWLS
+from md_logger import MarkdownLogger
+from utils import get_geoip
+
+class DemoRunner:
+
+    def __init__(self, config_file: str = 'cfg.yaml'):
+        self._cfg = OmegaConf.to_container(OmegaConf.load(config_file), resolve=True)
+        self._env = None
+        self._md_logger = MarkdownLogger()
+
+    def make_LMP(self, env, cfg_choice):
+        # LMP env wrapper
+        cfg = copy.deepcopy(self._cfg)
+        # cfg['env'] = {
+        #     'init_objs': list(env.obj_name_to_id.keys()),
+        #     'coords': cfg['tabletop_coords']
+        # }
+
+        if cfg_choice == "voxposer":
+            LMP_env = VoxPoserWrapper(env, cfg)
+        elif cfg_choice == "agibot":
+            LMP_env = AgibotWrapper(env, cfg)
+
+        # creating APIs that the LMPs can interact with
+        fixed_vars = {
+            'np': np
+        }
+        fixed_vars.update({
+            name: eval(name)
+            for name in shapely.geometry.__all__ + shapely.affinity.__all__
+        })
+
+        variable_vars = {
+            k: getattr(LMP_env, k)
+            for k in dir(LMP_env)
+            if not k.startswith("__") and callable(getattr(LMP_env, k))
+        }
+        variable_vars['say'] = lambda msg: self._md_logger.log_text(f'Robot says: "{msg}"')
+
+        # creating the function-generating LMP
+        lmp_fgen = LMPFGen(cfg['lmps']['fgen'], fixed_vars, variable_vars, self._md_logger)
+
+        # creating other low-level LMPs
+        variable_vars.update({
+            k: LMP(k, cfg['lmps'][k], lmp_fgen, fixed_vars, variable_vars, self._md_logger)
+            for k in cfg['lmps'].keys() if k != 'fgen'
+        })
+
+        # creating the LMP that deals w/ high-level language commands
+        lmp_planner = LMP(
+            'planner', cfg['lmps']['planner'], lmp_fgen, fixed_vars, variable_vars, self._md_logger
+        )
+
+        return lmp_planner
+
+    def setup(self, api_key, n_blocks, n_bowls, proxy, cfg_choice):
+        openai.api_key = api_key
+
+        # self._env = PickPlaceEnv(render=True, high_res=True, high_frame_rate=False)
+        # list_idxs = np.random.choice(len(ALL_BLOCKS), size=max(n_blocks, n_bowls), replace=False)
+        # block_list = [ALL_BLOCKS[i] for i in list_idxs[:n_blocks]]
+        # bowl_list = [ALL_BOWLS[i] for i in list_idxs[:n_bowls]]
+        # obj_list = block_list + bowl_list
+        # self._env.reset(obj_list)
+
+        self._lmp_planner = self.make_LMP(self._env, cfg_choice)
+
+        # info = '### Available Objects: \n- ' + '\n- '.join(obj_list)
+        # img = self._env.get_camera_image()
+
+        info, img = None, None
+
+        return info, img
+
+    def run(self, instruction):
+        # if self._env is None:
+        #     return 'Please run setup first!', None, None
+
+        # self._env.cache_video = []
+        self._md_logger.clear()
+
+        self._lmp_planner(instruction)
+
+        # try:
+        #     self._lmp_planner(instruction, f'objects = {self._env.object_list}')
+        # except Exception as e:
+        #     return f'Error: {e}', None, None
+
+        # video_file_name = None
+        # if self._env.cache_video:
+        #     rendered_clip = ImageSequenceClip(self._env.cache_video, fps=25)
+        #     video_file_name = NamedTemporaryFile(suffix='.mp4').name
+        #     rendered_clip.write_videofile(video_file_name, fps=25)
+        video_file_name = None
+
+        # return self._md_logger.get_log(), self._env.get_camera_image(), video_file_name
+
+        return self._md_logger.get_log(), None, video_file_name
+
+
+def setup(api_key, n_blocks, n_bowls, proxy_addr, cfg_choice):
+    if not api_key:
+        return 'Please enter your OpenAI API key!', None, None
+
+    if not proxy_addr:
+        return 'Please enter your local proxy address!', None, None
+
+    if n_blocks + n_bowls == 0:
+        return 'Please select at least one object!', None, None
+
+    if "http://" in proxy_addr:
+        openai.proxy = proxy_addr
+    else:
+        openai.proxy = f"http://{proxy_addr}"
+
+    # ip_status, ip_info = get_geoip(openai.proxy)
+    # if ip_status == -1:
+    #     return ip_info, None, None
+    # elif ip_status == 0:
+    #     pressed_key = input('Continue with current ip location? (y/n)')
+    #     if pressed_key.lower() != 'y':
+    #         return ip_info, None, None
+    # else:
+    #     print(f'{ip_info} IP location check passed.')
+
+    if cfg_choice == "voxposer":
+        cfg_file = 'cfg_voxposer.yaml'
+    elif cfg_choice == "agibot":
+        cfg_file = 'cfg_agibot.yaml'
+
+    demo_runner = DemoRunner(cfg_file)
+
+    info, img = demo_runner.setup(api_key, n_blocks, n_bowls, proxy_addr, cfg_choice)
+    return info, img, demo_runner
+
+
+def run(instruction, demo_runner):
+    if demo_runner is None:
+        return 'Please run setup first!', None, None
+    return demo_runner.run(instruction)
+
+
+if __name__ == '__main__':
+    with open('README.md', 'r') as f:
+        for _ in range(12):
+            next(f)
+        readme_text = f.read()
+
+    with gr.Blocks() as demo:
+        state = gr.State(None)
+
+        # gr.Markdown(readme_text)
+        gr.Markdown('# Interactive Demo')
+        with gr.Row():
+            with gr.Column():
+                with gr.Column():
+                    inp_api_key = gr.Textbox(label='OpenAI API Key (this is not stored anywhere)', lines=1)
+                    inp_proxy_addr = gr.Textbox(label='Your local proxy address', lines=1)
+                    inp_cfg = gr.Dropdown(label='Configuration', choices=['voxposer', 'agibot'])
+                with gr.Row():
+                    inp_n_blocks = gr.Slider(label='Number of Blocks', minimum=0, maximum=4, value=3, step=1)
+                    inp_n_bowls = gr.Slider(label='Number of Bowls', minimum=0, maximum=4, value=3, step=1)
+
+                btn_setup = gr.Button("Setup/Reset Simulation")
+                info_setup = gr.Markdown(label='Setup Info')
+            with gr.Column():
+                img_setup = gr.Image(label='Current Simulation')
+
+        with gr.Row():
+            with gr.Column():
+                inp_instruction = gr.Textbox(label='Instruction', lines=1)
+                btn_run = gr.Button("Run (this may take 30+ seconds)")
+                info_run = gr.Markdown(label='Generated Code')
+            with gr.Column():
+                video_run = gr.Video(label='Video of Last Instruction')
+
+        btn_setup.click(
+            setup,
+            inputs=[inp_api_key, inp_n_blocks, inp_n_bowls, inp_proxy_addr, inp_cfg],
+            outputs=[info_setup, img_setup, state]
+        )
+        btn_run.click(
+            run,
+            inputs=[inp_instruction, state],
+            outputs=[info_run, img_setup, video_run]
+        )
+
+    demo.launch()
\ No newline at end of file
diff --git a/bowl/bowl.urdf b/bowl/bowl.urdf
new file mode 100644
index 0000000000000000000000000000000000000000..cec22b4e7d54bf263ebe88b3d1c140c0c50aa2d1
--- /dev/null
+++ b/bowl/bowl.urdf
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:505058761a89c12542f9ba28c04061774a4016a65b53c3bfd822cf5a9dd19d1b
+size 806
diff --git a/bowl/cup.obj b/bowl/cup.obj
new file mode 100644
index 0000000000000000000000000000000000000000..4baa7c583e7418e80881d14c0ca839d21eb09bac
--- /dev/null
+++ b/bowl/cup.obj
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:624cba3eaf09307b766ef791eea49a188a28f8969ec8b28c724ae1df4d4ce446
+size 27464
diff --git a/bowl/textured-0008192.obj b/bowl/textured-0008192.obj
new file mode 100644
index 0000000000000000000000000000000000000000..d1b50343247fad3fdacc9ebd4877cf08c17ebdd2
--- /dev/null
+++ b/bowl/textured-0008192.obj
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:3fee6d4c3f8b4fe40de17801e9a475463c435b9e3f151fc5a9bcfc5a91355f2a
+size 724293
diff --git a/cfg.yaml b/cfg.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..243f449e514d1ccbf661f841fd94b8ff324a6bed
--- /dev/null
+++ b/cfg.yaml
@@ -0,0 +1,90 @@
+
+lmps:
+  tabletop_ui:
+    prompt_path: prompts/tabletop_ui.py
+    engine: text-davinci-003
+    max_tokens: 256
+    temperature: 0
+    query_prefix: '# '
+    query_suffix: '.'
+    stop: ['#', 'objects = [']
+    maintain_session: True
+    debug_mode: False
+    include_context: True
+    has_return: False
+    return_val_name: ret_val
+  parse_obj_name:
+    prompt_path: prompts/parse_obj_name.py
+    engine: text-davinci-003
+    max_tokens: 512
+    temperature: 0
+    query_prefix: '# '
+    query_suffix: '.'
+    stop: ['#', 'objects = [']
+    maintain_session: False
+    debug_mode: False
+    include_context: True
+    has_return: True
+    return_val_name: ret_val
+  parse_position:
+    prompt_path: prompts/parse_position.py
+    engine: text-davinci-003
+    max_tokens: 512
+    temperature: 0
+    query_prefix: '# '
+    query_suffix: '.'
+    stop: ['#']
+    maintain_session: False
+    debug_mode: False
+    include_context: True
+    has_return: True
+    return_val_name: ret_val
+  parse_question:
+    prompt_path: prompts/parse_question.py
+    engine: text-davinci-003
+    max_tokens: 512
+    temperature: 0
+    query_prefix: '# '
+    query_suffix: '.'
+    stop: ['#', 'objects = [']
+    maintain_session: False
+    debug_mode: False
+    include_context: True
+    has_return: True
+    return_val_name: ret_val
+  transform_shape_pts:
+    prompt_path: prompts/transform_shape_pts.py
+    engine: text-davinci-003
+    max_tokens: 512
+    temperature: 0
+    query_prefix: '# '
+    query_suffix: '.'
+    stop: ['#']
+    maintain_session: False
+    debug_mode: False
+    include_context: True
+    has_return: True
+    return_val_name: new_shape_pts
+  fgen:
+    prompt_path: prompts/fgen.py
+    engine: text-davinci-003
+    max_tokens: 512
+    temperature: 0
+    query_prefix: '# define function: '
+    query_suffix: '.'
+    stop: ['# define', '# example']
+    maintain_session: False
+    debug_mode: False
+    include_context: True
+
+tabletop_coords:
+  top_left: [-0.25, -0.25]
+  top_side: [0, -0.25]
+  top_right: [0.25, -0.25]
+  left_side: [-0.25, -0.5]
+  middle: [0, -0.5]
+  right_side: [0.25, -0.5]
+  bottom_left: [-0.25, -0.75]
+  bottom_side: [0, -0.75]
+  bottom_right: [0.25, -0.75]
+  table_z: 0
\ No newline at end of file
diff --git a/cfg_agibot.yaml b/cfg_agibot.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..54feb6613e120daa1b76c817e81b4526e41ab18a
--- /dev/null
+++ b/cfg_agibot.yaml
@@ -0,0 +1,51 @@
+lmps:
+  planner:
+    prompt_path: agibot_prompts/agibot_planner.py
+    model: gpt-4
+    max_tokens: 1024
+    temperature: 0
+    query_prefix: '# Query: '
+    query_suffix: '.'
+    stop: ['#', 'objects = [']
+    maintain_session: True
+    debug_mode: False
+    include_context: True
+    has_return: False
+    return_val_name: ret_val
+  composer:
+    prompt_path: agibot_prompts/agibot_composer.py
+    model: gpt-4
+    max_tokens: 1024
+    temperature: 0
+    query_prefix: '# Query: '
+    query_suffix: '.'
+    stop: ['#', 'objects = [']
+    maintain_session: False
+    debug_mode: False
+    include_context: True
+    has_return: False
+    return_val_name: ret_val
+  search_object:
+    prompt_path: agibot_prompts/agibot_perceptor.py
+    model: gpt-4
+    max_tokens: 512
+    temperature: 0
+    query_prefix: '# Query: '
+    query_suffix: '.'
+    stop: ['#']
+    maintain_session: False
+    debug_mode: True
+    include_context: True
+    has_return: True
+    return_val_name: ret_val
+  fgen:
+    prompt_path: prompts/fgen.py
+    engine: gpt-4
+    max_tokens: 512
+    temperature: 0
+    query_prefix: '# define function: '
+    query_suffix: '.'
+    stop: ['# define', '# example']
+    maintain_session: False
+    debug_mode: False
+    include_context: True
\ No newline at end of file
diff --git a/cfg_voxposer.yaml b/cfg_voxposer.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..cdae848c8330c25e65679a20f30eb44dc9e606fe
--- /dev/null
+++ b/cfg_voxposer.yaml
@@ -0,0 +1,114 @@
+
+lmps:
+  planner:
+    prompt_path: VoxPoserApiExamples/Real/planner/real_planner_prompt.py
+    model: gpt-4
+    max_tokens: 1024
+    temperature: 0
+    query_prefix: '# Query: '
+    query_suffix: '.'
+    stop: ['#', 'objects = [']
+    maintain_session: True
+    debug_mode: False
+    include_context: True
+    has_return: False
+    return_val_name: ret_val
+  composer:
+    prompt_path: VoxPoserApiExamples/Real/composer/real_composer_prompt.py
+    model: gpt-4
+    max_tokens: 1024
+    temperature: 0
+    query_prefix: '# Query: '
+    query_suffix: '.'
+    stop: ['#', 'objects = [']
+    maintain_session: False
+    debug_mode: False
+    include_context: True
+    has_return: False
+    return_val_name: ret_val
+  get_affordance_map:
+    prompt_path: VoxPoserApiExamples/Real/value_maps/real_get_affordance_map_prompt.py
+    model: gpt-4
+    max_tokens: 512
+    temperature: 0
+    query_prefix: '# Query: '
+    query_suffix: '.'
+    stop: ['#']
+    maintain_session: False
+    debug_mode: True
+    include_context: True
+    has_return: True
+    return_val_name: ret_val
+  get_avoidance_map:
+    prompt_path: VoxPoserApiExamples/Real/value_maps/real_get_avoidance_map_prompt.py
+    model: gpt-4
+    max_tokens: 512
+    temperature: 0
+    query_prefix: '# Query: '
+    query_suffix: '.'
+    stop: ['#', 'objects = [']
+    maintain_session: False
+    debug_mode: True
+    include_context: True
+    has_return: True
+    return_val_name: ret_val
+  get_gripper_map:
+    prompt_path: VoxPoserApiExamples/Real/value_maps/real_get_gripper_map_prompt.py
+    model: gpt-4
+    max_tokens: 512
+    temperature: 0
+    query_prefix: '# Query: '
+    query_suffix: '.'
+    stop: ['#']
+    maintain_session: False
+    debug_mode: True
+    include_context: True
+    has_return: True
+    return_val_name: new_shape_pts
+  get_rotation_map:
+    prompt_path: VoxPoserApiExamples/Real/value_maps/real_get_rotation_map_prompt.py
+    model: gpt-4
+    max_tokens: 512
+    temperature: 0
+    query_prefix: '# define function: '
+    query_suffix: '.'
+    stop: ['# define', '# example']
+    maintain_session: False
+    debug_mode: True
+    include_context: True
+    has_return: True
+  get_velocity_map:
+    prompt_path: VoxPoserApiExamples/Real/value_maps/real_get_velocity_map_prompt.py
+    model: gpt-4
+    max_tokens: 512
+    temperature: 0
+    query_prefix: '# define function: '
+    query_suffix: '.'
+    stop: ['# define', '# example']
+    maintain_session: False
+    debug_mode: True
+    include_context: True
+    has_return: True
+  fgen:
+    prompt_path: prompts/fgen.py
+    engine: gpt-4
+    max_tokens: 512
+    temperature: 0
+    query_prefix: '# define function: '
+    query_suffix: '.'
+    stop: ['# define', '# example']
+    maintain_session: False
+    debug_mode: False
+    include_context: True
+
+tabletop_coords:
+  top_left: [-0.25, -0.25]
+  top_side: [0, -0.25]
+  top_right: [0.25, -0.25]
+  left_side: [-0.25, -0.5]
+  middle: [0, -0.5]
+  right_side: [0.25, -0.5]
+  bottom_left: [-0.25, -0.75]
+  bottom_side: [0, -0.75]
+  bottom_right: [0.25, -0.75]
+  table_z: 0
\ No newline at end of file
diff --git a/consts.py b/consts.py
new file mode 100644
index 0000000000000000000000000000000000000000..1e894ff2b31740f59f66203cc1d377684386aaac
--- /dev/null
+++ b/consts.py
@@ -0,0 +1,33 @@
+import numpy as np
+
+# # Global constants: pick and place objects, colors, workspace bounds
+COLORS = {
+    'blue':   (78/255,  121/255, 167/255, 255/255),
+    'red':    (255/255,  87/255,  89/255, 255/255),
+    'green':  (89/255,  169/255,  79/255, 255/255),
+    'orange': (242/255, 142/255,  43/255, 255/255),
+    'yellow': (237/255, 201/255,  72/255, 255/255),
+    'purple': (176/255, 122/255, 161/255, 255/255),
+    'pink':   (255/255, 157/255, 167/255, 255/255),
+    'cyan':   (118/255, 183/255, 178/255, 255/255),
+    'brown':  (156/255, 117/255,  95/255, 255/255),
+    'gray':   (186/255, 176/255, 172/255, 255/255),
+}
+
+CORNER_POS = {
+  'top left corner':     (-0.3 + 0.05, -0.2 - 0.05, 0),
+  'top side':            (0,           -0.2 - 0.05, 0),
+  'top right corner':    (0.3 - 0.05,  -0.2 - 0.05, 0),
+  'left side':           (-0.3 + 0.05, -0.5,        0),
+  'middle':              (0,           -0.5,        0),
+  'right side':          (0.3 - 0.05,  -0.5,        0),
+  'bottom left corner':  (-0.3 + 0.05, -0.8 + 0.05, 0),
+  'bottom side':         (0,           -0.8 + 0.05, 0),
+  'bottom right corner': (0.3 - 0.05,  -0.8 + 0.05, 0),
+}
+
+ALL_BLOCKS = ['blue block', 'red block', 'green block', 'orange block', 'yellow block', 'purple block', 'pink block', 'cyan block', 'brown block', 'gray block']
+ALL_BOWLS = ['blue bowl', 'red bowl', 'green bowl', 'orange bowl', 'yellow bowl', 'purple bowl', 'pink bowl', 'cyan bowl', 'brown bowl', 'gray bowl']
+
+PIXEL_SIZE = 0.00267857
+BOUNDS = np.float32([[-0.3, 0.3], [-0.8, -0.2], [0, 0.15]])  # X Y Z
\ No newline at end of file
diff --git a/lmp.py b/lmp.py
new file mode 100644
index 0000000000000000000000000000000000000000..253c7dcba73fe3e44d15c2f45a3a1bb4b7a8e3f8
--- /dev/null
+++ b/lmp.py
@@ -0,0 +1,253 @@
+from time import sleep
+import ast
+import astunparse
+import openai
+from openai.error import RateLimitError, APIConnectionError
+from pygments import highlight
+from pygments.lexers import PythonLexer
+from pygments.formatters import TerminalFormatter
+
+from utils import retrieve_proxy, chat_completion_request, generate_messages
+
+class LMP:
+
+    def __init__(self, name, cfg, lmp_fgen, fixed_vars, variable_vars, md_logger):
+        self._name = name
+        self._cfg = cfg
+        self._md_logger = md_logger
+
+        with open(self._cfg['prompt_path'], 'r') as f:
+            self._base_prompt = f.read()
+
+        self._stop_tokens = list(self._cfg['stop'])
+
+        self._lmp_fgen = lmp_fgen
+
+        self._fixed_vars = fixed_vars
+        self._variable_vars = variable_vars
+        self.exec_hist = ''
+
+    def clear_exec_hist(self):
+        self.exec_hist = ''
+
+    def build_prompt(self, query, context=''):
+        if len(self._variable_vars) > 0:
+            variable_vars_imports_str = f"from utils import {', '.join(self._variable_vars.keys())}"
+        else:
+            variable_vars_imports_str = ''
+        prompt = self._base_prompt.replace('{variable_vars_imports}', variable_vars_imports_str)
+
+        if self._cfg['maintain_session']:
+            prompt += f'\n{self.exec_hist}'
+
+        if context != '':
+            prompt += f'\n{context}'
+
+        use_query = f'{self._cfg["query_prefix"]}{query}{self._cfg["query_suffix"]}'
+        prompt += f'\n{use_query}'
+
+        return prompt, use_query
+
+    def __call__(self, query, context='', **kwargs):
+        prompt, use_query = self.build_prompt(query, context=context)
+
+        response = chat_completion_request(
+            generate_messages(prompt),
+            # stop=self._stop_tokens,
+            temperature=self._cfg['temperature'],
+            model=self._cfg['model'],
+            max_tokens=self._cfg['max_tokens'],
+            proxy=openai.proxy,
+        )
+        code_str = response.json()["choices"][0]["message"]['content']
+
+        if self._cfg['include_context'] and context != '':
+            to_exec = f'{context}\n{code_str}'
+            to_log = f'{context}\n{use_query}\n{code_str}'
+        else:
+            to_exec = code_str
+            to_log = f'{use_query}\n{to_exec}'
+
+        to_log_pretty = highlight(to_log, PythonLexer(), TerminalFormatter())
+        print(f'LMP {self._name} generated code:\n{to_log_pretty}')
+        self._md_logger.log_text(f'LMP {self._name} Generated Code:')
+        self._md_logger.log_code(to_log)
+
+        new_fs = self._lmp_fgen.create_new_fs_from_code(code_str)
+        self._variable_vars.update(new_fs)
+
+        gvars = merge_dicts([self._fixed_vars, self._variable_vars])
+        lvars = kwargs
+
+        if not self._cfg['debug_mode']:
+            exec_safe(to_exec, gvars, lvars)
+
+        self.exec_hist += f'\n{to_exec}'
+
+        if self._cfg['maintain_session']:
+            self._variable_vars.update(lvars)
+
+        if self._cfg['has_return'] and not self._cfg['debug_mode']:
+            return lvars[self._cfg['return_val_name']]
+
+class LMPFGen:
+
+    def __init__(self, cfg, fixed_vars, variable_vars, md_logger):
+        self._cfg = cfg
+
+        self._stop_tokens = list(self._cfg['stop'])
+        self._fixed_vars = fixed_vars
+        self._variable_vars = variable_vars
+        self._md_logger = md_logger
+
+        with open(self._cfg['prompt_path'], 'r') as f:
+            self._base_prompt = f.read()
+
+    def create_f_from_sig(self, f_name, f_sig, other_vars=None, fix_bugs=False, return_src=False):
+        print(f'Creating function: {f_sig}')
+
+        use_query = f'{self._cfg["query_prefix"]}{f_sig}{self._cfg["query_suffix"]}'
+        prompt = f'{self._base_prompt}\n{use_query}'
+
+        response = chat_completion_request(
+            generate_messages(prompt),
+            temperature=self._cfg['temperature'],
+            model=self._cfg['model'],
+            max_tokens=self._cfg['max_tokens'],
+            proxy=openai.proxy,
+        )
+        f_src = response.json()["choices"][0]["message"]['content']
+
+        if fix_bugs:
+            f_src = openai.Edit.create(
+                model='code-davinci-edit-001',
+                input='# ' + f_src,
+                temperature=0,
+                instruction='Fix the bug if there is one. Improve readability. Keep same inputs and outputs. Only small changes. No comments.',
+            )['choices'][0]['text'].strip()
+
+        if other_vars is None:
+            other_vars = {}
+        gvars = merge_dicts([self._fixed_vars, self._variable_vars, other_vars])
+        lvars = {}
+        
+        exec_safe(f_src, gvars, lvars)
+
+        f = lvars[f_name]
+
+        to_print = f'{use_query}\n{f_src}'
+        to_print_pretty = highlight(to_print, PythonLexer(), TerminalFormatter())
+        print(f'LMPFGen generated code:\n{to_print_pretty}')
+        self._md_logger.log_text('Generated Function:')
+        self._md_logger.log_code(to_print)
+
+        if return_src:
+            return f, f_src
+        return f
+
+    def create_new_fs_from_code(self, code_str, other_vars=None, fix_bugs=False, return_src=False):
+        fs, f_assigns = {}, {}
+        f_parser = FunctionParser(fs, f_assigns)
+        f_parser.visit(ast.parse(code_str))
+        for f_name, f_assign in f_assigns.items():
+            if f_name in fs:
+                fs[f_name] = f_assign
+
+        if other_vars is None:
+            other_vars = {}
+
+        new_fs = {}
+        srcs = {}
+        for f_name, f_sig in fs.items():
+            all_vars = merge_dicts([self._fixed_vars, self._variable_vars, new_fs, other_vars])
+            if not var_exists(f_name, all_vars):
+                f, f_src = self.create_f_from_sig(f_name, f_sig, new_fs, fix_bugs=fix_bugs, return_src=True)
+
+                # recursively define child_fs in the function body if needed
+                f_def_body = astunparse.unparse(ast.parse(f_src).body[0].body)
+                child_fs, child_f_srcs = self.create_new_fs_from_code(
+                    f_def_body, other_vars=all_vars, fix_bugs=fix_bugs, return_src=True
+                )
+
+                if len(child_fs) > 0:
+                    new_fs.update(child_fs)
+                    srcs.update(child_f_srcs)
+
+                    # redefine parent f so newly created child_fs are in scope
+                    gvars = merge_dicts([self._fixed_vars, self._variable_vars, new_fs, other_vars])
+                    lvars = {}
+                    
+                    exec_safe(f_src, gvars, lvars)
+                    
+                    f = lvars[f_name]
+
+                new_fs[f_name], srcs[f_name] = f, f_src
+
+        if return_src:
+            return new_fs, srcs
+        return new_fs
+
+
+class FunctionParser(ast.NodeTransformer):
+
+    def __init__(self, fs, f_assigns):
+      super().__init__()
+      self._fs = fs
+      self._f_assigns = f_assigns
+
+    def visit_Call(self, node):
+        self.generic_visit(node)
+        if isinstance(node.func, ast.Name):
+            f_sig = astunparse.unparse(node).strip()
+            f_name = astunparse.unparse(node.func).strip()
+            self._fs[f_name] = f_sig
+        return node
+
+    def visit_Assign(self, node):
+        self.generic_visit(node)
+        if isinstance(node.value, ast.Call):
+            assign_str = astunparse.unparse(node).strip()
+            f_name = astunparse.unparse(node.value.func).strip()
+            self._f_assigns[f_name] = assign_str
+        return node
+
+
+def var_exists(name, all_vars):
+    try:
+        eval(name, all_vars)
+    except:
+        exists = False
+    else:
+        exists = True
+    return exists
+
+
+def merge_dicts(dicts):
+    return {
+        k : v 
+        for d in dicts
+        for k, v in d.items()
+    }
+
+
+import traceback
+def exec_safe(code_str, gvars=None, lvars=None):
+    banned_phrases = ['import', '__']
+    for phrase in banned_phrases:
+        assert phrase not in code_str
+  
+    if gvars is None:
+        gvars = {}
+    if lvars is None:
+        lvars = {}
+    empty_fn = lambda *args, **kwargs: None
+    custom_gvars = merge_dicts([
+        gvars,
+        {'exec': empty_fn, 'eval': empty_fn}
+    ])
+
+    try:
+        exec(code_str, custom_gvars, lvars)
+    except Exception as e:
+        traceback.print_exc()
+    # exec(code_str, custom_gvars, lvars)
\ No newline at end of file
diff --git a/md_logger.py b/md_logger.py
new file mode 100644
index 0000000000000000000000000000000000000000..5e496c5d3200381a7bacfd4fc1456bbc9541b0a7
--- /dev/null
+++ b/md_logger.py
@@ -0,0 +1,16 @@
+class MarkdownLogger:
+
+    def __init__(self):
+        self._log = ''
+
+    def log_text(self, text):
+        self._log += '\n' + text + '\n'
+    
+    def log_code(self, code):
+        self._log += f'\n```python\n{code}\n```\n'
+
+    def clear(self):
+        self._log = ''
+
+    def get_log(self):
+        return self._log
\ No newline at end of file
diff --git a/prompts/fgen.py b/prompts/fgen.py
new file mode 100644
index 0000000000000000000000000000000000000000..e3623264cf6bc506e1ce350c2379bc264390887a
--- /dev/null
+++ b/prompts/fgen.py
@@ -0,0 +1,49 @@
+import numpy as np
+from shapely.geometry import *
+from shapely.affinity import *
+
+from env_utils import get_obj_pos, get_obj_names
+from ctrl_utils import put_first_on_second
+
+# define function: total = get_total(xs=numbers).
+def get_total(xs):
+    return np.sum(xs)
+
+# define function: y = eval_line(x, slope, y_intercept=0).
+def eval_line(x, slope, y_intercept):
+    return x * slope + y_intercept
+
+# define function: pt = get_pt_to_the_left(pt, dist).
+def get_pt_to_the_left(pt, dist):
+    return pt + [-dist, 0]
+
+# define function: pt = get_pt_to_the_top(pt, dist).
+def get_pt_to_the_top(pt, dist):
+    return pt + [0, dist]
+
+# define function line = make_line_by_length(length=x).
+def make_line_by_length(length):
+  line = LineString([[0, 0], [length, 0]])
+  return line
+
+# define function: line = make_vertical_line_by_length(length=x).
+def make_vertical_line_by_length(length):
+  line = make_line_by_length(length)
+  vertical_line = rotate(line, 90)
+  return vertical_line
+
+# define function: pt = interpolate_line(line, t=0.5).
+def interpolate_line(line, t):
+  pt = line.interpolate(t, normalized=True)
+  return np.array(pt.coords[0])
+
+# example: scale a line by 2.
+line = make_line_by_length(1)
+new_shape = scale(line, xfact=2, yfact=2)
+
+# example: put object1 on top of object0.
+put_first_on_second('object1', 'object0')
+
+# example: get the position of the first object.
+obj_names = get_obj_names()
+pos_2d = get_obj_pos(obj_names[0])
\ No newline at end of file
diff --git a/prompts/parse_obj_name.py b/prompts/parse_obj_name.py
new file mode 100644
index 0000000000000000000000000000000000000000..536be8ab1e6c1f473b4e592da23f3b4951627080
--- /dev/null
+++ b/prompts/parse_obj_name.py
@@ -0,0 +1,59 @@
+import numpy as np
+from env_utils import get_obj_pos, parse_position
+from utils import get_obj_positions_np
+
+objects = ['blue block', 'cyan block', 'purple bowl', 'gray bowl', 'brown bowl', 'pink block', 'purple block']
+# the block closest to the purple bowl.
+block_names = ['blue block', 'cyan block', 'purple block']
+closest_block_idx = get_closest_idx(points=get_obj_positions_np(block_names), point=get_obj_pos('purple bowl'))
+closest_block_name = block_names[closest_block_idx]
+ret_val = closest_block_name
+objects = ['brown bowl', 'banana', 'brown block', 'apple', 'blue bowl', 'blue block']
+# the blocks.
+ret_val = ['brown block', 'blue block']
+objects = ['brown bowl', 'banana', 'brown block', 'apple', 'blue bowl', 'blue block']
+# the brown objects.
+ret_val = ['brown bowl', 'brown block']
+objects = ['brown bowl', 'banana', 'brown block', 'apple', 'blue bowl', 'blue block']
+# a fruit that's not the apple
+fruit_names = ['banana', 'apple']
+for fruit_name in fruit_names:
+    if fruit_name != 'apple':
+        ret_val = fruit_name
+objects = ['blue block', 'cyan block', 'purple bowl', 'brown bowl', 'purple block']
+# blocks above the brown bowl.
+block_names = ['blue block', 'cyan block', 'purple block']
+brown_bowl_pos = get_obj_pos('brown bowl')
+use_block_names = []
+for block_name in block_names:
+    if get_obj_pos(block_name)[1] > brown_bowl_pos[1]:
+        use_block_names.append(block_name)
+ret_val = use_block_names
+objects = ['blue block', 'cyan block', 'purple bowl', 'brown bowl', 'purple block']
+# the blue block.
+ret_val = 'blue block'
+objects = ['blue block', 'cyan block', 'purple bowl', 'brown bowl', 'purple block']
+# the block closest to the bottom right corner.
+corner_pos = parse_position('bottom right corner')
+block_names = ['blue block', 'cyan block', 'purple block']
+closest_block_idx = get_closest_idx(points=get_obj_positions_np(block_names), point=corner_pos)
+closest_block_name = block_names[closest_block_idx]
+ret_val = closest_block_name
+objects = ['brown bowl', 'green block', 'brown block', 'green bowl', 'blue bowl', 'blue block']
+# the left most block.
+block_names = ['green block', 'brown block', 'blue block']
+left_block_idx = np.argsort(get_obj_positions_np(block_names)[:, 0])[0]
+left_block_name = block_names[left_block_idx]
+ret_val = left_block_name
+objects = ['brown bowl', 'green block', 'brown block', 'green bowl', 'blue bowl', 'blue block']
+# the bowl on near the top.
+bowl_names = ['brown bowl', 'green bowl', 'blue bowl']
+top_bowl_idx = np.argsort(get_obj_positions_np(bowl_names)[:, 1])[-1]
+top_bowl_name = bowl_names[top_bowl_idx]
+ret_val = top_bowl_name
+objects = ['yellow bowl', 'purple block', 'yellow block', 'purple bowl', 'pink bowl', 'pink block']
+# the third bowl from the right.
+bowl_names = ['yellow bowl', 'purple bowl', 'pink bowl']
+bowl_idx = np.argsort(get_obj_positions_np(bowl_names)[:, 0])[-3]
+bowl_name = bowl_names[bowl_idx]
+ret_val = bowl_name
\ No newline at end of file
diff --git a/prompts/parse_position.py b/prompts/parse_position.py
new file mode 100644
index 0000000000000000000000000000000000000000..f88e8eeb69ddd2605e9ee5275115a917b2387e3f
--- /dev/null
+++ b/prompts/parse_position.py
@@ -0,0 +1,54 @@
+import numpy as np
+from shapely.geometry import *
+from shapely.affinity import *
+from env_utils import denormalize_xy, parse_obj_name, get_obj_names, get_obj_pos
+
+# a 30cm horizontal line in the middle with 3 points.
+middle_pos = denormalize_xy([0.5, 0.5]) 
+start_pos = middle_pos + [-0.3/2, 0]
+end_pos = middle_pos + [0.3/2, 0]
+line = make_line(start=start_pos, end=end_pos)
+points = interpolate_pts_on_line(line=line, n=3)
+ret_val = points
+# a 20cm vertical line near the right with 4 points.
+middle_pos = denormalize_xy([1, 0.5]) 
+start_pos = middle_pos + [0, -0.2/2]
+end_pos = middle_pos + [0, 0.2/2]
+line = make_line(start=start_pos, end=end_pos)
+points = interpolate_pts_on_line(line=line, n=4)
+ret_val = points
+# a diagonal line from the top left to the bottom right corner with 5 points.
+top_left_corner = denormalize_xy([0, 1])
+bottom_right_corner = denormalize_xy([1, 0])
+line = make_line(start=top_left_corner, end=bottom_right_corner)
+points = interpolate_pts_on_line(line=line, n=5)
+ret_val = points
+# a triangle with size 10cm with 3 points.
+polygon = make_triangle(size=0.1, center=denormalize_xy([0.5, 0.5]))
+points = get_points_from_polygon(polygon)
+ret_val = points
+# the corner closest to the sun colored block.
+block_name = parse_obj_name('the sun colored block', f'objects = {get_obj_names()}')
+corner_positions = np.array([denormalize_xy(pos) for pos in [[0, 0], [0, 1], [1, 1], [1, 0]]])
+closest_corner_pos = get_closest_point(points=corner_positions, point=get_obj_pos(block_name))
+ret_val = closest_corner_pos
+# the side farthest from the right most bowl.
+bowl_name = parse_obj_name('the right most bowl', f'objects = {get_obj_names()}')
+side_positions = np.array([denormalize_xy(pos) for pos in [[0.5, 0], [0.5, 1], [1, 0.5], [0, 0.5]]])
+farthest_side_pos = get_farthest_point(points=side_positions, point=get_obj_pos(bowl_name))
+ret_val = farthest_side_pos
+# a point above the third block from the bottom.
+block_name = parse_obj_name('the third block from the bottom', f'objects = {get_obj_names()}')
+ret_val = get_obj_pos(block_name) + [0.1, 0]
+# a point 10cm left of the bowls.
+bowl_names = parse_obj_name('the bowls', f'objects = {get_obj_names()}')
+bowl_positions = get_all_object_positions_np(obj_names=bowl_names)
+left_obj_pos = bowl_positions[np.argmin(bowl_positions[:, 0])] + [-0.1, 0]
+ret_val = left_obj_pos
+# the bottom side.
+bottom_pos = denormalize_xy([0.5, 0])
+ret_val = bottom_pos
+# the top corners.
+top_left_pos = denormalize_xy([0, 1])
+top_right_pos = denormalize_xy([1, 1])
+ret_val = [top_left_pos, top_right_pos]
\ No newline at end of file
diff --git a/prompts/parse_question.py b/prompts/parse_question.py
new file mode 100644
index 0000000000000000000000000000000000000000..bbc2332dc1dd7ef924adcd3608accf63a2c1db02
--- /dev/null
+++ b/prompts/parse_question.py
@@ -0,0 +1,27 @@
+from utils import get_obj_pos, get_obj_names, parse_obj_name, bbox_contains_pt, is_obj_visible
+
+objects = ['yellow bowl', 'blue block', 'yellow block', 'blue bowl', 'fruit', 'green block', 'black bowl']
+# is the blue block to the right of the yellow bowl?
+ret_val = get_obj_pos('blue block')[0] > get_obj_pos('yellow bowl')[0]
+objects = ['yellow bowl', 'blue block', 'yellow block', 'blue bowl', 'fruit', 'green block', 'black bowl']
+# how many yellow objects are there?
+yellow_object_names = parse_obj_name('the yellow objects', f'objects = {get_obj_names()}')
+ret_val = len(yellow_object_names)
+objects = ['pink block', 'green block', 'pink bowl', 'blue block', 'blue bowl', 'green bowl']
+# what are the blocks left of the green bowl?
+block_names = parse_obj_name('the blocks', f'objects = {get_obj_names()}')
+green_bowl_pos = get_obj_pos('green bowl')
+left_block_names = []
+for block_name in block_names:
+  if get_obj_pos(block_name)[0] < green_bowl_pos[0]:
+    left_block_names.append(block_name)
+ret_val = left_block_names
+objects = ['pink block', 'yellow block', 'pink bowl', 'blue block', 'blue bowl', 'yellow bowl']
+# is the sun colored block above the blue bowl?
+sun_block_name = parse_obj_name('sun colored block', f'objects = {get_obj_names()}')
+sun_block_pos = get_obj_pos(sun_block_name)
+blue_bowl_pos = get_obj_pos('blue bowl')
+ret_val = sun_block_pos[1] > blue_bowl_pos[1]
+objects = ['pink block', 'yellow block', 'pink bowl', 'blue block', 'blue bowl', 'yellow bowl']
+# is the green block below the blue bowl?
+ret_val = get_obj_pos('green block')[1] < get_obj_pos('blue bowl')[1]
\ No newline at end of file
diff --git a/prompts/tabletop_ui.py b/prompts/tabletop_ui.py
new file mode 100644
index 0000000000000000000000000000000000000000..ae654730a480720f0995a14c5fd891156db78a44
--- /dev/null
+++ b/prompts/tabletop_ui.py
@@ -0,0 +1,173 @@
+# Python 2D robot control script
+import numpy as np
+from env_utils import put_first_on_second, get_obj_pos, get_obj_names, say, get_corner_name, get_side_name, is_obj_visible, stack_objects_in_order
+from plan_utils import parse_obj_name, parse_position, parse_question, transform_shape_pts
+
+objects = ['yellow block', 'green block', 'yellow bowl', 'blue block', 'blue bowl', 'green bowl']
+# place the yellow block on the yellow bowl.
+say('Ok - putting the yellow block on the yellow bowl')
+put_first_on_second('yellow block', 'yellow bowl')
+objects = ['yellow block', 'green block', 'yellow bowl', 'blue block', 'blue bowl', 'green bowl']
+# which block did you move.
+say('I moved the yellow block')
+objects = ['yellow block', 'green block', 'yellow bowl', 'blue block', 'blue bowl', 'green bowl']
+# move the green block to the top right corner.
+say('Got it - putting the green block on the top right corner')
+corner_pos = parse_position('top right corner')
+put_first_on_second('green block', corner_pos)
+objects = ['yellow block', 'green block', 'yellow bowl', 'blue block', 'blue bowl', 'green bowl']
+# stack the blue bowl on the yellow bowl on the green block.
+order_bottom_to_top = ['green block', 'yellow block', 'blue bowl']
+say(f'Sure - stacking from top to bottom: {", ".join(order_bottom_to_top)}')
+stack_objects_in_order(object_names=order_bottom_to_top)
+objects = ['cyan block', 'white block', 'cyan bowl', 'blue block', 'blue bowl', 'white bowl']
+# move the cyan block into its corresponding bowl.
+matches = {'cyan block': 'cyan bowl'}
+say('Got it - placing the cyan block on the cyan bowl')
+for first, second in matches.items():
+  put_first_on_second(first, get_obj_pos(second))
+objects = ['cyan block', 'white block', 'cyan bowl', 'blue block', 'blue bowl', 'white bowl']
+# make a line of blocks on the right side.
+say('No problem! Making a line of blocks on the right side')
+block_names = parse_obj_name('the blocks', f'objects = {get_obj_names()}')
+line_pts = parse_position(f'a 30cm vertical line on the right with {len(block_names)} points')
+for block_name, pt in zip(block_names, line_pts):
+  put_first_on_second(block_name, pt)
+objects = ['yellow block', 'red block', 'yellow bowl', 'gray block', 'gray bowl', 'red bowl']
+# put the small banana colored thing in between the blue bowl and green block.
+say('Sure thing - putting the yellow block between the blue bowl and the green block')
+target_pos = parse_position('a point in the middle betweeen the blue bowl and the green block')
+put_first_on_second('yellow block', target_pos)
+objects = ['yellow block', 'red block', 'yellow bowl', 'gray block', 'gray bowl', 'red bowl']
+# can you cut the bowls in half.
+say('no, I can only move objects around')
+objects = ['yellow block', 'green block', 'yellow bowl', 'gray block', 'gray bowl', 'green bowl']
+# stack the blocks on the right side with the gray one on the bottom.
+say('Ok. stacking the blocks on the right side with the gray block on the bottom')
+right_side = parse_position('the right side')
+put_first_on_second('gray block', right_side)
+order_bottom_to_top = ['gray block', 'green block', 'yellow block']
+stack_objects_in_order(object_names=order_bottom_to_top)
+objects = ['yellow block', 'green block', 'yellow bowl', 'blue block', 'blue bowl', 'green bowl']
+# hide the blue bowl.
+bowl_name = np.random.choice(['yellow bowl', 'green bowl'])
+say(f'Sounds good! Hiding the blue bowl under the {bowl_name}')
+put_first_on_second(bowl_name, 'blue bowl')
+objects = ['pink block', 'green block', 'pink bowl', 'blue block', 'blue bowl', 'green bowl']
+# move the grass-colored bowl to the left.
+say('Sure - moving the green bowl left by 10 centimeters')
+left_pos = parse_position('a point 10cm left of the green bowl')
+put_first_on_second('green bowl', left_pos)
+objects = ['pink block', 'green block', 'pink bowl', 'blue block', 'blue bowl', 'green bowl']
+# why did you move the red bowl.
+say(f'I did not move the red bowl')
+objects = ['pink block', 'green block', 'pink bowl', 'blue block', 'blue bowl', 'green bowl']
+# undo that.
+say('Sure - moving the green bowl right by 10 centimeters')
+left_pos = parse_position('a point 10cm right of the green bowl')
+put_first_on_second('green bowl', left_pos)
+objects = ['brown bowl', 'green block', 'brown block', 'green bowl', 'blue bowl', 'blue block']
+# place the top most block to the corner closest to the bottom most block.
+top_block_name = parse_obj_name('top most block', f'objects = {get_obj_names()}')
+bottom_block_name = parse_obj_name('bottom most block', f'objects = {get_obj_names()}')
+closest_corner_pos = parse_position(f'the corner closest to the {bottom_block_name}', f'objects = {get_obj_names()}')
+say(f'Putting the {top_block_name} on the {get_corner_name(closest_corner_pos)}')
+put_first_on_second(top_block_name, closest_corner_pos)
+objects = ['brown bowl', 'green block', 'brown block', 'green bowl', 'blue bowl', 'blue block']
+# move the brown bowl to the side closest to the green block.
+closest_side_position = parse_position('the side closest to the green block')
+say(f'Got it - putting the brown bowl on the {get_side_name(closest_side_position)}')
+put_first_on_second('brown bowl', closest_side_position)
+objects = ['brown bowl', 'green block', 'brown block', 'green bowl', 'blue bowl', 'blue block']
+# place the green block to the right of the bowl that has the blue block.
+bowl_name = parse_obj_name('the bowl that has the blue block', f'objects = {get_obj_names()}')
+if bowl_name:
+  target_pos = parse_position(f'a point 10cm to the right of the {bowl_name}')
+  say(f'No problem - placing the green block to the right of the {bowl_name}')
+  put_first_on_second('green block', target_pos)
+else:
+  say('There are no bowls that has the blue block')
+objects = ['brown bowl', 'green block', 'brown block', 'green bowl', 'blue bowl', 'blue block']
+# move the other blocks to the bottom corners.
+block_names = parse_obj_name('blocks other than the blue block', f'objects = {get_obj_names()}')
+corners = parse_position('the bottom corners')
+for block_name, pos in zip(block_names, corners):
+  put_first_on_second(block_name, pos)
+objects = ['pink block', 'gray block', 'orange block']
+# move the pinkish colored block on the bottom side.
+say('Ok - putting the pink block on the bottom side')
+bottom_side_pos = parse_position('the bottom side')
+put_first_on_second('pink block', bottom_side_pos)
+objects = ['yellow bowl', 'blue block', 'yellow block', 'blue bowl']
+# is the blue block to the right of the yellow bowl?
+if parse_question('is the blue block to the right of the yellow bowl?', f'objects = {get_obj_names()}'):
+  say('yes, there is a blue block to the right of the yellow bow')
+else:
+  say('no, there is\'t a blue block to the right of the yellow bow')
+objects = ['yellow bowl', 'blue block', 'yellow block', 'blue bowl']
+# how many yellow objects are there?
+n_yellow_objs = parse_question('how many yellow objects are there', f'objects = {get_obj_names()}')
+say(f'there are {n_yellow_objs} yellow object')
+objects = ['pink block', 'green block', 'pink bowl', 'blue block', 'blue bowl', 'green bowl']
+# move the left most block to the green bowl.
+left_block_name = parse_obj_name('left most block', f'objects = {get_obj_names()}')
+say(f'Moving the {left_block_name} on the green bowl')
+put_first_on_second(left_block_name, 'green bowl')
+objects = ['pink block', 'green block', 'pink bowl', 'blue block', 'blue bowl', 'green bowl']
+# move the other blocks to different corners.
+block_names = parse_obj_name(f'blocks other than the {left_block_name}', f'objects = {get_obj_names()}')
+corners = parse_position('the corners')
+say(f'Ok - moving the other {len(block_names)} blocks to different corners')
+for block_name, pos in zip(block_names, corners):
+  put_first_on_second(block_name, pos)
+objects = ['pink block', 'green block', 'pink bowl', 'blue block', 'blue bowl', 'green bowl']
+# is the pink block on the green bowl.
+if parse_question('is the pink block on the green bowl', f'objects = {get_obj_names()}'):
+  say('Yes - the pink block is on the green bowl.')
+else:
+  say('No - the pink block is not on the green bowl.')
+objects = ['pink block', 'green block', 'pink bowl', 'blue block', 'blue bowl', 'green bowl']
+# what are the blocks left of the green bowl.
+left_block_names =  parse_question('what are the blocks left of the green bowl', f'objects = {get_obj_names()}')
+if len(left_block_names) > 0:
+  say(f'These blocks are left of the green bowl: {", ".join(left_block_names)}')
+else:
+  say('There are no blocks left of the green bowl')
+objects = ['yellow block', 'green block', 'yellow bowl', 'blue block', 'blue bowl', 'green bowl']
+# imagine that the bowls are different biomes on earth and imagine that the blocks are parts of a building.
+say('ok')
+objects = ['yellow block', 'green block', 'yellow bowl', 'blue block', 'blue bowl', 'green bowl']
+# now build a tower in the grasslands.
+order_bottom_to_top = ['green bowl', 'blue block', 'green block', 'yellow block']
+say('stacking the blocks on the green bowl')
+stack_objects_in_order(object_names=order_bottom_to_top)
+objects = ['yellow block', 'green block', 'yellow bowl', 'gray block', 'gray bowl', 'green bowl']
+# show me what happens when the desert gets flooded by the ocean.
+say('putting the yellow bowl on the blue bowl')
+put_first_on_second('yellow bowl', 'blue bowl')
+objects = ['pink block', 'gray block', 'orange block']
+# move all blocks 5cm toward the top.
+say('Ok - moving all blocks 5cm toward the top')
+block_names = parse_obj_name('the blocks', f'objects = {get_obj_names()}')
+for block_name in block_names:
+  target_pos = parse_position(f'a point 5cm above the {block_name}')
+  put_first_on_second(block_name, target_pos)
+objects = ['cyan block', 'white block', 'purple bowl', 'blue block', 'blue bowl', 'white bowl']
+# make a triangle of blocks in the middle.
+block_names = parse_obj_name('the blocks', f'objects = {get_obj_names()}')
+triangle_pts = parse_position(f'a triangle with size 10cm around the middle with {len(block_names)} points')
+say('Making a triangle of blocks around the middle of the workspace')
+for block_name, pt in zip(block_names, triangle_pts):
+  put_first_on_second(block_name, pt)
+objects = ['cyan block', 'white block', 'purple bowl', 'blue block', 'blue bowl', 'white bowl']
+# make the triangle smaller.
+triangle_pts = transform_shape_pts('scale it by 0.5x', shape_pts=triangle_pts)
+say('Making the triangle smaller')
+block_names = parse_obj_name('the blocks', f'objects = {get_obj_names()}')
+for block_name, pt in zip(block_names, triangle_pts):
+  put_first_on_second(block_name, pt)
+objects = ['brown bowl', 'red block', 'brown block', 'red bowl', 'pink bowl', 'pink block']
+# put the red block on the farthest bowl.
+farthest_bowl_name = parse_obj_name('the bowl farthest from the red block', f'objects = {get_obj_names()}')
+say(f'Putting the red block on the {farthest_bowl_name}')
+put_first_on_second('red block', farthest_bowl_name)
\ No newline at end of file
diff --git a/prompts/transform_shape_pts.py b/prompts/transform_shape_pts.py
new file mode 100644
index 0000000000000000000000000000000000000000..e5c74b856805818dc788de1df4b0495d9cc404ba
--- /dev/null
+++ b/prompts/transform_shape_pts.py
@@ -0,0 +1,19 @@
+import numpy as np
+from utils import get_obj_pos, get_obj_names, parse_position, parse_obj_name
+
+# make it bigger by 1.5.
+new_shape_pts = scale_pts_around_centroid_np(shape_pts, scale_x=1.5, scale_y=1.5)
+# move it to the right by 10cm.
+new_shape_pts = translate_pts_np(shape_pts, delta=[0.1, 0])
+# move it to the top by 20cm.
+new_shape_pts = translate_pts_np(shape_pts, delta=[0, 0.2])
+# rotate it clockwise by 40 degrees.
+new_shape_pts = rotate_pts_around_centroid_np(shape_pts, angle=-np.deg2rad(40))
+# rotate by 30 degrees and make it slightly smaller
+new_shape_pts = rotate_pts_around_centroid_np(shape_pts, angle=np.deg2rad(30))
+new_shape_pts = scale_pts_around_centroid_np(new_shape_pts, scale_x=0.7, scale_y=0.7)
+# move it toward the blue block.
+block_name = parse_obj_name('the blue block', f'objects = {get_obj_names()}')
+block_pos = get_obj_pos(block_name)
+mean_delta = np.mean(block_pos - shape_pts, axis=1)
+new_shape_pts = translate_pts_np(shape_pts, mean_delta)
\ No newline at end of file
diff --git a/requirements.txt b/requirements.txt
new file mode 100644
index 0000000000000000000000000000000000000000..737d4c35a19d07bfa07bfd24beba0da0f1b2e3b4
--- /dev/null
+++ b/requirements.txt
@@ -0,0 +1,11 @@
+numpy
+scipy
+shapely
+astunparse
+pygments
+openai
+pybullet
+imageio==2.4.1
+imageio-ffmpeg
+moviepy
+omegaconf
\ No newline at end of file
diff --git a/robotiq_2f_85/README.md b/robotiq_2f_85/README.md
new file mode 100644
index 0000000000000000000000000000000000000000..82cbae156ff1b2cdf7e4d0858bb4f3e7092cf7a3
--- /dev/null
+++ b/robotiq_2f_85/README.md
@@ -0,0 +1,52 @@
+## Robotiq 2F 85 gripper
+For this gripper, the following Github repo can be used as a reference: https://github.com/Shreeyak/robotiq.git
+
+### mimic tag in URDF
+This gripper is developed for ROS and uses the `mimic` tag within the URDF files to make the gripper move. From our research `mimic` tag within URDF is not supported by pybullet. To overcome this, one can use the `createConstraint` function. Please refer to [this](https://github.com/bulletphysics/bullet3/blob/master/examples/pybullet/examples/mimicJointConstraint.py) example from the bullet3 repo to see how to replicate a `mimic` joint:
+
+```python
+#a mimic joint can act as a gear between two joints
+#you can control the gear ratio in magnitude and sign (>0 reverses direction)
+
+import pybullet as p
+import time
+p.connect(p.GUI)
+p.loadURDF("plane.urdf",0,0,-2)
+wheelA = p.loadURDF("differential/diff_ring.urdf",[0,0,0])
+for i in range(p.getNumJoints(wheelA)):
+	print(p.getJointInfo(wheelA,i))
+	p.setJointMotorControl2(wheelA,i,p.VELOCITY_CONTROL,targetVelocity=0,force=0)
+
+
+c = p.createConstraint(wheelA,1,wheelA,3,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0])
+p.changeConstraint(c,gearRatio=1, maxForce=10000)
+
+c = p.createConstraint(wheelA,2,wheelA,4,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0])
+p.changeConstraint(c,gearRatio=-1, maxForce=10000)
+
+c = p.createConstraint(wheelA,1,wheelA,4,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0])
+p.changeConstraint(c,gearRatio=-1, maxForce=10000)
+
+
+p.setRealTimeSimulation(1)
+while(1):
+	p.setGravity(0,0,-10)
+	time.sleep(0.01)
+#p.removeConstraint(c)
+
+```
+
+
+Details on `createConstraint` can be found in the pybullet [getting started](https://docs.google.com/document/d/10sXEhzFRSnvFcl3XxNGhnD4N2SedqwdAvK3dsihxVUA/edit#heading=h.fq749wu22x4c) guide.
+
+### Files in folder
+Since parameters like gear ratio and direction are required, one can find the `robotiq_2f_85_mimic_joints.urdf` which contains the mimic tags as in original URDF, which can be used as a reference. It was generated from `robotiq/robotiq_2f_robot/robot/simple_rq2f85_pybullet.urdf.xacro` as so:
+```
+rosrun xacro xacro --inorder simple_rq2f85_pybullet.urdf.xacro
+adaptive_transmission:="true" > robotiq_2f_85_mimic_joints.urdf
+```
+
+The URDF meant for use in pybullet is `robotiq_2f_85.urdf` and it is generated in a similar manner as above by running:
+```
+rosrun xacro xacro --inorder simple_rq2f85_pybullet.urdf.xacro > robotiq_2f_85.urdf
+```
\ No newline at end of file
diff --git a/robotiq_2f_85/robotiq-2f-base.mtl b/robotiq_2f_85/robotiq-2f-base.mtl
new file mode 100644
index 0000000000000000000000000000000000000000..f0fcf076182067d8e318cee9def59956cf425071
--- /dev/null
+++ b/robotiq_2f_85/robotiq-2f-base.mtl
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:b3a0877bfbfe9688f766e6e8850acddd6ac02906b787a5595a9e809cdb82bb63
+size 280
diff --git a/robotiq_2f_85/robotiq-2f-base.obj b/robotiq_2f_85/robotiq-2f-base.obj
new file mode 100644
index 0000000000000000000000000000000000000000..9cdda5f7d10db92dcf5ec0cb0697bbf3a2fc9bdc
--- /dev/null
+++ b/robotiq_2f_85/robotiq-2f-base.obj
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:ec7d532c8901680af2d4aafe1d6584057c7eb197989b106c1c5dcd3d596c83e6
+size 3025788
diff --git a/robotiq_2f_85/robotiq-2f-base.stl b/robotiq_2f_85/robotiq-2f-base.stl
new file mode 100644
index 0000000000000000000000000000000000000000..22a9c2b3b06d71c5a82d63792b08c55db9d348e3
--- /dev/null
+++ b/robotiq_2f_85/robotiq-2f-base.stl
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:1c7b9f2bd92d705fc4e897c94e905973a3c05f406845e942229433deb7041453
+size 1712484
diff --git a/robotiq_2f_85/robotiq-2f-coupler.mtl b/robotiq_2f_85/robotiq-2f-coupler.mtl
new file mode 100644
index 0000000000000000000000000000000000000000..f0fcf076182067d8e318cee9def59956cf425071
--- /dev/null
+++ b/robotiq_2f_85/robotiq-2f-coupler.mtl
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:b3a0877bfbfe9688f766e6e8850acddd6ac02906b787a5595a9e809cdb82bb63
+size 280
diff --git a/robotiq_2f_85/robotiq-2f-coupler.obj b/robotiq_2f_85/robotiq-2f-coupler.obj
new file mode 100644
index 0000000000000000000000000000000000000000..0c9f535e064990c1a973dfdfb476bfb8d9e899e0
--- /dev/null
+++ b/robotiq_2f_85/robotiq-2f-coupler.obj
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:5da95d0e14d00beae1c59840633189a1f716f4c5abdc0c690cd36d256aedbff6
+size 134560
diff --git a/robotiq_2f_85/robotiq-2f-coupler.stl b/robotiq_2f_85/robotiq-2f-coupler.stl
new file mode 100644
index 0000000000000000000000000000000000000000..cbfd80ed74fce7a506dad27909a007a2b24aaf7e
--- /dev/null
+++ b/robotiq_2f_85/robotiq-2f-coupler.stl
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:d5ee95e62f8415bf5b6e503c831a958f5fc1990bf9b2865329ec38a28932727c
+size 89084
diff --git a/robotiq_2f_85/robotiq-2f-driver.mtl b/robotiq_2f_85/robotiq-2f-driver.mtl
new file mode 100644
index 0000000000000000000000000000000000000000..f0fcf076182067d8e318cee9def59956cf425071
--- /dev/null
+++ b/robotiq_2f_85/robotiq-2f-driver.mtl
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:b3a0877bfbfe9688f766e6e8850acddd6ac02906b787a5595a9e809cdb82bb63
+size 280
diff --git a/robotiq_2f_85/robotiq-2f-driver.obj b/robotiq_2f_85/robotiq-2f-driver.obj
new file mode 100644
index 0000000000000000000000000000000000000000..34d7fdd03db984d4b39c397c1e557c7ecca9c4f1
--- /dev/null
+++ b/robotiq_2f_85/robotiq-2f-driver.obj
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:d29f643a8083cc71bf67287dd27ce5316ce7b5908d6f613e28cb50b6439e0319
+size 106449
diff --git a/robotiq_2f_85/robotiq-2f-driver.stl b/robotiq_2f_85/robotiq-2f-driver.stl
new file mode 100644
index 0000000000000000000000000000000000000000..0b3f90a5c5e4d4fc856d90ac6672a3e56a96f649
--- /dev/null
+++ b/robotiq_2f_85/robotiq-2f-driver.stl
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:bbd6e868b1778bead60d2516c7ede581d7ad4e431744b1828757d6ea5129c112
+size 67084
diff --git a/robotiq_2f_85/robotiq-2f-follower.mtl b/robotiq_2f_85/robotiq-2f-follower.mtl
new file mode 100644
index 0000000000000000000000000000000000000000..f0fcf076182067d8e318cee9def59956cf425071
--- /dev/null
+++ b/robotiq_2f_85/robotiq-2f-follower.mtl
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:b3a0877bfbfe9688f766e6e8850acddd6ac02906b787a5595a9e809cdb82bb63
+size 280
diff --git a/robotiq_2f_85/robotiq-2f-follower.obj b/robotiq_2f_85/robotiq-2f-follower.obj
new file mode 100644
index 0000000000000000000000000000000000000000..9ce0e3bf5e79c2c5d9d95825bfa943ba679ef1d5
--- /dev/null
+++ b/robotiq_2f_85/robotiq-2f-follower.obj
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:df51e9e7c786ae41681b16378f3558e565165fa36eea0f1e2516ca6555b58c38
+size 181419
diff --git a/robotiq_2f_85/robotiq-2f-follower.stl b/robotiq_2f_85/robotiq-2f-follower.stl
new file mode 100644
index 0000000000000000000000000000000000000000..9614b95e154d5b297eb390fc822c35833a55ccec
--- /dev/null
+++ b/robotiq_2f_85/robotiq-2f-follower.stl
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:633793332c081641ce200df27a40643cc293b29956e3cb5cb29cb811c33ef1c7
+size 110484
diff --git a/robotiq_2f_85/robotiq-2f-pad.stl b/robotiq_2f_85/robotiq-2f-pad.stl
new file mode 100644
index 0000000000000000000000000000000000000000..5b482f5b9627bce0465aab78d92ce7e37acae35b
--- /dev/null
+++ b/robotiq_2f_85/robotiq-2f-pad.stl
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:6c4e73a07a7d6853777450d0019691d2d43f50c9fa0bd6b0189d4af164a0ce4b
+size 684
diff --git a/robotiq_2f_85/robotiq-2f-spring_link.mtl b/robotiq_2f_85/robotiq-2f-spring_link.mtl
new file mode 100644
index 0000000000000000000000000000000000000000..f0fcf076182067d8e318cee9def59956cf425071
--- /dev/null
+++ b/robotiq_2f_85/robotiq-2f-spring_link.mtl
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:b3a0877bfbfe9688f766e6e8850acddd6ac02906b787a5595a9e809cdb82bb63
+size 280
diff --git a/robotiq_2f_85/robotiq-2f-spring_link.obj b/robotiq_2f_85/robotiq-2f-spring_link.obj
new file mode 100644
index 0000000000000000000000000000000000000000..004081dc9950fba52df27555a0899bacf4f61a29
--- /dev/null
+++ b/robotiq_2f_85/robotiq-2f-spring_link.obj
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:9d368263f644ce929c9bc0ddfdabc6582cebb50a124a15f25e8e7dc18fc22945
+size 131061
diff --git a/robotiq_2f_85/robotiq-2f-spring_link.stl b/robotiq_2f_85/robotiq-2f-spring_link.stl
new file mode 100644
index 0000000000000000000000000000000000000000..01ec1e2a5616b1e85b51e0d64f8ac8250b386306
--- /dev/null
+++ b/robotiq_2f_85/robotiq-2f-spring_link.stl
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:4fb74a8a0d76c0e471cf19fd48bc676fd5b19123798e7a39eb6aa56869354283
+size 84884
diff --git a/robotiq_2f_85/robotiq_2f_85.urdf b/robotiq_2f_85/robotiq_2f_85.urdf
new file mode 100644
index 0000000000000000000000000000000000000000..20c203382d56cd526b5ed11d8055d75e5c7882e0
--- /dev/null
+++ b/robotiq_2f_85/robotiq_2f_85.urdf
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:70b6d72d6ebe2bd0c03a4da05daff8e7fa65659378290a5bb63677800a135af2
+size 10659
diff --git a/robotiq_2f_85/textures/gripper-2f_BaseColor.jpg b/robotiq_2f_85/textures/gripper-2f_BaseColor.jpg
new file mode 100644
index 0000000000000000000000000000000000000000..08f88722cb58e7e834e76ff9e67bbf0407f80d9c
--- /dev/null
+++ b/robotiq_2f_85/textures/gripper-2f_BaseColor.jpg
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:3b9b99f99d2189aa229acf509bb48a50ecd27bcfb3244678f579f8642cbf87fa
+size 365086
diff --git a/robotiq_2f_85/textures/gripper-2f_Metallic.jpg b/robotiq_2f_85/textures/gripper-2f_Metallic.jpg
new file mode 100644
index 0000000000000000000000000000000000000000..5379a1a2853a20a77401e0f98a25a98a80a1ef31
--- /dev/null
+++ b/robotiq_2f_85/textures/gripper-2f_Metallic.jpg
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:49bbf365c6c02d09d3b4d9494790fcf9c81ff3e80af74bc1952f8e0aa85dc565
+size 103185
diff --git a/robotiq_2f_85/textures/gripper-2f_Normal.jpg b/robotiq_2f_85/textures/gripper-2f_Normal.jpg
new file mode 100644
index 0000000000000000000000000000000000000000..dc1c528654de42f352a08beafedebe2dfec370dd
--- /dev/null
+++ b/robotiq_2f_85/textures/gripper-2f_Normal.jpg
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:1addadf065fa3b6de22c629270cb3fd6ae25124108e12bb868351165bce4d59e
+size 85637
diff --git a/robotiq_2f_85/textures/gripper-2f_Roughness.jpg b/robotiq_2f_85/textures/gripper-2f_Roughness.jpg
new file mode 100644
index 0000000000000000000000000000000000000000..3b1dba5348f4c52eed08b6a62f41b7920e1a8db9
--- /dev/null
+++ b/robotiq_2f_85/textures/gripper-2f_Roughness.jpg
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:34d717acb16e2daf66e4d1d7229227cf51fa7d2292b634c81968bade10b27233
+size 433961
diff --git a/sim.py b/sim.py
new file mode 100644
index 0000000000000000000000000000000000000000..cd67bf0eec9f7172d7152536e76dcc673d4dd7a9
--- /dev/null
+++ b/sim.py
@@ -0,0 +1,795 @@
+import pybullet
+from pybullet_utils.bullet_client import BulletClient
+import pybullet_data
+import threading
+from time import sleep
+import numpy as np
+import os
+from consts import BOUNDS, COLORS, PIXEL_SIZE, CORNER_POS
+from shapely.geometry import box
+import random
+
+
+# Gripper (Robotiq 2F85) code
+class Robotiq2F85:
+  """Gripper handling for Robotiq 2F85."""
+
+  def __init__(self, robot, tool, p):
+    self.robot = robot
+    self.tool = tool
+    self._p = p
+    pos = [0.1339999999999999, -0.49199999999872496, 0.5]
+    rot = self._p.getQuaternionFromEuler([np.pi, 0, np.pi])
+    urdf = 'robotiq_2f_85/robotiq_2f_85.urdf'
+    self.body = self._p.loadURDF(urdf, pos, rot)
+    self.n_joints = self._p.getNumJoints(self.body)
+    self.activated = False
+
+    # Connect gripper base to robot tool.
+    self._p.createConstraint(self.robot, tool, self.body, 0, jointType=self._p.JOINT_FIXED, jointAxis=[0, 0, 0], parentFramePosition=[0, 0, 0], childFramePosition=[0, 0, -0.07], childFrameOrientation=self._p.getQuaternionFromEuler([0, 0, np.pi / 2]))
+
+    # Set friction coefficients for gripper fingers.
+    for i in range(self._p.getNumJoints(self.body)):
+      self._p.changeDynamics(self.body, i, lateralFriction=10.0, spinningFriction=1.0, rollingFriction=1.0, frictionAnchor=True)
+
+    # Start thread to handle additional gripper constraints.
+    self.motor_joint = 1
+    self.constraints_thread = threading.Thread(target=self.step)
+    self.constraints_thread.daemon = True
+    self.constraints_thread.start()
+
+  # Control joint positions by enforcing hard contraints on gripper behavior.
+  # Set one joint as the open/close motor joint (other joints should mimic).
+  def step(self):
+    while True:
+      try:
+        currj = [self._p.getJointState(self.body, i)[0] for i in range(self.n_joints)]
+        indj = [6, 3, 8, 5, 10]
+        targj = [currj[1], -currj[1], -currj[1], currj[1], currj[1]]
+        self._p.setJointMotorControlArray(self.body, indj, self._p.POSITION_CONTROL, targj, positionGains=np.ones(5))
+      except:
+        return
+      sleep(0.001)
+
+  # Close gripper fingers.
+  def activate(self):
+    self._p.setJointMotorControl2(self.body, self.motor_joint, self._p.VELOCITY_CONTROL, targetVelocity=1, force=10)
+    self.activated = True
+
+  # Open gripper fingers.
+  def release(self):
+    self._p.setJointMotorControl2(self.body, self.motor_joint, self._p.VELOCITY_CONTROL, targetVelocity=-1, force=10)
+    self.activated = False
+
+  # If activated and object in gripper: check object contact.
+  # If activated and nothing in gripper: check gripper contact.
+  # If released: check proximity to surface (disabled).
+  def detect_contact(self):
+    obj, _, ray_frac = self.check_proximity()
+    if self.activated:
+      empty = self.grasp_width() < 0.01
+      cbody = self.body if empty else obj
+      if obj == self.body or obj == 0:
+        return False
+      return self.external_contact(cbody)
+  #   else:
+  #     return ray_frac < 0.14 or self.external_contact()
+
+  # Return if body is in contact with something other than gripper
+  def external_contact(self, body=None):
+    if body is None:
+      body = self.body
+    pts = self._p.getContactPoints(bodyA=body)
+    pts = [pt for pt in pts if pt[2] != self.body]
+    return len(pts) > 0  # pylint: disable=g-explicit-length-test
+
+  def check_grasp(self):
+    while self.moving():
+      sleep(0.001)
+    success = self.grasp_width() > 0.01
+    return success
+
+  def grasp_width(self):
+    lpad = np.array(self._p.getLinkState(self.body, 4)[0])
+    rpad = np.array(self._p.getLinkState(self.body, 9)[0])
+    dist = np.linalg.norm(lpad - rpad) - 0.047813
+    return dist
+
+  def check_proximity(self):
+    ee_pos = np.array(self._p.getLinkState(self.robot, self.tool)[0])
+    tool_pos = np.array(self._p.getLinkState(self.body, 0)[0])
+    vec = (tool_pos - ee_pos) / np.linalg.norm((tool_pos - ee_pos))
+    ee_targ = ee_pos + vec
+    ray_data = self._p.rayTest(ee_pos, ee_targ)[0]
+    obj, link, ray_frac = ray_data[0], ray_data[1], ray_data[2]
+    return obj, link, ray_frac
+
+
+# Gym-style environment code
+class PickPlaceEnv():
+
+  def __init__(self, render=False, high_res=False, high_frame_rate=False):
+    self.dt = 1/480
+    self.sim_step = 0
+
+    # Configure and start PyBullet
+    # self._p = pybullet.connect(pybullet.DIRECT)
+    self._p = BulletClient(connection_mode=pybullet.DIRECT)
+    self._p.configureDebugVisualizer(self._p.COV_ENABLE_GUI, 0)
+    self._p.setPhysicsEngineParameter(enableFileCaching=0)
+    assets_path = os.path.dirname(os.path.abspath(""))
+    self._p.setAdditionalSearchPath(assets_path)
+    self._p.setAdditionalSearchPath(pybullet_data.getDataPath())
+    self._p.setTimeStep(self.dt)
+
+    self.home_joints = (np.pi / 2, -np.pi / 2, np.pi / 2, -np.pi / 2, 3 * np.pi / 2, 0)  # Joint angles: (J0, J1, J2, J3, J4, J5).
+    self.home_ee_euler = (np.pi, 0, np.pi)  # (RX, RY, RZ) rotation in Euler angles.
+    self.ee_link_id = 9  # Link ID of UR5 end effector.
+    self.tip_link_id = 10  # Link ID of gripper finger tips.
+    self.gripper = None
+
+    self.render = render
+    self.high_res = high_res
+    self.high_frame_rate = high_frame_rate
+
+  def reset(self, object_list):
+    self._p.resetSimulation(self._p.RESET_USE_DEFORMABLE_WORLD)
+    self._p.setGravity(0, 0, -9.8)
+    self.cache_video = []
+
+    # Temporarily disable rendering to load URDFs faster.
+    self._p.configureDebugVisualizer(self._p.COV_ENABLE_RENDERING, 0)
+
+    # Add robot.
+    self._p.loadURDF("plane.urdf", [0, 0, -0.001])
+    self.robot_id = self._p.loadURDF("ur5e/ur5e.urdf", [0, 0, 0], flags=self._p.URDF_USE_MATERIAL_COLORS_FROM_MTL)
+    self.ghost_id = self._p.loadURDF("ur5e/ur5e.urdf", [0, 0, -10])  # For forward kinematics.
+    self.joint_ids = [self._p.getJointInfo(self.robot_id, i) for i in range(self._p.getNumJoints(self.robot_id))]
+    self.joint_ids = [j[0] for j in self.joint_ids if j[2] == self._p.JOINT_REVOLUTE]
+
+    # Move robot to home configuration.
+    for i in range(len(self.joint_ids)):
+      self._p.resetJointState(self.robot_id, self.joint_ids[i], self.home_joints[i])
+
+    # Add gripper.
+    if self.gripper is not None:
+      while self.gripper.constraints_thread.is_alive():
+        self.constraints_thread_active = False
+    self.gripper = Robotiq2F85(self.robot_id, self.ee_link_id, self._p)
+    self.gripper.release()
+
+    # Add workspace.
+    plane_shape = self._p.createCollisionShape(self._p.GEOM_BOX, halfExtents=[0.3, 0.3, 0.001])
+    plane_visual = self._p.createVisualShape(self._p.GEOM_BOX, halfExtents=[0.3, 0.3, 0.001])
+    plane_id = self._p.createMultiBody(0, plane_shape, plane_visual, basePosition=[0, -0.5, 0])
+    self._p.changeVisualShape(plane_id, -1, rgbaColor=[0.2, 0.2, 0.2, 1.0])
+
+    # Load objects according to config.
+    self.object_list = object_list
+    self.obj_name_to_id = {}
+    obj_xyz = np.zeros((0, 3))
+    for obj_name in object_list:
+      if ('block' in obj_name) or ('bowl' in obj_name):
+
+        # Get random position 15cm+ from other objects.
+        while True:
+          rand_x = np.random.uniform(BOUNDS[0, 0] + 0.1, BOUNDS[0, 1] - 0.1)
+          rand_y = np.random.uniform(BOUNDS[1, 0] + 0.1, BOUNDS[1, 1] - 0.1)
+          rand_xyz = np.float32([rand_x, rand_y, 0.03]).reshape(1, 3)
+          if len(obj_xyz) == 0:
+            obj_xyz = np.concatenate((obj_xyz, rand_xyz), axis=0)
+            break
+          else:
+            nn_dist = np.min(np.linalg.norm(obj_xyz - rand_xyz, axis=1)).squeeze()
+            if nn_dist > 0.15:
+              obj_xyz = np.concatenate((obj_xyz, rand_xyz), axis=0)
+              break
+        
+        object_color = COLORS[obj_name.split(' ')[0]]
+        object_type = obj_name.split(' ')[1]
+        object_position = rand_xyz.squeeze()
+        if object_type == 'block':
+          object_shape = self._p.createCollisionShape(self._p.GEOM_BOX, halfExtents=[0.02, 0.02, 0.02])
+          object_visual = self._p.createVisualShape(self._p.GEOM_BOX, halfExtents=[0.02, 0.02, 0.02])
+          object_id = self._p.createMultiBody(0.01, object_shape, object_visual, basePosition=object_position)
+        elif object_type == 'bowl':
+          object_position[2] = 0
+          object_id = self._p.loadURDF("bowl/bowl.urdf", object_position, useFixedBase=1)
+        self._p.changeVisualShape(object_id, -1, rgbaColor=object_color)
+        self.obj_name_to_id[obj_name] = object_id
+
+    # Re-enable rendering.
+    self._p.configureDebugVisualizer(self._p.COV_ENABLE_RENDERING, 1)
+
+    for _ in range(200):
+      self._p.stepSimulation()
+
+    # record object positions at reset
+    self.init_pos = {name: self.get_obj_pos(name) for name in object_list}
+
+    return self.get_observation()
+
+  def servoj(self, joints):
+    """Move to target joint positions with position control."""
+    self._p.setJointMotorControlArray(
+      bodyIndex=self.robot_id,
+      jointIndices=self.joint_ids,
+      controlMode=self._p.POSITION_CONTROL,
+      targetPositions=joints,
+      positionGains=[0.01]*6)
+  
+  def movep(self, position):
+    """Move to target end effector position."""
+    joints = self._p.calculateInverseKinematics(
+        bodyUniqueId=self.robot_id,
+        endEffectorLinkIndex=self.tip_link_id,
+        targetPosition=position,
+        targetOrientation=self._p.getQuaternionFromEuler(self.home_ee_euler),
+        maxNumIterations=100)
+    self.servoj(joints)
+
+  def get_ee_pos(self):
+    ee_xyz = np.float32(self._p.getLinkState(self.robot_id, self.tip_link_id)[0])
+    return ee_xyz
+
+  def step(self, action=None):
+    """Do pick and place motion primitive."""
+    pick_pos, place_pos = action['pick'].copy(), action['place'].copy()
+
+    # Set fixed primitive z-heights.
+    hover_xyz = np.float32([pick_pos[0], pick_pos[1], 0.2])
+    if pick_pos.shape[-1] == 2:
+      pick_xyz = np.append(pick_pos, 0.025)
+    else:
+      pick_xyz = pick_pos
+      pick_xyz[2] = 0.025
+    if place_pos.shape[-1] == 2:
+      place_xyz = np.append(place_pos, 0.15)
+    else:
+      place_xyz = place_pos
+      place_xyz[2] = 0.15
+
+    # Move to object.
+    ee_xyz = self.get_ee_pos()
+    while np.linalg.norm(hover_xyz - ee_xyz) > 0.01:
+      self.movep(hover_xyz)
+      self.step_sim_and_render()
+      ee_xyz = self.get_ee_pos()
+
+    while np.linalg.norm(pick_xyz - ee_xyz) > 0.01:
+      self.movep(pick_xyz)
+      self.step_sim_and_render()
+      ee_xyz = self.get_ee_pos()
+
+    # Pick up object.
+    self.gripper.activate()
+    for _ in range(240):
+      self.step_sim_and_render()
+    while np.linalg.norm(hover_xyz - ee_xyz) > 0.01:
+      self.movep(hover_xyz)
+      self.step_sim_and_render()
+      ee_xyz = self.get_ee_pos()
+    
+    for _ in range(50):
+      self.step_sim_and_render()
+    
+    # Move to place location.
+    while np.linalg.norm(place_xyz - ee_xyz) > 0.01:
+      self.movep(place_xyz)
+      self.step_sim_and_render()
+      ee_xyz = self.get_ee_pos()
+
+    # Place down object.
+    while (not self.gripper.detect_contact()) and (place_xyz[2] > 0.03):
+      place_xyz[2] -= 0.001
+      self.movep(place_xyz)
+      for _ in range(3):
+        self.step_sim_and_render()
+    self.gripper.release()
+    for _ in range(240):
+      self.step_sim_and_render()
+    place_xyz[2] = 0.2
+    ee_xyz = self.get_ee_pos()
+    while np.linalg.norm(place_xyz - ee_xyz) > 0.01:
+      self.movep(place_xyz)
+      self.step_sim_and_render()
+      ee_xyz = self.get_ee_pos()
+    place_xyz = np.float32([0, -0.5, 0.2])
+    while np.linalg.norm(place_xyz - ee_xyz) > 0.01:
+      self.movep(place_xyz)
+      self.step_sim_and_render()
+      ee_xyz = self.get_ee_pos()
+
+    observation = self.get_observation()
+    reward = self.get_reward()
+    done = False
+    info = {}
+    return observation, reward, done, info
+
+  def set_alpha_transparency(self, alpha: float) -> None:
+    for id in range(20):
+      visual_shape_data = self._p.getVisualShapeData(id)
+      for i in range(len(visual_shape_data)):
+        object_id, link_index, _, _, _, _, _, rgba_color = visual_shape_data[i]
+        rgba_color = list(rgba_color[0:3]) +  [alpha]
+        self._p.changeVisualShape(
+            self.robot_id, linkIndex=i, rgbaColor=rgba_color)      
+        self._p.changeVisualShape(
+            self.gripper.body, linkIndex=i, rgbaColor=rgba_color)
+
+  def step_sim_and_render(self):
+    self._p.stepSimulation()
+    self.sim_step += 1
+
+    interval = 40 if self.high_frame_rate else 60
+    # Render current image at 8 FPS.
+    if self.sim_step % interval == 0 and self.render:
+      self.cache_video.append(self.get_camera_image())
+
+  def get_camera_image(self):
+    if not self.high_res:
+      image_size = (240, 240)
+      intrinsics = (120., 0, 120., 0, 120., 120., 0, 0, 1)
+    else:
+      image_size=(360, 360)
+      intrinsics=(180., 0, 180., 0, 180., 180., 0, 0, 1)
+    color, _, _, _, _ = self.render_image(image_size, intrinsics)
+    return color
+
+  def get_reward(self):
+    return None
+
+  def get_observation(self):
+    observation = {}
+
+    # Render current image.
+    color, depth, position, orientation, intrinsics = self.render_image()
+
+    # Get heightmaps and colormaps.
+    points = self.get_pointcloud(depth, intrinsics)
+    position = np.float32(position).reshape(3, 1)
+    rotation = self._p.getMatrixFromQuaternion(orientation)
+    rotation = np.float32(rotation).reshape(3, 3)
+    transform = np.eye(4)
+    transform[:3, :] = np.hstack((rotation, position))
+    points = self.transform_pointcloud(points, transform)
+    heightmap, colormap, xyzmap = self.get_heightmap(points, color, BOUNDS, PIXEL_SIZE)
+
+    observation["image"] = colormap
+    observation["xyzmap"] = xyzmap
+
+    return observation
+
+  def render_image(self, image_size=(720, 720), intrinsics=(360., 0, 360., 0, 360., 360., 0, 0, 1)):
+
+    # Camera parameters.
+    position = (0, -0.85, 0.4)
+    orientation = (np.pi / 4 + np.pi / 48, np.pi, np.pi)
+    orientation = self._p.getQuaternionFromEuler(orientation)
+    zrange = (0.01, 10.)
+    noise=True
+
+    # OpenGL camera settings.
+    lookdir = np.float32([0, 0, 1]).reshape(3, 1)
+    updir = np.float32([0, -1, 0]).reshape(3, 1)
+    rotation = self._p.getMatrixFromQuaternion(orientation)
+    rotm = np.float32(rotation).reshape(3, 3)
+    lookdir = (rotm @ lookdir).reshape(-1)
+    updir = (rotm @ updir).reshape(-1)
+    lookat = position + lookdir
+    focal_len = intrinsics[0]
+    znear, zfar = (0.01, 10.)
+    viewm = self._p.computeViewMatrix(position, lookat, updir)
+    fovh = (image_size[0] / 2) / focal_len
+    fovh = 180 * np.arctan(fovh) * 2 / np.pi
+
+    # Notes: 1) FOV is vertical FOV 2) aspect must be float
+    aspect_ratio = image_size[1] / image_size[0]
+    projm = self._p.computeProjectionMatrixFOV(fovh, aspect_ratio, znear, zfar)
+
+    # Render with OpenGL camera settings.
+    _, _, color, depth, segm = self._p.getCameraImage(
+        width=image_size[1],
+        height=image_size[0],
+        viewMatrix=viewm,
+        projectionMatrix=projm,
+        shadow=1,
+        flags=self._p.ER_SEGMENTATION_MASK_OBJECT_AND_LINKINDEX,
+        renderer=self._p.ER_BULLET_HARDWARE_OPENGL)
+
+    # Get color image.
+    color_image_size = (image_size[0], image_size[1], 4)
+    color = np.array(color, dtype=np.uint8).reshape(color_image_size)
+    color = color[:, :, :3]  # remove alpha channel
+    if noise:
+      color = np.int32(color)
+      color += np.int32(np.random.normal(0, 3, color.shape))
+      color = np.uint8(np.clip(color, 0, 255))
+
+    # Get depth image.
+    depth_image_size = (image_size[0], image_size[1])
+    zbuffer = np.float32(depth).reshape(depth_image_size)
+    depth = (zfar + znear - (2 * zbuffer - 1) * (zfar - znear))
+    depth = (2 * znear * zfar) / depth
+    if noise:
+      depth += np.random.normal(0, 0.003, depth.shape)
+
+    intrinsics = np.float32(intrinsics).reshape(3, 3)
+    return color, depth, position, orientation, intrinsics
+
+  def get_pointcloud(self, depth, intrinsics):
+    """Get 3D pointcloud from perspective depth image.
+    Args:
+      depth: HxW float array of perspective depth in meters.
+      intrinsics: 3x3 float array of camera intrinsics matrix.
+    Returns:
+      points: HxWx3 float array of 3D points in camera coordinates.
+    """
+    height, width = depth.shape
+    xlin = np.linspace(0, width - 1, width)
+    ylin = np.linspace(0, height - 1, height)
+    px, py = np.meshgrid(xlin, ylin)
+    px = (px - intrinsics[0, 2]) * (depth / intrinsics[0, 0])
+    py = (py - intrinsics[1, 2]) * (depth / intrinsics[1, 1])
+    points = np.float32([px, py, depth]).transpose(1, 2, 0)
+    return points
+
+  def transform_pointcloud(self, points, transform):
+    """Apply rigid transformation to 3D pointcloud.
+    Args:
+      points: HxWx3 float array of 3D points in camera coordinates.
+      transform: 4x4 float array representing a rigid transformation matrix.
+    Returns:
+      points: HxWx3 float array of transformed 3D points.
+    """
+    padding = ((0, 0), (0, 0), (0, 1))
+    homogen_points = np.pad(points.copy(), padding,
+                            'constant', constant_values=1)
+    for i in range(3):
+      points[Ellipsis, i] = np.sum(transform[i, :] * homogen_points, axis=-1)
+    return points
+
+  def get_heightmap(self, points, colors, bounds, pixel_size):
+    """Get top-down (z-axis) orthographic heightmap image from 3D pointcloud.
+    Args:
+      points: HxWx3 float array of 3D points in world coordinates.
+      colors: HxWx3 uint8 array of values in range 0-255 aligned with points.
+      bounds: 3x2 float array of values (rows: X,Y,Z; columns: min,max) defining
+        region in 3D space to generate heightmap in world coordinates.
+      pixel_size: float defining size of each pixel in meters.
+    Returns:
+      heightmap: HxW float array of height (from lower z-bound) in meters.
+      colormap: HxWx3 uint8 array of backprojected color aligned with heightmap.
+      xyzmap: HxWx3 float array of XYZ points in world coordinates.
+    """
+    width = int(np.round((bounds[0, 1] - bounds[0, 0]) / pixel_size))
+    height = int(np.round((bounds[1, 1] - bounds[1, 0]) / pixel_size))
+    heightmap = np.zeros((height, width), dtype=np.float32)
+    colormap = np.zeros((height, width, colors.shape[-1]), dtype=np.uint8)
+    xyzmap = np.zeros((height, width, 3), dtype=np.float32)
+
+    # Filter out 3D points that are outside of the predefined bounds.
+    ix = (points[Ellipsis, 0] >= bounds[0, 0]) & (points[Ellipsis, 0] < bounds[0, 1])
+    iy = (points[Ellipsis, 1] >= bounds[1, 0]) & (points[Ellipsis, 1] < bounds[1, 1])
+    iz = (points[Ellipsis, 2] >= bounds[2, 0]) & (points[Ellipsis, 2] < bounds[2, 1])
+    valid = ix & iy & iz
+    points = points[valid]
+    colors = colors[valid]
+
+    # Sort 3D points by z-value, which works with array assignment to simulate
+    # z-buffering for rendering the heightmap image.
+    iz = np.argsort(points[:, -1])
+    points, colors = points[iz], colors[iz]
+    px = np.int32(np.floor((points[:, 0] - bounds[0, 0]) / pixel_size))
+    py = np.int32(np.floor((points[:, 1] - bounds[1, 0]) / pixel_size))
+    px = np.clip(px, 0, width - 1)
+    py = np.clip(py, 0, height - 1)
+    heightmap[py, px] = points[:, 2] - bounds[2, 0]
+    for c in range(colors.shape[-1]):
+      colormap[py, px, c] = colors[:, c]
+      xyzmap[py, px, c] = points[:, c]
+    colormap = colormap[::-1, :, :]  # Flip up-down.
+    xv, yv = np.meshgrid(np.linspace(BOUNDS[0, 0], BOUNDS[0, 1], height),
+                         np.linspace(BOUNDS[1, 0], BOUNDS[1, 1], width))
+    xyzmap[:, :, 0] = xv
+    xyzmap[:, :, 1] = yv
+    xyzmap = xyzmap[::-1, :, :]  # Flip up-down.
+    heightmap = heightmap[::-1, :]  # Flip up-down.
+    return heightmap, colormap, xyzmap
+  
+  def on_top_of(self, obj_a, obj_b):
+    """
+    check if obj_a is on top of obj_b
+    condition 1: l2 distance on xy plane is less than a threshold
+    condition 2: obj_a is higher than obj_b
+    """
+    obj_a_pos = self.get_obj_pos(obj_a)
+    obj_b_pos = self.get_obj_pos(obj_b)
+    xy_dist = np.linalg.norm(obj_a_pos[:2] - obj_b_pos[:2])
+    if obj_b in CORNER_POS:
+      is_near = xy_dist < 0.06
+      return is_near
+    elif 'bowl' in obj_b:
+      is_near = xy_dist < 0.06
+      is_higher = obj_a_pos[2] > obj_b_pos[2]
+      return is_near and is_higher
+    else:
+      is_near = xy_dist < 0.04
+      is_higher = obj_a_pos[2] > obj_b_pos[2]
+      return is_near and is_higher
+  
+  def get_obj_id(self, obj_name):
+    try:
+      if obj_name in self.obj_name_to_id:
+        obj_id = self.obj_name_to_id[obj_name]
+      else:
+        obj_name = obj_name.replace('circle', 'bowl').replace('square', 'block').replace('small', '').strip()
+        obj_id = self.obj_name_to_id[obj_name]
+      return obj_id
+    except:
+      raise Exception('Object name "{}" not found'.format(obj_name))
+  
+  def get_obj_pos(self, obj_name):
+    obj_name = obj_name.replace('the', '').replace('_', ' ').strip()
+    if obj_name in CORNER_POS:
+      position = np.float32(np.array(CORNER_POS[obj_name]))
+    else:
+      pick_id = self.get_obj_id(obj_name)
+      pose = self._p.getBasePositionAndOrientation(pick_id)
+      position = np.float32(pose[0])
+    return position
+  
+  def get_bounding_box(self, obj_name):
+    obj_id = self.get_obj_id(obj_name)
+    return self._p.getAABB(obj_id)
+
+
+class LMP_wrapper():
+
+  def __init__(self, env, cfg, render=False):
+    self.env = env
+    self._cfg = cfg
+    self.object_names = list(self._cfg['env']['init_objs'])
+    
+    self._min_xy = np.array(self._cfg['env']['coords']['bottom_left'])
+    self._max_xy = np.array(self._cfg['env']['coords']['top_right'])
+    self._range_xy = self._max_xy - self._min_xy
+
+    self._table_z = self._cfg['env']['coords']['table_z']
+    self.render = render
+
+  def is_obj_visible(self, obj_name):
+    return obj_name in self.object_names
+
+  def get_obj_names(self):
+    return self.object_names[::]
+
+  def denormalize_xy(self, pos_normalized):
+    return pos_normalized * self._range_xy + self._min_xy
+
+  def get_corner_positions(self):
+    unit_square = box(0, 0, 1, 1)
+    normalized_corners = np.array(list(unit_square.exterior.coords))[:4]
+    corners = np.array(([self.denormalize_xy(corner) for corner in normalized_corners]))
+    return corners
+
+  def get_side_positions(self):
+    side_xs = np.array([0, 0.5, 0.5, 1])
+    side_ys = np.array([0.5, 0, 1, 0.5])
+    normalized_side_positions = np.c_[side_xs, side_ys]
+    side_positions = np.array(([self.denormalize_xy(corner) for corner in normalized_side_positions]))
+    return side_positions
+
+  def get_obj_pos(self, obj_name):
+    # return the xy position of the object in robot base frame
+    return self.env.get_obj_pos(obj_name)[:2]
+
+  def get_obj_position_np(self, obj_name):
+    return self.get_pos(obj_name)
+
+  def get_bbox(self, obj_name):
+    # return the axis-aligned object bounding box in robot base frame (not in pixels)
+    # the format is (min_x, min_y, max_x, max_y)
+    bbox = self.env.get_bounding_box(obj_name)
+    return bbox
+
+  def get_color(self, obj_name):
+    for color, rgb in COLORS.items():
+      if color in obj_name:
+        return rgb
+
+  def pick_place(self, pick_pos, place_pos):
+    pick_pos_xyz = np.r_[pick_pos, [self._table_z]]
+    place_pos_xyz = np.r_[place_pos, [self._table_z]]
+    pass
+
+  def put_first_on_second(self, arg1, arg2):
+    # put the object with obj_name on top of target
+    # target can either be another object name, or it can be an x-y position in robot base frame
+    pick_pos = self.get_obj_pos(arg1) if isinstance(arg1, str) else arg1
+    place_pos = self.get_obj_pos(arg2) if isinstance(arg2, str) else arg2
+    self.env.step(action={'pick': pick_pos, 'place': place_pos})
+
+  def get_robot_pos(self):
+    # return robot end-effector xy position in robot base frame
+    return self.env.get_ee_pos()
+
+  def goto_pos(self, position_xy):
+    # move the robot end-effector to the desired xy position while maintaining same z
+    ee_xyz = self.env.get_ee_pos()
+    position_xyz = np.concatenate([position_xy, ee_xyz[-1]])
+    while np.linalg.norm(position_xyz - ee_xyz) > 0.01:
+      self.env.movep(position_xyz)
+      self.env.step_sim_and_render()
+      ee_xyz = self.env.get_ee_pos()
+
+  def follow_traj(self, traj):
+    for pos in traj:
+      self.goto_pos(pos)
+
+  def get_corner_positions(self):
+    normalized_corners = np.array([
+        [0, 1],
+        [1, 1],
+        [0, 0],
+        [1, 0]
+    ])
+    return np.array(([self.denormalize_xy(corner) for corner in normalized_corners]))
+
+  def get_side_positions(self):
+    normalized_sides = np.array([
+        [0.5, 1],
+        [1, 0.5],
+        [0.5, 0],
+        [0, 0.5]
+    ])
+    return np.array(([self.denormalize_xy(side) for side in normalized_sides]))
+
+  def get_corner_name(self, pos):
+    corner_positions = self.get_corner_positions()
+    corner_idx = np.argmin(np.linalg.norm(corner_positions - pos, axis=1))
+    return ['top left corner', 'top right corner', 'bottom left corner', 'botom right corner'][corner_idx]
+
+  def get_side_name(self, pos):
+    side_positions = self.get_side_positions()
+    side_idx = np.argmin(np.linalg.norm(side_positions - pos, axis=1))
+    return ['top side', 'right side', 'bottom side', 'left side'][side_idx]
+
+
+class DummyClass:
+    def __getattr__(self, attr):
+        return None
+
+class VoxPoserWrapper():
+
+  def __init__(self, env, cfg, render=False):
+    self.env = env
+    self._cfg = cfg
+
+  def parse_query_obj(self, *args, **kwargs):
+    print("`parse_query_obj` called.\n")
+    return DummyClass()
+
+  def get_empty_affordance_map(self, *args, **kwargs):
+    print("`get_empty_affordance_map` called.\n")
+    return DummyClass()
+
+  def set_voxel_by_radius(self, *args, **kwargs):
+    print("`set_voxel_by_radius` called.\n")
+
+  def cm2index(self, *args, **kwargs):
+    print("`cm2index` called.\n")
+
+  def detect(self, *args, **kwargs):
+    print("`detect` called.\n")
+    return DummyClass()
+
+  def get_empty_avoidance_map(self, *args, **kwargs):
+    print("`get_empty_avoidance_map` called.\n")
+    return DummyClass()
+
+  def get_empty_gripper_map(self, *args, **kwargs):
+    print("`get_empty_gripper_map` called.\n")
+    return DummyClass()
+
+  def get_empty_rotation_map(self, *args, **kwargs):
+    print("`get_empty_rotation_map` called.\n")
+    return DummyClass()
+
+  def vec2quat(self, *args, **kwargs):
+    print("`vec2quat` called.\n")
+    return DummyClass()
+
+  def euler2quat(self, *args, **kwargs):
+    print("`euler2quat` called.\n")
+    return DummyClass()
+
+  def quat2euler(self, *args, **kwargs):
+    print("`quat2euler` called.\n")
+    return DummyClass()
+
+  def qmult(self, *args, **kwargs):
+    print("`qmult` called.\n")
+    return DummyClass()
+
+  def qinverse(self, *args, **kwargs):
+    print("`qinverse` called.\n")
+    return DummyClass()
+
+  def get_empty_velocity_map(self, *args, **kwargs):
+    print("`get_empty_velocity_map` called.\n")
+    return DummyClass()
+
+  def execute(self, *args, **kwargs):
+    print("`execute` called.\n")
+
+  def reset_to_default_pose(self, *args, **kwargs):
+    print("`reset_to_default_pose` called.\n")
+
+
+class AgibotWrapper():
+
+  def __init__(self, env, cfg, render=False):
+    self.env = env
+    self._cfg = cfg
+
+  def track(self, *args, **kwargs):
+    print("`track` called.")
+    print("detect")
+    print("establish tracking")
+    return DummyClass()
+  
+  def get_target_pose(self, *args, **kwargs):
+    print("`get_target_pose` called.\n")
+    return DummyClass()
+
+  def get_operation(self, *args, **kwargs):
+    print("`get_operation` called.\n")
+    return DummyClass()
+
+  def execute(self, movable, *args, target_pose=None, operation=None, **kwargs) -> bool:
+    print("`execute` called.")
+
+    while not self.lock_object(target_pose):    
+      pass
+    print("locked\n")
+
+    if not self.observe_object_if_moving(movable):
+      print("object is moving, execution exit.\n")
+      return False
+
+    if target_pose is not None:
+      contact_ret = self.contact_object(target_pose)
+      print("contact_object:", "success" if contact_ret else "failed")
+      if not contact_ret:
+        print("execution failed\n")
+        return False
+
+    if operation is not None:
+      operation_ret = self.operate_object(operation)
+      print("operate_object:", "success" if operation_ret else "failed")
+      if not operation_ret:
+        print("execution failed\n")
+        return False
+
+    print("execution done\n")
+    return True
+
+  def lock_object(self, *args, **kwargs):
+    print("`lock_object` called.\n")
+    sleep(1.0)
+    return True
+
+  def observe_object_if_moving(self, *args, **kwargs):
+    return random.choice([True, True, False])
+
+  def contact_object(self, *args, **kwargs):
+    print("`contact_object` called.")
+    return random.choice([True, True, False])
+
+  def operate_object(self, *args, **kwargs):
+    print("`operate_object` called.")
+    return random.choice([True, True, False])
+
+  def reset_to_default_pose(self, *args, **kwargs):
+    print("`reset_to_default_pose` called.\n")
diff --git a/ur5e/collision/base.stl b/ur5e/collision/base.stl
new file mode 100644
index 0000000000000000000000000000000000000000..17532f8b2337d284b57814b0b04df1cad492e7f1
--- /dev/null
+++ b/ur5e/collision/base.stl
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:9a145e0d46f2130afdf2a2e8825a00a929870c4c3d6d8e4d1adc5f04db3aac1b
+size 21084
diff --git a/ur5e/collision/forearm.stl b/ur5e/collision/forearm.stl
new file mode 100644
index 0000000000000000000000000000000000000000..f45e03826c43012b7d691055029ff858dd11173d
--- /dev/null
+++ b/ur5e/collision/forearm.stl
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:2e7423cab807c34160ec4f770daee5e747d70e777eb01b7beeace2b8c5751816
+size 53284
diff --git a/ur5e/collision/shoulder.stl b/ur5e/collision/shoulder.stl
new file mode 100644
index 0000000000000000000000000000000000000000..2afa86568142f0b614e766e149fd19b5ca23ff43
--- /dev/null
+++ b/ur5e/collision/shoulder.stl
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:ceb92532177daa77682f5fbd628e01c2137d168f949a7a706ce1dabe9f002387
+size 70084
diff --git a/ur5e/collision/upperarm.stl b/ur5e/collision/upperarm.stl
new file mode 100644
index 0000000000000000000000000000000000000000..d0535c386a41cb0f1086573e24660180d3635c2d
--- /dev/null
+++ b/ur5e/collision/upperarm.stl
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:ee893044caf00075cb55b4cf666d1f1311c7979786212a501009f33bee945209
+size 99684
diff --git a/ur5e/collision/wrist1.stl b/ur5e/collision/wrist1.stl
new file mode 100644
index 0000000000000000000000000000000000000000..a4c0a83322115f8d31fbcf8a8a83968fd01497fa
--- /dev/null
+++ b/ur5e/collision/wrist1.stl
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:f8c9f9337b6fd98c75f052e96de10e14a107ddb6874ba6b904e546f8a4e4f43a
+size 59584
diff --git a/ur5e/collision/wrist2.stl b/ur5e/collision/wrist2.stl
new file mode 100644
index 0000000000000000000000000000000000000000..44ce896e6b5194de0a569779e5dd2e8766552685
--- /dev/null
+++ b/ur5e/collision/wrist2.stl
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:2964a63f60ce3e3cf3ad55bcf190d7876d50e373cb64b70a57cea5885eaf3c86
+size 67584
diff --git a/ur5e/collision/wrist3.stl b/ur5e/collision/wrist3.stl
new file mode 100644
index 0000000000000000000000000000000000000000..2db25af90ecf3a43388f22482dd8344c34380df5
--- /dev/null
+++ b/ur5e/collision/wrist3.stl
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:83b3666b4ae2badd54af0d2c25a921682ecbc29e849eec646c3ed55fb74c78a3
+size 7184
diff --git a/ur5e/ur5e.urdf b/ur5e/ur5e.urdf
new file mode 100644
index 0000000000000000000000000000000000000000..6e571716bc852b0e4afefd3db9e895042de8cf9a
--- /dev/null
+++ b/ur5e/ur5e.urdf
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:3b0177194350c447e978403efc6ae36f6bbe33e6ac7ecc77c4c5dfae8fb09b1f
+size 9167
diff --git a/ur5e/visual/base.dae b/ur5e/visual/base.dae
new file mode 100644
index 0000000000000000000000000000000000000000..858cb12e429b814657e2cef0309956e930c18dd7
--- /dev/null
+++ b/ur5e/visual/base.dae
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:a7d37bb19bd062ca80ea00dfcd758b1145455c5cfad87d41756b101ac5b2a8a4
+size 358055
diff --git a/ur5e/visual/forearm.dae b/ur5e/visual/forearm.dae
new file mode 100644
index 0000000000000000000000000000000000000000..2483da94c077d3cb5a078906a71730bda836e628
--- /dev/null
+++ b/ur5e/visual/forearm.dae
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:6cb2f094ffba59f124f70881cabea985abfd399f2fbbe76fba7c18d2ece943b9
+size 1140936
diff --git a/ur5e/visual/shoulder.dae b/ur5e/visual/shoulder.dae
new file mode 100644
index 0000000000000000000000000000000000000000..d942729f28425811f372408f256c60f5c3279f35
--- /dev/null
+++ b/ur5e/visual/shoulder.dae
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:b2b58abbef50ce03d4704465d3b619a7da5b2ecb2efc236eadfd221116cbbef0
+size 1797082
diff --git a/ur5e/visual/upperarm.dae b/ur5e/visual/upperarm.dae
new file mode 100644
index 0000000000000000000000000000000000000000..712cceaaaffa5c2e96e060dbd4304faeee0ef8b6
--- /dev/null
+++ b/ur5e/visual/upperarm.dae
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:6486526456e11585a41080958b97a0c8821da856e93b2059c74b07f8102bf6cd
+size 3082485
diff --git a/ur5e/visual/wrist1.dae b/ur5e/visual/wrist1.dae
new file mode 100644
index 0000000000000000000000000000000000000000..4eaa7dba1291817b44551da862aadb5c7d4823ba
--- /dev/null
+++ b/ur5e/visual/wrist1.dae
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:c36649cf4deec6da427d72d45163a059dcb668eb61aac760d5f2f979948fa13a
+size 1334662
diff --git a/ur5e/visual/wrist2.dae b/ur5e/visual/wrist2.dae
new file mode 100644
index 0000000000000000000000000000000000000000..351f8942d4d382b536eb75fe4570e97b48be4596
--- /dev/null
+++ b/ur5e/visual/wrist2.dae
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:c99f5538357a7b42a9b207a12e52e713dd3eb7e587645144bde7ab31c1cfc76b
+size 1554838
diff --git a/ur5e/visual/wrist3.dae b/ur5e/visual/wrist3.dae
new file mode 100644
index 0000000000000000000000000000000000000000..4a91d5b7bd2c2709dc0ddbc0a8391327570fbcc7
--- /dev/null
+++ b/ur5e/visual/wrist3.dae
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:9f5d4f544ae72698a41f75ed4897a76386a91b814dff22e30e93a5bd105a717c
+size 66076
diff --git a/utils.py b/utils.py
new file mode 100644
index 0000000000000000000000000000000000000000..012ab4b4aed259e61230b29f5acd8904270f596c
--- /dev/null
+++ b/utils.py
@@ -0,0 +1,109 @@
+import os
+import requests
+import logging
+import openai
+from contextlib import contextmanager
+from tenacity import retry, wait_random_exponential, stop_after_attempt
+
+
+## 处理代理：
+http_proxy = ""
+https_proxy = ""
+http_proxy = os.environ.get("HTTP_PROXY", http_proxy)
+https_proxy = os.environ.get("HTTPS_PROXY", https_proxy)
+
+# 重置系统变量，在不需要设置的时候不设置环境变量，以免引起全局代理报错
+os.environ["HTTP_PROXY"] = ""
+os.environ["HTTPS_PROXY"] = ""
+
+# GPT_MODEL = "gpt-3.5-turbo-0613"
+GPT_MODEL = "gpt-4"
+
+
+@contextmanager
+def retrieve_proxy(proxy=None):
+    """
+    1, 如果proxy = NONE，设置环境变量，并返回最新设置的代理
+    2，如果proxy ！= NONE，更新当前的代理配置，但是不更新环境变量
+    """
+    global http_proxy, https_proxy
+    if proxy is not None:
+        old_var = os.environ["HTTP_PROXY"], os.environ["HTTPS_PROXY"]
+
+        os.environ["HTTP_PROXY"] = proxy
+        os.environ["HTTPS_PROXY"] = proxy
+        yield proxy, proxy
+
+        # return old proxy
+        os.environ["HTTP_PROXY"], os.environ["HTTPS_PROXY"] = old_var
+
+    else:
+        old_var = os.environ["HTTP_PROXY"], os.environ["HTTPS_PROXY"]
+        os.environ["HTTP_PROXY"] = http_proxy
+        os.environ["HTTPS_PROXY"] = https_proxy
+        yield http_proxy, https_proxy # return new proxy
+
+        # return old proxy
+        os.environ["HTTP_PROXY"], os.environ["HTTPS_PROXY"] = old_var
+
+
+@retry(wait=wait_random_exponential(multiplier=1, max=40), stop=stop_after_attempt(10))
+def get_geoip(proxy=None):
+    try:
+        with retrieve_proxy(proxy):
+            response = requests.get("https://ipapi.co/json/", timeout=5)
+        data = response.json()
+    except Exception as e:
+        data = {"error": True, "reason": f"{e}"}
+        return e
+
+    country = data["country_name"]
+    if country == "China":
+        text = "**您的IP区域：中国。请立即检查代理设置，在不受支持的地区使用API可能导致账号被封禁。**"
+        logging.error(text)
+        return -1, text
+    else:
+        text = f"您的IP区域：{country}。"
+        logging.info(text)
+        return 1, text
+
+
+@retry(wait=wait_random_exponential(multiplier=1, max=20), stop=stop_after_attempt(5))
+def chat_completion_request(
+    messages,
+    functions=None,
+    function_call=None,
+    model=GPT_MODEL,
+    proxy=None,
+    **kwargs,
+):
+    headers = {
+        "Content-Type": "application/json",
+        "Authorization": "Bearer " + openai.api_key,
+    }
+    json_data = {"model": model, "messages": messages}
+    if functions is not None:
+        json_data.update({"functions": functions})
+    if function_call is not None:
+        json_data.update({"function_call": function_call})
+    if kwargs is not None:
+        json_data.update(kwargs)
+    with retrieve_proxy(proxy):
+        try:
+            response = requests.post(
+                "https://api.openai.com/v1/chat/completions",
+                headers=headers,
+                json=json_data,
+            )
+            return response
+        except Exception as e:
+            print("Unable to generate ChatCompletion response")
+            print(f"Exception: {e}")
+            return e
+
+
+def generate_messages(prompt):
+    messages = []
+    messages.append({"role": "system", "content": "You are an robot auto control code generator."})
+    messages.append({"role": "user", "content": f"{prompt}"})        
+    return messages