working video

.gitattributes

@@ -29,3 +29,17 @@ 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
+ur5e/collision/wrist2.stl filter=lfs diff=lfs merge=lfs -text
+ur5e/collision/wrist3.stl filter=lfs diff=lfs merge=lfs -text
+ur5e/collision/base.stl filter=lfs diff=lfs merge=lfs -text
+ur5e/collision/forearm.stl filter=lfs diff=lfs merge=lfs -text
+ur5e/collision/shoulder.stl filter=lfs diff=lfs merge=lfs -text
+ur5e/collision/upperarm.stl filter=lfs diff=lfs merge=lfs -text
+ur5e/collision/wrist1.stl filter=lfs diff=lfs merge=lfs -text
+ur5e/visual filter=lfs diff=lfs merge=lfs -text
+robotiq_2f_85/robotiq-2f-pad.stl filter=lfs diff=lfs merge=lfs -text
+robotiq_2f_85/robotiq-2f-spring_link.stl filter=lfs diff=lfs merge=lfs -text
+robotiq_2f_85/robotiq-2f-base.stl filter=lfs diff=lfs merge=lfs -text
+robotiq_2f_85/robotiq-2f-coupler.stl filter=lfs diff=lfs merge=lfs -text
+robotiq_2f_85/robotiq-2f-driver.stl filter=lfs diff=lfs merge=lfs -text
+robotiq_2f_85/robotiq-2f-follower.stl filter=lfs diff=lfs merge=lfs -text

.gitignore

@@ -0,0 +1,162 @@
+# 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

app.py

@@ -0,0 +1,143 @@
+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
+from consts import ALL_BLOCKS, ALL_BOWLS
+
+
+class DemoRunner:
+
+    def __init__(self):
+        self._cfg = OmegaConf.to_container(OmegaConf.load('cfg.yaml'), resolve=True)
+        self._env = None
+        self._model_name = ''
+
+    def make_LMP(self, env):
+        # LMP env wrapper
+        cfg = copy.deepcopy(self._cfg)
+        cfg['env'] = {
+            'init_objs': list(env.obj_name_to_id.keys()),
+            'coords': cfg['tabletop_coords']
+        }
+        for vs in cfg['lmps'].values():
+            vs['engine'] = self._model_name
+
+        LMP_env = LMP_wrapper(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 [
+                'get_bbox', 'get_obj_pos', 'get_color', 'is_obj_visible', 'denormalize_xy',
+                'put_first_on_second', 'get_obj_names',
+                'get_corner_name', 'get_side_name',
+            ]
+        }
+        variable_vars['say'] = lambda msg: print(f'robot says: {msg}')
+
+        # creating the function-generating LMP
+        lmp_fgen = LMPFGen(cfg['lmps']['fgen'], fixed_vars, variable_vars)
+
+        # creating other low-level LMPs
+        variable_vars.update({
+            k: LMP(k, cfg['lmps'][k], lmp_fgen, fixed_vars, variable_vars)
+            for k in ['parse_obj_name', 'parse_position', 'parse_question', 'transform_shape_pts']
+        })
+
+        # creating the LMP that deals w/ high-level language commands
+        lmp_tabletop_ui = LMP(
+            'tabletop_ui', cfg['lmps']['tabletop_ui'], lmp_fgen, fixed_vars, variable_vars
+        )
+
+        return lmp_tabletop_ui
+
+    def setup(self, api_key, model_name, n_blocks, n_bowls):
+        openai.api_key = api_key
+        self._model_name = model_name
+
+        self._env = PickPlaceEnv(render=True, high_res=False, high_frame_rate=False)
+        block_list = np.random.choice(ALL_BLOCKS, size=n_blocks, replace=False).tolist()
+        bowl_list = np.random.choice(ALL_BOWLS, size=n_bowls, replace=False).tolist()
+        obj_list = block_list + bowl_list
+        self._env.reset(obj_list)
+
+        self._lmp_tabletop_ui = self.make_LMP(self._env)
+
+        info = '## Available objects: \n- ' + '\n- '.join(obj_list)
+        img = self._env.get_camera_image()
+
+        return info, img
+
+    def run(self, instruction):
+        if self._env is None:
+            return 'Please run setup first'
+
+        self._env.cache_video = []
+
+        self._lmp_tabletop_ui(instruction, f'objects = {self._env.object_list}')
+
+        video_file_name = ''
+        if self._env.cache_video:
+            rendered_clip = ImageSequenceClip(self._env.cache_video, fps=25)
+            video_file_name = NamedTemporaryFile(suffix='.mp4', delete=False).name
+            rendered_clip.write_videofile(video_file_name, fps=25)
+
+        return 'Done', video_file_name
+
+
+if __name__ == '__main__':
+    demo_runner = DemoRunner()
+    demo = gr.Blocks()
+
+    with demo:
+        with gr.Row():
+            with gr.Column():
+                with gr.Row():
+                    inp_api_key = gr.Textbox(label='OpenAI API Key', lines=1, value='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')
+                    inp_model_name = gr.Dropdown(label='Model Name', choices=['code-davinci-002', 'text-davinci-002'], value='code-davinci-002')
+                with gr.Row():
+                    inp_n_blocks = gr.Slider(label='Num Blocks', minimum=0, maximum=3, value=3, step=1)
+                    inp_n_bowls = gr.Slider(label='Num Bowls', minimum=0, maximum=3, value=3, step=1)
+            
+                btn_setup = gr.Button("1) Setup/Reset Env")
+                info_setup = gr.Markdown(label='Setup Info')
+            with gr.Column():
+                img_setup = gr.Image(label='Setup Image')
+
+        with gr.Row():
+            with gr.Column():
+                
+                inp_instruction = gr.Textbox(label='Instruction', lines=1) 
+                btn_run = gr.Button("2) Run Instruction")
+                info_run = gr.Label(label='Run Info')
+            with gr.Column():
+                video_run = gr.Video(label='Run Video')
+        
+        btn_setup.click(
+            demo_runner.setup, 
+            inputs=[inp_api_key, inp_model_name, inp_n_blocks, inp_n_bowls], 
+            outputs=[info_setup, img_setup]
+        )
+        btn_run.click(
+            demo_runner.run, 
+            inputs=[inp_instruction], 
+            outputs=[info_run, video_run]
+        )
+        
+    demo.launch()

bowl/bowl.urdf

@@ -0,0 +1,29 @@
+<?xml version="1.0" ?>
+<robot name="bowl.urdf">
+  <link name="baseLink">
+    <contact>
+      <lateral_friction value="1.0"/>
+      <inertia_scaling value="3.0"/>
+    </contact>
+    <inertial>
+      <origin rpy="0 0 0" xyz="-0.025 0 0.02"/>
+       <mass value=".1"/>
+       <inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+				<mesh filename="textured-0008192.obj" scale="1.25 1.25 0.25"/>
+      </geometry>
+      <material name="green">
+        <color rgba="0.34901961 0.6627451 0.30980392 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+	 	    <mesh filename="cup.obj" scale="1.25 1.25 0.25"/>
+      </geometry>
+    </collision>
+  </link>
+</robot>

bowl/cup.obj

@@ -0,0 +1,1413 @@
+# Blender v2.69 (sub 0) OBJ File: ''
+# www.blender.org
+mtllib cup_vhacd.mtl
+o ShapeIndexedFaceSet.013_ShapeIndexedFaceSet
+v 0.000000 0.000000 0.000000
+v -0.018137 0.054263 0.137619
+v -0.033504 0.036700 0.027862
+v -0.033504 0.041091 0.027862
+v -0.033504 0.052066 0.139822
+v -0.018137 0.045481 0.122254
+v -0.018137 0.043286 0.027862
+v -0.033504 0.045481 0.139822
+v -0.022528 0.036700 0.030065
+v -0.029114 0.043286 0.027862
+v -0.031308 0.041091 0.100303
+v -0.018137 0.047673 0.139822
+v -0.018137 0.038897 0.027862
+v -0.029114 0.052066 0.122254
+v -0.033504 0.036700 0.045430
+v -0.020333 0.054263 0.135427
+v -0.029114 0.043286 0.122254
+v -0.033504 0.043286 0.041035
+v -0.018137 0.045481 0.045430
+v -0.018137 0.038897 0.041035
+v -0.033504 0.052066 0.135427
+v -0.018137 0.043286 0.098100
+v -0.022528 0.045481 0.045430
+v -0.031308 0.043286 0.032257
+v -0.033504 0.041091 0.100303
+v -0.018137 0.052066 0.139822
+v -0.031308 0.036700 0.045430
+v -0.031308 0.045481 0.139822
+usemtl Shape.027
+s off
+f 17 12 28
+f 4 3 5
+f 3 4 7
+f 2 6 7
+f 5 3 8
+f 7 4 10
+f 6 2 12
+f 5 8 12
+f 3 7 13
+f 7 6 13
+f 9 3 13
+f 8 3 15
+f 3 9 15
+f 5 2 16
+f 10 14 16
+f 11 9 17
+f 6 12 17
+f 4 5 18
+f 2 7 19
+f 7 10 19
+f 16 2 19
+f 13 6 20
+f 9 13 20
+f 5 16 21
+f 16 14 21
+f 18 5 21
+f 6 17 22
+f 17 9 22
+f 20 6 22
+f 9 20 22
+f 10 16 23
+f 19 10 23
+f 16 19 23
+f 10 4 24
+f 14 10 24
+f 4 18 24
+f 21 14 24
+f 18 21 24
+f 8 15 25
+f 15 11 25
+f 17 8 25
+f 11 17 25
+f 2 5 26
+f 12 2 26
+f 5 12 26
+f 9 11 27
+f 15 9 27
+f 11 15 27
+f 12 8 28
+f 8 17 28
+o ShapeIndexedFaceSet.012_ShapeIndexedFaceSet.027
+v 0.000000 0.000000 0.000000
+v -0.033504 0.049872 0.139822
+v -0.046676 0.025724 0.027862
+v -0.046676 0.034505 0.027862
+v -0.033504 0.034505 0.027862
+v -0.046676 0.036704 0.133235
+v -0.046676 0.045480 0.131032
+v -0.035700 0.041091 0.027862
+v -0.033504 0.043284 0.133235
+v -0.042285 0.025724 0.030065
+v -0.037896 0.049872 0.124446
+v -0.044480 0.034505 0.117848
+v -0.033504 0.041091 0.027862
+v -0.044480 0.047676 0.135427
+v -0.046676 0.038892 0.139822
+v -0.042285 0.038892 0.036651
+v -0.033504 0.049872 0.120062
+v -0.035700 0.043284 0.139822
+v -0.040090 0.049872 0.139822
+v -0.046676 0.025724 0.034449
+v -0.037896 0.041091 0.034449
+v -0.033504 0.038892 0.084927
+v -0.046676 0.045480 0.139822
+v -0.044480 0.036704 0.133235
+v -0.044480 0.036704 0.030065
+v -0.033504 0.034505 0.032257
+v -0.042285 0.047676 0.122254
+v -0.046676 0.034505 0.117848
+v -0.035700 0.043284 0.049824
+v -0.042285 0.025724 0.027862
+v -0.044480 0.025724 0.034449
+v -0.033504 0.041091 0.111273
+usemtl Shape.026
+s off
+f 38 50 60
+f 31 32 33
+f 32 31 34
+f 32 34 35
+f 33 32 36
+f 33 30 37
+f 38 37 40
+f 30 33 41
+f 33 36 41
+f 35 34 43
+f 39 30 45
+f 30 41 45
+f 37 30 46
+f 30 43 46
+f 30 39 47
+f 39 42 47
+f 43 30 47
+f 34 31 48
+f 31 38 48
+f 33 37 50
+f 42 35 51
+f 35 43 51
+f 47 42 51
+f 43 47 51
+f 34 40 52
+f 40 37 52
+f 43 34 52
+f 37 46 52
+f 46 43 52
+f 32 35 53
+f 36 32 53
+f 35 42 53
+f 42 44 53
+f 49 36 53
+f 44 49 53
+f 38 33 54
+f 33 50 54
+f 50 38 54
+f 42 39 55
+f 44 42 55
+f 39 49 55
+f 49 44 55
+f 40 34 56
+f 34 48 56
+f 48 40 56
+f 41 36 57
+f 39 45 57
+f 45 41 57
+f 36 49 57
+f 49 39 57
+f 31 33 58
+f 38 31 58
+f 33 38 58
+f 38 40 59
+f 48 38 59
+f 40 48 59
+f 37 38 60
+f 50 37 60
+o ShapeIndexedFaceSet.011_ShapeIndexedFaceSet.026
+v 0.000000 0.000000 0.000000
+v -0.066434 -0.002813 0.135413
+v -0.051068 -0.007203 0.058606
+v -0.051068 -0.009399 0.058606
+v -0.057654 0.005965 0.038839
+v -0.059846 0.005965 0.137608
+v -0.057654 -0.009399 0.139812
+v -0.057654 -0.009399 0.052024
+v -0.053264 0.005965 0.038839
+v -0.066434 0.005965 0.139812
+v -0.064236 -0.009399 0.139812
+v -0.051068 -0.009399 0.038839
+v -0.059846 0.001575 0.049829
+v -0.057654 -0.007203 0.041052
+v -0.055458 -0.007203 0.122248
+v -0.066434 -0.005009 0.137608
+v -0.059846 0.005965 0.049829
+v -0.057654 0.005965 0.109073
+v -0.055458 -0.009399 0.038839
+v -0.053264 0.005965 0.043247
+v -0.066434 0.005965 0.135413
+v -0.051068 -0.005009 0.038839
+v -0.062042 0.005965 0.139812
+v -0.055458 -0.009399 0.122248
+v -0.064236 -0.009399 0.135413
+v -0.057654 -0.007203 0.139812
+v -0.057654 -0.005009 0.038839
+v -0.053264 -0.007203 0.091519
+usemtl Shape.025
+s off
+f 75 84 88
+f 64 67 68
+f 65 66 69
+f 66 65 70
+f 67 70 71
+f 68 67 71
+f 63 64 72
+f 64 68 72
+f 65 69 72
+f 70 62 76
+f 71 70 76
+f 62 73 76
+f 74 68 76
+f 73 74 76
+f 70 65 77
+f 65 73 77
+f 73 62 77
+f 69 66 78
+f 66 75 78
+f 72 68 79
+f 65 72 79
+f 68 74 79
+f 78 63 80
+f 69 78 80
+f 62 70 81
+f 77 62 81
+f 70 77 81
+f 63 72 82
+f 72 69 82
+f 80 63 82
+f 69 80 82
+f 66 70 83
+f 70 67 83
+f 67 64 84
+f 75 67 84
+f 68 71 85
+f 71 76 85
+f 76 68 85
+f 75 66 86
+f 67 75 86
+f 66 83 86
+f 83 67 86
+f 73 65 87
+f 74 73 87
+f 65 79 87
+f 79 74 87
+f 64 63 88
+f 63 78 88
+f 78 75 88
+f 84 64 88
+o ShapeIndexedFaceSet.010_ShapeIndexedFaceSet.025
+v 0.000000 0.000000 0.000000
+v -0.062040 -0.013792 0.122248
+v -0.042286 -0.018180 0.049829
+v -0.053261 -0.009401 0.104685
+v -0.055455 -0.009401 0.038839
+v -0.059844 -0.020377 0.139812
+v -0.051069 -0.020377 0.049829
+v -0.051069 -0.020377 0.139812
+v -0.064236 -0.009401 0.139812
+v -0.048873 -0.009401 0.038839
+v -0.042286 -0.020377 0.038839
+v -0.055455 -0.011597 0.139812
+v -0.053261 -0.015986 0.038839
+v -0.048873 -0.018180 0.122248
+v -0.055455 -0.011597 0.038839
+v -0.057651 -0.009401 0.139812
+v -0.064236 -0.011597 0.137608
+v -0.048873 -0.020377 0.038839
+v -0.042286 -0.020377 0.049829
+v -0.048873 -0.009401 0.045441
+v -0.059844 -0.018180 0.122248
+v -0.055455 -0.013792 0.047635
+v -0.051069 -0.018180 0.139812
+v -0.042286 -0.018180 0.038839
+v -0.053261 -0.011597 0.122248
+v -0.059844 -0.020377 0.137608
+v -0.048873 -0.020377 0.122248
+usemtl Shape.024
+s off
+f 107 102 115
+f 94 95 96
+f 92 93 97
+f 94 96 97
+f 93 92 98
+f 96 95 99
+f 93 98 99
+f 97 96 100
+f 93 99 101
+f 93 101 103
+f 92 97 104
+f 100 92 104
+f 97 100 104
+f 97 93 105
+f 94 97 105
+f 93 103 105
+f 99 95 106
+f 95 101 106
+f 101 99 106
+f 99 91 107
+f 96 99 107
+f 91 102 107
+f 92 91 108
+f 98 92 108
+f 91 98 108
+f 103 101 110
+f 90 105 110
+f 105 103 110
+f 109 90 110
+f 101 109 110
+f 100 96 111
+f 96 102 111
+f 102 100 111
+f 98 91 112
+f 91 99 112
+f 99 98 112
+f 91 92 113
+f 92 100 113
+f 102 91 113
+f 100 102 113
+f 95 94 114
+f 101 95 114
+f 105 90 114
+f 94 105 114
+f 90 109 114
+f 109 101 114
+f 102 96 115
+f 96 107 115
+o ShapeIndexedFaceSet.009_ShapeIndexedFaceSet.024
+v 0.000000 0.000000 0.000000
+v -0.018137 0.043284 0.036653
+v 0.019183 0.034509 0.139822
+v 0.019183 0.030119 0.139822
+v -0.018137 0.047673 0.139822
+v -0.004964 0.052067 0.135419
+v 0.001618 0.038898 0.036653
+v -0.000573 0.030119 0.038855
+v 0.010401 0.030119 0.045439
+v 0.012596 0.030119 0.137610
+v -0.018137 0.038898 0.045439
+v 0.008205 0.045480 0.139822
+v -0.018137 0.052067 0.120048
+v -0.007160 0.043284 0.038855
+v 0.010401 0.032313 0.045439
+v -0.018137 0.043284 0.100295
+v -0.015942 0.045480 0.045439
+v -0.018137 0.052067 0.139822
+v 0.008205 0.045480 0.135419
+v 0.008205 0.030119 0.036653
+v 0.019183 0.030119 0.122250
+v -0.018137 0.038898 0.036653
+v 0.001618 0.047673 0.120048
+v 0.019183 0.034509 0.135419
+v 0.008205 0.030119 0.109071
+v -0.011551 0.052067 0.122250
+v -0.004964 0.052067 0.139822
+v -0.018137 0.045480 0.122250
+v 0.014792 0.038898 0.133217
+v 0.008205 0.034509 0.045439
+v 0.019183 0.032313 0.122250
+v -0.018137 0.045480 0.045439
+v -0.000573 0.041092 0.045439
+v 0.014792 0.030119 0.139822
+v -0.000573 0.030119 0.036653
+v -0.011551 0.045480 0.052024
+v -0.009355 0.043284 0.036653
+v 0.008205 0.032313 0.036653
+v -0.002769 0.049869 0.122250
+v 0.006009 0.045480 0.122250
+usemtl Shape.023
+s off
+f 148 138 155
+f 119 118 120
+f 119 123 124
+f 123 119 125
+f 120 117 126
+f 120 118 127
+f 117 120 128
+f 120 126 131
+f 120 127 133
+f 128 120 133
+f 121 128 133
+f 121 127 134
+f 117 122 135
+f 124 123 135
+f 130 124 135
+f 118 119 136
+f 119 124 136
+f 124 130 136
+f 123 126 137
+f 126 117 137
+f 117 135 137
+f 121 134 138
+f 118 136 139
+f 123 125 140
+f 126 123 140
+f 125 131 140
+f 131 126 140
+f 128 121 141
+f 132 128 141
+f 127 121 142
+f 133 127 142
+f 121 133 142
+f 125 120 143
+f 120 131 143
+f 131 125 143
+f 127 118 144
+f 134 127 144
+f 118 139 144
+f 139 130 144
+f 122 134 145
+f 134 144 145
+f 144 130 145
+f 136 130 146
+f 130 139 146
+f 139 136 146
+f 117 128 147
+f 132 117 147
+f 128 132 147
+f 122 129 148
+f 119 120 149
+f 125 119 149
+f 120 125 149
+f 135 123 150
+f 123 137 150
+f 137 135 150
+f 121 129 151
+f 129 132 151
+f 141 121 151
+f 132 141 151
+f 122 117 152
+f 129 122 152
+f 117 132 152
+f 132 129 152
+f 135 122 153
+f 130 135 153
+f 122 145 153
+f 145 130 153
+f 129 121 154
+f 121 138 154
+f 148 129 154
+f 138 148 154
+f 134 122 155
+f 138 134 155
+f 122 148 155
+o ShapeIndexedFaceSet.008_ShapeIndexedFaceSet.023
+v 0.000000 0.000000 0.000000
+v 0.014792 0.030114 0.139822
+v 0.012598 0.005966 0.036653
+v 0.016986 0.005966 0.036653
+v 0.010404 0.030114 0.036653
+v 0.025768 0.021331 0.139822
+v 0.021376 0.005966 0.139822
+v 0.003816 0.027919 0.049832
+v 0.016986 0.019135 0.036653
+v 0.025768 0.005966 0.120048
+v 0.021376 0.030114 0.135419
+v 0.003816 0.030114 0.036653
+v 0.010404 0.027919 0.113464
+v 0.019180 0.008162 0.043248
+v 0.025768 0.016946 0.122250
+v 0.019180 0.005966 0.122250
+v 0.025768 0.005966 0.139822
+v 0.012598 0.027919 0.036653
+v 0.021376 0.027919 0.120048
+v 0.012598 0.005966 0.041046
+v 0.010404 0.030114 0.113464
+v 0.025768 0.021331 0.135419
+v 0.014792 0.025720 0.137610
+v 0.019180 0.005966 0.043248
+v 0.021376 0.030114 0.139822
+v 0.003816 0.027919 0.036653
+v 0.012598 0.030114 0.049832
+v 0.003816 0.030114 0.049832
+v 0.019180 0.012557 0.047631
+v 0.014792 0.025720 0.045439
+v 0.016986 0.005966 0.095902
+v 0.025768 0.012557 0.120048
+usemtl Shape.022
+s off
+f 169 170 187
+f 159 158 160
+f 158 159 162
+f 161 157 162
+f 159 160 164
+f 162 159 165
+f 160 157 166
+f 160 158 167
+f 157 160 167
+f 159 164 169
+f 161 165 170
+f 158 162 171
+f 168 163 171
+f 162 168 171
+f 161 162 172
+f 165 161 172
+f 162 165 172
+f 164 160 173
+f 163 158 175
+f 158 171 175
+f 157 167 176
+f 163 168 176
+f 168 157 176
+f 166 161 177
+f 161 170 177
+f 170 164 177
+f 174 166 177
+f 164 174 177
+f 162 157 178
+f 157 168 178
+f 168 162 178
+f 165 159 179
+f 159 169 179
+f 169 165 179
+f 157 161 180
+f 166 157 180
+f 161 166 180
+f 158 163 181
+f 167 158 181
+f 163 167 181
+f 160 166 182
+f 173 160 182
+f 166 174 182
+f 174 173 182
+f 167 163 183
+f 176 167 183
+f 163 176 183
+f 169 164 184
+f 170 169 184
+f 164 170 184
+f 164 173 185
+f 173 174 185
+f 174 164 185
+f 171 163 186
+f 163 175 186
+f 175 171 186
+f 165 169 187
+f 170 165 187
+o ShapeIndexedFaceSet.007_ShapeIndexedFaceSet.022
+v 0.000000 0.000000 0.000000
+v -0.051069 0.043286 0.139822
+v -0.057651 0.005966 0.027862
+v -0.057651 0.014748 0.027862
+v -0.066430 0.016948 0.139822
+v -0.059846 0.005966 0.135427
+v -0.046678 0.023530 0.036651
+v -0.046678 0.036700 0.139822
+v -0.048875 0.032305 0.027862
+v -0.051069 0.008162 0.030065
+v -0.062040 0.030110 0.135427
+v -0.066430 0.005966 0.139822
+v -0.046678 0.043286 0.122254
+v -0.046678 0.032305 0.027862
+v -0.055459 0.023530 0.036651
+v -0.057651 0.036700 0.135427
+v -0.064235 0.025722 0.137619
+v -0.053264 0.005966 0.027862
+v -0.066430 0.016948 0.135427
+v -0.046678 0.032305 0.115667
+v -0.059846 0.005966 0.049824
+v -0.051069 0.030110 0.030065
+v -0.057651 0.005966 0.109081
+v -0.051069 0.043286 0.135427
+v -0.062040 0.005966 0.139822
+v -0.046678 0.043286 0.139822
+v -0.046678 0.023530 0.027862
+v -0.062040 0.030110 0.139822
+v -0.048875 0.043286 0.122254
+v -0.066430 0.005966 0.135427
+v -0.055459 0.021335 0.027862
+v -0.053264 0.005966 0.043238
+v -0.064235 0.021335 0.122254
+v -0.057651 0.016948 0.034449
+v -0.059846 0.008162 0.049824
+usemtl Shape.021
+s off
+f 208 206 222
+f 189 192 195
+f 190 191 196
+f 190 193 199
+f 195 192 199
+f 195 194 200
+f 196 200 201
+f 190 196 201
+f 200 194 201
+f 198 202 204
+f 193 190 205
+f 190 201 205
+f 199 192 206
+f 192 204 206
+f 194 195 207
+f 197 194 207
+f 191 190 208
+f 190 199 208
+f 202 198 209
+f 203 196 209
+f 198 203 209
+f 195 193 210
+f 193 205 210
+f 207 195 210
+f 197 207 210
+f 189 200 211
+f 203 189 211
+f 196 203 211
+f 193 195 212
+f 199 193 212
+f 195 199 212
+f 189 195 213
+f 200 189 213
+f 195 200 213
+f 194 197 214
+f 201 194 214
+f 197 205 214
+f 205 201 214
+f 192 189 215
+f 189 203 215
+f 203 198 215
+f 198 204 215
+f 204 192 215
+f 200 196 216
+f 211 200 216
+f 196 211 216
+f 199 206 217
+f 208 199 217
+f 206 208 217
+f 196 191 218
+f 204 202 218
+f 209 196 218
+f 202 209 218
+f 205 197 219
+f 210 205 219
+f 197 210 219
+f 191 206 220
+f 206 204 220
+f 218 191 221
+f 204 218 221
+f 191 220 221
+f 220 204 221
+f 206 191 222
+f 191 208 222
+o ShapeIndexedFaceSet.006_ShapeIndexedFaceSet.021
+v 0.000000 0.000000 0.000000
+v 0.023572 -0.013791 0.139822
+v 0.001623 -0.020377 0.041034
+v 0.008210 -0.020377 0.041034
+v 0.014791 0.005966 0.041034
+v 0.021376 0.005966 0.139822
+v 0.012600 -0.020377 0.139822
+v 0.019181 0.001575 0.043239
+v 0.025768 0.005966 0.120056
+v 0.001623 -0.018180 0.045434
+v 0.019181 -0.020377 0.124437
+v 0.014791 -0.013791 0.045434
+v 0.008210 -0.018180 0.113471
+v 0.016987 0.005966 0.098096
+v 0.025768 -0.007205 0.135422
+v 0.025768 0.005966 0.139822
+v 0.016987 -0.007205 0.041034
+v 0.019181 0.005966 0.043239
+v 0.014791 0.005966 0.067384
+v 0.023572 -0.011594 0.122251
+v 0.025768 -0.002819 0.122251
+v 0.010404 -0.020377 0.045434
+v 0.019181 -0.020377 0.139822
+v 0.019181 0.005966 0.122251
+v 0.012600 -0.018180 0.139822
+v 0.008210 -0.020377 0.113471
+v 0.025768 -0.005014 0.126642
+v 0.012600 -0.018180 0.049824
+v 0.012600 -0.015986 0.041034
+v 0.001623 -0.018180 0.041034
+v 0.025768 -0.007205 0.139822
+v 0.021376 -0.018180 0.135422
+v 0.006014 -0.018180 0.091520
+v 0.019181 -0.002819 0.052019
+v 0.016987 -0.009400 0.047629
+v 0.025768 -0.000620 0.120056
+v 0.021376 -0.015986 0.122251
+usemtl Shape.020
+s off
+f 254 233 259
+f 226 225 227
+f 225 226 229
+f 224 228 229
+f 227 228 231
+f 229 226 233
+f 228 227 236
+f 228 224 238
+f 231 228 238
+f 237 231 238
+f 226 227 239
+f 227 231 240
+f 231 230 240
+f 239 227 240
+f 230 239 240
+f 227 232 241
+f 232 236 241
+f 236 227 241
+f 237 224 242
+f 231 237 243
+f 233 226 244
+f 224 229 245
+f 229 233 245
+f 235 228 246
+f 236 232 246
+f 228 236 246
+f 229 228 247
+f 235 229 247
+f 228 235 247
+f 225 229 248
+f 232 225 248
+f 229 235 248
+f 237 242 249
+f 242 239 249
+f 243 237 249
+f 234 233 250
+f 233 244 250
+f 250 244 251
+f 226 239 251
+f 239 234 251
+f 244 226 251
+f 234 250 251
+f 227 225 252
+f 225 232 252
+f 232 227 252
+f 224 237 253
+f 238 224 253
+f 237 238 253
+f 242 224 254
+f 224 245 254
+f 245 233 254
+f 235 246 255
+f 246 232 255
+f 232 248 255
+f 248 235 255
+f 239 230 256
+f 230 243 256
+f 249 239 256
+f 243 249 256
+f 234 239 257
+f 242 234 257
+f 239 242 257
+f 230 231 258
+f 243 230 258
+f 231 243 258
+f 233 234 259
+f 234 242 259
+f 242 254 259
+o ShapeIndexedFaceSet.005_ShapeIndexedFaceSet.020
+v 0.000000 0.000000 0.000000
+v -0.004961 -0.037936 0.122251
+v 0.019183 -0.020377 0.139822
+v 0.019183 -0.022576 0.139822
+v 0.006008 -0.020377 0.041034
+v -0.020333 -0.033546 0.133227
+v -0.020333 -0.033546 0.045434
+v -0.018136 -0.026966 0.043239
+v -0.000575 -0.029160 0.045434
+v 0.010398 -0.020377 0.139822
+v -0.020333 -0.040133 0.139822
+v 0.003815 -0.035741 0.139822
+v -0.000575 -0.020377 0.052019
+v 0.008205 -0.022576 0.045434
+v 0.010398 -0.031351 0.135422
+v -0.007154 -0.031351 0.041034
+v -0.009351 -0.040133 0.135422
+v -0.020333 -0.029160 0.071784
+v -0.020333 -0.029160 0.041034
+v -0.020333 -0.035741 0.139822
+v -0.020333 -0.040133 0.128837
+v -0.015939 -0.033546 0.045434
+v -0.000575 -0.037936 0.137607
+v 0.006008 -0.020377 0.111276
+v -0.000575 -0.020377 0.041034
+v 0.008205 -0.020377 0.045434
+v 0.019183 -0.022576 0.137607
+v -0.020333 -0.031351 0.104681
+v 0.003815 -0.026966 0.052019
+v 0.001618 -0.026966 0.041034
+v 0.003815 -0.035741 0.137607
+v -0.009351 -0.040133 0.139822
+v 0.016985 -0.024771 0.135422
+v -0.020333 -0.031351 0.041034
+v -0.011545 -0.033546 0.054224
+v -0.018136 -0.040133 0.128837
+v 0.010398 -0.031351 0.139822
+usemtl Shape.019
+s off
+f 274 292 296
+f 263 262 269
+f 262 264 269
+f 265 266 270
+f 263 269 270
+f 263 270 271
+f 269 264 272
+f 275 261 276
+f 266 265 277
+f 264 275 278
+f 266 277 278
+f 277 267 278
+f 269 265 279
+f 265 270 279
+f 270 269 279
+f 270 266 280
+f 276 270 280
+f 266 275 281
+f 280 266 281
+f 261 275 282
+f 275 268 282
+f 276 261 282
+f 272 267 283
+f 269 272 283
+f 267 277 283
+f 272 264 284
+f 267 272 284
+f 264 278 284
+f 278 267 284
+f 264 262 285
+f 273 264 285
+f 273 285 286
+f 262 263 286
+f 285 262 286
+f 265 269 287
+f 277 265 287
+f 269 283 287
+f 283 277 287
+f 274 268 288
+f 273 274 288
+f 264 273 289
+f 275 264 289
+f 268 275 289
+f 288 268 289
+f 273 288 289
+f 268 274 290
+f 274 271 290
+f 282 268 290
+f 271 282 290
+f 271 270 291
+f 270 276 291
+f 282 271 291
+f 276 282 291
+f 274 273 292
+f 273 286 292
+f 286 263 292
+f 275 266 293
+f 266 278 293
+f 278 275 293
+f 275 276 294
+f 276 281 294
+f 281 275 294
+f 276 280 295
+f 281 276 295
+f 280 281 295
+f 263 271 296
+f 271 274 296
+f 292 263 296
+o ShapeIndexedFaceSet.004_ShapeIndexedFaceSet.019
+v 0.000000 0.000000 0.000000
+v -0.046671 -0.022576 0.038839
+v -0.020333 -0.035741 0.139812
+v -0.020333 -0.040133 0.139812
+v -0.057652 -0.020377 0.139812
+v -0.022528 -0.026966 0.038839
+v -0.037897 -0.037936 0.135413
+v -0.024724 -0.033546 0.045441
+v -0.048870 -0.020377 0.139812
+v -0.040088 -0.020377 0.038839
+v -0.046671 -0.033546 0.139812
+v -0.033509 -0.031351 0.038839
+v -0.020333 -0.031351 0.106879
+v -0.020333 -0.033546 0.045441
+v -0.048870 -0.022576 0.045441
+v -0.031314 -0.040133 0.137608
+v -0.057652 -0.022576 0.135413
+v -0.040088 -0.029160 0.045441
+v -0.048870 -0.020377 0.045441
+v -0.020333 -0.029160 0.071781
+v -0.040088 -0.020377 0.054218
+v -0.053261 -0.026966 0.126637
+v -0.020333 -0.033546 0.131025
+v -0.020333 -0.040133 0.131025
+v -0.020333 -0.031351 0.038839
+v -0.046671 -0.033546 0.137608
+v -0.046671 -0.024771 0.047635
+v -0.022528 -0.026966 0.043247
+v -0.046671 -0.020377 0.122248
+v -0.042284 -0.026966 0.038839
+v -0.031314 -0.040133 0.139812
+v -0.020333 -0.029160 0.038839
+v -0.044476 -0.033546 0.122248
+v -0.022528 -0.033546 0.137608
+v -0.057652 -0.022576 0.139812
+v -0.046671 -0.020377 0.038839
+v -0.022528 -0.040133 0.131025
+v -0.035701 -0.031351 0.047635
+v -0.057652 -0.020377 0.135413
+usemtl Shape.018
+s off
+f 315 313 335
+f 300 299 301
+f 301 299 305
+f 302 298 306
+f 301 305 306
+f 300 301 307
+f 298 302 308
+f 299 300 309
+f 304 308 310
+f 309 300 310
+f 303 308 312
+f 308 304 312
+f 301 306 315
+f 298 311 315
+f 311 313 315
+f 305 309 316
+f 309 310 316
+f 302 306 317
+f 306 305 317
+f 313 311 318
+f 299 309 319
+f 309 305 319
+f 310 300 320
+f 304 310 320
+f 300 312 320
+f 308 302 321
+f 310 308 321
+f 316 310 321
+f 303 307 322
+f 307 318 322
+f 322 318 323
+f 318 311 323
+f 314 322 323
+f 316 302 324
+f 302 317 324
+f 324 317 325
+f 305 316 325
+f 317 305 325
+f 316 324 325
+f 298 308 326
+f 311 298 326
+f 308 314 326
+f 323 311 326
+f 314 323 326
+f 300 307 327
+f 307 303 327
+f 312 300 327
+f 303 312 327
+f 302 316 328
+f 321 302 328
+f 316 321 328
+f 314 303 329
+f 303 322 329
+f 322 314 329
+f 305 299 330
+f 299 319 330
+f 319 305 330
+f 307 301 331
+f 301 313 331
+f 318 307 331
+f 313 318 331
+f 306 298 332
+f 298 315 332
+f 315 306 332
+f 312 304 333
+f 304 320 333
+f 320 312 333
+f 308 303 334
+f 303 314 334
+f 314 308 334
+f 313 301 335
+f 301 315 335
+o ShapeIndexedFaceSet.003_ShapeIndexedFaceSet.018
+v 0.000000 0.000000 0.000000
+v 0.001618 0.038895 0.036643
+v -0.004962 0.005966 -0.000677
+v 0.014790 0.005966 -0.000677
+v -0.013743 0.041091 -0.000677
+v -0.018137 0.005966 0.025666
+v 0.016987 0.005966 0.036643
+v -0.018137 0.043286 0.036643
+v 0.008206 0.030110 0.003714
+v -0.018137 0.005966 0.001519
+v 0.014790 0.023530 0.036643
+v -0.018137 0.041091 -0.000677
+v 0.014790 0.016948 0.001519
+v -0.004962 0.038895 -0.000677
+v 0.010400 0.005966 0.036643
+v -0.009353 0.043286 0.030057
+v -0.018137 0.038895 0.036643
+v 0.016987 0.014748 0.023467
+v 0.012597 0.021335 -0.000677
+v 0.012597 0.027918 0.034448
+v 0.016987 0.005966 0.012500
+v 0.006009 0.032305 0.001519
+v -0.018137 0.023530 -0.000677
+v -0.018137 0.043286 0.019079
+v -0.002765 0.038895 0.005910
+v 0.016987 0.016948 0.036643
+v 0.008206 0.032305 0.023467
+v 0.012597 0.023530 0.005910
+v -0.009353 0.043286 0.036643
+v -0.009353 0.041091 0.003714
+v -0.002765 0.041091 0.032252
+v 0.014790 0.023530 0.032252
+v -0.013743 0.043286 0.019079
+v 0.003816 0.036700 0.027861
+v 0.001618 0.036700 0.010301
+v 0.016987 0.016948 0.030057
+v 0.008206 0.027918 -0.000677
+usemtl Shape.017
+s off
+f 357 344 372
+f 339 338 340
+f 338 339 341
+f 341 339 342
+f 342 337 343
+f 338 341 345
+f 341 343 345
+f 337 342 346
+f 340 338 347
+f 345 343 347
+f 339 340 349
+f 341 342 350
+f 342 343 350
+f 343 341 352
+f 341 350 352
+f 350 343 352
+f 348 339 354
+f 339 349 354
+f 337 346 355
+f 342 339 356
+f 339 348 356
+f 353 342 356
+f 348 353 356
+f 338 345 358
+f 347 338 358
+f 345 347 358
+f 340 347 359
+f 347 343 359
+f 343 351 359
+f 357 349 360
+f 346 342 361
+f 342 353 361
+f 355 344 362
+f 344 357 362
+f 354 344 363
+f 348 354 363
+f 344 355 363
+f 343 337 364
+f 351 343 364
+f 349 340 365
+f 351 360 365
+f 360 349 365
+f 337 360 366
+f 360 351 366
+f 364 337 366
+f 351 364 366
+f 355 346 367
+f 346 361 367
+f 348 363 367
+f 363 355 367
+f 340 359 368
+f 359 351 368
+f 365 340 368
+f 351 365 368
+f 337 355 369
+f 355 362 369
+f 362 357 369
+f 360 337 370
+f 357 360 370
+f 337 369 370
+f 369 357 370
+f 353 348 371
+f 361 353 371
+f 348 367 371
+f 367 361 371
+f 344 354 372
+f 354 349 372
+f 349 357 372
+o ShapeIndexedFaceSet.002_ShapeIndexedFaceSet.017
+v 0.000000 0.000000 0.000000
+v -0.053255 0.021335 0.001519
+v -0.018137 0.043286 0.027862
+v -0.018137 0.043286 0.016883
+v -0.018137 0.005966 0.025666
+v -0.057649 0.005966 0.027862
+v -0.018137 0.005966 0.001519
+v -0.046672 0.034504 0.027862
+v -0.026925 0.041091 -0.000677
+v -0.055452 0.005966 -0.000677
+v -0.018137 0.041091 -0.000677
+v -0.042283 0.034504 -0.000677
+v -0.035704 0.041091 0.023468
+v -0.057649 0.014748 0.027862
+v -0.018137 0.025722 -0.000677
+v -0.053255 0.025722 0.021272
+v -0.029117 0.043286 0.027862
+v -0.048865 0.027918 -0.000677
+v -0.040086 0.005966 -0.000677
+v -0.037893 0.038895 0.008106
+v -0.055452 0.016948 0.003718
+v -0.018137 0.036700 0.027862
+v -0.051062 0.005966 0.027862
+v -0.044476 0.034504 0.005911
+v -0.057649 0.008162 0.016883
+v -0.031314 0.041091 0.003718
+v -0.051062 0.027918 0.012500
+v -0.055452 0.021335 0.027862
+v -0.055452 0.012553 -0.000677
+v -0.040086 0.038895 0.021272
+v -0.026925 0.043286 0.021272
+v -0.040086 0.038895 0.027862
+v -0.053255 0.023530 0.008106
+v -0.055452 0.021335 0.023468
+v -0.035704 0.038895 0.001519
+usemtl Shape.016
+s off
+f 392 398 407
+f 376 375 377
+f 376 377 379
+f 377 378 379
+f 378 375 380
+f 379 378 382
+f 382 381 383
+f 376 379 383
+f 381 376 383
+f 381 382 384
+f 378 380 386
+f 382 383 387
+f 383 379 387
+f 375 376 389
+f 380 375 389
+f 384 382 390
+f 379 382 391
+f 387 379 391
+f 382 387 391
+f 377 375 394
+f 375 378 394
+f 394 378 395
+f 378 377 395
+f 377 394 395
+f 384 390 396
+f 382 378 397
+f 378 386 397
+f 386 393 397
+f 385 389 398
+f 392 385 398
+f 388 380 399
+f 380 396 399
+f 396 390 399
+f 386 380 400
+f 380 388 400
+f 374 390 401
+f 390 382 401
+f 393 374 401
+f 382 397 401
+f 397 393 401
+f 385 392 402
+f 396 380 402
+f 392 396 402
+f 376 381 403
+f 389 376 403
+f 381 398 403
+f 398 389 403
+f 380 389 404
+f 389 385 404
+f 402 380 404
+f 385 402 404
+f 390 374 405
+f 374 393 405
+f 388 399 405
+f 399 390 405
+f 393 386 406
+f 386 400 406
+f 400 388 406
+f 405 393 406
+f 388 405 406
+f 381 384 407
+f 384 396 407
+f 396 392 407
+f 398 381 407
+o ShapeIndexedFaceSet.001_ShapeIndexedFaceSet.016
+v 0.000000 0.000000 0.000000
+v 0.006010 -0.022571 0.023469
+v 0.016987 0.005966 0.041034
+v 0.012596 0.005966 0.041034
+v -0.004964 0.005966 -0.000677
+v -0.020333 -0.031353 0.041034
+v -0.020333 0.005966 0.025666
+v -0.020333 -0.029158 -0.000677
+v 0.014792 -0.002819 -0.000677
+v 0.012596 -0.015985 0.041034
+v -0.020333 0.005966 0.001520
+v -0.002765 -0.026962 0.001520
+v -0.007160 -0.031353 0.041034
+v 0.014792 0.005966 -0.000677
+v 0.010401 -0.013789 -0.000677
+v 0.016987 -0.007210 0.036641
+v -0.020333 -0.026962 0.041034
+v -0.015942 -0.031353 0.012503
+v 0.016987 0.005966 0.012503
+v 0.001619 -0.026962 0.041034
+v 0.010401 -0.015985 0.005913
+v -0.011551 -0.029158 -0.000677
+v 0.003814 -0.022571 0.001520
+v -0.020333 -0.013789 -0.000677
+v 0.014792 -0.007210 0.008106
+v -0.007160 -0.029158 0.005913
+v 0.012596 -0.015985 0.032247
+v 0.001619 -0.026962 0.034444
+v 0.016987 -0.000620 0.016888
+v -0.020333 -0.031353 0.012503
+v 0.016987 -0.007210 0.041034
+v -0.002765 -0.029158 0.030055
+v -0.000577 -0.024767 -0.000677
+v 0.012596 -0.011601 0.003717
+v -0.007160 -0.031353 0.036641
+v 0.008205 -0.020376 0.021273
+v 0.014792 -0.011601 0.030055
+v 0.008205 -0.018180 0.001520
+v 0.001619 -0.024767 0.005913
+usemtl Shape.015
+s off
+f 409 435 446
+f 411 410 412
+f 410 411 413
+f 411 412 414
+f 413 414 415
+f 415 412 416
+f 410 413 417
+f 414 412 418
+f 415 414 418
+f 417 413 420
+f 412 410 421
+f 416 412 421
+f 415 416 422
+f 413 411 424
+f 411 414 424
+f 414 413 424
+f 420 413 425
+f 421 410 426
+f 416 421 426
+f 410 423 426
+f 417 420 427
+f 415 422 429
+f 425 415 429
+f 412 415 431
+f 418 412 431
+f 415 418 431
+f 429 419 433
+f 425 429 433
+f 417 427 434
+f 434 427 435
+f 416 426 436
+f 426 423 436
+f 432 416 436
+f 423 432 436
+f 413 415 437
+f 425 413 437
+f 415 425 437
+f 410 417 438
+f 423 410 438
+f 427 420 439
+f 433 419 439
+f 419 435 439
+f 435 427 439
+f 419 429 440
+f 429 422 440
+f 430 419 440
+f 422 430 440
+f 422 416 441
+f 416 432 441
+f 434 428 441
+f 420 425 442
+f 425 433 442
+f 439 420 442
+f 433 439 442
+f 409 430 443
+f 428 434 443
+f 435 409 443
+f 434 435 443
+f 432 423 444
+f 417 434 444
+f 438 417 444
+f 423 438 444
+f 441 432 444
+f 434 441 444
+f 430 422 445
+f 422 441 445
+f 441 428 445
+f 443 430 445
+f 428 443 445
+f 430 409 446
+f 419 430 446
+f 435 419 446
+o ShapeIndexedFaceSet_ShapeIndexedFaceSet.015
+v 0.000000 0.000000 0.000000
+v -0.057652 -0.000620 0.021270
+v -0.022528 -0.031353 0.012498
+v -0.020333 -0.031353 0.012498
+v -0.020333 -0.031353 0.038838
+v -0.020333 0.005966 0.001520
+v -0.051066 -0.013789 -0.000677
+v -0.053261 0.005966 0.038838
+v -0.042284 -0.026962 0.038838
+v -0.020333 0.005966 0.025664
+v -0.055457 0.005966 -0.000677
+v -0.026919 -0.029158 -0.000677
+v -0.055457 -0.011601 0.038838
+v -0.037897 -0.026962 0.001520
+v -0.020333 -0.029158 -0.000677
+v -0.057652 0.005966 0.038838
+v -0.031314 -0.031353 0.030050
+v -0.020333 -0.026962 0.038838
+v -0.048870 -0.018180 0.001520
+v -0.055457 -0.005015 0.001520
+v -0.020333 -0.013789 -0.000677
+v -0.053261 -0.015985 0.036641
+v -0.040088 0.005966 -0.000677
+v -0.046671 -0.022571 0.023467
+v -0.057652 -0.005015 0.038838
+v -0.031314 -0.031353 0.038838
+v -0.033509 -0.029158 0.005911
+v -0.057652 0.005966 0.016892
+v -0.053261 -0.011601 0.005911
+v -0.037897 -0.029158 0.032247
+v -0.044476 -0.022571 0.001520
+v -0.040088 -0.024767 -0.000677
+v -0.057652 -0.005015 0.032247
+v -0.042284 -0.026962 0.034444
+v -0.055457 -0.011601 0.034444
+v -0.055457 -0.000620 -0.000677
+v -0.026919 -0.031353 0.016892
+v -0.031314 -0.029158 0.001520
+usemtl Shape.014
+s off
+f 449 483 484
+f 449 450 451
+f 451 450 452
+f 451 454 455
+f 451 452 456
+f 452 454 456
+f 454 452 457
+f 450 449 458
+f 453 457 458
+f 455 454 459
+f 452 450 461
+f 450 458 461
+f 458 457 461
+f 454 457 462
+f 459 454 462
+f 449 451 463
+f 454 451 464
+f 451 456 464
+f 456 454 464
+f 452 461 467
+f 461 457 467
+f 455 459 468
+f 457 452 469
+f 452 467 469
+f 467 457 469
+f 468 465 470
+f 462 448 471
+f 459 462 471
+f 451 455 472
+f 463 451 472
+f 448 462 474
+f 462 457 474
+f 466 448 474
+f 453 465 475
+f 466 453 475
+f 465 468 475
+f 472 455 476
+f 463 472 476
+f 460 473 476
+f 473 463 476
+f 470 465 477
+f 453 458 478
+f 465 453 478
+f 460 477 478
+f 477 465 478
+f 448 466 479
+f 471 448 479
+f 459 471 479
+f 455 468 480
+f 468 470 480
+f 476 455 480
+f 460 476 480
+f 477 460 480
+f 470 477 480
+f 468 459 481
+f 466 475 481
+f 475 468 481
+f 479 466 481
+f 459 479 481
+f 457 453 482
+f 453 466 482
+f 474 457 482
+f 466 474 482
+f 449 463 483
+f 463 473 483
+f 483 473 484
+f 458 449 484
+f 473 460 484
+f 478 458 484
+f 460 478 484

cfg.yaml

@@ -0,0 +1,90 @@
+
+lmps:
+  tabletop_ui:
+    prompt_path: prompts/tabletop_ui.py
+    engine: model_name
+    max_tokens: 512
+    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: model_name
+    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: model_name
+    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: model_name
+    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: model_name
+    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: model_name
+    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

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

lmp.py

@@ -0,0 +1,251 @@
+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
+
+
+class LMP:
+
+    def __init__(self, name, cfg, lmp_fgen, fixed_vars, variable_vars):
+        self._name = name
+        self._cfg = cfg
+
+        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)
+
+        while True:
+            try:
+                code_str = openai.Completion.create(
+                    prompt=prompt,
+                    stop=self._stop_tokens,
+                    temperature=self._cfg['temperature'],
+                    engine=self._cfg['engine'],
+                    max_tokens=self._cfg['max_tokens']
+                )['choices'][0]['text'].strip()
+                break
+            except (RateLimitError, APIConnectionError) as e:
+                print(f'OpenAI API got err {e}')
+                print('Retrying after 10s.')
+                sleep(10)
+
+        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} exec:\n\n{to_log_pretty}\n')
+
+        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']:
+            return lvars[self._cfg['return_val_name']]
+
+
+class LMPFGen:
+
+    def __init__(self, cfg, fixed_vars, variable_vars):
+        self._cfg = cfg
+
+        self._stop_tokens = list(self._cfg['stop'])
+        self._fixed_vars = fixed_vars
+        self._variable_vars = variable_vars
+
+        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}'
+
+        while True:
+            try:
+                f_src = openai.Completion.create(
+                    prompt=prompt, 
+                    stop=self._stop_tokens,
+                    temperature=self._cfg['temperature'],
+                    engine=self._cfg['engine'],
+                    max_tokens=self._cfg['max_tokens']
+                )['choices'][0]['text'].strip()
+                break
+            except (RateLimitError, APIConnectionError) as e:
+                print(f'OpenAI API got err {e}')
+                print('Retrying after 10s.')
+                sleep(10)
+
+        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 = highlight(f'{use_query}\n{f_src}', PythonLexer(), TerminalFormatter())
+        print(f'LMP FGEN created:\n\n{to_print}\n')
+
+        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()
+    }
+    
+
+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}
+    ])
+    exec(code_str, custom_gvars, lvars)

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])

prompts/parse_obj_name.py

@@ -0,0 +1,64 @@
+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']
+block_positions = get_obj_positions_np(block_names)
+closest_block_idx = get_closest_idx(points=block_positions, 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']
+block_positions = get_obj_positions_np(block_names)
+closest_block_idx = get_closest_idx(points=block_positions, 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']
+block_positions = get_obj_positions_np(block_names)
+left_block_idx = np.argsort(block_positions[:, 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']
+bowl_positions = get_obj_positions_np(bowl_names)
+top_bowl_idx = np.argsort(block_positions[:, 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_positions = get_obj_positions_np(bowl_names)
+bowl_idx = np.argsort(block_positions[:, 0])[-3]
+bowl_name = bowl_names[bowl_idx]
+ret_val = bowl_name

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]

prompts/parse_question.py

@@ -0,0 +1,30 @@
+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']
+# is the pink block on the green bowl?
+ret_val = bbox_contains_pt(container_name='green bowl', obj_name='pink block')
+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]

prompts/tabletop_ui.py

@@ -0,0 +1,198 @@
+# 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']
+# 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']
+# stack everything with the green block on top.
+say('Ok! Stacking everything with the green block on the top')
+order_bottom_to_top = ['blue bowl', 'pink bowl', 'green bowl', 'pink block', 'blue block', 'green block']
+stack_objects_in_order(object_names=order_bottom_to_top)
+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']
+# place the blue block in the empty bowl.
+empty_bowl_name = parse_obj_name('the empty bowl', f'objects = {get_obj_names()}')
+if empty_bowl_name:
+  say(f'Ok! Putting the blue block on the {empty_bowl_name}')
+  put_first_on_second('blue block', empty_bowl_name)
+else:
+  say('There are no empty bowls')
+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 = ['brown bowl', 'green block', 'brown block', 'green bowl', 'blue bowl', 'blue block']
+# move the red bowl a lot to the left of the blocks.
+say('Sure! Moving the red bowl to a point left of the blocks')
+left_pos = parse_position('a point 20cm left of the blocks')
+put_first_on_second('red bowl', left_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 = ['pink block', 'green block', 'pink bowl', 'blue block', 'blue bowl', 'green bowl']
+# if you see a purple bowl put it on the blue bowl
+if is_obj_visible('purple bowl'):
+  say('Putting the purple bowl on the pink bowl')
+  put_first_on_second('purple bowl', 'pink bowl')
+else:
+  say('I don\'t see a purple 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)

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)

requirements.txt

@@ -0,0 +1,11 @@
+numpy
+scipy
+shapely
+astunparse
+pygments
+openai
+pybullet
+imageio==2.4.1
+imageio-ffmpeg
+moviepy
+omegaconf

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
+```

robotiq_2f_85/robotiq-2f-base.mtl

@@ -0,0 +1,13 @@
+# Blender MTL File: 'gripper-2f.blend'
+# Material Count: 1
+
+newmtl Default
+Ns 96.078431
+Ka 1.000000 1.000000 1.000000
+Kd 0.640000 0.640000 0.640000
+Ks 0.500000 0.500000 0.500000
+Ke 0.000000 0.000000 0.000000
+Ni 1.000000
+d 1.000000
+illum 2
+map_Kd textures/gripper-2f_BaseColor.jpg

robotiq_2f_85/robotiq-2f-base.stl

@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:1c7b9f2bd92d705fc4e897c94e905973a3c05f406845e942229433deb7041453
+size 1712484

robotiq_2f_85/robotiq-2f-coupler.mtl

@@ -0,0 +1,13 @@
+# Blender MTL File: 'gripper-2f.blend'
+# Material Count: 1
+
+newmtl Default
+Ns 96.078431
+Ka 1.000000 1.000000 1.000000
+Kd 0.640000 0.640000 0.640000
+Ks 0.500000 0.500000 0.500000
+Ke 0.000000 0.000000 0.000000
+Ni 1.000000
+d 1.000000
+illum 2
+map_Kd textures/gripper-2f_BaseColor.jpg

robotiq_2f_85/robotiq-2f-coupler.stl

@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:d5ee95e62f8415bf5b6e503c831a958f5fc1990bf9b2865329ec38a28932727c
+size 89084

robotiq_2f_85/robotiq-2f-driver.mtl

@@ -0,0 +1,13 @@
+# Blender MTL File: 'gripper-2f.blend'
+# Material Count: 1
+
+newmtl Default
+Ns 96.078431
+Ka 1.000000 1.000000 1.000000
+Kd 0.640000 0.640000 0.640000
+Ks 0.500000 0.500000 0.500000
+Ke 0.000000 0.000000 0.000000
+Ni 1.000000
+d 1.000000
+illum 2
+map_Kd textures/gripper-2f_BaseColor.jpg

robotiq_2f_85/robotiq-2f-driver.stl

@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:bbd6e868b1778bead60d2516c7ede581d7ad4e431744b1828757d6ea5129c112
+size 67084

robotiq_2f_85/robotiq-2f-follower.mtl

@@ -0,0 +1,13 @@
+# Blender MTL File: 'gripper-2f.blend'
+# Material Count: 1
+
+newmtl Default
+Ns 96.078431
+Ka 1.000000 1.000000 1.000000
+Kd 0.640000 0.640000 0.640000
+Ks 0.500000 0.500000 0.500000
+Ke 0.000000 0.000000 0.000000
+Ni 1.000000
+d 1.000000
+illum 2
+map_Kd textures/gripper-2f_BaseColor.jpg

robotiq_2f_85/robotiq-2f-follower.stl

@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:633793332c081641ce200df27a40643cc293b29956e3cb5cb29cb811c33ef1c7
+size 110484

robotiq_2f_85/robotiq-2f-pad.stl

@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:6c4e73a07a7d6853777450d0019691d2d43f50c9fa0bd6b0189d4af164a0ce4b
+size 684

robotiq_2f_85/robotiq-2f-spring_link.mtl

@@ -0,0 +1,13 @@
+# Blender MTL File: 'gripper-2f.blend'
+# Material Count: 1
+
+newmtl Default
+Ns 96.078431
+Ka 1.000000 1.000000 1.000000
+Kd 0.640000 0.640000 0.640000
+Ks 0.500000 0.500000 0.500000
+Ke 0.000000 0.000000 0.000000
+Ni 1.000000
+d 1.000000
+illum 2
+map_Kd textures/gripper-2f_BaseColor.jpg

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

robotiq_2f_85/robotiq_2f_85.urdf

@@ -0,0 +1,299 @@
+<?xml version="1.0" ?>
+<!-- =================================================================================== -->
+<!-- |    This document was autogenerated by xacro from simple_rq2f85_pybullet.urdf.xacro | -->
+<!-- |    EDITING THIS FILE BY HAND IS NOT RECOMMENDED                                 | -->
+<!-- =================================================================================== -->
+<robot name="example" xmlns:xacro="http://ros.org/wiki/xacro">
+  <material name="Blanc">
+    <color rgba="1.0 1.0 1.0 1.0"/>
+  </material>
+  <link name="base_link">
+    <origin xyz="0 0 0"/>
+    <inertial>
+      <mass value="0.001"/>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <inertia ixx="0" ixy="0.0" ixz="0.0" iyy="0.0" iyz="0.0" izz="0.0"/>
+    </inertial>
+  </link>
+  <!-- base -->
+  <joint name="base_link_robotiq_2f_85_base_joint" type="fixed">
+    <origin rpy="0 0 0" xyz="0 0 0"/>
+    <parent link="base_link"/>
+    <child link="robotiq_2f_85_base"/>
+  </joint>
+  <link name="robotiq_2f_85_base">
+    <inertial>
+      <mass value="0.1"/>
+      <origin xyz="0 0 0.055"/>
+      <inertia ixx="0.000190833333333" ixy="0" ixz="0" iyy="0.00018" iyz="0" izz="0.000190833333333"/>
+    </inertial>
+    <visual>
+      <geometry>
+        <mesh filename="robotiq-2f-base.obj" scale="0.1 0.1 0.1"/>
+      </geometry>
+      <material name="Blanc"/>
+    </visual>
+    <!-- <collision>
+      <geometry>
+        <mesh filename="robotiq-2f-base.stl" scale="0.001 0.001 0.001"/>
+      </geometry>
+    </collision> -->
+  </link>
+  <!-- right finger -->
+  <joint name="robotiq_2f_85_right_driver_joint" type="revolute">
+    <origin rpy="0 0 0" xyz="0 0.0306011 0.054904"/>
+    <parent link="robotiq_2f_85_base"/>
+    <child link="robotiq_2f_85_right_driver"/>
+    <axis xyz="1 0 0"/>
+    <limit effort="60" lower="0.0" upper="0.834" velocity="1.91986177778"/>
+  </joint>
+  <link name="robotiq_2f_85_right_driver">
+    <inertial>
+      <mass value="0.1"/>
+      <origin xyz="0 0 0.055"/>
+      <inertia ixx="0.000190833333333" ixy="0" ixz="0" iyy="0.00018" iyz="0" izz="0.000190833333333"/>
+    </inertial>
+    <visual>
+      <geometry>
+        <mesh filename="robotiq-2f-driver.obj" scale="0.1 0.1 0.1"/>
+      </geometry>
+      <material name="Blanc"/>
+    </visual>
+    <!-- <collision>
+      <geometry>
+        <mesh filename="robotiq-2f-driver.stl" scale="0.001 0.001 0.001"/>
+      </geometry>
+    </collision> -->
+  </link>
+  <joint name="robotiq_2f_85_right_coupler_joint" type="fixed">
+    <origin rpy="0 0 0" xyz="0 0.0315 -0.0041"/>
+    <parent link="robotiq_2f_85_right_driver"/>
+    <child link="robotiq_2f_85_right_coupler"/>
+  </joint>
+  <link name="robotiq_2f_85_right_coupler">
+    <inertial>
+      <mass value="0.1"/>
+      <origin xyz="0 0 0.055"/>
+      <inertia ixx="0.000190833333333" ixy="0" ixz="0" iyy="0.00018" iyz="0" izz="0.000190833333333"/>
+    </inertial>
+    <visual>
+      <geometry>
+        <mesh filename="robotiq-2f-coupler.obj" scale="0.1 0.1 0.1"/>
+      </geometry>
+      <material name="Blanc"/>
+    </visual>
+    <!-- <collision>
+      <geometry>
+        <mesh filename="robotiq-2f-coupler.stl" scale="0.001 0.001 0.001"/>
+      </geometry>
+    </collision> -->
+  </link>
+  <joint name="robotiq_2f_85_right_follower_joint" type="revolute">
+    <origin rpy="0 0 0" xyz="0 0.0061 0.0471"/>
+    <parent link="robotiq_2f_85_right_coupler"/>
+    <child link="robotiq_2f_85_right_follower"/>
+    <axis xyz="1 0 0"/>
+    <limit effort="176" lower="-2.96705911111" upper="2.96705911111" velocity="1.91986177778"/>
+    <mimic joint="robotiq_2f_85_right_driver_joint" multiplier="-1"/>
+  </joint>
+  <link name="robotiq_2f_85_right_follower">
+    <visual>
+      <geometry>
+        <mesh filename="robotiq-2f-follower.obj" scale="0.1 0.1 0.1"/>
+      </geometry>
+      <material name="Blanc"/>
+    </visual>
+    <!-- <collision>
+      <geometry>
+        <mesh filename="robotiq-2f-follower.stl" scale="0.001 0.001 0.001"/>
+      </geometry>
+    </collision> -->
+    <inertial>
+      <mass value="0.1"/>
+      <origin xyz="0 0 0.055"/>
+      <inertia ixx="0.000190833333333" ixy="0" ixz="0" iyy="0.00018" iyz="0" izz="0.000190833333333"/>
+    </inertial>
+  </link>
+  <joint name="robotiq_2f_85_right_pad_joint" type="fixed">
+			<parent link="robotiq_2f_85_right_follower"/>
+			<child link="robotiq_2f_85_right_pad"/>
+	</joint>
+	<link name="robotiq_2f_85_right_pad">
+		<inertial>
+			<mass value="0.1"/>
+			<origin xyz="0 0 0.055"  />
+      <inertia ixx="0.00019" iyy="0.00018" izz="0.00019" ixy="0" iyz="0" ixz="0"/>
+		</inertial>
+		<visual>
+			<geometry>
+				<mesh filename="robotiq-2f-pad.stl" scale="0.0001 0.0001 0.0001"/>
+			</geometry>
+      <material name="Blanc"/>
+		</visual>
+		<collision>
+			<geometry>
+				<mesh filename="robotiq-2f-pad.stl" scale="0.001 0.001 0.001"/>
+			</geometry>
+		</collision>
+	</link>
+  <joint name="robotiq_2f_85_right_spring_link_joint" type="revolute">
+    <origin rpy="0 0 0" xyz="0 0.012 0.0614"/>
+    <parent link="robotiq_2f_85_base"/>
+    <child link="robotiq_2f_85_right_spring_link"/>
+    <axis xyz="1 0 0"/>
+    <limit effort="176" lower="-2.96705911111" upper="2.96705911111" velocity="1.91986177778"/>
+    <mimic joint="robotiq_2f_85_right_driver_joint" multiplier="1"/>
+  </joint>
+  <link name="robotiq_2f_85_right_spring_link">
+    <inertial>
+      <mass value="0.1"/>
+      <origin xyz="0 0 0.055"/>
+      <inertia ixx="0.000190833333333" ixy="0" ixz="0" iyy="0.00018" iyz="0" izz="0.000190833333333"/>
+    </inertial>
+    <visual>
+      <geometry>
+        <mesh filename="robotiq-2f-spring_link.obj" scale="0.1 0.1 0.1"/>
+      </geometry>
+      <material name="Blanc"/>
+    </visual>
+    <!-- <collision>
+      <geometry>
+        <mesh filename="robotiq-2f-spring_link.stl" scale="0.001 0.001 0.001"/>
+      </geometry>
+    </collision> -->
+  </link>
+  <!-- left finger -->
+  <joint name="robotiq_2f_85_left_driver_joint" type="revolute">
+    <origin rpy="0 0 3.141592653589793" xyz="0 -0.0306011 0.054904"/>
+    <parent link="robotiq_2f_85_base"/>
+    <child link="robotiq_2f_85_left_driver"/>
+    <axis xyz="1 0 0"/>
+    <limit effort="176" lower="0.0" upper="0.834" velocity="1.91986177778"/>
+    <mimic joint="robotiq_2f_85_right_driver_joint" multiplier="1"/>
+  </joint>
+  <link name="robotiq_2f_85_left_driver">
+    <inertial>
+      <mass value="0.1"/>
+      <origin xyz="0 0 0.055"/>
+      <inertia ixx="0.000190833333333" ixy="0" ixz="0" iyy="0.00018" iyz="0" izz="0.000190833333333"/>
+    </inertial>
+    <visual>
+      <geometry>
+        <mesh filename="robotiq-2f-driver.obj" scale="0.1 0.1 0.1"/>
+      </geometry>
+      <material name="Blanc"/>
+    </visual>
+    <!-- <collision>
+      <geometry>
+        <mesh filename="robotiq-2f-driver.stl" scale="0.001 0.001 0.001"/>
+      </geometry>
+    </collision> -->
+  </link>
+  <joint name="robotiq_2f_85_left_coupler_joint" type="fixed">
+    <origin rpy="0 0 0" xyz="0 0.0315 -0.0041"/>
+    <parent link="robotiq_2f_85_left_driver"/>
+    <child link="robotiq_2f_85_left_coupler"/>
+  </joint>
+  <link name="robotiq_2f_85_left_coupler">
+    <inertial>
+      <mass value="0.1"/>
+      <origin xyz="0 0 0.055"/>
+      <inertia ixx="0.000190833333333" ixy="0" ixz="0" iyy="0.00018" iyz="0" izz="0.000190833333333"/>
+    </inertial>
+    <visual>
+      <geometry>
+        <mesh filename="robotiq-2f-coupler.obj" scale="0.1 0.1 0.1"/>
+      </geometry>
+      <material name="Blanc"/>
+    </visual>
+    <!-- <collision>
+      <geometry>
+        <mesh filename="robotiq-2f-coupler.stl" scale="0.001 0.001 0.001"/>
+      </geometry>
+    </collision> -->
+  </link>
+  <joint name="robotiq_2f_85_left_follower_joint" type="revolute">
+    <origin rpy="0 0 0" xyz="0 0.0061 0.0471"/>
+    <parent link="robotiq_2f_85_left_coupler"/>
+    <child link="robotiq_2f_85_left_follower"/>
+    <axis xyz="1 0 0"/>
+    <limit effort="176" lower="-2.96705911111" upper="2.96705911111" velocity="1.91986177778"/>
+    <mimic joint="robotiq_2f_85_right_driver_joint" multiplier="-1"/>
+  </joint>
+  <link name="robotiq_2f_85_left_follower">
+    <visual>
+      <geometry>
+        <mesh filename="robotiq-2f-follower.obj" scale="0.1 0.1 0.1"/>
+      </geometry>
+      <material name="Blanc"/>
+    </visual>
+    <!-- <collision>
+      <geometry>
+        <mesh filename="robotiq-2f-follower.stl" scale="0.001 0.001 0.001"/>
+      </geometry>
+    </collision> -->
+    <inertial>
+      <mass value="0.1"/>
+      <origin xyz="0 0 0.055"/>
+      <inertia ixx="0.000190833333333" ixy="0" ixz="0" iyy="0.00018" iyz="0" izz="0.000190833333333"/>
+    </inertial>
+  </link>
+  <joint name="robotiq_2f_85_left_pad_joint" type="fixed">
+			<parent link="robotiq_2f_85_left_follower"/>
+			<child link="robotiq_2f_85_left_pad"/>
+		</joint>
+		<link name="robotiq_2f_85_left_pad">
+			<inertial>
+				<mass value="0.1"/>
+				<origin xyz="0 0 0.055"  />
+				<inertia ixx="0.00019" iyy="0.00018" izz="0.00019" ixy="0" iyz="0" ixz="0"/>
+			</inertial>
+      <visual>
+        <geometry>
+          <mesh filename="robotiq-2f-pad.stl" scale="0.0001 0.0001 0.0001"/>
+        </geometry>
+        <material name="Blanc"/>
+      </visual>
+			<collision>
+				<geometry>
+					<mesh filename="robotiq-2f-pad.stl" scale="0.001 0.001 0.001"/>
+				</geometry>
+			</collision>
+		</link>
+  <joint name="robotiq_2f_85_left_spring_link_joint" type="revolute">
+    <origin rpy="0 0 3.141592653589793" xyz="0 -0.012 0.0614"/>
+    <parent link="robotiq_2f_85_base"/>
+    <child link="robotiq_2f_85_left_spring_link"/>
+    <axis xyz="1 0 0"/>
+    <limit effort="176" lower="-2.96705911111" upper="2.96705911111" velocity="1.91986177778"/>
+    <mimic joint="robotiq_2f_85_right_driver_joint" multiplier="1"/>
+  </joint>
+  <link name="robotiq_2f_85_left_spring_link">
+    <inertial>
+      <mass value="0.1"/>
+      <origin xyz="0 0 0.055"/>
+      <inertia ixx="0.000190833333333" ixy="0" ixz="0" iyy="0.00018" iyz="0" izz="0.000190833333333"/>
+    </inertial>
+    <visual>
+      <geometry>
+        <mesh filename="robotiq-2f-spring_link.obj" scale="0.1 0.1 0.1"/>
+      </geometry>
+      <material name="Blanc"/>
+    </visual>
+    <!-- <collision>
+      <geometry>
+        <mesh filename="robotiq-2f-spring_link.stl" scale="0.001 0.001 0.001"/>
+      </geometry>
+    </collision> -->
+  </link>
+  <transmission name="robotiq_2f_85_right_driver_trans">
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="robotiq_2f_85_right_driver_joint">
+      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+    </joint>
+    <actuator name="robotiq_2f_85_right_driver_motor">
+      <mechanicalReduction>1</mechanicalReduction>
+    </actuator>
+  </transmission>
+</robot>
+

sim.py

@@ -0,0 +1,655 @@
+import pybullet
+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
+
+# Gripper (Robotiq 2F85) code
+class Robotiq2F85:
+  """Gripper handling for Robotiq 2F85."""
+
+  def __init__(self, robot, tool):
+    self.robot = robot
+    self.tool = tool
+    pos = [0.1339999999999999, -0.49199999999872496, 0.5]
+    rot = pybullet.getQuaternionFromEuler([np.pi, 0, np.pi])
+    urdf = 'robotiq_2f_85/robotiq_2f_85.urdf'
+    self.body = pybullet.loadURDF(urdf, pos, rot)
+    self.n_joints = pybullet.getNumJoints(self.body)
+    self.activated = False
+
+    # Connect gripper base to robot tool.
+    pybullet.createConstraint(self.robot, tool, self.body, 0, jointType=pybullet.JOINT_FIXED, jointAxis=[0, 0, 0], parentFramePosition=[0, 0, 0], childFramePosition=[0, 0, -0.07], childFrameOrientation=pybullet.getQuaternionFromEuler([0, 0, np.pi / 2]))
+
+    # Set friction coefficients for gripper fingers.
+    for i in range(pybullet.getNumJoints(self.body)):
+      pybullet.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 = [pybullet.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]]
+        pybullet.setJointMotorControlArray(self.body, indj, pybullet.POSITION_CONTROL, targj, positionGains=np.ones(5))
+      except:
+        return
+      sleep(0.001)
+
+  # Close gripper fingers.
+  def activate(self):
+    pybullet.setJointMotorControl2(self.body, self.motor_joint, pybullet.VELOCITY_CONTROL, targetVelocity=1, force=10)
+    self.activated = True
+
+  # Open gripper fingers.
+  def release(self):
+    pybullet.setJointMotorControl2(self.body, self.motor_joint, pybullet.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 = pybullet.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(pybullet.getLinkState(self.body, 4)[0])
+    rpad = np.array(pybullet.getLinkState(self.body, 9)[0])
+    dist = np.linalg.norm(lpad - rpad) - 0.047813
+    return dist
+
+  def check_proximity(self):
+    ee_pos = np.array(pybullet.getLinkState(self.robot, self.tool)[0])
+    tool_pos = np.array(pybullet.getLinkState(self.body, 0)[0])
+    vec = (tool_pos - ee_pos) / np.linalg.norm((tool_pos - ee_pos))
+    ee_targ = ee_pos + vec
+    ray_data = pybullet.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.
+    # python3 -m pybullet_utils.runServer
+    # pybullet.connect(pybullet.SHARED_MEMORY)  # pybullet.GUI for local GUI.
+    pybullet.connect(pybullet.DIRECT)  # pybullet.GUI for local GUI.
+    pybullet.configureDebugVisualizer(pybullet.COV_ENABLE_GUI, 0)
+    pybullet.setPhysicsEngineParameter(enableFileCaching=0)
+    assets_path = os.path.dirname(os.path.abspath(""))
+    pybullet.setAdditionalSearchPath(assets_path)
+    pybullet.setAdditionalSearchPath(pybullet_data.getDataPath())
+    pybullet.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):
+    pybullet.resetSimulation(pybullet.RESET_USE_DEFORMABLE_WORLD)
+    pybullet.setGravity(0, 0, -9.8)
+    self.cache_video = []
+
+    # Temporarily disable rendering to load URDFs faster.
+    pybullet.configureDebugVisualizer(pybullet.COV_ENABLE_RENDERING, 0)
+
+    # Add robot.
+    pybullet.loadURDF("plane.urdf", [0, 0, -0.001])
+    self.robot_id = pybullet.loadURDF("ur5e/ur5e.urdf", [0, 0, 0], flags=pybullet.URDF_USE_MATERIAL_COLORS_FROM_MTL)
+    self.ghost_id = pybullet.loadURDF("ur5e/ur5e.urdf", [0, 0, -10])  # For forward kinematics.
+    self.joint_ids = [pybullet.getJointInfo(self.robot_id, i) for i in range(pybullet.getNumJoints(self.robot_id))]
+    self.joint_ids = [j[0] for j in self.joint_ids if j[2] == pybullet.JOINT_REVOLUTE]
+
+    # Move robot to home configuration.
+    for i in range(len(self.joint_ids)):
+      pybullet.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.gripper.release()
+
+    # Add workspace.
+    plane_shape = pybullet.createCollisionShape(pybullet.GEOM_BOX, halfExtents=[0.3, 0.3, 0.001])
+    plane_visual = pybullet.createVisualShape(pybullet.GEOM_BOX, halfExtents=[0.3, 0.3, 0.001])
+    plane_id = pybullet.createMultiBody(0, plane_shape, plane_visual, basePosition=[0, -0.5, 0])
+    pybullet.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 = pybullet.createCollisionShape(pybullet.GEOM_BOX, halfExtents=[0.02, 0.02, 0.02])
+          object_visual = pybullet.createVisualShape(pybullet.GEOM_BOX, halfExtents=[0.02, 0.02, 0.02])
+          object_id = pybullet.createMultiBody(0.01, object_shape, object_visual, basePosition=object_position)
+        elif object_type == 'bowl':
+          object_position[2] = 0
+          object_id = pybullet.loadURDF("bowl/bowl.urdf", object_position, useFixedBase=1)
+        pybullet.changeVisualShape(object_id, -1, rgbaColor=object_color)
+        self.obj_name_to_id[obj_name] = object_id
+    
+
+    # Re-enable rendering.
+    pybullet.configureDebugVisualizer(pybullet.COV_ENABLE_RENDERING, 1)
+
+    for _ in range(200):
+      pybullet.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."""
+    pybullet.setJointMotorControlArray(
+      bodyIndex=self.robot_id,
+      jointIndices=self.joint_ids,
+      controlMode=pybullet.POSITION_CONTROL,
+      targetPositions=joints,
+      positionGains=[0.01]*6)
+  
+  def movep(self, position):
+    """Move to target end effector position."""
+    joints = pybullet.calculateInverseKinematics(
+        bodyUniqueId=self.robot_id,
+        endEffectorLinkIndex=self.tip_link_id,
+        targetPosition=position,
+        targetOrientation=pybullet.getQuaternionFromEuler(self.home_ee_euler),
+        maxNumIterations=100)
+    self.servoj(joints)
+
+  def get_ee_pos(self):
+    ee_xyz = np.float32(pybullet.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 = pybullet.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]
+        pybullet.changeVisualShape(
+            self.robot_id, linkIndex=i, rgbaColor=rgba_color)      
+        pybullet.changeVisualShape(
+            self.gripper.body, linkIndex=i, rgbaColor=rgba_color)
+
+  def step_sim_and_render(self):
+    pybullet.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 = pybullet.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 = pybullet.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 = pybullet.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 = pybullet.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 = pybullet.computeProjectionMatrixFOV(fovh, aspect_ratio, znear, zfar)
+
+    # Render with OpenGL camera settings.
+    _, _, color, depth, segm = pybullet.getCameraImage(
+        width=image_size[1],
+        height=image_size[0],
+        viewMatrix=viewm,
+        projectionMatrix=projm,
+        shadow=1,
+        flags=pybullet.ER_SEGMENTATION_MASK_OBJECT_AND_LINKINDEX,
+        renderer=pybullet.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]
+    except:
+      print(f'requested_name="{obj_name}"')
+      print(f'available_objects_and_id="{self.obj_name_to_id}')
+    return obj_id
+  
+  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 = pybullet.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 pybullet.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]

ur5e/collision/base.stl

@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:9a145e0d46f2130afdf2a2e8825a00a929870c4c3d6d8e4d1adc5f04db3aac1b
+size 21084

ur5e/collision/forearm.stl

@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:2e7423cab807c34160ec4f770daee5e747d70e777eb01b7beeace2b8c5751816
+size 53284

ur5e/collision/shoulder.stl

@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:ceb92532177daa77682f5fbd628e01c2137d168f949a7a706ce1dabe9f002387
+size 70084

ur5e/collision/upperarm.stl

@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:ee893044caf00075cb55b4cf666d1f1311c7979786212a501009f33bee945209
+size 99684

ur5e/collision/wrist1.stl

@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:f8c9f9337b6fd98c75f052e96de10e14a107ddb6874ba6b904e546f8a4e4f43a
+size 59584

ur5e/collision/wrist2.stl

@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:2964a63f60ce3e3cf3ad55bcf190d7876d50e373cb64b70a57cea5885eaf3c86
+size 67584

ur5e/collision/wrist3.stl

@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:83b3666b4ae2badd54af0d2c25a921682ecbc29e849eec646c3ed55fb74c78a3
+size 7184

ur5e/ur5e.urdf

@@ -0,0 +1,279 @@
+<?xml version="1.0" ?>
+<!-- ============================================================================================= -->
+<!-- |    This document was autogenerated by xacro from ur_description/urdf/ur5_robot.urdf.xacro | -->
+<!-- |    EDITING THIS FILE BY HAND IS NOT RECOMMENDED                                           | -->
+<!-- ============================================================================================= -->
+<robot name="ur5" xmlns:xacro="http://ros.org/wiki/xacro">
+  <material name="LightGrey">
+    <color rgba="0.8 0.8 0.8 1.0"/>
+  </material>
+
+  <material name="DarkGrey">
+    <color rgba="0.2 0.2 0.2 1.0"/>
+  </material>
+
+  <link name="base_link">
+    <visual>
+      <geometry>
+        <mesh filename="visual/base.dae"/>
+      </geometry>
+      <material name="LightGrey"/>
+    </visual>
+    <!-- <collision>
+      <geometry>
+      	<mesh filename="collision/base.stl"/>
+      </geometry>
+    </collision> -->
+    <inertial>
+      <mass value="4.0"/>
+      <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
+      <inertia ixx="0.00443333156" ixy="0.0" ixz="0.0" iyy="0.00443333156" iyz="0.0" izz="0.0072"/>
+    </inertial>
+  </link>
+  <joint name="shoulder_pan_joint" type="revolute">
+    <parent link="base_link"/>
+    <child link="shoulder_link"/>
+    <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.163"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="150.0" lower="-6.28318530718" upper="6.28318530718" velocity="3.15"/>
+    <dynamics damping="0.0" friction="0.0"/>
+  </joint>
+  <link name="shoulder_link">
+    <visual>
+      <geometry>
+        <mesh filename="visual/shoulder.dae"/>
+      </geometry>
+      <material name="DarkGrey"/>
+    </visual>
+    <!-- <collision>
+      <geometry>
+        <mesh filename="collision/shoulder.stl"/>
+      </geometry>
+    </collision> -->
+    <inertial>
+      <mass value="3.7"/>
+      <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
+      <inertia ixx="0.010267495893" ixy="0.0" ixz="0.0" iyy="0.010267495893" iyz="0.0" izz="0.00666"/>
+    </inertial>
+  </link>
+  <joint name="shoulder_lift_joint" type="revolute">
+    <parent link="shoulder_link"/>
+    <child link="upper_arm_link"/>
+    <origin rpy="0.0 1.57079632679 0.0" xyz="0.0 0.138 0.0"/>
+    <axis xyz="0 1 0"/>
+    <limit effort="150.0" lower="-6.28318530718" upper="6.28318530718" velocity="3.15"/>
+    <dynamics damping="0.0" friction="0.0"/>
+  </joint>
+  <link name="upper_arm_link">
+    <visual>
+      <geometry>
+        <mesh filename="visual/upperarm.dae"/>
+      </geometry>
+      <material name="LightGrey"/>
+    </visual>
+    <!-- <collision>
+      <geometry>
+        <mesh filename="collision/upperarm.stl"/>
+      </geometry>
+    </collision> -->
+    <inertial>
+      <mass value="8.393"/>
+      <origin rpy="0 0 0" xyz="0.0 0.0 0.28"/>
+      <inertia ixx="0.22689067591" ixy="0.0" ixz="0.0" iyy="0.22689067591" iyz="0.0" izz="0.0151074"/>
+    </inertial>
+  </link>
+  <joint name="elbow_joint" type="revolute">
+    <parent link="upper_arm_link"/>
+    <child link="forearm_link"/>
+    <origin rpy="0.0 0.0 0.0" xyz="0.0 -0.131 0.425"/>
+    <axis xyz="0 1 0"/>
+    <limit effort="150.0" lower="-3.14159265359" upper="3.14159265359" velocity="3.15"/>
+    <dynamics damping="0.0" friction="0.0"/>
+  </joint>
+  <link name="forearm_link">
+    <visual>
+      <geometry>
+        <mesh filename="visual/forearm.dae"/>
+      </geometry>
+      <material name="DarkGrey"/>
+    </visual>
+    <!-- <collision>
+      <geometry>
+        <mesh filename="collision/forearm.stl"/>
+      </geometry>
+    </collision> -->
+    <inertial>
+      <mass value="2.275"/>
+      <origin rpy="0 0 0" xyz="0.0 0.0 0.25"/>
+      <inertia ixx="0.049443313556" ixy="0.0" ixz="0.0" iyy="0.049443313556" iyz="0.0" izz="0.004095"/>
+    </inertial>
+  </link>
+  <joint name="wrist_1_joint" type="revolute">
+    <parent link="forearm_link"/>
+    <child link="wrist_1_link"/>
+    <origin rpy="0.0 1.57079632679 0.0" xyz="0.0 0.0 0.392"/>
+    <axis xyz="0 1 0"/>
+    <limit effort="28.0" lower="-6.28318530718" upper="6.28318530718" velocity="3.2"/>
+    <dynamics damping="0.0" friction="0.0"/>
+  </joint>
+  <link name="wrist_1_link">
+    <visual>
+      <geometry>
+        <mesh filename="visual/wrist1.dae"/>
+      </geometry>
+      <material name="LightGrey"/>
+    </visual>
+    <!-- <collision>
+      <geometry>
+        <mesh filename="collision/wrist1.stl"/>
+      </geometry>
+    </collision> -->
+    <inertial>
+      <mass value="1.219"/>
+      <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
+      <inertia ixx="0.111172755531" ixy="0.0" ixz="0.0" iyy="0.111172755531" iyz="0.0" izz="0.21942"/>
+    </inertial>
+  </link>
+  <joint name="wrist_2_joint" type="revolute">
+    <parent link="wrist_1_link"/>
+    <child link="wrist_2_link"/>
+    <origin rpy="0.0 0.0 0.0" xyz="0.0 0.127 0.0"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="28.0" lower="-6.28318530718" upper="6.28318530718" velocity="3.2"/>
+    <dynamics damping="0.0" friction="0.0"/>
+  </joint>
+  <link name="wrist_2_link">
+    <visual>
+      <geometry>
+        <mesh filename="visual/wrist2.dae"/>
+      </geometry>
+      <material name="DarkGrey"/>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="collision/wrist2.stl"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="1.219"/>
+      <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
+      <inertia ixx="0.111172755531" ixy="0.0" ixz="0.0" iyy="0.111172755531" iyz="0.0" izz="0.21942"/>
+    </inertial>
+  </link>
+  <joint name="wrist_3_joint" type="revolute">
+    <parent link="wrist_2_link"/>
+    <child link="wrist_3_link"/>
+    <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.100"/>
+    <axis xyz="0 1 0"/>
+    <limit effort="28.0" lower="-6.28318530718" upper="6.28318530718" velocity="3.2"/>
+    <dynamics damping="0.0" friction="0.0"/>
+  </joint>
+  <link name="wrist_3_link">
+    <visual>
+      <geometry>
+        <mesh filename="visual/wrist3.dae"/>
+      </geometry>
+      <material name="LightGrey"/>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="collision/wrist3.stl"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="0.1879"/>
+      <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
+      <inertia ixx="0.0171364731454" ixy="0.0" ixz="0.0" iyy="0.0171364731454" iyz="0.0" izz="0.033822"/>
+    </inertial>
+  </link>
+  <joint name="ee_fixed_joint" type="fixed">
+    <parent link="wrist_3_link"/>
+    <child link="ee_link"/>
+    <origin rpy="-1.5707963267948966 0.0 0.0" xyz="0.0 0.100 0.0"/>
+  </joint>
+  <link name="ee_link">
+    <!-- <collision>
+      <geometry>
+        <box size="0.01 0.01 0.01"/>
+      </geometry>
+      <origin rpy="0 0 0" xyz="-0.01 0 0"/>
+    </collision> -->
+    <inertial>
+      <mass value="0.0"/>
+      <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
+      <inertia ixx="0.0" ixy="0.0" ixz="0.0" iyy="0.0" iyz="0.0" izz="0.0"/>
+    </inertial>
+  </link>
+  <!-- nothing to do here at the moment -->
+  <!-- ROS base_link to UR 'Base' Coordinates transform -->
+  <link name="base">
+    <inertial>
+      <mass value="0.0"/>
+      <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
+      <inertia ixx="0.0" ixy="0.0" ixz="0.0" iyy="0.0" iyz="0.0" izz="0.0"/>
+    </inertial>
+  </link>
+  <joint name="base_link-base_fixed_joint" type="fixed">
+    <!-- NOTE: this rotation is only needed as long as base_link itself is
+                 not corrected wrt the real robot (ie: rotated over 180
+                 degrees)
+      -->
+    <origin rpy="0 0 -3.14159265359" xyz="0 0 0"/>
+    <parent link="base_link"/>
+    <child link="base"/>
+  </joint>
+  <!-- Frame coincident with all-zeros TCP on UR controller -->
+  <link name="tool0">
+    <inertial>
+      <mass value="0.0"/>
+      <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
+      <inertia ixx="0.0" ixy="0.0" ixz="0.0" iyy="0.0" iyz="0.0" izz="0.0"/>
+    </inertial>
+  </link>
+  <joint name="wrist_3_link-tool0_fixed_joint" type="fixed">
+    <origin rpy="-1.57079632679 0 0" xyz="0 0.0823 0"/>
+    <parent link="wrist_3_link"/>
+    <child link="tool0"/>
+  </joint>
+
+  <link name="tool_tip">
+    <inertial>
+      <mass value="0.0"/>
+      <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
+      <inertia ixx="0.0" ixy="0.0" ixz="0.0" iyy="0.0" iyz="0.0" izz="0.0"/>
+    </inertial>
+    <!-- <visual>
+      <geometry>
+        <box size="0.01 0.01 0.01"/>
+      </geometry>
+      <material name="DarkGrey"/>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+    </visual> -->
+  </link>
+  <joint name="tool0_fixed_joint-tool_tip" type="fixed">
+    <origin rpy="0 0 0" xyz="0 0 0.175"/>
+    <parent link="tool0"/>
+    <child link="tool_tip"/>
+  </joint>
+
+  <link name="world"/>
+  <joint name="world_joint" type="fixed">
+    <parent link="world"/>
+    <child link="rotated_base_link"/>
+    <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
+  </joint>
+
+  <link name="rotated_base_link">
+    <inertial>
+      <mass value="0.0"/>
+      <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
+      <inertia ixx="0.0" ixy="0.0" ixz="0.0" iyy="0.0" iyz="0.0" izz="0.0"/>
+    </inertial>
+  </link>>
+
+  <joint name="rotated_base-base_fixed_joint" type="fixed">
+    <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 3.141592653589793"/>
+    <parent link="rotated_base_link"/>
+    <child link="base_link"/>
+  </joint>
+</robot>